4 #include <HelicopterPlatform.h>
5 #include <MathConstants.h>
94 double newRoll, pitch, yaw;
95 for(
size_t i=1 ; ; i++){
97 if(angle >= PI_2)
break;
98 hp.
rotate(angle, 0.0, 0.0);
100 if(newRoll < 0.0) newRoll += PI_2;
101 double diff = newRoll - angle;
102 if(diff < -eps || diff >
eps)
return false;
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);
117 double diff = newPitch - angle;
118 if(diff < -eps || diff >
eps)
return false;
127 double roll, pitch, newYaw;
128 for(
size_t i=1 ; ; i++){
130 if(angle >= PI_2)
break;
131 hp.
rotate(0.0, 0.0, angle);
133 if(newYaw < 0.0) newYaw += PI_2;
134 double diff = newYaw - angle;
135 if(diff < -eps || diff >
eps)
return false;
142 double angle1, angle2;
143 double step1 = 0.09, step2 = 0.03;
144 double newRoll, newPitch, yaw;
145 for(
size_t i = 1 ; ; i++){
147 angle2 = i*step2 - PI_HALF;
148 if(angle1 >= PI_2)
break;
149 hp.
rotate(angle1, angle2, 0.0);
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;
162 double angle1, angle2;
163 double step1 = 0.09, step2 = 0.09;
164 double newRoll, pitch, newYaw;
165 for(
size_t i = 1 ; ; i++){
168 if(angle1 >= PI_2)
break;
169 hp.
rotate(angle1, 0.0, angle2);
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;
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;
189 if(angle2 >= PI_2)
break;
190 hp.
rotate(0.0, angle1, angle2);
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;
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++){
208 angle2 = i*step2 - PI_HALF;
210 if(angle1 >= PI_2)
break;
211 hp.
rotate(angle1, angle2, angle3);
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;
BaseTest class.
Definition: BaseTest.h:20