Directory: | ./ |
---|---|
File: | include/ndcurves/so3_linear.h |
Date: | 2025-03-05 17:18:30 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 129 | 136 | 94.9% |
Branches: | 127 | 234 | 54.3% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | #ifndef _STRUCT_SO3_LINEAR_H | ||
2 | #define _STRUCT_SO3_LINEAR_H | ||
3 | |||
4 | #include <Eigen/Geometry> | ||
5 | #include <boost/math/constants/constants.hpp> | ||
6 | #include <boost/serialization/split_free.hpp> | ||
7 | #include <boost/serialization/vector.hpp> | ||
8 | |||
9 | #include "MathDefs.h" | ||
10 | #include "constant_curve.h" | ||
11 | #include "curve_abc.h" | ||
12 | |||
13 | namespace ndcurves { | ||
14 | |||
15 | /// \class SO3Linear. | ||
16 | /// \brief Represents a linear interpolation in SO3, using the slerp method | ||
17 | /// provided by Eigen::Quaternion | ||
18 | /// | ||
19 | /// | ||
20 | template <typename Time = double, typename Numeric = Time, bool Safe = false> | ||
21 | struct SO3Linear : public curve_abc<Time, Numeric, Safe, matrix3_t, point3_t> { | ||
22 | typedef Numeric Scalar; | ||
23 | typedef matrix3_t point_t; | ||
24 | typedef point3_t point_derivate_t; | ||
25 | typedef Eigen::Quaternion<Scalar> quaternion_t; | ||
26 | typedef Time time_t; | ||
27 | typedef curve_abc<Time, Numeric, Safe, point_t, point_derivate_t> curve_abc_t; | ||
28 | typedef constant_curve<Time, Numeric, Safe, point_derivate_t> | ||
29 | curve_derivate_t; | ||
30 | typedef SO3Linear<Time, Numeric, Safe> SO3Linear_t; | ||
31 | |||
32 | public: | ||
33 | /* Constructors - destructors */ | ||
34 | /// \brief Empty constructor. Curve obtained this way can not perform other | ||
35 | /// class functions. | ||
36 | /// | ||
37 | 29 | SO3Linear() | |
38 | : curve_abc_t(), | ||
39 | 29 | dim_(3), | |
40 | 29 | init_rot_(), | |
41 |
1/2✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
|
29 | end_rot_(), |
42 |
1/2✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
|
29 | angular_vel_(), |
43 | 29 | T_min_(0), | |
44 |
1/2✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
|
58 | T_max_(0) {} |
45 | |||
46 | /// \brief constructor with initial and final rotation and time bounds | ||
47 | 9 | SO3Linear(const quaternion_t& init_rot, const quaternion_t& end_rot, | |
48 | const time_t t_min, const time_t t_max) | ||
49 | : curve_abc_t(), | ||
50 | 9 | dim_(3), | |
51 | 9 | init_rot_(init_rot), | |
52 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
9 | end_rot_(end_rot), |
53 |
3/6✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
|
9 | angular_vel_(computeAngularVelocity(init_rot.toRotationMatrix(), |
54 | end_rot.toRotationMatrix(), t_min, | ||
55 | t_max)), | ||
56 | 9 | T_min_(t_min), | |
57 |
1/2✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
|
9 | T_max_(t_max) { |
58 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
9 | safe_check(); |
59 | 9 | } | |
60 | |||
61 | /// \brief constructor with initial and final rotation expressed as rotation | ||
62 | /// matrix and time bounds | ||
63 | 38 | SO3Linear(const matrix3_t& init_rot, const matrix3_t& end_rot, | |
64 | const time_t t_min, const time_t t_max) | ||
65 | : curve_abc_t(), | ||
66 | 38 | dim_(3), | |
67 | 38 | init_rot_(quaternion_t(init_rot)), | |
68 |
1/2✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
|
38 | end_rot_(quaternion_t(end_rot)), |
69 |
1/2✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
|
38 | angular_vel_(computeAngularVelocity(init_rot, end_rot, t_min, t_max)), |
70 | 38 | T_min_(t_min), | |
71 |
1/2✓ Branch 2 taken 38 times.
✗ Branch 3 not taken.
|
38 | T_max_(t_max) { |
72 |
2/2✓ Branch 1 taken 37 times.
✓ Branch 2 taken 1 times.
|
38 | safe_check(); |
73 | 38 | } | |
74 | |||
75 | /// \brief constructor with initial and final rotation, time bounds are set to | ||
76 | /// [0;1] | ||
77 | SO3Linear(const quaternion_t& init_rot, const quaternion_t& end_rot) | ||
78 | : curve_abc_t(), | ||
79 | dim_(3), | ||
80 | init_rot_(init_rot), | ||
81 | end_rot_(end_rot), | ||
82 | angular_vel_(computeAngularVelocity( | ||
83 | init_rot.toRotationMatrix(), end_rot.toRotationMatrix(), 0., 1.)), | ||
84 | T_min_(0.), | ||
85 | T_max_(1.) { | ||
86 | safe_check(); | ||
87 | } | ||
88 | |||
89 | /// \brief constructor with initial and final rotation expressed as rotation | ||
90 | /// matrix, time bounds are set to [0;1] | ||
91 | SO3Linear(const matrix3_t& init_rot, const matrix3_t& end_rot) | ||
92 | : curve_abc_t(), | ||
93 | dim_(3), | ||
94 | init_rot_(quaternion_t(init_rot)), | ||
95 | end_rot_(quaternion_t(end_rot)), | ||
96 | angular_vel_(computeAngularVelocity(init_rot, end_rot, 0., 1.)), | ||
97 | T_min_(0.), | ||
98 | T_max_(1.) { | ||
99 | safe_check(); | ||
100 | } | ||
101 | |||
102 | /// \brief Destructor | ||
103 | 282 | virtual ~SO3Linear() {} | |
104 | |||
105 | 1 | SO3Linear(const SO3Linear& other) | |
106 | 1 | : dim_(other.dim_), | |
107 | 1 | init_rot_(other.init_rot_), | |
108 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | end_rot_(other.end_rot_), |
109 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | angular_vel_(other.angular_vel_), |
110 | 1 | T_min_(other.T_min_), | |
111 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | T_max_(other.T_max_) {} |
112 | |||
113 | 47 | point3_t computeAngularVelocity(const matrix3_t& init_rot, | |
114 | const matrix3_t& end_rot, const double t_min, | ||
115 | const double t_max) { | ||
116 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 47 times.
|
47 | if (t_min == t_max) { |
117 | ✗ | return point3_t::Zero(); | |
118 | } else { | ||
119 |
6/12✓ Branch 1 taken 47 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 47 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 47 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 47 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 47 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 47 times.
✗ Branch 17 not taken.
|
47 | return log3(init_rot.transpose() * end_rot) / (t_max - t_min); |
120 | } | ||
121 | } | ||
122 | |||
123 | 11942 | quaternion_t computeAsQuaternion(const time_t t) const { | |
124 |
6/6✓ Branch 0 taken 11939 times.
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 11937 times.
✓ Branch 4 taken 5 times.
✓ Branch 5 taken 11937 times.
|
11942 | if (Safe & !(T_min_ <= t && t <= T_max_)) { |
125 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
5 | throw std::invalid_argument( |
126 | "can't evaluate bezier curve, time t is out of range"); // TODO | ||
127 | } | ||
128 |
3/4✓ Branch 0 taken 50 times.
✓ Branch 1 taken 11887 times.
✓ Branch 3 taken 50 times.
✗ Branch 4 not taken.
|
11937 | if (t >= T_max_) return end_rot_; |
129 |
3/4✓ Branch 0 taken 130 times.
✓ Branch 1 taken 11757 times.
✓ Branch 3 taken 130 times.
✗ Branch 4 not taken.
|
11887 | if (t <= T_min_) return init_rot_; |
130 | 11757 | Scalar u = (t - T_min_) / (T_max_ - T_min_); | |
131 |
1/2✓ Branch 1 taken 11757 times.
✗ Branch 2 not taken.
|
11757 | return init_rot_.slerp(u, end_rot_); |
132 | } | ||
133 | |||
134 | /// \brief Evaluation of the SO3Linear at time t using Eigen slerp. | ||
135 | /// \param t : time when to evaluate the spline. | ||
136 | /// \return \f$x(t)\f$ point corresponding on spline at time t. | ||
137 | 11936 | virtual point_t operator()(const time_t t) const { | |
138 |
1/2✓ Branch 2 taken 11931 times.
✗ Branch 3 not taken.
|
11936 | return computeAsQuaternion(t).toRotationMatrix(); |
139 | } | ||
140 | |||
141 | /** | ||
142 | * @brief isApprox check if other and *this are approximately equals. | ||
143 | * Only two curves of the same class can be approximately equals, for | ||
144 | * comparison between different type of curves see isEquivalent | ||
145 | * @param other the other curve to check | ||
146 | * @param prec the precision threshold, default | ||
147 | * Eigen::NumTraits<Numeric>::dummy_precision() | ||
148 | * @return true is the two curves are approximately equals | ||
149 | */ | ||
150 | 13 | bool isApprox( | |
151 | const SO3Linear_t& other, | ||
152 | const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const { | ||
153 |
2/4✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
|
26 | return ndcurves::isApprox<Numeric>(T_min_, other.min()) && |
154 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
13 | ndcurves::isApprox<Numeric>(T_max_, other.max()) && |
155 |
2/4✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
|
13 | dim_ == other.dim() && |
156 |
3/6✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 13 times.
✗ Branch 7 not taken.
|
26 | init_rot_.toRotationMatrix().isApprox( |
157 |
2/4✓ Branch 0 taken 13 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
|
39 | other.init_rot_.toRotationMatrix(), prec) && |
158 |
4/6✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 10 times.
✓ Branch 7 taken 3 times.
|
26 | end_rot_.toRotationMatrix().isApprox( |
159 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
26 | other.end_rot_.toRotationMatrix(), prec); |
160 | } | ||
161 | |||
162 | 8 | virtual bool isApprox( | |
163 | const curve_abc_t* other, | ||
164 | const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const { | ||
165 |
1/2✓ Branch 0 taken 8 times.
✗ Branch 1 not taken.
|
8 | const SO3Linear_t* other_cast = dynamic_cast<const SO3Linear_t*>(other); |
166 |
1/2✓ Branch 0 taken 8 times.
✗ Branch 1 not taken.
|
8 | if (other_cast) |
167 | 8 | return isApprox(*other_cast, prec); | |
168 | else | ||
169 | ✗ | return false; | |
170 | } | ||
171 | |||
172 | 5 | virtual bool operator==(const SO3Linear_t& other) const { | |
173 | 5 | return isApprox(other); | |
174 | } | ||
175 | |||
176 | 3 | virtual bool operator!=(const SO3Linear_t& other) const { | |
177 | 3 | return !(*this == other); | |
178 | } | ||
179 | |||
180 | /// \brief Evaluation of the derivative of order N of spline at time t. | ||
181 | /// \param t : the time when to evaluate the spline. | ||
182 | /// \param order : order of derivative. | ||
183 | /// \return \f$\frac{d^Nx(t)}{dt^N}\f$ point corresponding on derivative | ||
184 | /// spline at time t. | ||
185 | 2046 | virtual point_derivate_t derivate(const time_t t, | |
186 | const std::size_t order) const { | ||
187 |
4/4✓ Branch 0 taken 2045 times.
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 2044 times.
|
2046 | if ((t < T_min_ || t > T_max_) && Safe) { |
188 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
2 | throw std::invalid_argument( |
189 | "error in SO3_linear : time t to evaluate derivative should be in " | ||
190 | "range [Tmin, Tmax] of the curve"); | ||
191 | } | ||
192 |
4/6✓ Branch 0 taken 1160 times.
✓ Branch 1 taken 884 times.
✓ Branch 2 taken 1160 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1160 times.
|
2044 | if (order > 1 || t > T_max_ || t < T_min_) { |
193 |
1/2✓ Branch 2 taken 884 times.
✗ Branch 3 not taken.
|
884 | return point3_t::Zero(3); |
194 |
2/2✓ Branch 0 taken 1155 times.
✓ Branch 1 taken 5 times.
|
1160 | } else if (order == 1) { |
195 | 1155 | return angular_vel_; | |
196 | } else { | ||
197 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
5 | throw std::invalid_argument("Order must be > 0 "); |
198 | } | ||
199 | } | ||
200 | |||
201 | 2 | curve_derivate_t compute_derivate(const std::size_t order) const { | |
202 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
2 | return curve_derivate_t(derivate(T_min_, order), T_min_, T_max_); |
203 | } | ||
204 | |||
205 | /// \brief Compute the derived curve at order N. | ||
206 | /// \param order : order of derivative. | ||
207 | /// \return A pointer to \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the | ||
208 | /// curve. | ||
209 | ✗ | curve_derivate_t* compute_derivate_ptr(const std::size_t order) const { | |
210 | ✗ | return new curve_derivate_t(compute_derivate(order)); | |
211 | } | ||
212 | |||
213 | /*Helpers*/ | ||
214 | /// \brief Get dimension of curve. | ||
215 | /// \return dimension of curve. | ||
216 | 13 | std::size_t virtual dim() const { return dim_; }; | |
217 | /// \brief Get the minimum time for which the curve is defined | ||
218 | /// \return \f$t_{min}\f$ lower bound of time range. | ||
219 | 39 | time_t min() const { return T_min_; } | |
220 | /// \brief Get the maximum time for which the curve is defined. | ||
221 | /// \return \f$t_{max}\f$ upper bound of time range. | ||
222 | 37 | time_t max() const { return T_max_; } | |
223 | /// \brief Get the degree of the curve. | ||
224 | /// \return \f$degree\f$, the degree of the curve. | ||
225 | ✗ | virtual std::size_t degree() const { return 1; } | |
226 | 29 | matrix3_t getInitRotation() const { return init_rot_.toRotationMatrix(); } | |
227 | 29 | matrix3_t getEndRotation() const { return end_rot_.toRotationMatrix(); } | |
228 | matrix3_t getInitRotation() { return init_rot_.toRotationMatrix(); } | ||
229 | matrix3_t getEndRotation() { return end_rot_.toRotationMatrix(); } | ||
230 | |||
231 | /*Helpers*/ | ||
232 | |||
233 | /*Attributes*/ | ||
234 | std::size_t dim_; // const | ||
235 | quaternion_t init_rot_, end_rot_; | ||
236 | point3_t angular_vel_; // const | ||
237 | time_t T_min_, T_max_; // const | ||
238 | /*Attributes*/ | ||
239 | |||
240 | // Serialization of the class | ||
241 | friend class boost::serialization::access; | ||
242 | |||
243 | template <class Archive> | ||
244 | 58 | void load(Archive& ar, const unsigned int version) { | |
245 | if (version) { | ||
246 | // Do something depending on version ? | ||
247 | } | ||
248 |
2/4✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 29 times.
✗ Branch 6 not taken.
|
58 | ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t); |
249 |
1/2✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
|
58 | ar >> boost::serialization::make_nvp("dim", dim_); |
250 |
2/4✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29 times.
✗ Branch 5 not taken.
|
58 | matrix3_t init, end; |
251 |
1/2✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
|
58 | ar >> boost::serialization::make_nvp("init_rotation", init); |
252 |
1/2✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
|
58 | ar >> boost::serialization::make_nvp("end_rotation", end); |
253 |
1/2✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
|
58 | init_rot_ = quaternion_t(init); |
254 |
1/2✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
|
58 | end_rot_ = quaternion_t(end); |
255 |
1/2✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
|
58 | ar >> boost::serialization::make_nvp("angular_vel", angular_vel_); |
256 |
1/2✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
|
58 | ar >> boost::serialization::make_nvp("T_min", T_min_); |
257 |
1/2✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
|
58 | ar >> boost::serialization::make_nvp("T_max", T_max_); |
258 | 58 | } | |
259 | |||
260 | template <class Archive> | ||
261 | 58 | void save(Archive& ar, const unsigned int version) const { | |
262 | if (version) { | ||
263 | // Do something depending on version ? | ||
264 | } | ||
265 |
2/4✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 29 times.
✗ Branch 6 not taken.
|
58 | ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t); |
266 |
1/2✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
|
58 | ar << boost::serialization::make_nvp("dim", dim_); |
267 |
1/2✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
|
58 | matrix3_t init_matrix(getInitRotation()); |
268 |
1/2✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
|
58 | matrix3_t end_matrix(getEndRotation()); |
269 |
1/2✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
|
58 | ar << boost::serialization::make_nvp("init_rotation", init_matrix); |
270 |
1/2✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
|
58 | ar << boost::serialization::make_nvp("end_rotation", end_matrix); |
271 |
1/2✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
|
58 | ar << boost::serialization::make_nvp("angular_vel", angular_vel_); |
272 |
1/2✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
|
58 | ar << boost::serialization::make_nvp("T_min", T_min_); |
273 |
1/2✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
|
58 | ar << boost::serialization::make_nvp("T_max", T_max_); |
274 | 58 | } | |
275 | |||
276 | 116 | BOOST_SERIALIZATION_SPLIT_MEMBER() | |
277 | |||
278 | /// \brief Log: SO3 -> so3. | ||
279 | /// | ||
280 | /// Pseudo-inverse of log from \f$ SO3 -> { v \in so3, ||v|| \le pi } \f$. | ||
281 | /// | ||
282 | /// Code from pinocchio. | ||
283 | /// | ||
284 | /// \param[in] R the rotation matrix. /// | ||
285 | /// \return The angular velocity vector associated to the rotation matrix. | ||
286 | /// | ||
287 | 47 | point3_t log3(const matrix3_t& R) { | |
288 | Scalar theta; | ||
289 | static const Scalar PI_value = boost::math::constants::pi<Scalar>(); | ||
290 | |||
291 | 47 | point3_t res; | |
292 | 47 | const Scalar tr = R.trace(); | |
293 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 46 times.
|
47 | if (tr > Scalar(3)) |
294 | 1 | theta = 0; // acos((3-1)/2) | |
295 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 46 times.
|
46 | else if (tr < Scalar(-1)) |
296 | ✗ | theta = PI_value; // acos((-1-1)/2) | |
297 | else | ||
298 | 46 | theta = acos((tr - Scalar(1)) / Scalar(2)); | |
299 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 47 times.
|
47 | if (!std::isfinite(theta)) { |
300 | ✗ | throw std::runtime_error("theta contains some NaN"); | |
301 | } | ||
302 | |||
303 | // From runs of hpp-constraints/tests/logarithm.cc: 1e-6 is too small. | ||
304 |
2/2✓ Branch 0 taken 44 times.
✓ Branch 1 taken 3 times.
|
47 | if (theta < PI_value - 1e-2) { |
305 | 44 | const Scalar t = | |
306 | ((theta > | ||
307 | 44 | std::pow(std::numeric_limits<Scalar>::epsilon(), | |
308 | Scalar(1) / | ||
309 | Scalar(4))) // precision of taylor serie of degree 3 | ||
310 |
2/2✓ Branch 0 taken 39 times.
✓ Branch 1 taken 5 times.
|
44 | ? theta / sin(theta) |
311 | : Scalar(1)) / | ||
312 | Scalar(2); | ||
313 | 44 | res(0) = t * (R(2, 1) - R(1, 2)); | |
314 | 44 | res(1) = t * (R(0, 2) - R(2, 0)); | |
315 | 44 | res(2) = t * (R(1, 0) - R(0, 1)); | |
316 | } else { | ||
317 | // 1e-2: A low value is not required since the computation is | ||
318 | // using explicit formula. However, the precision of this method | ||
319 | // is the square root of the precision with the antisymmetric | ||
320 | // method (Nominal case). | ||
321 | 3 | const Scalar cphi = cos(theta - PI_value); | |
322 | 3 | const Scalar beta = theta * theta / (Scalar(1) + cphi); | |
323 |
5/10✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3 times.
✗ Branch 14 not taken.
|
3 | point3_t tmp((R.diagonal().array() + cphi) * beta); |
324 |
4/8✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 3 times.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
|
6 | res(0) = (R(2, 1) > R(1, 2) ? Scalar(1) : Scalar(-1)) * |
325 |
3/6✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
|
3 | (tmp[0] > Scalar(0) ? sqrt(tmp[0]) : Scalar(0)); |
326 |
4/8✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 3 times.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
|
6 | res(1) = (R(0, 2) > R(2, 0) ? Scalar(1) : Scalar(-1)) * |
327 |
2/6✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
|
3 | (tmp[1] > Scalar(0) ? sqrt(tmp[1]) : Scalar(0)); |
328 |
4/8✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 3 times.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
|
6 | res(2) = (R(1, 0) > R(0, 1) ? Scalar(1) : Scalar(-1)) * |
329 |
2/6✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
|
3 | (tmp[2] > Scalar(0) ? sqrt(tmp[2]) : Scalar(0)); |
330 | } | ||
331 | |||
332 | 47 | return res; | |
333 | } | ||
334 | |||
335 | private: | ||
336 | 47 | void safe_check() { | |
337 | if (Safe) { | ||
338 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 46 times.
|
47 | if (T_min_ > T_max_) { |
339 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | throw std::invalid_argument("Tmin should be inferior to Tmax"); |
340 | } | ||
341 | } | ||
342 | 46 | } | |
343 | |||
344 | }; // struct SO3Linear | ||
345 | |||
346 | } // namespace ndcurves | ||
347 | |||
348 | DEFINE_CLASS_TEMPLATE_VERSION( | ||
349 | SINGLE_ARG(typename Time, typename Numeric, bool Safe), | ||
350 | SINGLE_ARG(ndcurves::SO3Linear<Time, Numeric, Safe>)) | ||
351 | |||
352 | #endif // _STRUCT_SO3_LINEAR_H | ||
353 |