1 // Copyright (C) 2006 Mathias Froehlich - Mathias.Froehlich@web.de
3 // This library is free software; you can redistribute it and/or
4 // modify it under the terms of the GNU Library General Public
5 // License as published by the Free Software Foundation; either
6 // version 2 of the License, or (at your option) any later version.
8 // This library is distributed in the hope that it will be useful,
9 // but WITHOUT ANY WARRANTY; without even the implied warranty of
10 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
11 // Library General Public License for more details.
13 // You should have received a copy of the GNU General Public License
14 // along with this program; if not, write to the Free Software
15 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
19 # include <simgear_config.h>
22 #include <simgear/misc/test_macros.hxx>
29 #include "sg_random.h"
37 // Check if the equivalent function works
38 v1 = SGVec3<T>(1, 2, 3);
39 v2 = SGVec3<T>(3, 2, 1);
40 if (equivalent(v1, v2))
43 // Check the unary minus operator
44 v3 = SGVec3<T>(-1, -2, -3);
45 if (!equivalent(-v1, v3))
48 // Check the unary plus operator
49 v3 = SGVec3<T>(1, 2, 3);
50 if (!equivalent(+v1, v3))
53 // Check the addition operator
54 v3 = SGVec3<T>(4, 4, 4);
55 if (!equivalent(v1 + v2, v3))
58 // Check the subtraction operator
59 v3 = SGVec3<T>(-2, 0, 2);
60 if (!equivalent(v1 - v2, v3))
63 // Check the scaler multiplication operator
64 v3 = SGVec3<T>(2, 4, 6);
65 if (!equivalent(2*v1, v3))
68 // Check the dot product
69 if (fabs(dot(v1, v2) - 10) > 10*SGLimits<T>::epsilon())
72 // Check the cross product
73 v3 = SGVec3<T>(-4, 8, -4);
74 if (!equivalent(cross(v1, v2), v3))
77 // Check the euclidean length
78 if (fabs(14 - length(v1)*length(v1)) > 14*SGLimits<T>::epsilon())
86 isSameRotation(const SGQuat<T>& q1, const SGQuat<T>& q2)
88 const SGVec3<T> e1(1, 0, 0);
89 const SGVec3<T> e2(0, 1, 0);
90 const SGVec3<T> e3(0, 0, 1);
91 if (!equivalent(q1.transform(e1), q2.transform(e1)))
93 if (!equivalent(q1.transform(e2), q2.transform(e2)))
95 if (!equivalent(q1.transform(e3), q2.transform(e3)))
104 const SGVec3<T> e1(1, 0, 0);
105 const SGVec3<T> e2(0, 1, 0);
106 const SGVec3<T> e3(0, 0, 1);
108 SGQuat<T> q1, q2, q3, q4;
109 // Check a rotation around the x axis
110 q1 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), e1);
111 v1 = SGVec3<T>(1, 2, 3);
112 v2 = SGVec3<T>(1, -2, -3);
113 if (!equivalent(q1.transform(v1), v2))
116 // Check a rotation around the x axis
117 q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e1);
118 v2 = SGVec3<T>(1, 3, -2);
119 if (!equivalent(q1.transform(v1), v2))
122 // Check a rotation around the y axis
123 q1 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), e2);
124 v2 = SGVec3<T>(-1, 2, -3);
125 if (!equivalent(q1.transform(v1), v2))
128 // Check a rotation around the y axis
129 q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e2);
130 v2 = SGVec3<T>(-3, 2, 1);
131 if (!equivalent(q1.transform(v1), v2))
134 // Check a rotation around the z axis
135 q1 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), e3);
136 v2 = SGVec3<T>(-1, -2, 3);
137 if (!equivalent(q1.transform(v1), v2))
140 // Check a rotation around the z axis
141 q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e3);
142 v2 = SGVec3<T>(2, -1, 3);
143 if (!equivalent(q1.transform(v1), v2))
146 // Now check some successive transforms
147 // We can reuse the prevously tested stuff
148 q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e1);
149 q2 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e2);
151 v2 = q2.transform(q1.transform(v1));
152 if (!equivalent(q3.transform(v1), v2))
155 /// Test from Euler angles
156 float x = 0.2*SGMisc<T>::pi();
157 float y = 0.3*SGMisc<T>::pi();
158 float z = 0.4*SGMisc<T>::pi();
159 q1 = SGQuat<T>::fromAngleAxis(z, e3);
160 q2 = SGQuat<T>::fromAngleAxis(y, e2);
161 q3 = SGQuat<T>::fromAngleAxis(x, e1);
162 v2 = q3.transform(q2.transform(q1.transform(v1)));
163 q4 = SGQuat<T>::fromEulerRad(z, y, x);
164 if (!equivalent(q4.transform(v1), v2))
167 /// Test angle axis forward and back transform
168 q1 = SGQuat<T>::fromAngleAxis(0.2*SGMisc<T>::pi(), e1);
169 q2 = SGQuat<T>::fromAngleAxis(0.7*SGMisc<T>::pi(), e2);
172 q1.getAngleAxis(angleAxis);
173 q4 = SGQuat<T>::fromAngleAxis(angleAxis);
174 if (!isSameRotation(q1, q4))
176 q2.getAngleAxis(angleAxis);
177 q4 = SGQuat<T>::fromAngleAxis(angleAxis);
178 if (!isSameRotation(q2, q4))
180 q3.getAngleAxis(angleAxis);
181 q4 = SGQuat<T>::fromAngleAxis(angleAxis);
182 if (!isSameRotation(q3, q4))
185 /// Test angle axis forward and back transform
186 q1 = SGQuat<T>::fromAngleAxis(0.2*SGMisc<T>::pi(), e1);
187 q2 = SGQuat<T>::fromAngleAxis(1.7*SGMisc<T>::pi(), e2);
189 SGVec3<T> positiveAngleAxis = q1.getPositiveRealImag();
190 q4 = SGQuat<T>::fromPositiveRealImag(positiveAngleAxis);
191 if (!isSameRotation(q1, q4))
193 positiveAngleAxis = q2.getPositiveRealImag();
194 q4 = SGQuat<T>::fromPositiveRealImag(positiveAngleAxis);
195 if (!isSameRotation(q2, q4))
197 positiveAngleAxis = q3.getPositiveRealImag();
198 q4 = SGQuat<T>::fromPositiveRealImag(positiveAngleAxis);
199 if (!isSameRotation(q3, q4))
207 QuatDerivativeTest(void)
209 for (unsigned i = 0; i < 100; ++i) {
210 // Generate the test case:
211 // Give a lower bound to the distance, so avoid testing cancelation
212 T dt = T(0.01) + sg_random();
213 // Start with orientation o0, angular velocity av and a random stepsize
214 SGQuat<T> o0 = SGQuat<T>::fromEulerDeg(T(360)*sg_random(), T(360)*sg_random(), T(360)*sg_random());
215 SGVec3<T> av(sg_random(), sg_random(), sg_random());
216 // Do one euler step and renormalize
217 SGQuat<T> o1 = normalize(o0 + dt*o0.derivative(av));
219 // Check if we can restore the angular velocity
220 SGVec3<T> av2 = SGQuat<T>::forwardDifferenceVelocity(o0, o1, dt);
221 if (!equivalent(av, av2))
224 // Test with the equivalent orientation
226 av2 = SGQuat<T>::forwardDifferenceVelocity(o0, o1, dt);
227 if (!equivalent(av, av2))
237 // Create some test matrix
238 SGVec3<T> v0(2, 7, 17);
239 SGQuat<T> q0 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), normalize(v0));
240 SGMatrix<T> m0 = SGMatrix<T>::unit();
241 m0.postMultTranslate(v0);
242 m0.postMultRotate(q0);
244 // Check the three forms of the inverse for that kind of special matrix
245 SGMatrix<T> m1 = SGMatrix<T>::unit();
246 m1.preMultTranslate(-v0);
247 m1.preMultRotate(inverse(q0));
252 if (!equivalent(m1, m2))
254 if (!equivalent(m2, m3))
257 // Check matrix multiplication and inversion
258 if (!equivalent(m0*m1, SGMatrix<T>::unit()))
260 if (!equivalent(m1*m0, SGMatrix<T>::unit()))
262 if (!equivalent(m0*m2, SGMatrix<T>::unit()))
264 if (!equivalent(m2*m0, SGMatrix<T>::unit()))
266 if (!equivalent(m0*m3, SGMatrix<T>::unit()))
268 if (!equivalent(m3*m0, SGMatrix<T>::unit()))
277 SGRect<T> rect(10, 15, 20, 25);
279 COMPARE(rect.x(), 10)
280 COMPARE(rect.y(), 15)
281 COMPARE(rect.width(), 20)
282 COMPARE(rect.height(), 25)
284 COMPARE(rect.pos(), SGVec2<T>(10, 15))
285 COMPARE(rect.size(), SGVec2<T>(20, 25))
287 COMPARE(rect.l(), 10)
288 COMPARE(rect.t(), 15)
289 COMPARE(rect.r(), 30)
290 COMPARE(rect.b(), 40)
293 VERIFY(rect == SGRect<T>(10, 15, 20, 25))
294 VERIFY(rect != SGRect<T>(11, 15, 20, 25))
296 VERIFY(rect.contains(10, 15))
297 VERIFY(!rect.contains(9, 15))
298 VERIFY(rect.contains(9, 15, 1))
304 // We know that the values are on the order of 1
305 double epsDeg = 10*360*SGLimits<double>::epsilon();
306 // For the altitude values we need to tolerate relative errors in the order
308 double epsM = 10*6e6*SGLimits<double>::epsilon();
310 SGVec3<double> cart0, cart1;
314 // create some geodetic position
315 geod0 = SGGeod::fromDegM(30, 20, 17);
317 // Test the conversion routines to cartesian coordinates
318 cart0 = SGVec3<double>::fromGeod(geod0);
319 geod1 = SGGeod::fromCart(cart0);
320 if (epsDeg < fabs(geod0.getLongitudeDeg() - geod1.getLongitudeDeg()) ||
321 epsDeg < fabs(geod0.getLatitudeDeg() - geod1.getLatitudeDeg()) ||
322 epsM < fabs(geod0.getElevationM() - geod1.getElevationM()))
325 // Test the conversion routines to radial coordinates
326 geoc0 = SGGeoc::fromCart(cart0);
327 cart1 = SGVec3<double>::fromGeoc(geoc0);
328 if (!equivalent(cart0, cart1))
331 // test course / advance routines
332 // uses examples from Williams aviation formulary
333 SGGeoc lax = SGGeoc::fromRadM(-2.066470, 0.592539, 10.0);
334 SGGeoc jfk = SGGeoc::fromRadM(-1.287762, 0.709186, 10.0);
336 double distNm = SGGeodesy::distanceRad(lax, jfk) * SG_RAD_TO_NM;
337 std::cout << "distance is " << distNm << std::endl;
338 if (0.5 < fabs(distNm - 2144)) // 2144 nm
341 double crsDeg = SGGeodesy::courseRad(lax, jfk) * SG_RADIANS_TO_DEGREES;
342 std::cout << "course is " << crsDeg << std::endl;
343 if (0.5 < fabs(crsDeg - 66)) // 66 degrees
347 SGGeodesy::advanceRadM(lax, crsDeg * SG_DEGREES_TO_RADIANS, 100 * SG_NM_TO_METER, adv);
348 std::cout << "lon:" << adv.getLongitudeRad() << ", lat:" << adv.getLatitudeRad() << std::endl;
350 if (0.01 < fabs(adv.getLongitudeRad() - (-2.034206)) ||
351 0.01 < fabs(adv.getLatitudeRad() - 0.604180))
363 if (!Vec3Test<float>())
365 if (!Vec3Test<double>())
368 // Do quaternion tests
369 if (!QuatTest<float>())
371 if (!QuatTest<double>())
373 if (!QuatDerivativeTest<float>())
375 if (!QuatDerivativeTest<double>())
379 if (!MatrixTest<float>())
381 if (!MatrixTest<double>())
386 doRectTest<double>();
388 // Check geodetic/geocentric/cartesian conversions
392 std::cout << "Successfully passed all tests!" << std::endl;