2 * The 3D Studio File Format Library
3 * Copyright (C) 1996-2001 by J.E. Hoffmann <je-h@gmx.net>
6 * This program is free software; you can redistribute it and/or modify it
7 * under the terms of the GNU Lesser General Public License as published by
8 * the Free Software Foundation; either version 2.1 of the License, or (at
9 * your option) any later version.
11 * This program is distributed in the hope that it will be useful, but
12 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
13 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public
14 * License for more details.
16 * You should have received a copy of the GNU Lesser General Public License
17 * along with this program; if not, write to the Free Software Foundation,
18 * Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
20 * $Id: matrix.c,v 1.10 2004/11/16 07:41:44 efalk Exp $
23 #include <lib3ds/matrix.h>
24 #include <lib3ds/quat.h>
25 #include <lib3ds/vector.h>
31 * \defgroup matrix Matrix Mathematics
33 * \author J.E. Hoffmann <je-h@gmx.net>
36 * \typedef Lib3dsMatrix
42 * Clear a matrix to all zeros.
44 * \param m Matrix to be cleared.
49 lib3ds_matrix_zero(Lib3dsMatrix m)
54 for (j=0; j<4; j++) m[i][j]=0.0f;
60 * Set a matrix to identity.
62 * \param m Matrix to be set.
67 lib3ds_matrix_identity(Lib3dsMatrix m)
72 for (j=0; j<4; j++) m[i][j]=0.0;
74 for (i=0; i<4; i++) m[i][i]=1.0;
84 lib3ds_matrix_copy(Lib3dsMatrix dest, Lib3dsMatrix src)
86 memcpy(dest, src, sizeof(Lib3dsMatrix));
91 * Negate a matrix -- all elements negated.
96 lib3ds_matrix_neg(Lib3dsMatrix m)
100 for (j=0; j<4; j++) {
101 for (i=0; i<4; i++) {
109 * Set all matrix elements to their absolute value.
114 lib3ds_matrix_abs(Lib3dsMatrix m)
118 for (j=0; j<4; j++) {
119 for (i=0; i<4; i++) {
120 m[j][i]=(Lib3dsFloat)fabs(m[j][i]);
127 * Transpose a matrix in place.
132 lib3ds_matrix_transpose(Lib3dsMatrix m)
137 for (j=0; j<4; j++) {
138 for (i=j+1; i<4; i++) {
153 lib3ds_matrix_add(Lib3dsMatrix m, Lib3dsMatrix a, Lib3dsMatrix b)
157 for (j=0; j<4; j++) {
158 for (i=0; i<4; i++) {
159 m[j][i]=a[j][i]+b[j][i];
166 * Subtract two matrices.
175 lib3ds_matrix_sub(Lib3dsMatrix m, Lib3dsMatrix a, Lib3dsMatrix b)
179 for (j=0; j<4; j++) {
180 for (i=0; i<4; i++) {
181 m[j][i]=a[j][i]-b[j][i];
188 * Multiply two matrices.
191 * \param a Left matrix.
192 * \param b Right matrix.
197 lib3ds_matrix_mul(Lib3dsMatrix m, Lib3dsMatrix a, Lib3dsMatrix b)
202 for (j=0; j<4; j++) {
203 for (i=0; i<4; i++) {
205 for (k=0; k<4; k++) ab+=a[k][i]*b[j][k];
213 * Multiply a matrix by a scalar.
215 * \param m Matrix to be set.
221 lib3ds_matrix_scalar(Lib3dsMatrix m, Lib3dsFloat k)
225 for (j=0; j<4; j++) {
226 for (i=0; i<4; i++) {
235 Lib3dsFloat a, Lib3dsFloat b,
236 Lib3dsFloat c, Lib3dsFloat d)
238 return((a)*(d)-(b)*(c));
244 Lib3dsFloat a1, Lib3dsFloat a2, Lib3dsFloat a3,
245 Lib3dsFloat b1, Lib3dsFloat b2, Lib3dsFloat b3,
246 Lib3dsFloat c1, Lib3dsFloat c2, Lib3dsFloat c3)
249 a1*det2x2(b2,b3,c2,c3)-
250 b1*det2x2(a2,a3,c2,c3)+
251 c1*det2x2(a2,a3,b2,b3)
257 * Find determinant of a matrix.
262 lib3ds_matrix_det(Lib3dsMatrix m)
264 Lib3dsFloat a1,a2,a3,a4,b1,b2,b3,b4,c1,c2,c3,c4,d1,d2,d3,d4;
283 a1 * det3x3(b2, b3, b4, c2, c3, c4, d2, d3, d4)-
284 b1 * det3x3(a2, a3, a4, c2, c3, c4, d2, d3, d4)+
285 c1 * det3x3(a2, a3, a4, b2, b3, b4, d2, d3, d4)-
286 d1 * det3x3(a2, a3, a4, b2, b3, b4, c2, c3, c4)
292 * Find the adjoint of a matrix.
297 lib3ds_matrix_adjoint(Lib3dsMatrix m)
299 Lib3dsFloat a1,a2,a3,a4,b1,b2,b3,b4,c1,c2,c3,c4,d1,d2,d3,d4;
317 m[0][0]= det3x3 (b2, b3, b4, c2, c3, c4, d2, d3, d4);
318 m[0][1]= -det3x3 (a2, a3, a4, c2, c3, c4, d2, d3, d4);
319 m[0][2]= det3x3 (a2, a3, a4, b2, b3, b4, d2, d3, d4);
320 m[0][3]= -det3x3 (a2, a3, a4, b2, b3, b4, c2, c3, c4);
321 m[1][0]= -det3x3 (b1, b3, b4, c1, c3, c4, d1, d3, d4);
322 m[1][1]= det3x3 (a1, a3, a4, c1, c3, c4, d1, d3, d4);
323 m[1][2]= -det3x3 (a1, a3, a4, b1, b3, b4, d1, d3, d4);
324 m[1][3]= det3x3 (a1, a3, a4, b1, b3, b4, c1, c3, c4);
325 m[2][0]= det3x3 (b1, b2, b4, c1, c2, c4, d1, d2, d4);
326 m[2][1]= -det3x3 (a1, a2, a4, c1, c2, c4, d1, d2, d4);
327 m[2][2]= det3x3 (a1, a2, a4, b1, b2, b4, d1, d2, d4);
328 m[2][3]= -det3x3 (a1, a2, a4, b1, b2, b4, c1, c2, c4);
329 m[3][0]= -det3x3 (b1, b2, b3, c1, c2, c3, d1, d2, d3);
330 m[3][1]= det3x3 (a1, a2, a3, c1, c2, c3, d1, d2, d3);
331 m[3][2]= -det3x3 (a1, a2, a3, b1, b2, b3, d1, d2, d3);
332 m[3][3]= det3x3 (a1, a2, a3, b1, b2, b3, c1, c2, c3);
337 * Invert a matrix in place.
339 * \param m Matrix to invert.
341 * \return LIB3DS_TRUE on success, LIB3DS_FALSE on failure.
344 * GGemsII, K.Wu, Fast Matrix Inversion
347 lib3ds_matrix_inv(Lib3dsMatrix m)
350 int pvt_i[4], pvt_j[4]; /* Locations of pivot elements */
351 Lib3dsFloat pvt_val; /* Value of current pivot element */
352 Lib3dsFloat hold; /* Temporary storage */
353 Lib3dsFloat determinat;
356 for (k=0; k<4; k++) {
357 /* Locate k'th pivot element */
358 pvt_val=m[k][k]; /* Initialize for search */
361 for (i=k; i<4; i++) {
362 for (j=k; j<4; j++) {
363 if (fabs(m[i][j]) > fabs(pvt_val)) {
371 /* Product of pivots, gives determinant when finished */
373 if (fabs(determinat)<LIB3DS_EPSILON) {
374 return(LIB3DS_FALSE); /* Matrix is singular (zero determinant) */
377 /* "Interchange" rows (with sign change stuff) */
379 if (i!=k) { /* If rows are different */
380 for (j=0; j<4; j++) {
387 /* "Interchange" columns */
389 if (j!=k) { /* If columns are different */
390 for (i=0; i<4; i++) {
397 /* Divide column by minus pivot value */
398 for (i=0; i<4; i++) {
399 if (i!=k) m[i][k]/=( -pvt_val) ;
402 /* Reduce the matrix */
403 for (i=0; i<4; i++) {
405 for (j=0; j<4; j++) {
406 if (i!=k && j!=k) m[i][j]+=hold*m[k][j];
410 /* Divide row by pivot */
411 for (j=0; j<4; j++) {
412 if (j!=k) m[k][j]/=pvt_val;
415 /* Replace pivot by reciprocal (at last we can touch it). */
416 m[k][k] = 1.0f/pvt_val;
419 /* That was most of the work, one final pass of row/column interchange */
421 for (k=4-2; k>=0; k--) { /* Don't need to work with 1 by 1 corner*/
422 i=pvt_j[k]; /* Rows to swap correspond to pivot COLUMN */
423 if (i!=k) { /* If rows are different */
431 j=pvt_i[k]; /* Columns to swap correspond to pivot ROW */
432 if (j!=k) /* If columns are different */
433 for (i=0; i<4; i++) {
444 * Apply a translation to a matrix.
449 lib3ds_matrix_translate_xyz(Lib3dsMatrix m, Lib3dsFloat x, Lib3dsFloat y, Lib3dsFloat z)
453 for (i=0; i<3; i++) {
454 m[3][i]+= m[0][i]*x + m[1][i]*y + m[2][i]*z;
460 * Apply a translation to a matrix.
465 lib3ds_matrix_translate(Lib3dsMatrix m, Lib3dsVector t)
469 for (i=0; i<3; i++) {
470 m[3][i]+= m[0][i]*t[0] + m[1][i]*t[1] + m[2][i]*t[2];
476 * Apply scale factors to a matrix.
481 lib3ds_matrix_scale_xyz(Lib3dsMatrix m, Lib3dsFloat x, Lib3dsFloat y, Lib3dsFloat z)
485 for (i=0; i<4; i++) {
494 * Apply scale factors to a matrix.
499 lib3ds_matrix_scale(Lib3dsMatrix m, Lib3dsVector s)
503 for (i=0; i<4; i++) {
512 * Apply a rotation about the x axis to a matrix.
517 lib3ds_matrix_rotate_x(Lib3dsMatrix m, Lib3dsFloat phi)
519 Lib3dsFloat SinPhi,CosPhi;
520 Lib3dsFloat a1[4],a2[4];
522 SinPhi=(Lib3dsFloat)sin(phi);
523 CosPhi=(Lib3dsFloat)cos(phi);
524 memcpy(a1,m[1],4*sizeof(Lib3dsFloat));
525 memcpy(a2,m[2],4*sizeof(Lib3dsFloat));
526 m[1][0]=CosPhi*a1[0]+SinPhi*a2[0];
527 m[1][1]=CosPhi*a1[1]+SinPhi*a2[1];
528 m[1][2]=CosPhi*a1[2]+SinPhi*a2[2];
529 m[1][3]=CosPhi*a1[3]+SinPhi*a2[3];
530 m[2][0]=-SinPhi*a1[0]+CosPhi*a2[0];
531 m[2][1]=-SinPhi*a1[1]+CosPhi*a2[1];
532 m[2][2]=-SinPhi*a1[2]+CosPhi*a2[2];
533 m[2][3]=-SinPhi*a1[3]+CosPhi*a2[3];
538 * Apply a rotation about the y axis to a matrix.
543 lib3ds_matrix_rotate_y(Lib3dsMatrix m, Lib3dsFloat phi)
545 Lib3dsFloat SinPhi,CosPhi;
546 Lib3dsFloat a0[4],a2[4];
548 SinPhi=(Lib3dsFloat)sin(phi);
549 CosPhi=(Lib3dsFloat)cos(phi);
550 memcpy(a0,m[0],4*sizeof(Lib3dsFloat));
551 memcpy(a2,m[2],4*sizeof(Lib3dsFloat));
552 m[0][0]=CosPhi*a0[0]-SinPhi*a2[0];
553 m[0][1]=CosPhi*a0[1]-SinPhi*a2[1];
554 m[0][2]=CosPhi*a0[2]-SinPhi*a2[2];
555 m[0][3]=CosPhi*a0[3]-SinPhi*a2[3];
556 m[2][0]=SinPhi*a0[0]+CosPhi*a2[0];
557 m[2][1]=SinPhi*a0[1]+CosPhi*a2[1];
558 m[2][2]=SinPhi*a0[2]+CosPhi*a2[2];
559 m[2][3]=SinPhi*a0[3]+CosPhi*a2[3];
564 * Apply a rotation about the z axis to a matrix.
569 lib3ds_matrix_rotate_z(Lib3dsMatrix m, Lib3dsFloat phi)
571 Lib3dsFloat SinPhi,CosPhi;
572 Lib3dsFloat a0[4],a1[4];
574 SinPhi=(Lib3dsFloat)sin(phi);
575 CosPhi=(Lib3dsFloat)cos(phi);
576 memcpy(a0,m[0],4*sizeof(Lib3dsFloat));
577 memcpy(a1,m[1],4*sizeof(Lib3dsFloat));
578 m[0][0]=CosPhi*a0[0]+SinPhi*a1[0];
579 m[0][1]=CosPhi*a0[1]+SinPhi*a1[1];
580 m[0][2]=CosPhi*a0[2]+SinPhi*a1[2];
581 m[0][3]=CosPhi*a0[3]+SinPhi*a1[3];
582 m[1][0]=-SinPhi*a0[0]+CosPhi*a1[0];
583 m[1][1]=-SinPhi*a0[1]+CosPhi*a1[1];
584 m[1][2]=-SinPhi*a0[2]+CosPhi*a1[2];
585 m[1][3]=-SinPhi*a0[3]+CosPhi*a1[3];
590 * Apply a rotation about an arbitrary axis to a matrix.
595 lib3ds_matrix_rotate(Lib3dsMatrix m, Lib3dsQuat q)
597 Lib3dsFloat s,xs,ys,zs,wx,wy,wz,xx,xy,xz,yy,yz,zz,l;
600 lib3ds_matrix_copy(a, m);
602 l=q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3];
603 if (fabs(l)<LIB3DS_EPSILON) {
610 xs = q[0] * s; ys = q[1] * s; zs = q[2] * s;
611 wx = q[3] * xs; wy = q[3] * ys; wz = q[3] * zs;
612 xx = q[0] * xs; xy = q[0] * ys; xz = q[0] * zs;
613 yy = q[1] * ys; yz = q[1] * zs; zz = q[2] * zs;
615 b[0][0]=1.0f - (yy +zz);
619 b[1][1]=1.0f - (xx +zz);
623 b[2][2]=1.0f - (xx + yy);
624 b[3][0]=b[3][1]=b[3][2]=b[0][3]=b[1][3]=b[2][3]=0.0f;
627 lib3ds_matrix_mul(m,a,b);
632 * Apply a rotation about an arbitrary axis to a matrix.
637 lib3ds_matrix_rotate_axis(Lib3dsMatrix m, Lib3dsVector axis, Lib3dsFloat angle)
641 lib3ds_quat_axis_angle(q,axis,angle);
642 lib3ds_matrix_rotate(m,q);
647 * Compute a camera matrix based on position, target and roll.
649 * Generates a translate/rotate matrix that maps world coordinates
650 * to camera coordinates. Resulting matrix does not include perspective
653 * \param matrix Destination matrix.
654 * \param pos Camera position
655 * \param tgt Camera target
656 * \param roll Roll angle
661 lib3ds_matrix_camera(Lib3dsMatrix matrix, Lib3dsVector pos,
662 Lib3dsVector tgt, Lib3dsFloat roll)
665 Lib3dsVector x, y, z;
667 lib3ds_vector_sub(y, tgt, pos);
668 lib3ds_vector_normalize(y);
670 if (y[0] != 0. || y[1] != 0) {
675 else { /* Special case: looking straight up or down z axis */
681 lib3ds_vector_cross(x, y, z);
682 lib3ds_vector_cross(z, x, y);
683 lib3ds_vector_normalize(x);
684 lib3ds_vector_normalize(z);
686 lib3ds_matrix_identity(M);
697 lib3ds_matrix_identity(R);
698 lib3ds_matrix_rotate_y(R, roll);
699 lib3ds_matrix_mul(matrix, R,M);
700 lib3ds_matrix_translate_xyz(matrix, -pos[0],-pos[1],-pos[2]);
708 lib3ds_matrix_dump(Lib3dsMatrix matrix)
712 for (i=0; i<4; ++i) {
713 for (j=0; j<4; ++j) {
714 printf("%f ", matrix[j][i]);