| 1 | #ifndef GIM_LINEAR_H_INCLUDED |
|---|
| 2 | #define GIM_LINEAR_H_INCLUDED |
|---|
| 3 | |
|---|
| 4 | /*! \file gim_linear_math.h |
|---|
| 5 | *\author Francisco Len Nßjera |
|---|
| 6 | Type Independant Vector and matrix operations. |
|---|
| 7 | */ |
|---|
| 8 | /* |
|---|
| 9 | ----------------------------------------------------------------------------- |
|---|
| 10 | This source file is part of GIMPACT Library. |
|---|
| 11 | |
|---|
| 12 | For the latest info, see http://gimpact.sourceforge.net/ |
|---|
| 13 | |
|---|
| 14 | Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. |
|---|
| 15 | email: projectileman@yahoo.com |
|---|
| 16 | |
|---|
| 17 | This library is free software; you can redistribute it and/or |
|---|
| 18 | modify it under the terms of EITHER: |
|---|
| 19 | (1) The GNU Lesser General Public License as published by the Free |
|---|
| 20 | Software Foundation; either version 2.1 of the License, or (at |
|---|
| 21 | your option) any later version. The text of the GNU Lesser |
|---|
| 22 | General Public License is included with this library in the |
|---|
| 23 | file GIMPACT-LICENSE-LGPL.TXT. |
|---|
| 24 | (2) The BSD-style license that is included with this library in |
|---|
| 25 | the file GIMPACT-LICENSE-BSD.TXT. |
|---|
| 26 | (3) The zlib/libpng license that is included with this library in |
|---|
| 27 | the file GIMPACT-LICENSE-ZLIB.TXT. |
|---|
| 28 | |
|---|
| 29 | This library is distributed in the hope that it will be useful, |
|---|
| 30 | but WITHOUT ANY WARRANTY; without even the implied warranty of |
|---|
| 31 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files |
|---|
| 32 | GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. |
|---|
| 33 | |
|---|
| 34 | ----------------------------------------------------------------------------- |
|---|
| 35 | */ |
|---|
| 36 | |
|---|
| 37 | |
|---|
| 38 | #include "gim_math.h" |
|---|
| 39 | #include "gim_geom_types.h" |
|---|
| 40 | |
|---|
| 41 | |
|---|
| 42 | |
|---|
| 43 | |
|---|
| 44 | //! Zero out a 2D vector |
|---|
| 45 | #define VEC_ZERO_2(a) \ |
|---|
| 46 | { \ |
|---|
| 47 | (a)[0] = (a)[1] = 0.0f; \ |
|---|
| 48 | }\ |
|---|
| 49 | |
|---|
| 50 | |
|---|
| 51 | //! Zero out a 3D vector |
|---|
| 52 | #define VEC_ZERO(a) \ |
|---|
| 53 | { \ |
|---|
| 54 | (a)[0] = (a)[1] = (a)[2] = 0.0f; \ |
|---|
| 55 | }\ |
|---|
| 56 | |
|---|
| 57 | |
|---|
| 58 | /// Zero out a 4D vector |
|---|
| 59 | #define VEC_ZERO_4(a) \ |
|---|
| 60 | { \ |
|---|
| 61 | (a)[0] = (a)[1] = (a)[2] = (a)[3] = 0.0f; \ |
|---|
| 62 | }\ |
|---|
| 63 | |
|---|
| 64 | |
|---|
| 65 | /// Vector copy |
|---|
| 66 | #define VEC_COPY_2(b,a) \ |
|---|
| 67 | { \ |
|---|
| 68 | (b)[0] = (a)[0]; \ |
|---|
| 69 | (b)[1] = (a)[1]; \ |
|---|
| 70 | }\ |
|---|
| 71 | |
|---|
| 72 | |
|---|
| 73 | /// Copy 3D vector |
|---|
| 74 | #define VEC_COPY(b,a) \ |
|---|
| 75 | { \ |
|---|
| 76 | (b)[0] = (a)[0]; \ |
|---|
| 77 | (b)[1] = (a)[1]; \ |
|---|
| 78 | (b)[2] = (a)[2]; \ |
|---|
| 79 | }\ |
|---|
| 80 | |
|---|
| 81 | |
|---|
| 82 | /// Copy 4D vector |
|---|
| 83 | #define VEC_COPY_4(b,a) \ |
|---|
| 84 | { \ |
|---|
| 85 | (b)[0] = (a)[0]; \ |
|---|
| 86 | (b)[1] = (a)[1]; \ |
|---|
| 87 | (b)[2] = (a)[2]; \ |
|---|
| 88 | (b)[3] = (a)[3]; \ |
|---|
| 89 | }\ |
|---|
| 90 | |
|---|
| 91 | /// VECTOR SWAP |
|---|
| 92 | #define VEC_SWAP(b,a) \ |
|---|
| 93 | { \ |
|---|
| 94 | GIM_SWAP_NUMBERS((b)[0],(a)[0]);\ |
|---|
| 95 | GIM_SWAP_NUMBERS((b)[1],(a)[1]);\ |
|---|
| 96 | GIM_SWAP_NUMBERS((b)[2],(a)[2]);\ |
|---|
| 97 | }\ |
|---|
| 98 | |
|---|
| 99 | /// Vector difference |
|---|
| 100 | #define VEC_DIFF_2(v21,v2,v1) \ |
|---|
| 101 | { \ |
|---|
| 102 | (v21)[0] = (v2)[0] - (v1)[0]; \ |
|---|
| 103 | (v21)[1] = (v2)[1] - (v1)[1]; \ |
|---|
| 104 | }\ |
|---|
| 105 | |
|---|
| 106 | |
|---|
| 107 | /// Vector difference |
|---|
| 108 | #define VEC_DIFF(v21,v2,v1) \ |
|---|
| 109 | { \ |
|---|
| 110 | (v21)[0] = (v2)[0] - (v1)[0]; \ |
|---|
| 111 | (v21)[1] = (v2)[1] - (v1)[1]; \ |
|---|
| 112 | (v21)[2] = (v2)[2] - (v1)[2]; \ |
|---|
| 113 | }\ |
|---|
| 114 | |
|---|
| 115 | |
|---|
| 116 | /// Vector difference |
|---|
| 117 | #define VEC_DIFF_4(v21,v2,v1) \ |
|---|
| 118 | { \ |
|---|
| 119 | (v21)[0] = (v2)[0] - (v1)[0]; \ |
|---|
| 120 | (v21)[1] = (v2)[1] - (v1)[1]; \ |
|---|
| 121 | (v21)[2] = (v2)[2] - (v1)[2]; \ |
|---|
| 122 | (v21)[3] = (v2)[3] - (v1)[3]; \ |
|---|
| 123 | }\ |
|---|
| 124 | |
|---|
| 125 | |
|---|
| 126 | /// Vector sum |
|---|
| 127 | #define VEC_SUM_2(v21,v2,v1) \ |
|---|
| 128 | { \ |
|---|
| 129 | (v21)[0] = (v2)[0] + (v1)[0]; \ |
|---|
| 130 | (v21)[1] = (v2)[1] + (v1)[1]; \ |
|---|
| 131 | }\ |
|---|
| 132 | |
|---|
| 133 | |
|---|
| 134 | /// Vector sum |
|---|
| 135 | #define VEC_SUM(v21,v2,v1) \ |
|---|
| 136 | { \ |
|---|
| 137 | (v21)[0] = (v2)[0] + (v1)[0]; \ |
|---|
| 138 | (v21)[1] = (v2)[1] + (v1)[1]; \ |
|---|
| 139 | (v21)[2] = (v2)[2] + (v1)[2]; \ |
|---|
| 140 | }\ |
|---|
| 141 | |
|---|
| 142 | |
|---|
| 143 | /// Vector sum |
|---|
| 144 | #define VEC_SUM_4(v21,v2,v1) \ |
|---|
| 145 | { \ |
|---|
| 146 | (v21)[0] = (v2)[0] + (v1)[0]; \ |
|---|
| 147 | (v21)[1] = (v2)[1] + (v1)[1]; \ |
|---|
| 148 | (v21)[2] = (v2)[2] + (v1)[2]; \ |
|---|
| 149 | (v21)[3] = (v2)[3] + (v1)[3]; \ |
|---|
| 150 | }\ |
|---|
| 151 | |
|---|
| 152 | |
|---|
| 153 | /// scalar times vector |
|---|
| 154 | #define VEC_SCALE_2(c,a,b) \ |
|---|
| 155 | { \ |
|---|
| 156 | (c)[0] = (a)*(b)[0]; \ |
|---|
| 157 | (c)[1] = (a)*(b)[1]; \ |
|---|
| 158 | }\ |
|---|
| 159 | |
|---|
| 160 | |
|---|
| 161 | /// scalar times vector |
|---|
| 162 | #define VEC_SCALE(c,a,b) \ |
|---|
| 163 | { \ |
|---|
| 164 | (c)[0] = (a)*(b)[0]; \ |
|---|
| 165 | (c)[1] = (a)*(b)[1]; \ |
|---|
| 166 | (c)[2] = (a)*(b)[2]; \ |
|---|
| 167 | }\ |
|---|
| 168 | |
|---|
| 169 | |
|---|
| 170 | /// scalar times vector |
|---|
| 171 | #define VEC_SCALE_4(c,a,b) \ |
|---|
| 172 | { \ |
|---|
| 173 | (c)[0] = (a)*(b)[0]; \ |
|---|
| 174 | (c)[1] = (a)*(b)[1]; \ |
|---|
| 175 | (c)[2] = (a)*(b)[2]; \ |
|---|
| 176 | (c)[3] = (a)*(b)[3]; \ |
|---|
| 177 | }\ |
|---|
| 178 | |
|---|
| 179 | |
|---|
| 180 | /// accumulate scaled vector |
|---|
| 181 | #define VEC_ACCUM_2(c,a,b) \ |
|---|
| 182 | { \ |
|---|
| 183 | (c)[0] += (a)*(b)[0]; \ |
|---|
| 184 | (c)[1] += (a)*(b)[1]; \ |
|---|
| 185 | }\ |
|---|
| 186 | |
|---|
| 187 | |
|---|
| 188 | /// accumulate scaled vector |
|---|
| 189 | #define VEC_ACCUM(c,a,b) \ |
|---|
| 190 | { \ |
|---|
| 191 | (c)[0] += (a)*(b)[0]; \ |
|---|
| 192 | (c)[1] += (a)*(b)[1]; \ |
|---|
| 193 | (c)[2] += (a)*(b)[2]; \ |
|---|
| 194 | }\ |
|---|
| 195 | |
|---|
| 196 | |
|---|
| 197 | /// accumulate scaled vector |
|---|
| 198 | #define VEC_ACCUM_4(c,a,b) \ |
|---|
| 199 | { \ |
|---|
| 200 | (c)[0] += (a)*(b)[0]; \ |
|---|
| 201 | (c)[1] += (a)*(b)[1]; \ |
|---|
| 202 | (c)[2] += (a)*(b)[2]; \ |
|---|
| 203 | (c)[3] += (a)*(b)[3]; \ |
|---|
| 204 | }\ |
|---|
| 205 | |
|---|
| 206 | |
|---|
| 207 | /// Vector dot product |
|---|
| 208 | #define VEC_DOT_2(a,b) ((a)[0]*(b)[0] + (a)[1]*(b)[1]) |
|---|
| 209 | |
|---|
| 210 | |
|---|
| 211 | /// Vector dot product |
|---|
| 212 | #define VEC_DOT(a,b) ((a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2]) |
|---|
| 213 | |
|---|
| 214 | /// Vector dot product |
|---|
| 215 | #define VEC_DOT_4(a,b) ((a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2] + (a)[3]*(b)[3]) |
|---|
| 216 | |
|---|
| 217 | /// vector impact parameter (squared) |
|---|
| 218 | #define VEC_IMPACT_SQ(bsq,direction,position) {\ |
|---|
| 219 | GREAL _llel_ = VEC_DOT(direction, position);\ |
|---|
| 220 | bsq = VEC_DOT(position, position) - _llel_*_llel_;\ |
|---|
| 221 | }\ |
|---|
| 222 | |
|---|
| 223 | |
|---|
| 224 | /// vector impact parameter |
|---|
| 225 | #define VEC_IMPACT(bsq,direction,position) {\ |
|---|
| 226 | VEC_IMPACT_SQ(bsq,direction,position); \ |
|---|
| 227 | GIM_SQRT(bsq,bsq); \ |
|---|
| 228 | }\ |
|---|
| 229 | |
|---|
| 230 | /// Vector length |
|---|
| 231 | #define VEC_LENGTH_2(a,l)\ |
|---|
| 232 | {\ |
|---|
| 233 | GREAL _pp = VEC_DOT_2(a,a);\ |
|---|
| 234 | GIM_SQRT(_pp,l);\ |
|---|
| 235 | }\ |
|---|
| 236 | |
|---|
| 237 | |
|---|
| 238 | /// Vector length |
|---|
| 239 | #define VEC_LENGTH(a,l)\ |
|---|
| 240 | {\ |
|---|
| 241 | GREAL _pp = VEC_DOT(a,a);\ |
|---|
| 242 | GIM_SQRT(_pp,l);\ |
|---|
| 243 | }\ |
|---|
| 244 | |
|---|
| 245 | |
|---|
| 246 | /// Vector length |
|---|
| 247 | #define VEC_LENGTH_4(a,l)\ |
|---|
| 248 | {\ |
|---|
| 249 | GREAL _pp = VEC_DOT_4(a,a);\ |
|---|
| 250 | GIM_SQRT(_pp,l);\ |
|---|
| 251 | }\ |
|---|
| 252 | |
|---|
| 253 | /// Vector inv length |
|---|
| 254 | #define VEC_INV_LENGTH_2(a,l)\ |
|---|
| 255 | {\ |
|---|
| 256 | GREAL _pp = VEC_DOT_2(a,a);\ |
|---|
| 257 | GIM_INV_SQRT(_pp,l);\ |
|---|
| 258 | }\ |
|---|
| 259 | |
|---|
| 260 | |
|---|
| 261 | /// Vector inv length |
|---|
| 262 | #define VEC_INV_LENGTH(a,l)\ |
|---|
| 263 | {\ |
|---|
| 264 | GREAL _pp = VEC_DOT(a,a);\ |
|---|
| 265 | GIM_INV_SQRT(_pp,l);\ |
|---|
| 266 | }\ |
|---|
| 267 | |
|---|
| 268 | |
|---|
| 269 | /// Vector inv length |
|---|
| 270 | #define VEC_INV_LENGTH_4(a,l)\ |
|---|
| 271 | {\ |
|---|
| 272 | GREAL _pp = VEC_DOT_4(a,a);\ |
|---|
| 273 | GIM_INV_SQRT(_pp,l);\ |
|---|
| 274 | }\ |
|---|
| 275 | |
|---|
| 276 | |
|---|
| 277 | |
|---|
| 278 | /// distance between two points |
|---|
| 279 | #define VEC_DISTANCE(_len,_va,_vb) {\ |
|---|
| 280 | vec3f _tmp_; \ |
|---|
| 281 | VEC_DIFF(_tmp_, _vb, _va); \ |
|---|
| 282 | VEC_LENGTH(_tmp_,_len); \ |
|---|
| 283 | }\ |
|---|
| 284 | |
|---|
| 285 | |
|---|
| 286 | /// Vector length |
|---|
| 287 | #define VEC_CONJUGATE_LENGTH(a,l)\ |
|---|
| 288 | {\ |
|---|
| 289 | GREAL _pp = 1.0 - a[0]*a[0] - a[1]*a[1] - a[2]*a[2];\ |
|---|
| 290 | GIM_SQRT(_pp,l);\ |
|---|
| 291 | }\ |
|---|
| 292 | |
|---|
| 293 | |
|---|
| 294 | /// Vector length |
|---|
| 295 | #define VEC_NORMALIZE(a) { \ |
|---|
| 296 | GREAL len;\ |
|---|
| 297 | VEC_INV_LENGTH(a,len); \ |
|---|
| 298 | if(len<G_REAL_INFINITY)\ |
|---|
| 299 | {\ |
|---|
| 300 | a[0] *= len; \ |
|---|
| 301 | a[1] *= len; \ |
|---|
| 302 | a[2] *= len; \ |
|---|
| 303 | } \ |
|---|
| 304 | }\ |
|---|
| 305 | |
|---|
| 306 | /// Set Vector size |
|---|
| 307 | #define VEC_RENORMALIZE(a,newlen) { \ |
|---|
| 308 | GREAL len;\ |
|---|
| 309 | VEC_INV_LENGTH(a,len); \ |
|---|
| 310 | if(len<G_REAL_INFINITY)\ |
|---|
| 311 | {\ |
|---|
| 312 | len *= newlen;\ |
|---|
| 313 | a[0] *= len; \ |
|---|
| 314 | a[1] *= len; \ |
|---|
| 315 | a[2] *= len; \ |
|---|
| 316 | } \ |
|---|
| 317 | }\ |
|---|
| 318 | |
|---|
| 319 | /// Vector cross |
|---|
| 320 | #define VEC_CROSS(c,a,b) \ |
|---|
| 321 | { \ |
|---|
| 322 | c[0] = (a)[1] * (b)[2] - (a)[2] * (b)[1]; \ |
|---|
| 323 | c[1] = (a)[2] * (b)[0] - (a)[0] * (b)[2]; \ |
|---|
| 324 | c[2] = (a)[0] * (b)[1] - (a)[1] * (b)[0]; \ |
|---|
| 325 | }\ |
|---|
| 326 | |
|---|
| 327 | |
|---|
| 328 | /*! Vector perp -- assumes that n is of unit length |
|---|
| 329 | * accepts vector v, subtracts out any component parallel to n */ |
|---|
| 330 | #define VEC_PERPENDICULAR(vp,v,n) \ |
|---|
| 331 | { \ |
|---|
| 332 | GREAL dot = VEC_DOT(v, n); \ |
|---|
| 333 | vp[0] = (v)[0] - dot*(n)[0]; \ |
|---|
| 334 | vp[1] = (v)[1] - dot*(n)[1]; \ |
|---|
| 335 | vp[2] = (v)[2] - dot*(n)[2]; \ |
|---|
| 336 | }\ |
|---|
| 337 | |
|---|
| 338 | |
|---|
| 339 | /*! Vector parallel -- assumes that n is of unit length */ |
|---|
| 340 | #define VEC_PARALLEL(vp,v,n) \ |
|---|
| 341 | { \ |
|---|
| 342 | GREAL dot = VEC_DOT(v, n); \ |
|---|
| 343 | vp[0] = (dot) * (n)[0]; \ |
|---|
| 344 | vp[1] = (dot) * (n)[1]; \ |
|---|
| 345 | vp[2] = (dot) * (n)[2]; \ |
|---|
| 346 | }\ |
|---|
| 347 | |
|---|
| 348 | /*! Same as Vector parallel -- n can have any length |
|---|
| 349 | * accepts vector v, subtracts out any component perpendicular to n */ |
|---|
| 350 | #define VEC_PROJECT(vp,v,n) \ |
|---|
| 351 | { \ |
|---|
| 352 | GREAL scalar = VEC_DOT(v, n); \ |
|---|
| 353 | scalar/= VEC_DOT(n, n); \ |
|---|
| 354 | vp[0] = (scalar) * (n)[0]; \ |
|---|
| 355 | vp[1] = (scalar) * (n)[1]; \ |
|---|
| 356 | vp[2] = (scalar) * (n)[2]; \ |
|---|
| 357 | }\ |
|---|
| 358 | |
|---|
| 359 | |
|---|
| 360 | /*! accepts vector v*/ |
|---|
| 361 | #define VEC_UNPROJECT(vp,v,n) \ |
|---|
| 362 | { \ |
|---|
| 363 | GREAL scalar = VEC_DOT(v, n); \ |
|---|
| 364 | scalar = VEC_DOT(n, n)/scalar; \ |
|---|
| 365 | vp[0] = (scalar) * (n)[0]; \ |
|---|
| 366 | vp[1] = (scalar) * (n)[1]; \ |
|---|
| 367 | vp[2] = (scalar) * (n)[2]; \ |
|---|
| 368 | }\ |
|---|
| 369 | |
|---|
| 370 | |
|---|
| 371 | /*! Vector reflection -- assumes n is of unit length |
|---|
| 372 | Takes vector v, reflects it against reflector n, and returns vr */ |
|---|
| 373 | #define VEC_REFLECT(vr,v,n) \ |
|---|
| 374 | { \ |
|---|
| 375 | GREAL dot = VEC_DOT(v, n); \ |
|---|
| 376 | vr[0] = (v)[0] - 2.0 * (dot) * (n)[0]; \ |
|---|
| 377 | vr[1] = (v)[1] - 2.0 * (dot) * (n)[1]; \ |
|---|
| 378 | vr[2] = (v)[2] - 2.0 * (dot) * (n)[2]; \ |
|---|
| 379 | }\ |
|---|
| 380 | |
|---|
| 381 | |
|---|
| 382 | /*! Vector blending |
|---|
| 383 | Takes two vectors a, b, blends them together with two scalars */ |
|---|
| 384 | #define VEC_BLEND_AB(vr,sa,a,sb,b) \ |
|---|
| 385 | { \ |
|---|
| 386 | vr[0] = (sa) * (a)[0] + (sb) * (b)[0]; \ |
|---|
| 387 | vr[1] = (sa) * (a)[1] + (sb) * (b)[1]; \ |
|---|
| 388 | vr[2] = (sa) * (a)[2] + (sb) * (b)[2]; \ |
|---|
| 389 | }\ |
|---|
| 390 | |
|---|
| 391 | /*! Vector blending |
|---|
| 392 | Takes two vectors a, b, blends them together with s <=1 */ |
|---|
| 393 | #define VEC_BLEND(vr,a,b,s) VEC_BLEND_AB(vr,(1-s),a,s,b) |
|---|
| 394 | |
|---|
| 395 | #define VEC_SET3(a,b,op,c) a[0]=b[0] op c[0]; a[1]=b[1] op c[1]; a[2]=b[2] op c[2]; |
|---|
| 396 | |
|---|
| 397 | //! Finds the bigger cartesian coordinate from a vector |
|---|
| 398 | #define VEC_MAYOR_COORD(vec, maxc)\ |
|---|
| 399 | {\ |
|---|
| 400 | GREAL A[] = {fabs(vec[0]),fabs(vec[1]),fabs(vec[2])};\ |
|---|
| 401 | maxc = A[0]>A[1]?(A[0]>A[2]?0:2):(A[1]>A[2]?1:2);\ |
|---|
| 402 | }\ |
|---|
| 403 | |
|---|
| 404 | //! Finds the 2 smallest cartesian coordinates from a vector |
|---|
| 405 | #define VEC_MINOR_AXES(vec, i0, i1)\ |
|---|
| 406 | {\ |
|---|
| 407 | VEC_MAYOR_COORD(vec,i0);\ |
|---|
| 408 | i0 = (i0+1)%3;\ |
|---|
| 409 | i1 = (i0+1)%3;\ |
|---|
| 410 | }\ |
|---|
| 411 | |
|---|
| 412 | |
|---|
| 413 | |
|---|
| 414 | |
|---|
| 415 | #define VEC_EQUAL(v1,v2) (v1[0]==v2[0]&&v1[1]==v2[1]&&v1[2]==v2[2]) |
|---|
| 416 | |
|---|
| 417 | #define VEC_NEAR_EQUAL(v1,v2) (GIM_NEAR_EQUAL(v1[0],v2[0])&&GIM_NEAR_EQUAL(v1[1],v2[1])&&GIM_NEAR_EQUAL(v1[2],v2[2])) |
|---|
| 418 | |
|---|
| 419 | |
|---|
| 420 | /// Vector cross |
|---|
| 421 | #define X_AXIS_CROSS_VEC(dst,src)\ |
|---|
| 422 | { \ |
|---|
| 423 | dst[0] = 0.0f; \ |
|---|
| 424 | dst[1] = -src[2]; \ |
|---|
| 425 | dst[2] = src[1]; \ |
|---|
| 426 | }\ |
|---|
| 427 | |
|---|
| 428 | #define Y_AXIS_CROSS_VEC(dst,src)\ |
|---|
| 429 | { \ |
|---|
| 430 | dst[0] = src[2]; \ |
|---|
| 431 | dst[1] = 0.0f; \ |
|---|
| 432 | dst[2] = -src[0]; \ |
|---|
| 433 | }\ |
|---|
| 434 | |
|---|
| 435 | #define Z_AXIS_CROSS_VEC(dst,src)\ |
|---|
| 436 | { \ |
|---|
| 437 | dst[0] = -src[1]; \ |
|---|
| 438 | dst[1] = src[0]; \ |
|---|
| 439 | dst[2] = 0.0f; \ |
|---|
| 440 | }\ |
|---|
| 441 | |
|---|
| 442 | |
|---|
| 443 | |
|---|
| 444 | |
|---|
| 445 | |
|---|
| 446 | |
|---|
| 447 | /// initialize matrix |
|---|
| 448 | #define IDENTIFY_MATRIX_3X3(m) \ |
|---|
| 449 | { \ |
|---|
| 450 | m[0][0] = 1.0; \ |
|---|
| 451 | m[0][1] = 0.0; \ |
|---|
| 452 | m[0][2] = 0.0; \ |
|---|
| 453 | \ |
|---|
| 454 | m[1][0] = 0.0; \ |
|---|
| 455 | m[1][1] = 1.0; \ |
|---|
| 456 | m[1][2] = 0.0; \ |
|---|
| 457 | \ |
|---|
| 458 | m[2][0] = 0.0; \ |
|---|
| 459 | m[2][1] = 0.0; \ |
|---|
| 460 | m[2][2] = 1.0; \ |
|---|
| 461 | }\ |
|---|
| 462 | |
|---|
| 463 | /*! initialize matrix */ |
|---|
| 464 | #define IDENTIFY_MATRIX_4X4(m) \ |
|---|
| 465 | { \ |
|---|
| 466 | m[0][0] = 1.0; \ |
|---|
| 467 | m[0][1] = 0.0; \ |
|---|
| 468 | m[0][2] = 0.0; \ |
|---|
| 469 | m[0][3] = 0.0; \ |
|---|
| 470 | \ |
|---|
| 471 | m[1][0] = 0.0; \ |
|---|
| 472 | m[1][1] = 1.0; \ |
|---|
| 473 | m[1][2] = 0.0; \ |
|---|
| 474 | m[1][3] = 0.0; \ |
|---|
| 475 | \ |
|---|
| 476 | m[2][0] = 0.0; \ |
|---|
| 477 | m[2][1] = 0.0; \ |
|---|
| 478 | m[2][2] = 1.0; \ |
|---|
| 479 | m[2][3] = 0.0; \ |
|---|
| 480 | \ |
|---|
| 481 | m[3][0] = 0.0; \ |
|---|
| 482 | m[3][1] = 0.0; \ |
|---|
| 483 | m[3][2] = 0.0; \ |
|---|
| 484 | m[3][3] = 1.0; \ |
|---|
| 485 | }\ |
|---|
| 486 | |
|---|
| 487 | /*! initialize matrix */ |
|---|
| 488 | #define ZERO_MATRIX_4X4(m) \ |
|---|
| 489 | { \ |
|---|
| 490 | m[0][0] = 0.0; \ |
|---|
| 491 | m[0][1] = 0.0; \ |
|---|
| 492 | m[0][2] = 0.0; \ |
|---|
| 493 | m[0][3] = 0.0; \ |
|---|
| 494 | \ |
|---|
| 495 | m[1][0] = 0.0; \ |
|---|
| 496 | m[1][1] = 0.0; \ |
|---|
| 497 | m[1][2] = 0.0; \ |
|---|
| 498 | m[1][3] = 0.0; \ |
|---|
| 499 | \ |
|---|
| 500 | m[2][0] = 0.0; \ |
|---|
| 501 | m[2][1] = 0.0; \ |
|---|
| 502 | m[2][2] = 0.0; \ |
|---|
| 503 | m[2][3] = 0.0; \ |
|---|
| 504 | \ |
|---|
| 505 | m[3][0] = 0.0; \ |
|---|
| 506 | m[3][1] = 0.0; \ |
|---|
| 507 | m[3][2] = 0.0; \ |
|---|
| 508 | m[3][3] = 0.0; \ |
|---|
| 509 | }\ |
|---|
| 510 | |
|---|
| 511 | /*! matrix rotation X */ |
|---|
| 512 | #define ROTX_CS(m,cosine,sine) \ |
|---|
| 513 | { \ |
|---|
| 514 | /* rotation about the x-axis */ \ |
|---|
| 515 | \ |
|---|
| 516 | m[0][0] = 1.0; \ |
|---|
| 517 | m[0][1] = 0.0; \ |
|---|
| 518 | m[0][2] = 0.0; \ |
|---|
| 519 | m[0][3] = 0.0; \ |
|---|
| 520 | \ |
|---|
| 521 | m[1][0] = 0.0; \ |
|---|
| 522 | m[1][1] = (cosine); \ |
|---|
| 523 | m[1][2] = (sine); \ |
|---|
| 524 | m[1][3] = 0.0; \ |
|---|
| 525 | \ |
|---|
| 526 | m[2][0] = 0.0; \ |
|---|
| 527 | m[2][1] = -(sine); \ |
|---|
| 528 | m[2][2] = (cosine); \ |
|---|
| 529 | m[2][3] = 0.0; \ |
|---|
| 530 | \ |
|---|
| 531 | m[3][0] = 0.0; \ |
|---|
| 532 | m[3][1] = 0.0; \ |
|---|
| 533 | m[3][2] = 0.0; \ |
|---|
| 534 | m[3][3] = 1.0; \ |
|---|
| 535 | }\ |
|---|
| 536 | |
|---|
| 537 | /*! matrix rotation Y */ |
|---|
| 538 | #define ROTY_CS(m,cosine,sine) \ |
|---|
| 539 | { \ |
|---|
| 540 | /* rotation about the y-axis */ \ |
|---|
| 541 | \ |
|---|
| 542 | m[0][0] = (cosine); \ |
|---|
| 543 | m[0][1] = 0.0; \ |
|---|
| 544 | m[0][2] = -(sine); \ |
|---|
| 545 | m[0][3] = 0.0; \ |
|---|
| 546 | \ |
|---|
| 547 | m[1][0] = 0.0; \ |
|---|
| 548 | m[1][1] = 1.0; \ |
|---|
| 549 | m[1][2] = 0.0; \ |
|---|
| 550 | m[1][3] = 0.0; \ |
|---|
| 551 | \ |
|---|
| 552 | m[2][0] = (sine); \ |
|---|
| 553 | m[2][1] = 0.0; \ |
|---|
| 554 | m[2][2] = (cosine); \ |
|---|
| 555 | m[2][3] = 0.0; \ |
|---|
| 556 | \ |
|---|
| 557 | m[3][0] = 0.0; \ |
|---|
| 558 | m[3][1] = 0.0; \ |
|---|
| 559 | m[3][2] = 0.0; \ |
|---|
| 560 | m[3][3] = 1.0; \ |
|---|
| 561 | }\ |
|---|
| 562 | |
|---|
| 563 | /*! matrix rotation Z */ |
|---|
| 564 | #define ROTZ_CS(m,cosine,sine) \ |
|---|
| 565 | { \ |
|---|
| 566 | /* rotation about the z-axis */ \ |
|---|
| 567 | \ |
|---|
| 568 | m[0][0] = (cosine); \ |
|---|
| 569 | m[0][1] = (sine); \ |
|---|
| 570 | m[0][2] = 0.0; \ |
|---|
| 571 | m[0][3] = 0.0; \ |
|---|
| 572 | \ |
|---|
| 573 | m[1][0] = -(sine); \ |
|---|
| 574 | m[1][1] = (cosine); \ |
|---|
| 575 | m[1][2] = 0.0; \ |
|---|
| 576 | m[1][3] = 0.0; \ |
|---|
| 577 | \ |
|---|
| 578 | m[2][0] = 0.0; \ |
|---|
| 579 | m[2][1] = 0.0; \ |
|---|
| 580 | m[2][2] = 1.0; \ |
|---|
| 581 | m[2][3] = 0.0; \ |
|---|
| 582 | \ |
|---|
| 583 | m[3][0] = 0.0; \ |
|---|
| 584 | m[3][1] = 0.0; \ |
|---|
| 585 | m[3][2] = 0.0; \ |
|---|
| 586 | m[3][3] = 1.0; \ |
|---|
| 587 | }\ |
|---|
| 588 | |
|---|
| 589 | /*! matrix copy */ |
|---|
| 590 | #define COPY_MATRIX_2X2(b,a) \ |
|---|
| 591 | { \ |
|---|
| 592 | b[0][0] = a[0][0]; \ |
|---|
| 593 | b[0][1] = a[0][1]; \ |
|---|
| 594 | \ |
|---|
| 595 | b[1][0] = a[1][0]; \ |
|---|
| 596 | b[1][1] = a[1][1]; \ |
|---|
| 597 | \ |
|---|
| 598 | }\ |
|---|
| 599 | |
|---|
| 600 | |
|---|
| 601 | /*! matrix copy */ |
|---|
| 602 | #define COPY_MATRIX_2X3(b,a) \ |
|---|
| 603 | { \ |
|---|
| 604 | b[0][0] = a[0][0]; \ |
|---|
| 605 | b[0][1] = a[0][1]; \ |
|---|
| 606 | b[0][2] = a[0][2]; \ |
|---|
| 607 | \ |
|---|
| 608 | b[1][0] = a[1][0]; \ |
|---|
| 609 | b[1][1] = a[1][1]; \ |
|---|
| 610 | b[1][2] = a[1][2]; \ |
|---|
| 611 | }\ |
|---|
| 612 | |
|---|
| 613 | |
|---|
| 614 | /*! matrix copy */ |
|---|
| 615 | #define COPY_MATRIX_3X3(b,a) \ |
|---|
| 616 | { \ |
|---|
| 617 | b[0][0] = a[0][0]; \ |
|---|
| 618 | b[0][1] = a[0][1]; \ |
|---|
| 619 | b[0][2] = a[0][2]; \ |
|---|
| 620 | \ |
|---|
| 621 | b[1][0] = a[1][0]; \ |
|---|
| 622 | b[1][1] = a[1][1]; \ |
|---|
| 623 | b[1][2] = a[1][2]; \ |
|---|
| 624 | \ |
|---|
| 625 | b[2][0] = a[2][0]; \ |
|---|
| 626 | b[2][1] = a[2][1]; \ |
|---|
| 627 | b[2][2] = a[2][2]; \ |
|---|
| 628 | }\ |
|---|
| 629 | |
|---|
| 630 | |
|---|
| 631 | /*! matrix copy */ |
|---|
| 632 | #define COPY_MATRIX_4X4(b,a) \ |
|---|
| 633 | { \ |
|---|
| 634 | b[0][0] = a[0][0]; \ |
|---|
| 635 | b[0][1] = a[0][1]; \ |
|---|
| 636 | b[0][2] = a[0][2]; \ |
|---|
| 637 | b[0][3] = a[0][3]; \ |
|---|
| 638 | \ |
|---|
| 639 | b[1][0] = a[1][0]; \ |
|---|
| 640 | b[1][1] = a[1][1]; \ |
|---|
| 641 | b[1][2] = a[1][2]; \ |
|---|
| 642 | b[1][3] = a[1][3]; \ |
|---|
| 643 | \ |
|---|
| 644 | b[2][0] = a[2][0]; \ |
|---|
| 645 | b[2][1] = a[2][1]; \ |
|---|
| 646 | b[2][2] = a[2][2]; \ |
|---|
| 647 | b[2][3] = a[2][3]; \ |
|---|
| 648 | \ |
|---|
| 649 | b[3][0] = a[3][0]; \ |
|---|
| 650 | b[3][1] = a[3][1]; \ |
|---|
| 651 | b[3][2] = a[3][2]; \ |
|---|
| 652 | b[3][3] = a[3][3]; \ |
|---|
| 653 | }\ |
|---|
| 654 | |
|---|
| 655 | |
|---|
| 656 | /*! matrix transpose */ |
|---|
| 657 | #define TRANSPOSE_MATRIX_2X2(b,a) \ |
|---|
| 658 | { \ |
|---|
| 659 | b[0][0] = a[0][0]; \ |
|---|
| 660 | b[0][1] = a[1][0]; \ |
|---|
| 661 | \ |
|---|
| 662 | b[1][0] = a[0][1]; \ |
|---|
| 663 | b[1][1] = a[1][1]; \ |
|---|
| 664 | }\ |
|---|
| 665 | |
|---|
| 666 | |
|---|
| 667 | /*! matrix transpose */ |
|---|
| 668 | #define TRANSPOSE_MATRIX_3X3(b,a) \ |
|---|
| 669 | { \ |
|---|
| 670 | b[0][0] = a[0][0]; \ |
|---|
| 671 | b[0][1] = a[1][0]; \ |
|---|
| 672 | b[0][2] = a[2][0]; \ |
|---|
| 673 | \ |
|---|
| 674 | b[1][0] = a[0][1]; \ |
|---|
| 675 | b[1][1] = a[1][1]; \ |
|---|
| 676 | b[1][2] = a[2][1]; \ |
|---|
| 677 | \ |
|---|
| 678 | b[2][0] = a[0][2]; \ |
|---|
| 679 | b[2][1] = a[1][2]; \ |
|---|
| 680 | b[2][2] = a[2][2]; \ |
|---|
| 681 | }\ |
|---|
| 682 | |
|---|
| 683 | |
|---|
| 684 | /*! matrix transpose */ |
|---|
| 685 | #define TRANSPOSE_MATRIX_4X4(b,a) \ |
|---|
| 686 | { \ |
|---|
| 687 | b[0][0] = a[0][0]; \ |
|---|
| 688 | b[0][1] = a[1][0]; \ |
|---|
| 689 | b[0][2] = a[2][0]; \ |
|---|
| 690 | b[0][3] = a[3][0]; \ |
|---|
| 691 | \ |
|---|
| 692 | b[1][0] = a[0][1]; \ |
|---|
| 693 | b[1][1] = a[1][1]; \ |
|---|
| 694 | b[1][2] = a[2][1]; \ |
|---|
| 695 | b[1][3] = a[3][1]; \ |
|---|
| 696 | \ |
|---|
| 697 | b[2][0] = a[0][2]; \ |
|---|
| 698 | b[2][1] = a[1][2]; \ |
|---|
| 699 | b[2][2] = a[2][2]; \ |
|---|
| 700 | b[2][3] = a[3][2]; \ |
|---|
| 701 | \ |
|---|
| 702 | b[3][0] = a[0][3]; \ |
|---|
| 703 | b[3][1] = a[1][3]; \ |
|---|
| 704 | b[3][2] = a[2][3]; \ |
|---|
| 705 | b[3][3] = a[3][3]; \ |
|---|
| 706 | }\ |
|---|
| 707 | |
|---|
| 708 | |
|---|
| 709 | /*! multiply matrix by scalar */ |
|---|
| 710 | #define SCALE_MATRIX_2X2(b,s,a) \ |
|---|
| 711 | { \ |
|---|
| 712 | b[0][0] = (s) * a[0][0]; \ |
|---|
| 713 | b[0][1] = (s) * a[0][1]; \ |
|---|
| 714 | \ |
|---|
| 715 | b[1][0] = (s) * a[1][0]; \ |
|---|
| 716 | b[1][1] = (s) * a[1][1]; \ |
|---|
| 717 | }\ |
|---|
| 718 | |
|---|
| 719 | |
|---|
| 720 | /*! multiply matrix by scalar */ |
|---|
| 721 | #define SCALE_MATRIX_3X3(b,s,a) \ |
|---|
| 722 | { \ |
|---|
| 723 | b[0][0] = (s) * a[0][0]; \ |
|---|
| 724 | b[0][1] = (s) * a[0][1]; \ |
|---|
| 725 | b[0][2] = (s) * a[0][2]; \ |
|---|
| 726 | \ |
|---|
| 727 | b[1][0] = (s) * a[1][0]; \ |
|---|
| 728 | b[1][1] = (s) * a[1][1]; \ |
|---|
| 729 | b[1][2] = (s) * a[1][2]; \ |
|---|
| 730 | \ |
|---|
| 731 | b[2][0] = (s) * a[2][0]; \ |
|---|
| 732 | b[2][1] = (s) * a[2][1]; \ |
|---|
| 733 | b[2][2] = (s) * a[2][2]; \ |
|---|
| 734 | }\ |
|---|
| 735 | |
|---|
| 736 | |
|---|
| 737 | /*! multiply matrix by scalar */ |
|---|
| 738 | #define SCALE_MATRIX_4X4(b,s,a) \ |
|---|
| 739 | { \ |
|---|
| 740 | b[0][0] = (s) * a[0][0]; \ |
|---|
| 741 | b[0][1] = (s) * a[0][1]; \ |
|---|
| 742 | b[0][2] = (s) * a[0][2]; \ |
|---|
| 743 | b[0][3] = (s) * a[0][3]; \ |
|---|
| 744 | \ |
|---|
| 745 | b[1][0] = (s) * a[1][0]; \ |
|---|
| 746 | b[1][1] = (s) * a[1][1]; \ |
|---|
| 747 | b[1][2] = (s) * a[1][2]; \ |
|---|
| 748 | b[1][3] = (s) * a[1][3]; \ |
|---|
| 749 | \ |
|---|
| 750 | b[2][0] = (s) * a[2][0]; \ |
|---|
| 751 | b[2][1] = (s) * a[2][1]; \ |
|---|
| 752 | b[2][2] = (s) * a[2][2]; \ |
|---|
| 753 | b[2][3] = (s) * a[2][3]; \ |
|---|
| 754 | \ |
|---|
| 755 | b[3][0] = s * a[3][0]; \ |
|---|
| 756 | b[3][1] = s * a[3][1]; \ |
|---|
| 757 | b[3][2] = s * a[3][2]; \ |
|---|
| 758 | b[3][3] = s * a[3][3]; \ |
|---|
| 759 | }\ |
|---|
| 760 | |
|---|
| 761 | |
|---|
| 762 | /*! multiply matrix by scalar */ |
|---|
| 763 | #define SCALE_VEC_MATRIX_2X2(b,svec,a) \ |
|---|
| 764 | { \ |
|---|
| 765 | b[0][0] = svec[0] * a[0][0]; \ |
|---|
| 766 | b[1][0] = svec[0] * a[1][0]; \ |
|---|
| 767 | \ |
|---|
| 768 | b[0][1] = svec[1] * a[0][1]; \ |
|---|
| 769 | b[1][1] = svec[1] * a[1][1]; \ |
|---|
| 770 | }\ |
|---|
| 771 | |
|---|
| 772 | |
|---|
| 773 | /*! multiply matrix by scalar. Each columns is scaled by each scalar vector component */ |
|---|
| 774 | #define SCALE_VEC_MATRIX_3X3(b,svec,a) \ |
|---|
| 775 | { \ |
|---|
| 776 | b[0][0] = svec[0] * a[0][0]; \ |
|---|
| 777 | b[1][0] = svec[0] * a[1][0]; \ |
|---|
| 778 | b[2][0] = svec[0] * a[2][0]; \ |
|---|
| 779 | \ |
|---|
| 780 | b[0][1] = svec[1] * a[0][1]; \ |
|---|
| 781 | b[1][1] = svec[1] * a[1][1]; \ |
|---|
| 782 | b[2][1] = svec[1] * a[2][1]; \ |
|---|
| 783 | \ |
|---|
| 784 | b[0][2] = svec[2] * a[0][2]; \ |
|---|
| 785 | b[1][2] = svec[2] * a[1][2]; \ |
|---|
| 786 | b[2][2] = svec[2] * a[2][2]; \ |
|---|
| 787 | }\ |
|---|
| 788 | |
|---|
| 789 | |
|---|
| 790 | /*! multiply matrix by scalar */ |
|---|
| 791 | #define SCALE_VEC_MATRIX_4X4(b,svec,a) \ |
|---|
| 792 | { \ |
|---|
| 793 | b[0][0] = svec[0] * a[0][0]; \ |
|---|
| 794 | b[1][0] = svec[0] * a[1][0]; \ |
|---|
| 795 | b[2][0] = svec[0] * a[2][0]; \ |
|---|
| 796 | b[3][0] = svec[0] * a[3][0]; \ |
|---|
| 797 | \ |
|---|
| 798 | b[0][1] = svec[1] * a[0][1]; \ |
|---|
| 799 | b[1][1] = svec[1] * a[1][1]; \ |
|---|
| 800 | b[2][1] = svec[1] * a[2][1]; \ |
|---|
| 801 | b[3][1] = svec[1] * a[3][1]; \ |
|---|
| 802 | \ |
|---|
| 803 | b[0][2] = svec[2] * a[0][2]; \ |
|---|
| 804 | b[1][2] = svec[2] * a[1][2]; \ |
|---|
| 805 | b[2][2] = svec[2] * a[2][2]; \ |
|---|
| 806 | b[3][2] = svec[2] * a[3][2]; \ |
|---|
| 807 | \ |
|---|
| 808 | b[0][3] = svec[3] * a[0][3]; \ |
|---|
| 809 | b[1][3] = svec[3] * a[1][3]; \ |
|---|
| 810 | b[2][3] = svec[3] * a[2][3]; \ |
|---|
| 811 | b[3][3] = svec[3] * a[3][3]; \ |
|---|
| 812 | }\ |
|---|
| 813 | |
|---|
| 814 | |
|---|
| 815 | /*! multiply matrix by scalar */ |
|---|
| 816 | #define ACCUM_SCALE_MATRIX_2X2(b,s,a) \ |
|---|
| 817 | { \ |
|---|
| 818 | b[0][0] += (s) * a[0][0]; \ |
|---|
| 819 | b[0][1] += (s) * a[0][1]; \ |
|---|
| 820 | \ |
|---|
| 821 | b[1][0] += (s) * a[1][0]; \ |
|---|
| 822 | b[1][1] += (s) * a[1][1]; \ |
|---|
| 823 | }\ |
|---|
| 824 | |
|---|
| 825 | |
|---|
| 826 | /*! multiply matrix by scalar */ |
|---|
| 827 | #define ACCUM_SCALE_MATRIX_3X3(b,s,a) \ |
|---|
| 828 | { \ |
|---|
| 829 | b[0][0] += (s) * a[0][0]; \ |
|---|
| 830 | b[0][1] += (s) * a[0][1]; \ |
|---|
| 831 | b[0][2] += (s) * a[0][2]; \ |
|---|
| 832 | \ |
|---|
| 833 | b[1][0] += (s) * a[1][0]; \ |
|---|
| 834 | b[1][1] += (s) * a[1][1]; \ |
|---|
| 835 | b[1][2] += (s) * a[1][2]; \ |
|---|
| 836 | \ |
|---|
| 837 | b[2][0] += (s) * a[2][0]; \ |
|---|
| 838 | b[2][1] += (s) * a[2][1]; \ |
|---|
| 839 | b[2][2] += (s) * a[2][2]; \ |
|---|
| 840 | }\ |
|---|
| 841 | |
|---|
| 842 | |
|---|
| 843 | /*! multiply matrix by scalar */ |
|---|
| 844 | #define ACCUM_SCALE_MATRIX_4X4(b,s,a) \ |
|---|
| 845 | { \ |
|---|
| 846 | b[0][0] += (s) * a[0][0]; \ |
|---|
| 847 | b[0][1] += (s) * a[0][1]; \ |
|---|
| 848 | b[0][2] += (s) * a[0][2]; \ |
|---|
| 849 | b[0][3] += (s) * a[0][3]; \ |
|---|
| 850 | \ |
|---|
| 851 | b[1][0] += (s) * a[1][0]; \ |
|---|
| 852 | b[1][1] += (s) * a[1][1]; \ |
|---|
| 853 | b[1][2] += (s) * a[1][2]; \ |
|---|
| 854 | b[1][3] += (s) * a[1][3]; \ |
|---|
| 855 | \ |
|---|
| 856 | b[2][0] += (s) * a[2][0]; \ |
|---|
| 857 | b[2][1] += (s) * a[2][1]; \ |
|---|
| 858 | b[2][2] += (s) * a[2][2]; \ |
|---|
| 859 | b[2][3] += (s) * a[2][3]; \ |
|---|
| 860 | \ |
|---|
| 861 | b[3][0] += (s) * a[3][0]; \ |
|---|
| 862 | b[3][1] += (s) * a[3][1]; \ |
|---|
| 863 | b[3][2] += (s) * a[3][2]; \ |
|---|
| 864 | b[3][3] += (s) * a[3][3]; \ |
|---|
| 865 | }\ |
|---|
| 866 | |
|---|
| 867 | /*! matrix product */ |
|---|
| 868 | /*! c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y];*/ |
|---|
| 869 | #define MATRIX_PRODUCT_2X2(c,a,b) \ |
|---|
| 870 | { \ |
|---|
| 871 | c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0]; \ |
|---|
| 872 | c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1]; \ |
|---|
| 873 | \ |
|---|
| 874 | c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0]; \ |
|---|
| 875 | c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1]; \ |
|---|
| 876 | \ |
|---|
| 877 | }\ |
|---|
| 878 | |
|---|
| 879 | /*! matrix product */ |
|---|
| 880 | /*! c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y];*/ |
|---|
| 881 | #define MATRIX_PRODUCT_3X3(c,a,b) \ |
|---|
| 882 | { \ |
|---|
| 883 | c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0]+a[0][2]*b[2][0]; \ |
|---|
| 884 | c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1]+a[0][2]*b[2][1]; \ |
|---|
| 885 | c[0][2] = a[0][0]*b[0][2]+a[0][1]*b[1][2]+a[0][2]*b[2][2]; \ |
|---|
| 886 | \ |
|---|
| 887 | c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0]+a[1][2]*b[2][0]; \ |
|---|
| 888 | c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1]+a[1][2]*b[2][1]; \ |
|---|
| 889 | c[1][2] = a[1][0]*b[0][2]+a[1][1]*b[1][2]+a[1][2]*b[2][2]; \ |
|---|
| 890 | \ |
|---|
| 891 | c[2][0] = a[2][0]*b[0][0]+a[2][1]*b[1][0]+a[2][2]*b[2][0]; \ |
|---|
| 892 | c[2][1] = a[2][0]*b[0][1]+a[2][1]*b[1][1]+a[2][2]*b[2][1]; \ |
|---|
| 893 | c[2][2] = a[2][0]*b[0][2]+a[2][1]*b[1][2]+a[2][2]*b[2][2]; \ |
|---|
| 894 | }\ |
|---|
| 895 | |
|---|
| 896 | |
|---|
| 897 | /*! matrix product */ |
|---|
| 898 | /*! c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y];*/ |
|---|
| 899 | #define MATRIX_PRODUCT_4X4(c,a,b) \ |
|---|
| 900 | { \ |
|---|
| 901 | c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0]+a[0][2]*b[2][0]+a[0][3]*b[3][0];\ |
|---|
| 902 | c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1]+a[0][2]*b[2][1]+a[0][3]*b[3][1];\ |
|---|
| 903 | c[0][2] = a[0][0]*b[0][2]+a[0][1]*b[1][2]+a[0][2]*b[2][2]+a[0][3]*b[3][2];\ |
|---|
| 904 | c[0][3] = a[0][0]*b[0][3]+a[0][1]*b[1][3]+a[0][2]*b[2][3]+a[0][3]*b[3][3];\ |
|---|
| 905 | \ |
|---|
| 906 | c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0]+a[1][2]*b[2][0]+a[1][3]*b[3][0];\ |
|---|
| 907 | c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1]+a[1][2]*b[2][1]+a[1][3]*b[3][1];\ |
|---|
| 908 | c[1][2] = a[1][0]*b[0][2]+a[1][1]*b[1][2]+a[1][2]*b[2][2]+a[1][3]*b[3][2];\ |
|---|
| 909 | c[1][3] = a[1][0]*b[0][3]+a[1][1]*b[1][3]+a[1][2]*b[2][3]+a[1][3]*b[3][3];\ |
|---|
| 910 | \ |
|---|
| 911 | c[2][0] = a[2][0]*b[0][0]+a[2][1]*b[1][0]+a[2][2]*b[2][0]+a[2][3]*b[3][0];\ |
|---|
| 912 | c[2][1] = a[2][0]*b[0][1]+a[2][1]*b[1][1]+a[2][2]*b[2][1]+a[2][3]*b[3][1];\ |
|---|
| 913 | c[2][2] = a[2][0]*b[0][2]+a[2][1]*b[1][2]+a[2][2]*b[2][2]+a[2][3]*b[3][2];\ |
|---|
| 914 | c[2][3] = a[2][0]*b[0][3]+a[2][1]*b[1][3]+a[2][2]*b[2][3]+a[2][3]*b[3][3];\ |
|---|
| 915 | \ |
|---|
| 916 | c[3][0] = a[3][0]*b[0][0]+a[3][1]*b[1][0]+a[3][2]*b[2][0]+a[3][3]*b[3][0];\ |
|---|
| 917 | c[3][1] = a[3][0]*b[0][1]+a[3][1]*b[1][1]+a[3][2]*b[2][1]+a[3][3]*b[3][1];\ |
|---|
| 918 | c[3][2] = a[3][0]*b[0][2]+a[3][1]*b[1][2]+a[3][2]*b[2][2]+a[3][3]*b[3][2];\ |
|---|
| 919 | c[3][3] = a[3][0]*b[0][3]+a[3][1]*b[1][3]+a[3][2]*b[2][3]+a[3][3]*b[3][3];\ |
|---|
| 920 | }\ |
|---|
| 921 | |
|---|
| 922 | |
|---|
| 923 | /*! matrix times vector */ |
|---|
| 924 | #define MAT_DOT_VEC_2X2(p,m,v) \ |
|---|
| 925 | { \ |
|---|
| 926 | p[0] = m[0][0]*v[0] + m[0][1]*v[1]; \ |
|---|
| 927 | p[1] = m[1][0]*v[0] + m[1][1]*v[1]; \ |
|---|
| 928 | }\ |
|---|
| 929 | |
|---|
| 930 | |
|---|
| 931 | /*! matrix times vector */ |
|---|
| 932 | #define MAT_DOT_VEC_3X3(p,m,v) \ |
|---|
| 933 | { \ |
|---|
| 934 | p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2]; \ |
|---|
| 935 | p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2]; \ |
|---|
| 936 | p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2]; \ |
|---|
| 937 | }\ |
|---|
| 938 | |
|---|
| 939 | |
|---|
| 940 | /*! matrix times vector |
|---|
| 941 | v is a vec4f |
|---|
| 942 | */ |
|---|
| 943 | #define MAT_DOT_VEC_4X4(p,m,v) \ |
|---|
| 944 | { \ |
|---|
| 945 | p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2] + m[0][3]*v[3]; \ |
|---|
| 946 | p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2] + m[1][3]*v[3]; \ |
|---|
| 947 | p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2] + m[2][3]*v[3]; \ |
|---|
| 948 | p[3] = m[3][0]*v[0] + m[3][1]*v[1] + m[3][2]*v[2] + m[3][3]*v[3]; \ |
|---|
| 949 | }\ |
|---|
| 950 | |
|---|
| 951 | /*! matrix times vector |
|---|
| 952 | v is a vec3f |
|---|
| 953 | and m is a mat4f<br> |
|---|
| 954 | Last column is added as the position |
|---|
| 955 | */ |
|---|
| 956 | #define MAT_DOT_VEC_3X4(p,m,v) \ |
|---|
| 957 | { \ |
|---|
| 958 | p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2] + m[0][3]; \ |
|---|
| 959 | p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2] + m[1][3]; \ |
|---|
| 960 | p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2] + m[2][3]; \ |
|---|
| 961 | }\ |
|---|
| 962 | |
|---|
| 963 | |
|---|
| 964 | /*! vector transpose times matrix */ |
|---|
| 965 | /*! p[j] = v[0]*m[0][j] + v[1]*m[1][j] + v[2]*m[2][j]; */ |
|---|
| 966 | #define VEC_DOT_MAT_3X3(p,v,m) \ |
|---|
| 967 | { \ |
|---|
| 968 | p[0] = v[0]*m[0][0] + v[1]*m[1][0] + v[2]*m[2][0]; \ |
|---|
| 969 | p[1] = v[0]*m[0][1] + v[1]*m[1][1] + v[2]*m[2][1]; \ |
|---|
| 970 | p[2] = v[0]*m[0][2] + v[1]*m[1][2] + v[2]*m[2][2]; \ |
|---|
| 971 | }\ |
|---|
| 972 | |
|---|
| 973 | |
|---|
| 974 | /*! affine matrix times vector */ |
|---|
| 975 | /** The matrix is assumed to be an affine matrix, with last two |
|---|
| 976 | * entries representing a translation */ |
|---|
| 977 | #define MAT_DOT_VEC_2X3(p,m,v) \ |
|---|
| 978 | { \ |
|---|
| 979 | p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]; \ |
|---|
| 980 | p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]; \ |
|---|
| 981 | }\ |
|---|
| 982 | |
|---|
| 983 | //! Transform a plane |
|---|
| 984 | #define MAT_TRANSFORM_PLANE_4X4(pout,m,plane)\ |
|---|
| 985 | { \ |
|---|
| 986 | pout[0] = m[0][0]*plane[0] + m[0][1]*plane[1] + m[0][2]*plane[2];\ |
|---|
| 987 | pout[1] = m[1][0]*plane[0] + m[1][1]*plane[1] + m[1][2]*plane[2];\ |
|---|
| 988 | pout[2] = m[2][0]*plane[0] + m[2][1]*plane[1] + m[2][2]*plane[2];\ |
|---|
| 989 | pout[3] = m[0][3]*pout[0] + m[1][3]*pout[1] + m[2][3]*pout[2] + plane[3];\ |
|---|
| 990 | }\ |
|---|
| 991 | |
|---|
| 992 | |
|---|
| 993 | |
|---|
| 994 | /** inverse transpose of matrix times vector |
|---|
| 995 | * |
|---|
| 996 | * This macro computes inverse transpose of matrix m, |
|---|
| 997 | * and multiplies vector v into it, to yeild vector p |
|---|
| 998 | * |
|---|
| 999 | * DANGER !!! Do Not use this on normal vectors!!! |
|---|
| 1000 | * It will leave normals the wrong length !!! |
|---|
| 1001 | * See macro below for use on normals. |
|---|
| 1002 | */ |
|---|
| 1003 | #define INV_TRANSP_MAT_DOT_VEC_2X2(p,m,v) \ |
|---|
| 1004 | { \ |
|---|
| 1005 | GREAL det; \ |
|---|
| 1006 | \ |
|---|
| 1007 | det = m[0][0]*m[1][1] - m[0][1]*m[1][0]; \ |
|---|
| 1008 | p[0] = m[1][1]*v[0] - m[1][0]*v[1]; \ |
|---|
| 1009 | p[1] = - m[0][1]*v[0] + m[0][0]*v[1]; \ |
|---|
| 1010 | \ |
|---|
| 1011 | /* if matrix not singular, and not orthonormal, then renormalize */ \ |
|---|
| 1012 | if ((det!=1.0f) && (det != 0.0f)) { \ |
|---|
| 1013 | det = 1.0f / det; \ |
|---|
| 1014 | p[0] *= det; \ |
|---|
| 1015 | p[1] *= det; \ |
|---|
| 1016 | } \ |
|---|
| 1017 | }\ |
|---|
| 1018 | |
|---|
| 1019 | |
|---|
| 1020 | /** transform normal vector by inverse transpose of matrix |
|---|
| 1021 | * and then renormalize the vector |
|---|
| 1022 | * |
|---|
| 1023 | * This macro computes inverse transpose of matrix m, |
|---|
| 1024 | * and multiplies vector v into it, to yeild vector p |
|---|
| 1025 | * Vector p is then normalized. |
|---|
| 1026 | */ |
|---|
| 1027 | #define NORM_XFORM_2X2(p,m,v) \ |
|---|
| 1028 | { \ |
|---|
| 1029 | GREAL len; \ |
|---|
| 1030 | \ |
|---|
| 1031 | /* do nothing if off-diagonals are zero and diagonals are \ |
|---|
| 1032 | * equal */ \ |
|---|
| 1033 | if ((m[0][1] != 0.0) || (m[1][0] != 0.0) || (m[0][0] != m[1][1])) { \ |
|---|
| 1034 | p[0] = m[1][1]*v[0] - m[1][0]*v[1]; \ |
|---|
| 1035 | p[1] = - m[0][1]*v[0] + m[0][0]*v[1]; \ |
|---|
| 1036 | \ |
|---|
| 1037 | len = p[0]*p[0] + p[1]*p[1]; \ |
|---|
| 1038 | GIM_INV_SQRT(len,len); \ |
|---|
| 1039 | p[0] *= len; \ |
|---|
| 1040 | p[1] *= len; \ |
|---|
| 1041 | } else { \ |
|---|
| 1042 | VEC_COPY_2 (p, v); \ |
|---|
| 1043 | } \ |
|---|
| 1044 | }\ |
|---|
| 1045 | |
|---|
| 1046 | |
|---|
| 1047 | /** outer product of vector times vector transpose |
|---|
| 1048 | * |
|---|
| 1049 | * The outer product of vector v and vector transpose t yeilds |
|---|
| 1050 | * dyadic matrix m. |
|---|
| 1051 | */ |
|---|
| 1052 | #define OUTER_PRODUCT_2X2(m,v,t) \ |
|---|
| 1053 | { \ |
|---|
| 1054 | m[0][0] = v[0] * t[0]; \ |
|---|
| 1055 | m[0][1] = v[0] * t[1]; \ |
|---|
| 1056 | \ |
|---|
| 1057 | m[1][0] = v[1] * t[0]; \ |
|---|
| 1058 | m[1][1] = v[1] * t[1]; \ |
|---|
| 1059 | }\ |
|---|
| 1060 | |
|---|
| 1061 | |
|---|
| 1062 | /** outer product of vector times vector transpose |
|---|
| 1063 | * |
|---|
| 1064 | * The outer product of vector v and vector transpose t yeilds |
|---|
| 1065 | * dyadic matrix m. |
|---|
| 1066 | */ |
|---|
| 1067 | #define OUTER_PRODUCT_3X3(m,v,t) \ |
|---|
| 1068 | { \ |
|---|
| 1069 | m[0][0] = v[0] * t[0]; \ |
|---|
| 1070 | m[0][1] = v[0] * t[1]; \ |
|---|
| 1071 | m[0][2] = v[0] * t[2]; \ |
|---|
| 1072 | \ |
|---|
| 1073 | m[1][0] = v[1] * t[0]; \ |
|---|
| 1074 | m[1][1] = v[1] * t[1]; \ |
|---|
| 1075 | m[1][2] = v[1] * t[2]; \ |
|---|
| 1076 | \ |
|---|
| 1077 | m[2][0] = v[2] * t[0]; \ |
|---|
| 1078 | m[2][1] = v[2] * t[1]; \ |
|---|
| 1079 | m[2][2] = v[2] * t[2]; \ |
|---|
| 1080 | }\ |
|---|
| 1081 | |
|---|
| 1082 | |
|---|
| 1083 | /** outer product of vector times vector transpose |
|---|
| 1084 | * |
|---|
| 1085 | * The outer product of vector v and vector transpose t yeilds |
|---|
| 1086 | * dyadic matrix m. |
|---|
| 1087 | */ |
|---|
| 1088 | #define OUTER_PRODUCT_4X4(m,v,t) \ |
|---|
| 1089 | { \ |
|---|
| 1090 | m[0][0] = v[0] * t[0]; \ |
|---|
| 1091 | m[0][1] = v[0] * t[1]; \ |
|---|
| 1092 | m[0][2] = v[0] * t[2]; \ |
|---|
| 1093 | m[0][3] = v[0] * t[3]; \ |
|---|
| 1094 | \ |
|---|
| 1095 | m[1][0] = v[1] * t[0]; \ |
|---|
| 1096 | m[1][1] = v[1] * t[1]; \ |
|---|
| 1097 | m[1][2] = v[1] * t[2]; \ |
|---|
| 1098 | m[1][3] = v[1] * t[3]; \ |
|---|
| 1099 | \ |
|---|
| 1100 | m[2][0] = v[2] * t[0]; \ |
|---|
| 1101 | m[2][1] = v[2] * t[1]; \ |
|---|
| 1102 | m[2][2] = v[2] * t[2]; \ |
|---|
| 1103 | m[2][3] = v[2] * t[3]; \ |
|---|
| 1104 | \ |
|---|
| 1105 | m[3][0] = v[3] * t[0]; \ |
|---|
| 1106 | m[3][1] = v[3] * t[1]; \ |
|---|
| 1107 | m[3][2] = v[3] * t[2]; \ |
|---|
| 1108 | m[3][3] = v[3] * t[3]; \ |
|---|
| 1109 | }\ |
|---|
| 1110 | |
|---|
| 1111 | |
|---|
| 1112 | /** outer product of vector times vector transpose |
|---|
| 1113 | * |
|---|
| 1114 | * The outer product of vector v and vector transpose t yeilds |
|---|
| 1115 | * dyadic matrix m. |
|---|
| 1116 | */ |
|---|
| 1117 | #define ACCUM_OUTER_PRODUCT_2X2(m,v,t) \ |
|---|
| 1118 | { \ |
|---|
| 1119 | m[0][0] += v[0] * t[0]; \ |
|---|
| 1120 | m[0][1] += v[0] * t[1]; \ |
|---|
| 1121 | \ |
|---|
| 1122 | m[1][0] += v[1] * t[0]; \ |
|---|
| 1123 | m[1][1] += v[1] * t[1]; \ |
|---|
| 1124 | }\ |
|---|
| 1125 | |
|---|
| 1126 | |
|---|
| 1127 | /** outer product of vector times vector transpose |
|---|
| 1128 | * |
|---|
| 1129 | * The outer product of vector v and vector transpose t yeilds |
|---|
| 1130 | * dyadic matrix m. |
|---|
| 1131 | */ |
|---|
| 1132 | #define ACCUM_OUTER_PRODUCT_3X3(m,v,t) \ |
|---|
| 1133 | { \ |
|---|
| 1134 | m[0][0] += v[0] * t[0]; \ |
|---|
| 1135 | m[0][1] += v[0] * t[1]; \ |
|---|
| 1136 | m[0][2] += v[0] * t[2]; \ |
|---|
| 1137 | \ |
|---|
| 1138 | m[1][0] += v[1] * t[0]; \ |
|---|
| 1139 | m[1][1] += v[1] * t[1]; \ |
|---|
| 1140 | m[1][2] += v[1] * t[2]; \ |
|---|
| 1141 | \ |
|---|
| 1142 | m[2][0] += v[2] * t[0]; \ |
|---|
| 1143 | m[2][1] += v[2] * t[1]; \ |
|---|
| 1144 | m[2][2] += v[2] * t[2]; \ |
|---|
| 1145 | }\ |
|---|
| 1146 | |
|---|
| 1147 | |
|---|
| 1148 | /** outer product of vector times vector transpose |
|---|
| 1149 | * |
|---|
| 1150 | * The outer product of vector v and vector transpose t yeilds |
|---|
| 1151 | * dyadic matrix m. |
|---|
| 1152 | */ |
|---|
| 1153 | #define ACCUM_OUTER_PRODUCT_4X4(m,v,t) \ |
|---|
| 1154 | { \ |
|---|
| 1155 | m[0][0] += v[0] * t[0]; \ |
|---|
| 1156 | m[0][1] += v[0] * t[1]; \ |
|---|
| 1157 | m[0][2] += v[0] * t[2]; \ |
|---|
| 1158 | m[0][3] += v[0] * t[3]; \ |
|---|
| 1159 | \ |
|---|
| 1160 | m[1][0] += v[1] * t[0]; \ |
|---|
| 1161 | m[1][1] += v[1] * t[1]; \ |
|---|
| 1162 | m[1][2] += v[1] * t[2]; \ |
|---|
| 1163 | m[1][3] += v[1] * t[3]; \ |
|---|
| 1164 | \ |
|---|
| 1165 | m[2][0] += v[2] * t[0]; \ |
|---|
| 1166 | m[2][1] += v[2] * t[1]; \ |
|---|
| 1167 | m[2][2] += v[2] * t[2]; \ |
|---|
| 1168 | m[2][3] += v[2] * t[3]; \ |
|---|
| 1169 | \ |
|---|
| 1170 | m[3][0] += v[3] * t[0]; \ |
|---|
| 1171 | m[3][1] += v[3] * t[1]; \ |
|---|
| 1172 | m[3][2] += v[3] * t[2]; \ |
|---|
| 1173 | m[3][3] += v[3] * t[3]; \ |
|---|
| 1174 | }\ |
|---|
| 1175 | |
|---|
| 1176 | |
|---|
| 1177 | /** determinant of matrix |
|---|
| 1178 | * |
|---|
| 1179 | * Computes determinant of matrix m, returning d |
|---|
| 1180 | */ |
|---|
| 1181 | #define DETERMINANT_2X2(d,m) \ |
|---|
| 1182 | { \ |
|---|
| 1183 | d = m[0][0] * m[1][1] - m[0][1] * m[1][0]; \ |
|---|
| 1184 | }\ |
|---|
| 1185 | |
|---|
| 1186 | |
|---|
| 1187 | /** determinant of matrix |
|---|
| 1188 | * |
|---|
| 1189 | * Computes determinant of matrix m, returning d |
|---|
| 1190 | */ |
|---|
| 1191 | #define DETERMINANT_3X3(d,m) \ |
|---|
| 1192 | { \ |
|---|
| 1193 | d = m[0][0] * (m[1][1]*m[2][2] - m[1][2] * m[2][1]); \ |
|---|
| 1194 | d -= m[0][1] * (m[1][0]*m[2][2] - m[1][2] * m[2][0]); \ |
|---|
| 1195 | d += m[0][2] * (m[1][0]*m[2][1] - m[1][1] * m[2][0]); \ |
|---|
| 1196 | }\ |
|---|
| 1197 | |
|---|
| 1198 | |
|---|
| 1199 | /** i,j,th cofactor of a 4x4 matrix |
|---|
| 1200 | * |
|---|
| 1201 | */ |
|---|
| 1202 | #define COFACTOR_4X4_IJ(fac,m,i,j) \ |
|---|
| 1203 | { \ |
|---|
| 1204 | GUINT __ii[4], __jj[4], __k; \ |
|---|
| 1205 | \ |
|---|
| 1206 | for (__k=0; __k<i; __k++) __ii[__k] = __k; \ |
|---|
| 1207 | for (__k=i; __k<3; __k++) __ii[__k] = __k+1; \ |
|---|
| 1208 | for (__k=0; __k<j; __k++) __jj[__k] = __k; \ |
|---|
| 1209 | for (__k=j; __k<3; __k++) __jj[__k] = __k+1; \ |
|---|
| 1210 | \ |
|---|
| 1211 | (fac) = m[__ii[0]][__jj[0]] * (m[__ii[1]][__jj[1]]*m[__ii[2]][__jj[2]] \ |
|---|
| 1212 | - m[__ii[1]][__jj[2]]*m[__ii[2]][__jj[1]]); \ |
|---|
| 1213 | (fac) -= m[__ii[0]][__jj[1]] * (m[__ii[1]][__jj[0]]*m[__ii[2]][__jj[2]] \ |
|---|
| 1214 | - m[__ii[1]][__jj[2]]*m[__ii[2]][__jj[0]]);\ |
|---|
| 1215 | (fac) += m[__ii[0]][__jj[2]] * (m[__ii[1]][__jj[0]]*m[__ii[2]][__jj[1]] \ |
|---|
| 1216 | - m[__ii[1]][__jj[1]]*m[__ii[2]][__jj[0]]);\ |
|---|
| 1217 | \ |
|---|
| 1218 | __k = i+j; \ |
|---|
| 1219 | if ( __k != (__k/2)*2) { \ |
|---|
| 1220 | (fac) = -(fac); \ |
|---|
| 1221 | } \ |
|---|
| 1222 | }\ |
|---|
| 1223 | |
|---|
| 1224 | |
|---|
| 1225 | /** determinant of matrix |
|---|
| 1226 | * |
|---|
| 1227 | * Computes determinant of matrix m, returning d |
|---|
| 1228 | */ |
|---|
| 1229 | #define DETERMINANT_4X4(d,m) \ |
|---|
| 1230 | { \ |
|---|
| 1231 | GREAL cofac; \ |
|---|
| 1232 | COFACTOR_4X4_IJ (cofac, m, 0, 0); \ |
|---|
| 1233 | d = m[0][0] * cofac; \ |
|---|
| 1234 | COFACTOR_4X4_IJ (cofac, m, 0, 1); \ |
|---|
| 1235 | d += m[0][1] * cofac; \ |
|---|
| 1236 | COFACTOR_4X4_IJ (cofac, m, 0, 2); \ |
|---|
| 1237 | d += m[0][2] * cofac; \ |
|---|
| 1238 | COFACTOR_4X4_IJ (cofac, m, 0, 3); \ |
|---|
| 1239 | d += m[0][3] * cofac; \ |
|---|
| 1240 | }\ |
|---|
| 1241 | |
|---|
| 1242 | |
|---|
| 1243 | /** cofactor of matrix |
|---|
| 1244 | * |
|---|
| 1245 | * Computes cofactor of matrix m, returning a |
|---|
| 1246 | */ |
|---|
| 1247 | #define COFACTOR_2X2(a,m) \ |
|---|
| 1248 | { \ |
|---|
| 1249 | a[0][0] = (m)[1][1]; \ |
|---|
| 1250 | a[0][1] = - (m)[1][0]; \ |
|---|
| 1251 | a[1][0] = - (m)[0][1]; \ |
|---|
| 1252 | a[1][1] = (m)[0][0]; \ |
|---|
| 1253 | }\ |
|---|
| 1254 | |
|---|
| 1255 | |
|---|
| 1256 | /** cofactor of matrix |
|---|
| 1257 | * |
|---|
| 1258 | * Computes cofactor of matrix m, returning a |
|---|
| 1259 | */ |
|---|
| 1260 | #define COFACTOR_3X3(a,m) \ |
|---|
| 1261 | { \ |
|---|
| 1262 | a[0][0] = m[1][1]*m[2][2] - m[1][2]*m[2][1]; \ |
|---|
| 1263 | a[0][1] = - (m[1][0]*m[2][2] - m[2][0]*m[1][2]); \ |
|---|
| 1264 | a[0][2] = m[1][0]*m[2][1] - m[1][1]*m[2][0]; \ |
|---|
| 1265 | a[1][0] = - (m[0][1]*m[2][2] - m[0][2]*m[2][1]); \ |
|---|
| 1266 | a[1][1] = m[0][0]*m[2][2] - m[0][2]*m[2][0]; \ |
|---|
| 1267 | a[1][2] = - (m[0][0]*m[2][1] - m[0][1]*m[2][0]); \ |
|---|
| 1268 | a[2][0] = m[0][1]*m[1][2] - m[0][2]*m[1][1]; \ |
|---|
| 1269 | a[2][1] = - (m[0][0]*m[1][2] - m[0][2]*m[1][0]); \ |
|---|
| 1270 | a[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0]); \ |
|---|
| 1271 | }\ |
|---|
| 1272 | |
|---|
| 1273 | |
|---|
| 1274 | /** cofactor of matrix |
|---|
| 1275 | * |
|---|
| 1276 | * Computes cofactor of matrix m, returning a |
|---|
| 1277 | */ |
|---|
| 1278 | #define COFACTOR_4X4(a,m) \ |
|---|
| 1279 | { \ |
|---|
| 1280 | int i,j; \ |
|---|
| 1281 | \ |
|---|
| 1282 | for (i=0; i<4; i++) { \ |
|---|
| 1283 | for (j=0; j<4; j++) { \ |
|---|
| 1284 | COFACTOR_4X4_IJ (a[i][j], m, i, j); \ |
|---|
| 1285 | } \ |
|---|
| 1286 | } \ |
|---|
| 1287 | }\ |
|---|
| 1288 | |
|---|
| 1289 | |
|---|
| 1290 | /** adjoint of matrix |
|---|
| 1291 | * |
|---|
| 1292 | * Computes adjoint of matrix m, returning a |
|---|
| 1293 | * (Note that adjoint is just the transpose of the cofactor matrix) |
|---|
| 1294 | */ |
|---|
| 1295 | #define ADJOINT_2X2(a,m) \ |
|---|
| 1296 | { \ |
|---|
| 1297 | a[0][0] = (m)[1][1]; \ |
|---|
| 1298 | a[1][0] = - (m)[1][0]; \ |
|---|
| 1299 | a[0][1] = - (m)[0][1]; \ |
|---|
| 1300 | a[1][1] = (m)[0][0]; \ |
|---|
| 1301 | }\ |
|---|
| 1302 | |
|---|
| 1303 | |
|---|
| 1304 | /** adjoint of matrix |
|---|
| 1305 | * |
|---|
| 1306 | * Computes adjoint of matrix m, returning a |
|---|
| 1307 | * (Note that adjoint is just the transpose of the cofactor matrix) |
|---|
| 1308 | */ |
|---|
| 1309 | #define ADJOINT_3X3(a,m) \ |
|---|
| 1310 | { \ |
|---|
| 1311 | a[0][0] = m[1][1]*m[2][2] - m[1][2]*m[2][1]; \ |
|---|
| 1312 | a[1][0] = - (m[1][0]*m[2][2] - m[2][0]*m[1][2]); \ |
|---|
| 1313 | a[2][0] = m[1][0]*m[2][1] - m[1][1]*m[2][0]; \ |
|---|
| 1314 | a[0][1] = - (m[0][1]*m[2][2] - m[0][2]*m[2][1]); \ |
|---|
| 1315 | a[1][1] = m[0][0]*m[2][2] - m[0][2]*m[2][0]; \ |
|---|
| 1316 | a[2][1] = - (m[0][0]*m[2][1] - m[0][1]*m[2][0]); \ |
|---|
| 1317 | a[0][2] = m[0][1]*m[1][2] - m[0][2]*m[1][1]; \ |
|---|
| 1318 | a[1][2] = - (m[0][0]*m[1][2] - m[0][2]*m[1][0]); \ |
|---|
| 1319 | a[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0]); \ |
|---|
| 1320 | }\ |
|---|
| 1321 | |
|---|
| 1322 | |
|---|
| 1323 | /** adjoint of matrix |
|---|
| 1324 | * |
|---|
| 1325 | * Computes adjoint of matrix m, returning a |
|---|
| 1326 | * (Note that adjoint is just the transpose of the cofactor matrix) |
|---|
| 1327 | */ |
|---|
| 1328 | #define ADJOINT_4X4(a,m) \ |
|---|
| 1329 | { \ |
|---|
| 1330 | char _i_,_j_; \ |
|---|
| 1331 | \ |
|---|
| 1332 | for (_i_=0; _i_<4; _i_++) { \ |
|---|
| 1333 | for (_j_=0; _j_<4; _j_++) { \ |
|---|
| 1334 | COFACTOR_4X4_IJ (a[_j_][_i_], m, _i_, _j_); \ |
|---|
| 1335 | } \ |
|---|
| 1336 | } \ |
|---|
| 1337 | }\ |
|---|
| 1338 | |
|---|
| 1339 | |
|---|
| 1340 | /** compute adjoint of matrix and scale |
|---|
| 1341 | * |
|---|
| 1342 | * Computes adjoint of matrix m, scales it by s, returning a |
|---|
| 1343 | */ |
|---|
| 1344 | #define SCALE_ADJOINT_2X2(a,s,m) \ |
|---|
| 1345 | { \ |
|---|
| 1346 | a[0][0] = (s) * m[1][1]; \ |
|---|
| 1347 | a[1][0] = - (s) * m[1][0]; \ |
|---|
| 1348 | a[0][1] = - (s) * m[0][1]; \ |
|---|
| 1349 | a[1][1] = (s) * m[0][0]; \ |
|---|
| 1350 | }\ |
|---|
| 1351 | |
|---|
| 1352 | |
|---|
| 1353 | /** compute adjoint of matrix and scale |
|---|
| 1354 | * |
|---|
| 1355 | * Computes adjoint of matrix m, scales it by s, returning a |
|---|
| 1356 | */ |
|---|
| 1357 | #define SCALE_ADJOINT_3X3(a,s,m) \ |
|---|
| 1358 | { \ |
|---|
| 1359 | a[0][0] = (s) * (m[1][1] * m[2][2] - m[1][2] * m[2][1]); \ |
|---|
| 1360 | a[1][0] = (s) * (m[1][2] * m[2][0] - m[1][0] * m[2][2]); \ |
|---|
| 1361 | a[2][0] = (s) * (m[1][0] * m[2][1] - m[1][1] * m[2][0]); \ |
|---|
| 1362 | \ |
|---|
| 1363 | a[0][1] = (s) * (m[0][2] * m[2][1] - m[0][1] * m[2][2]); \ |
|---|
| 1364 | a[1][1] = (s) * (m[0][0] * m[2][2] - m[0][2] * m[2][0]); \ |
|---|
| 1365 | a[2][1] = (s) * (m[0][1] * m[2][0] - m[0][0] * m[2][1]); \ |
|---|
| 1366 | \ |
|---|
| 1367 | a[0][2] = (s) * (m[0][1] * m[1][2] - m[0][2] * m[1][1]); \ |
|---|
| 1368 | a[1][2] = (s) * (m[0][2] * m[1][0] - m[0][0] * m[1][2]); \ |
|---|
| 1369 | a[2][2] = (s) * (m[0][0] * m[1][1] - m[0][1] * m[1][0]); \ |
|---|
| 1370 | }\ |
|---|
| 1371 | |
|---|
| 1372 | |
|---|
| 1373 | /** compute adjoint of matrix and scale |
|---|
| 1374 | * |
|---|
| 1375 | * Computes adjoint of matrix m, scales it by s, returning a |
|---|
| 1376 | */ |
|---|
| 1377 | #define SCALE_ADJOINT_4X4(a,s,m) \ |
|---|
| 1378 | { \ |
|---|
| 1379 | char _i_,_j_; \ |
|---|
| 1380 | for (_i_=0; _i_<4; _i_++) { \ |
|---|
| 1381 | for (_j_=0; _j_<4; _j_++) { \ |
|---|
| 1382 | COFACTOR_4X4_IJ (a[_j_][_i_], m, _i_, _j_); \ |
|---|
| 1383 | a[_j_][_i_] *= s; \ |
|---|
| 1384 | } \ |
|---|
| 1385 | } \ |
|---|
| 1386 | }\ |
|---|
| 1387 | |
|---|
| 1388 | /** inverse of matrix |
|---|
| 1389 | * |
|---|
| 1390 | * Compute inverse of matrix a, returning determinant m and |
|---|
| 1391 | * inverse b |
|---|
| 1392 | */ |
|---|
| 1393 | #define INVERT_2X2(b,det,a) \ |
|---|
| 1394 | { \ |
|---|
| 1395 | GREAL _tmp_; \ |
|---|
| 1396 | DETERMINANT_2X2 (det, a); \ |
|---|
| 1397 | _tmp_ = 1.0 / (det); \ |
|---|
| 1398 | SCALE_ADJOINT_2X2 (b, _tmp_, a); \ |
|---|
| 1399 | }\ |
|---|
| 1400 | |
|---|
| 1401 | |
|---|
| 1402 | /** inverse of matrix |
|---|
| 1403 | * |
|---|
| 1404 | * Compute inverse of matrix a, returning determinant m and |
|---|
| 1405 | * inverse b |
|---|
| 1406 | */ |
|---|
| 1407 | #define INVERT_3X3(b,det,a) \ |
|---|
| 1408 | { \ |
|---|
| 1409 | GREAL _tmp_; \ |
|---|
| 1410 | DETERMINANT_3X3 (det, a); \ |
|---|
| 1411 | _tmp_ = 1.0 / (det); \ |
|---|
| 1412 | SCALE_ADJOINT_3X3 (b, _tmp_, a); \ |
|---|
| 1413 | }\ |
|---|
| 1414 | |
|---|
| 1415 | |
|---|
| 1416 | /** inverse of matrix |
|---|
| 1417 | * |
|---|
| 1418 | * Compute inverse of matrix a, returning determinant m and |
|---|
| 1419 | * inverse b |
|---|
| 1420 | */ |
|---|
| 1421 | #define INVERT_4X4(b,det,a) \ |
|---|
| 1422 | { \ |
|---|
| 1423 | GREAL _tmp_; \ |
|---|
| 1424 | DETERMINANT_4X4 (det, a); \ |
|---|
| 1425 | _tmp_ = 1.0 / (det); \ |
|---|
| 1426 | SCALE_ADJOINT_4X4 (b, _tmp_, a); \ |
|---|
| 1427 | }\ |
|---|
| 1428 | |
|---|
| 1429 | //! Get the triple(3) row of a transform matrix |
|---|
| 1430 | #define MAT_GET_ROW(mat,vec3,rowindex)\ |
|---|
| 1431 | {\ |
|---|
| 1432 | vec3[0] = mat[rowindex][0];\ |
|---|
| 1433 | vec3[1] = mat[rowindex][1];\ |
|---|
| 1434 | vec3[2] = mat[rowindex][2]; \ |
|---|
| 1435 | }\ |
|---|
| 1436 | |
|---|
| 1437 | //! Get the tuple(2) row of a transform matrix |
|---|
| 1438 | #define MAT_GET_ROW2(mat,vec2,rowindex)\ |
|---|
| 1439 | {\ |
|---|
| 1440 | vec2[0] = mat[rowindex][0];\ |
|---|
| 1441 | vec2[1] = mat[rowindex][1];\ |
|---|
| 1442 | }\ |
|---|
| 1443 | |
|---|
| 1444 | |
|---|
| 1445 | //! Get the quad (4) row of a transform matrix |
|---|
| 1446 | #define MAT_GET_ROW4(mat,vec4,rowindex)\ |
|---|
| 1447 | {\ |
|---|
| 1448 | vec4[0] = mat[rowindex][0];\ |
|---|
| 1449 | vec4[1] = mat[rowindex][1];\ |
|---|
| 1450 | vec4[2] = mat[rowindex][2];\ |
|---|
| 1451 | vec4[3] = mat[rowindex][3];\ |
|---|
| 1452 | }\ |
|---|
| 1453 | |
|---|
| 1454 | //! Get the triple(3) col of a transform matrix |
|---|
| 1455 | #define MAT_GET_COL(mat,vec3,colindex)\ |
|---|
| 1456 | {\ |
|---|
| 1457 | vec3[0] = mat[0][colindex];\ |
|---|
| 1458 | vec3[1] = mat[1][colindex];\ |
|---|
| 1459 | vec3[2] = mat[2][colindex]; \ |
|---|
| 1460 | }\ |
|---|
| 1461 | |
|---|
| 1462 | //! Get the tuple(2) col of a transform matrix |
|---|
| 1463 | #define MAT_GET_COL2(mat,vec2,colindex)\ |
|---|
| 1464 | {\ |
|---|
| 1465 | vec2[0] = mat[0][colindex];\ |
|---|
| 1466 | vec2[1] = mat[1][colindex];\ |
|---|
| 1467 | }\ |
|---|
| 1468 | |
|---|
| 1469 | |
|---|
| 1470 | //! Get the quad (4) col of a transform matrix |
|---|
| 1471 | #define MAT_GET_COL4(mat,vec4,colindex)\ |
|---|
| 1472 | {\ |
|---|
| 1473 | vec4[0] = mat[0][colindex];\ |
|---|
| 1474 | vec4[1] = mat[1][colindex];\ |
|---|
| 1475 | vec4[2] = mat[2][colindex];\ |
|---|
| 1476 | vec4[3] = mat[3][colindex];\ |
|---|
| 1477 | }\ |
|---|
| 1478 | |
|---|
| 1479 | //! Get the triple(3) col of a transform matrix |
|---|
| 1480 | #define MAT_GET_X(mat,vec3)\ |
|---|
| 1481 | {\ |
|---|
| 1482 | MAT_GET_COL(mat,vec3,0);\ |
|---|
| 1483 | }\ |
|---|
| 1484 | |
|---|
| 1485 | //! Get the triple(3) col of a transform matrix |
|---|
| 1486 | #define MAT_GET_Y(mat,vec3)\ |
|---|
| 1487 | {\ |
|---|
| 1488 | MAT_GET_COL(mat,vec3,1);\ |
|---|
| 1489 | }\ |
|---|
| 1490 | |
|---|
| 1491 | //! Get the triple(3) col of a transform matrix |
|---|
| 1492 | #define MAT_GET_Z(mat,vec3)\ |
|---|
| 1493 | {\ |
|---|
| 1494 | MAT_GET_COL(mat,vec3,2);\ |
|---|
| 1495 | }\ |
|---|
| 1496 | |
|---|
| 1497 | |
|---|
| 1498 | //! Get the triple(3) col of a transform matrix |
|---|
| 1499 | #define MAT_SET_X(mat,vec3)\ |
|---|
| 1500 | {\ |
|---|
| 1501 | mat[0][0] = vec3[0];\ |
|---|
| 1502 | mat[1][0] = vec3[1];\ |
|---|
| 1503 | mat[2][0] = vec3[2];\ |
|---|
| 1504 | }\ |
|---|
| 1505 | |
|---|
| 1506 | //! Get the triple(3) col of a transform matrix |
|---|
| 1507 | #define MAT_SET_Y(mat,vec3)\ |
|---|
| 1508 | {\ |
|---|
| 1509 | mat[0][1] = vec3[0];\ |
|---|
| 1510 | mat[1][1] = vec3[1];\ |
|---|
| 1511 | mat[2][1] = vec3[2];\ |
|---|
| 1512 | }\ |
|---|
| 1513 | |
|---|
| 1514 | //! Get the triple(3) col of a transform matrix |
|---|
| 1515 | #define MAT_SET_Z(mat,vec3)\ |
|---|
| 1516 | {\ |
|---|
| 1517 | mat[0][2] = vec3[0];\ |
|---|
| 1518 | mat[1][2] = vec3[1];\ |
|---|
| 1519 | mat[2][2] = vec3[2];\ |
|---|
| 1520 | }\ |
|---|
| 1521 | |
|---|
| 1522 | |
|---|
| 1523 | //! Get the triple(3) col of a transform matrix |
|---|
| 1524 | #define MAT_GET_TRANSLATION(mat,vec3)\ |
|---|
| 1525 | {\ |
|---|
| 1526 | vec3[0] = mat[0][3];\ |
|---|
| 1527 | vec3[1] = mat[1][3];\ |
|---|
| 1528 | vec3[2] = mat[2][3]; \ |
|---|
| 1529 | }\ |
|---|
| 1530 | |
|---|
| 1531 | //! Set the triple(3) col of a transform matrix |
|---|
| 1532 | #define MAT_SET_TRANSLATION(mat,vec3)\ |
|---|
| 1533 | {\ |
|---|
| 1534 | mat[0][3] = vec3[0];\ |
|---|
| 1535 | mat[1][3] = vec3[1];\ |
|---|
| 1536 | mat[2][3] = vec3[2]; \ |
|---|
| 1537 | }\ |
|---|
| 1538 | |
|---|
| 1539 | |
|---|
| 1540 | |
|---|
| 1541 | //! Returns the dot product between a vec3f and the row of a matrix |
|---|
| 1542 | #define MAT_DOT_ROW(mat,vec3,rowindex) (vec3[0]*mat[rowindex][0] + vec3[1]*mat[rowindex][1] + vec3[2]*mat[rowindex][2]) |
|---|
| 1543 | |
|---|
| 1544 | //! Returns the dot product between a vec2f and the row of a matrix |
|---|
| 1545 | #define MAT_DOT_ROW2(mat,vec2,rowindex) (vec2[0]*mat[rowindex][0] + vec2[1]*mat[rowindex][1]) |
|---|
| 1546 | |
|---|
| 1547 | //! Returns the dot product between a vec4f and the row of a matrix |
|---|
| 1548 | #define MAT_DOT_ROW4(mat,vec4,rowindex) (vec4[0]*mat[rowindex][0] + vec4[1]*mat[rowindex][1] + vec4[2]*mat[rowindex][2] + vec4[3]*mat[rowindex][3]) |
|---|
| 1549 | |
|---|
| 1550 | |
|---|
| 1551 | //! Returns the dot product between a vec3f and the col of a matrix |
|---|
| 1552 | #define MAT_DOT_COL(mat,vec3,colindex) (vec3[0]*mat[0][colindex] + vec3[1]*mat[1][colindex] + vec3[2]*mat[2][colindex]) |
|---|
| 1553 | |
|---|
| 1554 | //! Returns the dot product between a vec2f and the col of a matrix |
|---|
| 1555 | #define MAT_DOT_COL2(mat,vec2,colindex) (vec2[0]*mat[0][colindex] + vec2[1]*mat[1][colindex]) |
|---|
| 1556 | |
|---|
| 1557 | //! Returns the dot product between a vec4f and the col of a matrix |
|---|
| 1558 | #define MAT_DOT_COL4(mat,vec4,colindex) (vec4[0]*mat[0][colindex] + vec4[1]*mat[1][colindex] + vec4[2]*mat[2][colindex] + vec4[3]*mat[3][colindex]) |
|---|
| 1559 | |
|---|
| 1560 | /*!Transpose matrix times vector |
|---|
| 1561 | v is a vec3f |
|---|
| 1562 | and m is a mat4f<br> |
|---|
| 1563 | */ |
|---|
| 1564 | #define INV_MAT_DOT_VEC_3X3(p,m,v) \ |
|---|
| 1565 | { \ |
|---|
| 1566 | p[0] = MAT_DOT_COL(m,v,0); \ |
|---|
| 1567 | p[1] = MAT_DOT_COL(m,v,1); \ |
|---|
| 1568 | p[2] = MAT_DOT_COL(m,v,2); \ |
|---|
| 1569 | }\ |
|---|
| 1570 | |
|---|
| 1571 | |
|---|
| 1572 | |
|---|
| 1573 | #endif // GIM_VECTOR_H_INCLUDED |
|---|
