|
Blender
V2.59
|
00001 /* 00002 * $Id: math_matrix.c 36792 2011-05-20 10:09:03Z campbellbarton $ 00003 * 00004 * ***** BEGIN GPL LICENSE BLOCK ***** 00005 * 00006 * This program is free software; you can redistribute it and/or 00007 * modify it under the terms of the GNU General Public License 00008 * as published by the Free Software Foundation; either version 2 00009 * of the License, or (at your option) any later version. 00010 * 00011 * This program is distributed in the hope that it will be useful, 00012 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00013 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00014 * GNU General Public License for more details. 00015 * 00016 * You should have received a copy of the GNU General Public License 00017 * along with this program; if not, write to the Free Software Foundation, 00018 * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 00019 * 00020 * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. 00021 * All rights reserved. 00022 * 00023 * The Original Code is: some of this file. 00024 * 00025 * ***** END GPL LICENSE BLOCK ***** 00026 */ 00027 00033 #include <assert.h> 00034 #include "BLI_math.h" 00035 00036 /********************************* Init **************************************/ 00037 00038 void zero_m3(float m[3][3]) 00039 { 00040 memset(m, 0, 3*3*sizeof(float)); 00041 } 00042 00043 void zero_m4(float m[4][4]) 00044 { 00045 memset(m, 0, 4*4*sizeof(float)); 00046 } 00047 00048 void unit_m3(float m[][3]) 00049 { 00050 m[0][0]= m[1][1]= m[2][2]= 1.0; 00051 m[0][1]= m[0][2]= 0.0; 00052 m[1][0]= m[1][2]= 0.0; 00053 m[2][0]= m[2][1]= 0.0; 00054 } 00055 00056 void unit_m4(float m[][4]) 00057 { 00058 m[0][0]= m[1][1]= m[2][2]= m[3][3]= 1.0; 00059 m[0][1]= m[0][2]= m[0][3]= 0.0; 00060 m[1][0]= m[1][2]= m[1][3]= 0.0; 00061 m[2][0]= m[2][1]= m[2][3]= 0.0; 00062 m[3][0]= m[3][1]= m[3][2]= 0.0; 00063 } 00064 00065 void copy_m3_m3(float m1[][3], float m2[][3]) 00066 { 00067 /* destination comes first: */ 00068 memcpy(&m1[0], &m2[0], 9*sizeof(float)); 00069 } 00070 00071 void copy_m4_m4(float m1[][4], float m2[][4]) 00072 { 00073 memcpy(m1, m2, 4*4*sizeof(float)); 00074 } 00075 00076 void copy_m3_m4(float m1[][3], float m2[][4]) 00077 { 00078 m1[0][0]= m2[0][0]; 00079 m1[0][1]= m2[0][1]; 00080 m1[0][2]= m2[0][2]; 00081 00082 m1[1][0]= m2[1][0]; 00083 m1[1][1]= m2[1][1]; 00084 m1[1][2]= m2[1][2]; 00085 00086 m1[2][0]= m2[2][0]; 00087 m1[2][1]= m2[2][1]; 00088 m1[2][2]= m2[2][2]; 00089 } 00090 00091 void copy_m4_m3(float m1[][4], float m2[][3]) /* no clear */ 00092 { 00093 m1[0][0]= m2[0][0]; 00094 m1[0][1]= m2[0][1]; 00095 m1[0][2]= m2[0][2]; 00096 00097 m1[1][0]= m2[1][0]; 00098 m1[1][1]= m2[1][1]; 00099 m1[1][2]= m2[1][2]; 00100 00101 m1[2][0]= m2[2][0]; 00102 m1[2][1]= m2[2][1]; 00103 m1[2][2]= m2[2][2]; 00104 00105 /* Reevan's Bugfix */ 00106 m1[0][3]=0.0F; 00107 m1[1][3]=0.0F; 00108 m1[2][3]=0.0F; 00109 00110 m1[3][0]=0.0F; 00111 m1[3][1]=0.0F; 00112 m1[3][2]=0.0F; 00113 m1[3][3]=1.0F; 00114 00115 } 00116 00117 void swap_m3m3(float m1[][3], float m2[][3]) 00118 { 00119 float t; 00120 int i, j; 00121 00122 for(i = 0; i < 3; i++) { 00123 for (j = 0; j < 3; j++) { 00124 t = m1[i][j]; 00125 m1[i][j] = m2[i][j]; 00126 m2[i][j] = t; 00127 } 00128 } 00129 } 00130 00131 void swap_m4m4(float m1[][4], float m2[][4]) 00132 { 00133 float t; 00134 int i, j; 00135 00136 for(i = 0; i < 4; i++) { 00137 for (j = 0; j < 4; j++) { 00138 t = m1[i][j]; 00139 m1[i][j] = m2[i][j]; 00140 m2[i][j] = t; 00141 } 00142 } 00143 } 00144 00145 /******************************** Arithmetic *********************************/ 00146 00147 void mul_m4_m4m4(float m1[][4], float m2_[][4], float m3_[][4]) 00148 { 00149 float m2[4][4], m3[4][4]; 00150 00151 /* copy so it works when m1 is the same pointer as m2 or m3 */ 00152 copy_m4_m4(m2, m2_); 00153 copy_m4_m4(m3, m3_); 00154 00155 /* matrix product: m1[j][k] = m2[j][i].m3[i][k] */ 00156 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]; 00157 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]; 00158 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]; 00159 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]; 00160 00161 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]; 00162 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]; 00163 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]; 00164 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]; 00165 00166 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]; 00167 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]; 00168 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]; 00169 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]; 00170 00171 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]; 00172 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]; 00173 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]; 00174 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]; 00175 00176 } 00177 00178 void mul_m3_m3m3(float m1[][3], float m3_[][3], float m2_[][3]) 00179 { 00180 float m2[3][3], m3[3][3]; 00181 00182 /* copy so it works when m1 is the same pointer as m2 or m3 */ 00183 copy_m3_m3(m2, m2_); 00184 copy_m3_m3(m3, m3_); 00185 00186 /* m1[i][j] = m2[i][k]*m3[k][j], args are flipped! */ 00187 m1[0][0]= m2[0][0]*m3[0][0] + m2[0][1]*m3[1][0] + m2[0][2]*m3[2][0]; 00188 m1[0][1]= m2[0][0]*m3[0][1] + m2[0][1]*m3[1][1] + m2[0][2]*m3[2][1]; 00189 m1[0][2]= m2[0][0]*m3[0][2] + m2[0][1]*m3[1][2] + m2[0][2]*m3[2][2]; 00190 00191 m1[1][0]= m2[1][0]*m3[0][0] + m2[1][1]*m3[1][0] + m2[1][2]*m3[2][0]; 00192 m1[1][1]= m2[1][0]*m3[0][1] + m2[1][1]*m3[1][1] + m2[1][2]*m3[2][1]; 00193 m1[1][2]= m2[1][0]*m3[0][2] + m2[1][1]*m3[1][2] + m2[1][2]*m3[2][2]; 00194 00195 m1[2][0]= m2[2][0]*m3[0][0] + m2[2][1]*m3[1][0] + m2[2][2]*m3[2][0]; 00196 m1[2][1]= m2[2][0]*m3[0][1] + m2[2][1]*m3[1][1] + m2[2][2]*m3[2][1]; 00197 m1[2][2]= m2[2][0]*m3[0][2] + m2[2][1]*m3[1][2] + m2[2][2]*m3[2][2]; 00198 } 00199 00200 void mul_m4_m4m3(float (*m1)[4], float (*m3)[4], float (*m2)[3]) 00201 { 00202 m1[0][0]= m2[0][0]*m3[0][0] + m2[0][1]*m3[1][0] + m2[0][2]*m3[2][0]; 00203 m1[0][1]= m2[0][0]*m3[0][1] + m2[0][1]*m3[1][1] + m2[0][2]*m3[2][1]; 00204 m1[0][2]= m2[0][0]*m3[0][2] + m2[0][1]*m3[1][2] + m2[0][2]*m3[2][2]; 00205 m1[1][0]= m2[1][0]*m3[0][0] + m2[1][1]*m3[1][0] + m2[1][2]*m3[2][0]; 00206 m1[1][1]= m2[1][0]*m3[0][1] + m2[1][1]*m3[1][1] + m2[1][2]*m3[2][1]; 00207 m1[1][2]= m2[1][0]*m3[0][2] + m2[1][1]*m3[1][2] + m2[1][2]*m3[2][2]; 00208 m1[2][0]= m2[2][0]*m3[0][0] + m2[2][1]*m3[1][0] + m2[2][2]*m3[2][0]; 00209 m1[2][1]= m2[2][0]*m3[0][1] + m2[2][1]*m3[1][1] + m2[2][2]*m3[2][1]; 00210 m1[2][2]= m2[2][0]*m3[0][2] + m2[2][1]*m3[1][2] + m2[2][2]*m3[2][2]; 00211 } 00212 00213 /* m1 = m2 * m3, ignore the elements on the 4th row/column of m3*/ 00214 void mul_m3_m3m4(float m1[][3], float m2[][3], float m3[][4]) 00215 { 00216 /* m1[i][j] = m2[i][k] * m3[k][j] */ 00217 m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] +m2[0][2] * m3[2][0]; 00218 m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] +m2[0][2] * m3[2][1]; 00219 m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] +m2[0][2] * m3[2][2]; 00220 00221 m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] +m2[1][2] * m3[2][0]; 00222 m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] +m2[1][2] * m3[2][1]; 00223 m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] +m2[1][2] * m3[2][2]; 00224 00225 m1[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] +m2[2][2] * m3[2][0]; 00226 m1[2][1] = m2[2][0] * m3[0][1] + m2[2][1] * m3[1][1] +m2[2][2] * m3[2][1]; 00227 m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] +m2[2][2] * m3[2][2]; 00228 } 00229 00230 void mul_m4_m3m4(float (*m1)[4], float (*m3)[3], float (*m2)[4]) 00231 { 00232 m1[0][0]= m2[0][0]*m3[0][0] + m2[0][1]*m3[1][0] + m2[0][2]*m3[2][0]; 00233 m1[0][1]= m2[0][0]*m3[0][1] + m2[0][1]*m3[1][1] + m2[0][2]*m3[2][1]; 00234 m1[0][2]= m2[0][0]*m3[0][2] + m2[0][1]*m3[1][2] + m2[0][2]*m3[2][2]; 00235 m1[1][0]= m2[1][0]*m3[0][0] + m2[1][1]*m3[1][0] + m2[1][2]*m3[2][0]; 00236 m1[1][1]= m2[1][0]*m3[0][1] + m2[1][1]*m3[1][1] + m2[1][2]*m3[2][1]; 00237 m1[1][2]= m2[1][0]*m3[0][2] + m2[1][1]*m3[1][2] + m2[1][2]*m3[2][2]; 00238 m1[2][0]= m2[2][0]*m3[0][0] + m2[2][1]*m3[1][0] + m2[2][2]*m3[2][0]; 00239 m1[2][1]= m2[2][0]*m3[0][1] + m2[2][1]*m3[1][1] + m2[2][2]*m3[2][1]; 00240 m1[2][2]= m2[2][0]*m3[0][2] + m2[2][1]*m3[1][2] + m2[2][2]*m3[2][2]; 00241 } 00242 00243 void mul_serie_m3(float answ[][3], 00244 float m1[][3], float m2[][3], float m3[][3], 00245 float m4[][3], float m5[][3], float m6[][3], 00246 float m7[][3], float m8[][3]) 00247 { 00248 float temp[3][3]; 00249 00250 if(m1==NULL || m2==NULL) return; 00251 00252 mul_m3_m3m3(answ, m2, m1); 00253 if(m3) { 00254 mul_m3_m3m3(temp, m3, answ); 00255 if(m4) { 00256 mul_m3_m3m3(answ, m4, temp); 00257 if(m5) { 00258 mul_m3_m3m3(temp, m5, answ); 00259 if(m6) { 00260 mul_m3_m3m3(answ, m6, temp); 00261 if(m7) { 00262 mul_m3_m3m3(temp, m7, answ); 00263 if(m8) { 00264 mul_m3_m3m3(answ, m8, temp); 00265 } 00266 else copy_m3_m3(answ, temp); 00267 } 00268 } 00269 else copy_m3_m3(answ, temp); 00270 } 00271 } 00272 else copy_m3_m3(answ, temp); 00273 } 00274 } 00275 00276 void mul_serie_m4(float answ[][4], float m1[][4], 00277 float m2[][4], float m3[][4], float m4[][4], 00278 float m5[][4], float m6[][4], float m7[][4], 00279 float m8[][4]) 00280 { 00281 float temp[4][4]; 00282 00283 if(m1==NULL || m2==NULL) return; 00284 00285 mul_m4_m4m4(answ, m2, m1); 00286 if(m3) { 00287 mul_m4_m4m4(temp, m3, answ); 00288 if(m4) { 00289 mul_m4_m4m4(answ, m4, temp); 00290 if(m5) { 00291 mul_m4_m4m4(temp, m5, answ); 00292 if(m6) { 00293 mul_m4_m4m4(answ, m6, temp); 00294 if(m7) { 00295 mul_m4_m4m4(temp, m7, answ); 00296 if(m8) { 00297 mul_m4_m4m4(answ, m8, temp); 00298 } 00299 else copy_m4_m4(answ, temp); 00300 } 00301 } 00302 else copy_m4_m4(answ, temp); 00303 } 00304 } 00305 else copy_m4_m4(answ, temp); 00306 } 00307 } 00308 00309 void mul_m4_v3(float mat[][4], float *vec) 00310 { 00311 float x,y; 00312 00313 x=vec[0]; 00314 y=vec[1]; 00315 vec[0]=x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2] + mat[3][0]; 00316 vec[1]=x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2] + mat[3][1]; 00317 vec[2]=x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2] + mat[3][2]; 00318 } 00319 00320 void mul_v3_m4v3(float *in, float mat[][4], float *vec) 00321 { 00322 float x,y; 00323 00324 x=vec[0]; 00325 y=vec[1]; 00326 in[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2] + mat[3][0]; 00327 in[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2] + mat[3][1]; 00328 in[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2] + mat[3][2]; 00329 } 00330 00331 /* same as mul_m4_v3() but doesnt apply translation component */ 00332 void mul_mat3_m4_v3(float mat[][4], float *vec) 00333 { 00334 float x,y; 00335 00336 x= vec[0]; 00337 y= vec[1]; 00338 vec[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2]; 00339 vec[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2]; 00340 vec[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2]; 00341 } 00342 00343 void mul_project_m4_v3(float mat[][4], float vec[3]) 00344 { 00345 const float w= vec[0]*mat[0][3] + vec[1]*mat[1][3] + vec[2]*mat[2][3] + mat[3][3]; 00346 mul_m4_v3(mat, vec); 00347 00348 vec[0] /= w; 00349 vec[1] /= w; 00350 vec[2] /= w; 00351 } 00352 00353 void mul_v4_m4v4(float r[4], float mat[4][4], float v[4]) 00354 { 00355 float x, y, z; 00356 00357 x= v[0]; 00358 y= v[1]; 00359 z= v[2]; 00360 00361 r[0]= x*mat[0][0] + y*mat[1][0] + z*mat[2][0] + mat[3][0]*v[3]; 00362 r[1]= x*mat[0][1] + y*mat[1][1] + z*mat[2][1] + mat[3][1]*v[3]; 00363 r[2]= x*mat[0][2] + y*mat[1][2] + z*mat[2][2] + mat[3][2]*v[3]; 00364 r[3]= x*mat[0][3] + y*mat[1][3] + z*mat[2][3] + mat[3][3]*v[3]; 00365 } 00366 00367 void mul_m4_v4(float mat[4][4], float r[4]) 00368 { 00369 mul_v4_m4v4(r, mat, r); 00370 } 00371 00372 void mul_v3_m3v3(float r[3], float M[3][3], float a[3]) 00373 { 00374 r[0]= M[0][0]*a[0] + M[1][0]*a[1] + M[2][0]*a[2]; 00375 r[1]= M[0][1]*a[0] + M[1][1]*a[1] + M[2][1]*a[2]; 00376 r[2]= M[0][2]*a[0] + M[1][2]*a[1] + M[2][2]*a[2]; 00377 } 00378 00379 void mul_m3_v3(float M[3][3], float r[3]) 00380 { 00381 float tmp[3]; 00382 00383 mul_v3_m3v3(tmp, M, r); 00384 copy_v3_v3(r, tmp); 00385 } 00386 00387 void mul_transposed_m3_v3(float mat[][3], float *vec) 00388 { 00389 float x,y; 00390 00391 x=vec[0]; 00392 y=vec[1]; 00393 vec[0]= x*mat[0][0] + y*mat[0][1] + mat[0][2]*vec[2]; 00394 vec[1]= x*mat[1][0] + y*mat[1][1] + mat[1][2]*vec[2]; 00395 vec[2]= x*mat[2][0] + y*mat[2][1] + mat[2][2]*vec[2]; 00396 } 00397 00398 void mul_m3_fl(float m[3][3], float f) 00399 { 00400 int i, j; 00401 00402 for(i=0;i<3;i++) 00403 for(j=0;j<3;j++) 00404 m[i][j] *= f; 00405 } 00406 00407 void mul_m4_fl(float m[4][4], float f) 00408 { 00409 int i, j; 00410 00411 for(i=0;i<4;i++) 00412 for(j=0;j<4;j++) 00413 m[i][j] *= f; 00414 } 00415 00416 void mul_mat3_m4_fl(float m[4][4], float f) 00417 { 00418 int i, j; 00419 00420 for(i=0; i<3; i++) 00421 for(j=0; j<3; j++) 00422 m[i][j] *= f; 00423 } 00424 00425 void mul_m3_v3_double(float mat[][3], double *vec) 00426 { 00427 double x,y; 00428 00429 x=vec[0]; 00430 y=vec[1]; 00431 vec[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2]; 00432 vec[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2]; 00433 vec[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2]; 00434 } 00435 00436 void add_m3_m3m3(float m1[][3], float m2[][3], float m3[][3]) 00437 { 00438 int i, j; 00439 00440 for(i=0;i<3;i++) 00441 for(j=0;j<3;j++) 00442 m1[i][j]= m2[i][j] + m3[i][j]; 00443 } 00444 00445 void add_m4_m4m4(float m1[][4], float m2[][4], float m3[][4]) 00446 { 00447 int i, j; 00448 00449 for(i=0;i<4;i++) 00450 for(j=0;j<4;j++) 00451 m1[i][j]= m2[i][j] + m3[i][j]; 00452 } 00453 00454 int invert_m3(float m[3][3]) 00455 { 00456 float tmp[3][3]; 00457 int success; 00458 00459 success= invert_m3_m3(tmp, m); 00460 copy_m3_m3(m, tmp); 00461 00462 return success; 00463 } 00464 00465 int invert_m3_m3(float m1[3][3], float m2[3][3]) 00466 { 00467 float det; 00468 int a, b, success; 00469 00470 /* calc adjoint */ 00471 adjoint_m3_m3(m1,m2); 00472 00473 /* then determinant old matrix! */ 00474 det= m2[0][0]* (m2[1][1]*m2[2][2] - m2[1][2]*m2[2][1]) 00475 -m2[1][0]* (m2[0][1]*m2[2][2] - m2[0][2]*m2[2][1]) 00476 +m2[2][0]* (m2[0][1]*m2[1][2] - m2[0][2]*m2[1][1]); 00477 00478 success= (det != 0); 00479 00480 if(det==0) det=1; 00481 det= 1/det; 00482 for(a=0;a<3;a++) { 00483 for(b=0;b<3;b++) { 00484 m1[a][b]*=det; 00485 } 00486 } 00487 00488 return success; 00489 } 00490 00491 int invert_m4(float m[4][4]) 00492 { 00493 float tmp[4][4]; 00494 int success; 00495 00496 success= invert_m4_m4(tmp, m); 00497 copy_m4_m4(m, tmp); 00498 00499 return success; 00500 } 00501 00502 /* 00503 * invertmat - 00504 * computes the inverse of mat and puts it in inverse. Returns 00505 * TRUE on success (i.e. can always find a pivot) and FALSE on failure. 00506 * Uses Gaussian Elimination with partial (maximal column) pivoting. 00507 * 00508 * Mark Segal - 1992 00509 */ 00510 00511 int invert_m4_m4(float inverse[4][4], float mat[4][4]) 00512 { 00513 int i, j, k; 00514 double temp; 00515 float tempmat[4][4]; 00516 float max; 00517 int maxj; 00518 00519 /* Set inverse to identity */ 00520 for (i=0; i<4; i++) 00521 for (j=0; j<4; j++) 00522 inverse[i][j] = 0; 00523 for (i=0; i<4; i++) 00524 inverse[i][i] = 1; 00525 00526 /* Copy original matrix so we don't mess it up */ 00527 for(i = 0; i < 4; i++) 00528 for(j = 0; j <4; j++) 00529 tempmat[i][j] = mat[i][j]; 00530 00531 for(i = 0; i < 4; i++) { 00532 /* Look for row with max pivot */ 00533 max = fabs(tempmat[i][i]); 00534 maxj = i; 00535 for(j = i + 1; j < 4; j++) { 00536 if(fabsf(tempmat[j][i]) > max) { 00537 max = fabs(tempmat[j][i]); 00538 maxj = j; 00539 } 00540 } 00541 /* Swap rows if necessary */ 00542 if (maxj != i) { 00543 for(k = 0; k < 4; k++) { 00544 SWAP(float, tempmat[i][k], tempmat[maxj][k]); 00545 SWAP(float, inverse[i][k], inverse[maxj][k]); 00546 } 00547 } 00548 00549 temp = tempmat[i][i]; 00550 if (temp == 0) 00551 return 0; /* No non-zero pivot */ 00552 for(k = 0; k < 4; k++) { 00553 tempmat[i][k] = (float)(tempmat[i][k]/temp); 00554 inverse[i][k] = (float)(inverse[i][k]/temp); 00555 } 00556 for(j = 0; j < 4; j++) { 00557 if(j != i) { 00558 temp = tempmat[j][i]; 00559 for(k = 0; k < 4; k++) { 00560 tempmat[j][k] -= (float)(tempmat[i][k]*temp); 00561 inverse[j][k] -= (float)(inverse[i][k]*temp); 00562 } 00563 } 00564 } 00565 } 00566 return 1; 00567 } 00568 00569 /****************************** Linear Algebra *******************************/ 00570 00571 void transpose_m3(float mat[][3]) 00572 { 00573 float t; 00574 00575 t = mat[0][1] ; 00576 mat[0][1] = mat[1][0] ; 00577 mat[1][0] = t; 00578 t = mat[0][2] ; 00579 mat[0][2] = mat[2][0] ; 00580 mat[2][0] = t; 00581 t = mat[1][2] ; 00582 mat[1][2] = mat[2][1] ; 00583 mat[2][1] = t; 00584 } 00585 00586 void transpose_m4(float mat[][4]) 00587 { 00588 float t; 00589 00590 t = mat[0][1] ; 00591 mat[0][1] = mat[1][0] ; 00592 mat[1][0] = t; 00593 t = mat[0][2] ; 00594 mat[0][2] = mat[2][0] ; 00595 mat[2][0] = t; 00596 t = mat[0][3] ; 00597 mat[0][3] = mat[3][0] ; 00598 mat[3][0] = t; 00599 00600 t = mat[1][2] ; 00601 mat[1][2] = mat[2][1] ; 00602 mat[2][1] = t; 00603 t = mat[1][3] ; 00604 mat[1][3] = mat[3][1] ; 00605 mat[3][1] = t; 00606 00607 t = mat[2][3] ; 00608 mat[2][3] = mat[3][2] ; 00609 mat[3][2] = t; 00610 } 00611 00612 void orthogonalize_m3(float mat[][3], int axis) 00613 { 00614 float size[3]; 00615 mat3_to_size(size, mat); 00616 normalize_v3(mat[axis]); 00617 switch(axis) 00618 { 00619 case 0: 00620 if (dot_v3v3(mat[0], mat[1]) < 1) { 00621 cross_v3_v3v3(mat[2], mat[0], mat[1]); 00622 normalize_v3(mat[2]); 00623 cross_v3_v3v3(mat[1], mat[2], mat[0]); 00624 } else if (dot_v3v3(mat[0], mat[2]) < 1) { 00625 cross_v3_v3v3(mat[1], mat[2], mat[0]); 00626 normalize_v3(mat[1]); 00627 cross_v3_v3v3(mat[2], mat[0], mat[1]); 00628 } else { 00629 float vec[3]; 00630 00631 vec[0]= mat[0][1]; 00632 vec[1]= mat[0][2]; 00633 vec[2]= mat[0][0]; 00634 00635 cross_v3_v3v3(mat[2], mat[0], vec); 00636 normalize_v3(mat[2]); 00637 cross_v3_v3v3(mat[1], mat[2], mat[0]); 00638 } 00639 case 1: 00640 if (dot_v3v3(mat[1], mat[0]) < 1) { 00641 cross_v3_v3v3(mat[2], mat[0], mat[1]); 00642 normalize_v3(mat[2]); 00643 cross_v3_v3v3(mat[0], mat[1], mat[2]); 00644 } else if (dot_v3v3(mat[0], mat[2]) < 1) { 00645 cross_v3_v3v3(mat[0], mat[1], mat[2]); 00646 normalize_v3(mat[0]); 00647 cross_v3_v3v3(mat[2], mat[0], mat[1]); 00648 } else { 00649 float vec[3]; 00650 00651 vec[0]= mat[1][1]; 00652 vec[1]= mat[1][2]; 00653 vec[2]= mat[1][0]; 00654 00655 cross_v3_v3v3(mat[0], mat[1], vec); 00656 normalize_v3(mat[0]); 00657 cross_v3_v3v3(mat[2], mat[0], mat[1]); 00658 } 00659 case 2: 00660 if (dot_v3v3(mat[2], mat[0]) < 1) { 00661 cross_v3_v3v3(mat[1], mat[2], mat[0]); 00662 normalize_v3(mat[1]); 00663 cross_v3_v3v3(mat[0], mat[1], mat[2]); 00664 } else if (dot_v3v3(mat[2], mat[1]) < 1) { 00665 cross_v3_v3v3(mat[0], mat[1], mat[2]); 00666 normalize_v3(mat[0]); 00667 cross_v3_v3v3(mat[1], mat[2], mat[0]); 00668 } else { 00669 float vec[3]; 00670 00671 vec[0]= mat[2][1]; 00672 vec[1]= mat[2][2]; 00673 vec[2]= mat[2][0]; 00674 00675 cross_v3_v3v3(mat[0], vec, mat[2]); 00676 normalize_v3(mat[0]); 00677 cross_v3_v3v3(mat[1], mat[2], mat[0]); 00678 } 00679 } 00680 mul_v3_fl(mat[0], size[0]); 00681 mul_v3_fl(mat[1], size[1]); 00682 mul_v3_fl(mat[2], size[2]); 00683 } 00684 00685 void orthogonalize_m4(float mat[][4], int axis) 00686 { 00687 float size[3]; 00688 mat4_to_size(size, mat); 00689 normalize_v3(mat[axis]); 00690 switch(axis) 00691 { 00692 case 0: 00693 if (dot_v3v3(mat[0], mat[1]) < 1) { 00694 cross_v3_v3v3(mat[2], mat[0], mat[1]); 00695 normalize_v3(mat[2]); 00696 cross_v3_v3v3(mat[1], mat[2], mat[0]); 00697 } else if (dot_v3v3(mat[0], mat[2]) < 1) { 00698 cross_v3_v3v3(mat[1], mat[2], mat[0]); 00699 normalize_v3(mat[1]); 00700 cross_v3_v3v3(mat[2], mat[0], mat[1]); 00701 } else { 00702 float vec[3]; 00703 00704 vec[0]= mat[0][1]; 00705 vec[1]= mat[0][2]; 00706 vec[2]= mat[0][0]; 00707 00708 cross_v3_v3v3(mat[2], mat[0], vec); 00709 normalize_v3(mat[2]); 00710 cross_v3_v3v3(mat[1], mat[2], mat[0]); 00711 } 00712 case 1: 00713 normalize_v3(mat[0]); 00714 if (dot_v3v3(mat[1], mat[0]) < 1) { 00715 cross_v3_v3v3(mat[2], mat[0], mat[1]); 00716 normalize_v3(mat[2]); 00717 cross_v3_v3v3(mat[0], mat[1], mat[2]); 00718 } else if (dot_v3v3(mat[0], mat[2]) < 1) { 00719 cross_v3_v3v3(mat[0], mat[1], mat[2]); 00720 normalize_v3(mat[0]); 00721 cross_v3_v3v3(mat[2], mat[0], mat[1]); 00722 } else { 00723 float vec[3]; 00724 00725 vec[0]= mat[1][1]; 00726 vec[1]= mat[1][2]; 00727 vec[2]= mat[1][0]; 00728 00729 cross_v3_v3v3(mat[0], mat[1], vec); 00730 normalize_v3(mat[0]); 00731 cross_v3_v3v3(mat[2], mat[0], mat[1]); 00732 } 00733 case 2: 00734 if (dot_v3v3(mat[2], mat[0]) < 1) { 00735 cross_v3_v3v3(mat[1], mat[2], mat[0]); 00736 normalize_v3(mat[1]); 00737 cross_v3_v3v3(mat[0], mat[1], mat[2]); 00738 } else if (dot_v3v3(mat[2], mat[1]) < 1) { 00739 cross_v3_v3v3(mat[0], mat[1], mat[2]); 00740 normalize_v3(mat[0]); 00741 cross_v3_v3v3(mat[1], mat[2], mat[0]); 00742 } else { 00743 float vec[3]; 00744 00745 vec[0]= mat[2][1]; 00746 vec[1]= mat[2][2]; 00747 vec[2]= mat[2][0]; 00748 00749 cross_v3_v3v3(mat[0], vec, mat[2]); 00750 normalize_v3(mat[0]); 00751 cross_v3_v3v3(mat[1], mat[2], mat[0]); 00752 } 00753 } 00754 mul_v3_fl(mat[0], size[0]); 00755 mul_v3_fl(mat[1], size[1]); 00756 mul_v3_fl(mat[2], size[2]); 00757 } 00758 00759 int is_orthogonal_m3(float mat[][3]) 00760 { 00761 if (fabsf(dot_v3v3(mat[0], mat[1])) > 1.5f * FLT_EPSILON) 00762 return 0; 00763 00764 if (fabsf(dot_v3v3(mat[1], mat[2])) > 1.5f * FLT_EPSILON) 00765 return 0; 00766 00767 if (fabsf(dot_v3v3(mat[0], mat[2])) > 1.5f * FLT_EPSILON) 00768 return 0; 00769 00770 return 1; 00771 } 00772 00773 int is_orthogonal_m4(float mat[][4]) 00774 { 00775 if (fabsf(dot_v3v3(mat[0], mat[1])) > 1.5f * FLT_EPSILON) 00776 return 0; 00777 00778 if (fabsf(dot_v3v3(mat[1], mat[2])) > 1.5f * FLT_EPSILON) 00779 return 0; 00780 00781 if (fabsf(dot_v3v3(mat[0], mat[2])) > 1.5f * FLT_EPSILON) 00782 return 0; 00783 00784 return 1; 00785 } 00786 00787 void normalize_m3(float mat[][3]) 00788 { 00789 normalize_v3(mat[0]); 00790 normalize_v3(mat[1]); 00791 normalize_v3(mat[2]); 00792 } 00793 00794 void normalize_m3_m3(float rmat[][3], float mat[][3]) 00795 { 00796 normalize_v3_v3(rmat[0], mat[0]); 00797 normalize_v3_v3(rmat[1], mat[1]); 00798 normalize_v3_v3(rmat[2], mat[2]); 00799 } 00800 00801 00802 void normalize_m4(float mat[][4]) 00803 { 00804 float len; 00805 00806 len= normalize_v3(mat[0]); 00807 if(len!=0.0f) mat[0][3]/= len; 00808 len= normalize_v3(mat[1]); 00809 if(len!=0.0f) mat[1][3]/= len; 00810 len= normalize_v3(mat[2]); 00811 if(len!=0.0f) mat[2][3]/= len; 00812 } 00813 00814 void normalize_m4_m4(float rmat[][4], float mat[][4]) 00815 { 00816 float len; 00817 00818 len= normalize_v3_v3(rmat[0], mat[0]); 00819 if(len!=0.0f) rmat[0][3]= mat[0][3] / len; 00820 len= normalize_v3_v3(rmat[1], mat[1]); 00821 if(len!=0.0f) rmat[1][3]= mat[1][3] / len; 00822 len= normalize_v3_v3(rmat[2], mat[2]); 00823 if(len!=0.0f) rmat[2][3]= mat[2][3] / len;; 00824 } 00825 00826 void adjoint_m3_m3(float m1[][3], float m[][3]) 00827 { 00828 m1[0][0]=m[1][1]*m[2][2]-m[1][2]*m[2][1]; 00829 m1[0][1]= -m[0][1]*m[2][2]+m[0][2]*m[2][1]; 00830 m1[0][2]=m[0][1]*m[1][2]-m[0][2]*m[1][1]; 00831 00832 m1[1][0]= -m[1][0]*m[2][2]+m[1][2]*m[2][0]; 00833 m1[1][1]=m[0][0]*m[2][2]-m[0][2]*m[2][0]; 00834 m1[1][2]= -m[0][0]*m[1][2]+m[0][2]*m[1][0]; 00835 00836 m1[2][0]=m[1][0]*m[2][1]-m[1][1]*m[2][0]; 00837 m1[2][1]= -m[0][0]*m[2][1]+m[0][1]*m[2][0]; 00838 m1[2][2]=m[0][0]*m[1][1]-m[0][1]*m[1][0]; 00839 } 00840 00841 void adjoint_m4_m4(float out[][4], float in[][4]) /* out = ADJ(in) */ 00842 { 00843 float a1, a2, a3, a4, b1, b2, b3, b4; 00844 float c1, c2, c3, c4, d1, d2, d3, d4; 00845 00846 a1= in[0][0]; 00847 b1= in[0][1]; 00848 c1= in[0][2]; 00849 d1= in[0][3]; 00850 00851 a2= in[1][0]; 00852 b2= in[1][1]; 00853 c2= in[1][2]; 00854 d2= in[1][3]; 00855 00856 a3= in[2][0]; 00857 b3= in[2][1]; 00858 c3= in[2][2]; 00859 d3= in[2][3]; 00860 00861 a4= in[3][0]; 00862 b4= in[3][1]; 00863 c4= in[3][2]; 00864 d4= in[3][3]; 00865 00866 00867 out[0][0] = determinant_m3(b2, b3, b4, c2, c3, c4, d2, d3, d4); 00868 out[1][0] = - determinant_m3(a2, a3, a4, c2, c3, c4, d2, d3, d4); 00869 out[2][0] = determinant_m3(a2, a3, a4, b2, b3, b4, d2, d3, d4); 00870 out[3][0] = - determinant_m3(a2, a3, a4, b2, b3, b4, c2, c3, c4); 00871 00872 out[0][1] = - determinant_m3(b1, b3, b4, c1, c3, c4, d1, d3, d4); 00873 out[1][1] = determinant_m3(a1, a3, a4, c1, c3, c4, d1, d3, d4); 00874 out[2][1] = - determinant_m3(a1, a3, a4, b1, b3, b4, d1, d3, d4); 00875 out[3][1] = determinant_m3(a1, a3, a4, b1, b3, b4, c1, c3, c4); 00876 00877 out[0][2] = determinant_m3(b1, b2, b4, c1, c2, c4, d1, d2, d4); 00878 out[1][2] = - determinant_m3(a1, a2, a4, c1, c2, c4, d1, d2, d4); 00879 out[2][2] = determinant_m3(a1, a2, a4, b1, b2, b4, d1, d2, d4); 00880 out[3][2] = - determinant_m3(a1, a2, a4, b1, b2, b4, c1, c2, c4); 00881 00882 out[0][3] = - determinant_m3(b1, b2, b3, c1, c2, c3, d1, d2, d3); 00883 out[1][3] = determinant_m3(a1, a2, a3, c1, c2, c3, d1, d2, d3); 00884 out[2][3] = - determinant_m3(a1, a2, a3, b1, b2, b3, d1, d2, d3); 00885 out[3][3] = determinant_m3(a1, a2, a3, b1, b2, b3, c1, c2, c3); 00886 } 00887 00888 float determinant_m2(float a,float b,float c,float d) 00889 { 00890 00891 return a*d - b*c; 00892 } 00893 00894 float determinant_m3(float a1, float a2, float a3, 00895 float b1, float b2, float b3, 00896 float c1, float c2, float c3) 00897 { 00898 float ans; 00899 00900 ans = a1 * determinant_m2(b2, b3, c2, c3) 00901 - b1 * determinant_m2(a2, a3, c2, c3) 00902 + c1 * determinant_m2(a2, a3, b2, b3); 00903 00904 return ans; 00905 } 00906 00907 float determinant_m4(float m[][4]) 00908 { 00909 float ans; 00910 float a1,a2,a3,a4,b1,b2,b3,b4,c1,c2,c3,c4,d1,d2,d3,d4; 00911 00912 a1= m[0][0]; 00913 b1= m[0][1]; 00914 c1= m[0][2]; 00915 d1= m[0][3]; 00916 00917 a2= m[1][0]; 00918 b2= m[1][1]; 00919 c2= m[1][2]; 00920 d2= m[1][3]; 00921 00922 a3= m[2][0]; 00923 b3= m[2][1]; 00924 c3= m[2][2]; 00925 d3= m[2][3]; 00926 00927 a4= m[3][0]; 00928 b4= m[3][1]; 00929 c4= m[3][2]; 00930 d4= m[3][3]; 00931 00932 ans = a1 * determinant_m3(b2, b3, b4, c2, c3, c4, d2, d3, d4) 00933 - b1 * determinant_m3(a2, a3, a4, c2, c3, c4, d2, d3, d4) 00934 + c1 * determinant_m3(a2, a3, a4, b2, b3, b4, d2, d3, d4) 00935 - d1 * determinant_m3(a2, a3, a4, b2, b3, b4, c2, c3, c4); 00936 00937 return ans; 00938 } 00939 00940 /****************************** Transformations ******************************/ 00941 00942 void size_to_mat3(float mat[][3], const float size[3]) 00943 { 00944 mat[0][0]= size[0]; 00945 mat[0][1]= 0.0f; 00946 mat[0][2]= 0.0f; 00947 mat[1][1]= size[1]; 00948 mat[1][0]= 0.0f; 00949 mat[1][2]= 0.0f; 00950 mat[2][2]= size[2]; 00951 mat[2][1]= 0.0f; 00952 mat[2][0]= 0.0f; 00953 } 00954 00955 void size_to_mat4(float mat[][4], const float size[3]) 00956 { 00957 float tmat[3][3]; 00958 00959 size_to_mat3(tmat,size); 00960 unit_m4(mat); 00961 copy_m4_m3(mat, tmat); 00962 } 00963 00964 void mat3_to_size(float *size, float mat[][3]) 00965 { 00966 size[0]= len_v3(mat[0]); 00967 size[1]= len_v3(mat[1]); 00968 size[2]= len_v3(mat[2]); 00969 } 00970 00971 void mat4_to_size(float *size, float mat[][4]) 00972 { 00973 size[0]= len_v3(mat[0]); 00974 size[1]= len_v3(mat[1]); 00975 size[2]= len_v3(mat[2]); 00976 } 00977 00978 /* this gets the average scale of a matrix, only use when your scaling 00979 * data that has no idea of scale axis, examples are bone-envelope-radius 00980 * and curve radius */ 00981 float mat3_to_scale(float mat[][3]) 00982 { 00983 /* unit length vector */ 00984 float unit_vec[3] = {0.577350269189626f, 0.577350269189626f, 0.577350269189626f}; 00985 mul_m3_v3(mat, unit_vec); 00986 return len_v3(unit_vec); 00987 } 00988 00989 float mat4_to_scale(float mat[][4]) 00990 { 00991 float tmat[3][3]; 00992 copy_m3_m4(tmat, mat); 00993 return mat3_to_scale(tmat); 00994 } 00995 00996 00997 void mat3_to_rot_size(float rot[3][3], float size[3], float mat3[3][3]) 00998 { 00999 float mat3_n[3][3]; /* mat3 -> normalized, 3x3 */ 01000 float imat3_n[3][3]; /* mat3 -> normalized & inverted, 3x3 */ 01001 01002 /* rotation & scale are linked, we need to create the mat's 01003 * for these together since they are related. */ 01004 01005 /* so scale doesnt interfear with rotation [#24291] */ 01006 /* note: this is a workaround for negative matrix not working for rotation conversion, FIXME */ 01007 normalize_m3_m3(mat3_n, mat3); 01008 if(is_negative_m3(mat3)) { 01009 negate_v3(mat3_n[0]); 01010 negate_v3(mat3_n[1]); 01011 negate_v3(mat3_n[2]); 01012 } 01013 01014 /* rotation */ 01015 /* keep rot as a 3x3 matrix, the caller can convert into a quat or euler */ 01016 copy_m3_m3(rot, mat3_n); 01017 01018 /* scale */ 01019 /* note: mat4_to_size(ob->size, mat) fails for negative scale */ 01020 invert_m3_m3(imat3_n, mat3_n); 01021 mul_m3_m3m3(mat3, imat3_n, mat3); 01022 01023 size[0]= mat3[0][0]; 01024 size[1]= mat3[1][1]; 01025 size[2]= mat3[2][2]; 01026 } 01027 01028 void mat4_to_loc_rot_size(float loc[3], float rot[3][3], float size[3], float wmat[][4]) 01029 { 01030 float mat3[3][3]; /* wmat -> 3x3 */ 01031 01032 copy_m3_m4(mat3, wmat); 01033 mat3_to_rot_size(rot, size, mat3); 01034 01035 /* location */ 01036 copy_v3_v3(loc, wmat[3]); 01037 } 01038 01039 void scale_m3_fl(float m[][3], float scale) 01040 { 01041 m[0][0]= m[1][1]= m[2][2]= scale; 01042 m[0][1]= m[0][2]= 0.0; 01043 m[1][0]= m[1][2]= 0.0; 01044 m[2][0]= m[2][1]= 0.0; 01045 } 01046 01047 void scale_m4_fl(float m[][4], float scale) 01048 { 01049 m[0][0]= m[1][1]= m[2][2]= scale; 01050 m[3][3]= 1.0; 01051 m[0][1]= m[0][2]= m[0][3]= 0.0; 01052 m[1][0]= m[1][2]= m[1][3]= 0.0; 01053 m[2][0]= m[2][1]= m[2][3]= 0.0; 01054 m[3][0]= m[3][1]= m[3][2]= 0.0; 01055 } 01056 01057 void translate_m4(float mat[][4],float Tx, float Ty, float Tz) 01058 { 01059 mat[3][0] += (Tx*mat[0][0] + Ty*mat[1][0] + Tz*mat[2][0]); 01060 mat[3][1] += (Tx*mat[0][1] + Ty*mat[1][1] + Tz*mat[2][1]); 01061 mat[3][2] += (Tx*mat[0][2] + Ty*mat[1][2] + Tz*mat[2][2]); 01062 } 01063 01064 void rotate_m4(float mat[][4], const char axis, const float angle) 01065 { 01066 int col; 01067 float temp[4]= {0.0f, 0.0f, 0.0f, 0.0f}; 01068 float cosine, sine; 01069 01070 assert(axis >= 'X' && axis <= 'Z'); 01071 01072 cosine = (float)cos(angle); 01073 sine = (float)sin(angle); 01074 switch(axis){ 01075 case 'X': 01076 for(col=0 ; col<4 ; col++) 01077 temp[col] = cosine*mat[1][col] + sine*mat[2][col]; 01078 for(col=0 ; col<4 ; col++) { 01079 mat[2][col] = - sine*mat[1][col] + cosine*mat[2][col]; 01080 mat[1][col] = temp[col]; 01081 } 01082 break; 01083 01084 case 'Y': 01085 for(col=0 ; col<4 ; col++) 01086 temp[col] = cosine*mat[0][col] - sine*mat[2][col]; 01087 for(col=0 ; col<4 ; col++) { 01088 mat[2][col] = sine*mat[0][col] + cosine*mat[2][col]; 01089 mat[0][col] = temp[col]; 01090 } 01091 break; 01092 01093 case 'Z': 01094 for(col=0 ; col<4 ; col++) 01095 temp[col] = cosine*mat[0][col] + sine*mat[1][col]; 01096 for(col=0 ; col<4 ; col++) { 01097 mat[1][col] = - sine*mat[0][col] + cosine*mat[1][col]; 01098 mat[0][col] = temp[col]; 01099 } 01100 break; 01101 } 01102 } 01103 01104 void blend_m3_m3m3(float out[][3], float dst[][3], float src[][3], const float srcweight) 01105 { 01106 float srot[3][3], drot[3][3]; 01107 float squat[4], dquat[4], fquat[4]; 01108 float ssize[3], dsize[3], fsize[3]; 01109 float rmat[3][3], smat[3][3]; 01110 01111 mat3_to_rot_size(drot, dsize, dst); 01112 mat3_to_rot_size(srot, ssize, src); 01113 01114 mat3_to_quat(dquat, drot); 01115 mat3_to_quat(squat, srot); 01116 01117 /* do blending */ 01118 interp_qt_qtqt(fquat, dquat, squat, srcweight); 01119 interp_v3_v3v3(fsize, dsize, ssize, srcweight); 01120 01121 /* compose new matrix */ 01122 quat_to_mat3(rmat,fquat); 01123 size_to_mat3(smat,fsize); 01124 mul_m3_m3m3(out, rmat, smat); 01125 } 01126 01127 void blend_m4_m4m4(float out[][4], float dst[][4], float src[][4], const float srcweight) 01128 { 01129 float sloc[3], dloc[3], floc[3]; 01130 float srot[3][3], drot[3][3]; 01131 float squat[4], dquat[4], fquat[4]; 01132 float ssize[3], dsize[3], fsize[3]; 01133 01134 mat4_to_loc_rot_size(dloc, drot, dsize, dst); 01135 mat4_to_loc_rot_size(sloc, srot, ssize, src); 01136 01137 mat3_to_quat(dquat, drot); 01138 mat3_to_quat(squat, srot); 01139 01140 /* do blending */ 01141 interp_v3_v3v3(floc, dloc, sloc, srcweight); 01142 interp_qt_qtqt(fquat, dquat, squat, srcweight); 01143 interp_v3_v3v3(fsize, dsize, ssize, srcweight); 01144 01145 /* compose new matrix */ 01146 loc_quat_size_to_mat4(out, floc, fquat, fsize); 01147 } 01148 01149 01150 int is_negative_m3(float mat[][3]) 01151 { 01152 float vec[3]; 01153 cross_v3_v3v3(vec, mat[0], mat[1]); 01154 return (dot_v3v3(vec, mat[2]) < 0.0f); 01155 } 01156 01157 int is_negative_m4(float mat[][4]) 01158 { 01159 float vec[3]; 01160 cross_v3_v3v3(vec, mat[0], mat[1]); 01161 return (dot_v3v3(vec, mat[2]) < 0.0f); 01162 } 01163 01164 /* make a 4x4 matrix out of 3 transform components */ 01165 /* matrices are made in the order: scale * rot * loc */ 01166 // TODO: need to have a version that allows for rotation order... 01167 void loc_eul_size_to_mat4(float mat[4][4], const float loc[3], const float eul[3], const float size[3]) 01168 { 01169 float rmat[3][3], smat[3][3], tmat[3][3]; 01170 01171 /* initialise new matrix */ 01172 unit_m4(mat); 01173 01174 /* make rotation + scaling part */ 01175 eul_to_mat3(rmat,eul); 01176 size_to_mat3(smat,size); 01177 mul_m3_m3m3(tmat, rmat, smat); 01178 01179 /* copy rot/scale part to output matrix*/ 01180 copy_m4_m3(mat, tmat); 01181 01182 /* copy location to matrix */ 01183 mat[3][0] = loc[0]; 01184 mat[3][1] = loc[1]; 01185 mat[3][2] = loc[2]; 01186 } 01187 01188 /* make a 4x4 matrix out of 3 transform components */ 01189 /* matrices are made in the order: scale * rot * loc */ 01190 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) 01191 { 01192 float rmat[3][3], smat[3][3], tmat[3][3]; 01193 01194 /* initialise new matrix */ 01195 unit_m4(mat); 01196 01197 /* make rotation + scaling part */ 01198 eulO_to_mat3(rmat,eul, rotOrder); 01199 size_to_mat3(smat,size); 01200 mul_m3_m3m3(tmat, rmat, smat); 01201 01202 /* copy rot/scale part to output matrix*/ 01203 copy_m4_m3(mat, tmat); 01204 01205 /* copy location to matrix */ 01206 mat[3][0] = loc[0]; 01207 mat[3][1] = loc[1]; 01208 mat[3][2] = loc[2]; 01209 } 01210 01211 01212 /* make a 4x4 matrix out of 3 transform components */ 01213 /* matrices are made in the order: scale * rot * loc */ 01214 void loc_quat_size_to_mat4(float mat[4][4], const float loc[3], const float quat[4], const float size[3]) 01215 { 01216 float rmat[3][3], smat[3][3], tmat[3][3]; 01217 01218 /* initialise new matrix */ 01219 unit_m4(mat); 01220 01221 /* make rotation + scaling part */ 01222 quat_to_mat3(rmat,quat); 01223 size_to_mat3(smat,size); 01224 mul_m3_m3m3(tmat, rmat, smat); 01225 01226 /* copy rot/scale part to output matrix*/ 01227 copy_m4_m3(mat, tmat); 01228 01229 /* copy location to matrix */ 01230 mat[3][0] = loc[0]; 01231 mat[3][1] = loc[1]; 01232 mat[3][2] = loc[2]; 01233 } 01234 01235 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]) 01236 { 01237 float q[4]; 01238 axis_angle_to_quat(q, axis, angle); 01239 loc_quat_size_to_mat4(mat, loc, q, size); 01240 } 01241 01242 /*********************************** Other ***********************************/ 01243 01244 void print_m3(const char *str, float m[][3]) 01245 { 01246 printf("%s\n", str); 01247 printf("%f %f %f\n",m[0][0],m[1][0],m[2][0]); 01248 printf("%f %f %f\n",m[0][1],m[1][1],m[2][1]); 01249 printf("%f %f %f\n",m[0][2],m[1][2],m[2][2]); 01250 printf("\n"); 01251 } 01252 01253 void print_m4(const char *str, float m[][4]) 01254 { 01255 printf("%s\n", str); 01256 printf("%f %f %f %f\n",m[0][0],m[1][0],m[2][0],m[3][0]); 01257 printf("%f %f %f %f\n",m[0][1],m[1][1],m[2][1],m[3][1]); 01258 printf("%f %f %f %f\n",m[0][2],m[1][2],m[2][2],m[3][2]); 01259 printf("%f %f %f %f\n",m[0][3],m[1][3],m[2][3],m[3][3]); 01260 printf("\n"); 01261 } 01262 01263 /*********************************** SVD ************************************ 01264 * from TNT matrix library 01265 01266 * Compute the Single Value Decomposition of an arbitrary matrix A 01267 * That is compute the 3 matrices U,W,V with U column orthogonal (m,n) 01268 * ,W a diagonal matrix and V an orthogonal square matrix s.t. 01269 * A = U.W.Vt. From this decomposition it is trivial to compute the 01270 * (pseudo-inverse) of A as Ainv = V.Winv.tranpose(U). 01271 */ 01272 01273 void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) 01274 { 01275 float A[4][4]; 01276 float work1[4], work2[4]; 01277 int m = 4; 01278 int n = 4; 01279 int maxiter = 200; 01280 int nu = minf(m,n); 01281 01282 float *work = work1; 01283 float *e = work2; 01284 float eps; 01285 01286 int i=0, j=0, k=0, p, pp, iter; 01287 01288 // Reduce A to bidiagonal form, storing the diagonal elements 01289 // in s and the super-diagonal elements in e. 01290 01291 int nct = minf(m-1,n); 01292 int nrt = maxf(0,minf(n-2,m)); 01293 01294 copy_m4_m4(A, A_); 01295 zero_m4(U); 01296 zero_v4(s); 01297 01298 for (k = 0; k < maxf(nct,nrt); k++) { 01299 if (k < nct) { 01300 01301 // Compute the transformation for the k-th column and 01302 // place the k-th diagonal in s[k]. 01303 // Compute 2-norm of k-th column without under/overflow. 01304 s[k] = 0; 01305 for (i = k; i < m; i++) { 01306 s[k] = hypotf(s[k],A[i][k]); 01307 } 01308 if (s[k] != 0.0f) { 01309 float invsk; 01310 if (A[k][k] < 0.0f) { 01311 s[k] = -s[k]; 01312 } 01313 invsk = 1.0f/s[k]; 01314 for (i = k; i < m; i++) { 01315 A[i][k] *= invsk; 01316 } 01317 A[k][k] += 1.0f; 01318 } 01319 s[k] = -s[k]; 01320 } 01321 for (j = k+1; j < n; j++) { 01322 if ((k < nct) && (s[k] != 0.0f)) { 01323 01324 // Apply the transformation. 01325 01326 float t = 0; 01327 for (i = k; i < m; i++) { 01328 t += A[i][k]*A[i][j]; 01329 } 01330 t = -t/A[k][k]; 01331 for (i = k; i < m; i++) { 01332 A[i][j] += t*A[i][k]; 01333 } 01334 } 01335 01336 // Place the k-th row of A into e for the 01337 // subsequent calculation of the row transformation. 01338 01339 e[j] = A[k][j]; 01340 } 01341 if (k < nct) { 01342 01343 // Place the transformation in U for subsequent back 01344 // multiplication. 01345 01346 for (i = k; i < m; i++) 01347 U[i][k] = A[i][k]; 01348 } 01349 if (k < nrt) { 01350 01351 // Compute the k-th row transformation and place the 01352 // k-th super-diagonal in e[k]. 01353 // Compute 2-norm without under/overflow. 01354 e[k] = 0; 01355 for (i = k+1; i < n; i++) { 01356 e[k] = hypotf(e[k],e[i]); 01357 } 01358 if (e[k] != 0.0f) { 01359 float invek; 01360 if (e[k+1] < 0.0f) { 01361 e[k] = -e[k]; 01362 } 01363 invek = 1.0f/e[k]; 01364 for (i = k+1; i < n; i++) { 01365 e[i] *= invek; 01366 } 01367 e[k+1] += 1.0f; 01368 } 01369 e[k] = -e[k]; 01370 if ((k+1 < m) & (e[k] != 0.0f)) { 01371 float invek1; 01372 01373 // Apply the transformation. 01374 01375 for (i = k+1; i < m; i++) { 01376 work[i] = 0.0f; 01377 } 01378 for (j = k+1; j < n; j++) { 01379 for (i = k+1; i < m; i++) { 01380 work[i] += e[j]*A[i][j]; 01381 } 01382 } 01383 invek1 = 1.0f/e[k+1]; 01384 for (j = k+1; j < n; j++) { 01385 float t = -e[j]*invek1; 01386 for (i = k+1; i < m; i++) { 01387 A[i][j] += t*work[i]; 01388 } 01389 } 01390 } 01391 01392 // Place the transformation in V for subsequent 01393 // back multiplication. 01394 01395 for (i = k+1; i < n; i++) 01396 V[i][k] = e[i]; 01397 } 01398 } 01399 01400 // Set up the final bidiagonal matrix or order p. 01401 01402 p = minf(n,m+1); 01403 if (nct < n) { 01404 s[nct] = A[nct][nct]; 01405 } 01406 if (m < p) { 01407 s[p-1] = 0.0f; 01408 } 01409 if (nrt+1 < p) { 01410 e[nrt] = A[nrt][p-1]; 01411 } 01412 e[p-1] = 0.0f; 01413 01414 // If required, generate U. 01415 01416 for (j = nct; j < nu; j++) { 01417 for (i = 0; i < m; i++) { 01418 U[i][j] = 0.0f; 01419 } 01420 U[j][j] = 1.0f; 01421 } 01422 for (k = nct-1; k >= 0; k--) { 01423 if (s[k] != 0.0f) { 01424 for (j = k+1; j < nu; j++) { 01425 float t = 0; 01426 for (i = k; i < m; i++) { 01427 t += U[i][k]*U[i][j]; 01428 } 01429 t = -t/U[k][k]; 01430 for (i = k; i < m; i++) { 01431 U[i][j] += t*U[i][k]; 01432 } 01433 } 01434 for (i = k; i < m; i++ ) { 01435 U[i][k] = -U[i][k]; 01436 } 01437 U[k][k] = 1.0f + U[k][k]; 01438 for (i = 0; i < k-1; i++) { 01439 U[i][k] = 0.0f; 01440 } 01441 } else { 01442 for (i = 0; i < m; i++) { 01443 U[i][k] = 0.0f; 01444 } 01445 U[k][k] = 1.0f; 01446 } 01447 } 01448 01449 // If required, generate V. 01450 01451 for (k = n-1; k >= 0; k--) { 01452 if ((k < nrt) & (e[k] != 0.0f)) { 01453 for (j = k+1; j < nu; j++) { 01454 float t = 0; 01455 for (i = k+1; i < n; i++) { 01456 t += V[i][k]*V[i][j]; 01457 } 01458 t = -t/V[k+1][k]; 01459 for (i = k+1; i < n; i++) { 01460 V[i][j] += t*V[i][k]; 01461 } 01462 } 01463 } 01464 for (i = 0; i < n; i++) { 01465 V[i][k] = 0.0f; 01466 } 01467 V[k][k] = 1.0f; 01468 } 01469 01470 // Main iteration loop for the singular values. 01471 01472 pp = p-1; 01473 iter = 0; 01474 eps = powf(2.0f,-52.0f); 01475 while (p > 0) { 01476 int kase=0; 01477 01478 // Test for maximum iterations to avoid infinite loop 01479 if(maxiter == 0) 01480 break; 01481 maxiter--; 01482 01483 // This section of the program inspects for 01484 // negligible elements in the s and e arrays. On 01485 // completion the variables kase and k are set as follows. 01486 01487 // kase = 1 if s(p) and e[k-1] are negligible and k<p 01488 // kase = 2 if s(k) is negligible and k<p 01489 // kase = 3 if e[k-1] is negligible, k<p, and 01490 // s(k), ..., s(p) are not negligible (qr step). 01491 // kase = 4 if e(p-1) is negligible (convergence). 01492 01493 for (k = p-2; k >= -1; k--) { 01494 if (k == -1) { 01495 break; 01496 } 01497 if (fabsf(e[k]) <= eps*(fabsf(s[k]) + fabsf(s[k+1]))) { 01498 e[k] = 0.0f; 01499 break; 01500 } 01501 } 01502 if (k == p-2) { 01503 kase = 4; 01504 } else { 01505 int ks; 01506 for (ks = p-1; ks >= k; ks--) { 01507 float t; 01508 if (ks == k) { 01509 break; 01510 } 01511 t = (ks != p ? fabsf(e[ks]) : 0.f) + 01512 (ks != k+1 ? fabsf(e[ks-1]) : 0.0f); 01513 if (fabsf(s[ks]) <= eps*t) { 01514 s[ks] = 0.0f; 01515 break; 01516 } 01517 } 01518 if (ks == k) { 01519 kase = 3; 01520 } else if (ks == p-1) { 01521 kase = 1; 01522 } else { 01523 kase = 2; 01524 k = ks; 01525 } 01526 } 01527 k++; 01528 01529 // Perform the task indicated by kase. 01530 01531 switch (kase) { 01532 01533 // Deflate negligible s(p). 01534 01535 case 1: { 01536 float f = e[p-2]; 01537 e[p-2] = 0.0f; 01538 for (j = p-2; j >= k; j--) { 01539 float t = hypotf(s[j],f); 01540 float invt = 1.0f/t; 01541 float cs = s[j]*invt; 01542 float sn = f*invt; 01543 s[j] = t; 01544 if (j != k) { 01545 f = -sn*e[j-1]; 01546 e[j-1] = cs*e[j-1]; 01547 } 01548 01549 for (i = 0; i < n; i++) { 01550 t = cs*V[i][j] + sn*V[i][p-1]; 01551 V[i][p-1] = -sn*V[i][j] + cs*V[i][p-1]; 01552 V[i][j] = t; 01553 } 01554 } 01555 } 01556 break; 01557 01558 // Split at negligible s(k). 01559 01560 case 2: { 01561 float f = e[k-1]; 01562 e[k-1] = 0.0f; 01563 for (j = k; j < p; j++) { 01564 float t = hypotf(s[j],f); 01565 float invt = 1.0f/t; 01566 float cs = s[j]*invt; 01567 float sn = f*invt; 01568 s[j] = t; 01569 f = -sn*e[j]; 01570 e[j] = cs*e[j]; 01571 01572 for (i = 0; i < m; i++) { 01573 t = cs*U[i][j] + sn*U[i][k-1]; 01574 U[i][k-1] = -sn*U[i][j] + cs*U[i][k-1]; 01575 U[i][j] = t; 01576 } 01577 } 01578 } 01579 break; 01580 01581 // Perform one qr step. 01582 01583 case 3: { 01584 01585 // Calculate the shift. 01586 01587 float scale = maxf(maxf(maxf(maxf( 01588 fabsf(s[p-1]),fabsf(s[p-2])),fabsf(e[p-2])), 01589 fabsf(s[k])),fabsf(e[k])); 01590 float invscale = 1.0f/scale; 01591 float sp = s[p-1]*invscale; 01592 float spm1 = s[p-2]*invscale; 01593 float epm1 = e[p-2]*invscale; 01594 float sk = s[k]*invscale; 01595 float ek = e[k]*invscale; 01596 float b = ((spm1 + sp)*(spm1 - sp) + epm1*epm1)*0.5f; 01597 float c = (sp*epm1)*(sp*epm1); 01598 float shift = 0.0f; 01599 float f, g; 01600 if ((b != 0.0f) || (c != 0.0f)) { 01601 shift = sqrtf(b*b + c); 01602 if (b < 0.0f) { 01603 shift = -shift; 01604 } 01605 shift = c/(b + shift); 01606 } 01607 f = (sk + sp)*(sk - sp) + shift; 01608 g = sk*ek; 01609 01610 // Chase zeros. 01611 01612 for (j = k; j < p-1; j++) { 01613 float t = hypotf(f,g); 01614 /* division by zero checks added to avoid NaN (brecht) */ 01615 float cs = (t == 0.0f)? 0.0f: f/t; 01616 float sn = (t == 0.0f)? 0.0f: g/t; 01617 if (j != k) { 01618 e[j-1] = t; 01619 } 01620 f = cs*s[j] + sn*e[j]; 01621 e[j] = cs*e[j] - sn*s[j]; 01622 g = sn*s[j+1]; 01623 s[j+1] = cs*s[j+1]; 01624 01625 for (i = 0; i < n; i++) { 01626 t = cs*V[i][j] + sn*V[i][j+1]; 01627 V[i][j+1] = -sn*V[i][j] + cs*V[i][j+1]; 01628 V[i][j] = t; 01629 } 01630 01631 t = hypotf(f,g); 01632 /* division by zero checks added to avoid NaN (brecht) */ 01633 cs = (t == 0.0f)? 0.0f: f/t; 01634 sn = (t == 0.0f)? 0.0f: g/t; 01635 s[j] = t; 01636 f = cs*e[j] + sn*s[j+1]; 01637 s[j+1] = -sn*e[j] + cs*s[j+1]; 01638 g = sn*e[j+1]; 01639 e[j+1] = cs*e[j+1]; 01640 if (j < m-1) { 01641 for (i = 0; i < m; i++) { 01642 t = cs*U[i][j] + sn*U[i][j+1]; 01643 U[i][j+1] = -sn*U[i][j] + cs*U[i][j+1]; 01644 U[i][j] = t; 01645 } 01646 } 01647 } 01648 e[p-2] = f; 01649 iter = iter + 1; 01650 } 01651 break; 01652 01653 // Convergence. 01654 01655 case 4: { 01656 01657 // Make the singular values positive. 01658 01659 if (s[k] <= 0.0f) { 01660 s[k] = (s[k] < 0.0f ? -s[k] : 0.0f); 01661 01662 for (i = 0; i <= pp; i++) 01663 V[i][k] = -V[i][k]; 01664 } 01665 01666 // Order the singular values. 01667 01668 while (k < pp) { 01669 float t; 01670 if (s[k] >= s[k+1]) { 01671 break; 01672 } 01673 t = s[k]; 01674 s[k] = s[k+1]; 01675 s[k+1] = t; 01676 if (k < n-1) { 01677 for (i = 0; i < n; i++) { 01678 t = V[i][k+1]; V[i][k+1] = V[i][k]; V[i][k] = t; 01679 } 01680 } 01681 if (k < m-1) { 01682 for (i = 0; i < m; i++) { 01683 t = U[i][k+1]; U[i][k+1] = U[i][k]; U[i][k] = t; 01684 } 01685 } 01686 k++; 01687 } 01688 iter = 0; 01689 p--; 01690 } 01691 break; 01692 } 01693 } 01694 } 01695 01696 void pseudoinverse_m4_m4(float Ainv[4][4], float A[4][4], float epsilon) 01697 { 01698 /* compute moon-penrose pseudo inverse of matrix, singular values 01699 below epsilon are ignored for stability (truncated SVD) */ 01700 float V[4][4], W[4], Wm[4][4], U[4][4]; 01701 int i; 01702 01703 transpose_m4(A); 01704 svd_m4(V, W, U, A); 01705 transpose_m4(U); 01706 transpose_m4(V); 01707 01708 zero_m4(Wm); 01709 for(i=0; i<4; i++) 01710 Wm[i][i]= (W[i] < epsilon)? 0.0f: 1.0f/W[i]; 01711 01712 transpose_m4(V); 01713 01714 mul_serie_m4(Ainv, U, Wm, V, NULL, NULL, NULL, NULL, NULL); 01715 }