real44.h (6056B)
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 "rsys.h" 17 18 #ifndef REAL_TYPE__ 19 #error Missing arguments 20 #endif 21 22 #ifdef REAL33_FUNC__ 23 #error Unexpected macro definition 24 #endif 25 26 #define REAL33_FUNC__(Func) CONCAT(CONCAT(CONCAT(REAL_LETTER__, 33), _), Func) 27 28 /* Generate common realXY funcs */ 29 #define REALX_DIMENSION__ 4 30 #define REALY_DIMENSION__ 4 31 #include "realXY_begin.h" 32 #include "realXY.h" 33 34 /* Specific REAL_TYPE__44 funcs*/ 35 static FINLINE REAL_TYPE__* 36 REALXY_CTOR__ 37 (REAL_TYPE__* dst, 38 const REAL_TYPE__ a, const REAL_TYPE__ b, const REAL_TYPE__ c, const REAL_TYPE__ d, 39 const REAL_TYPE__ e, const REAL_TYPE__ f, const REAL_TYPE__ g, const REAL_TYPE__ h, 40 const REAL_TYPE__ i, const REAL_TYPE__ j, const REAL_TYPE__ k, const REAL_TYPE__ l, 41 const REAL_TYPE__ m, const REAL_TYPE__ n, const REAL_TYPE__ o, const REAL_TYPE__ p) 42 { 43 ASSERT(dst); 44 dst[0] = a; dst[1] = b; dst[2] = c; dst[3] = d; 45 dst[4] = e; dst[5] = f; dst[6] = g; dst[7] = h; 46 dst[8] = i; dst[9] = j; dst[10] = k; dst[11] = l; 47 dst[12] = m; dst[13] = n; dst[14] = o; dst[15] = p; 48 return dst; 49 } 50 51 static FINLINE REAL_TYPE__ 52 REALXY_FUNC__(det)(const REAL_TYPE__* mat) 53 { 54 REAL_TYPE__ m33[9]; 55 REAL_TYPE__ row3[4], tmp[4]; 56 ASSERT(mat); 57 58 #define C3( Dst, Mat, ICol ) \ 59 (Dst)[0] = Mat[ICol*4], (Dst)[1] = Mat[ICol*4+1], (Dst)[2] = Mat[ICol*4+2] 60 C3(m33 + 0, mat, 1); C3(m33 + 3, mat, 2); C3(m33 + 6, mat, 3); 61 tmp[0] = -REAL33_FUNC__(det)(m33); 62 C3(m33 + 0, mat, 0); C3(m33 + 3, mat, 2); C3(m33 + 6, mat, 3); 63 tmp[1] = REAL33_FUNC__(det)(m33); 64 C3(m33 + 0, mat, 0); C3(m33 + 3, mat, 1); C3(m33 + 6, mat, 3); 65 tmp[2] = -REAL33_FUNC__(det)(m33); 66 C3(m33 + 0, mat, 0); C3(m33 + 3, mat, 1); C3(m33 + 6, mat, 2); 67 #undef C3 68 tmp[3] = REAL33_FUNC__(det)(m33); 69 REALXY_FUNC__(row)(row3, mat, 3); 70 return REALX_FUNC__(dot)(tmp, row3); 71 } 72 73 static INLINE REAL_TYPE__ 74 REALXY_FUNC__(inverse)(REAL_TYPE__* dst, const REAL_TYPE__* m) 75 { 76 REAL_TYPE__ tmp[9]; 77 REAL_TYPE__ det_012[4], det_023[4], det_123[4], det_013[4]; 78 REAL_TYPE__ cofacts[4]; 79 REAL_TYPE__ det, rcp_det, mpmp_rcp_det[4], pmpm_rcp_det[4]; 80 ASSERT( dst && m ); 81 82 /* Define the 3x3 sub matrices and compute their determinants */ 83 #define C3( Dst, Mat, ICol, X, Y, Z ) \ 84 (Dst)[0] = Mat[ICol*4+X], (Dst)[1] = Mat[ICol*4+Y], (Dst)[2] = Mat[ICol*4+Z] 85 C3(tmp+0 ,m, 1, 0, 1, 2); C3(tmp+3, m, 2, 0, 1, 2); C3(tmp+6, m, 3, 0, 1, 2); 86 det_012[0] = REAL33_FUNC__(det)(tmp); 87 C3(tmp+0 ,m, 0, 0, 1, 2); C3(tmp+3, m, 2, 0, 1, 2); C3(tmp+6, m, 3, 0, 1, 2); 88 det_012[1] = REAL33_FUNC__(det)(tmp); 89 C3(tmp+0 ,m, 0, 0, 1, 2); C3(tmp+3, m, 1, 0, 1, 2); C3(tmp+6, m, 3, 0, 1, 2); 90 det_012[2] = REAL33_FUNC__(det)(tmp); 91 C3(tmp+0 ,m, 0, 0, 1, 2); C3(tmp+3, m, 1, 0, 1, 2); C3(tmp+6, m, 2, 0, 1, 2); 92 det_012[3] = REAL33_FUNC__(det)(tmp); 93 94 C3(tmp+0 ,m, 1, 0, 2, 3); C3(tmp+3, m, 2, 0, 2, 3); C3(tmp+6, m, 3, 0, 2, 3); 95 det_023[0] = REAL33_FUNC__(det)(tmp); 96 C3(tmp+0 ,m, 0, 0, 2, 3); C3(tmp+3, m, 2, 0, 2, 3); C3(tmp+6, m, 3, 0, 2, 3); 97 det_023[1] = REAL33_FUNC__(det)(tmp); 98 C3(tmp+0 ,m, 0, 0, 2, 3); C3(tmp+3, m, 1, 0, 2, 3); C3(tmp+6, m, 3, 0, 2, 3); 99 det_023[2] = REAL33_FUNC__(det)(tmp); 100 C3(tmp+0 ,m, 0, 0, 2, 3); C3(tmp+3, m, 1, 0, 2, 3); C3(tmp+6, m, 2, 0, 2, 3); 101 det_023[3] = REAL33_FUNC__(det)(tmp); 102 103 C3(tmp+0 ,m, 1, 1, 2, 3); C3(tmp+3, m, 2, 1, 2, 3); C3(tmp+6, m, 3, 1, 2, 3); 104 det_123[0] = REAL33_FUNC__(det)(tmp); 105 C3(tmp+0 ,m, 0, 1, 2, 3); C3(tmp+3, m, 2, 1, 2, 3); C3(tmp+6, m, 3, 1, 2, 3); 106 det_123[1] = REAL33_FUNC__(det)(tmp); 107 C3(tmp+0 ,m, 0, 1, 2, 3); C3(tmp+3, m, 1, 1, 2, 3); C3(tmp+6, m, 3, 1, 2, 3); 108 det_123[2] = REAL33_FUNC__(det)(tmp); 109 C3(tmp+0 ,m, 0, 1, 2, 3); C3(tmp+3, m, 1, 1, 2, 3); C3(tmp+6, m, 2, 1, 2, 3); 110 det_123[3] = REAL33_FUNC__(det)(tmp); 111 112 C3(tmp+0 ,m, 1, 0, 1, 3); C3(tmp+3, m, 2, 0, 1, 3); C3(tmp+6, m, 3, 0, 1, 3); 113 det_013[0] = REAL33_FUNC__(det)(tmp); 114 C3(tmp+0 ,m, 0, 0, 1, 3); C3(tmp+3, m, 2, 0, 1, 3); C3(tmp+6, m, 3, 0, 1, 3); 115 det_013[1] = REAL33_FUNC__(det)(tmp); 116 C3(tmp+0 ,m, 0, 0, 1, 3); C3(tmp+3, m, 1, 0, 1, 3); C3(tmp+6, m, 3, 0, 1, 3); 117 det_013[2] = REAL33_FUNC__(det)(tmp); 118 C3(tmp+0 ,m, 0, 0, 1, 3); C3(tmp+3, m, 1, 0, 1, 3); C3(tmp+6, m, 2, 0, 1, 3); 119 det_013[3] = REAL33_FUNC__(det)(tmp); 120 #undef C3 121 122 cofacts[0] = -det_012[0]; 123 cofacts[1] = det_012[1]; 124 cofacts[2] = -det_012[2]; 125 cofacts[3] = det_012[3]; 126 127 /* Compute the determinant of src */ 128 tmp[0] = m[3], tmp[1] = m[7], tmp[2] = m[11], tmp[3] = m[15]; 129 det = REALX_FUNC__(dot)(cofacts, tmp); 130 131 /* Invert the matrix */ 132 if(det == 0.f) 133 return det; 134 135 rcp_det = 1.f / det; 136 137 mpmp_rcp_det[0] = mpmp_rcp_det[2] = -rcp_det; 138 mpmp_rcp_det[1] = mpmp_rcp_det[3] = rcp_det; 139 pmpm_rcp_det[0] = pmpm_rcp_det[2] = rcp_det; 140 pmpm_rcp_det[1] = pmpm_rcp_det[3] = -rcp_det; 141 REALX_FUNC__(mul)(REALXY_FUNC__(col_ptr)(dst, 0), det_123, pmpm_rcp_det); 142 REALX_FUNC__(mul)(REALXY_FUNC__(col_ptr)(dst, 1), det_023, mpmp_rcp_det); 143 REALX_FUNC__(mul)(REALXY_FUNC__(col_ptr)(dst, 2), det_013, pmpm_rcp_det); 144 REALX_FUNC__(mul)(REALXY_FUNC__(col_ptr)(dst, 3), det_012, mpmp_rcp_det); 145 return det; 146 } 147 static FINLINE REAL_TYPE__ 148 REALXY_FUNC__(invtrans)(REAL_TYPE__* dst, const REAL_TYPE__* mat) 149 { 150 const REAL_TYPE__ det = REALXY_FUNC__(inverse)(dst, mat); 151 REALXY_FUNC__(transpose)(dst, dst); 152 return det; 153 } 154 155 #undef REAL33_FUNC__ 156 157 #include "realXY_end.h"