4 #include <HelicopterPlatform.h> 5 #include <MathConstants.h> 93 double newRoll, pitch, yaw;
94 for(
size_t i=1 ; ; i++){
96 if(angle >= PI_2)
break;
97 hp.
rotate(angle, 0.0, 0.0);
99 if(newRoll < 0.0) newRoll += PI_2;
100 double diff = newRoll - angle;
101 if(diff < -eps || diff >
eps)
return false;
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);
116 double diff = newPitch - angle;
117 if(diff < -eps || diff >
eps)
return false;
126 double roll, pitch, newYaw;
127 for(
size_t i=1 ; ; i++){
129 if(angle >= PI_2)
break;
130 hp.
rotate(0.0, 0.0, angle);
132 if(newYaw < 0.0) newYaw += PI_2;
133 double diff = newYaw - angle;
134 if(diff < -eps || diff >
eps)
return false;
141 double angle1, angle2;
142 double step1 = 0.09, step2 = 0.03;
143 double newRoll, newPitch, yaw;
144 for(
size_t i = 1 ; ; i++){
146 angle2 = i*step2 - PI_HALF;
147 if(angle1 >= PI_2)
break;
148 hp.
rotate(angle1, angle2, 0.0);
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;
161 double angle1, angle2;
162 double step1 = 0.09, step2 = 0.09;
163 double newRoll, pitch, newYaw;
164 for(
size_t i = 1 ; ; i++){
167 if(angle1 >= PI_2)
break;
168 hp.
rotate(angle1, 0.0, angle2);
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;
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;
188 if(angle2 >= PI_2)
break;
189 hp.
rotate(0.0, angle1, angle2);
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;
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++){
207 angle2 = i*step2 - PI_HALF;
209 if(angle1 >= PI_2)
break;
210 hp.
rotate(angle1, angle2, angle3);
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;
BaseTest class.
Definition: BaseTest.h:19