test_aosq.c (5264B)
1 /* Copyright (C) 2014-2019, 2021, 2023, 2025 Vincent Forest (vaplv@free.fr) 2 * 3 * The RSIMD 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 RSIMD 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 RSIMD library. If not, see <http://www.gnu.org/licenses/>. */ 15 16 #include "aosq.h" 17 #include "aosf33.h" 18 #include <rsys/float33.h> 19 20 #define AOSF33_EQ_EPS(M, A, B, C, D, E, F, G, H, I, Eps) \ 21 { \ 22 float a[9], b[9]; \ 23 b[0] = (A); b[1] = (B); b[2] = (C); \ 24 b[3] = (D); b[4] = (E); b[5] = (F); \ 25 b[6] = (G); b[7] = (H); b[8] = (I); \ 26 CHK(f33_eq_eps(aosf33_store(a, (M)), b, Eps) == 1); \ 27 } (void)0 28 29 int 30 main(int argc, char** argv) 31 { 32 union { int32_t i; float f; } cast; 33 v4f_T q0, q1, q2, t; 34 v4f_T m[3]; 35 (void)argc, (void)argv; 36 37 q0 = aosq_identity(); 38 CHK(v4f_x(q0) == 0.f); 39 CHK(v4f_y(q0) == 0.f); 40 CHK(v4f_z(q0) == 0.f); 41 CHK(v4f_w(q0) == 1.f); 42 43 q0 = aosq_set_axis_angle(v4f_set(2.f, 5.f, 1.f, 0.f), v4f_set1((float)PI*0.3f)); 44 CHK(eq_eps(v4f_x(q0), 0.907981f, 1.e-6f) == 1); 45 CHK(eq_eps(v4f_y(q0), 2.269953f, 1.e-6f) == 1); 46 CHK(eq_eps(v4f_z(q0), 0.453991f, 1.e-6f) == 1); 47 CHK(eq_eps(v4f_w(q0), 0.891007f, 1.e-6f) == 1); 48 49 q0 = v4f_set(1.f, 2.f, 3.f, -3.f); 50 q1 = v4f_set(1.f, 2.f, 3.f, -3.f); 51 t = aosq_eq(q0, q1); 52 cast.f = v4f_x(t); CHK(cast.i == (int32_t)0xFFFFFFFF); 53 cast.f = v4f_y(t); CHK(cast.i == (int32_t)0xFFFFFFFF); 54 cast.f = v4f_z(t); CHK(cast.i == (int32_t)0xFFFFFFFF); 55 cast.f = v4f_w(t); CHK(cast.i == (int32_t)0xFFFFFFFF); 56 57 q1 = v4f_set(0.f, 2.f, 3.f, -3.f); 58 t = aosq_eq(q0, q1); 59 cast.f = v4f_x(t); CHK(cast.i == (int32_t)0x00000000); 60 cast.f = v4f_y(t); CHK(cast.i == (int32_t)0x00000000); 61 cast.f = v4f_z(t); CHK(cast.i == (int32_t)0x00000000); 62 cast.f = v4f_w(t); CHK(cast.i == (int32_t)0x00000000); 63 64 q1 = v4f_set(1.f, 0.f, 3.f, -3.f); 65 t = aosq_eq(q0, q1); 66 cast.f = v4f_x(t); CHK(cast.i == (int32_t)0x00000000); 67 cast.f = v4f_y(t); CHK(cast.i == (int32_t)0x00000000); 68 cast.f = v4f_z(t); CHK(cast.i == (int32_t)0x00000000); 69 cast.f = v4f_w(t); CHK(cast.i == (int32_t)0x00000000); 70 71 q1 = v4f_set(1.f, 2.f, 0.f, -3.f); 72 t = aosq_eq(q0, q1); 73 cast.f = v4f_x(t); CHK(cast.i == (int32_t)0x00000000); 74 cast.f = v4f_y(t); CHK(cast.i == (int32_t)0x00000000); 75 cast.f = v4f_z(t); CHK(cast.i == (int32_t)0x00000000); 76 cast.f = v4f_w(t); CHK(cast.i == (int32_t)0x00000000); 77 78 q1 = v4f_set(1.f, 2.f, 3.f, 0.f); 79 t = aosq_eq(q0, q1); 80 cast.f = v4f_x(t); CHK(cast.i == (int32_t)0x00000000); 81 cast.f = v4f_y(t); CHK(cast.i == (int32_t)0x00000000); 82 cast.f = v4f_z(t); CHK(cast.i == (int32_t)0x00000000); 83 cast.f = v4f_w(t); CHK(cast.i == (int32_t)0x00000000); 84 85 q1 = v4f_set(1.01f, 2.f, 3.02f, -3.f); 86 t = aosq_eq_eps(q0, q1, v4f_set1(0.01f)); 87 cast.f = v4f_x(t); CHK(cast.i == (int32_t)0x00000000); 88 cast.f = v4f_y(t); CHK(cast.i == (int32_t)0x00000000); 89 cast.f = v4f_z(t); CHK(cast.i == (int32_t)0x00000000); 90 cast.f = v4f_w(t); CHK(cast.i == (int32_t)0x00000000); 91 t = aosq_eq_eps(q0, q1, v4f_set1(0.02f)); 92 cast.f = v4f_x(t); CHK(cast.i == (int32_t)0xFFFFFFFF); 93 cast.f = v4f_y(t); CHK(cast.i == (int32_t)0xFFFFFFFF); 94 cast.f = v4f_z(t); CHK(cast.i == (int32_t)0xFFFFFFFF); 95 cast.f = v4f_w(t); CHK(cast.i == (int32_t)0xFFFFFFFF); 96 97 q0 = v4f_set(1.f, 2.f, 3.f, 4.f); 98 q1 = v4f_set(5.f, 6.f, 7.f, 8.f); 99 q2 = aosq_mul(q0, q1); 100 CHK(v4f_x(q2) == 24.f); 101 CHK(v4f_y(q2) == 48.f); 102 CHK(v4f_z(q2) == 48.f); 103 CHK(v4f_w(q2) == -6.f); 104 105 q2 = aosq_conj(q0); 106 CHK(v4f_x(q2) == -1.f); 107 CHK(v4f_y(q2) == -2.f); 108 CHK(v4f_z(q2) == -3.f); 109 CHK(v4f_w(q2) == 4.f); 110 111 q0 = v4f_normalize(v4f_set(1.f, 2.f, 5.f, 0.5f)); 112 q1 = v4f_xyzz(q0); 113 q1 = v4f_xyzd(q1, aosq_calca(q1)); 114 CHK(v4f_x(q0) == v4f_x(q1)); 115 CHK(v4f_y(q0) == v4f_y(q1)); 116 CHK(v4f_z(q0) == v4f_z(q1)); 117 CHK(eq_eps(v4f_w(q0), v4f_w(q1), 1.e-6f) == 1); 118 119 q0 = v4f_set(1.f, 2.f, 3.f, 5.f); 120 q1 = v4f_set(2.f, 6.f, 7.f, 6.f); 121 q2 = aosq_slerp(q0, q1, v4f_set1(0.3f)); 122 CHK(eq_eps(v4f_x(q2), 1.3f, 1.e-6f) == 1); 123 CHK(eq_eps(v4f_y(q2), 3.2f, 1.e-6f) == 1); 124 CHK(eq_eps(v4f_z(q2), 4.2f, 1.e-6f) == 1); 125 CHK(eq_eps(v4f_w(q2), 5.3f, 1.e-6f) == 1); 126 127 q0 = v4f_set(2.f, 5.f, 17.f, 9.f); 128 aosq_to_aosf33(q0, m); 129 AOSF33_EQ_EPS(m, 130 -627.f, 326.f, -22.f, 131 -286.f, -585.f, 206.f, 132 158.f, 134.f, -57.f, 133 1.e-6f); 134 135 q0 = v4f_normalize(q0); 136 aosq_to_aosf33(q0, m); 137 AOSF33_EQ_EPS(m, 138 -0.573935f, 0.817043f, -0.055138f, 139 -0.716792f, -0.468672f, 0.516291f, 140 0.395990f, 0.335840f, 0.854637f, 141 1.e-6f); 142 143 return 0; 144 } 145