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>
35 // Check if the equivalent function works
36 v1 = SGVec3<T>(1, 2, 3);
37 v2 = SGVec3<T>(3, 2, 1);
38 if (equivalent(v1, v2))
41 // Check the unary minus operator
42 v3 = SGVec3<T>(-1, -2, -3);
43 if (!equivalent(-v1, v3))
46 // Check the unary plus operator
47 v3 = SGVec3<T>(1, 2, 3);
48 if (!equivalent(+v1, v3))
51 // Check the addition operator
52 v3 = SGVec3<T>(4, 4, 4);
53 if (!equivalent(v1 + v2, v3))
56 // Check the subtraction operator
57 v3 = SGVec3<T>(-2, 0, 2);
58 if (!equivalent(v1 - v2, v3))
61 // Check the scaler multiplication operator
62 v3 = SGVec3<T>(2, 4, 6);
63 if (!equivalent(2*v1, v3))
66 // Check the dot product
67 if (fabs(dot(v1, v2) - 10) > 10*SGLimits<T>::epsilon())
70 // Check the cross product
71 v3 = SGVec3<T>(-4, 8, -4);
72 if (!equivalent(cross(v1, v2), v3))
75 // Check the euclidean length
76 if (fabs(14 - length(v1)*length(v1)) > 14*SGLimits<T>::epsilon())
86 const SGVec3<T> e1(1, 0, 0);
87 const SGVec3<T> e2(0, 1, 0);
88 const SGVec3<T> e3(0, 0, 1);
90 SGQuat<T> q1, q2, q3, q4;
91 // Check a rotation around the x axis
92 q1 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), e1);
93 v1 = SGVec3<T>(1, 2, 3);
94 v2 = SGVec3<T>(1, -2, -3);
95 if (!equivalent(q1.transform(v1), v2))
98 // Check a rotation around the x axis
99 q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e1);
100 v2 = SGVec3<T>(1, 3, -2);
101 if (!equivalent(q1.transform(v1), v2))
104 // Check a rotation around the y axis
105 q1 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), e2);
106 v2 = SGVec3<T>(-1, 2, -3);
107 if (!equivalent(q1.transform(v1), v2))
110 // Check a rotation around the y axis
111 q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e2);
112 v2 = SGVec3<T>(-3, 2, 1);
113 if (!equivalent(q1.transform(v1), v2))
116 // Check a rotation around the z axis
117 q1 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), e3);
118 v2 = SGVec3<T>(-1, -2, 3);
119 if (!equivalent(q1.transform(v1), v2))
122 // Check a rotation around the z axis
123 q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e3);
124 v2 = SGVec3<T>(2, -1, 3);
125 if (!equivalent(q1.transform(v1), v2))
128 // Now check some successive transforms
129 // We can reuse the prevously tested stuff
130 q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e1);
131 q2 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e2);
133 v2 = q2.transform(q1.transform(v1));
134 if (!equivalent(q3.transform(v1), v2))
137 /// Test from Euler angles
138 float x = 0.2*SGMisc<T>::pi();
139 float y = 0.3*SGMisc<T>::pi();
140 float z = 0.4*SGMisc<T>::pi();
141 q1 = SGQuat<T>::fromAngleAxis(z, e3);
142 q2 = SGQuat<T>::fromAngleAxis(y, e2);
143 q3 = SGQuat<T>::fromAngleAxis(x, e1);
144 v2 = q3.transform(q2.transform(q1.transform(v1)));
145 q4 = SGQuat<T>::fromEulerRad(z, y, x);
146 if (!equivalent(q4.transform(v1), v2))
149 /// Test angle axis forward and back transform
150 q1 = SGQuat<T>::fromAngleAxis(0.2*SGMisc<T>::pi(), e1);
151 q2 = SGQuat<T>::fromAngleAxis(0.7*SGMisc<T>::pi(), e2);
154 q1.getAngleAxis(angleAxis);
155 q4 = SGQuat<T>::fromAngleAxis(angleAxis);
156 if (!equivalent(q1, q4))
158 q2.getAngleAxis(angleAxis);
159 q4 = SGQuat<T>::fromAngleAxis(angleAxis);
160 if (!equivalent(q2, q4))
162 q3.getAngleAxis(angleAxis);
163 q4 = SGQuat<T>::fromAngleAxis(angleAxis);
164 if (!equivalent(q3, q4))
174 // Create some test matrix
175 SGVec3<T> v0(2, 7, 17);
176 SGQuat<T> q0 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), normalize(v0));
177 SGMatrix<T> m0 = SGMatrix<T>::unit();
178 m0.postMultTranslate(v0);
179 m0.postMultRotate(q0);
181 // Check the three forms of the inverse for that kind of special matrix
182 SGMatrix<T> m1 = SGMatrix<T>::unit();
183 m1.preMultTranslate(-v0);
184 m1.preMultRotate(inverse(q0));
189 if (!equivalent(m1, m2))
191 if (!equivalent(m2, m3))
194 // Check matrix multiplication and inversion
195 if (!equivalent(m0*m1, SGMatrix<T>::unit()))
197 if (!equivalent(m1*m0, SGMatrix<T>::unit()))
199 if (!equivalent(m0*m2, SGMatrix<T>::unit()))
201 if (!equivalent(m2*m0, SGMatrix<T>::unit()))
203 if (!equivalent(m0*m3, SGMatrix<T>::unit()))
205 if (!equivalent(m3*m0, SGMatrix<T>::unit()))
214 // We know that the values are on the order of 1
215 double epsDeg = 10*360*SGLimits<double>::epsilon();
216 // For the altitude values we need to tolerate relative errors in the order
218 double epsM = 10*6e6*SGLimits<double>::epsilon();
220 SGVec3<double> cart0, cart1;
224 // create some geodetic position
225 geod0 = SGGeod::fromDegM(30, 20, 17);
227 // Test the conversion routines to cartesian coordinates
228 cart0 = SGVec3<double>::fromGeod(geod0);
229 geod1 = SGGeod::fromCart(cart0);
230 if (epsDeg < fabs(geod0.getLongitudeDeg() - geod1.getLongitudeDeg()) ||
231 epsDeg < fabs(geod0.getLatitudeDeg() - geod1.getLatitudeDeg()) ||
232 epsM < fabs(geod0.getElevationM() - geod1.getElevationM()))
235 // Test the conversion routines to radial coordinates
236 geoc0 = SGGeoc::fromCart(cart0);
237 cart1 = SGVec3<double>::fromGeoc(geoc0);
238 if (!equivalent(cart0, cart1))
246 sgInterfaceTest(void)
248 SGVec3f v3f = SGVec3f::e2();
249 SGVec4f v4f = SGVec4f::e2();
250 SGQuatf qf = SGQuatf::fromEulerRad(1.2, 1.3, -0.4);
252 mf.postMultTranslate(v3f);
253 mf.postMultRotate(qf);
255 // Copy to and from plibs types check if result is equal,
256 // test for exact equality
259 sgCopyVec3(sv3f, v3f.sg());
260 sgCopyVec3(tv3f.sg(), sv3f);
264 // Copy to and from plibs types check if result is equal,
265 // test for exact equality
268 sgCopyVec4(sv4f, v4f.sg());
269 sgCopyVec4(tv4f.sg(), sv4f);
273 // Copy to and from plibs types check if result is equal,
274 // test for exact equality
277 sgCopyQuat(sqf, qf.sg());
278 sgCopyQuat(tqf.sg(), sqf);
282 // Copy to and from plibs types check if result is equal,
283 // test for exact equality
286 sgCopyMat4(smf, mf.sg());
287 sgCopyMat4(tmf.sg(), smf);
295 sgdInterfaceTest(void)
297 SGVec3d v3d = SGVec3d::e2();
298 SGVec4d v4d = SGVec4d::e2();
299 SGQuatd qd = SGQuatd::fromEulerRad(1.2, 1.3, -0.4);
301 md.postMultTranslate(v3d);
302 md.postMultRotate(qd);
304 // Copy to and from plibs types check if result is equal,
305 // test for exact equality
308 sgdCopyVec3(sv3d, v3d.sg());
309 sgdCopyVec3(tv3d.sg(), sv3d);
313 // Copy to and from plibs types check if result is equal,
314 // test for exact equality
317 sgdCopyVec4(sv4d, v4d.sg());
318 sgdCopyVec4(tv4d.sg(), sv4d);
322 // Copy to and from plibs types check if result is equal,
323 // test for exact equality
326 sgdCopyQuat(sqd, qd.sg());
327 sgdCopyQuat(tqd.sg(), sqd);
331 // Copy to and from plibs types check if result is equal,
332 // test for exact equality
335 sgdCopyMat4(smd, md.sg());
336 sgdCopyMat4(tmd.sg(), smd);
347 if (!Vec3Test<float>())
349 if (!Vec3Test<double>())
352 // Do quaternion tests
353 if (!QuatTest<float>())
355 if (!QuatTest<double>())
359 if (!MatrixTest<float>())
361 if (!MatrixTest<double>())
364 // Check geodetic/geocentric/cartesian conversions
368 // Check interaction with sg*/sgd*
369 if (!sgInterfaceTest())
371 if (!sgdInterfaceTest())
374 std::cout << "Successfully passed all tests!" << std::endl;