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 Library General Public
14 // License along with this library; if not, write to the
15 // Free Software Foundation, Inc., 59 Temple Place - Suite 330,
16 // Boston, MA 02111-1307, USA.
20 # include <simgear_config.h>
36 // Check if the equivalent function works
37 v1 = SGVec3<T>(1, 2, 3);
38 v2 = SGVec3<T>(3, 2, 1);
39 if (equivalent(v1, v2))
42 // Check the unary minus operator
43 v3 = SGVec3<T>(-1, -2, -3);
44 if (!equivalent(-v1, v3))
47 // Check the unary plus operator
48 v3 = SGVec3<T>(1, 2, 3);
49 if (!equivalent(+v1, v3))
52 // Check the addition operator
53 v3 = SGVec3<T>(4, 4, 4);
54 if (!equivalent(v1 + v2, v3))
57 // Check the subtraction operator
58 v3 = SGVec3<T>(-2, 0, 2);
59 if (!equivalent(v1 - v2, v3))
62 // Check the scaler multiplication operator
63 v3 = SGVec3<T>(2, 4, 6);
64 if (!equivalent(2*v1, v3))
67 // Check the dot product
68 if (fabs(dot(v1, v2) - 10) > 10*SGLimits<T>::epsilon())
71 // Check the cross product
72 v3 = SGVec3<T>(-4, 8, -4);
73 if (!equivalent(cross(v1, v2), v3))
76 // Check the euclidean length
77 if (fabs(14 - length(v1)*length(v1)) > 14*SGLimits<T>::epsilon())
87 const SGVec3<T> e1(1, 0, 0);
88 const SGVec3<T> e2(0, 1, 0);
89 const SGVec3<T> e3(0, 0, 1);
91 SGQuat<T> q1, q2, q3, q4;
92 // Check a rotation around the x axis
93 q1 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), e1);
94 v1 = SGVec3<T>(1, 2, 3);
95 v2 = SGVec3<T>(1, -2, -3);
96 if (!equivalent(q1.transform(v1), v2))
99 // Check a rotation around the x axis
100 q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e1);
101 v2 = SGVec3<T>(1, 3, -2);
102 if (!equivalent(q1.transform(v1), v2))
105 // Check a rotation around the y axis
106 q1 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), e2);
107 v2 = SGVec3<T>(-1, 2, -3);
108 if (!equivalent(q1.transform(v1), v2))
111 // Check a rotation around the y axis
112 q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e2);
113 v2 = SGVec3<T>(-3, 2, 1);
114 if (!equivalent(q1.transform(v1), v2))
117 // Check a rotation around the z axis
118 q1 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), e3);
119 v2 = SGVec3<T>(-1, -2, 3);
120 if (!equivalent(q1.transform(v1), v2))
123 // Check a rotation around the z axis
124 q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e3);
125 v2 = SGVec3<T>(2, -1, 3);
126 if (!equivalent(q1.transform(v1), v2))
129 // Now check some successive transforms
130 // We can reuse the prevously tested stuff
131 q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e1);
132 q2 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e2);
134 v2 = q2.transform(q1.transform(v1));
135 if (!equivalent(q3.transform(v1), v2))
138 /// Test from Euler angles
139 float x = 0.2*SGMisc<T>::pi();
140 float y = 0.3*SGMisc<T>::pi();
141 float z = 0.4*SGMisc<T>::pi();
142 q1 = SGQuat<T>::fromAngleAxis(z, e3);
143 q2 = SGQuat<T>::fromAngleAxis(y, e2);
144 q3 = SGQuat<T>::fromAngleAxis(x, e1);
145 v2 = q3.transform(q2.transform(q1.transform(v1)));
146 q4 = SGQuat<T>::fromEulerRad(z, y, x);
147 if (!equivalent(q4.transform(v1), v2))
150 /// Test angle axis forward and back transform
151 q1 = SGQuat<T>::fromAngleAxis(0.2*SGMisc<T>::pi(), e1);
152 q2 = SGQuat<T>::fromAngleAxis(0.7*SGMisc<T>::pi(), e2);
155 q1.getAngleAxis(angleAxis);
156 q4 = SGQuat<T>::fromAngleAxis(angleAxis);
157 if (!equivalent(q1, q4))
159 q2.getAngleAxis(angleAxis);
160 q4 = SGQuat<T>::fromAngleAxis(angleAxis);
161 if (!equivalent(q2, q4))
163 q3.getAngleAxis(angleAxis);
164 q4 = SGQuat<T>::fromAngleAxis(angleAxis);
165 if (!equivalent(q3, q4))
175 // Create some test matrix
176 SGVec3<T> v0(2, 7, 17);
177 SGQuat<T> q0 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), normalize(v0));
178 SGMatrix<T> m0(q0, v0);
180 // Check the tqo forms of the inverse for that kind of special matrix
184 if (!equivalent(m1, m2))
187 // Check matrix multiplication and inversion
188 if (!equivalent(m0*m1, SGMatrix<T>::unit()))
190 if (!equivalent(m1*m0, SGMatrix<T>::unit()))
192 if (!equivalent(m0*m2, SGMatrix<T>::unit()))
194 if (!equivalent(m2*m0, SGMatrix<T>::unit()))
203 // We know that the values are on the order of 1
204 double epsDeg = 10*SGLimits<double>::epsilon();
205 // For the altitude values we need to tolerate relative errors in the order
207 double epsM = 1e6*SGLimits<double>::epsilon();
209 SGVec3<double> cart0, cart1;
213 // create some geodetic position
214 geod0 = SGGeod::fromDegM(30, 20, 17);
216 // Test the conversion routines to cartesian coordinates
219 if (epsDeg < fabs(geod0.getLongitudeDeg() - geod1.getLongitudeDeg()) ||
220 epsDeg < fabs(geod0.getLatitudeDeg() - geod1.getLatitudeDeg()) ||
221 epsM < fabs(geod0.getElevationM() - geod1.getElevationM()))
224 // Test the conversion routines to radial coordinates
227 if (!equivalent(cart0, cart1))
235 sgInterfaceTest(void)
237 SGVec3f v3f = SGVec3f::e2();
238 SGVec4f v4f = SGVec4f::e2();
239 SGQuatf qf = SGQuatf::fromEulerRad(1.2, 1.3, -0.4);
240 SGMatrixf mf(qf, v3f);
242 // Copy to and from plibs types check if result is equal,
243 // test for exact equality
246 sgCopyVec3(sv3f, v3f.sg());
247 sgCopyVec3(tv3f.sg(), sv3f);
251 // Copy to and from plibs types check if result is equal,
252 // test for exact equality
255 sgCopyVec4(sv4f, v4f.sg());
256 sgCopyVec4(tv4f.sg(), sv4f);
260 // Copy to and from plibs types check if result is equal,
261 // test for exact equality
264 sgCopyQuat(sqf, qf.sg());
265 sgCopyQuat(tqf.sg(), sqf);
269 // Copy to and from plibs types check if result is equal,
270 // test for exact equality
273 sgCopyMat4(smf, mf.sg());
274 sgCopyMat4(tmf.sg(), smf);
282 sgdInterfaceTest(void)
284 SGVec3d v3d = SGVec3d::e2();
285 SGVec4d v4d = SGVec4d::e2();
286 SGQuatd qd = SGQuatd::fromEulerRad(1.2, 1.3, -0.4);
287 SGMatrixd md(qd, v3d);
289 // Copy to and from plibs types check if result is equal,
290 // test for exact equality
293 sgdCopyVec3(sv3d, v3d.sg());
294 sgdCopyVec3(tv3d.sg(), sv3d);
298 // Copy to and from plibs types check if result is equal,
299 // test for exact equality
302 sgdCopyVec4(sv4d, v4d.sg());
303 sgdCopyVec4(tv4d.sg(), sv4d);
307 // Copy to and from plibs types check if result is equal,
308 // test for exact equality
311 sgdCopyQuat(sqd, qd.sg());
312 sgdCopyQuat(tqd.sg(), sqd);
316 // Copy to and from plibs types check if result is equal,
317 // test for exact equality
320 sgdCopyMat4(smd, md.sg());
321 sgdCopyMat4(tmd.sg(), smd);
332 if (!Vec3Test<float>())
334 if (!Vec3Test<double>())
337 // Do quaternion tests
338 if (!QuatTest<float>())
340 if (!QuatTest<double>())
344 if (!MatrixTest<float>())
346 if (!MatrixTest<double>())
349 // Check geodetic/geocentric/cartesian conversions
350 // if (!GeodesyTest())
351 // return EXIT_FAILURE;
353 // Check interaction with sg*/sgd*
354 if (!sgInterfaceTest())
356 if (!sgdInterfaceTest())
359 std::cout << "Successfully passed all tests!" << std::endl;