Helios++
Helios software for LiDAR simulations
RigidMotionTest.h
1 #pragma once
2 
3 #include "BaseTest.h"
4 #include <MathConstants.h>
5 #include <maths/rigidmotion/RigidMotionException.h>
6 #include <maths/rigidmotion/RigidMotionEngine.h>
7 #include <maths/rigidmotion/RigidMotionR2Factory.h>
8 #include <maths/rigidmotion/RigidMotionR3Factory.h>
9 #include <scene/primitives/Triangle.h>
10 #include <scene/dynamic/DynMotionEngine.h>
11 #include <scene/dynamic/DynMotion.h>
12 #include <scene/dynamic/DynMovingObject.h>
13 
14 
15 #include <armadillo>
16 
17 using namespace arma;
18 using namespace rigidmotion;
19 
20 namespace HeliosTests{
21 
27 class RigidMotionTest : public BaseTest{
28 public:
29  // *** ATTRIBUTES *** //
30  // ******************** //
34  double const eps = 0.00001;
50  mat R2X;
54  mat R3X;
59 
60  // *** CONSTRUCTOR *** //
61  // ********************* //
65  RigidMotionTest() : BaseTest("Rigid motion test"){}
66 
67  // *** R U N *** //
68  // *************** //
72  bool run() override;
73 
74  // *** SUB-TESTS *** //
75  // ******************* //
80  bool testPureRigidMotion();
85  bool testHeliosRigidMotion();
86 
87  // *** PURE TESTS *** //
88  // ******************** //
93  bool testPureIdentityR2();
98  bool testPureTranslationR2();
103  bool testPureReflectionR2();
108  bool testPureGlideReflectionR2();
113  bool testPureRotationR2();
118  bool testPureIdentityR3();
123  bool testPureTranslationR3();
128  bool testPureReflectionR3();
133  bool testPureGlideReflectionR3();
138  bool testPureRotationR3();
143  bool testPureHelicalR3();
148  bool testPureRotationalSymmetryR3();
149 
150  // *** HELIOS TESTS *** //
151  // ********************** //
156  bool testHeliosIdentityR3(DynMovingObject const &dmo);
161  bool testHeliosTranslationR3(DynMovingObject const &dmo);
166  bool testHeliosReflectionR3(DynMovingObject const &dmo);
171  bool testHeliosGlideReflectionR3(DynMovingObject const &dmo);
176  bool testHeliosRotationR3(DynMovingObject const &dmo);
181  bool testHeliosHelicalR3(DynMovingObject const &dmo);
186  bool testHeliosRotationalSymmetryR3(DynMovingObject const &dmo);
187 };
188 
189 // *** R U N *** //
190 // *************** //
191 bool RigidMotionTest::run(){
192  // Initialize data
193  R2X = mat(2, 5);
194  for(size_t i = 0 ; i < 10 ; ++i){
195  R2X.at(i) = (((double)i)-2.5)*std::pow(-1.0, (double)i);
196  }
197  R3X = mat(3, 5);
198  for(size_t i = 0 ; i < 15 ; ++i){
199  R3X.at(i) = (((double)i)-2.5)*std::pow(-1.0, (double)i);
200  }
201 
202  // Run tests
203  if(!testPureRigidMotion()) return false;
204  if(!testHeliosRigidMotion()) return false;
205  return true;
206 }
207 
208 // *** SUB-TESTS *** //
209 // ******************* //
210 bool RigidMotionTest::testPureRigidMotion(){
211  // R2 tests
212  if(!testPureIdentityR2()) return false;
213  if(!testPureTranslationR2()) return false;
214  if(!testPureReflectionR2()) return false;
215  if(!testPureGlideReflectionR2()) return false;
216  if(!testPureRotationR2()) return false;
217 
218  // R3 tests
219  if(!testPureIdentityR3()) return false;
220  if(!testPureTranslationR3()) return false;
221  if(!testPureReflectionR3()) return false;
222  if(!testPureGlideReflectionR3()) return false;
223  if(!testPureRotationR3()) return false;
224  if(!testPureHelicalR3()) return false;
225  if(!testPureRotationalSymmetryR3()) return false;
226 
227  return true;
228 }
229 bool RigidMotionTest::testHeliosRigidMotion(){
230  // Build dynamic moving object for the tests
231  vector<Primitive *> primitives;
232  Vertex v0(-1.0, -1.0, 0.0);
233  Vertex v1(1.0, -1.0, 0.0);
234  Vertex v2(0.0, 1.0, 0.0);
235  Vertex v3(0.0, 0.0, 1.0);
236  Triangle tr0(v0, v1, v2);
237  Triangle tr1(v0, v1, v3);
238  Triangle tr2(v0, v2, v3);
239  Triangle tr3(v1, v2, v3);
240  primitives.push_back(&tr0);
241  primitives.push_back(&tr1);
242  primitives.push_back(&tr2);
243  primitives.push_back(&tr3);
244  DynMovingObject dmo("HRMTestDMO", primitives);
245  dmo.computeCentroid();
246 
247  // R3 tests
248  if(!testHeliosIdentityR3(dmo)) return false;
249  if(!testHeliosTranslationR3(dmo)) return false;
250  if(!testHeliosReflectionR3(dmo)) return false;
251  if(!testHeliosGlideReflectionR3(dmo)) return false;
252  if(!testHeliosRotationR3(dmo)) return false;
253  if(!testHeliosHelicalR3(dmo)) return false;
254  if(!testHeliosRotationalSymmetryR3(dmo)) return false;
255 
256  return true;
257 }
258 
259 // *** PURE TESTS *** //
260 // ******************** //
261 bool RigidMotionTest::testPureIdentityR2(){
262  RigidMotion f = rm2f.makeIdentity();
263  mat Y = rme.apply(f, R2X);
264  mat Z = abs(Y-R2X);
265  bool passed = !any(vectorise(Z) > eps);
266  if(!passed) return passed;
267 
268  passed = RigidMotion::SuperType::R2_BASE == f.findSuperType();
269  if(!passed) return passed;
270  passed = f.hasFixedPoints();
271  if(!passed) return passed;
272  passed = RigidMotion::Type::IDENTITY_R2 == f.findType();
273  if(!passed) return passed;
274  passed = f.findInvariantDimensionality() == 2;
275  if(!passed) return passed;
276  size_t dim;
277  mat L = rme.computeFixedPoints(f, dim);
278  mat EL = eye(2, 2);
279  passed = !any(vectorise(abs(L-EL)) > eps);
280  if(!passed) return passed;
281 
282  return passed;
283 }
284 bool RigidMotionTest::testPureTranslationR2(){
285  colvec shift(std::vector<double>({1.67, -2.27}));
286  RigidMotion f = rm2f.makeTranslation(shift);
287  mat Y = rme.apply(f, R2X);
288  mat EY = R2X.each_col() + shift;
289  mat Z = abs(Y-EY);
290  bool passed = !any(vectorise(Z) > eps);
291  if(!passed) return passed;
292 
293  passed = RigidMotion::SuperType::R2_BASE == f.findSuperType();
294  if(!passed) return passed;
295  passed = !f.hasFixedPoints();
296  if(!passed) return passed;
297  passed = RigidMotion::Type::TRANSLATION_R2 == f.findType();
298  if(!passed) return passed;
299  passed = f.findInvariantDimensionality() == 2;
300  if(!passed) return passed;
301  try{
302  passed = false;
303  size_t dim;
304  mat L = rme.computeFixedPoints(f, dim);
305  }
306  catch(RigidMotionException &ex){
307  passed = true;
308  }
309  if(!passed) return passed;
310 
311  return passed;
312 }
313 bool RigidMotionTest::testPureReflectionR2(){
314  colvec axis(std::vector<double>({2.1, -3.9}));
315  colvec X(std::vector<double>({-3, 1}));
316  RigidMotion f = rm2f.makeReflection(axis);
317  colvec Y = rme.apply(f, X);
318  colvec EY(std::vector<double>({0.81651376, 3.05504587}));
319  bool passed = !any(vectorise(abs(Y-EY)) > eps);
320  if(!passed) return passed;
321 
322  passed = RigidMotion::SuperType::R2_REFLECTION == f.findSuperType();
323  if(!passed) return passed;
324  passed = f.hasFixedPoints();
325  if(!passed) return passed;
326  passed = RigidMotion::Type::REFLECTION_R2 == f.findType();
327  if(!passed) return passed;
328  passed = f.findInvariantDimensionality() == 1;
329  if(!passed) return passed;
330  size_t dim;
331  mat L = rme.computeFixedPoints(f, dim);
332  if(L(0,0) < 0.0) L = -L; // Solve sign ambiguity wrt expected solution
333  mat EL("0.47409982; -0.8804711");
334  passed = !any(vectorise(abs(L-EL)) > eps);
335  if(!passed) return passed;
336 
337  mat Y2 = rme.apply(f, R2X);
338  mat EY2(2, 5);
339  EY2.at(0, 0) = 0.12385321; EY2.at(1, 0) = 2.91284404;
340  EY2.at(0, 1) = 0.69266055; EY2.at(1, 1) = 0.14220183;
341  EY2.at(0, 2) = 1.26146789; EY2.at(1, 2) = -2.62844037;
342  EY2.at(0, 3) = 1.83027523; EY2.at(1, 3) = -5.39908257;
343  EY2.at(0, 4) = 2.39908257; EY2.at(1, 4) = -8.16972477;
344  mat Z = abs(Y2-EY2);
345  passed = !any(vectorise(Z) > eps);
346  if(!passed) return passed;
347 
348  axis = std::vector<double>({2.1, 0.9});
349  f = rm2f.makeReflection(axis);
350  mat Y3 = rme.apply(f, R2X);
351  mat EY3(2, 5);
352  EY3.at(0, 0) = -0.63793103; EY3.at(1, 0) = -2.84482759;
353  EY3.at(0, 1) = -0.70689655; EY3.at(1, 1) = -0.01724138;
354  EY3.at(0, 2) = -0.77586207; EY3.at(1, 2) = 2.81034483;
355  EY3.at(0, 3) = -0.84482759; EY3.at(1, 3) = 5.63793103;
356  EY3.at(0, 4) = -0.9137931; EY3.at(1, 4) = 8.46551724;
357  Z = abs(Y3-EY3);
358  passed = !any(vectorise(Z) > eps);
359  if(!passed) return passed;
360 
361  return passed;
362 }
363 bool RigidMotionTest::testPureGlideReflectionR2(){
364  colvec axis(std::vector<double>({2.1, 1.9}));
365  RigidMotion f = rm2f.makeGlideReflection(axis, 2);
366  mat Y = rme.apply(f, R2X);
367  mat EY(2, 5);
368  EY.at(0, 0) = 2.7262137; EY.at(1, 0) = -1.29533046;
369  EY.at(0, 1) = 0.93569001; EY.at(1, 1) = 0.89419573;
370  EY.at(0, 2) = -0.85483368; EY.at(1, 2) = 3.08372191;
371  EY.at(0, 3) = -2.64535737; EY.at(1, 3) = 5.27324809;
372  EY.at(0, 4) = -4.43588106; EY.at(1, 4) = 7.46277428;
373  mat Z = abs(Y-EY);
374  bool passed = !any(vectorise(Z) > eps);
375  if(!passed) return passed;
376 
377  passed = RigidMotion::SuperType::R2_REFLECTION == f.findSuperType();
378  if(!passed) return passed;
379  passed = !f.hasFixedPoints();
380  if(!passed) return passed;
381  passed = RigidMotion::Type::GLIDE_REFLECTION_R2 == f.findType();
382  if(!passed) return passed;
383  passed = f.findInvariantDimensionality() == 1;
384  if(!passed) return passed;
385  size_t dim;
386  mat L = rme.computeAssociatedInvariant(f, dim);
387  if(L(0, 0) < 0) L = -L; // Solve sign ambiguity wrt expected solution
388  mat EL("0.74153578; 0.67091332");
389  passed = !any(vectorise(abs(L-EL)) > eps);
390  if(!passed) return passed;
391 
392  f = rm2f.makeTranslation(2*axis/norm(axis));
393  RigidMotion g = rm2f.makeReflection(axis);
394  RigidMotion h = rme.compose(f, g);
395  mat Y2 = rme.apply(h, R2X);
396  Z = abs(Y-Y2);
397  passed = !any(vectorise(Z) > eps);
398  if(!passed) return passed;
399 
400  return passed;
401 }
402 
403 bool RigidMotionTest::testPureRotationR2(){
404  colvec center = R2X.col(2);
405  RigidMotion f = rm2f.makeRotation(-M_PI/3, center);
406  mat Y = rme.apply(f, R2X);
407  mat EY(2, 5);
408  EY.at(0, 0) = 2.96410162; EY.at(1, 0) = 2.96410162;
409  EY.at(0, 1) = 2.23205081; EY.at(1, 1) = 0.23205081;
410  EY.at(0, 2) = 1.5; EY.at(1, 2) = -2.5;
411  EY.at(0, 3) = 0.76794919; EY.at(1, 3) = -5.23205081;
412  EY.at(0, 4) = 0.03589838; EY.at(1, 4) = -7.96410162;
413  mat Z = abs(Y-EY);
414  bool passed = !any(vectorise(Z) > eps);
415  if(!passed) return passed;
416 
417  passed = RigidMotion::SuperType::R2_ROTATION == f.findSuperType();
418  if(!passed) return passed;
419  passed = f.hasFixedPoints();
420  if(!passed) return passed;
421  passed = RigidMotion::Type::ROTATION_R2 == f.findType();
422  if(!passed) return passed;
423  passed = f.findInvariantDimensionality() == 0;
424  if(!passed) return passed;
425  size_t dim;
426  mat L = rme.computeFixedPoints(f, dim);
427  mat EL("1.5; -2.5");
428  passed = !any(vectorise(abs(L-EL)) > eps);
429  if(!passed) return passed;
430 
431  return passed;
432 }
433 
434 bool RigidMotionTest::testPureIdentityR3(){
435  RigidMotion f = rm3f.makeIdentity();
436  mat Y = rme.apply(f, R3X);
437  mat Z = abs(Y-R3X);
438  bool passed = !any(vectorise(Z) > eps);
439  if(!passed) return passed;
440 
441  passed = RigidMotion::SuperType::R3_BASE == f.findSuperType();
442  if(!passed) return passed;
443  passed = f.hasFixedPoints();
444  if(!passed) return passed;
445  passed = RigidMotion::Type::IDENTITY_R3 == f.findType();
446  if(!passed) return passed;
447  passed = f.findInvariantDimensionality() == 3;
448  if(!passed) return passed;
449  size_t dim;
450  mat L = rme.computeFixedPoints(f, dim);
451  mat EL(
452  "1 0 0;"
453  "0 1 0;"
454  "0 0 1"
455  );
456  passed = !any(vectorise(L-EL) > eps);
457  if(!passed) return passed;
458 
459  return passed;
460 }
461 
462 bool RigidMotionTest::testPureTranslationR3(){
463  colvec shift(std::vector<double>({1.1, -3.39, 0.24}));
464  RigidMotion f = rm3f.makeTranslation(shift);
465  mat Y = rme.apply(f, R3X);
466  mat EY = R3X.each_col() + shift;
467  mat Z = abs(Y-EY);
468  bool passed = !any(vectorise(Z) > eps);
469  if(!passed) return passed;
470 
471  passed = RigidMotion::SuperType::R3_BASE == f.findSuperType();
472  if(!passed) return passed;
473  passed = !f.hasFixedPoints();
474  if(!passed) return passed;
475  passed = RigidMotion::Type::TRANSLATION_R3 == f.findType();
476  if(!passed) return passed;
477  passed = f.findInvariantDimensionality() == 3;
478  if(!passed) return passed;
479 
480  return passed;
481 }
482 
483 bool RigidMotionTest::testPureReflectionR3(){
484  colvec ortho(std::vector<double>({1.0, 0.5, 0.3}));
485  RigidMotion f = rm3f.makeReflection(ortho);
486  mat Y = rme.apply(f, R3X);
487  mat EY(
488  "0.3358209 0.24626866 -0.82835821 1.41044776 -1.99253731; "
489  "2.91791045 1.87313433 -6.6641791 11.45522388 -16.24626866; "
490  "0.35074627 -2.2761194 4.20149254 -6.12686567 8.05223881"
491  );
492  mat Z = abs(Y-EY);
493  bool passed = !any(vectorise(Z) > eps);
494  if(!passed) return passed;
495  Y = rme.apply(f, Y);
496  Z = abs(Y-R3X);
497  passed = !any(vectorise(Z) > eps);
498  if(!passed) return passed;
499 
500  passed = RigidMotion::SuperType::R3_REFLECTION == f.findSuperType();
501  if(!passed) return passed;
502  passed = f.hasFixedPoints();
503  if(!passed) return passed;
504  passed = RigidMotion::Type::REFLECTION_R3 == f.findType();
505  if(!passed) return passed;
506  passed = f.findInvariantDimensionality() == 2;
507  if(!passed) return passed;
508  size_t dim;
509  mat L = rme.computeFixedPoints(f, dim);
510  if(L(0,0) < 0) L = -L; // Solve sign ambiguity wrt expected solution
511  mat EL("0.86386843; 0.43193421; 0.25916053");
512  passed = !any(vectorise(abs(L-EL)) > eps);
513  if(!passed) return passed;
514 
515  f = rm3f.makeReflectionX();
516  Y = rme.apply(f, R3X);
517  EY = mat(
518  "2.5 0.5 -3.5 6.5 -9.5;"
519  "1.5 1.5 -4.5 7.5 -10.5;"
520  "-0.5 -2.5 5.5 -8.5 11.5"
521  );
522  Z = abs(Y-EY);
523  passed = !any(vectorise(Z) > eps);
524  if(!passed) return passed;
525  f = rm3f.makeReflection(colvec(std::vector<double>({1.0, 0.0, 0.0})));
526  Y = rme.apply(f, R3X);
527  Z = abs(Y-EY);
528  passed = !any(vectorise(Z) > eps);
529  if(!passed) return passed;
530 
531  f = rm3f.makeReflectionY();
532  Y = rme.apply(f, R3X);
533  EY = mat(
534  "-2.5 -0.5 3.5 -6.5 9.5;"
535  "-1.5 -1.5 4.5 -7.5 10.5;"
536  "-0.5 -2.5 5.5 -8.5 11.5"
537  );
538  Z = abs(Y-EY);
539  passed = !any(vectorise(Z) > eps);
540  if(!passed) return passed;
541  f = rm3f.makeReflection(colvec(std::vector<double>({0.0, 1.0, 0.0})));
542  Y = rme.apply(f, R3X);
543  Z = abs(Y-EY);
544  passed = !any(vectorise(Z) > eps);
545  if(!passed) return passed;
546 
547  f = rm3f.makeReflectionY();
548  Y = rme.apply(f, R3X);
549  EY = mat(
550  "-2.5 -0.5 3.5 -6.5 9.5;"
551  "-1.5 -1.5 4.5 -7.5 10.5;"
552  "-0.5 -2.5 5.5 -8.5 11.5"
553  );
554  Z = abs(Y-EY);
555  passed = !any(vectorise(Z) > eps);
556  if(!passed) return passed;
557  f = rm3f.makeReflection(colvec(std::vector<double>({0.0, 1.0, 0.0})));
558  Y = rme.apply(f, R3X);
559  Z = abs(Y-EY);
560  passed = !any(vectorise(Z) > eps);
561  if(!passed) return passed;
562 
563  f = rm3f.makeReflectionZ();
564  Y = rme.apply(f, R3X);
565  EY = mat(
566  "-2.5 -0.5 3.5 -6.5 9.5;"
567  "1.5 1.5 -4.5 7.5 -10.5;"
568  "0.5 2.5 -5.5 8.5 -11.5;"
569  );
570  Z = abs(Y-EY);
571  passed = !any(vectorise(Z) > eps);
572  if(!passed) return passed;
573  f = rm3f.makeReflection(colvec(std::vector<double>({0.0, 0.0, 1.0})));
574  Y = rme.apply(f, R3X);
575  Z = abs(Y-EY);
576  passed = !any(vectorise(Z) > eps);
577  if(!passed) return passed;
578 
579  f = rm3f.makeReflectionX();
580  RigidMotion g = rm3f.makeReflectionY();
581  f = rme.compose(f, g);
582  g = rm3f.makeReflectionZ();
583  f = rme.compose(f, g);
584  Y = rme.apply(f, R3X);
585  EY = mat(
586  "2.5 0.5 -3.5 6.5 -9.5;"
587  "-1.5 -1.5 4.5 -7.5 10.5;"
588  "0.5 2.5 -5.5 8.5 -11.5;"
589  );
590  Z = abs(Y-EY);
591  passed = !any(vectorise(Z) > eps);
592  if(!passed) return passed;
593 
594  return passed;
595 }
596 
597 bool RigidMotionTest::testPureGlideReflectionR3(){
598  colvec ortho(std::vector<double>({1.0, 0.5, 0.3}));
599  colvec shift(std::vector<double>({-4.0/5.0, 1, 1}));
600  RigidMotion f = rm3f.makeGlideReflection(ortho, shift);
601  mat Y = rme.apply(f, R3X);
602  mat EY = (
603  "-0.4641791 -0.55373134 -1.62835821 0.61044776 -2.79253731;"
604  "3.91791045 2.87313433 -5.6641791 12.45522388 -15.24626866;"
605  "1.35074627 -1.2761194 5.20149254 -5.12686567 9.05223881"
606  );
607  mat Z = abs(Y-EY);
608  bool passed = !any(vectorise(Z) > eps);
609  if(!passed) return passed;
610 
611  passed = RigidMotion::SuperType::R3_REFLECTION == f.findSuperType();
612  if(!passed) return passed;
613  passed = !f.hasFixedPoints();
614  if(!passed) return passed;
615  passed = RigidMotion::Type::GLIDE_REFLECTION_R3 == f.findType();
616  if(!passed) return passed;
617  passed = f.findInvariantDimensionality() == 2;
618  if(!passed) return passed;
619  size_t dim;
620  mat L = rme.computeAssociatedInvariant(f, dim);
621  if(L(0, 0) < 0) L = -L; // Solve sign ambiguity wrt expected solution
622  mat EL("0.86386843; 0.43193421; 0.25916053");
623  passed = !any(vectorise(abs(L-EL)) > eps);
624  if(!passed) return passed;
625 
626  f = rm3f.makeGlideReflection(ortho, -shift);
627  Y = rme.apply(f, Y);
628  Z = abs(Y-R3X);
629  passed = !any(vectorise(Z) > eps);
630  if(!passed) return passed;
631 
632  try{
633  passed = false;
634  f = rm3f.makeGlideReflection(ortho, colvec("1 1 1"));
635  }
636  catch(RigidMotionException &rmex){
637  passed = true;
638  }
639 
640  return passed;
641 }
642 
643 bool RigidMotionTest::testPureRotationR3(){
644  colvec axis(std::vector<double>({0.4, 0.2, 1.0}));
645  double theta = 4.5;
646  RigidMotion f = rm3f.makeRotation(axis, theta);
647  mat Y = rme.apply(f, R3X);
648  mat EY(
649  "1.4704453 0.92147891 -3.31340313 5.70532734 -8.09725156;"
650  "1.49407244 -1.24669143 0.99931042 -0.75192941 0.5045484;"
651  "-2.08699261 -2.51925328 7.12549917 -11.73174506 16.33799094"
652  );
653  mat Z = abs(Y-EY);
654  bool passed = !any(vectorise(Z) > eps);
655  if(!passed) return passed;
656 
657  passed = RigidMotion::SuperType::R3_ROTATION == f.findSuperType();
658  if(!passed) return passed;
659  passed = f.hasFixedPoints();
660  if(!passed) return passed;
661  passed = RigidMotion::Type::ROTATION_R3 == f.findType();
662  if(!passed) return passed;
663  passed = f.findInvariantDimensionality() == 1;
664  if(!passed) return passed;
665  size_t dim;
666  mat L = rme.computeFixedPoints(f, dim);
667  if(L(0, 0) < 0) L = -L; // Solve sign ambiguity wrt expected solution
668  mat EL("0.36514837; 0.18257419; 0.91287093");
669  passed = !any(vectorise(abs(L-EL)) > eps);
670  if(!passed) return passed;
671 
672  f = rm3f.makeRotation(axis, -theta);
673  Y = rme.apply(f, Y);
674  Z = abs(Y-R3X);
675  passed = !any(vectorise(Z) > eps);
676  if(!passed) return passed;
677 
678  f = rm3f.makeRotationX(theta);
679  Y = rme.apply(f, R3X);
680  EY = mat(
681  "-2.5 -0.5 3.5 -6.5 9.5;"
682  "-0.80495876 -2.76001899 6.32499674 -9.8899745 13.45495225;"
683  "-1.36089728 -0.93930568 3.23950863 -5.53971159 7.83991454"
684  );
685  Z = abs(Y-EY);
686  passed = !any(vectorise(Z) > eps);
687  if(!passed) return passed;
688 
689  f = rm3f.makeRotationY(theta);
690  Y = rme.apply(f, R3X);
691  EY = mat(
692  "1.01575456 2.54922319 -6.11420095 9.6791787 -13.24415645;"
693  "1.5 1.5 -4.5 7.5 -10.5;"
694  "-2.33842739 0.03822444 2.26197851 -4.56218147 6.86238442"
695  );
696  Z = abs(Y-EY);
697  passed = !any(vectorise(Z) > eps);
698  if(!passed) return passed;
699 
700  f = rm3f.makeRotationZ(theta);
701  Y = rme.apply(f, R3X);
702  EY = mat(
703  "1.99328468 1.57169308 -5.13667083 8.70164858 -12.26662633;"
704  "2.1276316 0.17257136 -2.47277431 4.77297727 -7.07318022;"
705  "-0.5 -2.5 5.5 -8.5 11.5"
706  );
707  Z = abs(Y-EY);
708  passed = !any(vectorise(Z) > eps);
709  if(!passed) return passed;
710 
711  return passed;
712 }
713 
714 bool RigidMotionTest::testPureHelicalR3(){
715  colvec axis(std::vector<double>({0.4, 0.2, 1.0}));
716  double theta = 4.5;
717  double glide = 5.0;
718  RigidMotion f = rm3f.makeHelical(axis, theta, glide);
719  mat Y = rme.apply(f, R3X);
720  mat EY(
721  "3.29618716 2.74722077 -1.48766127 7.5310692 -6.2715097;"
722  "2.40694337 -0.3338205 1.91218135 0.16094152 1.41741933;"
723  "2.47736204 2.04510137 11.68985381 -7.16739041 20.90234559"
724  );
725  mat Z = abs(Y-EY);
726  bool passed = !any(vectorise(Z) > eps);
727  if(!passed) return passed;
728 
729  passed = RigidMotion::SuperType::R3_ROTATION == f.findSuperType();
730  if(!passed) return passed;
731  passed = !f.hasFixedPoints();
732  if(!passed) return passed;
733  passed = RigidMotion::Type::HELICAL_R3 == f.findType();
734  if(!passed) return passed;
735  passed = f.findInvariantDimensionality() == 1;
736  if(!passed) return passed;
737  size_t dim;
738  mat L = rme.computeAssociatedInvariant(f, dim);
739  if(L(0, 0) < 0) L = -L; // Solve sign ambiguity wrt expected solution
740  mat EL("0.36514837; 0.18257419; 0.91287093");
741  passed = !any(vectorise(abs(L-EL)) > eps);
742  if(!passed) return passed;
743 
744  f = rm3f.makeHelical(axis, -theta, -glide);
745  Y = rme.apply(f, Y);
746  Z = abs(Y-R3X);
747  passed = !any(vectorise(Z) > eps);
748  if(!passed) return passed;
749 
750  f = rm3f.makeHelicalX(theta, glide);
751  Y = rme.apply(f, R3X);
752  EY = mat(
753  "2.5 4.5 8.5 -1.5 14.5;"
754  "-0.80495876 -2.76001899 6.32499674 -9.8899745 13.45495225;"
755  "-1.36089728 -0.93930568 3.23950863 -5.53971159 7.83991454"
756  );
757  Z = abs(Y-EY);
758  passed = !any(vectorise(Z) > eps);
759  if(!passed) return passed;
760 
761  f = rm3f.makeHelicalY(theta, glide);
762  Y = rme.apply(f, R3X);
763  EY = mat(
764  "1.01575456 2.54922319 -6.11420095 9.6791787 -13.24415645;"
765  "6.5 6.5 0.5 12.5 -5.5;"
766  "-2.33842739 0.03822444 2.26197851 -4.56218147 6.86238442"
767  );
768  Z = abs(Y-EY);
769  passed = !any(vectorise(Z) > eps);
770  if(!passed) return passed;
771 
772  f = rm3f.makeHelicalZ(theta, glide);
773  Y = rme.apply(f, R3X);
774  EY = mat(
775  "1.99328468 1.57169308 -5.13667083 8.70164858 -12.26662633;"
776  "2.1276316 0.17257136 -2.47277431 4.77297727 -7.07318022;"
777  "4.5 2.5 10.5 -3.5 16.5"
778 
779  );
780  Z = abs(Y-EY);
781  passed = !any(vectorise(Z) > eps);
782  if(!passed) return passed;
783 
784  return passed;
785 }
786 
787 bool RigidMotionTest::testPureRotationalSymmetryR3(){
788  colvec axis(std::vector<double>({1.0, 0.5, 0.3}));
789  double theta = 4.5;
790  RigidMotion f = rm3f.makeRotationalSymmetry(axis, theta);
791  mat Y = rme.apply(f, R3X);
792  mat EY(
793  "2.23713056 1.83545467 -5.90803989 9.98062512 -14.05321035;"
794  "0.45443109 -2.15342901 3.85242692 -5.55142484 7.25042276;"
795  "-1.8811537 -0.8624672 3.6060881 -6.349709 9.0933299"
796  );
797  mat Z = abs(Y-EY);
798  bool passed = !any(vectorise(Z) > eps);
799  if(!passed) return passed;
800  f = rm3f.makeRotationalSymmetry(axis, -theta);
801  Y = rme.apply(f, Y);
802  Z = abs(Y-R3X);
803  passed = !any(vectorise(Z) > eps);
804  if(!passed) return passed;
805 
806  passed = RigidMotion::SuperType::R3_ROTATIONAL_SYMMETRY==f.findSuperType();
807  if(!passed) return passed;
808  passed = f.hasFixedPoints();
809  if(!passed) return passed;
810  passed = RigidMotion::Type::ROTATIONAL_SYMMETRY_R3 == f.findType();
811  if(!passed) return passed;
812  passed = f.findInvariantDimensionality() == 0;
813  if(!passed) return passed;
814  size_t dim;
815  mat L = rme.computeFixedPoints(f, dim);
816  mat EL("0; 0; 0;");
817  passed = !any(vectorise(abs(L-EL)) > eps);
818  if(!passed) return passed;
819 
820  f = rm3f.makeRotationalSymmetryX(theta);
821  Y = rme.apply(f, R3X);
822  EY = mat(
823  "2.5 0.5 -3.5 6.5 -9.5;"
824  "-0.80495876 -2.76001899 6.32499674 -9.8899745 13.45495225;"
825  "-1.36089728 -0.93930568 3.23950863 -5.53971159 7.83991454"
826  );
827  Z = abs(Y-EY);
828  passed = !any(vectorise(Z) > eps);
829  if(!passed) return passed;
830 
831  f = rm3f.makeRotationalSymmetryY(theta);
832  Y = rme.apply(f, R3X);
833  EY = mat(
834  "1.01575456 2.54922319 -6.11420095 9.6791787 -13.24415645;"
835  "-1.5 -1.5 4.5 -7.5 10.5;"
836  "-2.33842739 0.03822444 2.26197851 -4.56218147 6.86238442"
837  );
838  Z = abs(Y-EY);
839  passed = !any(vectorise(Z) > eps);
840  if(!passed) return passed;
841 
842  f = rm3f.makeRotationalSymmetryZ(theta);
843  Y = rme.apply(f, R3X);
844  EY = mat(
845  "1.99328468 1.57169308 -5.13667083 8.70164858 -12.26662633;"
846  "2.1276316 0.17257136 -2.47277431 4.77297727 -7.07318022;"
847  "0.5 2.5 -5.5 8.5 -11.5"
848  );
849  Z = abs(Y-EY);
850  passed = !any(vectorise(Z) > eps);
851  if(!passed) return passed;
852 
853  f = rm3f.makeRotationalSymmetry(axis, theta, colvec("1;1;-1"));
854  Y = rme.apply(f, R3X);
855  EY = mat(
856  "3.47911047 3.07743457 -4.66605999 11.22260503 -12.81123044;"
857  "3.11639653 0.50853643 6.51439236 -2.8894594 9.91238819;"
858  "-2.45769578 -1.43900929 3.02954602 -6.92625109 8.51678782"
859  );
860  Z = abs(Y-EY);
861  passed = !any(vectorise(Z) > eps);
862  if(!passed) return passed;
863  f = rm3f.makeRotationalSymmetry(axis, theta);
864  RigidMotion g = rm3f.makeTranslation(colvec("-1;-1;1"));
865  RigidMotion h = rm3f.makeTranslation(colvec("1;1;-1"));
866  f = f.compose(g);
867  f = h.compose(f);
868  mat Y2 = rme.apply(f, R3X);
869  Z = abs(Y2-EY) + abs(Y-EY);
870  passed = !any(vectorise(Z) > eps);
871  if(!passed) return passed;
872  L = rme.computeFixedPoints(f, dim);
873  passed = !any(vectorise(abs(L-colvec("1;1;-1"))) > eps);
874  if(!passed) return passed;
875 
876  return passed;
877 }
878 
879 
880 // *** HELIOS TESTS *** //
881 // ********************** //
882 bool RigidMotionTest::testHeliosIdentityR3(DynMovingObject const &dmo){
883  // Compute
884  arma::mat X = dmo.positionMatrixFromPrimitives();
885  DynMovingObject dynObj = dmo;
886  RigidMotion id = rm3f.makeIdentity();
887  DynMotion dm(id, false);
888  arma::mat dmeY = dme.apply(dm, X, dynObj);
889  arma::mat rmeY = rme.apply(static_cast<RigidMotion>(dm), X);
890 
891  // Delete primitives
892  for(Primitive *primitive : dynObj.getPrimitives()) delete primitive;
893 
894  // Validate
895  size_t const m = dmeY.n_elem;
896  size_t const n = rmeY.n_elem;
897  if(m!=n) return false;
898  for(size_t i = 0 ; i < m ; ++i)
899  if(std::fabs(dmeY[i]-rmeY[i]) > eps) return false;
900  return true;
901 }
902 bool RigidMotionTest::testHeliosTranslationR3(DynMovingObject const &dmo){
903  // Compute
904  arma::mat X = dmo.positionMatrixFromPrimitives();
905  DynMovingObject dynObj = dmo;
906  RigidMotion tr = rm3f.makeTranslation(arma::colvec("-0.1;0.1;0"));
907  DynMotion dm(tr, false);
908  arma::mat dmeY = dme.apply(dm, X, dynObj);
909  arma::mat rmeY = rme.apply(static_cast<RigidMotion>(dm), X);
910 
911  // Delete primitives
912  for(Primitive *primitive : dynObj.getPrimitives()) delete primitive;
913 
914  // Validate
915  size_t const m = dmeY.n_elem;
916  size_t const n = rmeY.n_elem;
917  if(m!=n) return false;
918  for(size_t i = 0 ; i < m ; ++i)
919  if(std::fabs(dmeY[i]-rmeY[i]) > eps) return false;
920  return true;
921 }
922 bool RigidMotionTest::testHeliosReflectionR3(DynMovingObject const &dmo){
923  // Compute
924  arma::mat X = dmo.positionMatrixFromPrimitives();
925  arma::colvec O = dmo.getCentroid();
926  DynMovingObject dynObj = dmo;
927  RigidMotion rf = rme.compose(
928  rm3f.makeTranslation(O),
929  rme.compose(rm3f.makeReflectionZ(), rm3f.makeTranslation(-O))
930  );
931  DynMotion dm(rm3f.makeReflectionZ(), true);
932  arma::mat dmeY = dme.apply(dm, X, dynObj);
933  arma::mat rmeY = rme.apply(rf, X);
934 
935  // Delete primitives
936  for(Primitive *primitive : dynObj.getPrimitives()) delete primitive;
937 
938  // Validate
939  size_t const m = dmeY.n_elem;
940  size_t const n = rmeY.n_elem;
941  if(m!=n) return false;
942  for(size_t i = 0 ; i < m ; ++i)
943  if(std::fabs(dmeY[i]-rmeY[i]) > eps) return false;
944  return true;
945 }
946 bool RigidMotionTest::testHeliosGlideReflectionR3(DynMovingObject const &dmo){
947  // Compute
948  arma::mat X = dmo.positionMatrixFromPrimitives();
949  arma::colvec O = dmo.getCentroid();
950  DynMovingObject dynObj = dmo;
951  RigidMotion rf = rme.compose(
952  rm3f.makeTranslation(O),
953  rme.compose(
954  rm3f.makeGlideReflection(
955  arma::colvec("0;0;1"),
956  arma::colvec("0.5;0.5;0")
957  ),
958  rm3f.makeTranslation(-O)
959  )
960  );
961  DynMotion dm(
962  rm3f.makeGlideReflection(
963  arma::colvec("0;0;1"),
964  arma::colvec("0.5;0.5;0")
965  ),
966  true
967  );
968  arma::mat dmeY = dme.apply(dm, X, dynObj);
969  arma::mat rmeY = rme.apply(rf, X);
970 
971  // Delete primitives
972  for(Primitive *primitive : dynObj.getPrimitives()) delete primitive;
973 
974  // Validate
975  size_t const m = dmeY.n_elem;
976  size_t const n = rmeY.n_elem;
977  if(m!=n) return false;
978  for(size_t i = 0 ; i < m ; ++i)
979  if(std::fabs(dmeY[i]-rmeY[i]) > eps) return false;
980  return true;
981 }
982 bool RigidMotionTest::testHeliosRotationR3(DynMovingObject const &dmo){
983  // Compute
984  arma::mat X = dmo.positionMatrixFromPrimitives();
985  arma::colvec O = dmo.getCentroid();
986  DynMovingObject dynObj = dmo;
987  RigidMotion rot = rme.compose(
988  rm3f.makeTranslation(O),
989  rme.compose(rm3f.makeRotationZ(0.67), rm3f.makeTranslation(-O))
990  );
991  DynMotion dm(rm3f.makeRotationZ(0.67), true);
992  arma::mat dmeY = dme.apply(dm, X, dynObj);
993  arma::mat rmeY = rme.apply(rot, X);
994 
995  // Delete primitives
996  for(Primitive *primitive : dynObj.getPrimitives()) delete primitive;
997 
998  // Validate
999  size_t const m = dmeY.n_elem;
1000  size_t const n = rmeY.n_elem;
1001  if(m!=n) return false;
1002  for(size_t i = 0 ; i < m ; ++i)
1003  if(std::fabs(dmeY[i]-rmeY[i]) > eps) return false;
1004  return true;
1005 }
1006 bool RigidMotionTest::testHeliosHelicalR3(DynMovingObject const &dmo){
1007  // Compute
1008  arma::mat X = dmo.positionMatrixFromPrimitives();
1009  arma::colvec O = dmo.getCentroid();
1010  DynMovingObject dynObj = dmo;
1011  RigidMotion hm = rme.compose(
1012  rm3f.makeTranslation(O),
1013  rme.compose(rm3f.makeHelicalZ(0.5, 1.0), rm3f.makeTranslation(-O))
1014  );
1015  DynMotion dm(rm3f.makeHelicalZ(0.5, 1.0), true);
1016  arma::mat dmeY = dme.apply(dm, X, dynObj);
1017  arma::mat rmeY = rme.apply(hm, X);
1018 
1019  // Delete primitives
1020  for(Primitive *primitive : dynObj.getPrimitives()) delete primitive;
1021 
1022  // Validate
1023  size_t const m = dmeY.n_elem;
1024  size_t const n = rmeY.n_elem;
1025  if(m!=n) return false;
1026  for(size_t i = 0 ; i < m ; ++i)
1027  if(std::fabs(dmeY[i]-rmeY[i]) > eps) return false;
1028  return true;
1029 }
1030 bool RigidMotionTest::testHeliosRotationalSymmetryR3(
1031  DynMovingObject const &dmo
1032 ){
1033  // Compute
1034  arma::mat X = dmo.positionMatrixFromPrimitives();
1035  arma::colvec O = dmo.getCentroid();
1036  DynMovingObject dynObj = dmo;
1037  RigidMotion rs = rme.compose(
1038  rm3f.makeTranslation(O),
1039  rme.compose(
1040  rm3f.makeRotationalSymmetryZ(0.34),
1041  rm3f.makeTranslation(-O)
1042  )
1043  );
1044  DynMotion dm(rm3f.makeRotationalSymmetryZ(0.34), true);
1045  arma::mat dmeY = dme.apply(dm, X, dynObj);
1046  arma::mat rmeY = rme.apply(rs, X);
1047 
1048  // Delete primitives
1049  for(Primitive *primitive : dynObj.getPrimitives()) delete primitive;
1050 
1051  // Validate
1052  size_t const m = dmeY.n_elem;
1053  size_t const n = rmeY.n_elem;
1054  if(m!=n) return false;
1055  for(size_t i = 0 ; i < m ; ++i)
1056  if(std::fabs(dmeY[i]-rmeY[i]) > eps) return false;
1057  return true;
1058 }
1059 
1060 }
Adapter which wraps a rigid motion engine to make it fit the dynamic Helios context.
Definition: DynMotionEngine.h:23
Class which wraps the RigidMotion class to implement extra features such as the self mode control mec...
Definition: DynMotion.h:26
Implementation of a dynamic object which supports dynamic motions (extended rigid motions)
Definition: DynMovingObject.h:39
arma::mat positionMatrixFromPrimitives() const
Obtain the position matrix for primitives defining the dynamic object.
Definition: DynObject.cpp:65
BaseTest class.
Definition: BaseTest.h:20
Rigid motion test.
Definition: RigidMotionTest.h:27
mat R3X
Matrix of points in R3 to be used by tests.
Definition: RigidMotionTest.h:54
RigidMotionR3Factory rm3f
The R3 rigid motion factory to be used by tests.
Definition: RigidMotionTest.h:46
RigidMotionR2Factory rm2f
The R2 rigid motion factory to be used by tests.
Definition: RigidMotionTest.h:42
mat R2X
Matrix of points in R2 to be used by tests.
Definition: RigidMotionTest.h:50
RigidMotionEngine rme
The rigid motion engine to be used by tests.
Definition: RigidMotionTest.h:38
DynMotionEngine dme
The dynamic motion engine to be used by helios tests.
Definition: RigidMotionTest.h:58
RigidMotionTest()
Rigid motion test constructor.
Definition: RigidMotionTest.h:65
Abstract class defining the common behavior for all primitives.
Definition: Primitive.h:24
void computeCentroid(bool const computeBound=false)
Compute the default centroid for the scene part as the midrange point and its boundaries too.
Definition: ScenePart.cpp:199
arma::colvec getCentroid() const
Obtain the centroid of the scene part.
Definition: ScenePart.h:289
std::vector< Primitive * > const & getPrimitives() const
Obtain the primitives of the scene part.
Definition: ScenePart.h:275
Class representing triangle primitive.
Definition: Triangle.h:13
Class representing a vertex.
Definition: Vertex.h:14
Class to handle operations with rigid motions.
Definition: RigidMotionEngine.h:19
Base class for rigid motion exceptions.
Definition: RigidMotionException.h:16
Class providing building methods for rigid motions in .
Definition: RigidMotionR2Factory.h:22
Class providing building methods for rigid motions in .
Definition: RigidMotionR3Factory.h:22
Interface that must be implemented by any class which represents a specific rigid motions.
Definition: RigidMotion.h:49
virtual Type findType() const
Obtain the type of the rigid motion.
Definition: RigidMotion.cpp:68
virtual SuperType findSuperType() const
Obtain the supertype of the rigid motion.
Definition: RigidMotion.cpp:22
virtual bool hasFixedPoints() const
Check if rigid motion has fixed points.
Definition: RigidMotion.cpp:61
virtual RigidMotion compose(RigidMotion const rm) const
Compose this rigid motion with given rigid motion so is obtained.
Definition: RigidMotion.cpp:11
virtual size_t findInvariantDimensionality() const
Obtain the dimensionality of the associated invariant which can be either the set of fixed points or ...
Definition: RigidMotion.cpp:105