root/OpenSceneGraph/trunk/src/osgPlugins/lib3ds/matrix.cpp @ 1563

Revision 1563, 12.4 kB (checked in by robert, 11 years ago)

Added an #ifdef to osgGLUT/Window.cpp to handle the case of compiling against
older versions of GLUT.

  • Property svn:eol-style set to native
  • Property svn:keywords set to Author Date Id Revision
RevLine 
[8]1/*
2 * The 3D Studio File Format Library
3 * Copyright (C) 1996-2001 by J.E. Hoffmann <je-h@gmx.net>
4 * All rights reserved.
5 *
6 * This program is  free  software;  you can redistribute it and/or modify it
[151]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
[8]9 * your option) any later version.
10 *
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
[151]13 * or  FITNESS FOR A  PARTICULAR PURPOSE.  See the  GNU Lesser General Public 
[8]14 * License for more details.
15 *
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.
19 *
20 * $Id$
21 */
22#define LIB3DS_EXPORT
[1563]23#include "matrix.h"
24#include "quat.h"
25#include "vector.h"
[8]26#include <string.h>
27#include <math.h>
28
[151]29
[8]30/*!
31 * \defgroup matrix Matrix Mathematics
32 *
33 * \author J.E. Hoffmann <je-h@gmx.net>
34 */
35/*!
36 * \typedef Lib3dsMatrix
37 *   \ingroup matrix
38 */
39
[151]40
[8]41/*!
42 * \ingroup matrix
43 */
44void
45lib3ds_matrix_zero(Lib3dsMatrix m)
46{
[151]47  int i,j;
[8]48
[151]49  for (i=0; i<4; i++) {
50    for (j=0; j<4; j++) m[i][j]=0.0f;
51  }
[8]52}
53
54
55/*!
56 * \ingroup matrix
57 */
58void
59lib3ds_matrix_identity(Lib3dsMatrix m)
60{
[151]61  int i,j;
[8]62
[151]63  for (i=0; i<4; i++) {
64    for (j=0; j<4; j++) m[i][j]=0.0;
65  }
66  for (i=0; i<4; i++) m[i][i]=1.0;
[8]67}
68
69
70/*!
71 * \ingroup matrix
72 */
73void
74lib3ds_matrix_copy(Lib3dsMatrix dest, Lib3dsMatrix src)
75{
[151]76  memcpy(dest, src, sizeof(Lib3dsMatrix));
[8]77}
78
79
80/*!
81 * \ingroup matrix
82 */
[151]83void 
[8]84lib3ds_matrix_neg(Lib3dsMatrix m)
85{
[151]86  int i,j;
[8]87
[151]88  for (j=0; j<4; j++) {
89    for (i=0; i<4; i++) {
90      m[j][i]=-m[j][i];
[8]91    }
[151]92  }
[8]93}
94
95
96/*!
97 * \ingroup matrix
98 */
[151]99void 
[8]100lib3ds_matrix_abs(Lib3dsMatrix m)
101{
[151]102  int i,j;
[8]103
[151]104  for (j=0; j<4; j++) {
105    for (i=0; i<4; i++) {
106      m[j][i]=(Lib3dsFloat)fabs(m[j][i]);
[8]107    }
[151]108  }
[8]109}
110
111
112/*!
113 * \ingroup matrix
114 */
115void
116lib3ds_matrix_transpose(Lib3dsMatrix m)
117{
[151]118  int i,j;
119  Lib3dsFloat swp;
[8]120
[151]121  for (j=0; j<4; j++) {
122    for (i=j+1; i<4; i++) {
123      swp=m[j][i];
124      m[j][i]=m[i][j];
125      m[i][j]=swp;
[8]126    }
[151]127  }
[8]128}
129
130
131/*!
132 * \ingroup matrix
133 */
134void
135lib3ds_matrix_add(Lib3dsMatrix m, Lib3dsMatrix a, Lib3dsMatrix b)
136{
[151]137  int i,j;
[8]138
[151]139  for (j=0; j<4; j++) {
140    for (i=0; i<4; i++) {
141      m[j][i]=a[j][i]+b[j][i];
[8]142    }
[151]143  }
[8]144}
145
146
147/*!
148 * \ingroup matrix
149 */
150void
151lib3ds_matrix_sub(Lib3dsMatrix m, Lib3dsMatrix a, Lib3dsMatrix b)
152{
[151]153  int i,j;
[8]154
[151]155  for (j=0; j<4; j++) {
156    for (i=0; i<4; i++) {
157      m[j][i]=a[j][i]-b[j][i];
[8]158    }
[151]159  }
[8]160}
161
162
163/*!
164 * \ingroup matrix
165 */
166void
167lib3ds_matrix_mul(Lib3dsMatrix m, Lib3dsMatrix a, Lib3dsMatrix b)
168{
[151]169  int i,j,k;
170  Lib3dsFloat ab;
[8]171
[151]172  for (j=0; j<4; j++) {
173    for (i=0; i<4; i++) {
174      ab=0.0f;
175      for (k=0; k<4; k++) ab+=a[k][i]*b[j][k];
176      m[j][i]=ab;
[8]177    }
[151]178  }
[8]179}
180
181
182/*!
183 * \ingroup matrix
184 */
185void
186lib3ds_matrix_scalar(Lib3dsMatrix m, Lib3dsFloat k)
187{
[151]188  int i,j;
[8]189
[151]190  for (j=0; j<4; j++) {
191    for (i=0; i<4; i++) {
192      m[j][i]*=k;
[8]193    }
[151]194  }
[8]195}
196
197
198static Lib3dsFloat
199det2x2(
[151]200  Lib3dsFloat a, Lib3dsFloat b,
201  Lib3dsFloat c, Lib3dsFloat d)
[8]202{
[151]203  return((a)*(d)-(b)*(c));
[8]204}
205
206
207static Lib3dsFloat
208det3x3(
[151]209  Lib3dsFloat a1, Lib3dsFloat a2, Lib3dsFloat a3,
210  Lib3dsFloat b1, Lib3dsFloat b2, Lib3dsFloat b3,
211  Lib3dsFloat c1, Lib3dsFloat c2, Lib3dsFloat c3)
[8]212{
[151]213  return(
214    a1*det2x2(b2,b3,c2,c3)-
215    b1*det2x2(a2,a3,c2,c3)+
216    c1*det2x2(a2,a3,b2,b3)
217  );
[8]218}
219
220
221/*!
222 * \ingroup matrix
223 */
224Lib3dsFloat
225lib3ds_matrix_det(Lib3dsMatrix m)
226{
[151]227  Lib3dsFloat a1,a2,a3,a4,b1,b2,b3,b4,c1,c2,c3,c4,d1,d2,d3,d4;
[8]228
[151]229  a1 = m[0][0];
230  b1 = m[1][0];
231  c1 = m[2][0];
232  d1 = m[3][0];
233  a2 = m[0][1];
234  b2 = m[1][1];
235  c2 = m[2][1];
236  d2 = m[3][1];
237  a3 = m[0][2];
238  b3 = m[1][2];
239  c3 = m[2][2];
240  d3 = m[3][2];
241  a4 = m[0][3];
242  b4 = m[1][3];
243  c4 = m[2][3];
244  d4 = m[3][3];
245  return(
246    a1 * det3x3(b2, b3, b4, c2, c3, c4, d2, d3, d4)-
247    b1 * det3x3(a2, a3, a4, c2, c3, c4, d2, d3, d4)+
248    c1 * det3x3(a2, a3, a4, b2, b3, b4, d2, d3, d4)-
249    d1 * det3x3(a2, a3, a4, b2, b3, b4, c2, c3, c4)
250  );
[8]251}
252
253
254/*!
255 * \ingroup matrix
256 */
257void
258lib3ds_matrix_adjoint(Lib3dsMatrix m)
259{
[151]260  Lib3dsFloat a1,a2,a3,a4,b1,b2,b3,b4,c1,c2,c3,c4,d1,d2,d3,d4;
[8]261
[151]262  a1 = m[0][0];
263  b1 = m[1][0];
264  c1 = m[2][0];
265  d1 = m[3][0];
266  a2 = m[0][1];
267  b2 = m[1][1];
268  c2 = m[2][1];
269  d2 = m[3][1];
270  a3 = m[0][2];
271  b3 = m[1][2];
272  c3 = m[2][2];
273  d3 = m[3][2];
274  a4 = m[0][3];
275  b4 = m[1][3];
276  c4 = m[2][3];
277  d4 = m[3][3];
278  m[0][0]=  det3x3 (b2, b3, b4, c2, c3, c4, d2, d3, d4);
279  m[0][1]= -det3x3 (a2, a3, a4, c2, c3, c4, d2, d3, d4);
280  m[0][2]=  det3x3 (a2, a3, a4, b2, b3, b4, d2, d3, d4);
281  m[0][3]= -det3x3 (a2, a3, a4, b2, b3, b4, c2, c3, c4);
282  m[1][0]= -det3x3 (b1, b3, b4, c1, c3, c4, d1, d3, d4);
283  m[1][1]=  det3x3 (a1, a3, a4, c1, c3, c4, d1, d3, d4);
284  m[1][2]= -det3x3 (a1, a3, a4, b1, b3, b4, d1, d3, d4);
285  m[1][3]=  det3x3 (a1, a3, a4, b1, b3, b4, c1, c3, c4);
286  m[2][0]=  det3x3 (b1, b2, b4, c1, c2, c4, d1, d2, d4);
287  m[2][1]= -det3x3 (a1, a2, a4, c1, c2, c4, d1, d2, d4);
288  m[2][2]=  det3x3 (a1, a2, a4, b1, b2, b4, d1, d2, d4);
289  m[2][3]= -det3x3 (a1, a2, a4, b1, b2, b4, c1, c2, c4);
290  m[3][0]= -det3x3 (b1, b2, b3, c1, c2, c3, d1, d2, d3);
291  m[3][1]=  det3x3 (a1, a2, a3, c1, c2, c3, d1, d2, d3);
292  m[3][2]= -det3x3 (a1, a2, a3, b1, b2, b3, d1, d2, d3);
293  m[3][3]=  det3x3 (a1, a2, a3, b1, b2, b3, c1, c2, c3);
[8]294}
295
296
297/*!
298 * \ingroup matrix
299 *
[151]300 * GGemsII, K.Wu, Fast Matrix Inversion
[8]301 */
302Lib3dsBool
303lib3ds_matrix_inv(Lib3dsMatrix m)
[151]304{                         
305  int i,j,k;               
306  int pvt_i[4], pvt_j[4];            /* Locations of pivot elements */
307  Lib3dsFloat pvt_val;               /* Value of current pivot element */
308  Lib3dsFloat hold;                  /* Temporary storage */
309  Lib3dsFloat determinat;           
[8]310
[151]311  determinat = 1.0f;
312  for (k=0; k<4; k++)  {
313    /* Locate k'th pivot element */
314    pvt_val=m[k][k];            /* Initialize for search */
315    pvt_i[k]=k;
316    pvt_j[k]=k;
317    for (i=k; i<4; i++) {
318      for (j=k; j<4; j++) {
319        if (fabs(m[i][j]) > fabs(pvt_val)) {
320          pvt_i[k]=i;
321          pvt_j[k]=j;
322          pvt_val=m[i][j];
[8]323        }
[151]324      }
325    }
[8]326
[151]327    /* Product of pivots, gives determinant when finished */
328    determinat*=pvt_val;
329    if (fabs(determinat)<LIB3DS_EPSILON) {   
330      return(LIB3DS_FALSE);  /* Matrix is singular (zero determinant) */
331    }
[8]332
[151]333    /* "Interchange" rows (with sign change stuff) */
334    i=pvt_i[k];
335    if (i!=k) {               /* If rows are different */
336      for (j=0; j<4; j++) {
337        hold=-m[k][j];
338        m[k][j]=m[i][j];
339        m[i][j]=hold;
340      }
341    }
[8]342
[151]343    /* "Interchange" columns */
344    j=pvt_j[k];
345    if (j!=k) {              /* If columns are different */
346      for (i=0; i<4; i++) {
347        hold=-m[i][k];
348        m[i][k]=m[i][j];
349        m[i][j]=hold;
350      }
351    }
352   
353    /* Divide column by minus pivot value */
354    for (i=0; i<4; i++) {
355      if (i!=k) m[i][k]/=( -pvt_val) ;
356    }
[8]357
[151]358    /* Reduce the matrix */
359    for (i=0; i<4; i++) {
360      hold = m[i][k];
361      for (j=0; j<4; j++) {
362        if (i!=k && j!=k) m[i][j]+=hold*m[k][j];
363      }
364    }
[8]365
[151]366    /* Divide row by pivot */
367    for (j=0; j<4; j++) {
368      if (j!=k) m[k][j]/=pvt_val;
369    }
[8]370
[151]371    /* Replace pivot by reciprocal (at last we can touch it). */
372    m[k][k] = 1.0f/pvt_val;
373  }
[8]374
[151]375  /* That was most of the work, one final pass of row/column interchange */
376  /* to finish */
377  for (k=4-2; k>=0; k--) { /* Don't need to work with 1 by 1 corner*/
378    i=pvt_j[k];            /* Rows to swap correspond to pivot COLUMN */
379    if (i!=k) {            /* If rows are different */
380      for(j=0; j<4; j++) {
381        hold = m[k][j];
382        m[k][j]=-m[i][j];
383        m[i][j]=hold;
384      }
[8]385    }
386
[151]387    j=pvt_i[k];           /* Columns to swap correspond to pivot ROW */
388    if (j!=k)             /* If columns are different */
389    for (i=0; i<4; i++) {
390      hold=m[i][k];
391      m[i][k]=-m[i][j];
392      m[i][j]=hold;
[8]393    }
[151]394  }
395  return(LIB3DS_TRUE);                         
[8]396}
397
398
399/*!
400 * \ingroup matrix
401 */
402void
403lib3ds_matrix_translate_xyz(Lib3dsMatrix m, Lib3dsFloat x, Lib3dsFloat y, Lib3dsFloat z)
404{
[151]405  int i;
406 
407  for (i=0; i<3; i++) {
408    m[3][i]+= m[0][i]*x + m[1][i]*y + m[2][i]*z;
409  }
[8]410}
411
412
413/*!
414 * \ingroup matrix
415 */
416void
417lib3ds_matrix_translate(Lib3dsMatrix m, Lib3dsVector t)
418{
[151]419  int i;
420 
421  for (i=0; i<3; i++) {
422    m[3][i]+= m[0][i]*t[0] + m[1][i]*t[1] + m[2][i]*t[2];
423  }
[8]424}
425
426
427/*!
428 * \ingroup matrix
429 */
430void
431lib3ds_matrix_scale_xyz(Lib3dsMatrix m, Lib3dsFloat x, Lib3dsFloat y, Lib3dsFloat z)
432{
[151]433  int i;
[8]434
[151]435  for (i=0; i<4; i++) {
436    m[0][i]*=x;
437    m[1][i]*=y;
438    m[2][i]*=z;
439  }
[8]440}
441
442
443/*!
444 * \ingroup matrix
445 */
446void
447lib3ds_matrix_scale(Lib3dsMatrix m, Lib3dsVector s)
448{
[151]449  int i;
[8]450
[151]451  for (i=0; i<4; i++) {
452    m[0][i]*=s[0];
453    m[1][i]*=s[1];
454    m[2][i]*=s[2];
455  }
[8]456}
457
458
459/*!
460 * \ingroup matrix
461 */
462void
463lib3ds_matrix_rotate_x(Lib3dsMatrix m, Lib3dsFloat phi)
464{
[151]465  Lib3dsFloat SinPhi,CosPhi;
466  Lib3dsFloat a1[4],a2[4];
[8]467
[151]468  SinPhi=(Lib3dsFloat)sin(phi);
469  CosPhi=(Lib3dsFloat)cos(phi);
470  memcpy(a1,m[1],4*sizeof(Lib3dsFloat));
471  memcpy(a2,m[2],4*sizeof(Lib3dsFloat));
472  m[1][0]=CosPhi*a1[0]+SinPhi*a2[0];
473  m[1][1]=CosPhi*a1[1]+SinPhi*a2[1];
474  m[1][2]=CosPhi*a1[2]+SinPhi*a2[2];
475  m[1][3]=CosPhi*a1[3]+SinPhi*a2[3];
476  m[2][0]=-SinPhi*a1[0]+CosPhi*a2[0];
477  m[2][1]=-SinPhi*a1[1]+CosPhi*a2[1];
478  m[2][2]=-SinPhi*a1[2]+CosPhi*a2[2];
479  m[2][3]=-SinPhi*a1[3]+CosPhi*a2[3];
[8]480}
481
482
483/*!
484 * \ingroup matrix
485 */
486void
487lib3ds_matrix_rotate_y(Lib3dsMatrix m, Lib3dsFloat phi)
488{
[151]489  Lib3dsFloat SinPhi,CosPhi;
490  Lib3dsFloat a0[4],a2[4];
[8]491
[151]492  SinPhi=(Lib3dsFloat)sin(phi);
493  CosPhi=(Lib3dsFloat)cos(phi);
494  memcpy(a0,m[0],4*sizeof(Lib3dsFloat));
495  memcpy(a2,m[2],4*sizeof(Lib3dsFloat));
496  m[0][0]=CosPhi*a0[0]-SinPhi*a2[0];
497  m[0][1]=CosPhi*a0[1]-SinPhi*a2[1];
498  m[0][2]=CosPhi*a0[2]-SinPhi*a2[2];
499  m[0][3]=CosPhi*a0[3]-SinPhi*a2[3];
500  m[2][0]=SinPhi*a0[0]+CosPhi*a2[0];
501  m[2][1]=SinPhi*a0[1]+CosPhi*a2[1];
502  m[2][2]=SinPhi*a0[2]+CosPhi*a2[2];
503  m[2][3]=SinPhi*a0[3]+CosPhi*a2[3];
[8]504}
505
506
507/*!
508 * \ingroup matrix
509 */
510void
511lib3ds_matrix_rotate_z(Lib3dsMatrix m, Lib3dsFloat phi)
512{
[151]513  Lib3dsFloat SinPhi,CosPhi;
514  Lib3dsFloat a0[4],a1[4];
515 
516  SinPhi=(Lib3dsFloat)sin(phi);
517  CosPhi=(Lib3dsFloat)cos(phi);
518  memcpy(a0,m[0],4*sizeof(Lib3dsFloat));
519  memcpy(a1,m[1],4*sizeof(Lib3dsFloat));
520  m[0][0]=CosPhi*a0[0]+SinPhi*a1[0];
521  m[0][1]=CosPhi*a0[1]+SinPhi*a1[1];
522  m[0][2]=CosPhi*a0[2]+SinPhi*a1[2];
523  m[0][3]=CosPhi*a0[3]+SinPhi*a1[3];
524  m[1][0]=-SinPhi*a0[0]+CosPhi*a1[0];
525  m[1][1]=-SinPhi*a0[1]+CosPhi*a1[1];
526  m[1][2]=-SinPhi*a0[2]+CosPhi*a1[2];
527  m[1][3]=-SinPhi*a0[3]+CosPhi*a1[3];
[8]528}
529
530
531/*!
532 * \ingroup matrix
533 */
534void
535lib3ds_matrix_rotate(Lib3dsMatrix m, Lib3dsQuat q)
536{
[151]537  Lib3dsFloat s,xs,ys,zs,wx,wy,wz,xx,xy,xz,yy,yz,zz,l;
538  Lib3dsMatrix a,b;
[8]539
[151]540  lib3ds_matrix_copy(a, m);
[8]541
[151]542  l=q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3];
543  if (fabs(l)<LIB3DS_EPSILON) {
544    s=1.0f;
545  }
546  else {
547    s=2.0f/l;
548  }
[8]549
[151]550  xs = q[0] * s;   ys = q[1] * s;  zs = q[2] * s;
551  wx = q[3] * xs;  wy = q[3] * ys; wz = q[3] * zs;
552  xx = q[0] * xs;  xy = q[0] * ys; xz = q[0] * zs;
553  yy = q[1] * ys;  yz = q[1] * zs; zz = q[2] * zs;
[8]554
[151]555  b[0][0]=1.0f - (yy +zz);
556  b[1][0]=xy - wz;
557  b[2][0]=xz + wy;
558  b[0][1]=xy + wz;
559  b[1][1]=1.0f - (xx +zz);
560  b[2][1]=yz - wx;
561  b[0][2]=xz - wy;
562  b[1][2]=yz + wx;
563  b[2][2]=1.0f - (xx + yy);
564  b[3][0]=b[3][1]=b[3][2]=b[0][3]=b[1][3]=b[2][3]=0.0f;
565  b[3][3]=1.0f;
[8]566
[151]567  lib3ds_matrix_mul(m,a,b);
[8]568}
569
570
571/*!
572 * \ingroup matrix
573 */
574void
575lib3ds_matrix_rotate_axis(Lib3dsMatrix m, Lib3dsVector axis, Lib3dsFloat angle)
576{
[151]577  Lib3dsQuat q;
578 
579  lib3ds_quat_axis_angle(q,axis,angle);
580  lib3ds_matrix_rotate(m,q);
[8]581}
582
583
584/*!
585 * \ingroup matrix
586 */
587void
588lib3ds_matrix_camera(Lib3dsMatrix matrix, Lib3dsVector pos,
[151]589  Lib3dsVector tgt, Lib3dsFloat roll)
[8]590{
[151]591  Lib3dsMatrix M,R;
592  Lib3dsVector x, y, z;
[8]593
[151]594  lib3ds_vector_sub(y, tgt, pos);
595  lib3ds_vector_normalize(y);
596 
597  z[0] = 0;
598  z[1] = 0;
599  z[2] = 1.0;
600 
601  lib3ds_vector_cross(x, y, z);
602  lib3ds_vector_cross(z, x, y);
603  lib3ds_vector_normalize(x);
604  lib3ds_vector_normalize(y);
[8]605
[151]606  lib3ds_matrix_identity(M);
607  M[0][0] = x[0];
608  M[1][0] = x[1];
609  M[2][0] = x[2];
610  M[0][1] = y[0];
611  M[1][1] = y[1];
612  M[2][1] = y[2];
613  M[0][2] = z[0];
614  M[1][2] = z[1];
615  M[2][2] = z[2];
[8]616
[151]617  lib3ds_matrix_identity(R);
618  lib3ds_matrix_rotate_y(R, roll);
619  lib3ds_matrix_mul(matrix, R,M);
620  lib3ds_matrix_translate_xyz(matrix, -pos[0],-pos[1],-pos[2]);
[8]621}
622
623
624/*!
625 * \ingroup matrix
626 */
627void
628lib3ds_matrix_dump(Lib3dsMatrix matrix)
629{
[151]630  int i,j;
[8]631
[151]632  for (i=0; i<4; ++i) {
[270]633    for (j=0; j<3; ++j) {
[151]634      printf("%f ", matrix[j][i]);
[8]635    }
[151]636    printf("%f\n", matrix[j][i]);
637  }
[8]638}
[151]639
640
641
642
Note: See TracBrowser for help on using the browser.