source/blender/blenlib/intern/matrixops.c

   1 /*
   2  *
   3  * Some matrix operations.
   4  *
   5  * Always use
   6  * - vector with x components :   float x[3], int x[3], etc
   7  *
   8  * $Id$
   9  *
  10  * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
  11  *
  12  * This program is free software; you can redistribute it and/or
  13  * modify it under the terms of the GNU General Public License
  14  * as published by the Free Software Foundation; either version 2
  15  * of the License, or (at your option) any later version. The Blender
  16  * Foundation also sells licenses for use in proprietary software under
  17  * the Blender License.  See http://www.blender.org/BL/ for information
  18  * about this.
  19  *
  20  * This program is distributed in the hope that it will be useful,
  21  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  22  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  23  * GNU General Public License for more details.
  24  *
  25  * You should have received a copy of the GNU General Public License
  26  * along with this program; if not, write to the Free Software Foundation,
  27  * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
  28  *
  29  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
  30  * All rights reserved.
  31  *
  32  * The Original Code is: all of this file.
  33  *
  34  * Contributor(s): none yet.
  35  *
  36  * ***** END GPL/BL DUAL LICENSE BLOCK *****
  37  */
  38
  39 /* ------------------------------------------------------------------------- */
  40 #include <string.h>
  41 #include "MTC_matrixops.h"
  42 #include "MTC_vectorops.h"
  43 /* ------------------------------------------------------------------------- */
  44
  45 #ifdef HAVE_CONFIG_H
  46 #include <config.h>
  47 #endif
  48
  49 #ifdef WIN32
  50 #include "BLI_winstuff.h"
  51 #endif
  52
  53 #if defined(__sun__) || defined( __sun ) || defined (__sparc) || defined (__sparc__)
  54 #include <strings.h>
  55 #endif
  56
  57 #define ABS(x)  ((x) < 0 ? -(x) : (x))
  58 #define SWAP(type, a, b)        { type sw_ap; sw_ap=(a); (a)=(b); (b)=sw_ap; }
  59
  60 void MTC_Mat4CpyMat4(float m1[][4], float m2[][4])
  61 {
  62         memcpy(m1, m2, 4*4*sizeof(float));
  63 }
  64
  65 /* ------------------------------------------------------------------------- */
  66
  67 void MTC_Mat4MulSerie(float answ[][4],
  68                                   float m1[][4], float m2[][4], float m3[][4],
  69                                   float m4[][4], float m5[][4], float m6[][4],
  70                                   float m7[][4], float m8[][4])
  71 {
  72         float temp[4][4];
  73
  74         if(m1==0 || m2==0) return;
  75
  76         MTC_Mat4MulMat4(answ, m2, m1);
  77         if(m3) {
  78                 MTC_Mat4MulMat4(temp, m3, answ);
  79                 if(m4) {
  80                         MTC_Mat4MulMat4(answ, m4, temp);
  81                         if(m5) {
  82                                 MTC_Mat4MulMat4(temp, m5, answ);
  83                                 if(m6) {
  84                                         MTC_Mat4MulMat4(answ, m6, temp);
  85                                         if(m7) {
  86                                                 MTC_Mat4MulMat4(temp, m7, answ);
  87                                                 if(m8) {
  88                                                         MTC_Mat4MulMat4(answ, m8, temp);
  89                                                 }
  90                                                 else MTC_Mat4CpyMat4(answ, temp);
  91                                         }
  92                                 }
  93                                 else MTC_Mat4CpyMat4(answ, temp);
  94                         }
  95                 }
  96                 else MTC_Mat4CpyMat4(answ, temp);
  97         }
  98 }
  99
 100 /* ------------------------------------------------------------------------- */
 101 void MTC_Mat4MulMat4(float m1[][4], float m2[][4], float m3[][4])
 102 {
 103   /* matrix product: c[j][k] = a[j][i].b[i][k] */
 104
 105         m1[0][0] = m2[0][0]*m3[0][0] + m2[0][1]*m3[1][0] + m2[0][2]*m3[2][0] + m2[0][3]*m3[3][0];
 106         m1[0][1] = m2[0][0]*m3[0][1] + m2[0][1]*m3[1][1] + m2[0][2]*m3[2][1] + m2[0][3]*m3[3][1];
 107         m1[0][2] = m2[0][0]*m3[0][2] + m2[0][1]*m3[1][2] + m2[0][2]*m3[2][2] + m2[0][3]*m3[3][2];
 108         m1[0][3] = m2[0][0]*m3[0][3] + m2[0][1]*m3[1][3] + m2[0][2]*m3[2][3] + m2[0][3]*m3[3][3];
 109
 110         m1[1][0] = m2[1][0]*m3[0][0] + m2[1][1]*m3[1][0] + m2[1][2]*m3[2][0] + m2[1][3]*m3[3][0];
 111         m1[1][1] = m2[1][0]*m3[0][1] + m2[1][1]*m3[1][1] + m2[1][2]*m3[2][1] + m2[1][3]*m3[3][1];
 112         m1[1][2] = m2[1][0]*m3[0][2] + m2[1][1]*m3[1][2] + m2[1][2]*m3[2][2] + m2[1][3]*m3[3][2];
 113         m1[1][3] = m2[1][0]*m3[0][3] + m2[1][1]*m3[1][3] + m2[1][2]*m3[2][3] + m2[1][3]*m3[3][3];
 114
 115         m1[2][0] = m2[2][0]*m3[0][0] + m2[2][1]*m3[1][0] + m2[2][2]*m3[2][0] + m2[2][3]*m3[3][0];
 116         m1[2][1] = m2[2][0]*m3[0][1] + m2[2][1]*m3[1][1] + m2[2][2]*m3[2][1] + m2[2][3]*m3[3][1];
 117         m1[2][2] = m2[2][0]*m3[0][2] + m2[2][1]*m3[1][2] + m2[2][2]*m3[2][2] + m2[2][3]*m3[3][2];
 118         m1[2][3] = m2[2][0]*m3[0][3] + m2[2][1]*m3[1][3] + m2[2][2]*m3[2][3] + m2[2][3]*m3[3][3];
 119
 120         m1[3][0] = m2[3][0]*m3[0][0] + m2[3][1]*m3[1][0] + m2[3][2]*m3[2][0] + m2[3][3]*m3[3][0];
 121         m1[3][1] = m2[3][0]*m3[0][1] + m2[3][1]*m3[1][1] + m2[3][2]*m3[2][1] + m2[3][3]*m3[3][1];
 122         m1[3][2] = m2[3][0]*m3[0][2] + m2[3][1]*m3[1][2] + m2[3][2]*m3[2][2] + m2[3][3]*m3[3][2];
 123         m1[3][3] = m2[3][0]*m3[0][3] + m2[3][1]*m3[1][3] + m2[3][2]*m3[2][3] + m2[3][3]*m3[3][3];
 124
 125 }
 126 /* ------------------------------------------------------------------------- */
 127
 128 void MTC_Mat4MulVecfl(float mat[][4], float *vec)
 129 {
 130         float x,y;
 131
 132         x=vec[0];
 133         y=vec[1];
 134         vec[0]=x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2] + mat[3][0];
 135         vec[1]=x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2] + mat[3][1];
 136         vec[2]=x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2] + mat[3][2];
 137 }
 138
 139 /* ------------------------------------------------------------------------- */
 140 void MTC_Mat3MulVecfl(float mat[][3], float *vec)
 141 {
 142         float x,y;
 143
 144         x=vec[0];
 145         y=vec[1];
 146         vec[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2];
 147         vec[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2];
 148         vec[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2];
 149 }
 150
 151 /* ------------------------------------------------------------------------- */
 152
 153 int MTC_Mat4Invert(float inverse[][4], float mat[][4])
 154 {
 155         int i, j, k;
 156         double temp;
 157         float tempmat[4][4];
 158         float max;
 159         int maxj;
 160
 161         /* Set inverse to identity */
 162         for (i=0; i<4; i++)
 163                 for (j=0; j<4; j++)
 164                         inverse[i][j] = 0;
 165         for (i=0; i<4; i++)
 166                 inverse[i][i] = 1;
 167
 168         /* Copy original matrix so we don't mess it up */
 169         for(i = 0; i < 4; i++)
 170                 for(j = 0; j <4; j++)
 171                         tempmat[i][j] = mat[i][j];
 172
 173         for(i = 0; i < 4; i++) {
 174                 /* Look for row with max pivot */
 175                 max = ABS(tempmat[i][i]);
 176                 maxj = i;
 177                 for(j = i + 1; j < 4; j++) {
 178                         if(ABS(tempmat[j][i]) > max) {
 179                                 max = ABS(tempmat[j][i]);
 180                                 maxj = j;
 181                         }
 182                 }
 183                 /* Swap rows if necessary */
 184                 if (maxj != i) {
 185                         for( k = 0; k < 4; k++) {
 186                                 SWAP(float, tempmat[i][k], tempmat[maxj][k]);
 187                                 SWAP(float, inverse[i][k], inverse[maxj][k]);
 188                         }
 189                 }
 190
 191                 temp = tempmat[i][i];
 192                 if (temp == 0)
 193                         return 0;  /* No non-zero pivot */
 194                 for(k = 0; k < 4; k++) {
 195                         tempmat[i][k] /= temp;
 196                         inverse[i][k] /= temp;
 197                 }
 198                 for(j = 0; j < 4; j++) {
 199                         if(j != i) {
 200                                 temp = tempmat[j][i];
 201                                 for(k = 0; k < 4; k++) {
 202                                         tempmat[j][k] -= tempmat[i][k]*temp;
 203                                         inverse[j][k] -= inverse[i][k]*temp;
 204                                 }
 205                         }
 206                 }
 207         }
 208         return 1;
 209 }
 210
 211 /* ------------------------------------------------------------------------- */
 212 void MTC_Mat3CpyMat4(float m1[][3], float m2[][4])
 213 {
 214
 215         m1[0][0]= m2[0][0];
 216         m1[0][1]= m2[0][1];
 217         m1[0][2]= m2[0][2];
 218
 219         m1[1][0]= m2[1][0];
 220         m1[1][1]= m2[1][1];
 221         m1[1][2]= m2[1][2];
 222
 223         m1[2][0]= m2[2][0];
 224         m1[2][1]= m2[2][1];
 225         m1[2][2]= m2[2][2];
 226 }
 227
 228 /* ------------------------------------------------------------------------- */
 229
 230 void MTC_Mat3CpyMat3(float m1[][3], float m2[][3])
 231 {
 232         memcpy(m1, m2, 3*3*sizeof(float));
 233 }
 234
 235 /* ------------------------------------------------------------------------- */
 236 /*  void Mat3MulMat3(float m1[][3], float m3[][3], float m2[][3]) */
 237 void MTC_Mat3MulMat3(float m1[][3], float m3[][3], float m2[][3])
 238 {
 239         /* be careful about this rewrite... */
 240             /* m1[i][j] = m2[i][k]*m3[k][j], args are flipped! */
 241         m1[0][0]= m2[0][0]*m3[0][0] + m2[0][1]*m3[1][0] + m2[0][2]*m3[2][0];
 242         m1[0][1]= m2[0][0]*m3[0][1] + m2[0][1]*m3[1][1] + m2[0][2]*m3[2][1];
 243         m1[0][2]= m2[0][0]*m3[0][2] + m2[0][1]*m3[1][2] + m2[0][2]*m3[2][2];
 244
 245         m1[1][0]= m2[1][0]*m3[0][0] + m2[1][1]*m3[1][0] + m2[1][2]*m3[2][0];
 246         m1[1][1]= m2[1][0]*m3[0][1] + m2[1][1]*m3[1][1] + m2[1][2]*m3[2][1];
 247         m1[1][2]= m2[1][0]*m3[0][2] + m2[1][1]*m3[1][2] + m2[1][2]*m3[2][2];
 248
 249         m1[2][0]= m2[2][0]*m3[0][0] + m2[2][1]*m3[1][0] + m2[2][2]*m3[2][0];
 250         m1[2][1]= m2[2][0]*m3[0][1] + m2[2][1]*m3[1][1] + m2[2][2]*m3[2][1];
 251         m1[2][2]= m2[2][0]*m3[0][2] + m2[2][1]*m3[1][2] + m2[2][2]*m3[2][2];
 252
 253 /*      m1[0]= m2[0]*m3[0] + m2[1]*m3[3] + m2[2]*m3[6]; */
 254 /*      m1[1]= m2[0]*m3[1] + m2[1]*m3[4] + m2[2]*m3[7]; */
 255 /*      m1[2]= m2[0]*m3[2] + m2[1]*m3[5] + m2[2]*m3[8]; */
 256 /*      m1+=3; */
 257 /*      m2+=3; */
 258 /*      m1[0]= m2[0]*m3[0] + m2[1]*m3[3] + m2[2]*m3[6]; */
 259 /*      m1[1]= m2[0]*m3[1] + m2[1]*m3[4] + m2[2]*m3[7]; */
 260 /*      m1[2]= m2[0]*m3[2] + m2[1]*m3[5] + m2[2]*m3[8]; */
 261 /*      m1+=3; */
 262 /*      m2+=3; */
 263 /*      m1[0]= m2[0]*m3[0] + m2[1]*m3[3] + m2[2]*m3[6]; */
 264 /*      m1[1]= m2[0]*m3[1] + m2[1]*m3[4] + m2[2]*m3[7]; */
 265 /*      m1[2]= m2[0]*m3[2] + m2[1]*m3[5] + m2[2]*m3[8]; */
 266 } /* end of void Mat3MulMat3(float m1[][3], float m3[][3], float m2[][3]) */
 267
 268 /* ------------------------------------------------------------------------- */
 269
 270 void MTC_Mat4Ortho(float mat[][4])
 271 {
 272         float len;
 273
 274         len= MTC_normalise3DF(mat[0]);
 275         if(len!=0.0) mat[0][3]/= len;
 276         len= MTC_normalise3DF(mat[1]);
 277         if(len!=0.0) mat[1][3]/= len;
 278         len= MTC_normalise3DF(mat[2]);
 279         if(len!=0.0) mat[2][3]/= len;
 280 }
 281
 282 /* ------------------------------------------------------------------------- */
 283
 284 void MTC_Mat4Mul3Vecfl(float mat[][4], float *vec)
 285 {
 286         float x,y;
 287         /* vec = mat^T dot vec !!! or vec a row, then vec = vec dot mat*/
 288
 289         x= vec[0];
 290         y= vec[1];
 291         vec[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2];
 292         vec[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2];
 293         vec[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2];
 294 }
 295
 296 /* ------------------------------------------------------------------------- */
 297
 298 void MTC_Mat4One(float m[][4])
 299 {
 300
 301         m[0][0]= m[1][1]= m[2][2]= m[3][3]= 1.0;
 302         m[0][1]= m[0][2]= m[0][3]= 0.0;
 303         m[1][0]= m[1][2]= m[1][3]= 0.0;
 304         m[2][0]= m[2][1]= m[2][3]= 0.0;
 305         m[3][0]= m[3][1]= m[3][2]= 0.0;
 306 }
 307
 308
 309 /* ------------------------------------------------------------------------- */
 310 /* Result is a 3-vector!*/
 311 void MTC_Mat3MulVecd(float mat[][3], double *vec)
 312 {
 313         double x,y;
 314
 315         /* vec = mat^T dot vec !!! or vec a row, then vec = vec dot mat*/
 316         x=vec[0];
 317         y=vec[1];
 318         vec[0]= x * mat[0][0] + y * mat[1][0] + mat[2][0] * vec[2];
 319         vec[1]= x * mat[0][1] + y * mat[1][1] + mat[2][1] * vec[2];
 320         vec[2]= x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2];
 321 }
 322
 323 /* ------------------------------------------------------------------------- */
 324
 325 void MTC_Mat3Inv(float m1[][3], float m2[][3])
 326 {
 327         short a,b;
 328         float det;
 329
 330         /* first adjoint */
 331         MTC_Mat3Adj(m1,m2);
 332
 333         /* then determinant old mat! */
 334         det= m2[0][0]* (m2[1][1]*m2[2][2] - m2[1][2]*m2[2][1])
 335             -m2[1][0]* (m2[0][1]*m2[2][2] - m2[0][2]*m2[2][1])
 336             +m2[2][0]* (m2[0][1]*m2[1][2] - m2[0][2]*m2[1][1]);
 337
 338         if(det==0) det=1;
 339         det= 1/det;
 340         for(a=0;a<3;a++) {
 341                 for(b=0;b<3;b++) {
 342                         m1[a][b]*=det;
 343                 }
 344         }
 345 }
 346
 347 /* ------------------------------------------------------------------------- */
 348
 349 void MTC_Mat3Adj(float m1[][3], float m[][3])
 350 {
 351         m1[0][0]=m[1][1]*m[2][2]-m[1][2]*m[2][1];
 352         m1[0][1]= -m[0][1]*m[2][2]+m[0][2]*m[2][1];
 353         m1[0][2]=m[0][1]*m[1][2]-m[0][2]*m[1][1];
 354
 355         m1[1][0]= -m[1][0]*m[2][2]+m[1][2]*m[2][0];
 356         m1[1][1]=m[0][0]*m[2][2]-m[0][2]*m[2][0];
 357         m1[1][2]= -m[0][0]*m[1][2]+m[0][2]*m[1][0];
 358
 359         m1[2][0]=m[1][0]*m[2][1]-m[1][1]*m[2][0];
 360         m1[2][1]= -m[0][0]*m[2][1]+m[0][1]*m[2][0];
 361         m1[2][2]=m[0][0]*m[1][1]-m[0][1]*m[1][0];
 362 }
 363
 364 /* ------------------------------------------------------------------------- */
 365
 366 void MTC_Mat3One(float m[][3])
 367 {
 368
 369         m[0][0]= m[1][1]= m[2][2]= 1.0;
 370         m[0][1]= m[0][2]= 0.0;
 371         m[1][0]= m[1][2]= 0.0;
 372         m[2][0]= m[2][1]= 0.0;
 373 }
 374
 375 /* ------------------------------------------------------------------------- */
 376
 377 void MTC_Mat4SwapMat4(float m1[][4], float m2[][4])
 378 {
 379         float t;
 380         int i, j;
 381
 382         for(i = 0; i < 4; i++) {
 383                 for (j = 0; j < 4; j++) {
 384                         t        = m1[i][j];
 385                         m1[i][j] = m2[i][j];
 386                         m2[i][j] = t;
 387                 }
 388         }
 389 }
 390
 391 /* ------------------------------------------------------------------------- */
 392
 393 void MTC_Mat4MulVec4fl(float mat[][4], float *vec)
 394 {
 395         float x,y,z;
 396
 397         x = vec[0];
 398         y = vec[1];
 399         z = vec[2];
 400         vec[0] = x*mat[0][0] + y*mat[1][0] + z*mat[2][0] + mat[3][0]*vec[3];
 401         vec[1] = x*mat[0][1] + y*mat[1][1] + z*mat[2][1] + mat[3][1]*vec[3];
 402         vec[2] = x*mat[0][2] + y*mat[1][2] + z*mat[2][2] + mat[3][2]*vec[3];
 403         vec[3] = x*mat[0][3] + y*mat[1][3] + z*mat[2][3] + mat[3][3]*vec[3];
 404 }
 405
 406 /* ------------------------------------------------------------------------- */
 407
 408 void MTC_Mat4CpyMat3nc(float m1[][4], float m2[][3])    /* no clear */
 409 {
 410         m1[0][0]= m2[0][0];
 411         m1[0][1]= m2[0][1];
 412         m1[0][2]= m2[0][2];
 413
 414         m1[1][0]= m2[1][0];
 415         m1[1][1]= m2[1][1];
 416         m1[1][2]= m2[1][2];
 417
 418         m1[2][0]= m2[2][0];
 419         m1[2][1]= m2[2][1];
 420         m1[2][2]= m2[2][2];
 421 }
 422
 423 /* ------------------------------------------------------------------------- */
 424
 425 void MTC_Mat4MulMat33(float m1[][3], float m2[][4], float m3[][3])
 426 {
 427         /* m1_i_j = m2_i_k * m3_k_j ? */
 428
 429         m1[0][0] = m2[0][0]*m3[0][0] + m2[0][1]*m3[1][0] + m2[0][2]*m3[2][0];
 430         m1[0][1] = m2[0][0]*m3[0][1] + m2[0][1]*m3[1][1] + m2[0][2]*m3[2][1];
 431         m1[0][2] = m2[0][0]*m3[0][2] + m2[0][1]*m3[1][2] + m2[0][2]*m3[2][2];
 432
 433         m1[1][0] = m2[1][0]*m3[0][0] + m2[1][1]*m3[1][0] + m2[1][2]*m3[2][0];
 434         m1[1][1] = m2[1][0]*m3[0][1] + m2[1][1]*m3[1][1] + m2[1][2]*m3[2][1];
 435         m1[1][2] = m2[1][0]*m3[0][2] + m2[1][1]*m3[1][2] + m2[1][2]*m3[2][2];
 436
 437         m1[2][0] = m2[2][0]*m3[0][0] + m2[2][1]*m3[1][0] + m2[2][2]*m3[2][0];
 438         m1[2][1] = m2[2][0]*m3[0][1] + m2[2][1]*m3[1][1] + m2[2][2]*m3[2][1];
 439         m1[2][2] = m2[2][0]*m3[0][2] + m2[2][1]*m3[1][2] + m2[2][2]*m3[2][2];
 440
 441 }
 442
 443 /* ------------------------------------------------------------------------- */
 444
 445 /* eof */