realXY.h (8545B)
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 /* 17 * Internal header used to generate funcs on column major REAL_TYPE__ matrix 18 * of X x Y dimensions 19 */ 20 21 #include "math.h" 22 #include "rsys.h" 23 24 #ifdef COMPILER_GCC 25 #pragma GCC push_options 26 #pragma GCC optimize("unroll-loops") 27 #endif 28 29 static FINLINE REAL_TYPE__* 30 REALXY_CAST__(REAL_TYPE__* dst, const REAL_TYPE_COMPATIBLE__* src) 31 { 32 int x, y, i; 33 ASSERT(dst && src); 34 i = 0; 35 FOR_EACH(x, 0, REALX_DIMENSION__) { 36 FOR_EACH(y, 0, REALY_DIMENSION__) { 37 dst[i] = (REAL_TYPE__)src[i]; 38 ++i; 39 }} 40 return dst; 41 } 42 43 static FINLINE REAL_TYPE__* 44 REALXY_FUNC__(splat)(REAL_TYPE__* dst, const REAL_TYPE__ val) 45 { 46 int i = 0; 47 ASSERT(dst); 48 FOR_EACH(i, 0, REALX_DIMENSION__*REALY_DIMENSION__) { 49 dst[i] = val; 50 } 51 return dst; 52 } 53 54 static FINLINE REAL_TYPE__* 55 REALXY_FUNC__(set__)(REAL_TYPE__* dst, const REAL_TYPE__* src) 56 { 57 int x, y, i; 58 ASSERT(dst && src); 59 ASSERT(!MEM_AREA_OVERLAP(dst, SIZEOF_REALXY__, src, SIZEOF_REALXY__)); 60 i = 0; 61 FOR_EACH(x, 0, REALX_DIMENSION__) { 62 FOR_EACH(y, 0, REALY_DIMENSION__) { 63 dst[i] = src[i]; 64 ++i; 65 }} 66 return dst; 67 } 68 69 static FINLINE REAL_TYPE__* 70 REALXY_FUNC__(set)(REAL_TYPE__* dst, const REAL_TYPE__* src) 71 { 72 ASSERT(dst && src); 73 if(!MEM_AREA_OVERLAP(dst, SIZEOF_REALXY__, src, SIZEOF_REALXY__)) { 74 return REALXY_FUNC__(set__)(dst, src); 75 } else { 76 REAL_TYPE__ tmp[REALX_DIMENSION__*REALY_DIMENSION__]; 77 return REALXY_FUNC__(set__)(dst, REALXY_FUNC__(set__)(tmp, src)); 78 } 79 } 80 81 static FINLINE REAL_TYPE__* 82 REALXY_FUNC__(set_row)(REAL_TYPE__* mat, const REAL_TYPE__* row, const int irow) 83 { 84 REAL_TYPE__ tmp[REALX_DIMENSION__]; 85 int x; 86 ASSERT(mat && row && irow < REALY_DIMENSION__ && irow >= 0); 87 88 FOR_EACH(x, 0, REALX_DIMENSION__) 89 tmp[x] = row[x]; 90 FOR_EACH(x, 0, REALX_DIMENSION__) 91 mat[x * REALY_DIMENSION__ + irow] = tmp[x]; 92 return mat; 93 } 94 95 static FINLINE REAL_TYPE__* 96 REALXY_FUNC__(set_col)(REAL_TYPE__* mat, const REAL_TYPE__* col, const int icol) 97 { 98 REAL_TYPE__ tmp[REALY_DIMENSION__]; 99 int y; 100 ASSERT(mat && col && icol < REALX_DIMENSION__ && icol >= 0); 101 FOR_EACH(y, 0, REALY_DIMENSION__) 102 tmp[y] = col[y]; 103 REALX_FUNC__(set)(mat + (icol * REALY_DIMENSION__), tmp); 104 return mat; 105 } 106 107 static FINLINE REAL_TYPE__* 108 REALXY_FUNC__(row)(REAL_TYPE__* row, const REAL_TYPE__* mat, const int irow) 109 { 110 REAL_TYPE__ tmp[REALX_DIMENSION__]; 111 int x; 112 ASSERT(row && mat && irow < REALY_DIMENSION__); 113 114 FOR_EACH(x, 0, REALX_DIMENSION__) 115 tmp[x] = mat[x * REALY_DIMENSION__ + irow]; 116 FOR_EACH(x, 0, REALX_DIMENSION__) 117 row[x] = tmp[x]; 118 return row; 119 } 120 121 static FINLINE REAL_TYPE__* 122 REALXY_FUNC__(col_ptr)(REAL_TYPE__* mat, const int icol) 123 { 124 ASSERT(mat && icol < REALX_DIMENSION__); 125 return mat + (icol * REALY_DIMENSION__); 126 } 127 128 static FINLINE const REAL_TYPE__* 129 REALXY_FUNC__(col_cptr)(const REAL_TYPE__* mat, const int icol) 130 { 131 ASSERT(mat && icol < REALX_DIMENSION__); 132 return mat + (icol * REALY_DIMENSION__); 133 } 134 135 static FINLINE REAL_TYPE__* 136 REALXY_FUNC__(col)(REAL_TYPE__* col, const REAL_TYPE__* mat, const int icol) 137 { 138 REAL_TYPE__ tmp[REALY_DIMENSION__]; 139 const REAL_TYPE__* col_ptr = REALXY_FUNC__(col_cptr)(mat, icol); 140 int x, y; 141 ASSERT(mat && icol < REALY_DIMENSION__); 142 143 FOR_EACH(y, 0, REALY_DIMENSION__) 144 tmp[y] = col_ptr[y]; 145 FOR_EACH(x, 0, REALX_DIMENSION__) 146 col[x] = tmp[x]; 147 return col; 148 } 149 150 static FINLINE REAL_TYPE__* 151 REALXY_FUNC__(add)(REAL_TYPE__* dst, const REAL_TYPE__* a, const REAL_TYPE__* b) 152 { 153 REAL_TYPE__ tmp[REALX_DIMENSION__*REALY_DIMENSION__]; 154 int i; 155 FOR_EACH(i, 0, REALX_DIMENSION__ * REALY_DIMENSION__) 156 tmp[i] = a[i] + b[i]; 157 return REALXY_FUNC__(set__)(dst, tmp); 158 } 159 160 static FINLINE REAL_TYPE__* 161 REALXY_FUNC__(sub)(REAL_TYPE__* dst, const REAL_TYPE__* a, const REAL_TYPE__* b) 162 { 163 REAL_TYPE__ tmp[REALX_DIMENSION__*REALY_DIMENSION__]; 164 int i; 165 FOR_EACH(i, 0, REALX_DIMENSION__ * REALY_DIMENSION__) 166 tmp[i] = a[i] - b[i]; 167 return REALXY_FUNC__(set__)(dst, tmp); 168 } 169 170 static FINLINE REAL_TYPE__* 171 REALXY_FUNC__(minus)(REAL_TYPE__* dst, const REAL_TYPE__* a) 172 { 173 REAL_TYPE__ tmp[REALX_DIMENSION__*REALY_DIMENSION__]; 174 int i; 175 FOR_EACH(i, 0, REALX_DIMENSION__ * REALY_DIMENSION__) 176 tmp[i] = -a[i]; 177 return REALXY_FUNC__(set__)(dst, tmp); 178 } 179 180 static FINLINE int 181 REALXY_FUNC__(eq)(const REAL_TYPE__* a, const REAL_TYPE__* b) 182 { 183 int is_eq = 1; 184 int x = 0; 185 ASSERT(a && b); 186 187 do { 188 const int i = x * REALY_DIMENSION__; 189 is_eq = REALX_FUNC__(eq)(a + i, b + i); 190 ++x; 191 } while(x < REALX_DIMENSION__ && is_eq); 192 return is_eq; 193 } 194 195 static FINLINE int 196 REALXY_FUNC__(eq_eps) 197 (const REAL_TYPE__* a, 198 const REAL_TYPE__* b, 199 const REAL_TYPE__ eps) 200 { 201 int is_eq = 1; 202 int x = 0; 203 ASSERT(a && b); 204 205 do { 206 const int i = x * REALY_DIMENSION__; 207 is_eq = REALX_FUNC__(eq_eps)(a + i, b + i, eps); 208 ++x; 209 } while(x < REALX_DIMENSION__ && is_eq); 210 return is_eq; 211 } 212 213 static FINLINE REAL_TYPE__* 214 REALXY_REALX_FUNC__(mul) 215 (REAL_TYPE__* dst, const REAL_TYPE__* mat, const REAL_TYPE__* vec) 216 { 217 REAL_TYPE__ row[REALX_DIMENSION__]; 218 REAL_TYPE__ tmp[REALY_DIMENSION__]; 219 int y; 220 ASSERT(dst && mat && vec); 221 222 FOR_EACH(y, 0, REALY_DIMENSION__) { 223 REALXY_FUNC__(row)(row, mat, y); 224 tmp[y] = REALX_FUNC__(dot)(row, vec); 225 } 226 return REALY_FUNC__(set)(dst, tmp); 227 } 228 229 static FINLINE REAL_TYPE__* 230 REALXY_FUNC__(mul)(REAL_TYPE__* dst, const REAL_TYPE__* mat, const REAL_TYPE__ b) 231 { 232 REAL_TYPE__ tmp[REALX_DIMENSION__*REALY_DIMENSION__]; 233 int i; 234 ASSERT(dst && mat); 235 FOR_EACH(i, 0, REALX_DIMENSION__ * REALY_DIMENSION__) 236 tmp[i] = mat[i] * b; 237 return REALXY_FUNC__(set__)(dst, tmp);; 238 } 239 240 static FINLINE REAL_TYPE__* 241 REALX_REALXY_FUNC__(mul) 242 (REAL_TYPE__* dst, const REAL_TYPE__* vec, const REAL_TYPE__* mat) 243 { 244 REAL_TYPE__ tmp[REALX_DIMENSION__]; 245 int x; 246 ASSERT(dst && vec && mat); 247 FOR_EACH(x, 0, REALX_DIMENSION__) { 248 const REAL_TYPE__* col = mat + (x * REALY_DIMENSION__); 249 tmp[x] = REALX_FUNC__(dot)(vec, col); 250 } 251 return REALX_FUNC__(set)(dst, tmp); 252 } 253 254 #if REALX_DIMENSION__ == REALY_DIMENSION__ 255 static FINLINE REAL_TYPE__* 256 REALXY_FUNC__(set_identity)(REAL_TYPE__* mat) 257 { 258 int x, y, i; 259 ASSERT(mat); 260 i = 0; 261 FOR_EACH(x, 0, REALX_DIMENSION__) { 262 FOR_EACH(y, 0, REALY_DIMENSION__) { 263 mat[i] = x == y ? 1.f : 0.f; 264 ++i; 265 }} 266 return mat; 267 } 268 269 static FINLINE int 270 REALXY_FUNC__(is_identity)(const REAL_TYPE__* mat) 271 { 272 int is_identity; 273 int x = 0; 274 do { 275 int y = 0; 276 do { 277 is_identity = mat[x*REALY_DIMENSION__ + y] == (REAL_TYPE__)(x==y); 278 ++y; 279 } while(y < REALY_DIMENSION__ && is_identity); 280 ++x; 281 } while(x < REALX_DIMENSION__ && is_identity); 282 return is_identity; 283 } 284 285 static FINLINE REAL_TYPE__* 286 REALXY_FUNC__(transpose)(REAL_TYPE__* dst, const REAL_TYPE__* src) 287 { 288 REAL_TYPE__ tmp[REALX_DIMENSION__ * REALY_DIMENSION__]; 289 int x, y, i; 290 ASSERT(dst && src); 291 292 i = 0; 293 FOR_EACH(x, 0, REALX_DIMENSION__) { 294 FOR_EACH(y, 0, REALY_DIMENSION__) { 295 tmp[y * REALY_DIMENSION__ + x] = src[i]; 296 ++i; 297 }} 298 return REALXY_FUNC__(set__)(dst, tmp); 299 } 300 301 static FINLINE REAL_TYPE__* 302 REALXY_REALXY_FUNC__(mul) 303 (REAL_TYPE__* dst, const REAL_TYPE__* a, const REAL_TYPE__* b) 304 { 305 REAL_TYPE__ tmp[REALX_DIMENSION__ * REALY_DIMENSION__]; 306 REAL_TYPE__ a_trans[REALX_DIMENSION__ * REALY_DIMENSION__]; 307 int x, y, i; 308 309 /* Transpose the a matrix */ 310 i = 0; 311 FOR_EACH(x, 0, REALX_DIMENSION__) { 312 FOR_EACH(y, 0, REALY_DIMENSION__) { 313 a_trans[y * REALY_DIMENSION__ + x] = a[i]; 314 ++i; 315 }} 316 /* Compute the a x b and store the result into tmp */ 317 i = 0; 318 FOR_EACH(x, 0, REALX_DIMENSION__) { 319 FOR_EACH(y, 0, REALY_DIMENSION__) { 320 tmp[i] = REALX_FUNC__(dot) 321 (a_trans + (y * REALY_DIMENSION__), b + (x * REALY_DIMENSION__)); 322 ++i; 323 }} 324 return REALXY_FUNC__(set__)(dst, tmp); 325 } 326 #endif /* REALX_DIMENSION__ == REALY_DIMENSION__ */ 327 328 #ifdef COMPILER_GCC 329 #pragma GCC pop_options 330 #endif