real33.h (6137B)
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 REAL_TYPE__ 17 #error Missing arguments 18 #endif 19 20 /* Generate common realXY funcs */ 21 #define REALX_DIMENSION__ 3 22 #define REALY_DIMENSION__ 3 23 #include "realXY_begin.h" 24 #include "realXY.h" 25 26 /* Specific real33 funcs */ 27 static FINLINE REAL_TYPE__* 28 REALXY_CTOR__ 29 (REAL_TYPE__* dst, 30 const REAL_TYPE__ a, const REAL_TYPE__ b, const REAL_TYPE__ c, 31 const REAL_TYPE__ d, const REAL_TYPE__ e, const REAL_TYPE__ f, 32 const REAL_TYPE__ g, const REAL_TYPE__ h, const REAL_TYPE__ i) 33 { 34 ASSERT(dst); 35 dst[0] = a; dst[1] = b; dst[2] = c; 36 dst[3] = d; dst[4] = e; dst[5] = f; 37 dst[6] = g; dst[7] = h; dst[8] = i; 38 return dst; 39 } 40 41 static FINLINE REAL_TYPE__ 42 REALXY_FUNC__(det)(const REAL_TYPE__* mat) 43 { 44 REAL_TYPE__ tmp[3]; 45 REALX_FUNC__(cross) 46 (tmp, REALXY_FUNC__(col_cptr)(mat, 0), REALXY_FUNC__(col_cptr)(mat, 1)); 47 return REALX_FUNC__(dot)(REALXY_FUNC__(col_cptr)(mat, 2), tmp); 48 } 49 50 static FINLINE REAL_TYPE__ 51 REALXY_FUNC__(invtrans)(REAL_TYPE__* dst, const REAL_TYPE__* src) 52 { 53 REAL_TYPE__ m33[9]; 54 REAL_TYPE__ det; 55 REALX_FUNC__(cross) 56 (REALXY_FUNC__(col_ptr)(m33, 0), 57 REALXY_FUNC__(col_cptr)(src, 1), 58 REALXY_FUNC__(col_cptr)(src, 2)); 59 REALX_FUNC__(cross) 60 (REALXY_FUNC__(col_ptr)(m33, 1), 61 REALXY_FUNC__(col_cptr)(src, 2), 62 REALXY_FUNC__(col_cptr)(src, 0)); 63 REALX_FUNC__(cross) 64 (REALXY_FUNC__(col_ptr)(m33, 2), 65 REALXY_FUNC__(col_cptr)(src, 0), 66 REALXY_FUNC__(col_cptr)(src, 1)); 67 68 det = REALX_FUNC__(dot) 69 (REALXY_FUNC__(col_cptr)(m33, 2), 70 REALXY_FUNC__(col_cptr)(src, 2)); 71 REALXY_FUNC__(mul)(dst, m33, 1.f / det); 72 return det; 73 } 74 75 static FINLINE REAL_TYPE__ 76 REALXY_FUNC__(inverse)(REAL_TYPE__* dst, const REAL_TYPE__* src) 77 { 78 REAL_TYPE__ m33[9]; 79 const REAL_TYPE__ det = REALXY_FUNC__(invtrans)(m33, src); 80 REALXY_FUNC__(transpose)(dst, m33); 81 return det; 82 } 83 84 static INLINE REAL_TYPE__* 85 REALXY_FUNC__(rotation) /* XYZ norm */ 86 (REAL_TYPE__* dst, 87 /* In radian */ 88 const REAL_TYPE__ pitch, 89 const REAL_TYPE__ yaw, 90 const REAL_TYPE__ roll) 91 { 92 const REAL_TYPE__ c1 = (REAL_TYPE__)cos((double)pitch); 93 const REAL_TYPE__ c2 = (REAL_TYPE__)cos((double)yaw); 94 const REAL_TYPE__ c3 = (REAL_TYPE__)cos((double)roll); 95 const REAL_TYPE__ s1 = (REAL_TYPE__)sin((double)pitch); 96 const REAL_TYPE__ s2 = (REAL_TYPE__)sin((double)yaw); 97 const REAL_TYPE__ s3 = (REAL_TYPE__)sin((double)roll); 98 ASSERT(dst); 99 dst[0] = c2*c3; dst[1] = c1*s3 + c3*s1*s2; dst[2] = s1*s3 - c1*c3*s2; 100 dst[3] =-c2*s3; dst[4] = c1*c3 - s1*s2*s3; dst[5] = c1*s2*s3 + c3*s1; 101 dst[6] = s2; dst[7] =-c2*s1; dst[8] = c1*c2; 102 return dst; 103 } 104 105 static INLINE REAL_TYPE__* 106 REALXY_FUNC__(rotation_axis_angle) 107 (REAL_TYPE__* dst, 108 const REAL_TYPE__ axis[3], /* Should be normalized */ 109 const REAL_TYPE__ angle) /* In radian */ 110 { 111 const REAL_TYPE__ c = (REAL_TYPE__)cos((double)angle); 112 const REAL_TYPE__ s = (REAL_TYPE__)sin((double)angle); 113 const REAL_TYPE__ C = 1 - c; 114 ASSERT(dst && axis && REALX_FUNC__(is_normalized)(axis)); 115 116 dst[0] = axis[0] * axis[0] * C + c; 117 dst[1] = axis[0] * axis[1] * C + s * axis[2]; 118 dst[2] = axis[0] * axis[2] * C - s * axis[1]; 119 120 dst[3] = axis[1] * axis[0] * C - s * axis[2]; 121 dst[4] = axis[1] * axis[1] * C + c; 122 dst[5] = axis[1] * axis[2] * C + s * axis[0]; 123 124 dst[6] = axis[2] * axis[0] * C + s * axis[1]; 125 dst[7] = axis[2] * axis[1] * C - s * axis[0]; 126 dst[8] = axis[2] * axis[2] * C + c; 127 return dst; 128 } 129 130 static INLINE REAL_TYPE__* 131 REALXY_FUNC__(rotation_pitch) 132 (REAL_TYPE__ dst[9], const REAL_TYPE__ pitch/* in radian */) 133 { 134 const REAL_TYPE__ c = (REAL_TYPE__)cos((double)pitch); 135 const REAL_TYPE__ s = (REAL_TYPE__)sin((double)pitch); 136 ASSERT(dst); 137 dst[0] = 1.f; dst[1] = 0.f; dst[2] = 0.f; 138 dst[3] = 0.f; dst[4] = c; dst[5] = s; 139 dst[6] = 0.f; dst[7] =-s; dst[8] = c; 140 return dst; 141 } 142 143 static INLINE REAL_TYPE__* 144 REALXY_FUNC__(rotation_yaw) 145 (REAL_TYPE__ dst[9], const REAL_TYPE__ yaw/* in radian */) 146 { 147 const REAL_TYPE__ c = (REAL_TYPE__)cos((double)yaw); 148 const REAL_TYPE__ s = (REAL_TYPE__)sin((double)yaw); 149 ASSERT(dst); 150 dst[0] = c; dst[1] = 0.f; dst[2] =-s; 151 dst[3] = 0.f; dst[4] = 1.f; dst[5] = 0.f; 152 dst[6] = s; dst[7] = 0.f; dst[8] = c; 153 return dst; 154 } 155 156 static INLINE REAL_TYPE__* 157 REALXY_FUNC__(rotation_roll) 158 (REAL_TYPE__ dst[9], const REAL_TYPE__ roll/* in radian */) 159 { 160 const REAL_TYPE__ c = (REAL_TYPE__)cos((double)roll); 161 const REAL_TYPE__ s = (REAL_TYPE__)sin((double)roll); 162 ASSERT(dst); 163 dst[0] = c; dst[1] = s; dst[2] = 0.f; 164 dst[3] =-s; dst[4] = c; dst[5] = 0.f; 165 dst[6] = 0.f; dst[7] = 0.f; dst[8] = 1.f; 166 return dst; 167 } 168 169 static INLINE REAL_TYPE__* 170 REALXY_FUNC__(basis)(REAL_TYPE__ dst[9], const REAL_TYPE__ N[3]) 171 { 172 REAL_TYPE__ a[3], b[3], x[3], y[3], normal[3], len; 173 ASSERT(N && REALX_FUNC__(is_normalized(N))); 174 REALX_FUNC__(set)(normal, N); 175 a[0] = 1.f, a[1] = 0.f, a[2] = 0.f; 176 b[0] = 0.f, b[1] = 1.f, b[2] = 0.f; 177 REALX_FUNC__(cross)(a, a, normal); 178 REALX_FUNC__(cross)(b, b, normal); 179 len = REALX_FUNC__(normalize) 180 (x, REALX_FUNC__(dot)(a, a) > REALX_FUNC__(dot)(b, b) ? a : b); 181 if(len <= 0.f) { 182 ASSERT(0 && "Degenerated normal"); 183 return REALXY_FUNC__(splat)(dst, 0.f); 184 } 185 len = REALX_FUNC__(normalize)(y, REALX_FUNC__(cross)(y, normal, x)); 186 ASSERT(len > 0); 187 REALX_FUNC__(set)(dst + 0, x); 188 REALX_FUNC__(set)(dst + 3, y); 189 REALX_FUNC__(set)(dst + 6, normal); 190 return dst; 191 } 192 193 #include "realXY_end.h"