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  virtual ~PlatformPhysicsTest() = default;
28 
29  // *** R U N *** //
30  // *************** //
34  bool run() override;
35 
36  // *** SUB-TESTS *** //
37  // ******************* //
42  bool testRollOnlyRotations();
52  bool testYawOnlyRotations();
62  bool testRollYawRotations();
67  bool testPitchYawRotations();
73 };
74 
75 // *** R U N *** //
76 // *************** //
78  if(!testRollOnlyRotations()) return false;
79  if(!testPitchOnlyRotations()) return false;
80  if(!testYawOnlyRotations()) return false;
81  if(!testRollPitchRotations()) return false;
82  if(!testRollYawRotations()) return false;
83  if(!testPitchYawRotations()) return false;
84  if(!testRollPitchYawRotations()) return false;
85  return true;
86 }
87 
88 // *** SUB-TESTS *** //
89 // ******************* //
92  double angle;
93  double step = 0.09;
94  double newRoll, pitch, yaw;
95  for(size_t i=1 ; ; i++){
96  angle = i*step;
97  if(angle >= PI_2) break;
98  hp.rotate(angle, 0.0, 0.0);
99  hp.getRollPitchYaw(newRoll, pitch, yaw);
100  if(newRoll < 0.0) newRoll += PI_2;
101  double diff = newRoll - angle;
102  if(diff < -eps || diff > eps) return false;
103  }
104  return true;
105 }
106 
109  double angle;
110  double step = 0.03;
111  double roll, newPitch, yaw;
112  for(size_t i=1 ; ; i++){
113  angle = i*step - PI_HALF;
114  if(angle >= PI_HALF) break;
115  hp.rotate(0.0, angle, 0.0);
116  hp.getRollPitchYaw(roll, newPitch, yaw);
117  double diff = newPitch - angle;
118  if(diff < -eps || diff > eps) return false;
119  }
120  return true;
121 }
122 
125  double angle;
126  double step = 0.09;
127  double roll, pitch, newYaw;
128  for(size_t i=1 ; ; i++){
129  angle = i*step;
130  if(angle >= PI_2) break;
131  hp.rotate(0.0, 0.0, angle);
132  hp.getRollPitchYaw(roll, pitch, newYaw);
133  if(newYaw < 0.0) newYaw += PI_2;
134  double diff = newYaw - angle;
135  if(diff < -eps || diff > eps) return false;
136  }
137  return true;
138 }
139 
142  double angle1, angle2;
143  double step1 = 0.09, step2 = 0.03;
144  double newRoll, newPitch, yaw;
145  for(size_t i = 1 ; ; i++){
146  angle1 = i*step1;
147  angle2 = i*step2 - PI_HALF;
148  if(angle1 >= PI_2) break;
149  hp.rotate(angle1, angle2, 0.0);
150  hp.getRollPitchYaw(newRoll, newPitch, yaw);
151  if(newRoll < 0.0) newRoll += PI_2;
152  double diff1 = newRoll - angle1;
153  double diff2 = newPitch - angle2;
154  if(diff1 < -eps || diff1 > eps) return false;
155  if(diff2 < -eps || diff2 > eps) return false;
156  }
157  return true;
158 }
159 
162  double angle1, angle2;
163  double step1 = 0.09, step2 = 0.09;
164  double newRoll, pitch, newYaw;
165  for(size_t i = 1 ; ; i++){
166  angle1 = i*step1;
167  angle2 = i*step2;
168  if(angle1 >= PI_2) break;
169  hp.rotate(angle1, 0.0, angle2);
170  hp.getRollPitchYaw(newRoll, pitch, newYaw);
171  if(newRoll < 0.0) newRoll += PI_2;
172  if(newYaw < 0.0) newYaw += PI_2;
173  double diff1 = newRoll - angle1;
174  double diff2 = newYaw - angle2;
175  if(diff1 < -eps || diff1 > eps) return false;
176  if(diff2 < -eps || diff2 > eps) return false;
177  }
178  return true;
179 }
180 
183  double angle1, angle2;
184  double step1 = 0.03, step2 = 0.09;
185  double roll, newPitch, newYaw;
186  for(size_t i = 1 ; ; i++){
187  angle1 = i*step1 - PI_HALF;
188  angle2 = i*step2;
189  if(angle2 >= PI_2) break;
190  hp.rotate(0.0, angle1, angle2);
191  hp.getRollPitchYaw(roll, newPitch, newYaw);
192  if(newYaw < 0.0) newYaw += PI_2;
193  double diff1 = newPitch - angle1;
194  double diff2 = newYaw - angle2;
195  if(diff1 < -eps || diff1 > eps) return false;
196  if(diff2 < -eps || diff2 > eps) return false;
197  }
198  return true;
199 }
200 
203  double angle1, angle2, angle3;
204  double step1 = 0.09, step2 = 0.03, step3 = -0.06;
205  double newRoll, newPitch, newYaw;
206  for(size_t i = 1 ; ; i++){
207  angle1 = i*step1;
208  angle2 = i*step2 - PI_HALF;
209  angle3 = i*step3;
210  if(angle1 >= PI_2) break;
211  hp.rotate(angle1, angle2, angle3);
212  hp.getRollPitchYaw(newRoll, newPitch, newYaw);
213  if(newRoll < 0.0) newRoll += PI_2;
214  if(newYaw < 0.0) newYaw += PI_2;
215  double diff1 = newRoll - angle1;
216  double diff2 = newPitch - angle2;
217  double diff3 = newYaw - (angle3+PI_2);
218  if(diff1 < -eps || diff1 > eps) return false;
219  if(diff2 < -eps || diff2 > eps) return false;
220  if(diff3 < -eps || diff3 > eps) return false;
221  }
222  return true;
223 }
224 
225 
226 }
Class representing a helicopter platform.
Definition: HelicopterPlatform.h:12
void rotate(double roll, double pitch, double yaw)
Rotate helicopter.
Definition: HelicopterPlatform.cpp:383
BaseTest class.
Definition: BaseTest.h:20
Platform physics test.
Definition: PlatformPhysicsTest.h:14
bool testRollOnlyRotations()
Roll only rotations test.
Definition: PlatformPhysicsTest.h:90
bool testPitchYawRotations()
Pitch yaw rotations test.
Definition: PlatformPhysicsTest.h:181
double eps
Decimal precision for validation purposes.
Definition: PlatformPhysicsTest.h:19
bool testPitchOnlyRotations()
Pitch only rotations test.
Definition: PlatformPhysicsTest.h:107
bool testRollPitchYawRotations()
Roll, pitch and yaw rotations test.
Definition: PlatformPhysicsTest.h:201
bool testRollPitchRotations()
Roll and pitch rotations test.
Definition: PlatformPhysicsTest.h:140
PlatformPhysicsTest()
Platform physics test constructor.
Definition: PlatformPhysicsTest.h:26
bool run() override
Definition: PlatformPhysicsTest.h:77
bool testYawOnlyRotations()
Yaw only rotations test.
Definition: PlatformPhysicsTest.h:123
bool testRollYawRotations()
Roll yaw rotations test.
Definition: PlatformPhysicsTest.h:160
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:395