Line |
Branch |
Exec |
Source |
1 |
|
|
// |
2 |
|
|
// Copyright (c) 2017-2020 CNRS INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#ifndef __pinocchio_cartesian_axis_hpp__ |
6 |
|
|
#define __pinocchio_cartesian_axis_hpp__ |
7 |
|
|
|
8 |
|
|
#include "pinocchio/fwd.hpp" |
9 |
|
|
|
10 |
|
|
namespace pinocchio |
11 |
|
|
{ |
12 |
|
|
|
13 |
|
|
template<int _axis> |
14 |
|
|
struct CartesianAxis |
15 |
|
|
{ |
16 |
|
|
enum |
17 |
|
|
{ |
18 |
|
|
axis = _axis, |
19 |
|
|
dim = 3 |
20 |
|
|
}; |
21 |
|
|
|
22 |
|
|
typedef Eigen::Matrix<double, 3, 1> Vector3; |
23 |
|
|
|
24 |
|
|
template<typename V3_in, typename V3_out> |
25 |
|
|
inline static void |
26 |
|
|
cross(const Eigen::MatrixBase<V3_in> & vin, const Eigen::MatrixBase<V3_out> & vout); |
27 |
|
|
|
28 |
|
|
template<typename V3> |
29 |
|
2208 |
static typename PINOCCHIO_EIGEN_PLAIN_TYPE(V3) cross(const Eigen::MatrixBase<V3> & vin) |
30 |
|
|
{ |
31 |
|
2208 |
typename PINOCCHIO_EIGEN_PLAIN_TYPE(V3) res; |
32 |
|
2208 |
cross(vin, res); |
33 |
|
2208 |
return res; |
34 |
|
|
} |
35 |
|
|
|
36 |
|
|
template<typename Scalar, typename V3_in, typename V3_out> |
37 |
|
|
inline static void alphaCross( |
38 |
|
|
const Scalar & s, |
39 |
|
|
const Eigen::MatrixBase<V3_in> & vin, |
40 |
|
|
const Eigen::MatrixBase<V3_out> & vout); |
41 |
|
|
|
42 |
|
|
template<typename Scalar, typename V3> |
43 |
|
|
static typename PINOCCHIO_EIGEN_PLAIN_TYPE(V3) |
44 |
|
✗ |
alphaCross(const Scalar & s, const Eigen::MatrixBase<V3> & vin) |
45 |
|
|
{ |
46 |
|
✗ |
typename PINOCCHIO_EIGEN_PLAIN_TYPE(V3) res; |
47 |
|
✗ |
alphaCross(s, vin, res); |
48 |
|
✗ |
return res; |
49 |
|
|
} |
50 |
|
|
|
51 |
|
|
template<typename Scalar> |
52 |
|
✗ |
Eigen::Matrix<Scalar, dim, 1> operator*(const Scalar & s) const |
53 |
|
|
{ |
54 |
|
|
typedef Eigen::Matrix<Scalar, dim, 1> ReturnType; |
55 |
|
✗ |
ReturnType res; |
56 |
|
✗ |
for (Eigen::DenseIndex i = 0; i < dim; ++i) |
57 |
|
✗ |
res[i] = i == axis ? s : Scalar(0); |
58 |
|
|
|
59 |
|
✗ |
return res; |
60 |
|
|
} |
61 |
|
|
|
62 |
|
|
template<typename Scalar> |
63 |
|
✗ |
friend inline Eigen::Matrix<Scalar, dim, 1> operator*(const Scalar & s, const CartesianAxis &) |
64 |
|
|
{ |
65 |
|
✗ |
return CartesianAxis() * s; |
66 |
|
|
} |
67 |
|
|
|
68 |
|
|
template<typename Vector3Like> |
69 |
|
|
static void setTo(const Eigen::MatrixBase<Vector3Like> v3) |
70 |
|
|
{ |
71 |
|
|
Vector3Like & v3_ = PINOCCHIO_EIGEN_CONST_CAST(Vector3Like, v3); |
72 |
|
|
typedef typename Vector3Like::Scalar Scalar; |
73 |
|
|
|
74 |
|
|
for (Eigen::DenseIndex i = 0; i < dim; ++i) |
75 |
|
|
v3_[i] = i == axis ? Scalar(1) : Scalar(0); |
76 |
|
|
} |
77 |
|
|
|
78 |
|
|
template<typename Scalar> |
79 |
|
60 |
static Eigen::Matrix<Scalar, 3, 1> vector() |
80 |
|
|
{ |
81 |
|
|
typedef Eigen::Matrix<Scalar, 3, 1> Vector3; |
82 |
0/2
✗ Branch 2 not taken.
✗ Branch 3 not taken.
|
60 |
return Vector3::Unit(axis); |
83 |
|
|
} |
84 |
|
|
|
85 |
|
✗ |
static Vector3 vector() |
86 |
|
|
{ |
87 |
|
✗ |
return vector<double>(); |
88 |
|
|
} |
89 |
|
|
|
90 |
|
|
}; // struct CartesianAxis |
91 |
|
|
|
92 |
|
|
template<> |
93 |
|
|
template<typename V3_in, typename V3_out> |
94 |
|
812 |
inline void CartesianAxis<0>::cross( |
95 |
|
|
const Eigen::MatrixBase<V3_in> & vin, const Eigen::MatrixBase<V3_out> & vout) |
96 |
|
|
{ |
97 |
|
|
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_in, 3) |
98 |
|
|
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_out, 3) |
99 |
|
812 |
V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out, vout); |
100 |
|
812 |
vout_[0] = 0.; |
101 |
|
812 |
vout_[1] = -vin[2]; |
102 |
|
812 |
vout_[2] = vin[1]; |
103 |
|
812 |
} |
104 |
|
|
|
105 |
|
|
template<> |
106 |
|
|
template<typename V3_in, typename V3_out> |
107 |
|
1200 |
inline void CartesianAxis<1>::cross( |
108 |
|
|
const Eigen::MatrixBase<V3_in> & vin, const Eigen::MatrixBase<V3_out> & vout) |
109 |
|
|
{ |
110 |
|
|
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_in, 3) |
111 |
|
|
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_out, 3) |
112 |
|
1200 |
V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out, vout); |
113 |
|
1200 |
vout_[0] = vin[2]; |
114 |
|
1200 |
vout_[1] = 0.; |
115 |
|
1200 |
vout_[2] = -vin[0]; |
116 |
|
1200 |
} |
117 |
|
|
|
118 |
|
|
template<> |
119 |
|
|
template<typename V3_in, typename V3_out> |
120 |
|
422 |
inline void CartesianAxis<2>::cross( |
121 |
|
|
const Eigen::MatrixBase<V3_in> & vin, const Eigen::MatrixBase<V3_out> & vout) |
122 |
|
|
{ |
123 |
|
|
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_in, 3) |
124 |
|
|
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_out, 3) |
125 |
|
422 |
V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out, vout); |
126 |
|
422 |
vout_[0] = -vin[1]; |
127 |
|
422 |
vout_[1] = vin[0]; |
128 |
|
422 |
vout_[2] = 0.; |
129 |
|
422 |
} |
130 |
|
|
|
131 |
|
|
template<> |
132 |
|
|
template<typename Scalar, typename V3_in, typename V3_out> |
133 |
|
56460 |
inline void CartesianAxis<0>::alphaCross( |
134 |
|
|
const Scalar & s, const Eigen::MatrixBase<V3_in> & vin, const Eigen::MatrixBase<V3_out> & vout) |
135 |
|
|
{ |
136 |
|
|
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_in, 3) |
137 |
|
|
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_out, 3) |
138 |
|
56460 |
V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out, vout); |
139 |
|
56460 |
vout_[0] = 0.; |
140 |
|
56460 |
vout_[1] = -s * vin[2]; |
141 |
|
56460 |
vout_[2] = s * vin[1]; |
142 |
|
|
} |
143 |
|
|
|
144 |
|
|
template<> |
145 |
|
|
template<typename Scalar, typename V3_in, typename V3_out> |
146 |
|
91652 |
inline void CartesianAxis<1>::alphaCross( |
147 |
|
|
const Scalar & s, const Eigen::MatrixBase<V3_in> & vin, const Eigen::MatrixBase<V3_out> & vout) |
148 |
|
|
{ |
149 |
|
|
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_in, 3) |
150 |
|
|
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_out, 3) |
151 |
|
91652 |
V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out, vout); |
152 |
|
91652 |
vout_[0] = s * vin[2]; |
153 |
|
91652 |
vout_[1] = 0.; |
154 |
|
91652 |
vout_[2] = -s * vin[0]; |
155 |
|
|
} |
156 |
|
|
|
157 |
|
|
template<> |
158 |
|
|
template<typename Scalar, typename V3_in, typename V3_out> |
159 |
|
35248 |
inline void CartesianAxis<2>::alphaCross( |
160 |
|
|
const Scalar & s, const Eigen::MatrixBase<V3_in> & vin, const Eigen::MatrixBase<V3_out> & vout) |
161 |
|
|
{ |
162 |
|
|
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_in, 3) |
163 |
|
|
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_out, 3) |
164 |
|
35248 |
V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out, vout); |
165 |
|
35248 |
vout_[0] = -s * vin[1]; |
166 |
|
35248 |
vout_[1] = s * vin[0]; |
167 |
|
35248 |
vout_[2] = 0.; |
168 |
|
|
} |
169 |
|
|
|
170 |
|
|
typedef CartesianAxis<0> XAxis; |
171 |
|
|
typedef XAxis AxisX; |
172 |
|
|
|
173 |
|
|
typedef CartesianAxis<1> YAxis; |
174 |
|
|
typedef YAxis AxisY; |
175 |
|
|
|
176 |
|
|
typedef CartesianAxis<2> ZAxis; |
177 |
|
|
typedef ZAxis AxisZ; |
178 |
|
|
|
179 |
|
|
} // namespace pinocchio |
180 |
|
|
|
181 |
|
|
#endif // __pinocchio_cartesian_axis_hpp__ |
182 |
|
|
|