source/blender/blenlib/intern/math_matrix.c

   1 /*
   2  * $Id$
   3  *
   4  * ***** BEGIN GPL LICENSE BLOCK *****
   5  *
   6  * This program is free software; you can redistribute it and/or
   7  * modify it under the terms of the GNU General Public License
   8  * as published by the Free Software Foundation; either version 2
   9  * of the License, or (at your option) any later version.
  10  *
  11  * This program is distributed in the hope that it will be useful,
  12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14  * GNU General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU General Public License
  17  * along with this program; if not, write to the Free Software Foundation,
  18  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
  19  *
  20  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
  21  * All rights reserved.
  22  *
  23  * The Original Code is: some of this file.
  24  *
  25  * ***** END GPL LICENSE BLOCK *****
  26  */
  27
  28 /** \file blender/blenlib/intern/math_matrix.c
  29  *  \ingroup bli
  30  */
  31
  32
  33 #include <assert.h>
  34 #include "BLI_math.h"
  35
  36 /********************************* Init **************************************/
  37
  38 void zero_m3(float m[3][3])
  39 {
  40         memset(m, 0, 3*3*sizeof(float));
  41 }
  42
  43 void zero_m4(float m[4][4])
  44 {
  45         memset(m, 0, 4*4*sizeof(float));
  46 }
  47
  48 void unit_m3(float m[][3])
  49 {
  50         m[0][0]= m[1][1]= m[2][2]= 1.0;
  51         m[0][1]= m[0][2]= 0.0;
  52         m[1][0]= m[1][2]= 0.0;
  53         m[2][0]= m[2][1]= 0.0;
  54 }
  55
  56 void unit_m4(float m[][4])
  57 {
  58         m[0][0]= m[1][1]= m[2][2]= m[3][3]= 1.0;
  59         m[0][1]= m[0][2]= m[0][3]= 0.0;
  60         m[1][0]= m[1][2]= m[1][3]= 0.0;
  61         m[2][0]= m[2][1]= m[2][3]= 0.0;
  62         m[3][0]= m[3][1]= m[3][2]= 0.0;
  63 }
  64
  65 void copy_m3_m3(float m1[][3], float m2[][3])
  66 {
  67         /* destination comes first: */
  68         memcpy(&m1[0], &m2[0], 9*sizeof(float));
  69 }
  70
  71 void copy_m4_m4(float m1[][4], float m2[][4])
  72 {
  73         memcpy(m1, m2, 4*4*sizeof(float));
  74 }
  75
  76 void copy_m3_m4(float m1[][3], float m2[][4])
  77 {
  78         m1[0][0]= m2[0][0];
  79         m1[0][1]= m2[0][1];
  80         m1[0][2]= m2[0][2];
  81
  82         m1[1][0]= m2[1][0];
  83         m1[1][1]= m2[1][1];
  84         m1[1][2]= m2[1][2];
  85
  86         m1[2][0]= m2[2][0];
  87         m1[2][1]= m2[2][1];
  88         m1[2][2]= m2[2][2];
  89 }
  90
  91 void copy_m4_m3(float m1[][4], float m2[][3])   /* no clear */
  92 {
  93         m1[0][0]= m2[0][0];
  94         m1[0][1]= m2[0][1];
  95         m1[0][2]= m2[0][2];
  96
  97         m1[1][0]= m2[1][0];
  98         m1[1][1]= m2[1][1];
  99         m1[1][2]= m2[1][2];
 100
 101         m1[2][0]= m2[2][0];
 102         m1[2][1]= m2[2][1];
 103         m1[2][2]= m2[2][2];
 104
 105         /*      Reevan's Bugfix */
 106         m1[0][3]=0.0F;
 107         m1[1][3]=0.0F;
 108         m1[2][3]=0.0F;
 109
 110         m1[3][0]=0.0F;
 111         m1[3][1]=0.0F;
 112         m1[3][2]=0.0F;
 113         m1[3][3]=1.0F;
 114
 115 }
 116
 117 void swap_m3m3(float m1[][3], float m2[][3])
 118 {
 119         float t;
 120         int i, j;
 121
 122         for(i = 0; i < 3; i++) {
 123                 for (j = 0; j < 3; j++) {
 124                         t        = m1[i][j];
 125                         m1[i][j] = m2[i][j];
 126                         m2[i][j] = t;
 127                 }
 128         }
 129 }
 130
 131 void swap_m4m4(float m1[][4], float m2[][4])
 132 {
 133         float t;
 134         int i, j;
 135
 136         for(i = 0; i < 4; i++) {
 137                 for (j = 0; j < 4; j++) {
 138                         t        = m1[i][j];
 139                         m1[i][j] = m2[i][j];
 140                         m2[i][j] = t;
 141                 }
 142         }
 143 }
 144
 145 /******************************** Arithmetic *********************************/
 146
 147 void mul_m4_m4m4(float m1[][4], float m2_[][4], float m3_[][4])
 148 {
 149         float m2[4][4], m3[4][4];
 150
 151         /* copy so it works when m1 is the same pointer as m2 or m3 */
 152         copy_m4_m4(m2, m2_);
 153         copy_m4_m4(m3, m3_);
 154
 155         /* matrix product: m1[j][k] = m2[j][i].m3[i][k] */
 156         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];
 157         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];
 158         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];
 159         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];
 160
 161         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];
 162         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];
 163         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];
 164         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];
 165
 166         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];
 167         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];
 168         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];
 169         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];
 170
 171         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];
 172         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];
 173         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];
 174         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];
 175
 176 }
 177
 178 void mul_m3_m3m3(float m1[][3], float m3_[][3], float m2_[][3])
 179 {
 180         float m2[3][3], m3[3][3];
 181
 182         /* copy so it works when m1 is the same pointer as m2 or m3 */
 183         copy_m3_m3(m2, m2_);
 184         copy_m3_m3(m3, m3_);
 185
 186         /* m1[i][j] = m2[i][k]*m3[k][j], args are flipped!  */
 187         m1[0][0]= m2[0][0]*m3[0][0] + m2[0][1]*m3[1][0] + m2[0][2]*m3[2][0];
 188         m1[0][1]= m2[0][0]*m3[0][1] + m2[0][1]*m3[1][1] + m2[0][2]*m3[2][1];
 189         m1[0][2]= m2[0][0]*m3[0][2] + m2[0][1]*m3[1][2] + m2[0][2]*m3[2][2];
 190
 191         m1[1][0]= m2[1][0]*m3[0][0] + m2[1][1]*m3[1][0] + m2[1][2]*m3[2][0];
 192         m1[1][1]= m2[1][0]*m3[0][1] + m2[1][1]*m3[1][1] + m2[1][2]*m3[2][1];
 193         m1[1][2]= m2[1][0]*m3[0][2] + m2[1][1]*m3[1][2] + m2[1][2]*m3[2][2];
 194
 195         m1[2][0]= m2[2][0]*m3[0][0] + m2[2][1]*m3[1][0] + m2[2][2]*m3[2][0];
 196         m1[2][1]= m2[2][0]*m3[0][1] + m2[2][1]*m3[1][1] + m2[2][2]*m3[2][1];
 197         m1[2][2]= m2[2][0]*m3[0][2] + m2[2][1]*m3[1][2] + m2[2][2]*m3[2][2];
 198 }
 199
 200 void mul_m4_m4m3(float (*m1)[4], float (*m3)[4], float (*m2)[3])
 201 {
 202         m1[0][0]= m2[0][0]*m3[0][0] + m2[0][1]*m3[1][0] + m2[0][2]*m3[2][0];
 203         m1[0][1]= m2[0][0]*m3[0][1] + m2[0][1]*m3[1][1] + m2[0][2]*m3[2][1];
 204         m1[0][2]= m2[0][0]*m3[0][2] + m2[0][1]*m3[1][2] + m2[0][2]*m3[2][2];
 205         m1[1][0]= m2[1][0]*m3[0][0] + m2[1][1]*m3[1][0] + m2[1][2]*m3[2][0];
 206         m1[1][1]= m2[1][0]*m3[0][1] + m2[1][1]*m3[1][1] + m2[1][2]*m3[2][1];
 207         m1[1][2]= m2[1][0]*m3[0][2] + m2[1][1]*m3[1][2] + m2[1][2]*m3[2][2];
 208         m1[2][0]= m2[2][0]*m3[0][0] + m2[2][1]*m3[1][0] + m2[2][2]*m3[2][0];
 209         m1[2][1]= m2[2][0]*m3[0][1] + m2[2][1]*m3[1][1] + m2[2][2]*m3[2][1];
 210         m1[2][2]= m2[2][0]*m3[0][2] + m2[2][1]*m3[1][2] + m2[2][2]*m3[2][2];
 211 }
 212
 213 /* m1 = m2 * m3, ignore the elements on the 4th row/column of m3*/
 214 void mul_m3_m3m4(float m1[][3], float m2[][3], float m3[][4])
 215 {
 216         /* m1[i][j] = m2[i][k] * m3[k][j] */
 217         m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] +m2[0][2] * m3[2][0];
 218         m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] +m2[0][2] * m3[2][1];
 219         m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] +m2[0][2] * m3[2][2];
 220
 221         m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] +m2[1][2] * m3[2][0];
 222         m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] +m2[1][2] * m3[2][1];
 223         m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] +m2[1][2] * m3[2][2];
 224
 225         m1[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] +m2[2][2] * m3[2][0];
 226         m1[2][1] = m2[2][0] * m3[0][1] + m2[2][1] * m3[1][1] +m2[2][2] * m3[2][1];
 227         m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] +m2[2][2] * m3[2][2];
 228 }
 229
 230 void mul_m4_m3m4(float (*m1)[4], float (*m3)[3], float (*m2)[4])
 231 {
 232         m1[0][0]= m2[0][0]*m3[0][0] + m2[0][1]*m3[1][0] + m2[0][2]*m3[2][0];
 233         m1[0][1]= m2[0][0]*m3[0][1] + m2[0][1]*m3[1][1] + m2[0][2]*m3[2][1];
 234         m1[0][2]= m2[0][0]*m3[0][2] + m2[0][1]*m3[1][2] + m2[0][2]*m3[2][2];
 235         m1[1][0]= m2[1][0]*m3[0][0] + m2[1][1]*m3[1][0] + m2[1][2]*m3[2][0];
 236         m1[1][1]= m2[1][0]*m3[0][1] + m2[1][1]*m3[1][1] + m2[1][2]*m3[2][1];
 237         m1[1][2]= m2[1][0]*m3[0][2] + m2[1][1]*m3[1][2] + m2[1][2]*m3[2][2];
 238         m1[2][0]= m2[2][0]*m3[0][0] + m2[2][1]*m3[1][0] + m2[2][2]*m3[2][0];
 239         m1[2][1]= m2[2][0]*m3[0][1] + m2[2][1]*m3[1][1] + m2[2][2]*m3[2][1];
 240         m1[2][2]= m2[2][0]*m3[0][2] + m2[2][1]*m3[1][2] + m2[2][2]*m3[2][2];
 241 }
 242
 243 void mul_serie_m3(float answ[][3],
 244                                    float m1[][3], float m2[][3], float m3[][3],
 245                                    float m4[][3], float m5[][3], float m6[][3],
 246                                    float m7[][3], float m8[][3])
 247 {
 248         float temp[3][3];
 249
 250         if(m1==NULL || m2==NULL) return;
 251
 252         mul_m3_m3m3(answ, m2, m1);
 253         if(m3) {
 254                 mul_m3_m3m3(temp, m3, answ);
 255                 if(m4) {
 256                         mul_m3_m3m3(answ, m4, temp);
 257                         if(m5) {
 258                                 mul_m3_m3m3(temp, m5, answ);
 259                                 if(m6) {
 260                                         mul_m3_m3m3(answ, m6, temp);
 261                                         if(m7) {
 262                                                 mul_m3_m3m3(temp, m7, answ);
 263                                                 if(m8) {
 264                                                         mul_m3_m3m3(answ, m8, temp);
 265                                                 }
 266                                                 else copy_m3_m3(answ, temp);
 267                                         }
 268                                 }
 269                                 else copy_m3_m3(answ, temp);
 270                         }
 271                 }
 272                 else copy_m3_m3(answ, temp);
 273         }
 274 }
 275
 276 void mul_serie_m4(float answ[][4], float m1[][4],
 277                                 float m2[][4], float m3[][4], float m4[][4],
 278                                 float m5[][4], float m6[][4], float m7[][4],
 279                                 float m8[][4])
 280 {
 281         float temp[4][4];
 282
 283         if(m1==NULL || m2==NULL) return;
 284
 285         mul_m4_m4m4(answ, m2, m1);
 286         if(m3) {
 287                 mul_m4_m4m4(temp, m3, answ);
 288                 if(m4) {
 289                         mul_m4_m4m4(answ, m4, temp);
 290                         if(m5) {
 291                                 mul_m4_m4m4(temp, m5, answ);
 292                                 if(m6) {
 293                                         mul_m4_m4m4(answ, m6, temp);
 294                                         if(m7) {
 295                                                 mul_m4_m4m4(temp, m7, answ);
 296                                                 if(m8) {
 297                                                         mul_m4_m4m4(answ, m8, temp);
 298                                                 }
 299                                                 else copy_m4_m4(answ, temp);
 300                                         }
 301                                 }
 302                                 else copy_m4_m4(answ, temp);
 303                         }
 304                 }
 305                 else copy_m4_m4(answ, temp);
 306         }
 307 }
 308
 309 void mul_m4_v3(float mat[][4], float *vec)
 310 {
 311         float x,y;
 312
 313         x=vec[0];
 314         y=vec[1];
 315         vec[0]=x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2] + mat[3][0];
 316         vec[1]=x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2] + mat[3][1];
 317         vec[2]=x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2] + mat[3][2];
 318 }
 319
 320 void mul_v3_m4v3(float *in, float mat[][4], float *vec)
 321 {
 322         float x,y;
 323
 324         x=vec[0];
 325         y=vec[1];
 326         in[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2] + mat[3][0];
 327         in[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2] + mat[3][1];
 328         in[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2] + mat[3][2];
 329 }
 330
 331 /* same as mul_m4_v3() but doesnt apply translation component */
 332 void mul_mat3_m4_v3(float mat[][4], float *vec)
 333 {
 334         float x,y;
 335
 336         x= vec[0];
 337         y= vec[1];
 338         vec[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2];
 339         vec[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2];
 340         vec[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2];
 341 }
 342
 343 void mul_project_m4_v3(float mat[][4], float vec[3])
 344 {
 345         const float w= vec[0]*mat[0][3] + vec[1]*mat[1][3] + vec[2]*mat[2][3] + mat[3][3];
 346         mul_m4_v3(mat, vec);
 347
 348         vec[0] /= w;
 349         vec[1] /= w;
 350         vec[2] /= w;
 351 }
 352
 353 void mul_v4_m4v4(float r[4], float mat[4][4], float v[4])
 354 {
 355         float x, y, z;
 356
 357         x= v[0];
 358         y= v[1];
 359         z= v[2];
 360
 361         r[0]= x*mat[0][0] + y*mat[1][0] + z*mat[2][0] + mat[3][0]*v[3];
 362         r[1]= x*mat[0][1] + y*mat[1][1] + z*mat[2][1] + mat[3][1]*v[3];
 363         r[2]= x*mat[0][2] + y*mat[1][2] + z*mat[2][2] + mat[3][2]*v[3];
 364         r[3]= x*mat[0][3] + y*mat[1][3] + z*mat[2][3] + mat[3][3]*v[3];
 365 }
 366
 367 void mul_m4_v4(float mat[4][4], float r[4])
 368 {
 369         mul_v4_m4v4(r, mat, r);
 370 }
 371
 372 void mul_v3_m3v3(float r[3], float M[3][3], float a[3])
 373 {
 374         r[0]= M[0][0]*a[0] + M[1][0]*a[1] + M[2][0]*a[2];
 375         r[1]= M[0][1]*a[0] + M[1][1]*a[1] + M[2][1]*a[2];
 376         r[2]= M[0][2]*a[0] + M[1][2]*a[1] + M[2][2]*a[2];
 377 }
 378
 379 void mul_m3_v3(float M[3][3], float r[3])
 380 {
 381         float tmp[3];
 382
 383         mul_v3_m3v3(tmp, M, r);
 384         copy_v3_v3(r, tmp);
 385 }
 386
 387 void mul_transposed_m3_v3(float mat[][3], float *vec)
 388 {
 389         float x,y;
 390
 391         x=vec[0];
 392         y=vec[1];
 393         vec[0]= x*mat[0][0] + y*mat[0][1] + mat[0][2]*vec[2];
 394         vec[1]= x*mat[1][0] + y*mat[1][1] + mat[1][2]*vec[2];
 395         vec[2]= x*mat[2][0] + y*mat[2][1] + mat[2][2]*vec[2];
 396 }
 397
 398 void mul_m3_fl(float m[3][3], float f)
 399 {
 400         int i, j;
 401
 402         for(i=0;i<3;i++)
 403                 for(j=0;j<3;j++)
 404                         m[i][j] *= f;
 405 }
 406
 407 void mul_m4_fl(float m[4][4], float f)
 408 {
 409         int i, j;
 410
 411         for(i=0;i<4;i++)
 412                 for(j=0;j<4;j++)
 413                         m[i][j] *= f;
 414 }
 415
 416 void mul_mat3_m4_fl(float m[4][4], float f)
 417 {
 418         int i, j;
 419
 420         for(i=0; i<3; i++)
 421                 for(j=0; j<3; j++)
 422                         m[i][j] *= f;
 423 }
 424
 425 void mul_m3_v3_double(float mat[][3], double *vec)
 426 {
 427         double x,y;
 428
 429         x=vec[0];
 430         y=vec[1];
 431         vec[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2];
 432         vec[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2];
 433         vec[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2];
 434 }
 435
 436 void add_m3_m3m3(float m1[][3], float m2[][3], float m3[][3])
 437 {
 438         int i, j;
 439
 440         for(i=0;i<3;i++)
 441                 for(j=0;j<3;j++)
 442                         m1[i][j]= m2[i][j] + m3[i][j];
 443 }
 444
 445 void add_m4_m4m4(float m1[][4], float m2[][4], float m3[][4])
 446 {
 447         int i, j;
 448
 449         for(i=0;i<4;i++)
 450                 for(j=0;j<4;j++)
 451                         m1[i][j]= m2[i][j] + m3[i][j];
 452 }
 453
 454 int invert_m3(float m[3][3])
 455 {
 456         float tmp[3][3];
 457         int success;
 458
 459         success= invert_m3_m3(tmp, m);
 460         copy_m3_m3(m, tmp);
 461
 462         return success;
 463 }
 464
 465 int invert_m3_m3(float m1[3][3], float m2[3][3])
 466 {
 467         float det;
 468         int a, b, success;
 469
 470         /* calc adjoint */
 471         adjoint_m3_m3(m1,m2);
 472
 473         /* then determinant old matrix! */
 474         det= m2[0][0]* (m2[1][1]*m2[2][2] - m2[1][2]*m2[2][1])
 475                 -m2[1][0]* (m2[0][1]*m2[2][2] - m2[0][2]*m2[2][1])
 476                 +m2[2][0]* (m2[0][1]*m2[1][2] - m2[0][2]*m2[1][1]);
 477
 478         success= (det != 0);
 479
 480         if(det==0) det=1;
 481         det= 1/det;
 482         for(a=0;a<3;a++) {
 483                 for(b=0;b<3;b++) {
 484                         m1[a][b]*=det;
 485                 }
 486         }
 487
 488         return success;
 489 }
 490
 491 int invert_m4(float m[4][4])
 492 {
 493         float tmp[4][4];
 494         int success;
 495
 496         success= invert_m4_m4(tmp, m);
 497         copy_m4_m4(m, tmp);
 498
 499         return success;
 500 }
 501
 502 /*
 503  * invertmat -
 504  *              computes the inverse of mat and puts it in inverse.  Returns
 505  *      TRUE on success (i.e. can always find a pivot) and FALSE on failure.
 506  *      Uses Gaussian Elimination with partial (maximal column) pivoting.
 507  *
 508  *                                      Mark Segal - 1992
 509  */
 510
 511 int invert_m4_m4(float inverse[4][4], float mat[4][4])
 512 {
 513         int i, j, k;
 514         double temp;
 515         float tempmat[4][4];
 516         float max;
 517         int maxj;
 518
 519         /* Set inverse to identity */
 520         for (i=0; i<4; i++)
 521                 for (j=0; j<4; j++)
 522                         inverse[i][j] = 0;
 523         for (i=0; i<4; i++)
 524                 inverse[i][i] = 1;
 525
 526         /* Copy original matrix so we don't mess it up */
 527         for(i = 0; i < 4; i++)
 528                 for(j = 0; j <4; j++)
 529                         tempmat[i][j] = mat[i][j];
 530
 531         for(i = 0; i < 4; i++) {
 532                 /* Look for row with max pivot */
 533                 max = fabs(tempmat[i][i]);
 534                 maxj = i;
 535                 for(j = i + 1; j < 4; j++) {
 536                         if(fabsf(tempmat[j][i]) > max) {
 537                                 max = fabs(tempmat[j][i]);
 538                                 maxj = j;
 539                         }
 540                 }
 541                 /* Swap rows if necessary */
 542                 if (maxj != i) {
 543                         for(k = 0; k < 4; k++) {
 544                                 SWAP(float, tempmat[i][k], tempmat[maxj][k]);
 545                                 SWAP(float, inverse[i][k], inverse[maxj][k]);
 546                         }
 547                 }
 548
 549                 temp = tempmat[i][i];
 550                 if (temp == 0)
 551                         return 0;  /* No non-zero pivot */
 552                 for(k = 0; k < 4; k++) {
 553                         tempmat[i][k] = (float)(tempmat[i][k]/temp);
 554                         inverse[i][k] = (float)(inverse[i][k]/temp);
 555                 }
 556                 for(j = 0; j < 4; j++) {
 557                         if(j != i) {
 558                                 temp = tempmat[j][i];
 559                                 for(k = 0; k < 4; k++) {
 560                                         tempmat[j][k] -= (float)(tempmat[i][k]*temp);
 561                                         inverse[j][k] -= (float)(inverse[i][k]*temp);
 562                                 }
 563                         }
 564                 }
 565         }
 566         return 1;
 567 }
 568
 569 /****************************** Linear Algebra *******************************/
 570
 571 void transpose_m3(float mat[][3])
 572 {
 573         float t;
 574
 575         t = mat[0][1] ;
 576         mat[0][1] = mat[1][0] ;
 577         mat[1][0] = t;
 578         t = mat[0][2] ;
 579         mat[0][2] = mat[2][0] ;
 580         mat[2][0] = t;
 581         t = mat[1][2] ;
 582         mat[1][2] = mat[2][1] ;
 583         mat[2][1] = t;
 584 }
 585
 586 void transpose_m4(float mat[][4])
 587 {
 588         float t;
 589
 590         t = mat[0][1] ;
 591         mat[0][1] = mat[1][0] ;
 592         mat[1][0] = t;
 593         t = mat[0][2] ;
 594         mat[0][2] = mat[2][0] ;
 595         mat[2][0] = t;
 596         t = mat[0][3] ;
 597         mat[0][3] = mat[3][0] ;
 598         mat[3][0] = t;
 599
 600         t = mat[1][2] ;
 601         mat[1][2] = mat[2][1] ;
 602         mat[2][1] = t;
 603         t = mat[1][3] ;
 604         mat[1][3] = mat[3][1] ;
 605         mat[3][1] = t;
 606
 607         t = mat[2][3] ;
 608         mat[2][3] = mat[3][2] ;
 609         mat[3][2] = t;
 610 }
 611
 612 void orthogonalize_m3(float mat[][3], int axis)
 613 {
 614         float size[3];
 615         mat3_to_size(size, mat);
 616         normalize_v3(mat[axis]);
 617         switch(axis)
 618         {
 619                 case 0:
 620                         if (dot_v3v3(mat[0], mat[1]) < 1) {
 621                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
 622                                 normalize_v3(mat[2]);
 623                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
 624                         } else if (dot_v3v3(mat[0], mat[2]) < 1) {
 625                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
 626                                 normalize_v3(mat[1]);
 627                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
 628                         } else {
 629                                 float vec[3];
 630
 631                                 vec[0]= mat[0][1];
 632                                 vec[1]= mat[0][2];
 633                                 vec[2]= mat[0][0];
 634
 635                                 cross_v3_v3v3(mat[2], mat[0], vec);
 636                                 normalize_v3(mat[2]);
 637                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
 638                         }
 639                 case 1:
 640                         if (dot_v3v3(mat[1], mat[0]) < 1) {
 641                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
 642                                 normalize_v3(mat[2]);
 643                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
 644                         } else if (dot_v3v3(mat[0], mat[2]) < 1) {
 645                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
 646                                 normalize_v3(mat[0]);
 647                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
 648                         } else {
 649                                 float vec[3];
 650
 651                                 vec[0]= mat[1][1];
 652                                 vec[1]= mat[1][2];
 653                                 vec[2]= mat[1][0];
 654
 655                                 cross_v3_v3v3(mat[0], mat[1], vec);
 656                                 normalize_v3(mat[0]);
 657                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
 658                         }
 659                 case 2:
 660                         if (dot_v3v3(mat[2], mat[0]) < 1) {
 661                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
 662                                 normalize_v3(mat[1]);
 663                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
 664                         } else if (dot_v3v3(mat[2], mat[1]) < 1) {
 665                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
 666                                 normalize_v3(mat[0]);
 667                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
 668                         } else {
 669                                 float vec[3];
 670
 671                                 vec[0]= mat[2][1];
 672                                 vec[1]= mat[2][2];
 673                                 vec[2]= mat[2][0];
 674
 675                                 cross_v3_v3v3(mat[0], vec, mat[2]);
 676                                 normalize_v3(mat[0]);
 677                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
 678                         }
 679         }
 680         mul_v3_fl(mat[0], size[0]);
 681         mul_v3_fl(mat[1], size[1]);
 682         mul_v3_fl(mat[2], size[2]);
 683 }
 684
 685 void orthogonalize_m4(float mat[][4], int axis)
 686 {
 687         float size[3];
 688         mat4_to_size(size, mat);
 689         normalize_v3(mat[axis]);
 690         switch(axis)
 691         {
 692                 case 0:
 693                         if (dot_v3v3(mat[0], mat[1]) < 1) {
 694                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
 695                                 normalize_v3(mat[2]);
 696                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
 697                         } else if (dot_v3v3(mat[0], mat[2]) < 1) {
 698                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
 699                                 normalize_v3(mat[1]);
 700                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
 701                         } else {
 702                                 float vec[3];
 703
 704                                 vec[0]= mat[0][1];
 705                                 vec[1]= mat[0][2];
 706                                 vec[2]= mat[0][0];
 707
 708                                 cross_v3_v3v3(mat[2], mat[0], vec);
 709                                 normalize_v3(mat[2]);
 710                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
 711                         }
 712                 case 1:
 713                         normalize_v3(mat[0]);
 714                         if (dot_v3v3(mat[1], mat[0]) < 1) {
 715                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
 716                                 normalize_v3(mat[2]);
 717                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
 718                         } else if (dot_v3v3(mat[0], mat[2]) < 1) {
 719                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
 720                                 normalize_v3(mat[0]);
 721                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
 722                         } else {
 723                                 float vec[3];
 724
 725                                 vec[0]= mat[1][1];
 726                                 vec[1]= mat[1][2];
 727                                 vec[2]= mat[1][0];
 728
 729                                 cross_v3_v3v3(mat[0], mat[1], vec);
 730                                 normalize_v3(mat[0]);
 731                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
 732                         }
 733                 case 2:
 734                         if (dot_v3v3(mat[2], mat[0]) < 1) {
 735                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
 736                                 normalize_v3(mat[1]);
 737                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
 738                         } else if (dot_v3v3(mat[2], mat[1]) < 1) {
 739                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
 740                                 normalize_v3(mat[0]);
 741                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
 742                         } else {
 743                                 float vec[3];
 744
 745                                 vec[0]= mat[2][1];
 746                                 vec[1]= mat[2][2];
 747                                 vec[2]= mat[2][0];
 748
 749                                 cross_v3_v3v3(mat[0], vec, mat[2]);
 750                                 normalize_v3(mat[0]);
 751                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
 752                         }
 753         }
 754         mul_v3_fl(mat[0], size[0]);
 755         mul_v3_fl(mat[1], size[1]);
 756         mul_v3_fl(mat[2], size[2]);
 757 }
 758
 759 int is_orthogonal_m3(float mat[][3])
 760 {
 761         if (fabsf(dot_v3v3(mat[0], mat[1])) > 1.5f * FLT_EPSILON)
 762                 return 0;
 763
 764         if (fabsf(dot_v3v3(mat[1], mat[2])) > 1.5f * FLT_EPSILON)
 765                 return 0;
 766
 767         if (fabsf(dot_v3v3(mat[0], mat[2])) > 1.5f * FLT_EPSILON)
 768                 return 0;
 769
 770         return 1;
 771 }
 772
 773 int is_orthogonal_m4(float mat[][4])
 774 {
 775         if (fabsf(dot_v3v3(mat[0], mat[1])) > 1.5f * FLT_EPSILON)
 776                 return 0;
 777
 778         if (fabsf(dot_v3v3(mat[1], mat[2])) > 1.5f * FLT_EPSILON)
 779                 return 0;
 780
 781         if (fabsf(dot_v3v3(mat[0], mat[2])) > 1.5f * FLT_EPSILON)
 782                 return 0;
 783
 784         return 1;
 785 }
 786
 787 void normalize_m3(float mat[][3])
 788 {
 789         normalize_v3(mat[0]);
 790         normalize_v3(mat[1]);
 791         normalize_v3(mat[2]);
 792 }
 793
 794 void normalize_m3_m3(float rmat[][3], float mat[][3])
 795 {
 796         normalize_v3_v3(rmat[0], mat[0]);
 797         normalize_v3_v3(rmat[1], mat[1]);
 798         normalize_v3_v3(rmat[2], mat[2]);
 799 }
 800
 801
 802 void normalize_m4(float mat[][4])
 803 {
 804         float len;
 805
 806         len= normalize_v3(mat[0]);
 807         if(len!=0.0f) mat[0][3]/= len;
 808         len= normalize_v3(mat[1]);
 809         if(len!=0.0f) mat[1][3]/= len;
 810         len= normalize_v3(mat[2]);
 811         if(len!=0.0f) mat[2][3]/= len;
 812 }
 813
 814 void normalize_m4_m4(float rmat[][4], float mat[][4])
 815 {
 816         float len;
 817
 818         len= normalize_v3_v3(rmat[0], mat[0]);
 819         if(len!=0.0f) rmat[0][3]= mat[0][3] / len;
 820         len= normalize_v3_v3(rmat[1], mat[1]);
 821         if(len!=0.0f) rmat[1][3]= mat[1][3] / len;
 822         len= normalize_v3_v3(rmat[2], mat[2]);
 823         if(len!=0.0f) rmat[2][3]= mat[2][3] / len;
 824 }
 825
 826 void adjoint_m3_m3(float m1[][3], float m[][3])
 827 {
 828         m1[0][0]=m[1][1]*m[2][2]-m[1][2]*m[2][1];
 829         m1[0][1]= -m[0][1]*m[2][2]+m[0][2]*m[2][1];
 830         m1[0][2]=m[0][1]*m[1][2]-m[0][2]*m[1][1];
 831
 832         m1[1][0]= -m[1][0]*m[2][2]+m[1][2]*m[2][0];
 833         m1[1][1]=m[0][0]*m[2][2]-m[0][2]*m[2][0];
 834         m1[1][2]= -m[0][0]*m[1][2]+m[0][2]*m[1][0];
 835
 836         m1[2][0]=m[1][0]*m[2][1]-m[1][1]*m[2][0];
 837         m1[2][1]= -m[0][0]*m[2][1]+m[0][1]*m[2][0];
 838         m1[2][2]=m[0][0]*m[1][1]-m[0][1]*m[1][0];
 839 }
 840
 841 void adjoint_m4_m4(float out[][4], float in[][4])       /* out = ADJ(in) */
 842 {
 843         float a1, a2, a3, a4, b1, b2, b3, b4;
 844         float c1, c2, c3, c4, d1, d2, d3, d4;
 845
 846         a1= in[0][0];
 847         b1= in[0][1];
 848         c1= in[0][2];
 849         d1= in[0][3];
 850
 851         a2= in[1][0];
 852         b2= in[1][1];
 853         c2= in[1][2];
 854         d2= in[1][3];
 855
 856         a3= in[2][0];
 857         b3= in[2][1];
 858         c3= in[2][2];
 859         d3= in[2][3];
 860
 861         a4= in[3][0];
 862         b4= in[3][1];
 863         c4= in[3][2];
 864         d4= in[3][3];
 865
 866
 867         out[0][0]  =   determinant_m3(b2, b3, b4, c2, c3, c4, d2, d3, d4);
 868         out[1][0]  = - determinant_m3(a2, a3, a4, c2, c3, c4, d2, d3, d4);
 869         out[2][0]  =   determinant_m3(a2, a3, a4, b2, b3, b4, d2, d3, d4);
 870         out[3][0]  = - determinant_m3(a2, a3, a4, b2, b3, b4, c2, c3, c4);
 871
 872         out[0][1]  = - determinant_m3(b1, b3, b4, c1, c3, c4, d1, d3, d4);
 873         out[1][1]  =   determinant_m3(a1, a3, a4, c1, c3, c4, d1, d3, d4);
 874         out[2][1]  = - determinant_m3(a1, a3, a4, b1, b3, b4, d1, d3, d4);
 875         out[3][1]  =   determinant_m3(a1, a3, a4, b1, b3, b4, c1, c3, c4);
 876
 877         out[0][2]  =   determinant_m3(b1, b2, b4, c1, c2, c4, d1, d2, d4);
 878         out[1][2]  = - determinant_m3(a1, a2, a4, c1, c2, c4, d1, d2, d4);
 879         out[2][2]  =   determinant_m3(a1, a2, a4, b1, b2, b4, d1, d2, d4);
 880         out[3][2]  = - determinant_m3(a1, a2, a4, b1, b2, b4, c1, c2, c4);
 881
 882         out[0][3]  = - determinant_m3(b1, b2, b3, c1, c2, c3, d1, d2, d3);
 883         out[1][3]  =   determinant_m3(a1, a2, a3, c1, c2, c3, d1, d2, d3);
 884         out[2][3]  = - determinant_m3(a1, a2, a3, b1, b2, b3, d1, d2, d3);
 885         out[3][3]  =   determinant_m3(a1, a2, a3, b1, b2, b3, c1, c2, c3);
 886 }
 887
 888 float determinant_m2(float a,float b,float c,float d)
 889 {
 890
 891         return a*d - b*c;
 892 }
 893
 894 float determinant_m3(float a1, float a2, float a3,
 895                          float b1, float b2, float b3,
 896                          float c1, float c2, float c3)
 897 {
 898         float ans;
 899
 900         ans = a1 * determinant_m2(b2, b3, c2, c3)
 901                 - b1 * determinant_m2(a2, a3, c2, c3)
 902                 + c1 * determinant_m2(a2, a3, b2, b3);
 903
 904         return ans;
 905 }
 906
 907 float determinant_m4(float m[][4])
 908 {
 909         float ans;
 910         float a1,a2,a3,a4,b1,b2,b3,b4,c1,c2,c3,c4,d1,d2,d3,d4;
 911
 912         a1= m[0][0];
 913         b1= m[0][1];
 914         c1= m[0][2];
 915         d1= m[0][3];
 916
 917         a2= m[1][0];
 918         b2= m[1][1];
 919         c2= m[1][2];
 920         d2= m[1][3];
 921
 922         a3= m[2][0];
 923         b3= m[2][1];
 924         c3= m[2][2];
 925         d3= m[2][3];
 926
 927         a4= m[3][0];
 928         b4= m[3][1];
 929         c4= m[3][2];
 930         d4= m[3][3];
 931
 932         ans = a1 * determinant_m3(b2, b3, b4, c2, c3, c4, d2, d3, d4)
 933                 - b1 * determinant_m3(a2, a3, a4, c2, c3, c4, d2, d3, d4)
 934                 + c1 * determinant_m3(a2, a3, a4, b2, b3, b4, d2, d3, d4)
 935                 - d1 * determinant_m3(a2, a3, a4, b2, b3, b4, c2, c3, c4);
 936
 937         return ans;
 938 }
 939
 940 /****************************** Transformations ******************************/
 941
 942 void size_to_mat3(float mat[][3], const float size[3])
 943 {
 944         mat[0][0]= size[0];
 945         mat[0][1]= 0.0f;
 946         mat[0][2]= 0.0f;
 947         mat[1][1]= size[1];
 948         mat[1][0]= 0.0f;
 949         mat[1][2]= 0.0f;
 950         mat[2][2]= size[2];
 951         mat[2][1]= 0.0f;
 952         mat[2][0]= 0.0f;
 953 }
 954
 955 void size_to_mat4(float mat[][4], const float size[3])
 956 {
 957         float tmat[3][3];
 958
 959         size_to_mat3(tmat,size);
 960         unit_m4(mat);
 961         copy_m4_m3(mat, tmat);
 962 }
 963
 964 void mat3_to_size(float *size, float mat[][3])
 965 {
 966         size[0]= len_v3(mat[0]);
 967         size[1]= len_v3(mat[1]);
 968         size[2]= len_v3(mat[2]);
 969 }
 970
 971 void mat4_to_size(float *size, float mat[][4])
 972 {
 973         size[0]= len_v3(mat[0]);
 974         size[1]= len_v3(mat[1]);
 975         size[2]= len_v3(mat[2]);
 976 }
 977
 978 /* this gets the average scale of a matrix, only use when your scaling
 979  * data that has no idea of scale axis, examples are bone-envelope-radius
 980  * and curve radius */
 981 float mat3_to_scale(float mat[][3])
 982 {
 983         /* unit length vector */
 984         float unit_vec[3] = {0.577350269189626f, 0.577350269189626f, 0.577350269189626f};
 985         mul_m3_v3(mat, unit_vec);
 986         return len_v3(unit_vec);
 987 }
 988
 989 float mat4_to_scale(float mat[][4])
 990 {
 991         float tmat[3][3];
 992         copy_m3_m4(tmat, mat);
 993         return mat3_to_scale(tmat);
 994 }
 995
 996
 997 void mat3_to_rot_size(float rot[3][3], float size[3], float mat3[3][3])
 998 {
 999         float mat3_n[3][3];  /* mat3 -> normalized, 3x3 */
1000         float imat3_n[3][3]; /* mat3 -> normalized & inverted, 3x3 */
1001
1002         /* rotation & scale are linked, we need to create the mat's
1003          * for these together since they are related. */
1004
1005         /* so scale doesnt interfear with rotation [#24291] */
1006         /* note: this is a workaround for negative matrix not working for rotation conversion, FIXME */
1007         normalize_m3_m3(mat3_n, mat3);
1008         if(is_negative_m3(mat3)) {
1009                 negate_v3(mat3_n[0]);
1010                 negate_v3(mat3_n[1]);
1011                 negate_v3(mat3_n[2]);
1012         }
1013
1014         /* rotation */
1015         /* keep rot as a 3x3 matrix, the caller can convert into a quat or euler */
1016         copy_m3_m3(rot, mat3_n);
1017
1018         /* scale */
1019         /* note: mat4_to_size(ob->size, mat) fails for negative scale */
1020         invert_m3_m3(imat3_n, mat3_n);
1021         mul_m3_m3m3(mat3, imat3_n, mat3);
1022
1023         size[0]= mat3[0][0];
1024         size[1]= mat3[1][1];
1025         size[2]= mat3[2][2];
1026 }
1027
1028 void mat4_to_loc_rot_size(float loc[3], float rot[3][3], float size[3], float wmat[][4])
1029 {
1030         float mat3[3][3];    /* wmat -> 3x3 */
1031
1032         copy_m3_m4(mat3, wmat);
1033         mat3_to_rot_size(rot, size, mat3);
1034
1035         /* location */
1036         copy_v3_v3(loc, wmat[3]);
1037 }
1038
1039 void scale_m3_fl(float m[][3], float scale)
1040 {
1041         m[0][0]= m[1][1]= m[2][2]= scale;
1042         m[0][1]= m[0][2]= 0.0;
1043         m[1][0]= m[1][2]= 0.0;
1044         m[2][0]= m[2][1]= 0.0;
1045 }
1046
1047 void scale_m4_fl(float m[][4], float scale)
1048 {
1049         m[0][0]= m[1][1]= m[2][2]= scale;
1050         m[3][3]= 1.0;
1051         m[0][1]= m[0][2]= m[0][3]= 0.0;
1052         m[1][0]= m[1][2]= m[1][3]= 0.0;
1053         m[2][0]= m[2][1]= m[2][3]= 0.0;
1054         m[3][0]= m[3][1]= m[3][2]= 0.0;
1055 }
1056
1057 void translate_m4(float mat[][4],float Tx, float Ty, float Tz)
1058 {
1059         mat[3][0] += (Tx*mat[0][0] + Ty*mat[1][0] + Tz*mat[2][0]);
1060         mat[3][1] += (Tx*mat[0][1] + Ty*mat[1][1] + Tz*mat[2][1]);
1061         mat[3][2] += (Tx*mat[0][2] + Ty*mat[1][2] + Tz*mat[2][2]);
1062 }
1063
1064 void rotate_m4(float mat[][4], const char axis, const float angle)
1065 {
1066         int col;
1067         float temp[4]= {0.0f, 0.0f, 0.0f, 0.0f};
1068         float cosine, sine;
1069
1070         assert(axis >= 'X' && axis <= 'Z');
1071
1072         cosine = (float)cos(angle);
1073         sine = (float)sin(angle);
1074         switch(axis){
1075         case 'X':
1076                 for(col=0 ; col<4 ; col++)
1077                         temp[col] = cosine*mat[1][col] + sine*mat[2][col];
1078                 for(col=0 ; col<4 ; col++) {
1079                 mat[2][col] = - sine*mat[1][col] + cosine*mat[2][col];
1080                         mat[1][col] = temp[col];
1081         }
1082                 break;
1083
1084         case 'Y':
1085                 for(col=0 ; col<4 ; col++)
1086                         temp[col] = cosine*mat[0][col] - sine*mat[2][col];
1087                 for(col=0 ; col<4 ; col++) {
1088                         mat[2][col] = sine*mat[0][col] + cosine*mat[2][col];
1089                         mat[0][col] = temp[col];
1090                 }
1091         break;
1092
1093         case 'Z':
1094                 for(col=0 ; col<4 ; col++)
1095                         temp[col] = cosine*mat[0][col] + sine*mat[1][col];
1096                 for(col=0 ; col<4 ; col++) {
1097                         mat[1][col] = - sine*mat[0][col] + cosine*mat[1][col];
1098                         mat[0][col] = temp[col];
1099                 }
1100         break;
1101         }
1102 }
1103
1104 void blend_m3_m3m3(float out[][3], float dst[][3], float src[][3], const float srcweight)
1105 {
1106         float srot[3][3], drot[3][3];
1107         float squat[4], dquat[4], fquat[4];
1108         float ssize[3], dsize[3], fsize[3];
1109         float rmat[3][3], smat[3][3];
1110
1111         mat3_to_rot_size(drot, dsize, dst);
1112         mat3_to_rot_size(srot, ssize, src);
1113
1114         mat3_to_quat(dquat, drot);
1115         mat3_to_quat(squat, srot);
1116
1117         /* do blending */
1118         interp_qt_qtqt(fquat, dquat, squat, srcweight);
1119         interp_v3_v3v3(fsize, dsize, ssize, srcweight);
1120
1121         /* compose new matrix */
1122         quat_to_mat3(rmat,fquat);
1123         size_to_mat3(smat,fsize);
1124         mul_m3_m3m3(out, rmat, smat);
1125 }
1126
1127 void blend_m4_m4m4(float out[][4], float dst[][4], float src[][4], const float srcweight)
1128 {
1129         float sloc[3], dloc[3], floc[3];
1130         float srot[3][3], drot[3][3];
1131         float squat[4], dquat[4], fquat[4];
1132         float ssize[3], dsize[3], fsize[3];
1133
1134         mat4_to_loc_rot_size(dloc, drot, dsize, dst);
1135         mat4_to_loc_rot_size(sloc, srot, ssize, src);
1136
1137         mat3_to_quat(dquat, drot);
1138         mat3_to_quat(squat, srot);
1139
1140         /* do blending */
1141         interp_v3_v3v3(floc, dloc, sloc, srcweight);
1142         interp_qt_qtqt(fquat, dquat, squat, srcweight);
1143         interp_v3_v3v3(fsize, dsize, ssize, srcweight);
1144
1145         /* compose new matrix */
1146         loc_quat_size_to_mat4(out, floc, fquat, fsize);
1147 }
1148
1149
1150 int is_negative_m3(float mat[][3])
1151 {
1152         float vec[3];
1153         cross_v3_v3v3(vec, mat[0], mat[1]);
1154         return (dot_v3v3(vec, mat[2]) < 0.0f);
1155 }
1156
1157 int is_negative_m4(float mat[][4])
1158 {
1159         float vec[3];
1160         cross_v3_v3v3(vec, mat[0], mat[1]);
1161         return (dot_v3v3(vec, mat[2]) < 0.0f);
1162 }
1163
1164 /* make a 4x4 matrix out of 3 transform components */
1165 /* matrices are made in the order: scale * rot * loc */
1166 // TODO: need to have a version that allows for rotation order...
1167 void loc_eul_size_to_mat4(float mat[4][4], const float loc[3], const float eul[3], const float size[3])
1168 {
1169         float rmat[3][3], smat[3][3], tmat[3][3];
1170
1171         /* initialise new matrix */
1172         unit_m4(mat);
1173
1174         /* make rotation + scaling part */
1175         eul_to_mat3(rmat,eul);
1176         size_to_mat3(smat,size);
1177         mul_m3_m3m3(tmat, rmat, smat);
1178
1179         /* copy rot/scale part to output matrix*/
1180         copy_m4_m3(mat, tmat);
1181
1182         /* copy location to matrix */
1183         mat[3][0] = loc[0];
1184         mat[3][1] = loc[1];
1185         mat[3][2] = loc[2];
1186 }
1187
1188 /* make a 4x4 matrix out of 3 transform components */
1189 /* matrices are made in the order: scale * rot * loc */
1190 void loc_eulO_size_to_mat4(float mat[4][4], const float loc[3], const float eul[3], const float size[3], const short rotOrder)
1191 {
1192         float rmat[3][3], smat[3][3], tmat[3][3];
1193
1194         /* initialise new matrix */
1195         unit_m4(mat);
1196
1197         /* make rotation + scaling part */
1198         eulO_to_mat3(rmat,eul, rotOrder);
1199         size_to_mat3(smat,size);
1200         mul_m3_m3m3(tmat, rmat, smat);
1201
1202         /* copy rot/scale part to output matrix*/
1203         copy_m4_m3(mat, tmat);
1204
1205         /* copy location to matrix */
1206         mat[3][0] = loc[0];
1207         mat[3][1] = loc[1];
1208         mat[3][2] = loc[2];
1209 }
1210
1211
1212 /* make a 4x4 matrix out of 3 transform components */
1213 /* matrices are made in the order: scale * rot * loc */
1214 void loc_quat_size_to_mat4(float mat[4][4], const float loc[3], const float quat[4], const float size[3])
1215 {
1216         float rmat[3][3], smat[3][3], tmat[3][3];
1217
1218         /* initialise new matrix */
1219         unit_m4(mat);
1220
1221         /* make rotation + scaling part */
1222         quat_to_mat3(rmat,quat);
1223         size_to_mat3(smat,size);
1224         mul_m3_m3m3(tmat, rmat, smat);
1225
1226         /* copy rot/scale part to output matrix*/
1227         copy_m4_m3(mat, tmat);
1228
1229         /* copy location to matrix */
1230         mat[3][0] = loc[0];
1231         mat[3][1] = loc[1];
1232         mat[3][2] = loc[2];
1233 }
1234
1235 void loc_axisangle_size_to_mat4(float mat[4][4], const float loc[3], const float axis[3], const float angle, const float size[3])
1236 {
1237         float q[4];
1238         axis_angle_to_quat(q, axis, angle);
1239         loc_quat_size_to_mat4(mat, loc, q, size);
1240 }
1241
1242 /*********************************** Other ***********************************/
1243
1244 void print_m3(const char *str, float m[][3])
1245 {
1246         printf("%s\n", str);
1247         printf("%f %f %f\n",m[0][0],m[1][0],m[2][0]);
1248         printf("%f %f %f\n",m[0][1],m[1][1],m[2][1]);
1249         printf("%f %f %f\n",m[0][2],m[1][2],m[2][2]);
1250         printf("\n");
1251 }
1252
1253 void print_m4(const char *str, float m[][4])
1254 {
1255         printf("%s\n", str);
1256         printf("%f %f %f %f\n",m[0][0],m[1][0],m[2][0],m[3][0]);
1257         printf("%f %f %f %f\n",m[0][1],m[1][1],m[2][1],m[3][1]);
1258         printf("%f %f %f %f\n",m[0][2],m[1][2],m[2][2],m[3][2]);
1259         printf("%f %f %f %f\n",m[0][3],m[1][3],m[2][3],m[3][3]);
1260         printf("\n");
1261 }
1262
1263 /*********************************** SVD ************************************
1264  * from TNT matrix library
1265
1266  * Compute the Single Value Decomposition of an arbitrary matrix A
1267  * That is compute the 3 matrices U,W,V with U column orthogonal (m,n)
1268  * ,W a diagonal matrix and V an orthogonal square matrix s.t.
1269  * A = U.W.Vt. From this decomposition it is trivial to compute the
1270  * (pseudo-inverse) of A as Ainv = V.Winv.tranpose(U).
1271  */
1272
1273 void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4])
1274 {
1275         float A[4][4];
1276         float work1[4], work2[4];
1277         int m = 4;
1278         int n = 4;
1279         int maxiter = 200;
1280         int nu = minf(m,n);
1281
1282         float *work = work1;
1283         float *e = work2;
1284         float eps;
1285
1286         int i=0, j=0, k=0, p, pp, iter;
1287
1288         // Reduce A to bidiagonal form, storing the diagonal elements
1289         // in s and the super-diagonal elements in e.
1290
1291         int nct = minf(m-1,n);
1292         int nrt = maxf(0,minf(n-2,m));
1293
1294         copy_m4_m4(A, A_);
1295         zero_m4(U);
1296         zero_v4(s);
1297
1298         for (k = 0; k < maxf(nct,nrt); k++) {
1299                 if (k < nct) {
1300
1301                         // Compute the transformation for the k-th column and
1302                         // place the k-th diagonal in s[k].
1303                         // Compute 2-norm of k-th column without under/overflow.
1304                         s[k] = 0;
1305                         for (i = k; i < m; i++) {
1306                                 s[k] = hypotf(s[k],A[i][k]);
1307                         }
1308                         if (s[k] != 0.0f) {
1309                                 float invsk;
1310                                 if (A[k][k] < 0.0f) {
1311                                         s[k] = -s[k];
1312                                 }
1313                                 invsk = 1.0f/s[k];
1314                                 for (i = k; i < m; i++) {
1315                                         A[i][k] *= invsk;
1316                                 }
1317                                 A[k][k] += 1.0f;
1318                         }
1319                         s[k] = -s[k];
1320                 }
1321                 for (j = k+1; j < n; j++) {
1322                         if ((k < nct) && (s[k] != 0.0f))  {
1323
1324                         // Apply the transformation.
1325
1326                                 float t = 0;
1327                                 for (i = k; i < m; i++) {
1328                                         t += A[i][k]*A[i][j];
1329                                 }
1330                                 t = -t/A[k][k];
1331                                 for (i = k; i < m; i++) {
1332                                         A[i][j] += t*A[i][k];
1333                                 }
1334                         }
1335
1336                         // Place the k-th row of A into e for the
1337                         // subsequent calculation of the row transformation.
1338
1339                         e[j] = A[k][j];
1340                 }
1341                 if (k < nct) {
1342
1343                         // Place the transformation in U for subsequent back
1344                         // multiplication.
1345
1346                         for (i = k; i < m; i++)
1347                                 U[i][k] = A[i][k];
1348                 }
1349                 if (k < nrt) {
1350
1351                         // Compute the k-th row transformation and place the
1352                         // k-th super-diagonal in e[k].
1353                         // Compute 2-norm without under/overflow.
1354                         e[k] = 0;
1355                         for (i = k+1; i < n; i++) {
1356                                 e[k] = hypotf(e[k],e[i]);
1357                         }
1358                         if (e[k] != 0.0f) {
1359                                 float invek;
1360                                 if (e[k+1] < 0.0f) {
1361                                         e[k] = -e[k];
1362                                 }
1363                                 invek = 1.0f/e[k];
1364                                 for (i = k+1; i < n; i++) {
1365                                         e[i] *= invek;
1366                                 }
1367                                 e[k+1] += 1.0f;
1368                         }
1369                         e[k] = -e[k];
1370                         if ((k+1 < m) & (e[k] != 0.0f)) {
1371                                 float invek1;
1372
1373                         // Apply the transformation.
1374
1375                                 for (i = k+1; i < m; i++) {
1376                                         work[i] = 0.0f;
1377                                 }
1378                                 for (j = k+1; j < n; j++) {
1379                                         for (i = k+1; i < m; i++) {
1380                                                 work[i] += e[j]*A[i][j];
1381                                         }
1382                                 }
1383                                 invek1 = 1.0f/e[k+1];
1384                                 for (j = k+1; j < n; j++) {
1385                                         float t = -e[j]*invek1;
1386                                         for (i = k+1; i < m; i++) {
1387                                                 A[i][j] += t*work[i];
1388                                         }
1389                                 }
1390                         }
1391
1392                         // Place the transformation in V for subsequent
1393                         // back multiplication.
1394
1395                         for (i = k+1; i < n; i++)
1396                                 V[i][k] = e[i];
1397                 }
1398         }
1399
1400         // Set up the final bidiagonal matrix or order p.
1401
1402         p = minf(n,m+1);
1403         if (nct < n) {
1404                 s[nct] = A[nct][nct];
1405         }
1406         if (m < p) {
1407                 s[p-1] = 0.0f;
1408         }
1409         if (nrt+1 < p) {
1410                 e[nrt] = A[nrt][p-1];
1411         }
1412         e[p-1] = 0.0f;
1413
1414         // If required, generate U.
1415
1416         for (j = nct; j < nu; j++) {
1417                 for (i = 0; i < m; i++) {
1418                         U[i][j] = 0.0f;
1419                 }
1420                 U[j][j] = 1.0f;
1421         }
1422         for (k = nct-1; k >= 0; k--) {
1423                 if (s[k] != 0.0f) {
1424                         for (j = k+1; j < nu; j++) {
1425                                 float t = 0;
1426                                 for (i = k; i < m; i++) {
1427                                         t += U[i][k]*U[i][j];
1428                                 }
1429                                 t = -t/U[k][k];
1430                                 for (i = k; i < m; i++) {
1431                                         U[i][j] += t*U[i][k];
1432                                 }
1433                         }
1434                         for (i = k; i < m; i++ ) {
1435                                 U[i][k] = -U[i][k];
1436                         }
1437                         U[k][k] = 1.0f + U[k][k];
1438                         for (i = 0; i < k-1; i++) {
1439                                 U[i][k] = 0.0f;
1440                         }
1441                 } else {
1442                         for (i = 0; i < m; i++) {
1443                                 U[i][k] = 0.0f;
1444                         }
1445                         U[k][k] = 1.0f;
1446                 }
1447         }
1448
1449         // If required, generate V.
1450
1451         for (k = n-1; k >= 0; k--) {
1452                 if ((k < nrt) & (e[k] != 0.0f)) {
1453                         for (j = k+1; j < nu; j++) {
1454                                 float t = 0;
1455                                 for (i = k+1; i < n; i++) {
1456                                         t += V[i][k]*V[i][j];
1457                                 }
1458                                 t = -t/V[k+1][k];
1459                                 for (i = k+1; i < n; i++) {
1460                                         V[i][j] += t*V[i][k];
1461                                 }
1462                         }
1463                 }
1464                 for (i = 0; i < n; i++) {
1465                         V[i][k] = 0.0f;
1466                 }
1467                 V[k][k] = 1.0f;
1468         }
1469
1470         // Main iteration loop for the singular values.
1471
1472         pp = p-1;
1473         iter = 0;
1474         eps = powf(2.0f,-52.0f);
1475         while (p > 0) {
1476                 int kase=0;
1477
1478                 // Test for maximum iterations to avoid infinite loop
1479                 if(maxiter == 0)
1480                         break;
1481                 maxiter--;
1482
1483                 // This section of the program inspects for
1484                 // negligible elements in the s and e arrays.  On
1485                 // completion the variables kase and k are set as follows.
1486
1487                 // kase = 1       if s(p) and e[k-1] are negligible and k<p
1488                 // kase = 2       if s(k) is negligible and k<p
1489                 // kase = 3       if e[k-1] is negligible, k<p, and
1490                 //                                s(k), ..., s(p) are not negligible (qr step).
1491                 // kase = 4       if e(p-1) is negligible (convergence).
1492
1493                 for (k = p-2; k >= -1; k--) {
1494                         if (k == -1) {
1495                                 break;
1496                         }
1497                         if (fabsf(e[k]) <= eps*(fabsf(s[k]) + fabsf(s[k+1]))) {
1498                                 e[k] = 0.0f;
1499                                 break;
1500                         }
1501                 }
1502                 if (k == p-2) {
1503                         kase = 4;
1504                 } else {
1505                         int ks;
1506                         for (ks = p-1; ks >= k; ks--) {
1507                                 float t;
1508                                 if (ks == k) {
1509                                         break;
1510                                 }
1511                                 t = (ks != p ? fabsf(e[ks]) : 0.f) +
1512                                                           (ks != k+1 ? fabsf(e[ks-1]) : 0.0f);
1513                                 if (fabsf(s[ks]) <= eps*t)  {
1514                                         s[ks] = 0.0f;
1515                                         break;
1516                                 }
1517                         }
1518                         if (ks == k) {
1519                                 kase = 3;
1520                         } else if (ks == p-1) {
1521                                 kase = 1;
1522                         } else {
1523                                 kase = 2;
1524                                 k = ks;
1525                         }
1526                 }
1527                 k++;
1528
1529                 // Perform the task indicated by kase.
1530
1531                 switch (kase) {
1532
1533                         // Deflate negligible s(p).
1534
1535                         case 1: {
1536                                 float f = e[p-2];
1537                                 e[p-2] = 0.0f;
1538                                 for (j = p-2; j >= k; j--) {
1539                                         float t = hypotf(s[j],f);
1540                                         float invt = 1.0f/t;
1541                                         float cs = s[j]*invt;
1542                                         float sn = f*invt;
1543                                         s[j] = t;
1544                                         if (j != k) {
1545                                                 f = -sn*e[j-1];
1546                                                 e[j-1] = cs*e[j-1];
1547                                         }
1548
1549                                         for (i = 0; i < n; i++) {
1550                                                 t = cs*V[i][j] + sn*V[i][p-1];
1551                                                 V[i][p-1] = -sn*V[i][j] + cs*V[i][p-1];
1552                                                 V[i][j] = t;
1553                                         }
1554                                 }
1555                         }
1556                         break;
1557
1558                         // Split at negligible s(k).
1559
1560                         case 2: {
1561                                 float f = e[k-1];
1562                                 e[k-1] = 0.0f;
1563                                 for (j = k; j < p; j++) {
1564                                         float t = hypotf(s[j],f);
1565                                         float invt = 1.0f/t;
1566                                         float cs = s[j]*invt;
1567                                         float sn = f*invt;
1568                                         s[j] = t;
1569                                         f = -sn*e[j];
1570                                         e[j] = cs*e[j];
1571
1572                                         for (i = 0; i < m; i++) {
1573                                                 t = cs*U[i][j] + sn*U[i][k-1];
1574                                                 U[i][k-1] = -sn*U[i][j] + cs*U[i][k-1];
1575                                                 U[i][j] = t;
1576                                         }
1577                                 }
1578                         }
1579                         break;
1580
1581                         // Perform one qr step.
1582
1583                         case 3: {
1584
1585                                 // Calculate the shift.
1586
1587                                 float scale = maxf(maxf(maxf(maxf(
1588                                                   fabsf(s[p-1]),fabsf(s[p-2])),fabsf(e[p-2])),
1589                                                   fabsf(s[k])),fabsf(e[k]));
1590                                 float invscale = 1.0f/scale;
1591                                 float sp = s[p-1]*invscale;
1592                                 float spm1 = s[p-2]*invscale;
1593                                 float epm1 = e[p-2]*invscale;
1594                                 float sk = s[k]*invscale;
1595                                 float ek = e[k]*invscale;
1596                                 float b = ((spm1 + sp)*(spm1 - sp) + epm1*epm1)*0.5f;
1597                                 float c = (sp*epm1)*(sp*epm1);
1598                                 float shift = 0.0f;
1599                                 float f, g;
1600                                 if ((b != 0.0f) || (c != 0.0f)) {
1601                                         shift = sqrtf(b*b + c);
1602                                         if (b < 0.0f) {
1603                                                 shift = -shift;
1604                                         }
1605                                         shift = c/(b + shift);
1606                                 }
1607                                 f = (sk + sp)*(sk - sp) + shift;
1608                                 g = sk*ek;
1609
1610                                 // Chase zeros.
1611
1612                                 for (j = k; j < p-1; j++) {
1613                                         float t = hypotf(f,g);
1614                                         /* division by zero checks added to avoid NaN (brecht) */
1615                                         float cs = (t == 0.0f)? 0.0f: f/t;
1616                                         float sn = (t == 0.0f)? 0.0f: g/t;
1617                                         if (j != k) {
1618                                                 e[j-1] = t;
1619                                         }
1620                                         f = cs*s[j] + sn*e[j];
1621                                         e[j] = cs*e[j] - sn*s[j];
1622                                         g = sn*s[j+1];
1623                                         s[j+1] = cs*s[j+1];
1624
1625                                         for (i = 0; i < n; i++) {
1626                                                 t = cs*V[i][j] + sn*V[i][j+1];
1627                                                 V[i][j+1] = -sn*V[i][j] + cs*V[i][j+1];
1628                                                 V[i][j] = t;
1629                                         }
1630
1631                                         t = hypotf(f,g);
1632                                         /* division by zero checks added to avoid NaN (brecht) */
1633                                         cs = (t == 0.0f)? 0.0f: f/t;
1634                                         sn = (t == 0.0f)? 0.0f: g/t;
1635                                         s[j] = t;
1636                                         f = cs*e[j] + sn*s[j+1];
1637                                         s[j+1] = -sn*e[j] + cs*s[j+1];
1638                                         g = sn*e[j+1];
1639                                         e[j+1] = cs*e[j+1];
1640                                         if (j < m-1) {
1641                                                 for (i = 0; i < m; i++) {
1642                                                         t = cs*U[i][j] + sn*U[i][j+1];
1643                                                         U[i][j+1] = -sn*U[i][j] + cs*U[i][j+1];
1644                                                         U[i][j] = t;
1645                                                 }
1646                                         }
1647                                 }
1648                                 e[p-2] = f;
1649                                 iter = iter + 1;
1650                         }
1651                         break;
1652
1653                         // Convergence.
1654
1655                         case 4: {
1656
1657                                 // Make the singular values positive.
1658
1659                                 if (s[k] <= 0.0f) {
1660                                         s[k] = (s[k] < 0.0f ? -s[k] : 0.0f);
1661
1662                                         for (i = 0; i <= pp; i++)
1663                                                 V[i][k] = -V[i][k];
1664                                 }
1665
1666                                 // Order the singular values.
1667
1668                                 while (k < pp) {
1669                                         float t;
1670                                         if (s[k] >= s[k+1]) {
1671                                                 break;
1672                                         }
1673                                         t = s[k];
1674                                         s[k] = s[k+1];
1675                                         s[k+1] = t;
1676                                         if (k < n-1) {
1677                                                 for (i = 0; i < n; i++) {
1678                                                         t = V[i][k+1]; V[i][k+1] = V[i][k]; V[i][k] = t;
1679                                                 }
1680                                         }
1681                                         if (k < m-1) {
1682                                                 for (i = 0; i < m; i++) {
1683                                                         t = U[i][k+1]; U[i][k+1] = U[i][k]; U[i][k] = t;
1684                                                 }
1685                                         }
1686                                         k++;
1687                                 }
1688                                 iter = 0;
1689                                 p--;
1690                         }
1691                         break;
1692                 }
1693         }
1694 }
1695
1696 void pseudoinverse_m4_m4(float Ainv[4][4], float A[4][4], float epsilon)
1697 {
1698         /* compute moon-penrose pseudo inverse of matrix, singular values
1699            below epsilon are ignored for stability (truncated SVD) */
1700         float V[4][4], W[4], Wm[4][4], U[4][4];
1701         int i;
1702
1703         transpose_m4(A);
1704         svd_m4(V, W, U, A);
1705         transpose_m4(U);
1706         transpose_m4(V);
1707
1708         zero_m4(Wm);
1709         for(i=0; i<4; i++)
1710                 Wm[i][i]= (W[i] < epsilon)? 0.0f: 1.0f/W[i];
1711
1712         transpose_m4(V);
1713
1714         mul_serie_m4(Ainv, U, Wm, V, NULL, NULL, NULL, NULL, NULL);
1715 }