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>
26 #include "sg_random.h"
34 // Check if the equivalent function works
35 v1 = SGVec3<T>(1, 2, 3);
36 v2 = SGVec3<T>(3, 2, 1);
37 if (equivalent(v1, v2))
40 // Check the unary minus operator
41 v3 = SGVec3<T>(-1, -2, -3);
42 if (!equivalent(-v1, v3))
45 // Check the unary plus operator
46 v3 = SGVec3<T>(1, 2, 3);
47 if (!equivalent(+v1, v3))
50 // Check the addition operator
51 v3 = SGVec3<T>(4, 4, 4);
52 if (!equivalent(v1 + v2, v3))
55 // Check the subtraction operator
56 v3 = SGVec3<T>(-2, 0, 2);
57 if (!equivalent(v1 - v2, v3))
60 // Check the scaler multiplication operator
61 v3 = SGVec3<T>(2, 4, 6);
62 if (!equivalent(2*v1, v3))
65 // Check the dot product
66 if (fabs(dot(v1, v2) - 10) > 10*SGLimits<T>::epsilon())
69 // Check the cross product
70 v3 = SGVec3<T>(-4, 8, -4);
71 if (!equivalent(cross(v1, v2), v3))
74 // Check the euclidean length
75 if (fabs(14 - length(v1)*length(v1)) > 14*SGLimits<T>::epsilon())
83 isSameRotation(const SGQuat<T>& q1, const SGQuat<T>& q2)
85 const SGVec3<T> e1(1, 0, 0);
86 const SGVec3<T> e2(0, 1, 0);
87 const SGVec3<T> e3(0, 0, 1);
88 if (!equivalent(q1.transform(e1), q2.transform(e1)))
90 if (!equivalent(q1.transform(e2), q2.transform(e2)))
92 if (!equivalent(q1.transform(e3), q2.transform(e3)))
101 const SGVec3<T> e1(1, 0, 0);
102 const SGVec3<T> e2(0, 1, 0);
103 const SGVec3<T> e3(0, 0, 1);
105 SGQuat<T> q1, q2, q3, q4;
106 // Check a rotation around the x axis
107 q1 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), e1);
108 v1 = SGVec3<T>(1, 2, 3);
109 v2 = SGVec3<T>(1, -2, -3);
110 if (!equivalent(q1.transform(v1), v2))
113 // Check a rotation around the x axis
114 q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e1);
115 v2 = SGVec3<T>(1, 3, -2);
116 if (!equivalent(q1.transform(v1), v2))
119 // Check a rotation around the y axis
120 q1 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), e2);
121 v2 = SGVec3<T>(-1, 2, -3);
122 if (!equivalent(q1.transform(v1), v2))
125 // Check a rotation around the y axis
126 q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e2);
127 v2 = SGVec3<T>(-3, 2, 1);
128 if (!equivalent(q1.transform(v1), v2))
131 // Check a rotation around the z axis
132 q1 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), e3);
133 v2 = SGVec3<T>(-1, -2, 3);
134 if (!equivalent(q1.transform(v1), v2))
137 // Check a rotation around the z axis
138 q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e3);
139 v2 = SGVec3<T>(2, -1, 3);
140 if (!equivalent(q1.transform(v1), v2))
143 // Now check some successive transforms
144 // We can reuse the prevously tested stuff
145 q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e1);
146 q2 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e2);
148 v2 = q2.transform(q1.transform(v1));
149 if (!equivalent(q3.transform(v1), v2))
152 /// Test from Euler angles
153 float x = 0.2*SGMisc<T>::pi();
154 float y = 0.3*SGMisc<T>::pi();
155 float z = 0.4*SGMisc<T>::pi();
156 q1 = SGQuat<T>::fromAngleAxis(z, e3);
157 q2 = SGQuat<T>::fromAngleAxis(y, e2);
158 q3 = SGQuat<T>::fromAngleAxis(x, e1);
159 v2 = q3.transform(q2.transform(q1.transform(v1)));
160 q4 = SGQuat<T>::fromEulerRad(z, y, x);
161 if (!equivalent(q4.transform(v1), v2))
164 /// Test angle axis forward and back transform
165 q1 = SGQuat<T>::fromAngleAxis(0.2*SGMisc<T>::pi(), e1);
166 q2 = SGQuat<T>::fromAngleAxis(0.7*SGMisc<T>::pi(), e2);
169 q1.getAngleAxis(angleAxis);
170 q4 = SGQuat<T>::fromAngleAxis(angleAxis);
171 if (!isSameRotation(q1, q4))
173 q2.getAngleAxis(angleAxis);
174 q4 = SGQuat<T>::fromAngleAxis(angleAxis);
175 if (!isSameRotation(q2, q4))
177 q3.getAngleAxis(angleAxis);
178 q4 = SGQuat<T>::fromAngleAxis(angleAxis);
179 if (!isSameRotation(q3, q4))
182 /// Test angle axis forward and back transform
183 q1 = SGQuat<T>::fromAngleAxis(0.2*SGMisc<T>::pi(), e1);
184 q2 = SGQuat<T>::fromAngleAxis(1.7*SGMisc<T>::pi(), e2);
186 SGVec3<T> positiveAngleAxis = q1.getPositiveRealImag();
187 q4 = SGQuat<T>::fromPositiveRealImag(positiveAngleAxis);
188 if (!isSameRotation(q1, q4))
190 positiveAngleAxis = q2.getPositiveRealImag();
191 q4 = SGQuat<T>::fromPositiveRealImag(positiveAngleAxis);
192 if (!isSameRotation(q2, q4))
194 positiveAngleAxis = q3.getPositiveRealImag();
195 q4 = SGQuat<T>::fromPositiveRealImag(positiveAngleAxis);
196 if (!isSameRotation(q3, q4))
204 QuatDerivativeTest(void)
206 for (unsigned i = 0; i < 100; ++i) {
207 // Generate the test case:
208 // Give a lower bound to the distance, so avoid testing cancelation
209 T dt = T(0.01) + sg_random();
210 // Start with orientation o0, angular velocity av and a random stepsize
211 SGQuat<T> o0 = SGQuat<T>::fromEulerDeg(T(360)*sg_random(), T(360)*sg_random(), T(360)*sg_random());
212 SGVec3<T> av(sg_random(), sg_random(), sg_random());
213 // Do one euler step and renormalize
214 SGQuat<T> o1 = normalize(o0 + dt*o0.derivative(av));
216 // Check if we can restore the angular velocity
217 SGVec3<T> av2 = SGQuat<T>::forwardDifferenceVelocity(o0, o1, dt);
218 if (!equivalent(av, av2))
221 // Test with the equivalent orientation
223 av2 = SGQuat<T>::forwardDifferenceVelocity(o0, o1, dt);
224 if (!equivalent(av, av2))
234 // Create some test matrix
235 SGVec3<T> v0(2, 7, 17);
236 SGQuat<T> q0 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), normalize(v0));
237 SGMatrix<T> m0 = SGMatrix<T>::unit();
238 m0.postMultTranslate(v0);
239 m0.postMultRotate(q0);
241 // Check the three forms of the inverse for that kind of special matrix
242 SGMatrix<T> m1 = SGMatrix<T>::unit();
243 m1.preMultTranslate(-v0);
244 m1.preMultRotate(inverse(q0));
249 if (!equivalent(m1, m2))
251 if (!equivalent(m2, m3))
254 // Check matrix multiplication and inversion
255 if (!equivalent(m0*m1, SGMatrix<T>::unit()))
257 if (!equivalent(m1*m0, SGMatrix<T>::unit()))
259 if (!equivalent(m0*m2, SGMatrix<T>::unit()))
261 if (!equivalent(m2*m0, SGMatrix<T>::unit()))
263 if (!equivalent(m0*m3, SGMatrix<T>::unit()))
265 if (!equivalent(m3*m0, SGMatrix<T>::unit()))
274 // We know that the values are on the order of 1
275 double epsDeg = 10*360*SGLimits<double>::epsilon();
276 // For the altitude values we need to tolerate relative errors in the order
278 double epsM = 10*6e6*SGLimits<double>::epsilon();
280 SGVec3<double> cart0, cart1;
284 // create some geodetic position
285 geod0 = SGGeod::fromDegM(30, 20, 17);
287 // Test the conversion routines to cartesian coordinates
288 cart0 = SGVec3<double>::fromGeod(geod0);
289 geod1 = SGGeod::fromCart(cart0);
290 if (epsDeg < fabs(geod0.getLongitudeDeg() - geod1.getLongitudeDeg()) ||
291 epsDeg < fabs(geod0.getLatitudeDeg() - geod1.getLatitudeDeg()) ||
292 epsM < fabs(geod0.getElevationM() - geod1.getElevationM()))
295 // Test the conversion routines to radial coordinates
296 geoc0 = SGGeoc::fromCart(cart0);
297 cart1 = SGVec3<double>::fromGeoc(geoc0);
298 if (!equivalent(cart0, cart1))
301 // test course / advance routines
302 // uses examples from Williams aviation formulary
303 SGGeoc lax = SGGeoc::fromRadM(-2.066470, 0.592539, 10.0);
304 SGGeoc jfk = SGGeoc::fromRadM(-1.287762, 0.709186, 10.0);
306 double distNm = SGGeodesy::distanceRad(lax, jfk) * SG_RAD_TO_NM;
307 std::cout << "distance is " << distNm << std::endl;
308 if (0.5 < fabs(distNm - 2144)) // 2144 nm
311 double crsDeg = SGGeodesy::courseRad(lax, jfk) * SG_RADIANS_TO_DEGREES;
312 std::cout << "course is " << crsDeg << std::endl;
313 if (0.5 < fabs(crsDeg - 66)) // 66 degrees
317 SGGeodesy::advanceRadM(lax, crsDeg * SG_DEGREES_TO_RADIANS, 100 * SG_NM_TO_METER, adv);
318 std::cout << "lon:" << adv.getLongitudeRad() << ", lat:" << adv.getLatitudeRad() << std::endl;
320 if (0.01 < fabs(adv.getLongitudeRad() - (-2.034206)) ||
321 0.01 < fabs(adv.getLatitudeRad() - 0.604180))
333 if (!Vec3Test<float>())
335 if (!Vec3Test<double>())
338 // Do quaternion tests
339 if (!QuatTest<float>())
341 if (!QuatTest<double>())
343 if (!QuatDerivativeTest<float>())
345 if (!QuatDerivativeTest<double>())
349 if (!MatrixTest<float>())
351 if (!MatrixTest<double>())
354 // Check geodetic/geocentric/cartesian conversions
358 std::cout << "Successfully passed all tests!" << std::endl;