1 /*
2 * Copyright (c) 2011-2021, The DART development contributors
3 * All rights reserved.
4 *
5 * The list of contributors can be found at:
6 * https://github.com/dartsim/dart/blob/master/LICENSE
7 *
8 * This file is provided under the following "BSD-style" License:
9 * Redistribution and use in source and binary forms, with or
10 * without modification, are permitted provided that the following
11 * conditions are met:
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26 * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30 * POSSIBILITY OF SUCH DAMAGE.
31 */
32
33 #include "dart/math/Geometry.hpp"
34
35 #include <algorithm>
36 #include <cassert>
37 #include <cmath>
38 #include <iomanip>
39 #include <iostream>
40 #include <vector>
41
42 #include "dart/common/Console.hpp"
43 #include "dart/math/Helpers.hpp"
44
45 #define DART_EPSILON 1e-6
46
47 namespace dart {
48 namespace math {
49
expToQuat(const Eigen::Vector3d & _v)50 Eigen::Quaterniond expToQuat(const Eigen::Vector3d& _v)
51 {
52 double mag = _v.norm();
53
54 if (mag > 1e-10)
55 {
56 Eigen::Quaterniond q(Eigen::AngleAxisd(mag, _v / mag));
57 return q;
58 }
59 else
60 {
61 Eigen::Quaterniond q(1, 0, 0, 0);
62 return q;
63 }
64 }
65
quatToExp(const Eigen::Quaterniond & _q)66 Eigen::Vector3d quatToExp(const Eigen::Quaterniond& _q)
67 {
68 Eigen::AngleAxisd aa(_q);
69 Eigen::Vector3d v = aa.axis();
70 return v * aa.angle();
71 }
72
73 // Reference:
74 // http://www.geometrictools.com/LibMathematics/Algebra/Wm5Matrix3.inl
matrixToEulerXYX(const Eigen::Matrix3d & _R)75 Eigen::Vector3d matrixToEulerXYX(const Eigen::Matrix3d& _R)
76 {
77 // +- -+ +- -+
78 // | r00 r01 r02 | | cy sy*sx1 sy*cx1 |
79 // | r10 r11 r12 | = | sy*sx0 cx0*cx1-cy*sx0*sx1 -cy*cx1*sx0-cx0*sx1 |
80 // | r20 r21 r22 | | -sy*cx0 cx1*sx0+cy*cx0*sx1 cy*cx0*cx1-sx0*sx1 |
81 // +- -+ +- -+
82
83 if (_R(0, 0) < 1.0)
84 {
85 if (_R(0, 0) > -1.0)
86 {
87 // y_angle = acos(r00)
88 // x0_angle = atan2(r10,-r20)
89 // x1_angle = atan2(r01,r02)
90 double y = acos(_R(0, 0));
91 double x0 = atan2(_R(1, 0), -_R(2, 0));
92 double x1 = atan2(_R(0, 1), _R(0, 2));
93 // return EA_UNIQUE;
94 return Eigen::Vector3d(x0, y, x1);
95 }
96 else
97 {
98 // Not a unique solution: x1_angle - x0_angle = atan2(-r12,r11)
99 double y = constantsd::pi();
100 double x0 = -atan2(-_R(1, 2), _R(1, 1));
101 double x1 = 0.0;
102 // return EA_NOT_UNIQUE_DIF;
103 return Eigen::Vector3d(x0, y, x1);
104 }
105 }
106 else
107 {
108 // Not a unique solution: x1_angle + x0_angle = atan2(-r12,r11)
109 double y = 0.0;
110 double x0 = -atan2(-_R(1, 2), _R(1, 1));
111 double x1 = 0.0;
112 // return EA_NOT_UNIQUE_SUM;
113 return Eigen::Vector3d(x0, y, x1);
114 }
115 }
116
matrixToEulerXYZ(const Eigen::Matrix3d & _R)117 Eigen::Vector3d matrixToEulerXYZ(const Eigen::Matrix3d& _R)
118 {
119 // +- -+ +- -+
120 // | r00 r01 r02 | | cy*cz -cy*sz sy |
121 // | r10 r11 r12 | = | cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx |
122 // | r20 r21 r22 | | -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy |
123 // +- -+ +- -+
124
125 double x, y, z;
126
127 if (_R(0, 2) > (1.0 - DART_EPSILON))
128 {
129 z = atan2(_R(1, 0), _R(1, 1));
130 y = constantsd::half_pi();
131 x = 0.0;
132 return Eigen::Vector3d(x, y, z);
133 }
134
135 if (_R(0, 2) < -(1.0 - DART_EPSILON))
136 {
137 z = atan2(_R(1, 0), _R(1, 1));
138 y = -constantsd::half_pi();
139 x = 0.0;
140 return Eigen::Vector3d(x, y, z);
141 }
142
143 z = -atan2(_R(0, 1), _R(0, 0));
144 y = asin(_R(0, 2));
145 x = -atan2(_R(1, 2), _R(2, 2));
146
147 // order of return is the order of input
148 return Eigen::Vector3d(x, y, z);
149 }
150
matrixToEulerZYX(const Eigen::Matrix3d & _R)151 Eigen::Vector3d matrixToEulerZYX(const Eigen::Matrix3d& _R)
152 {
153 double x, y, z;
154
155 if (_R(2, 0) > (1.0 - DART_EPSILON))
156 {
157 x = atan2(_R(0, 1), _R(0, 2));
158 y = -constantsd::half_pi();
159 z = 0.0;
160 return Eigen::Vector3d(z, y, x);
161 }
162
163 if (_R(2, 0) < -(1.0 - DART_EPSILON))
164 {
165 x = atan2(_R(0, 1), _R(0, 2));
166 y = constantsd::half_pi();
167 z = 0.0;
168 return Eigen::Vector3d(z, y, x);
169 }
170
171 x = atan2(_R(2, 1), _R(2, 2));
172 y = -asin(_R(2, 0));
173 z = atan2(_R(1, 0), _R(0, 0));
174
175 // order of return is the order of input
176 return Eigen::Vector3d(z, y, x);
177 }
178
matrixToEulerXZY(const Eigen::Matrix3d & _R)179 Eigen::Vector3d matrixToEulerXZY(const Eigen::Matrix3d& _R)
180 {
181 double x, y, z;
182
183 if (_R(0, 1) > (1.0 - DART_EPSILON))
184 {
185 y = atan2(_R(1, 2), _R(1, 0));
186 z = -constantsd::half_pi();
187 x = 0.0;
188 return Eigen::Vector3d(x, z, y);
189 }
190
191 if (_R(0, 1) < -(1.0 - DART_EPSILON))
192 {
193 y = atan2(_R(1, 2), _R(1, 0));
194 z = constantsd::half_pi();
195 x = 0.0;
196 return Eigen::Vector3d(x, z, y);
197 }
198
199 y = atan2(_R(0, 2), _R(0, 0));
200 z = -asin(_R(0, 1));
201 x = atan2(_R(2, 1), _R(1, 1));
202
203 // order of return is the order of input
204 return Eigen::Vector3d(x, z, y);
205 }
206
matrixToEulerYZX(const Eigen::Matrix3d & _R)207 Eigen::Vector3d matrixToEulerYZX(const Eigen::Matrix3d& _R)
208 {
209 double x, y, z;
210
211 if (_R(1, 0) > (1.0 - DART_EPSILON))
212 {
213 x = -atan2(_R(0, 2), _R(0, 1));
214 z = constantsd::half_pi();
215 y = 0.0;
216 return Eigen::Vector3d(y, z, x);
217 }
218
219 if (_R(1, 0) < -(1.0 - DART_EPSILON))
220 {
221 x = -atan2(_R(0, 2), _R(0, 1));
222 z = -constantsd::half_pi();
223 y = 0.0;
224 return Eigen::Vector3d(y, z, x);
225 }
226
227 x = -atan2(_R(1, 2), _R(1, 1));
228 z = asin(_R(1, 0));
229 y = -atan2(_R(2, 0), _R(0, 0));
230
231 // order of return is the order of input
232 return Eigen::Vector3d(y, z, x);
233 }
234
matrixToEulerZXY(const Eigen::Matrix3d & _R)235 Eigen::Vector3d matrixToEulerZXY(const Eigen::Matrix3d& _R)
236 {
237 double x, y, z;
238
239 if (_R(2, 1) > (1.0 - DART_EPSILON))
240 {
241 y = atan2(_R(0, 2), _R(0, 0));
242 x = constantsd::half_pi();
243 z = 0.0;
244 return Eigen::Vector3d(z, x, y);
245 }
246
247 if (_R(2, 1) < -(1.0 - DART_EPSILON))
248 {
249 y = atan2(_R(0, 2), _R(0, 0));
250 x = -constantsd::half_pi();
251 z = 0.0;
252 return Eigen::Vector3d(z, x, y);
253 }
254
255 y = -atan2(_R(2, 0), _R(2, 2));
256 x = asin(_R(2, 1));
257 z = -atan2(_R(0, 1), _R(1, 1));
258
259 // order of return is the order of input
260 return Eigen::Vector3d(z, x, y);
261 }
262
matrixToEulerYXZ(const Eigen::Matrix3d & _R)263 Eigen::Vector3d matrixToEulerYXZ(const Eigen::Matrix3d& _R)
264 {
265 double x, y, z;
266
267 if (_R(1, 2) > (1.0 - DART_EPSILON))
268 {
269 z = -atan2(_R(0, 1), _R(0, 0));
270 x = -constantsd::half_pi();
271 y = 0.0;
272 return Eigen::Vector3d(y, x, z);
273 }
274
275 if (_R(1, 2) < -(1.0 - DART_EPSILON))
276 {
277 z = -atan2(_R(0, 1), _R(0, 0));
278 x = constantsd::half_pi();
279 y = 0.0;
280 return Eigen::Vector3d(y, x, z);
281 }
282
283 z = atan2(_R(1, 0), _R(1, 1));
284 x = -asin(_R(1, 2));
285 y = atan2(_R(0, 2), _R(2, 2));
286
287 // order of return is the order of input
288 return Eigen::Vector3d(y, x, z);
289 }
290
291 // get the derivative of rotation matrix wrt el no.
quatDeriv(const Eigen::Quaterniond & _q,int _el)292 Eigen::Matrix3d quatDeriv(const Eigen::Quaterniond& _q, int _el)
293 {
294 Eigen::Matrix3d mat = Eigen::Matrix3d::Zero();
295
296 switch (_el)
297 {
298 case 0: // wrt w
299 mat(0, 0) = _q.w();
300 mat(1, 1) = _q.w();
301 mat(2, 2) = _q.w();
302 mat(0, 1) = -_q.z();
303 mat(1, 0) = _q.z();
304 mat(0, 2) = _q.y();
305 mat(2, 0) = -_q.y();
306 mat(1, 2) = -_q.x();
307 mat(2, 1) = _q.x();
308 break;
309 case 1: // wrt x
310 mat(0, 0) = _q.x();
311 mat(1, 1) = -_q.x();
312 mat(2, 2) = -_q.x();
313 mat(0, 1) = _q.y();
314 mat(1, 0) = _q.y();
315 mat(0, 2) = _q.z();
316 mat(2, 0) = _q.z();
317 mat(1, 2) = -_q.w();
318 mat(2, 1) = _q.w();
319 break;
320 case 2: // wrt y
321 mat(0, 0) = -_q.y();
322 mat(1, 1) = _q.y();
323 mat(2, 2) = -_q.y();
324 mat(0, 1) = _q.x();
325 mat(1, 0) = _q.x();
326 mat(0, 2) = _q.w();
327 mat(2, 0) = -_q.w();
328 mat(1, 2) = _q.z();
329 mat(2, 1) = _q.z();
330 break;
331 case 3: // wrt z
332 mat(0, 0) = -_q.z();
333 mat(1, 1) = -_q.z();
334 mat(2, 2) = _q.z();
335 mat(0, 1) = -_q.w();
336 mat(1, 0) = _q.w();
337 mat(0, 2) = _q.x();
338 mat(2, 0) = _q.x();
339 mat(1, 2) = _q.y();
340 mat(2, 1) = _q.y();
341 break;
342 default:
343 break;
344 }
345
346 return 2 * mat;
347 }
348
quatSecondDeriv(const Eigen::Quaterniond &,int _el1,int _el2)349 Eigen::Matrix3d quatSecondDeriv(
350 const Eigen::Quaterniond& /*_q*/, int _el1, int _el2)
351 {
352 Eigen::Matrix3d mat = Eigen::Matrix3d::Zero();
353
354 if (_el1 == _el2)
355 { // wrt same dof
356 switch (_el1)
357 {
358 case 0: // wrt w
359 mat(0, 0) = 1;
360 mat(1, 1) = 1;
361 mat(2, 2) = 1;
362 break;
363 case 1: // wrt x
364 mat(0, 0) = 1;
365 mat(1, 1) = -1;
366 mat(2, 2) = -1;
367 break;
368 case 2: // wrt y
369 mat(0, 0) = -1;
370 mat(1, 1) = 1;
371 mat(2, 2) = -1;
372 break;
373 case 3: // wrt z
374 mat(0, 0) = -1;
375 mat(1, 1) = -1;
376 mat(2, 2) = 1;
377 break;
378 }
379 }
380 else
381 { // wrt different dofs
382 // arrange in increasing order
383 if (_el1 > _el2)
384 {
385 int temp = _el2;
386 _el2 = _el1;
387 _el1 = temp;
388 }
389
390 switch (_el1)
391 {
392 case 0: // wrt w
393 switch (_el2)
394 {
395 case 1: // wrt x
396 mat(1, 2) = -1;
397 mat(2, 1) = 1;
398 break;
399 case 2: // wrt y
400 mat(0, 2) = 1;
401 mat(2, 0) = -1;
402 break;
403 case 3: // wrt z
404 mat(0, 1) = -1;
405 mat(1, 0) = 1;
406 break;
407 }
408 break;
409 case 1: // wrt x
410 switch (_el2)
411 {
412 case 2: // wrt y
413 mat(0, 1) = 1;
414 mat(1, 0) = 1;
415 break;
416 case 3: // wrt z
417 mat(0, 2) = 1;
418 mat(2, 0) = 1;
419 break;
420 }
421 break;
422 case 2: // wrt y
423 switch (_el2)
424 {
425 case 3: // wrt z
426 mat(1, 2) = 1;
427 mat(2, 1) = 1;
428 break;
429 }
430 break;
431 }
432 }
433
434 return 2 * mat;
435 }
436
rotatePoint(const Eigen::Quaterniond & _q,const Eigen::Vector3d & _pt)437 Eigen::Vector3d rotatePoint(
438 const Eigen::Quaterniond& _q, const Eigen::Vector3d& _pt)
439 {
440 Eigen::Quaterniond quat_pt(0, _pt[0], _pt[1], _pt[2]);
441 Eigen::Quaterniond qinv = _q.inverse();
442
443 Eigen::Quaterniond rot = _q * quat_pt * qinv;
444
445 // check below - assuming same format of point achieved
446 Eigen::Vector3d temp;
447 // VLOG(1) << "Point before: " << 0 << " "
448 // << pt.x << " " << pt.y << " " << pt.z << "\n";
449 // VLOG(1) << "Point after: "
450 // << rot.x << " " << rot.y << " " << rot.z << " " << rot.w << "\n";
451 temp[0] = rot.x();
452 temp[1] = rot.y();
453 temp[2] = rot.z();
454
455 // VLOG(1) << "Point after rotation: "
456 // << temp[0] << " " << temp[1] << " " << temp[2] << endl;
457 return temp;
458 }
459
rotatePoint(const Eigen::Quaterniond & _q,double _x,double _y,double _z)460 Eigen::Vector3d rotatePoint(
461 const Eigen::Quaterniond& _q, double _x, double _y, double _z)
462 {
463 Eigen::Vector3d pt(_x, _y, _z);
464 return rotatePoint(_q, pt);
465 }
466
467 // ----------- expmap computations -------------
468
469 #define EPSILON_EXPMAP_THETA 1.0e-3
470
expMapRot(const Eigen::Vector3d & _q)471 Eigen::Matrix3d expMapRot(const Eigen::Vector3d& _q)
472 {
473 double theta = _q.norm();
474
475 Eigen::Matrix3d R = Eigen::Matrix3d::Zero();
476 Eigen::Matrix3d qss = math::makeSkewSymmetric(_q);
477 Eigen::Matrix3d qss2 = qss * qss;
478
479 if (theta < EPSILON_EXPMAP_THETA)
480 R = Eigen::Matrix3d::Identity() + qss + 0.5 * qss2;
481 else
482 R = Eigen::Matrix3d::Identity() + (sin(theta) / theta) * qss
483 + ((1 - cos(theta)) / (theta * theta)) * qss2;
484
485 return R;
486 }
487
expMapJac(const Eigen::Vector3d & _q)488 Eigen::Matrix3d expMapJac(const Eigen::Vector3d& _q)
489 {
490 double theta = _q.norm();
491
492 Eigen::Matrix3d J = Eigen::Matrix3d::Zero();
493 Eigen::Matrix3d qss = math::makeSkewSymmetric(_q);
494 Eigen::Matrix3d qss2 = qss * qss;
495
496 if (theta < EPSILON_EXPMAP_THETA)
497 J = Eigen::Matrix3d::Identity() + 0.5 * qss + (1.0 / 6.0) * qss2;
498 else
499 J = Eigen::Matrix3d::Identity() + ((1 - cos(theta)) / (theta * theta)) * qss
500 + ((theta - sin(theta)) / (theta * theta * theta)) * qss2;
501
502 return J;
503 }
504
expMapJacDot(const Eigen::Vector3d & _q,const Eigen::Vector3d & _qdot)505 Eigen::Matrix3d expMapJacDot(
506 const Eigen::Vector3d& _q, const Eigen::Vector3d& _qdot)
507 {
508 double theta = _q.norm();
509
510 Eigen::Matrix3d Jdot = Eigen::Matrix3d::Zero();
511 Eigen::Matrix3d qss = math::makeSkewSymmetric(_q);
512 Eigen::Matrix3d qss2 = qss * qss;
513 Eigen::Matrix3d qdss = math::makeSkewSymmetric(_qdot);
514 double ttdot = _q.dot(_qdot); // theta*thetaDot
515 double st = sin(theta);
516 double ct = cos(theta);
517 double t2 = theta * theta;
518 double t3 = t2 * theta;
519 double t4 = t3 * theta;
520 double t5 = t4 * theta;
521
522 if (theta < EPSILON_EXPMAP_THETA)
523 {
524 Jdot = 0.5 * qdss + (1.0 / 6.0) * (qss * qdss + qdss * qss);
525 Jdot += (-1.0 / 12) * ttdot * qss + (-1.0 / 60) * ttdot * qss2;
526 }
527 else
528 {
529 Jdot = ((1 - ct) / t2) * qdss
530 + ((theta - st) / t3) * (qss * qdss + qdss * qss);
531 Jdot += ((theta * st + 2 * ct - 2) / t4) * ttdot * qss
532 + ((3 * st - theta * ct - 2 * theta) / t5) * ttdot * qss2;
533 }
534
535 return Jdot;
536 }
537
expMapJacDeriv(const Eigen::Vector3d & _q,int _qi)538 Eigen::Matrix3d expMapJacDeriv(const Eigen::Vector3d& _q, int _qi)
539 {
540 assert(_qi >= 0 && _qi <= 2);
541
542 Eigen::Vector3d qdot = Eigen::Vector3d::Zero();
543 qdot[_qi] = 1.0;
544 return expMapJacDot(_q, qdot);
545 }
546
547 // Vec3 AdInvTLinear(const SE3& T, const Vec3& v)
548 // {
549 // return Vec3(T(0,0)*v[0] + T(1,0)*v[1] + T(2,0)*v[2],
550 // T(0,1)*v[0] + T(1,1)*v[1] + T(2,1)*v[2],
551 // T(0,2)*v[0] + T(1,2)*v[1] + T(2,2)*v[2]);
552 // }
553
554 // Vec3 ad_Vec3_se3(const Vec3& s1, const se3& s2)
555 // {
556 // Vec3 ret;
557
558 // ret << s2[2]*s1[1] - s2[1]*s1[2],
559 // s2[0]*s1[2] - s2[2]*s1[0],
560 // s2[1]*s1[0] - s2[0]*s1[1];
561
562 // return ret;
563 // }
564
logMap(const Eigen::Matrix3d & _R)565 Eigen::Vector3d logMap(const Eigen::Matrix3d& _R)
566 {
567 //--------------------------------------------------------------------------
568 // T = (R, p) = exp([w, v]), t = ||w||
569 // v = beta*p + gamma*w + 1 / 2*cross(p, w)
570 // , beta = t*(1 + cos(t)) / (2*sin(t)), gamma = <w, p>*(1 - beta) / t^2
571 //--------------------------------------------------------------------------
572 // double theta =
573 // std::acos(
574 // std::max(
575 // std::min(0.5 * (_R(0, 0) + _R(1, 1) + _R(2, 2) - 1.0), 1.0),
576 // -1.0));
577
578 // if (theta > constantsd::pi() - DART_EPSILON) {
579 // double delta = 0.5 + 0.125*(constantsd::pi() - theta)*(constantsd::pi()
580 // - theta);
581
582 // return Eigen::Vector3d(
583 // _R(2, 1) > _R(1, 2) ? theta*sqrt(1.0 + (_R(0, 0) - 1.0)*delta) :
584 // -theta*sqrt(1.0 + (_R(0, 0) - 1.0)*delta),
585 // _R(0, 2) > _R(2, 0) ? theta*sqrt(1.0 + (_R(1, 1) - 1.0)*delta) :
586 // -theta*sqrt(1.0 + (_R(1, 1) - 1.0)*delta),
587 // _R(1, 0) > _R(0, 1) ? theta*sqrt(1.0 + (_R(2, 2) - 1.0)*delta) :
588 // -theta*sqrt(1.0 + (_R(2, 2) - 1.0)*delta));
589 // } else {
590 // double alpha = 0.0;
591
592 // if (theta > DART_EPSILON)
593 // alpha = 0.5*theta / sin(theta);
594 // else
595 // alpha = 0.5 + constantsd::one_div_12()*theta*theta;
596
597 // return Eigen::Vector3d(alpha*(_R(2, 1) - _R(1, 2)),
598 // alpha*(_R(0, 2) - _R(2, 0)),
599 // alpha*(_R(1, 0) - _R(0, 1)));
600 // }
601
602 Eigen::AngleAxisd aa(_R);
603 return aa.angle() * aa.axis();
604 }
605
logMap(const Eigen::Isometry3d & _T)606 Eigen::Vector6d logMap(const Eigen::Isometry3d& _T)
607 {
608 //--------------------------------------------------------------------------
609 // T = (R, p) = exp([w, v]), t = ||w||
610 // v = beta*p + gamma*w + 1 / 2*cross(p, w)
611 // , beta = t*(1 + cos(t)) / (2*sin(t)), gamma = <w, p>*(1 - beta) / t^2
612 //--------------------------------------------------------------------------
613 double theta = std::acos(std::max(
614 std::min(0.5 * (_T(0, 0) + _T(1, 1) + _T(2, 2) - 1.0), 1.0), -1.0));
615 double beta;
616 double gamma;
617 Eigen::Vector6d ret;
618
619 if (theta > constantsd::pi() - DART_EPSILON)
620 {
621 const double c1 = 0.10132118364234; // 1 / pi^2
622 const double c2 = 0.01507440267955; // 1 / 4 / pi - 2 / pi^3
623 const double c3 = 0.00546765085347; // 3 / pi^4 - 1 / 4 / pi^2
624
625 double phi = constantsd::pi() - theta;
626 double delta = 0.5 + 0.125 * phi * phi;
627
628 double w[]
629 = {_T(2, 1) > _T(1, 2) ? theta * sqrt(1.0 + (_T(0, 0) - 1.0) * delta)
630 : -theta * sqrt(1.0 + (_T(0, 0) - 1.0) * delta),
631 _T(0, 2) > _T(2, 0) ? theta * sqrt(1.0 + (_T(1, 1) - 1.0) * delta)
632 : -theta * sqrt(1.0 + (_T(1, 1) - 1.0) * delta),
633 _T(1, 0) > _T(0, 1) ? theta * sqrt(1.0 + (_T(2, 2) - 1.0) * delta)
634 : -theta * sqrt(1.0 + (_T(2, 2) - 1.0) * delta)};
635
636 beta = 0.25 * theta * (constantsd::pi() - theta);
637 gamma = (w[0] * _T(0, 3) + w[1] * _T(1, 3) + w[2] * _T(2, 3))
638 * (c1 - c2 * phi + c3 * phi * phi);
639
640 ret << w[0], w[1], w[2],
641 beta * _T(0, 3) - 0.5 * (w[1] * _T(2, 3) - w[2] * _T(1, 3))
642 + gamma * w[0],
643 beta * _T(1, 3) - 0.5 * (w[2] * _T(0, 3) - w[0] * _T(2, 3))
644 + gamma * w[1],
645 beta * _T(2, 3) - 0.5 * (w[0] * _T(1, 3) - w[1] * _T(0, 3))
646 + gamma * w[2];
647 }
648 else
649 {
650 double alpha;
651 if (theta > DART_EPSILON)
652 {
653 alpha = 0.5 * theta / sin(theta);
654 beta = (1.0 + cos(theta)) * alpha;
655 gamma = (1.0 - beta) / theta / theta;
656 }
657 else
658 {
659 alpha = 0.5 + 1.0 / 12.0 * theta * theta;
660 beta = 1.0 - 1.0 / 12.0 * theta * theta;
661 gamma = 1.0 / 12.0 + 1.0 / 720.0 * theta * theta;
662 }
663
664 double w[] = {alpha * (_T(2, 1) - _T(1, 2)),
665 alpha * (_T(0, 2) - _T(2, 0)),
666 alpha * (_T(1, 0) - _T(0, 1))};
667 gamma *= w[0] * _T(0, 3) + w[1] * _T(1, 3) + w[2] * _T(2, 3);
668
669 ret << w[0], w[1], w[2],
670 beta * _T(0, 3) + 0.5 * (w[2] * _T(1, 3) - w[1] * _T(2, 3))
671 + gamma * w[0],
672 beta * _T(1, 3) + 0.5 * (w[0] * _T(2, 3) - w[2] * _T(0, 3))
673 + gamma * w[1],
674 beta * _T(2, 3) + 0.5 * (w[1] * _T(0, 3) - w[0] * _T(1, 3))
675 + gamma * w[2];
676 }
677
678 return ret;
679 }
680
681 // res = T * s * Inv(T)
AdT(const Eigen::Isometry3d & _T,const Eigen::Vector6d & _V)682 Eigen::Vector6d AdT(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _V)
683 {
684 //--------------------------------------------------------------------------
685 // w' = R*w
686 // v' = p x R*w + R*v
687 //--------------------------------------------------------------------------
688 Eigen::Vector6d res;
689 res.head<3>().noalias() = _T.linear() * _V.head<3>();
690 res.tail<3>().noalias()
691 = _T.linear() * _V.tail<3>() + _T.translation().cross(res.head<3>());
692 return res;
693 }
694
695 //==============================================================================
getAdTMatrix(const Eigen::Isometry3d & T)696 Eigen::Matrix6d getAdTMatrix(const Eigen::Isometry3d& T)
697 {
698 Eigen::Matrix6d AdT;
699
700 AdT.topLeftCorner<3, 3>() = T.linear();
701 AdT.topRightCorner<3, 3>().setZero();
702 AdT.bottomLeftCorner<3, 3>()
703 = makeSkewSymmetric(T.translation()) * T.linear();
704 AdT.bottomRightCorner<3, 3>() = T.linear();
705
706 return AdT;
707 }
708
AdR(const Eigen::Isometry3d & _T,const Eigen::Vector6d & _V)709 Eigen::Vector6d AdR(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _V)
710 {
711 //--------------------------------------------------------------------------
712 // w' = R*w
713 // v' = R*v
714 //--------------------------------------------------------------------------
715 Eigen::Vector6d res;
716 res.head<3>().noalias() = _T.linear() * _V.head<3>();
717 res.tail<3>().noalias() = _T.linear() * _V.tail<3>();
718 return res;
719 }
720
AdTAngular(const Eigen::Isometry3d & _T,const Eigen::Vector3d & _w)721 Eigen::Vector6d AdTAngular(
722 const Eigen::Isometry3d& _T, const Eigen::Vector3d& _w)
723 {
724 //--------------------------------------------------------------------------
725 // w' = R*w
726 // v' = p x R*w
727 //--------------------------------------------------------------------------
728 Eigen::Vector6d res;
729 res.head<3>().noalias() = _T.linear() * _w;
730 res.tail<3>() = _T.translation().cross(res.head<3>());
731 return res;
732 }
733
AdTLinear(const Eigen::Isometry3d & _T,const Eigen::Vector3d & _v)734 Eigen::Vector6d AdTLinear(
735 const Eigen::Isometry3d& _T, const Eigen::Vector3d& _v)
736 {
737 //--------------------------------------------------------------------------
738 // w' = 0
739 // v' = R*v
740 //--------------------------------------------------------------------------
741 Eigen::Vector6d res = Eigen::Vector6d::Zero();
742 res.tail<3>().noalias() = _T.linear() * _v;
743 return res;
744 }
745
746 // se3 AdP(const Vec3& p, const se3& s)
747 // {
748 // //--------------------------------------------------------------------------
749 // // w' = w
750 // // v' = p x w + v
751 // //--------------------------------------------------------------------------
752 // se3 ret;
753 // ret << s[0],
754 // s[1],
755 // s[2],
756 // p[1]*s[2] - p[2]*s[1] + s[3],
757 // p[2]*s[0] - p[0]*s[2] + s[4],
758 // p[0]*s[1] - p[1]*s[0] + s[5];
759
760 // return ret;
761 // }
762
763 // Jacobian AdRJac(const SE3& T, const Jacobian& J)
764 // {
765 // Jacobian AdTJ(6,J.cols());
766
767 // for (int i = 0; i < J.cols(); ++i)
768 // {
769 // AdTJ.col(i) = math::AdR(T, J.col(i));
770 // }
771
772 // return AdTJ;
773 // }
774
775 // re = Inv(T)*s*T
AdInvT(const Eigen::Isometry3d & _T,const Eigen::Vector6d & _V)776 Eigen::Vector6d AdInvT(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _V)
777 {
778 Eigen::Vector6d res;
779 res.head<3>().noalias() = _T.linear().transpose() * _V.head<3>();
780 res.tail<3>().noalias()
781 = _T.linear().transpose()
782 * (_V.tail<3>() + _V.head<3>().cross(_T.translation()));
783 return res;
784 }
785
786 // se3 AdInvR(const SE3& T, const se3& s)
787 // {
788 // se3 ret;
789
790 // ret << T(0,0)*s[0] + T(1,0)*s[1] + T(2,0)*s[2],
791 // T(0,1)*s[0] + T(1,1)*s[1] + T(2,1)*s[2],
792 // T(0,2)*s[0] + T(1,2)*s[1] + T(2,2)*s[2],
793 // T(0,0)*s[3] + T(1,0)*s[4] + T(2,0)*s[5],
794 // T(0,1)*s[3] + T(1,1)*s[4] + T(2,1)*s[5],
795 // T(0,2)*s[3] + T(1,2)*s[4] + T(2,2)*s[5];
796
797 // return ret;
798 // }
799
AdInvRLinear(const Eigen::Isometry3d & _T,const Eigen::Vector3d & _v)800 Eigen::Vector6d AdInvRLinear(
801 const Eigen::Isometry3d& _T, const Eigen::Vector3d& _v)
802 {
803 Eigen::Vector6d res = Eigen::Vector6d::Zero();
804 res.tail<3>().noalias() = _T.linear().transpose() * _v;
805 return res;
806 }
807
ad(const Eigen::Vector6d & _X,const Eigen::Vector6d & _Y)808 Eigen::Vector6d ad(const Eigen::Vector6d& _X, const Eigen::Vector6d& _Y)
809 {
810 //--------------------------------------------------------------------------
811 // ad(s1, s2) = | [w1] 0 | | w2 |
812 // | [v1] [w1] | | v2 |
813 //
814 // = | [w1]w2 |
815 // | [v1]w2 + [w1]v2 |
816 //--------------------------------------------------------------------------
817 Eigen::Vector6d res;
818 res.head<3>() = _X.head<3>().cross(_Y.head<3>());
819 res.tail<3>()
820 = _X.head<3>().cross(_Y.tail<3>()) + _X.tail<3>().cross(_Y.head<3>());
821 return res;
822 }
823
dAdT(const Eigen::Isometry3d & _T,const Eigen::Vector6d & _F)824 Eigen::Vector6d dAdT(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _F)
825 {
826 Eigen::Vector6d res;
827 res.head<3>().noalias()
828 = _T.linear().transpose()
829 * (_F.head<3>() + _F.tail<3>().cross(_T.translation()));
830 res.tail<3>().noalias() = _T.linear().transpose() * _F.tail<3>();
831 return res;
832 }
833
834 // dse3 dAdTLinear(const SE3& T, const Vec3& v)
835 // {
836 // dse3 ret;
837 // double tmp[3] = { - T(1,3)*v[2] + T(2,3)*v[1],
838 // - T(2,3)*v[0] + T(0,3)*v[2],
839 // - T(0,3)*v[1] + T(1,3)*v[0] };
840 // ret << T(0,0)*tmp[0] + T(1,0)*tmp[1] + T(2,0)*tmp[2],
841 // T(0,1)*tmp[0] + T(1,1)*tmp[1] + T(2,1)*tmp[2],
842 // T(0,2)*tmp[0] + T(1,2)*tmp[1] + T(2,2)*tmp[2],
843 // T(0,0)*v[0] + T(1,0)*v[1] + T(2,0)*v[2],
844 // T(0,1)*v[0] + T(1,1)*v[1] + T(2,1)*v[2],
845 // T(0,2)*v[0] + T(1,2)*v[1] + T(2,2)*v[2];
846
847 // return ret;
848 // }
849
dAdInvT(const Eigen::Isometry3d & _T,const Eigen::Vector6d & _F)850 Eigen::Vector6d dAdInvT(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _F)
851 {
852 Eigen::Vector6d res;
853 res.tail<3>().noalias() = _T.linear() * _F.tail<3>();
854 res.head<3>().noalias() = _T.linear() * _F.head<3>();
855 res.head<3>() += _T.translation().cross(res.tail<3>());
856 return res;
857 }
858
dAdInvR(const Eigen::Isometry3d & _T,const Eigen::Vector6d & _F)859 Eigen::Vector6d dAdInvR(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _F)
860 {
861 Eigen::Vector6d res;
862 res.head<3>().noalias() = _T.linear() * _F.head<3>();
863 res.tail<3>().noalias() = _T.linear() * _F.tail<3>();
864 return res;
865 }
866
867 // dse3 dAdInvPLinear(const Vec3& p, const Vec3& f)
868 // {
869 // dse3 ret;
870
871 // ret << p[1]*f[2] - p[2]*f[1],
872 // p[2]*f[0] - p[0]*f[2],
873 // p[0]*f[1] - p[1]*f[0],
874 // f[0],
875 // f[1],
876 // f[2];
877
878 // return ret;
879 // }
880
881 // Reference:
882 // http://www.geometrictools.com/LibMathematics/Algebra/Wm5Matrix3.inl
eulerXYXToMatrix(const Eigen::Vector3d & _angle)883 Eigen::Matrix3d eulerXYXToMatrix(const Eigen::Vector3d& _angle)
884 {
885 // +- -+ +- -+
886 // | r00 r01 r02 | | cy sy*sx1 sy*cx1 |
887 // | r10 r11 r12 | = | sy*sx0 cx0*cx1-cy*sx0*sx1 -cy*cx1*sx0-cx0*sx1 |
888 // | r20 r21 r22 | | -sy*cx0 cx1*sx0+cy*cx0*sx1 cy*cx0*cx1-sx0*sx1 |
889 // +- -+ +- -+
890
891 Eigen::Matrix3d ret;
892
893 double cx0 = cos(_angle(0));
894 double sx0 = sin(_angle(0));
895 double cy = cos(_angle(1));
896 double sy = sin(_angle(1));
897 double cx1 = cos(_angle(2));
898 double sx1 = sin(_angle(2));
899
900 ret(0, 0) = cy;
901 ret(1, 0) = sy * sx0;
902 ret(2, 0) = -sy * cx0;
903
904 ret(0, 1) = sy * sx1;
905 ret(1, 1) = cx0 * cx1 - cy * sx0 * sx1;
906 ret(2, 1) = cx1 * sx0 + cy * cx0 * sx1;
907
908 ret(0, 2) = sy * cx1;
909 ret(1, 2) = -cy * cx1 * sx0 - cx0 * sx1;
910 ret(2, 2) = cy * cx0 * cx1 - sx0 * sx1;
911
912 return ret;
913 }
914
eulerXYZToMatrix(const Eigen::Vector3d & _angle)915 Eigen::Matrix3d eulerXYZToMatrix(const Eigen::Vector3d& _angle)
916 {
917 // +- -+ +- -+
918 // | r00 r01 r02 | | cy*cz -cy*sz sy |
919 // | r10 r11 r12 | = | cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx |
920 // | r20 r21 r22 | | -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy |
921 // +- -+ +- -+
922
923 Eigen::Matrix3d ret;
924
925 double cx = cos(_angle[0]);
926 double sx = sin(_angle[0]);
927 double cy = cos(_angle[1]);
928 double sy = sin(_angle[1]);
929 double cz = cos(_angle[2]);
930 double sz = sin(_angle[2]);
931
932 ret(0, 0) = cy * cz;
933 ret(1, 0) = cx * sz + cz * sx * sy;
934 ret(2, 0) = sx * sz - cx * cz * sy;
935
936 ret(0, 1) = -cy * sz;
937 ret(1, 1) = cx * cz - sx * sy * sz;
938 ret(2, 1) = cz * sx + cx * sy * sz;
939
940 ret(0, 2) = sy;
941 ret(1, 2) = -cy * sx;
942 ret(2, 2) = cx * cy;
943
944 return ret;
945 }
946
eulerXZXToMatrix(const Eigen::Vector3d & _angle)947 Eigen::Matrix3d eulerXZXToMatrix(const Eigen::Vector3d& _angle)
948 {
949 // +- -+ +- -+
950 // | r00 r01 r02 | | cz -sz*cx1 sz*sx1 |
951 // | r10 r11 r12 | = | sz*cx0 cz*cx0*cx1-sx0*sx1 -cx1*sx0-cz*cx0*sx1 |
952 // | r20 r21 r22 | | sz*sx0 cz*cx1*sx0+cx0*sx1 cx0*cx1-cz*sx0*sx1 |
953 // +- -+ +- -+
954
955 Eigen::Matrix3d ret;
956
957 double cx0 = cos(_angle(0));
958 double sx0 = sin(_angle(0));
959 double cz = cos(_angle(1));
960 double sz = sin(_angle(1));
961 double cx1 = cos(_angle(2));
962 double sx1 = sin(_angle(2));
963
964 ret(0, 0) = cz;
965 ret(1, 0) = sz * cx0;
966 ret(2, 0) = sz * sx0;
967
968 ret(0, 1) = -sz * cx1;
969 ret(1, 1) = cz * cx0 * cx1 - sx0 * sx1;
970 ret(2, 1) = cz * cx1 * sx0 + cx0 * sx1;
971
972 ret(0, 2) = sz * sx1;
973 ret(1, 2) = -cx1 * sx0 - cz * cx0 * sx1;
974 ret(2, 2) = cx0 * cx1 - cz * sx0 * sx1;
975
976 return ret;
977 }
978
eulerXZYToMatrix(const Eigen::Vector3d & _angle)979 Eigen::Matrix3d eulerXZYToMatrix(const Eigen::Vector3d& _angle)
980 {
981 // +- -+ +- -+
982 // | r00 r01 r02 | | cy*cz -sz cz*sy |
983 // | r10 r11 r12 | = | sx*sy+cx*cy*sz cx*cz -cy*sx+cx*sy*sz |
984 // | r20 r21 r22 | | -cx*sy+cy*sx*sz cz*sx cx*cy+sx*sy*sz |
985 // +- -+ +- -+
986
987 Eigen::Matrix3d ret;
988
989 double cx = cos(_angle(0));
990 double sx = sin(_angle(0));
991 double cz = cos(_angle(1));
992 double sz = sin(_angle(1));
993 double cy = cos(_angle(2));
994 double sy = sin(_angle(2));
995
996 ret(0, 0) = cy * cz;
997 ret(1, 0) = sx * sy + cx * cy * sz;
998 ret(2, 0) = -cx * sy + cy * sx * sz;
999
1000 ret(0, 1) = -sz;
1001 ret(1, 1) = cx * cz;
1002 ret(2, 1) = cz * sx;
1003
1004 ret(0, 2) = cz * sy;
1005 ret(1, 2) = -cy * sx + cx * sy * sz;
1006 ret(2, 2) = cx * cy + sx * sy * sz;
1007
1008 return ret;
1009 }
1010
eulerYXYToMatrix(const Eigen::Vector3d & _angle)1011 Eigen::Matrix3d eulerYXYToMatrix(const Eigen::Vector3d& _angle)
1012 {
1013 // +- -+ +- -+
1014 // | r00 r01 r02 | | cy0*cy1-cx*sy0*sy1 sx*sy0 cx*cy1*sy0+cy0*sy1 |
1015 // | r10 r11 r12 | = | sx*sy1 cx -sx*cy1 |
1016 // | r20 r21 r22 | | -cy1*sy0-cx*cy0*sy1 sx*cy0 cx*cy0*cy1-sy0*sy1 |
1017 // +- -+ +- -+
1018
1019 Eigen::Matrix3d ret;
1020
1021 double cy0 = cos(_angle(0));
1022 double sy0 = sin(_angle(0));
1023 double cx = cos(_angle(1));
1024 double sx = sin(_angle(1));
1025 double cy1 = cos(_angle(2));
1026 double sy1 = sin(_angle(2));
1027
1028 ret(0, 0) = cy0 * cy1 - cx * sy0 * sy1;
1029 ret(1, 0) = sx * sy1;
1030 ret(2, 0) = -cy1 * sy0 - cx * cy0 * sy1;
1031
1032 ret(0, 1) = sx * sy0;
1033 ret(1, 1) = cx;
1034 ret(2, 1) = sx * cy0;
1035
1036 ret(0, 2) = cx * cy1 * sy0 + cy0 * sy1;
1037 ret(1, 2) = -sx * cy1;
1038 ret(2, 2) = cx * cy0 * cy1 - sy0 * sy1;
1039
1040 return ret;
1041 }
1042
eulerYXZToMatrix(const Eigen::Vector3d & _angle)1043 Eigen::Matrix3d eulerYXZToMatrix(const Eigen::Vector3d& _angle)
1044 {
1045 // +- -+ +- -+
1046 // | r00 r01 r02 | | cy*cz+sx*sy*sz cz*sx*sy-cy*sz cx*sy |
1047 // | r10 r11 r12 | = | cx*sz cx*cz -sx |
1048 // | r20 r21 r22 | | -cz*sy+cy*sx*sz cy*cz*sx+sy*sz cx*cy |
1049 // +- -+ +- -+
1050
1051 Eigen::Matrix3d ret;
1052
1053 double cy = cos(_angle(0));
1054 double sy = sin(_angle(0));
1055 double cx = cos(_angle(1));
1056 double sx = sin(_angle(1));
1057 double cz = cos(_angle(2));
1058 double sz = sin(_angle(2));
1059
1060 ret(0, 0) = cy * cz + sx * sy * sz;
1061 ret(0, 1) = cz * sx * sy - cy * sz;
1062 ret(0, 2) = cx * sy;
1063 ret(1, 0) = cx * sz;
1064 ret(1, 1) = cx * cz;
1065 ret(1, 2) = -sx;
1066 ret(2, 0) = -cz * sy + cy * sx * sz;
1067 ret(2, 1) = cy * cz * sx + sy * sz;
1068 ret(2, 2) = cx * cy;
1069
1070 return ret;
1071 }
1072
eulerYZXToMatrix(const Eigen::Vector3d & _angle)1073 Eigen::Matrix3d eulerYZXToMatrix(const Eigen::Vector3d& _angle)
1074 {
1075 // +- -+ +- -+
1076 // | r00 r01 r02 | | cy*cz sx*sy-cx*cy*sz cx*sy+cy*sx*sz |
1077 // | r10 r11 r12 | = | sz cx*cz -cz*sx |
1078 // | r20 r21 r22 | | -cz*sy cy*sx+cx*sy*sz cx*cy-sx*sy*sz |
1079 // +- -+ +- -+
1080
1081 Eigen::Matrix3d ret;
1082
1083 double cy = cos(_angle(0));
1084 double sy = sin(_angle(0));
1085 double cz = cos(_angle(1));
1086 double sz = sin(_angle(1));
1087 double cx = cos(_angle(2));
1088 double sx = sin(_angle(2));
1089
1090 ret(0, 0) = cy * cz;
1091 ret(0, 1) = sx * sy - cx * cy * sz;
1092 ret(0, 2) = cx * sy + cy * sx * sz;
1093 ret(1, 0) = sz;
1094 ret(1, 1) = cx * cz;
1095 ret(1, 2) = -cz * sx;
1096 ret(2, 0) = -cz * sy;
1097 ret(2, 1) = cy * sx + cx * sy * sz;
1098 ret(2, 2) = cx * cy - sx * sy * sz;
1099
1100 return ret;
1101 }
1102
eulerYZYToMatrix(const Eigen::Vector3d & _angle)1103 Eigen::Matrix3d eulerYZYToMatrix(const Eigen::Vector3d& _angle)
1104 {
1105 // +- -+ +- -+
1106 // | r00 r01 r02 | | cz*cy0*cy1-sy0*sy1 -sz*cy0 cy1*sy0+cz*cy0*sy1 |
1107 // | r10 r11 r12 | = | sz*cy1 cz sz*sy1 |
1108 // | r20 r21 r22 | | -cz*cy1*sy0-cy0*sy1 sz*sy0 cy0*cy1-cz*sy0*sy1 |
1109 // +- -+ +- -+
1110
1111 Eigen::Matrix3d ret;
1112
1113 double cy0 = cos(_angle(0));
1114 double sy0 = sin(_angle(0));
1115 double cz = cos(_angle(1));
1116 double sz = sin(_angle(1));
1117 double cy1 = cos(_angle(2));
1118 double sy1 = sin(_angle(2));
1119
1120 ret(0, 0) = cz * cy0 * cy1 - sy0 * sy1;
1121 ret(1, 0) = sz * cy1;
1122 ret(2, 0) = -cz * cy1 * sy0 - cy0 * sy1;
1123
1124 ret(0, 1) = -sz * cy0;
1125 ret(1, 1) = cz;
1126 ret(2, 1) = sz * sy0;
1127
1128 ret(0, 2) = cy1 * sy0 + cz * cy0 * sy1;
1129 ret(1, 2) = sz * sy1;
1130 ret(2, 2) = cy0 * cy1 - cz * sy0 * sy1;
1131
1132 return ret;
1133 }
1134
eulerZXYToMatrix(const Eigen::Vector3d & _angle)1135 Eigen::Matrix3d eulerZXYToMatrix(const Eigen::Vector3d& _angle)
1136 {
1137 // +- -+ +- -+
1138 // | r00 r01 r02 | | cy*cz-sx*sy*sz -cx*sz cz*sy+cy*sx*sz |
1139 // | r10 r11 r12 | = | cz*sx*sy+cy*sz cx*cz -cy*cz*sx+sy*sz |
1140 // | r20 r21 r22 | | -cx*sy sx cx*cy |
1141 // +- -+ +- -+
1142
1143 Eigen::Matrix3d ret;
1144
1145 double cz = cos(_angle(0));
1146 double sz = sin(_angle(0));
1147 double cx = cos(_angle(1));
1148 double sx = sin(_angle(1));
1149 double cy = cos(_angle(2));
1150 double sy = sin(_angle(2));
1151
1152 ret(0, 0) = cy * cz - sx * sy * sz;
1153 ret(1, 0) = cz * sx * sy + cy * sz;
1154 ret(2, 0) = -cx * sy;
1155
1156 ret(0, 1) = -cx * sz;
1157 ret(1, 1) = cx * cz;
1158 ret(2, 1) = sx;
1159
1160 ret(0, 2) = cz * sy + cy * sx * sz;
1161 ret(1, 2) = -cy * cz * sx + sy * sz;
1162 ret(2, 2) = cx * cy;
1163
1164 return ret;
1165 }
1166
eulerZYXToMatrix(const Eigen::Vector3d & _angle)1167 Eigen::Matrix3d eulerZYXToMatrix(const Eigen::Vector3d& _angle)
1168 {
1169 // +- -+ +- -+
1170 // | r00 r01 r02 | | cy*cz cz*sx*sy-cx*sz cx*cz*sy+sx*sz |
1171 // | r10 r11 r12 | = | cy*sz cx*cz+sx*sy*sz -cz*sx+cx*sy*sz |
1172 // | r20 r21 r22 | | -sy cy*sx cx*cy |
1173 // +- -+ +- -+
1174
1175 Eigen::Matrix3d ret;
1176
1177 double cz = cos(_angle[0]);
1178 double sz = sin(_angle[0]);
1179 double cy = cos(_angle[1]);
1180 double sy = sin(_angle[1]);
1181 double cx = cos(_angle[2]);
1182 double sx = sin(_angle[2]);
1183
1184 ret(0, 0) = cz * cy;
1185 ret(1, 0) = sz * cy;
1186 ret(2, 0) = -sy;
1187
1188 ret(0, 1) = cz * sy * sx - sz * cx;
1189 ret(1, 1) = sz * sy * sx + cz * cx;
1190 ret(2, 1) = cy * sx;
1191
1192 ret(0, 2) = cz * sy * cx + sz * sx;
1193 ret(1, 2) = sz * sy * cx - cz * sx;
1194 ret(2, 2) = cy * cx;
1195
1196 return ret;
1197 }
1198
eulerZXZToMatrix(const Eigen::Vector3d & _angle)1199 Eigen::Matrix3d eulerZXZToMatrix(const Eigen::Vector3d& _angle)
1200 {
1201 // +- -+ +- -+
1202 // | r00 r01 r02 | | cz0*cz1-cx*sz0*sz1 -cx*cz1*sz0-cz0*sz1 sx*sz0 |
1203 // | r10 r11 r12 | = | cz1*sz0+cx*cz0*sz1 cx*cz0*cz1-sz0*sz1 -sz*cz0 |
1204 // | r20 r21 r22 | | sx*sz1 sx*cz1 cx |
1205 // +- -+ +- -+
1206
1207 Eigen::Matrix3d ret;
1208
1209 double cz0 = cos(_angle(0));
1210 double sz0 = sin(_angle(0));
1211 double cx = cos(_angle(1));
1212 double sx = sin(_angle(1));
1213 double cz1 = cos(_angle(2));
1214 double sz1 = sin(_angle(2));
1215
1216 ret(0, 0) = cz0 * cz1 - cx * sz0 * sz1;
1217 ret(1, 0) = cz1 * sz0 + cx * cz0 * sz1;
1218 ret(2, 0) = sx * sz1;
1219
1220 ret(0, 1) = -cx * cz1 * sz0 - cz0 * sz1;
1221 ret(1, 1) = cx * cz0 * cz1 - sz0 * sz1;
1222 ret(2, 1) = sx * cz1;
1223
1224 ret(0, 2) = sx * sz0;
1225 ret(1, 2) = -sx * cz0;
1226 ret(2, 2) = cx;
1227
1228 return ret;
1229 }
1230
eulerZYZToMatrix(const Eigen::Vector3d & _angle)1231 Eigen::Matrix3d eulerZYZToMatrix(const Eigen::Vector3d& _angle)
1232 {
1233 // +- -+ +- -+
1234 // | r00 r01 r02 | | cy*cz0*cz1-sz0*sz1 -cz1*sz0-cy*cz0*sz1 sy*cz0 |
1235 // | r10 r11 r12 | = | cy*cz1*sz0+cz0*sz1 cz0*cz1-cy*sz0*sz1 sy*sz0 |
1236 // | r20 r21 r22 | | -sy*cz1 sy*sz1 cy |
1237 // +- -+ +- -+
1238
1239 Eigen::Matrix3d ret = Eigen::Matrix3d::Identity();
1240
1241 double cz0 = cos(_angle[0]);
1242 double sz0 = sin(_angle[0]);
1243 double cy = cos(_angle[1]);
1244 double sy = sin(_angle[1]);
1245 double cz1 = cos(_angle[2]);
1246 double sz1 = sin(_angle[2]);
1247
1248 ret(0, 0) = cz0 * cy * cz1 - sz0 * sz1;
1249 ret(1, 0) = sz0 * cy * cz1 + cz0 * sz1;
1250 ret(2, 0) = -sy * cz1;
1251
1252 ret(0, 1) = -cz0 * cy * sz1 - sz0 * cz1;
1253 ret(1, 1) = cz0 * cz1 - sz0 * cy * sz1;
1254 ret(2, 1) = sy * sz1;
1255
1256 ret(0, 2) = cz0 * sy;
1257 ret(1, 2) = sz0 * sy;
1258 ret(2, 2) = cy;
1259
1260 return ret;
1261 }
1262
1263 // R = Exp(w)
1264 // p = sin(t) / t*v + (t - sin(t)) / t^3*<w, v>*w + (1 - cos(t)) / t^2*(w X v)
1265 // , when S = (w, v), t = |w|
expMap(const Eigen::Vector6d & _S)1266 Eigen::Isometry3d expMap(const Eigen::Vector6d& _S)
1267 {
1268 Eigen::Isometry3d ret = Eigen::Isometry3d::Identity();
1269 double s2[] = {_S[0] * _S[0], _S[1] * _S[1], _S[2] * _S[2]};
1270 double s3[] = {_S[0] * _S[1], _S[1] * _S[2], _S[2] * _S[0]};
1271 double theta = sqrt(s2[0] + s2[1] + s2[2]);
1272 double cos_t = cos(theta), alpha, beta, gamma;
1273
1274 if (theta > DART_EPSILON)
1275 {
1276 double sin_t = sin(theta);
1277 alpha = sin_t / theta;
1278 beta = (1.0 - cos_t) / theta / theta;
1279 gamma = (_S[0] * _S[3] + _S[1] * _S[4] + _S[2] * _S[5]) * (theta - sin_t)
1280 / theta / theta / theta;
1281 }
1282 else
1283 {
1284 alpha = 1.0 - theta * theta / 6.0;
1285 beta = 0.5 - theta * theta / 24.0;
1286 gamma = (_S[0] * _S[3] + _S[1] * _S[4] + _S[2] * _S[5]) / 6.0
1287 - theta * theta / 120.0;
1288 }
1289
1290 ret(0, 0) = beta * s2[0] + cos_t;
1291 ret(1, 0) = beta * s3[0] + alpha * _S[2];
1292 ret(2, 0) = beta * s3[2] - alpha * _S[1];
1293
1294 ret(0, 1) = beta * s3[0] - alpha * _S[2];
1295 ret(1, 1) = beta * s2[1] + cos_t;
1296 ret(2, 1) = beta * s3[1] + alpha * _S[0];
1297
1298 ret(0, 2) = beta * s3[2] + alpha * _S[1];
1299 ret(1, 2) = beta * s3[1] - alpha * _S[0];
1300 ret(2, 2) = beta * s2[2] + cos_t;
1301
1302 ret(0, 3)
1303 = alpha * _S[3] + beta * (_S[1] * _S[5] - _S[2] * _S[4]) + gamma * _S[0];
1304 ret(1, 3)
1305 = alpha * _S[4] + beta * (_S[2] * _S[3] - _S[0] * _S[5]) + gamma * _S[1];
1306 ret(2, 3)
1307 = alpha * _S[5] + beta * (_S[0] * _S[4] - _S[1] * _S[3]) + gamma * _S[2];
1308
1309 return ret;
1310 }
1311
1312 // I + sin(t) / t*[S] + (1 - cos(t)) / t^2*[S]^2, where t = |S|
expAngular(const Eigen::Vector3d & _s)1313 Eigen::Isometry3d expAngular(const Eigen::Vector3d& _s)
1314 {
1315 Eigen::Isometry3d ret = Eigen::Isometry3d::Identity();
1316 double s2[] = {_s[0] * _s[0], _s[1] * _s[1], _s[2] * _s[2]};
1317 double s3[] = {_s[0] * _s[1], _s[1] * _s[2], _s[2] * _s[0]};
1318 double theta = sqrt(s2[0] + s2[1] + s2[2]);
1319 double cos_t = cos(theta);
1320 double alpha = 0.0;
1321 double beta = 0.0;
1322
1323 if (theta > DART_EPSILON)
1324 {
1325 alpha = sin(theta) / theta;
1326 beta = (1.0 - cos_t) / theta / theta;
1327 }
1328 else
1329 {
1330 alpha = 1.0 - theta * theta / 6.0;
1331 beta = 0.5 - theta * theta / 24.0;
1332 }
1333
1334 ret(0, 0) = beta * s2[0] + cos_t;
1335 ret(1, 0) = beta * s3[0] + alpha * _s[2];
1336 ret(2, 0) = beta * s3[2] - alpha * _s[1];
1337
1338 ret(0, 1) = beta * s3[0] - alpha * _s[2];
1339 ret(1, 1) = beta * s2[1] + cos_t;
1340 ret(2, 1) = beta * s3[1] + alpha * _s[0];
1341
1342 ret(0, 2) = beta * s3[2] + alpha * _s[1];
1343 ret(1, 2) = beta * s3[1] - alpha * _s[0];
1344 ret(2, 2) = beta * s2[2] + cos_t;
1345
1346 return ret;
1347 }
1348
1349 // SE3 Normalize(const SE3& T)
1350 // {
1351 // SE3 ret = SE3::Identity();
1352 // double idet = 1.0 / (T(0,0)*(T(1,1)*T(2,2) - T(2,1)*T(1,2)) +
1353 // T(0,1)*(T(2,0)*T(1,2) - T(1,0)*T(2,2)) +
1354 // T(0,2)*(T(1,0)*T(2,1) - T(2,0)*T(1,1)));
1355
1356 // ret(0,0) = 1.0_2*(T(0,0) + idet*(T(1,1)*T(2,2) - T(2,1)*T(1,2)));
1357 // ret(0,1) = 1.0_2*(T(0,1) + idet*(T(2,0)*T(1,2) - T(1,0)*T(2,2)));
1358 // ret(0,2) = 1.0_2*(T(0,2) + idet*(T(1,0)*T(2,1) - T(2,0)*T(1,1)));
1359 // ret(0,3) = T(0,3);
1360
1361 // ret(1,0) = 1.0_2*(T(1,0) + idet*(T(2,1)*T(0,2) - T(0,1)*T(2,2)));
1362 // ret(1,1) = 1.0_2*(T(1,1) + idet*(T(0,0)*T(2,2) - T(2,0)*T(0,2)));
1363 // ret(1,2) = 1.0_2*(T(1,2) + idet*(T(2,0)*T(0,1) - T(0,0)*T(2,1)));
1364 // ret(1,3) = T(1,3);
1365
1366 // ret(2,0) = 1.0_2*(T(2,0) + idet*(T(0,1)*T(1,2) - T(1,1)*T(0,2)));
1367 // ret(2,1) = 1.0_2*(T(2,1) + idet*(T(1,0)*T(0,2) - T(0,0)*T(1,2)));
1368 // ret(2,2) = 1.0_2*(T(2,2) + idet*(T(0,0)*T(1,1) - T(1,0)*T(0,1)));
1369 // ret(2,3) = T(2,3);
1370
1371 // return ret;
1372 // }
1373
1374 // Axis Reparameterize(const Axis& s)
1375 // {
1376 // double theta = std::sqrt(s[0]*s[0] + s[1]*s[1] + s[2]*s[2]);
1377 // double eta = theta < LIE_EPS
1378 // ? 1.0
1379 // : 1.0 - (double)((int)(theta / M_PI + 1.0) / 2)*M_2PI / theta;
1380
1381 // return eta*s;
1382 // }
1383
1384 // Axis AdInvTAngular(const SE3& T, const Axis& v)
1385 // {
1386 // return Axis(T(0,0)*v[0] + T(1,0)*v[1] + T(2,0)*v[2],
1387 // T(0,1)*v[0] + T(1,1)*v[1] + T(2,1)*v[2],
1388 // T(0,2)*v[0] + T(1,2)*v[1] + T(2,2)*v[2]);
1389 // }
1390
1391 // Axis ad(const Axis& s1, const se3& s2)
1392 // {
1393 // return Axis(s2[2]*s1[1] - s2[1]*s1[2],
1394 // s2[0]*s1[2] - s2[2]*s1[0],
1395 // s2[1]*s1[0] - s2[0]*s1[1]);
1396 // }
1397
1398 // Axis ad_Axis_Axis(const Axis& s1, const Axis& s2)
1399 // {
1400 // return Axis(s2[2]*s1[1] - s2[1]*s1[2],
1401 // s2[0]*s1[2] - s2[2]*s1[0],
1402 // s2[1]*s1[0] - s2[0]*s1[1]);
1403 // }
1404
dad(const Eigen::Vector6d & _s,const Eigen::Vector6d & _t)1405 Eigen::Vector6d dad(const Eigen::Vector6d& _s, const Eigen::Vector6d& _t)
1406 {
1407 Eigen::Vector6d res;
1408 res.head<3>()
1409 = _t.head<3>().cross(_s.head<3>()) + _t.tail<3>().cross(_s.tail<3>());
1410 res.tail<3>() = _t.tail<3>().cross(_s.head<3>());
1411 return res;
1412 }
1413
transformInertia(const Eigen::Isometry3d & _T,const Inertia & _I)1414 Inertia transformInertia(const Eigen::Isometry3d& _T, const Inertia& _I)
1415 {
1416 // operation count: multiplication = 186, addition = 117, subtract = 21
1417
1418 Inertia ret = Inertia::Identity();
1419
1420 double d0 = _I(0, 3) + _T(2, 3) * _I(3, 4) - _T(1, 3) * _I(3, 5);
1421 double d1 = _I(1, 3) - _T(2, 3) * _I(3, 3) + _T(0, 3) * _I(3, 5);
1422 double d2 = _I(2, 3) + _T(1, 3) * _I(3, 3) - _T(0, 3) * _I(3, 4);
1423 double d3 = _I(0, 4) + _T(2, 3) * _I(4, 4) - _T(1, 3) * _I(4, 5);
1424 double d4 = _I(1, 4) - _T(2, 3) * _I(3, 4) + _T(0, 3) * _I(4, 5);
1425 double d5 = _I(2, 4) + _T(1, 3) * _I(3, 4) - _T(0, 3) * _I(4, 4);
1426 double d6 = _I(0, 5) + _T(2, 3) * _I(4, 5) - _T(1, 3) * _I(5, 5);
1427 double d7 = _I(1, 5) - _T(2, 3) * _I(3, 5) + _T(0, 3) * _I(5, 5);
1428 double d8 = _I(2, 5) + _T(1, 3) * _I(3, 5) - _T(0, 3) * _I(4, 5);
1429 double e0 = _I(0, 0) + _T(2, 3) * _I(0, 4) - _T(1, 3) * _I(0, 5)
1430 + d3 * _T(2, 3) - d6 * _T(1, 3);
1431 double e3 = _I(0, 1) + _T(2, 3) * _I(1, 4) - _T(1, 3) * _I(1, 5)
1432 - d0 * _T(2, 3) + d6 * _T(0, 3);
1433 double e4 = _I(1, 1) - _T(2, 3) * _I(1, 3) + _T(0, 3) * _I(1, 5)
1434 - d1 * _T(2, 3) + d7 * _T(0, 3);
1435 double e6 = _I(0, 2) + _T(2, 3) * _I(2, 4) - _T(1, 3) * _I(2, 5)
1436 + d0 * _T(1, 3) - d3 * _T(0, 3);
1437 double e7 = _I(1, 2) - _T(2, 3) * _I(2, 3) + _T(0, 3) * _I(2, 5)
1438 + d1 * _T(1, 3) - d4 * _T(0, 3);
1439 double e8 = _I(2, 2) + _T(1, 3) * _I(2, 3) - _T(0, 3) * _I(2, 4)
1440 + d2 * _T(1, 3) - d5 * _T(0, 3);
1441 double f0 = _T(0, 0) * e0 + _T(1, 0) * e3 + _T(2, 0) * e6;
1442 double f1 = _T(0, 0) * e3 + _T(1, 0) * e4 + _T(2, 0) * e7;
1443 double f2 = _T(0, 0) * e6 + _T(1, 0) * e7 + _T(2, 0) * e8;
1444 double f3 = _T(0, 0) * d0 + _T(1, 0) * d1 + _T(2, 0) * d2;
1445 double f4 = _T(0, 0) * d3 + _T(1, 0) * d4 + _T(2, 0) * d5;
1446 double f5 = _T(0, 0) * d6 + _T(1, 0) * d7 + _T(2, 0) * d8;
1447 double f6 = _T(0, 1) * e0 + _T(1, 1) * e3 + _T(2, 1) * e6;
1448 double f7 = _T(0, 1) * e3 + _T(1, 1) * e4 + _T(2, 1) * e7;
1449 double f8 = _T(0, 1) * e6 + _T(1, 1) * e7 + _T(2, 1) * e8;
1450 double g0 = _T(0, 1) * d0 + _T(1, 1) * d1 + _T(2, 1) * d2;
1451 double g1 = _T(0, 1) * d3 + _T(1, 1) * d4 + _T(2, 1) * d5;
1452 double g2 = _T(0, 1) * d6 + _T(1, 1) * d7 + _T(2, 1) * d8;
1453 double g3 = _T(0, 2) * d0 + _T(1, 2) * d1 + _T(2, 2) * d2;
1454 double g4 = _T(0, 2) * d3 + _T(1, 2) * d4 + _T(2, 2) * d5;
1455 double g5 = _T(0, 2) * d6 + _T(1, 2) * d7 + _T(2, 2) * d8;
1456 double h0 = _T(0, 0) * _I(3, 3) + _T(1, 0) * _I(3, 4) + _T(2, 0) * _I(3, 5);
1457 double h1 = _T(0, 0) * _I(3, 4) + _T(1, 0) * _I(4, 4) + _T(2, 0) * _I(4, 5);
1458 double h2 = _T(0, 0) * _I(3, 5) + _T(1, 0) * _I(4, 5) + _T(2, 0) * _I(5, 5);
1459 double h3 = _T(0, 1) * _I(3, 3) + _T(1, 1) * _I(3, 4) + _T(2, 1) * _I(3, 5);
1460 double h4 = _T(0, 1) * _I(3, 4) + _T(1, 1) * _I(4, 4) + _T(2, 1) * _I(4, 5);
1461 double h5 = _T(0, 1) * _I(3, 5) + _T(1, 1) * _I(4, 5) + _T(2, 1) * _I(5, 5);
1462
1463 ret(0, 0) = f0 * _T(0, 0) + f1 * _T(1, 0) + f2 * _T(2, 0);
1464 ret(0, 1) = f0 * _T(0, 1) + f1 * _T(1, 1) + f2 * _T(2, 1);
1465 ret(0, 2) = f0 * _T(0, 2) + f1 * _T(1, 2) + f2 * _T(2, 2);
1466 ret(0, 3) = f3 * _T(0, 0) + f4 * _T(1, 0) + f5 * _T(2, 0);
1467 ret(0, 4) = f3 * _T(0, 1) + f4 * _T(1, 1) + f5 * _T(2, 1);
1468 ret(0, 5) = f3 * _T(0, 2) + f4 * _T(1, 2) + f5 * _T(2, 2);
1469 ret(1, 1) = f6 * _T(0, 1) + f7 * _T(1, 1) + f8 * _T(2, 1);
1470 ret(1, 2) = f6 * _T(0, 2) + f7 * _T(1, 2) + f8 * _T(2, 2);
1471 ret(1, 3) = g0 * _T(0, 0) + g1 * _T(1, 0) + g2 * _T(2, 0);
1472 ret(1, 4) = g0 * _T(0, 1) + g1 * _T(1, 1) + g2 * _T(2, 1);
1473 ret(1, 5) = g0 * _T(0, 2) + g1 * _T(1, 2) + g2 * _T(2, 2);
1474 ret(2, 2) = (_T(0, 2) * e0 + _T(1, 2) * e3 + _T(2, 2) * e6) * _T(0, 2)
1475 + (_T(0, 2) * e3 + _T(1, 2) * e4 + _T(2, 2) * e7) * _T(1, 2)
1476 + (_T(0, 2) * e6 + _T(1, 2) * e7 + _T(2, 2) * e8) * _T(2, 2);
1477 ret(2, 3) = g3 * _T(0, 0) + g4 * _T(1, 0) + g5 * _T(2, 0);
1478 ret(2, 4) = g3 * _T(0, 1) + g4 * _T(1, 1) + g5 * _T(2, 1);
1479 ret(2, 5) = g3 * _T(0, 2) + g4 * _T(1, 2) + g5 * _T(2, 2);
1480 ret(3, 3) = h0 * _T(0, 0) + h1 * _T(1, 0) + h2 * _T(2, 0);
1481 ret(3, 4) = h0 * _T(0, 1) + h1 * _T(1, 1) + h2 * _T(2, 1);
1482 ret(3, 5) = h0 * _T(0, 2) + h1 * _T(1, 2) + h2 * _T(2, 2);
1483 ret(4, 4) = h3 * _T(0, 1) + h4 * _T(1, 1) + h5 * _T(2, 1);
1484 ret(4, 5) = h3 * _T(0, 2) + h4 * _T(1, 2) + h5 * _T(2, 2);
1485 ret(5, 5)
1486 = (_T(0, 2) * _I(3, 3) + _T(1, 2) * _I(3, 4) + _T(2, 2) * _I(3, 5))
1487 * _T(0, 2)
1488 + (_T(0, 2) * _I(3, 4) + _T(1, 2) * _I(4, 4) + _T(2, 2) * _I(4, 5))
1489 * _T(1, 2)
1490 + (_T(0, 2) * _I(3, 5) + _T(1, 2) * _I(4, 5) + _T(2, 2) * _I(5, 5))
1491 * _T(2, 2);
1492
1493 ret.triangularView<Eigen::StrictlyLower>() = ret.transpose();
1494
1495 return ret;
1496 }
1497
parallelAxisTheorem(const Eigen::Matrix3d & _original,const Eigen::Vector3d & _comShift,double _mass)1498 Eigen::Matrix3d parallelAxisTheorem(
1499 const Eigen::Matrix3d& _original,
1500 const Eigen::Vector3d& _comShift,
1501 double _mass)
1502 {
1503 const Eigen::Vector3d& p = _comShift;
1504 Eigen::Matrix3d result(_original);
1505 for (std::size_t i = 0; i < 3; ++i)
1506 for (std::size_t j = 0; j < 3; ++j)
1507 result(i, j) += _mass * (delta(i, j) * p.dot(p) - p(i) * p(j));
1508
1509 return result;
1510 }
1511
verifyRotation(const Eigen::Matrix3d & _T)1512 bool verifyRotation(const Eigen::Matrix3d& _T)
1513 {
1514 return !isNan(_T) && std::abs(_T.determinant() - 1.0) <= DART_EPSILON;
1515 }
1516
verifyTransform(const Eigen::Isometry3d & _T)1517 bool verifyTransform(const Eigen::Isometry3d& _T)
1518 {
1519 return !isNan(_T.matrix().topRows<3>())
1520 && std::abs(_T.linear().determinant() - 1.0) <= DART_EPSILON;
1521 }
1522
fromSkewSymmetric(const Eigen::Matrix3d & _m)1523 Eigen::Vector3d fromSkewSymmetric(const Eigen::Matrix3d& _m)
1524 {
1525 #ifndef NDEBUG
1526 if (std::abs(_m(0, 0)) > DART_EPSILON || std::abs(_m(1, 1)) > DART_EPSILON
1527 || std::abs(_m(2, 2)) > DART_EPSILON)
1528 {
1529 dtwarn << "[math::fromSkewSymmetric] Not skew a symmetric matrix:\n"
1530 << _m << "\n";
1531 return Eigen::Vector3d::Zero();
1532 }
1533 #endif
1534 Eigen::Vector3d ret;
1535 ret << _m(2, 1), _m(0, 2), _m(1, 0);
1536 return ret;
1537 }
1538
makeSkewSymmetric(const Eigen::Vector3d & _v)1539 Eigen::Matrix3d makeSkewSymmetric(const Eigen::Vector3d& _v)
1540 {
1541 Eigen::Matrix3d result = Eigen::Matrix3d::Zero();
1542
1543 result(0, 1) = -_v(2);
1544 result(1, 0) = _v(2);
1545 result(0, 2) = _v(1);
1546 result(2, 0) = -_v(1);
1547 result(1, 2) = -_v(0);
1548 result(2, 1) = _v(0);
1549
1550 return result;
1551 }
1552
1553 //==============================================================================
computeRotation(const Eigen::Vector3d & axis,const AxisType axisType)1554 Eigen::Matrix3d computeRotation(
1555 const Eigen::Vector3d& axis, const AxisType axisType)
1556 {
1557 assert(axis != Eigen::Vector3d::Zero());
1558
1559 // First axis
1560 const Eigen::Vector3d axis0 = axis.normalized();
1561
1562 // Second axis
1563 Eigen::Vector3d axis1 = axis0.cross(Eigen::Vector3d::UnitX());
1564 if (axis1.norm() < DART_EPSILON)
1565 axis1 = axis0.cross(Eigen::Vector3d::UnitY());
1566 axis1.normalize();
1567
1568 // Third axis
1569 const Eigen::Vector3d axis2 = axis0.cross(axis1).normalized();
1570
1571 // Assign the three axes
1572 Eigen::Matrix3d result;
1573 int index = axisType;
1574 result.col(index) = axis0;
1575 result.col(++index % 3) = axis1;
1576 result.col(++index % 3) = axis2;
1577
1578 assert(verifyRotation(result));
1579
1580 return result;
1581 }
1582
1583 //==============================================================================
computeTransform(const Eigen::Vector3d & axis,const Eigen::Vector3d & translation,AxisType axisType)1584 Eigen::Isometry3d computeTransform(
1585 const Eigen::Vector3d& axis,
1586 const Eigen::Vector3d& translation,
1587 AxisType axisType)
1588 {
1589 Eigen::Isometry3d result = Eigen::Isometry3d::Identity();
1590
1591 result.linear() = computeRotation(axis, axisType);
1592 result.translation() = translation;
1593
1594 // Verification
1595 assert(verifyTransform(result));
1596
1597 return result;
1598 }
1599
1600 //==============================================================================
getFrameOriginAxisZ(const Eigen::Vector3d & _origin,const Eigen::Vector3d & _axisZ)1601 Eigen::Isometry3d getFrameOriginAxisZ(
1602 const Eigen::Vector3d& _origin, const Eigen::Vector3d& _axisZ)
1603 {
1604 return computeTransform(_axisZ, _origin, AxisType::AXIS_Z);
1605 }
1606
1607 //==============================================================================
computeSupportPolgyon(const SupportGeometry & _geometry,const Eigen::Vector3d & _axis1,const Eigen::Vector3d & _axis2)1608 SupportPolygon computeSupportPolgyon(
1609 const SupportGeometry& _geometry,
1610 const Eigen::Vector3d& _axis1,
1611 const Eigen::Vector3d& _axis2)
1612 {
1613 std::vector<std::size_t> indices;
1614 indices.reserve(_geometry.size());
1615 return computeSupportPolgyon(indices, _geometry, _axis1, _axis2);
1616 }
1617
1618 //==============================================================================
computeSupportPolgyon(std::vector<std::size_t> & _originalIndices,const SupportGeometry & _geometry,const Eigen::Vector3d & _axis1,const Eigen::Vector3d & _axis2)1619 SupportPolygon computeSupportPolgyon(
1620 std::vector<std::size_t>& _originalIndices,
1621 const SupportGeometry& _geometry,
1622 const Eigen::Vector3d& _axis1,
1623 const Eigen::Vector3d& _axis2)
1624 {
1625 SupportPolygon polygon;
1626 polygon.reserve(_geometry.size());
1627 for (const Eigen::Vector3d& v : _geometry)
1628 polygon.push_back(Eigen::Vector2d(v.dot(_axis1), v.dot(_axis2)));
1629
1630 return computeConvexHull(_originalIndices, polygon);
1631 }
1632
1633 //==============================================================================
1634 // HullAngle is an internal struct used to facilitate the computation of 2D
1635 // convex hulls
1636 struct HullAngle
1637 {
HullAngledart::math::HullAngle1638 HullAngle(double angle, double distance, std::size_t index)
1639 : mAngle(angle), mDistance(distance), mIndex(index)
1640 {
1641 // Do nothing
1642 }
1643
1644 double mAngle;
1645 double mDistance;
1646 std::size_t mIndex;
1647 };
1648
1649 //==============================================================================
1650 // Comparison function to allow hull angles to be sorted
HullAngleComparison(const HullAngle & a,const HullAngle & b)1651 static bool HullAngleComparison(const HullAngle& a, const HullAngle& b)
1652 {
1653 return a.mAngle < b.mAngle;
1654 }
1655
1656 //==============================================================================
computeConvexHull(const SupportPolygon & _points)1657 SupportPolygon computeConvexHull(const SupportPolygon& _points)
1658 {
1659 std::vector<std::size_t> indices;
1660 indices.reserve(_points.size());
1661 return computeConvexHull(indices, _points);
1662 }
1663
1664 //==============================================================================
computeCentroidOfHull(const SupportPolygon & _convexHull)1665 Eigen::Vector2d computeCentroidOfHull(const SupportPolygon& _convexHull)
1666 {
1667 if (_convexHull.size() == 0)
1668 {
1669 Eigen::Vector2d invalid = Eigen::Vector2d::Constant(std::nan(""));
1670 dtwarn << "[computeCentroidOfHull] Requesting the centroid of an empty set "
1671 << "of points! We will return <" << invalid.transpose() << ">.\n";
1672 return invalid;
1673 }
1674
1675 if (_convexHull.size() == 1)
1676 return _convexHull[0];
1677
1678 if (_convexHull.size() == 2)
1679 return (_convexHull[0] + _convexHull[1]) / 2.0;
1680
1681 Eigen::Vector2d c(0, 0);
1682 Eigen::Vector2d intersect;
1683 double area = 0;
1684 double area_i;
1685 Eigen::Vector2d midp12, midp01;
1686
1687 for (std::size_t i = 2; i < _convexHull.size(); ++i)
1688 {
1689 const Eigen::Vector2d& p0 = _convexHull[0];
1690 const Eigen::Vector2d& p1 = _convexHull[i - 1];
1691 const Eigen::Vector2d& p2 = _convexHull[i];
1692
1693 area_i = 0.5
1694 * ((p1[0] - p0[0]) * (p2[1] - p0[1])
1695 - (p1[1] - p0[1]) * (p2[0] - p0[0]));
1696
1697 midp12 = 0.5 * (p1 + p2);
1698 midp01 = 0.5 * (p0 + p1);
1699
1700 IntersectionResult result
1701 = computeIntersection(intersect, p0, midp12, p2, midp01);
1702
1703 if (BEYOND_ENDPOINTS == result)
1704 {
1705 double a1 = atan2((p1 - p0)[1], (p1 - p0)[0]) * 180.0 / constantsd::pi();
1706 double a2 = atan2((p2 - p0)[1], (p2 - p0)[0]) * 180.0 / constantsd::pi();
1707 double diff = a1 - a2;
1708 dtwarn << "[computeCentroidOfHull] You have passed in a set of points "
1709 << "which is not a proper convex hull! The invalid segment "
1710 << "contains indices " << i - 1 << " -> " << i << ":\n"
1711 << i - 1 << ") " << p1.transpose() << " (" << a1 << " degrees)"
1712 << "\n"
1713 << i << ") " << p2.transpose() << " (" << a2 << " degrees)"
1714 << "\n"
1715 << "0) " << p0.transpose() << "\n"
1716 << "(" << result << ") "
1717 << (PARALLEL == result
1718 ? "These segments are parallel!\n"
1719 : "These segments are too short to intersect!\n")
1720 << "Difference in angle: " << diff << "\n\n";
1721 continue;
1722 }
1723
1724 area += area_i;
1725 c += area_i * intersect;
1726 }
1727
1728 // A negative area means we have a bug in the code
1729 assert(area >= 0.0);
1730
1731 if (area == 0.0)
1732 return c;
1733
1734 return c / area;
1735 }
1736
1737 //==============================================================================
1738 // Returns true if the path that goes from p1 -> p2 -> p3 turns left
isLeftTurn(const Eigen::Vector2d & p1,const Eigen::Vector2d & p2,const Eigen::Vector2d & p3)1739 static bool isLeftTurn(
1740 const Eigen::Vector2d& p1,
1741 const Eigen::Vector2d& p2,
1742 const Eigen::Vector2d& p3)
1743 {
1744 return (cross(p2 - p1, p3 - p1) > 0);
1745 }
1746
1747 //==============================================================================
computeConvexHull(std::vector<std::size_t> & _originalIndices,const SupportPolygon & _points)1748 SupportPolygon computeConvexHull(
1749 std::vector<std::size_t>& _originalIndices, const SupportPolygon& _points)
1750 {
1751 _originalIndices.clear();
1752
1753 if (_points.size() <= 3)
1754 {
1755 // Three or fewer points is already a convex hull
1756 for (std::size_t i = 0; i < _points.size(); ++i)
1757 _originalIndices.push_back(i);
1758
1759 return _points;
1760 }
1761
1762 // We'll use "Graham scan" to compute the convex hull in the general case
1763 std::size_t lowestIndex = static_cast<std::size_t>(-1);
1764 double lowestY = std::numeric_limits<double>::infinity();
1765 for (std::size_t i = 0; i < _points.size(); ++i)
1766 {
1767 if (_points[i][1] < lowestY)
1768 {
1769 lowestIndex = i;
1770 lowestY = _points[i][1];
1771 }
1772 else if (_points[i][1] == lowestY)
1773 {
1774 if (_points[i][0] < _points[lowestIndex][0])
1775 {
1776 lowestIndex = i;
1777 }
1778 }
1779 }
1780
1781 std::vector<HullAngle> angles;
1782 const Eigen::Vector2d& bottom = _points[lowestIndex];
1783 for (std::size_t i = 0; i < _points.size(); ++i)
1784 {
1785 const Eigen::Vector2d& p = _points[i];
1786 if (p != bottom)
1787 {
1788 const Eigen::Vector2d& v = p - bottom;
1789 angles.push_back(HullAngle(atan2(v[1], v[0]), v.norm(), i));
1790 }
1791 }
1792
1793 std::sort(angles.begin(), angles.end(), HullAngleComparison);
1794
1795 if (angles.size() > 1)
1796 {
1797 for (std::size_t i = 0; i < angles.size() - 1; ++i)
1798 {
1799 if (std::abs(angles[i].mAngle - angles[i + 1].mAngle) < 1e-12)
1800 {
1801 // If two points have the same angle, throw out the one that is closer
1802 // to the corner
1803 std::size_t tossout
1804 = (angles[i].mDistance < angles[i + 1].mDistance) ? i : i + 1;
1805 angles.erase(angles.begin() + tossout);
1806 --i;
1807 }
1808 }
1809 }
1810
1811 if (angles.size() <= 3)
1812 {
1813 // There were so many repeated points in the given set that we only have
1814 // three or fewer unique points
1815 _originalIndices.reserve(angles.size() + 1);
1816 _originalIndices.push_back(lowestIndex);
1817 for (std::size_t i = 0; i < angles.size(); ++i)
1818 _originalIndices.push_back(angles[i].mIndex);
1819
1820 SupportPolygon polygon;
1821 polygon.reserve(_originalIndices.size());
1822 for (std::size_t index : _originalIndices)
1823 polygon.push_back(_points[index]);
1824
1825 return polygon;
1826 }
1827
1828 std::vector<std::size_t>& edge = _originalIndices;
1829 std::size_t lastIndex = lowestIndex;
1830 std::size_t secondToLastIndex = angles[0].mIndex;
1831 edge.reserve(angles.size() + 1);
1832
1833 edge.push_back(lowestIndex);
1834
1835 for (std::size_t i = 1; i < angles.size(); ++i)
1836 {
1837 std::size_t currentIndex = angles[i].mIndex;
1838 const Eigen::Vector2d& p1 = _points[lastIndex];
1839 const Eigen::Vector2d& p2 = _points[secondToLastIndex];
1840 const Eigen::Vector2d& p3 = _points[currentIndex];
1841
1842 bool leftTurn = isLeftTurn(p1, p2, p3);
1843
1844 if (leftTurn)
1845 {
1846 edge.push_back(secondToLastIndex);
1847 lastIndex = secondToLastIndex;
1848 secondToLastIndex = currentIndex;
1849 }
1850 else
1851 {
1852 secondToLastIndex = edge.back();
1853 edge.pop_back();
1854 lastIndex = edge.back();
1855 --i;
1856 }
1857 }
1858
1859 const Eigen::Vector2d& p1 = _points[edge.back()];
1860 const Eigen::Vector2d& p2 = _points[angles.back().mIndex];
1861 const Eigen::Vector2d& p3 = _points[lowestIndex];
1862 if (isLeftTurn(p1, p2, p3))
1863 edge.push_back(angles.back().mIndex);
1864
1865 SupportPolygon polygon;
1866 polygon.reserve(edge.size());
1867 for (std::size_t index : edge)
1868 polygon.push_back(_points[index]);
1869
1870 // Note that we do not need to fill in _originalIndices, because "edge" is a
1871 // non-const reference to _originalIndices and it has been filled in with the
1872 // appropriate values.
1873 return polygon;
1874 }
1875
1876 //==============================================================================
computeIntersection(Eigen::Vector2d & _intersectionPoint,const Eigen::Vector2d & a1,const Eigen::Vector2d & a2,const Eigen::Vector2d & b1,const Eigen::Vector2d & b2)1877 IntersectionResult computeIntersection(
1878 Eigen::Vector2d& _intersectionPoint,
1879 const Eigen::Vector2d& a1,
1880 const Eigen::Vector2d& a2,
1881 const Eigen::Vector2d& b1,
1882 const Eigen::Vector2d& b2)
1883 {
1884 double dx_a = a2[0] - a1[0];
1885 double dy_a = a2[1] - a1[1];
1886
1887 double dx_b = b2[0] - b1[0];
1888 double dy_b = b2[1] - b1[1];
1889
1890 Eigen::Vector2d& point = _intersectionPoint;
1891
1892 if (std::abs(dx_b * dy_a - dx_a * dy_b) < 1e-12)
1893 {
1894 // The line segments are parallel, so give back an average of all the points
1895 point = (a1 + a2 + b1 + b2) / 4.0;
1896 return PARALLEL;
1897 }
1898
1899 point[0] = (dx_b * dy_a * a1[0] - dx_a * dy_b * b1[0]
1900 + dx_a * dx_b * (b1[1] - a1[1]))
1901 / (dx_b * dy_a - dx_a * dy_b);
1902
1903 if (dx_a != 0.0)
1904 point[1] = dy_a / dx_a * (point[0] - a1[0]) + a1[1];
1905 else
1906 point[1] = dy_b / dx_b * (point[0] - b1[0]) + b1[1];
1907
1908 for (std::size_t i = 0; i < 2; ++i)
1909 {
1910 if ((point[i] < std::min(a1[i], a2[i]))
1911 || (std::max(a1[i], a2[i]) < point[i]))
1912 {
1913 return BEYOND_ENDPOINTS;
1914 }
1915
1916 if ((point[i] < std::min(b1[i], b2[i]))
1917 || (std::max(b1[i], b2[i]) < point[i]))
1918 {
1919 return BEYOND_ENDPOINTS;
1920 }
1921 }
1922
1923 return INTERSECTING;
1924 }
1925
1926 //==============================================================================
cross(const Eigen::Vector2d & _v1,const Eigen::Vector2d & _v2)1927 double cross(const Eigen::Vector2d& _v1, const Eigen::Vector2d& _v2)
1928 {
1929 return _v1[0] * _v2[1] - _v1[1] * _v2[0];
1930 }
1931
1932 //==============================================================================
isInsideSupportPolygon(const Eigen::Vector2d & _p,const SupportPolygon & _support,bool _includeEdge)1933 bool isInsideSupportPolygon(
1934 const Eigen::Vector2d& _p,
1935 const SupportPolygon& _support,
1936 bool _includeEdge)
1937 {
1938 if (_support.size() == 0)
1939 return false;
1940
1941 if (_support.size() == 1)
1942 {
1943 if (!_includeEdge)
1944 return false;
1945
1946 return (_support[0] == _p);
1947 }
1948
1949 if (_support.size() == 2)
1950 {
1951 if (!_includeEdge)
1952 return false;
1953
1954 const Eigen::Vector2d& p1 = _support[0];
1955 const Eigen::Vector2d& p2 = _support[1];
1956 const Eigen::Vector2d& p3 = _p;
1957
1958 if (cross(p2 - p1, p3 - p1) == 0)
1959 {
1960 if (p3[0] < std::min(p1[0], p2[0]) || std::max(p1[0], p2[0]) < p3[0])
1961 return false;
1962
1963 return true;
1964 }
1965
1966 return false;
1967 }
1968
1969 for (std::size_t i = 0; i < _support.size(); ++i)
1970 {
1971 const Eigen::Vector2d& p1 = (i == 0) ? _support.back() : _support[i - 1];
1972 const Eigen::Vector2d& p2 = _support[i];
1973 const Eigen::Vector2d& p3 = _p;
1974
1975 double crossProduct = cross(p2 - p1, p3 - p1);
1976 if (crossProduct > 0.0)
1977 continue;
1978
1979 if (crossProduct == 0)
1980 {
1981 if (!_includeEdge)
1982 return false;
1983
1984 if (p3[0] < std::min(p1[0], p2[0]) || std::max(p1[0], p2[0]) < p3[0])
1985 return false;
1986
1987 return true;
1988 }
1989 else
1990 {
1991 return false;
1992 }
1993 }
1994
1995 return true;
1996 }
1997
1998 //==============================================================================
computeClosestPointOnLineSegment(const Eigen::Vector2d & _p,const Eigen::Vector2d & _s1,const Eigen::Vector2d & _s2)1999 Eigen::Vector2d computeClosestPointOnLineSegment(
2000 const Eigen::Vector2d& _p,
2001 const Eigen::Vector2d& _s1,
2002 const Eigen::Vector2d& _s2)
2003 {
2004 Eigen::Vector2d result;
2005
2006 if (_s1[0] - _s2[0] == 0)
2007 {
2008 result[0] = _s1[0];
2009 result[1] = _p[1];
2010
2011 if (result[1] < std::min(_s1[1], _s2[1])
2012 || std::max(_s1[1], _s2[1]) < result[1])
2013 {
2014 if (std::abs(_p[1] - _s2[1]) < std::abs(_p[1] - _s1[1]))
2015 result[1] = _s2[1];
2016 else
2017 result[1] = _s1[1];
2018 }
2019 }
2020 else
2021 {
2022 double m = (_s2[1] - _s1[1]) / (_s2[0] - _s1[0]);
2023 double k = _s1[1] - m * _s1[0];
2024 result[0] = (_p[0] + m * (_p[1] - k)) / (m * m + 1.0);
2025 result[1] = m * result[0] + k;
2026
2027 if (result[0] < std::min(_s1[0], _s2[0])
2028 || std::max(_s1[0], _s2[0]) < result[0])
2029 {
2030 if ((_p - _s2).norm() < (_p - _s1).norm())
2031 result = _s2;
2032 else
2033 result = _s1;
2034 }
2035 }
2036
2037 return result;
2038 }
2039
2040 //==============================================================================
computeClosestPointOnSupportPolygon(const Eigen::Vector2d & _p,const SupportPolygon & _support)2041 Eigen::Vector2d computeClosestPointOnSupportPolygon(
2042 const Eigen::Vector2d& _p, const SupportPolygon& _support)
2043 {
2044 std::size_t _index1;
2045 std::size_t _index2;
2046 return computeClosestPointOnSupportPolygon(_index1, _index2, _p, _support);
2047 }
2048
2049 //==============================================================================
computeClosestPointOnSupportPolygon(std::size_t & _index1,std::size_t & _index2,const Eigen::Vector2d & _p,const SupportPolygon & _support)2050 Eigen::Vector2d computeClosestPointOnSupportPolygon(
2051 std::size_t& _index1,
2052 std::size_t& _index2,
2053 const Eigen::Vector2d& _p,
2054 const SupportPolygon& _support)
2055 {
2056 if (_support.size() == 0)
2057 {
2058 _index1 = static_cast<std::size_t>(-1);
2059 _index2 = _index1;
2060 return _p;
2061 }
2062
2063 if (_support.size() == 1)
2064 {
2065 _index1 = 0;
2066 _index2 = 0;
2067 return _support[0];
2068 }
2069
2070 if (_support.size() == 2)
2071 {
2072 _index1 = 0;
2073 _index2 = 1;
2074 return computeClosestPointOnLineSegment(_p, _support[0], _support[1]);
2075 }
2076
2077 double best = std::numeric_limits<double>::infinity(), check;
2078 Eigen::Vector2d test, result;
2079 for (std::size_t i = 0; i < _support.size(); ++i)
2080 {
2081 const Eigen::Vector2d& p1 = (i == 0) ? _support.back() : _support[i - 1];
2082 const Eigen::Vector2d& p2 = _support[i];
2083
2084 test = computeClosestPointOnLineSegment(_p, p1, p2);
2085 check = (test - _p).norm();
2086 if (check < best)
2087 {
2088 best = check;
2089 result = test;
2090 _index1 = (i == 0) ? _support.size() - 1 : i - 1;
2091 _index2 = i;
2092 }
2093 }
2094
2095 return result;
2096 }
2097
BoundingBox()2098 BoundingBox::BoundingBox() : mMin(0, 0, 0), mMax(0, 0, 0)
2099 {
2100 }
BoundingBox(const Eigen::Vector3d & min,const Eigen::Vector3d & max)2101 BoundingBox::BoundingBox(const Eigen::Vector3d& min, const Eigen::Vector3d& max)
2102 : mMin(min), mMax(max)
2103 {
2104 }
2105
2106 } // namespace math
2107 } // namespace dart
2108