test_quaternion.c (2554B)
1 /* Copyright (C) 2013-2023, 2025 Vincent Forest (vaplv@free.fr) 2 * 3 * The RSys library is free software: you can redistribute it and/or modify 4 * it under the terms of the GNU General Public License as published 5 * by the Free Software Foundation, either version 3 of the License, or 6 * (at your option) any later version. 7 * 8 * The RSys 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 11 * GNU General Public License for more details. 12 * 13 * You should have received a copy of the GNU General Public License 14 * along with the RSys library. If not, see <http://www.gnu.org/licenses/>. */ 15 16 #include "quaternion.h" 17 18 int 19 main(int argc, char** argv) 20 { 21 float v3[3], m33[9]; 22 float q0[4], q1[4], q2[4]; 23 (void)argc, (void)argv; 24 25 CHK(quat_identity(q0) == q0); 26 CHK(q0[0] == 0.f); 27 CHK(q0[1] == 0.f); 28 CHK(q0[2] == 0.f); 29 CHK(q0[3] == 1.f); 30 31 f3(v3, 2.f, 5.f, 1.f); 32 CHK(quat_set_axis_angle(q0, v3, (float)PI*0.3f) == q0); 33 CHK(eq_eps(q0[0], 0.907981f, 1.e-6f) == 1); 34 CHK(eq_eps(q0[1], 2.269953f, 1.e-6f) == 1); 35 CHK(eq_eps(q0[2], 0.453991f, 1.e-6f) == 1); 36 CHK(eq_eps(q0[3], 0.891007f, 1.e-6f) == 1); 37 38 f4(q0, 1.f, 2.f, 3.f, 4.f); 39 f4(q1, 5.f, 6.f, 7.f, 8.f); 40 CHK(quat_mul(q2, q0, q1) == q2); 41 CHK(q2[0] == 24.f); 42 CHK(q2[1] == 48.f); 43 CHK(q2[2] == 48.f); 44 CHK(q2[3] == -6.f); 45 46 CHK(quat_conj(q2, q0) == q2); 47 CHK(q2[0] == -1.f); 48 CHK(q2[1] == -2.f); 49 CHK(q2[2] == -3.f); 50 CHK(q2[3] == 4.f); 51 52 f4_normalize(q0, f4(q0, 1.f, 2.f, 5.f, 0.5f)); 53 f3_set(q1, q0); 54 q1[3] = quat_calca(q1); 55 CHK(eq_eps(q1[3], q0[3], 1.e-6f) == 1); 56 57 f4(q0, 1.f, 2.F, 3.f, 5.f); 58 f4(q1, 2.f, 6.f, 7.f, 6.f); 59 CHK(quat_slerp(q2, q0, q1, 0.3f) == q2); 60 CHK(eq_eps(q2[0], 1.3f, 1.e-6f) == 1); 61 CHK(eq_eps(q2[1], 3.2f, 1.e-6f) == 1); 62 CHK(eq_eps(q2[2], 4.2f, 1.e-6f) == 1); 63 CHK(eq_eps(q2[3], 5.3f, 1.e-6f) == 1); 64 65 f4(q0, 2.f, 5.f, 17.f, 9.f); 66 CHK(quat_to_f33(m33, q0) == m33); 67 CHK(f3_eq_eps(m33 + 0, f3(v3, -627.f, 326.f, -22.f), 1.e-6f) == 1); 68 CHK(f3_eq_eps(m33 + 3, f3(v3, -286.f, -585.f, 206.f), 1.e-6f) == 1); 69 CHK(f3_eq_eps(m33 + 6, f3(v3, 158.f, 134.f, -57.f), 1.e-6f) == 1); 70 71 f4_normalize(q0, q0); 72 CHK(quat_to_f33(m33, q0) == m33); 73 CHK(f3_eq_eps(m33 + 0, f3(v3,-0.573935f, 0.817043f,-0.055138f), 1.e-6f) == 1); 74 CHK(f3_eq_eps(m33 + 3, f3(v3,-0.716792f,-0.468672f, 0.516291f), 1.e-6f) == 1); 75 CHK(f3_eq_eps(m33 + 6, f3(v3, 0.395990f, 0.335840f, 0.854637f), 1.e-6f) == 1); 76 77 return 0; 78 }