Helios++
Helios software for LiDAR simulations
PlatformPhysicsTest.h
1 #pragma once
2 
3 #include <BaseTest.h>
4 #include <HelicopterPlatform.h>
5 #include <MathConstants.h>
6 
7 namespace HeliosTests{
8 
15 public:
19  double eps = 0.0001; // Decimal precision for validation purposes
20 
21  // *** CONSTRUCTOR *** //
22  // ********************* //
26  PlatformPhysicsTest() : BaseTest("Platform physics test"){}
27 
28  // *** R U N *** //
29  // *************** //
33  bool run() override;
34 
35  // *** SUB-TESTS *** //
36  // ******************* //
41  bool testRollOnlyRotations();
51  bool testYawOnlyRotations();
61  bool testRollYawRotations();
66  bool testPitchYawRotations();
72 };
73 
74 // *** R U N *** //
75 // *************** //
77  if(!testRollOnlyRotations()) return false;
78  if(!testPitchOnlyRotations()) return false;
79  if(!testYawOnlyRotations()) return false;
80  if(!testRollPitchRotations()) return false;
81  if(!testRollYawRotations()) return false;
82  if(!testPitchYawRotations()) return false;
83  if(!testRollPitchYawRotations()) return false;
84  return true;
85 }
86 
87 // *** SUB-TESTS *** //
88 // ******************* //
91  double angle;
92  double step = 0.09;
93  double newRoll, pitch, yaw;
94  for(size_t i=1 ; ; i++){
95  angle = i*step;
96  if(angle >= PI_2) break;
97  hp.rotate(angle, 0.0, 0.0);
98  hp.getRollPitchYaw(newRoll, pitch, yaw);
99  if(newRoll < 0.0) newRoll += PI_2;
100  double diff = newRoll - angle;
101  if(diff < -eps || diff > eps) return false;
102  }
103  return true;
104 }
105 
108  double angle;
109  double step = 0.03;
110  double roll, newPitch, yaw;
111  for(size_t i=1 ; ; i++){
112  angle = i*step - PI_HALF;
113  if(angle >= PI_HALF) break;
114  hp.rotate(0.0, angle, 0.0);
115  hp.getRollPitchYaw(roll, newPitch, yaw);
116  double diff = newPitch - angle;
117  if(diff < -eps || diff > eps) return false;
118  }
119  return true;
120 }
121 
124  double angle;
125  double step = 0.09;
126  double roll, pitch, newYaw;
127  for(size_t i=1 ; ; i++){
128  angle = i*step;
129  if(angle >= PI_2) break;
130  hp.rotate(0.0, 0.0, angle);
131  hp.getRollPitchYaw(roll, pitch, newYaw);
132  if(newYaw < 0.0) newYaw += PI_2;
133  double diff = newYaw - angle;
134  if(diff < -eps || diff > eps) return false;
135  }
136  return true;
137 }
138 
141  double angle1, angle2;
142  double step1 = 0.09, step2 = 0.03;
143  double newRoll, newPitch, yaw;
144  for(size_t i = 1 ; ; i++){
145  angle1 = i*step1;
146  angle2 = i*step2 - PI_HALF;
147  if(angle1 >= PI_2) break;
148  hp.rotate(angle1, angle2, 0.0);
149  hp.getRollPitchYaw(newRoll, newPitch, yaw);
150  if(newRoll < 0.0) newRoll += PI_2;
151  double diff1 = newRoll - angle1;
152  double diff2 = newPitch - angle2;
153  if(diff1 < -eps || diff1 > eps) return false;
154  if(diff2 < -eps || diff2 > eps) return false;
155  }
156  return true;
157 }
158 
161  double angle1, angle2;
162  double step1 = 0.09, step2 = 0.09;
163  double newRoll, pitch, newYaw;
164  for(size_t i = 1 ; ; i++){
165  angle1 = i*step1;
166  angle2 = i*step2;
167  if(angle1 >= PI_2) break;
168  hp.rotate(angle1, 0.0, angle2);
169  hp.getRollPitchYaw(newRoll, pitch, newYaw);
170  if(newRoll < 0.0) newRoll += PI_2;
171  if(newYaw < 0.0) newYaw += PI_2;
172  double diff1 = newRoll - angle1;
173  double diff2 = newYaw - angle2;
174  if(diff1 < -eps || diff1 > eps) return false;
175  if(diff2 < -eps || diff2 > eps) return false;
176  }
177  return true;
178 }
179 
182  double angle1, angle2;
183  double step1 = 0.03, step2 = 0.09;
184  double roll, newPitch, newYaw;
185  for(size_t i = 1 ; ; i++){
186  angle1 = i*step1 - PI_HALF;
187  angle2 = i*step2;
188  if(angle2 >= PI_2) break;
189  hp.rotate(0.0, angle1, angle2);
190  hp.getRollPitchYaw(roll, newPitch, newYaw);
191  if(newYaw < 0.0) newYaw += PI_2;
192  double diff1 = newPitch - angle1;
193  double diff2 = newYaw - angle2;
194  if(diff1 < -eps || diff1 > eps) return false;
195  if(diff2 < -eps || diff2 > eps) return false;
196  }
197  return true;
198 }
199 
202  double angle1, angle2, angle3;
203  double step1 = 0.09, step2 = 0.03, step3 = -0.06;
204  double newRoll, newPitch, newYaw;
205  for(size_t i = 1 ; ; i++){
206  angle1 = i*step1;
207  angle2 = i*step2 - PI_HALF;
208  angle3 = i*step3;
209  if(angle1 >= PI_2) break;
210  hp.rotate(angle1, angle2, angle3);
211  hp.getRollPitchYaw(newRoll, newPitch, newYaw);
212  if(newRoll < 0.0) newRoll += PI_2;
213  if(newYaw < 0.0) newYaw += PI_2;
214  double diff1 = newRoll - angle1;
215  double diff2 = newPitch - angle2;
216  double diff3 = newYaw - (angle3+PI_2);
217  if(diff1 < -eps || diff1 > eps) return false;
218  if(diff2 < -eps || diff2 > eps) return false;
219  if(diff3 < -eps || diff3 > eps) return false;
220  }
221  return true;
222 }
223 
224 
225 }
bool testYawOnlyRotations()
Yaw only rotations test.
Definition: PlatformPhysicsTest.h:122
PlatformPhysicsTest()
Platform physics test constructor.
Definition: PlatformPhysicsTest.h:26
bool testRollOnlyRotations()
Roll only rotations test.
Definition: PlatformPhysicsTest.h:89
bool testPitchYawRotations()
Pitch yaw rotations test.
Definition: PlatformPhysicsTest.h:180
void rotate(double roll, double pitch, double yaw)
Rotate helicopter.
Definition: HelicopterPlatform.cpp:348
BaseTest class.
Definition: BaseTest.h:19
double eps
Decimal precision for validation purposes.
Definition: PlatformPhysicsTest.h:19
bool testRollPitchYawRotations()
Roll, pitch and yaw rotations test.
Definition: PlatformPhysicsTest.h:200
Definition: BaseTest.h:6
bool testRollYawRotations()
Roll yaw rotations test.
Definition: PlatformPhysicsTest.h:159
virtual void getRollPitchYaw(double &roll, double &pitch, double &yaw)
Obtain platform roll, pitch and yaw angles. Notice not all platforms track those angles.
Definition: Platform.h:381
Class representing a helicopter platform.
Definition: HelicopterPlatform.h:12
bool testRollPitchRotations()
Roll and pitch rotations test.
Definition: PlatformPhysicsTest.h:139
bool run() override
Definition: PlatformPhysicsTest.h:76
bool testPitchOnlyRotations()
Pitch only rotations test.
Definition: PlatformPhysicsTest.h:106
Platform physics test.
Definition: PlatformPhysicsTest.h:14