quaternion.h (2551B)
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 #ifndef QUATERNION_H 17 #define QUATERNION_H 18 19 #include "float3.h" 20 #include "float4.h" 21 22 /* 23 * Quaternion encoded in a float4 as { i, j, k, a } 24 */ 25 26 static FINLINE float* 27 quat_identity(float quat[4]) 28 { 29 return f4(quat, 0.f, 0.f, 0.f, 1.f); 30 } 31 32 static FINLINE float* 33 quat_set_axis_angle(float quat[4], const float axis[3], const float angle) 34 { 35 const float hangle = angle * 0.5f; 36 const float s = (float)sin((double)hangle); 37 const float c = (float)cos((double)hangle); 38 ASSERT(quat && axis); 39 40 f3_set(quat, axis); 41 f3_mulf(quat, quat, s); 42 quat[3] = c; 43 return quat; 44 } 45 46 static FINLINE float* 47 quat_conj(float dst[4], const float quat[4]) /* { -ix, -jy, -kz, a } */ 48 { 49 ASSERT(dst && quat); 50 return f4(dst, -quat[0], -quat[1], -quat[2], quat[3]); 51 } 52 53 static FINLINE float* 54 quat_mul(float dst[4], const float q0[4], const float q1[4]) 55 { 56 float res[4]; 57 ASSERT(dst && q0 && q1); 58 res[0] = q0[3]*q1[0] + q0[0]*q1[3] + q0[1]*q1[2] - q0[2]*q1[1]; 59 res[1] = q0[3]*q1[1] - q0[0]*q1[2] + q0[1]*q1[3] + q0[2]*q1[0]; 60 res[2] = q0[3]*q1[2] + q0[0]*q1[1] - q0[1]*q1[0] + q0[2]*q1[3]; 61 res[3] = q0[3]*q1[3] - q0[0]*q1[0] - q0[1]*q1[1] - q0[2]*q1[2]; 62 return f4_set__(dst, res); 63 } 64 65 static FINLINE float 66 quat_calca(const float ijk[3]) 67 { 68 float ijk_sqr; 69 ASSERT(ijk); 70 ijk_sqr = f3_dot(ijk, ijk); 71 return (float)sqrt((double)absf(1.f - ijk_sqr)); 72 } 73 74 static FINLINE float* /* Normalized Linear interpolation */ 75 quat_nlerp(float dst[4], const float from[4], const float to[4], const float t) 76 { 77 f4_normalize(dst, f4_lerp(dst, from, to, t)); 78 return dst; 79 } 80 81 RSYS_API float* 82 quat_slerp /* Spherical linear interplation */ 83 (float dst[4], 84 const float from[4], 85 const float to[4], 86 const float t); 87 88 RSYS_API float* 89 quat_to_f33 90 (float mat33[9] /* column major matrix */, 91 const float quat[4]); 92 93 #endif /* QUATERNION_H */