quaternion.c (2291B)
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 float* 19 quat_slerp(float dst[4], const float from[4], const float to[4], const float t) 20 { 21 float cos_omega; 22 float scale0, scale1; 23 float tmp0[4], tmp1[4]; 24 ASSERT(dst && from && to && t >= 0.f && t <= 1.f); 25 26 if(eq_eps(t, 0.f, 1.e-6f)) 27 return f4_set(dst, from); 28 if(eq_eps(t, 1.f, 1.e-6f)) 29 return f4_set(dst, to); 30 31 cos_omega = f4_dot(from, to); 32 if(cos_omega < 0.f) { 33 f4_minus(tmp0, to); 34 cos_omega = -cos_omega; 35 } else { 36 f4_set(tmp0, to); 37 } 38 if((1.f - cos_omega) > 1.e-6f) { 39 const double omega = acos((double)cos_omega); 40 const double sin_omega = sin(omega); 41 scale0 = (float)(sin(omega * t) / sin_omega); 42 scale1 = (float)(sin((1.0 - t) * omega) / sin_omega); 43 } else { 44 scale0 = t; 45 scale1 = 1 - t; 46 47 } 48 f4_mulf(tmp0, tmp0, scale0); 49 f4_mulf(tmp1, from, scale1); 50 return f4_add(dst, tmp0, tmp1); 51 } 52 53 float* 54 quat_to_f33(float mat33[9], const float quat[4]) 55 { 56 float i2j2k2[3]; 57 float ijka[4]; 58 ASSERT(mat33 && quat); 59 60 f3_mul(i2j2k2, quat, quat); 61 f4_set(ijka, quat); 62 63 mat33[0] = 1.f - 2.f * (i2j2k2[1] + i2j2k2[2]); 64 mat33[1] = 2.f * (ijka[0]*ijka[1] + ijka[2]*ijka[3]); 65 mat33[2] = 2.f * (ijka[0]*ijka[2] - ijka[1]*ijka[3]); 66 67 mat33[3] = 2.f * (ijka[0]*ijka[1] - ijka[2]*ijka[3]); 68 mat33[4] = 1.f - 2.f * (i2j2k2[0] + i2j2k2[2]); 69 mat33[5] = 2.f * (ijka[1]*ijka[2] + ijka[0]*ijka[3]); 70 71 mat33[6] = 2.f * (ijka[0]*ijka[2] + ijka[1]*ijka[3]); 72 mat33[7] = 2.f * (ijka[1]*ijka[2] - ijka[0]*ijka[3]); 73 mat33[8] = 1.f - 2.f * (i2j2k2[0] + i2j2k2[1]); 74 75 return mat33; 76 }