19 CPPUNIT_ASSERT_EQUAL(2*v-v,v);
20 CPPUNIT_ASSERT_EQUAL(v*2-v,v);
21 CPPUNIT_ASSERT_EQUAL(v+v+v-2*v,v);
23 CPPUNIT_ASSERT_EQUAL(v,v2);
26 CPPUNIT_ASSERT_EQUAL(2*v,v2);
28 CPPUNIT_ASSERT_EQUAL(v,v2);
30 CPPUNIT_ASSERT_EQUAL(v,-v2);
39 CPPUNIT_ASSERT_EQUAL(
std::hash<Vector>{}(v),
static_cast<size_t>(3450679443808348711u));
40 CPPUNIT_ASSERT_EQUAL(
std::hash<Vector>{}(v2),
static_cast<size_t>(11093822414574u));
43 CPPUNIT_ASSERT_EQUAL(nu.Norm(),0.0);
45 CPPUNIT_ASSERT_EQUAL(nu2.
Norm(),10.0);
50 CPPUNIT_ASSERT_EQUAL(nu.
Norm(), 0.0);
61 CPPUNIT_ASSERT_EQUAL(2*t-t,t);
62 CPPUNIT_ASSERT_EQUAL(t*2-t,t);
63 CPPUNIT_ASSERT_EQUAL(t+t+t-2*t,t);
65 CPPUNIT_ASSERT_EQUAL(t,t2);
68 CPPUNIT_ASSERT_EQUAL(2*t,t2);
70 CPPUNIT_ASSERT_EQUAL(t,t2);
72 CPPUNIT_ASSERT_EQUAL(t,-t2);
83 CPPUNIT_ASSERT_EQUAL(
std::hash<Twist>{}(t3),
static_cast<size_t>(3373832976806748309u));
84 CPPUNIT_ASSERT_EQUAL(
std::hash<Twist>{}(t2),
static_cast<size_t>(730713428471863u));
90 CPPUNIT_ASSERT_EQUAL(2*w-w,w);
91 CPPUNIT_ASSERT_EQUAL(w*2-w,w);
92 CPPUNIT_ASSERT_EQUAL(w+w+w-2*w,w);
94 CPPUNIT_ASSERT_EQUAL(w,w2);
97 CPPUNIT_ASSERT_EQUAL(2*w,w2);
99 CPPUNIT_ASSERT_EQUAL(w,w2);
101 CPPUNIT_ASSERT_EQUAL(w,-w2);
112 CPPUNIT_ASSERT_EQUAL(
std::hash<Wrench>{}(w3),
static_cast<size_t>(13897938943539516747u));
113 CPPUNIT_ASSERT_EQUAL(
std::hash<Wrench>{}(w2),
static_cast<size_t>(730713428471863u));
136 CPPUNIT_ASSERT_EQUAL(R,R2);
138 CPPUNIT_ASSERT_DOUBLES_EQUAL((R*v).
Norm(),v.Norm(),
epsilon);
139 CPPUNIT_ASSERT_EQUAL(R.
Inverse(R*v),v);
140 CPPUNIT_ASSERT_EQUAL(R.
Inverse(R*t),t);
141 CPPUNIT_ASSERT_EQUAL(R.
Inverse(R*w),w);
142 CPPUNIT_ASSERT_EQUAL(R*R.
Inverse(v),v);
145 CPPUNIT_ASSERT_EQUAL(R*(R*(R*v)),(R*R*R)*v);
146 CPPUNIT_ASSERT_EQUAL(R*(R*(R*t)),(R*R*R)*t);
147 CPPUNIT_ASSERT_EQUAL(R*(R*(R*w)),(R*R*R)*w);
150 CPPUNIT_ASSERT_EQUAL(R.Inverse()*v,R.Inverse(v));
152 CPPUNIT_ASSERT_DOUBLES_EQUAL(ra,a,
epsilon);
153 CPPUNIT_ASSERT_DOUBLES_EQUAL(rb,b,
epsilon);
154 CPPUNIT_ASSERT_DOUBLES_EQUAL(rc,c,
epsilon);
156 R.GetEulerZYX(ra,rb,rc);
157 CPPUNIT_ASSERT_DOUBLES_EQUAL(ra,a,
epsilon);
158 CPPUNIT_ASSERT_DOUBLES_EQUAL(rb,b,
epsilon);
159 CPPUNIT_ASSERT_DOUBLES_EQUAL(rc,c,
epsilon);
161 R.GetEulerZYZ(ra,rb,rc);
162 CPPUNIT_ASSERT_DOUBLES_EQUAL(ra,a,
epsilon);
163 CPPUNIT_ASSERT_DOUBLES_EQUAL(rb,b,
epsilon);
164 CPPUNIT_ASSERT_DOUBLES_EQUAL(rc,c,
epsilon);
165 double angle= R.GetRotAngle(v2);
167 CPPUNIT_ASSERT_EQUAL(R2,R);
169 CPPUNIT_ASSERT_EQUAL(R,R2);
178 const double expectedAngle,
185 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE(msg, expectedAngle, angle,
epsilon);
186 CPPUNIT_ASSERT_EQUAL_MESSAGE(msg, expectedAxis, axis);
187 CPPUNIT_ASSERT_EQUAL_MESSAGE(msg, expectedAngle * expectedAxis, R.
GetRot());
188 CPPUNIT_ASSERT_EQUAL_MESSAGE(msg,
Rotation::Rot(axis, angle), R);
190 CPPUNIT_ASSERT_EQUAL_MESSAGE(msg,
Rotation::Rot2(axis, angle), R);
198 const double expectedAngle,
202 ss <<
"rot(" << msg <<
"],(" << angle <<
")";
207 #define TESTEULERZYX(a,b,g) \
210 Rotation R = Rotation::EulerZYX((a),(b),(g));\
211 double alpha,beta,gamma;\
212 R.GetEulerZYX(alpha,beta,gamma);\
213 CPPUNIT_ASSERT_DOUBLES_EQUAL((a),alpha,eps);\
214 CPPUNIT_ASSERT_DOUBLES_EQUAL((b),beta,eps);\
215 CPPUNIT_ASSERT_DOUBLES_EQUAL((g),gamma,eps);\
218 #define TESTEULERZYZ(a,b,g) \
221 Rotation R = Rotation::EulerZYZ((a),(b),(g));\
222 double alpha,beta,gamma;\
223 R.GetEulerZYZ(alpha,beta,gamma);\
224 CPPUNIT_ASSERT_DOUBLES_EQUAL((a),alpha,eps);\
225 CPPUNIT_ASSERT_DOUBLES_EQUAL((b),beta,eps);\
226 CPPUNIT_ASSERT_DOUBLES_EQUAL((g),gamma,eps);\
228 #define TESTEULERZYX_INVARIANT(a,b,g,a2,b2,g2)\
231 Rotation R1=Rotation::EulerZYX(a,b,g);\
232 Rotation R2=Rotation::EulerZYX(a2,b2,g2);\
233 CPPUNIT_ASSERT_DOUBLES_EQUAL(0,diff(R2,R1).Norm(),eps);\
235 #define TESTEULERZYZ_INVARIANT(a,b,g,a2,b2,g2)\
238 Rotation R1=Rotation::EulerZYZ(a,b,g);\
239 Rotation R2=Rotation::EulerZYZ(a2,b2,g2);\
240 CPPUNIT_ASSERT_DOUBLES_EQUAL(0,diff(R2,R1).Norm(),eps);\
316 TestArbitraryRotation(msg, v, 0, 0,
Vector(0,0,1));
317 TestArbitraryRotation(msg, v, 45, 45, expectedVector);
318 TestArbitraryRotation(msg, v, 90, 90, expectedVector);
319 TestArbitraryRotation(msg, v, 179, 179, expectedVector);
324 TestArbitraryRotation(msg, v, 181, 179, -expectedVector);
325 TestArbitraryRotation(msg, v, 270, 90, -expectedVector);
326 TestArbitraryRotation(msg, v, 359, 1, -expectedVector);
327 TestArbitraryRotation(msg, v, 360, 0,
Vector(0,0,1));
328 TestArbitraryRotation(msg, v, 361, 1, expectedVector);
329 TestArbitraryRotation(msg, v, 450, 90, expectedVector);
330 TestArbitraryRotation(msg, v, 539, 179, expectedVector);
332 TestArbitraryRotation(msg, v, 541, 179, -expectedVector);
333 TestArbitraryRotation(msg, v, 630, 90, -expectedVector);
334 TestArbitraryRotation(msg, v, 719, 1, -expectedVector);
335 TestArbitraryRotation(msg, v, 720, 0,
Vector(0,0,1));
337 TestArbitraryRotation(msg, v, -45, 45, -expectedVector);
338 TestArbitraryRotation(msg, v, -90, 90, -expectedVector);
339 TestArbitraryRotation(msg, v, -179, 179, -expectedVector);
341 TestArbitraryRotation(msg, v, -181, 179, expectedVector);
342 TestArbitraryRotation(msg, v, -270, 90, expectedVector);
343 TestArbitraryRotation(msg, v, -359, 1, expectedVector);
344 TestArbitraryRotation(msg, v, -360, 0,
Vector(0,0,1));
345 TestArbitraryRotation(msg, v, -361, 1, -expectedVector);
346 TestArbitraryRotation(msg, v, -450, 90, -expectedVector);
347 TestArbitraryRotation(msg, v, -539, 179, -expectedVector);
349 TestArbitraryRotation(msg, v, -541, 179, expectedVector);
350 TestArbitraryRotation(msg, v, -630, 90, expectedVector);
351 TestArbitraryRotation(msg, v, -719, 1, expectedVector);
352 TestArbitraryRotation(msg, v, -720, 0,
Vector(0,0,1));
359 TestRangeArbitraryRotation(
"[1,0,0]",
Vector(1,0,0),
Vector(1,0,0));
360 TestRangeArbitraryRotation(
"[-1,0,0]",
Vector(-1,0,0),
Vector(-1,0,0));
362 TestRangeArbitraryRotation(
"[0,1,0]",
Vector(0,1,0),
Vector(0,1,0));
363 TestRangeArbitraryRotation(
"[0,-1,0]",
Vector(0,-1,0),
Vector(0,-1,0));
365 TestRangeArbitraryRotation(
"[0,0,1]",
Vector(0,0,1),
Vector(0,0,1));
366 TestRangeArbitraryRotation(
"[0,0,-1]",
Vector(0,0,-1),
Vector(0,0,-1));
368 TestRangeArbitraryRotation(
"[1,1,0]",
Vector(1,1,0),
Vector(1,1,0)/
sqrt(2.0));
369 TestRangeArbitraryRotation(
"[-1,-1,0]",
Vector(-1,-1,0),
Vector(-1,-1,0)/
sqrt(2.0));
371 TestRangeArbitraryRotation(
"[1,0,1]",
Vector(1,0,1),
Vector(1,0,1)/
sqrt(2.0));
372 TestRangeArbitraryRotation(
"[-1,0,-1]",
Vector(-1,0,-1),
Vector(-1,0,-1)/
sqrt(2.0));
374 TestRangeArbitraryRotation(
"[0,1,1]",
Vector(0,1,1),
Vector(0,1,1)/
sqrt(2.0));
375 TestRangeArbitraryRotation(
"[0,-1,-1]",
Vector(0,-1,-1),
Vector(0,-1,-1)/
sqrt(2.0));
377 TestRangeArbitraryRotation(
"[1,1,1]",
Vector(1,1,1),
Vector(1,1,1)/
sqrt(3.0));
378 TestRangeArbitraryRotation(
"[-1,-1,-1]",
Vector(-1,-1,-1),
Vector(-1,-1,-1)/
sqrt(3.0));
382 TestOneRotation(
"rot([1,0,0],180)",
KDL::Rotation::Rot(
KDL::Vector(1,0,0),180*
deg2rad), 180*
deg2rad,
Vector(1,0,0));
384 TestOneRotation(
"rot([-1,0,0],180)",
KDL::Rotation::Rot(
KDL::Vector(-1,0,0),180*
deg2rad), 180*
deg2rad,
Vector(1,0,0));
385 TestOneRotation(
"rot([0,1,0],180)",
KDL::Rotation::Rot(
KDL::Vector(0,1,0),180*
deg2rad), 180*
deg2rad,
Vector(0,1,0));
387 TestOneRotation(
"rot([0,-1,0],180)",
KDL::Rotation::Rot(
KDL::Vector(0,-1,0),180*
deg2rad), 180*
deg2rad,
Vector(0,1,0));
389 TestOneRotation(
"rot([0,0,1],180)",
KDL::Rotation::Rot(
KDL::Vector(0,0,1),180*
deg2rad), 180*
deg2rad,
Vector(0,0,1));
391 TestOneRotation(
"rot([0,0,-1],180)",
KDL::Rotation::Rot(
KDL::Vector(0,0,-1),180*
deg2rad), 180*
deg2rad,
Vector(0,0,1));
393 TestOneRotation(
"rot([1,0,1],180)",
KDL::Rotation::Rot(
KDL::Vector(1,0,1),180*
deg2rad), 180*
deg2rad,
Vector(1,0,1)/
sqrt(2.0));
395 TestOneRotation(
"rot([1,0,1],-180)",
KDL::Rotation::Rot(
KDL::Vector(1,0,1),-180*
deg2rad), 180*
deg2rad,
Vector(1,0,1)/
sqrt(2.0));
396 TestOneRotation(
"rot([-1,0,-1],180)",
KDL::Rotation::Rot(
KDL::Vector(-1,0,-1),180*
deg2rad), 180*
deg2rad,
Vector(1,0,1)/
sqrt(2.0));
398 TestOneRotation(
"rot([-1,0,-1],-180)",
KDL::Rotation::Rot(
KDL::Vector(-1,0,-1),-180*
deg2rad), 180*
deg2rad,
Vector(1,0,1)/
sqrt(2.0));
400 TestOneRotation(
"rot([1,1,0],180)",
KDL::Rotation::Rot(
KDL::Vector(1,1,0),180*
deg2rad), 180*
deg2rad,
Vector(1,1,0)/
sqrt(2.0));
402 TestOneRotation(
"rot([1,1,0],-180)",
KDL::Rotation::Rot(
KDL::Vector(1,1,0),-180*
deg2rad), 180*
deg2rad,
Vector(1,1,0)/
sqrt(2.0));
403 TestOneRotation(
"rot([-1,-1,0],180)",
KDL::Rotation::Rot(
KDL::Vector(-1,-1,0),180*
deg2rad), 180*
deg2rad,
Vector(1,1,0)/
sqrt(2.0));
405 TestOneRotation(
"rot([-1,-1,0],-180)",
KDL::Rotation::Rot(
KDL::Vector(-1,-1,0),-180*
deg2rad), 180*
deg2rad,
Vector(1,1,0)/
sqrt(2.0));
407 TestOneRotation(
"rot([0,1,1],180)",
KDL::Rotation::Rot(
KDL::Vector(0,1,1),180*
deg2rad), 180*
deg2rad,
Vector(0,1,1)/
sqrt(2.0));
409 TestOneRotation(
"rot([0,1,1],-180)",
KDL::Rotation::Rot(
KDL::Vector(0,1,1),-180*
deg2rad), 180*
deg2rad,
Vector(0,1,1)/
sqrt(2.0));
410 TestOneRotation(
"rot([0,-1,-1],180)",
KDL::Rotation::Rot(
KDL::Vector(0,-1,-1),180*
deg2rad), 180*
deg2rad,
Vector(0,1,1)/
sqrt(2.0));
412 TestOneRotation(
"rot([0,-1,-1],-180)",
KDL::Rotation::Rot(
KDL::Vector(0,-1,-1),-180*
deg2rad), 180*
deg2rad,
Vector(0,1,1)/
sqrt(2.0));
414 TestOneRotation(
"rot([1,1,1],180)",
KDL::Rotation::Rot(
KDL::Vector(1,1,1),180*
deg2rad), 180*
deg2rad,
Vector(1,1,1)/
sqrt(3.0));
416 TestOneRotation(
"rot([1,1,1],-180)",
KDL::Rotation::Rot(
KDL::Vector(1,1,1),-180*
deg2rad), 180*
deg2rad,
Vector(1,1,1)/
sqrt(3.0));
417 TestOneRotation(
"rot([-1,-1,-1],180)",
KDL::Rotation::Rot(
KDL::Vector(-1,-1,-1),180*
deg2rad), 180*
deg2rad,
Vector(1,1,1)/
sqrt(3.0));
419 TestOneRotation(
"rot([-1,-1,-1],-180)",
KDL::Rotation::Rot(
KDL::Vector(-1,-1,-1),-180*
deg2rad), 180*
deg2rad,
Vector(1,1,1)/
sqrt(3.0));
428 double roll = -2.9811968953315162;
429 double pitch = -
PI_2;
430 double yaw = -0.1603957582582825;
440 CPPUNIT_ASSERT(0==isnan(theta));
441 CPPUNIT_ASSERT(0==isnan(kdlAxis[0]));
442 CPPUNIT_ASSERT(0==isnan(kdlAxis[1]));
443 CPPUNIT_ASSERT(0==isnan(kdlAxis[2]));
449 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE(
"rot(NON-ORTHOGONAL, 0)", 0.0, angle,
epsilon);
455 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE(
"rot(NON-ORTHOGONAL, PI)",
PI, angle,
epsilon);
467 det = +R(0,0)*(R(1,1)*R(2,2)-R(2,1)*R(1,2))-R(0,1)*(R(1,0)*R(2,2)-R(2,0)*R(1,2))+R(0,2)*(R(1,0)*R(2,1)-R(2,0)*R(1,1));
468 CPPUNIT_ASSERT_EQUAL(det,-1.0);
471 CPPUNIT_ASSERT_ASSERTION_FAIL(CPPUNIT_ASSERT_EQUAL(R,Rout));
474 det = +R(0,0)*(R(1,1)*R(2,2)-R(2,1)*R(1,2))-R(0,1)*(R(1,0)*R(2,2)-R(2,0)*R(1,2))+R(0,2)*(R(1,0)*R(2,1)-R(2,0)*R(1,1));
475 CPPUNIT_ASSERT_EQUAL(det,-1.0);
478 CPPUNIT_ASSERT_ASSERTION_FAIL(CPPUNIT_ASSERT_EQUAL(R,Rout));
492 CPPUNIT_ASSERT_EQUAL(R,R2);
498 CPPUNIT_ASSERT_DOUBLES_EQUAL(y, 0,
epsilon);
499 CPPUNIT_ASSERT_DOUBLES_EQUAL(z, 0,
epsilon);
502 CPPUNIT_ASSERT_EQUAL(R,R2);
506 CPPUNIT_ASSERT_EQUAL(R,R2);
508 CPPUNIT_ASSERT_DOUBLES_EQUAL(x, x2,
epsilon);
509 CPPUNIT_ASSERT_DOUBLES_EQUAL(y, y2,
epsilon);
510 CPPUNIT_ASSERT_DOUBLES_EQUAL(z, z2,
epsilon);
511 CPPUNIT_ASSERT_DOUBLES_EQUAL(w, w2,
epsilon);
517 CPPUNIT_ASSERT_EQUAL(R,R2);
519 CPPUNIT_ASSERT_DOUBLES_EQUAL(x, x2,
epsilon);
520 CPPUNIT_ASSERT_DOUBLES_EQUAL(y, y2,
epsilon);
521 CPPUNIT_ASSERT_DOUBLES_EQUAL(z, z2,
epsilon);
522 CPPUNIT_ASSERT_DOUBLES_EQUAL(w, w2,
epsilon);
529 const Vector& expectedDiff) {
530 CPPUNIT_ASSERT_EQUAL_MESSAGE(msg,
diff(R_a_b1, R_a_b2), expectedDiff);
531 CPPUNIT_ASSERT_EQUAL_MESSAGE(msg,
addDelta(R_a_b1, expectedDiff), R_a_b2);
536 TestOneRotationDiff(
"diff(RotX(0*deg2rad),RotX(0*deg2rad))",
538 TestOneRotationDiff(
"diff(RotX(0*deg2rad),RotX(90*deg2rad))",
540 TestOneRotationDiff(
"diff(RotX(0*deg2rad),RotX(180*deg2rad))",
542 TestOneRotationDiff(
"diff(RotX(0*deg2rad),RotX(270*deg2rad))",
544 TestOneRotationDiff(
"diff(RotX(0*deg2rad),RotX(360*deg2rad))",
546 TestOneRotationDiff(
"diff(RotX(0*deg2rad),RotX(-360*deg2rad))",
548 TestOneRotationDiff(
"diff(RotX(0*deg2rad),RotX(-270*deg2rad))",
550 TestOneRotationDiff(
"diff(RotX(0*deg2rad),RotX(-180*deg2rad))",
552 TestOneRotationDiff(
"diff(RotX(0*deg2rad),RotX(-90*deg2rad))",
554 TestOneRotationDiff(
"diff(RotX(0*deg2rad),RotX(-0*deg2rad))",
557 TestOneRotationDiff(
"diff(RotY(0*deg2rad),RotY(0*deg2rad))",
559 TestOneRotationDiff(
"diff(RotY(0*deg2rad),RotY(90*deg2rad))",
561 TestOneRotationDiff(
"diff(RotY(0*deg2rad),RotY(180*deg2rad))",
563 TestOneRotationDiff(
"diff(RotY(0*deg2rad),RotY(270*deg2rad))",
565 TestOneRotationDiff(
"diff(RotY(0*deg2rad),RotY(360*deg2rad))",
567 TestOneRotationDiff(
"diff(RotY(0*deg2rad),RotY(-360*deg2rad))",
569 TestOneRotationDiff(
"diff(RotY(0*deg2rad),RotY(-270*deg2rad))",
571 TestOneRotationDiff(
"diff(RotY(0*deg2rad),RotY(-180*deg2rad))",
573 TestOneRotationDiff(
"diff(RotY(0*deg2rad),RotY(-90*deg2rad))",
575 TestOneRotationDiff(
"diff(RotY(0*deg2rad),RotY(-0*deg2rad))",
578 TestOneRotationDiff(
"diff(RotZ(0*deg2rad),RotZ(0*deg2rad))",
580 TestOneRotationDiff(
"diff(RotZ(0*deg2rad),RotZ(90*deg2rad))",
582 TestOneRotationDiff(
"diff(RotZ(0*deg2rad),RotZ(180*deg2rad))",
584 TestOneRotationDiff(
"diff(RotZ(0*deg2rad),RotZ(270*deg2rad))",
586 TestOneRotationDiff(
"diff(RotZ(0*deg2rad),RotZ(360*deg2rad))",
588 TestOneRotationDiff(
"diff(RotZ(0*deg2rad),RotZ(-360*deg2rad))",
590 TestOneRotationDiff(
"diff(RotZ(0*deg2rad),RotZ(-270*deg2rad))",
592 TestOneRotationDiff(
"diff(RotZ(0*deg2rad),RotZ(-180*deg2rad))",
594 TestOneRotationDiff(
"diff(RotZ(0*deg2rad),RotZ(-90*deg2rad))",
596 TestOneRotationDiff(
"diff(RotZ(0*deg2rad),RotZ(-0*deg2rad))",
599 TestOneRotationDiff(
"diff(RotX(0*deg2rad),RotZ(90*deg2rad))",
601 TestOneRotationDiff(
"diff(RotX(0*deg2rad),RotY(90*deg2rad))",
603 TestOneRotationDiff(
"diff(RotY(0*deg2rad),RotZ(90*deg2rad))",
606 TestOneRotationDiff(
"diff(Identity(),RotX(90*deg2rad))",
608 TestOneRotationDiff(
"diff(Identity(),RotY(0*deg2rad))",
610 TestOneRotationDiff(
"diff(Identity(),RotZ(0*deg2rad))",
613 TestOneRotationDiff(
"diff(Rotation::RPY(+0*deg2rad,0,-90*deg2rad),Rotation::RPY(-0*deg2rad,0,+90*deg2rad))",
617 TestOneRotationDiff(
"diff(Rotation::RPY(+5*deg2rad,0,-0*deg2rad),Rotation::RPY(-5*deg2rad,0,+0*deg2rad))",
623 TestOneRotationDiff(
"diff(Rotation::RPY(+5*deg2rad,0,-90*deg2rad),Rotation::RPY(-5*deg2rad,0,+90*deg2rad))",
637 CPPUNIT_ASSERT_EQUAL(F,F2);
639 CPPUNIT_ASSERT_EQUAL(F.
Inverse(F*v),v);
640 CPPUNIT_ASSERT_EQUAL(F.
Inverse(F*t),t);
641 CPPUNIT_ASSERT_EQUAL(F.
Inverse(F*w),w);
642 CPPUNIT_ASSERT_EQUAL(F*F.
Inverse(v),v);
643 CPPUNIT_ASSERT_EQUAL(F*F.
Inverse(t),t);
644 CPPUNIT_ASSERT_EQUAL(F*F.
Inverse(w),w);
647 CPPUNIT_ASSERT_EQUAL(F*(F*(F*v)),(F*F*F)*v);
648 CPPUNIT_ASSERT_EQUAL(F*(F*(F*t)),(F*F*F)*t);
649 CPPUNIT_ASSERT_EQUAL(F*(F*(F*w)),(F*F*F)*w);
652 CPPUNIT_ASSERT_EQUAL(F.Inverse()*v,F.Inverse(v));
660 CPPUNIT_ASSERT(
Equal(
Frame().DH(0.0, M_PI_2, 0.36, 0.0), F_DH));
667 CPPUNIT_ASSERT(
Equal(
Frame().DH_Craig1989(0.0, M_PI_2, 0.36, 0.0), F_DH_Craig1989));
669 CPPUNIT_ASSERT_EQUAL(
std::hash<Frame>{}(F),
static_cast<size_t>(6112004106257185417u));
676 for (
int i = 0; i<size; ++i)
685 CPPUNIT_ASSERT(
Equal(random1_copy,random1));
689 CPPUNIT_ASSERT(!
Equal(random1,zero_set_to_zero));
692 CPPUNIT_ASSERT(
Equal(zero_set_to_zero,zero));
695 almost_zero(0) = almost_zero(0)*1e-7;
696 almost_zero(1) = almost_zero(1)*1e-7;
697 almost_zero(2) = almost_zero(2)*1e-7;
698 almost_zero(3) = almost_zero(3)*1e-7;
701 CPPUNIT_ASSERT(
Equal(almost_zero,zero,1));
702 CPPUNIT_ASSERT(
Equal(almost_zero,zero,1e-6));
703 CPPUNIT_ASSERT(!
Equal(almost_zero,zero,1e-8));
707 Add(random1,zero_set_to_zero,sum_random_zero);
708 CPPUNIT_ASSERT(
Equal(random1,sum_random_zero));
713 Add(random1,random2,add_subtract);
714 Subtract(add_subtract,random2,add_subtract);
715 CPPUNIT_ASSERT(
Equal(random1,add_subtract));
719 Multiply(random1,2,random_multiply_by_2);
720 Add(random1,random1,sum_random_same_random);
721 CPPUNIT_ASSERT(
Equal(sum_random_same_random,random_multiply_by_2));
727 Multiply(random1,a,random_multiply_devide);
728 Divide(random_multiply_devide,a,random_multiply_devide);
729 CPPUNIT_ASSERT(
Equal(random_multiply_devide,random1));
740 CPPUNIT_ASSERT_EQUAL((
unsigned int)0,a1.
rows());
741 CPPUNIT_ASSERT(
Equal(a2,a1));
744 CPPUNIT_ASSERT(
Equal(a2,a1));
747 CPPUNIT_ASSERT(
Equal(a2,a1));
750 CPPUNIT_ASSERT(
Equal(a1,a3));
753 CPPUNIT_ASSERT(
Equal(a1,a3));
756 CPPUNIT_ASSERT(
Equal(a1,a3));
759 CPPUNIT_ASSERT(
Equal(a1,a3));
771 CPPUNIT_ASSERT_EQUAL((
unsigned int)3,a1.
rows());
772 CPPUNIT_ASSERT(
Equal(a2,a1));
778 CPPUNIT_ASSERT(
Equal(a1,a2));
779 CPPUNIT_ASSERT_EQUAL(a1(1),a2(1));
784 CPPUNIT_ASSERT(
Equal(a1,a3));