← Back to branch summary

~ubuntu-branches/debian/experimental/openscenegraph/experimental

« back to all changes in this revision

Viewing changes to OpenSceneGraph/src/osgPlugins/3ds/lib3ds/lib3ds_matrix.c

  • Committer: Bazaar Package Importer
  • Author(s): Alberto Luaces
  • Date: 2011-01-29 11:36:29 UTC
  • mfrom: (1.1.10 upstream)
  • Revision ID: james.westby@ubuntu.com-20110129113629-qisrm2kdqlurc7t3
Tags: 2.9.11-1
http://bugs.debian.org/592420
* Removed bug-555869-ftbfs_with_binutils_gold.dpatch since upstream has
  already taken care of the issue.
* Removed bug-528229.dpatch since the pkgconfig files are now also split
  in upstream.
* Removed explicit dependency on GLU.
* Upstream no longer includes osgIntrospection (Closes: #592420).
* Disabled zip plugin as its implementation stores an embedded copy of
  zlib.
* Enabled Qt support. Thanks James Goppert.
* Enabled SVG and PDF plugins. Thanks James Goppert.

Show diffs side-by-side

added added

removed removed

Lines of Context:
OpenSceneGraph/src/osgPlugins/3ds/lib3ds/lib3ds_matrix.c
1
 
/*
2
 
    Copyright (C) 1996-2008 by Jan Eric Kyprianidis <www.kyprianidis.com>
3
 
    All rights reserved.
4
 
    
5
 
    This program is free  software: you can redistribute it and/or modify 
6
 
    it under the terms of the GNU Lesser General Public License as published 
7
 
    by the Free Software Foundation, either version 2.1 of the License, or 
8
 
    (at your option) any later version.
9
 
 
10
 
    Thisprogram  is  distributed in the hope that it will be useful, 
11
 
    but WITHOUT ANY WARRANTY; without even the implied warranty of 
12
 
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 
13
 
    GNU Lesser General Public License for more details.
14
 
    
15
 
    You should  have received a copy of the GNU Lesser General Public License
16
 
    along with  this program; If not, see <http://www.gnu.org/licenses/>. 
17
 
*/
18
 
#include "lib3ds_impl.h"
19
 
 
20
 
 
21
 
/*!
22
 
 * Clear a matrix to all zeros.
23
 
 *
24
 
 * \param m Matrix to be cleared.
25
 
 */
26
 
void
27
 
lib3ds_matrix_zero(float m[4][4]) {
28
 
    int i, j;
29
 
 
30
 
    for (i = 0; i < 4; i++) {
31
 
        for (j = 0; j < 4; j++) m[i][j] = 0.0f;
32
 
    }
33
 
}
34
 
 
35
 
 
36
 
/*!
37
 
 * Set a matrix to identity.
38
 
 *
39
 
 * \param m Matrix to be set.
40
 
 */
41
 
void
42
 
lib3ds_matrix_identity(float m[4][4]) {
43
 
    int i, j;
44
 
 
45
 
    for (i = 0; i < 4; i++) {
46
 
        for (j = 0; j < 4; j++) m[i][j] = 0.0;
47
 
    }
48
 
    for (i = 0; i < 4; i++) m[i][i] = 1.0;
49
 
}
50
 
 
51
 
 
52
 
/*!
53
 
 * Copy a matrix.
54
 
 */
55
 
void
56
 
lib3ds_matrix_copy(float dest[4][4], float src[4][4]) {
57
 
    memcpy(dest, src, 16 * sizeof(float));
58
 
}
59
 
 
60
 
 
61
 
/*!
62
 
 * Negate a matrix -- all elements negated.
63
 
 */
64
 
void
65
 
lib3ds_matrix_neg(float m[4][4]) {
66
 
    int i, j;
67
 
 
68
 
    for (j = 0; j < 4; j++) {
69
 
        for (i = 0; i < 4; i++) {
70
 
            m[j][i] = -m[j][i];
71
 
        }
72
 
    }
73
 
}
74
 
 
75
 
 
76
 
/*!
77
 
 * Transpose a matrix in place.
78
 
 */
79
 
void
80
 
lib3ds_matrix_transpose(float m[4][4]) {
81
 
    int i, j;
82
 
    float swp;
83
 
 
84
 
    for (j = 0; j < 4; j++) {
85
 
        for (i = j + 1; i < 4; i++) {
86
 
            swp = m[j][i];
87
 
            m[j][i] = m[i][j];
88
 
            m[i][j] = swp;
89
 
        }
90
 
    }
91
 
}
92
 
 
93
 
 
94
 
/*!
95
 
 * Add two matrices.
96
 
 */
97
 
void
98
 
lib3ds_matrix_add(float m[4][4], float a[4][4], float b[4][4]) {
99
 
    int i, j;
100
 
 
101
 
    for (j = 0; j < 4; j++) {
102
 
        for (i = 0; i < 4; i++) {
103
 
            m[j][i] = a[j][i] + b[j][i];
104
 
        }
105
 
    }
106
 
}
107
 
 
108
 
 
109
 
/*!
110
 
 * Subtract two matrices.
111
 
 *
112
 
 * \param m Result.
113
 
 * \param a Addend.
114
 
 * \param b Minuend.
115
 
 */
116
 
void
117
 
lib3ds_matrix_sub(float m[4][4], float a[4][4], float b[4][4]) {
118
 
    int i, j;
119
 
 
120
 
    for (j = 0; j < 4; j++) {
121
 
        for (i = 0; i < 4; i++) {
122
 
            m[j][i] = a[j][i] - b[j][i];
123
 
        }
124
 
    }
125
 
}
126
 
 
127
 
 
128
 
/*!
129
 
 * Multiplies a matrix by a second one (m = m * n).
130
 
 */
131
 
void
132
 
lib3ds_matrix_mult(float m[4][4], float a[4][4], float b[4][4]) {
133
 
    float tmp[4][4];
134
 
    int i, j, k;
135
 
    float ab;
136
 
 
137
 
    memcpy(tmp, a, 16 * sizeof(float));
138
 
    for (j = 0; j < 4; j++) {
139
 
        for (i = 0; i < 4; i++) {
140
 
            ab = 0.0f;
141
 
            for (k = 0; k < 4; k++) ab += tmp[k][i] * b[j][k];
142
 
            m[j][i] = ab;
143
 
        }
144
 
    }
145
 
}
146
 
 
147
 
 
148
 
/*!
149
 
 * Multiply a matrix by a scalar.
150
 
 *
151
 
 * \param m Matrix to be set.
152
 
 * \param k Scalar.
153
 
 */
154
 
void
155
 
lib3ds_matrix_scalar(float m[4][4], float k) {
156
 
    int i, j;
157
 
 
158
 
    for (j = 0; j < 4; j++) {
159
 
        for (i = 0; i < 4; i++) {
160
 
            m[j][i] *= k;
161
 
        }
162
 
    }
163
 
}
164
 
 
165
 
 
166
 
static float
167
 
det2x2(
168
 
    float a, float b,
169
 
    float c, float d) {
170
 
    return((a)*(d) - (b)*(c));
171
 
}
172
 
 
173
 
 
174
 
static float
175
 
det3x3(
176
 
    float a1, float a2, float a3,
177
 
    float b1, float b2, float b3,
178
 
    float c1, float c2, float c3) {
179
 
    return(
180
 
              a1*det2x2(b2, b3, c2, c3) -
181
 
              b1*det2x2(a2, a3, c2, c3) +
182
 
              c1*det2x2(a2, a3, b2, b3)
183
 
          );
184
 
}
185
 
 
186
 
 
187
 
/*!
188
 
 * Find determinant of a matrix.
189
 
 */
190
 
float
191
 
lib3ds_matrix_det(float m[4][4]) {
192
 
    float a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4;
193
 
 
194
 
    a1 = m[0][0];
195
 
    b1 = m[1][0];
196
 
    c1 = m[2][0];
197
 
    d1 = m[3][0];
198
 
    a2 = m[0][1];
199
 
    b2 = m[1][1];
200
 
    c2 = m[2][1];
201
 
    d2 = m[3][1];
202
 
    a3 = m[0][2];
203
 
    b3 = m[1][2];
204
 
    c3 = m[2][2];
205
 
    d3 = m[3][2];
206
 
    a4 = m[0][3];
207
 
    b4 = m[1][3];
208
 
    c4 = m[2][3];
209
 
    d4 = m[3][3];
210
 
    return(
211
 
              a1 * det3x3(b2, b3, b4, c2, c3, c4, d2, d3, d4) -
212
 
              b1 * det3x3(a2, a3, a4, c2, c3, c4, d2, d3, d4) +
213
 
              c1 * det3x3(a2, a3, a4, b2, b3, b4, d2, d3, d4) -
214
 
              d1 * det3x3(a2, a3, a4, b2, b3, b4, c2, c3, c4)
215
 
          );
216
 
}
217
 
 
218
 
 
219
 
/*!
220
 
 * Invert a matrix in place.
221
 
 *
222
 
 * \param m Matrix to invert.
223
 
 *
224
 
 * \return LIB3DS_TRUE on success, LIB3DS_FALSE on failure.
225
 
 *
226
 
 * GGemsII, K.Wu, Fast Matrix Inversion
227
 
 */
228
 
int
229
 
lib3ds_matrix_inv(float m[4][4]) {
230
 
    int i, j, k;
231
 
    int pvt_i[4], pvt_j[4];            /* Locations of pivot elements */
232
 
    float pvt_val;               /* Value of current pivot element */
233
 
    float hold;                  /* Temporary storage */
234
 
    float determinat;
235
 
 
236
 
    determinat = 1.0f;
237
 
    for (k = 0; k < 4; k++)  {
238
 
        /* Locate k'th pivot element */
239
 
        pvt_val = m[k][k];          /* Initialize for search */
240
 
        pvt_i[k] = k;
241
 
        pvt_j[k] = k;
242
 
        for (i = k; i < 4; i++) {
243
 
            for (j = k; j < 4; j++) {
244
 
                if (fabs(m[i][j]) > fabs(pvt_val)) {
245
 
                    pvt_i[k] = i;
246
 
                    pvt_j[k] = j;
247
 
                    pvt_val = m[i][j];
248
 
                }
249
 
            }
250
 
        }
251
 
 
252
 
        /* Product of pivots, gives determinant when finished */
253
 
        determinat *= pvt_val;
254
 
        if (fabs(determinat) < LIB3DS_EPSILON) {
255
 
            return(FALSE);  /* Matrix is singular (zero determinant) */
256
 
        }
257
 
 
258
 
        /* "Interchange" rows (with sign change stuff) */
259
 
        i = pvt_i[k];
260
 
        if (i != k) {             /* If rows are different */
261
 
            for (j = 0; j < 4; j++) {
262
 
                hold = -m[k][j];
263
 
                m[k][j] = m[i][j];
264
 
                m[i][j] = hold;
265
 
            }
266
 
        }
267
 
 
268
 
        /* "Interchange" columns */
269
 
        j = pvt_j[k];
270
 
        if (j != k) {            /* If columns are different */
271
 
            for (i = 0; i < 4; i++) {
272
 
                hold = -m[i][k];
273
 
                m[i][k] = m[i][j];
274
 
                m[i][j] = hold;
275
 
            }
276
 
        }
277
 
 
278
 
        /* Divide column by minus pivot value */
279
 
        for (i = 0; i < 4; i++) {
280
 
            if (i != k) m[i][k] /= (-pvt_val) ;
281
 
        }
282
 
 
283
 
        /* Reduce the matrix */
284
 
        for (i = 0; i < 4; i++) {
285
 
            hold = m[i][k];
286
 
            for (j = 0; j < 4; j++) {
287
 
                if (i != k && j != k) m[i][j] += hold * m[k][j];
288
 
            }
289
 
        }
290
 
 
291
 
        /* Divide row by pivot */
292
 
        for (j = 0; j < 4; j++) {
293
 
            if (j != k) m[k][j] /= pvt_val;
294
 
        }
295
 
 
296
 
        /* Replace pivot by reciprocal (at last we can touch it). */
297
 
        m[k][k] = 1.0f / pvt_val;
298
 
    }
299
 
 
300
 
    /* That was most of the work, one final pass of row/column interchange */
301
 
    /* to finish */
302
 
    for (k = 4 - 2; k >= 0; k--) { /* Don't need to work with 1 by 1 corner*/
303
 
        i = pvt_j[k];          /* Rows to swap correspond to pivot COLUMN */
304
 
        if (i != k) {          /* If rows are different */
305
 
            for (j = 0; j < 4; j++) {
306
 
                hold = m[k][j];
307
 
                m[k][j] = -m[i][j];
308
 
                m[i][j] = hold;
309
 
            }
310
 
        }
311
 
 
312
 
        j = pvt_i[k];         /* Columns to swap correspond to pivot ROW */
313
 
        if (j != k)           /* If columns are different */
314
 
            for (i = 0; i < 4; i++) {
315
 
                hold = m[i][k];
316
 
                m[i][k] = -m[i][j];
317
 
                m[i][j] = hold;
318
 
            }
319
 
    }
320
 
    return(TRUE);
321
 
}
322
 
 
323
 
 
324
 
/*!
325
 
 * Apply a translation to a matrix.
326
 
 */
327
 
void
328
 
lib3ds_matrix_translate(float m[4][4], float x, float y, float z) {
329
 
    int i;
330
 
 
331
 
    for (i = 0; i < 3; i++) {
332
 
        m[3][i] += m[0][i] * x + m[1][i] * y + m[2][i] * z;
333
 
    }
334
 
}
335
 
 
336
 
 
337
 
/*!
338
 
 * Apply scale factors to a matrix.
339
 
 */
340
 
void
341
 
lib3ds_matrix_scale(float m[4][4], float x, float y, float z) {
342
 
    int i;
343
 
 
344
 
    for (i = 0; i < 4; i++) {
345
 
        m[0][i] *= x;
346
 
        m[1][i] *= y;
347
 
        m[2][i] *= z;
348
 
    }
349
 
}
350
 
 
351
 
 
352
 
/*!
353
 
 * Apply a rotation about an arbitrary axis to a matrix.
354
 
 */
355
 
void
356
 
lib3ds_matrix_rotate_quat(float m[4][4], float q[4]) {
357
 
    float s, xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz, l;
358
 
    float R[4][4];
359
 
 
360
 
    l = q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3];
361
 
    if (fabs(l) < LIB3DS_EPSILON) {
362
 
        s = 1.0f;
363
 
    } else {
364
 
        s = 2.0f / l;
365
 
    }
366
 
 
367
 
    xs = q[0] * s;
368
 
    ys = q[1] * s;
369
 
    zs = q[2] * s;
370
 
    wx = q[3] * xs;
371
 
    wy = q[3] * ys;
372
 
    wz = q[3] * zs;
373
 
    xx = q[0] * xs;
374
 
    xy = q[0] * ys;
375
 
    xz = q[0] * zs;
376
 
    yy = q[1] * ys;
377
 
    yz = q[1] * zs;
378
 
    zz = q[2] * zs;
379
 
 
380
 
    R[0][0] = 1.0f - (yy + zz);
381
 
    R[1][0] = xy - wz;
382
 
    R[2][0] = xz + wy;
383
 
    R[0][1] = xy + wz;
384
 
    R[1][1] = 1.0f - (xx + zz);
385
 
    R[2][1] = yz - wx;
386
 
    R[0][2] = xz - wy;
387
 
    R[1][2] = yz + wx;
388
 
    R[2][2] = 1.0f - (xx + yy);
389
 
    R[3][0] = R[3][1] = R[3][2] = R[0][3] = R[1][3] = R[2][3] = 0.0f;
390
 
    R[3][3] = 1.0f;
391
 
 
392
 
    lib3ds_matrix_mult(m, m, R);
393
 
}
394
 
 
395
 
 
396
 
/*!
397
 
 * Apply a rotation about an arbitrary axis to a matrix.
398
 
 */
399
 
void
400
 
lib3ds_matrix_rotate(float m[4][4], float angle, float ax, float ay, float az) {
401
 
    float q[4];
402
 
    float axis[3];
403
 
 
404
 
    lib3ds_vector_make(axis, ax, ay, az);
405
 
    lib3ds_quat_axis_angle(q, axis, angle);
406
 
    lib3ds_matrix_rotate_quat(m, q);
407
 
}
408
 
 
409
 
 
410
 
/*!
411
 
 * Compute a camera matrix based on position, target and roll.
412
 
 *
413
 
 * Generates a translate/rotate matrix that maps world coordinates
414
 
 * to camera coordinates.  Resulting matrix does not include perspective
415
 
 * transform.
416
 
 *
417
 
 * \param matrix Destination matrix.
418
 
 * \param pos Camera position
419
 
 * \param tgt Camera target
420
 
 * \param roll Roll angle
421
 
 */
422
 
void
423
 
lib3ds_matrix_camera(float matrix[4][4], float pos[3], float tgt[3], float roll) {
424
 
    float M[4][4];
425
 
    float x[3], y[3], z[3];
426
 
 
427
 
    lib3ds_vector_sub(y, tgt, pos);
428
 
    lib3ds_vector_normalize(y);
429
 
 
430
 
    if (y[0] != 0. || y[1] != 0) {
431
 
        z[0] = 0;
432
 
        z[1] = 0;
433
 
        z[2] = 1.0;
434
 
    } else { /* Special case:  looking straight up or down z axis */
435
 
        z[0] = -1.0;
436
 
        z[1] = 0;
437
 
        z[2] = 0;
438
 
    }
439
 
 
440
 
    lib3ds_vector_cross(x, y, z);
441
 
    lib3ds_vector_cross(z, x, y);
442
 
    lib3ds_vector_normalize(x);
443
 
    lib3ds_vector_normalize(z);
444
 
 
445
 
    lib3ds_matrix_identity(M);
446
 
    M[0][0] = x[0];
447
 
    M[1][0] = x[1];
448
 
    M[2][0] = x[2];
449
 
    M[0][1] = y[0];
450
 
    M[1][1] = y[1];
451
 
    M[2][1] = y[2];
452
 
    M[0][2] = z[0];
453
 
    M[1][2] = z[1];
454
 
    M[2][2] = z[2];
455
 
 
456
 
    lib3ds_matrix_identity(matrix);
457
 
    lib3ds_matrix_rotate(matrix, roll, 0, 1, 0);
458
 
    lib3ds_matrix_mult(matrix, matrix, M);
459
 
    lib3ds_matrix_translate(matrix, -pos[0], -pos[1], -pos[2]);
460
 
}
461
 
 
462
 
 
463
 
 
464
 
 
465
 
 
466
 
 
 
1
/*
 
2
    Copyright (C) 1996-2008 by Jan Eric Kyprianidis <www.kyprianidis.com>
 
3
    All rights reserved.
 
4
    
 
5
    This program is free  software: you can redistribute it and/or modify 
 
6
    it under the terms of the GNU Lesser General Public License as published 
 
7
    by the Free Software Foundation, either version 2.1 of the License, or 
 
8
    (at your option) any later version.
 
9
 
 
10
    Thisprogram  is  distributed in the hope that it will be useful, 
 
11
    but WITHOUT ANY WARRANTY; without even the implied warranty of 
 
12
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 
 
13
    GNU Lesser General Public License for more details.
 
14
    
 
15
    You should  have received a copy of the GNU Lesser General Public License
 
16
    along with  this program; If not, see <http://www.gnu.org/licenses/>. 
 
17
*/
 
18
#include "lib3ds_impl.h"
 
19
 
 
20
 
 
21
/*!
 
22
 * Clear a matrix to all zeros.
 
23
 *
 
24
 * \param m Matrix to be cleared.
 
25
 */
 
26
void
 
27
lib3ds_matrix_zero(float m[4][4]) {
 
28
    int i, j;
 
29
 
 
30
    for (i = 0; i < 4; i++) {
 
31
        for (j = 0; j < 4; j++) m[i][j] = 0.0f;
 
32
    }
 
33
}
 
34
 
 
35
 
 
36
/*!
 
37
 * Set a matrix to identity.
 
38
 *
 
39
 * \param m Matrix to be set.
 
40
 */
 
41
void
 
42
lib3ds_matrix_identity(float m[4][4]) {
 
43
    int i, j;
 
44
 
 
45
    for (i = 0; i < 4; i++) {
 
46
        for (j = 0; j < 4; j++) m[i][j] = 0.0;
 
47
    }
 
48
    for (i = 0; i < 4; i++) m[i][i] = 1.0;
 
49
}
 
50
 
 
51
 
 
52
/*!
 
53
 * Copy a matrix.
 
54
 */
 
55
void
 
56
lib3ds_matrix_copy(float dest[4][4], float src[4][4]) {
 
57
    memcpy(dest, src, 16 * sizeof(float));
 
58
}
 
59
 
 
60
 
 
61
/*!
 
62
 * Negate a matrix -- all elements negated.
 
63
 */
 
64
void
 
65
lib3ds_matrix_neg(float m[4][4]) {
 
66
    int i, j;
 
67
 
 
68
    for (j = 0; j < 4; j++) {
 
69
        for (i = 0; i < 4; i++) {
 
70
            m[j][i] = -m[j][i];
 
71
        }
 
72
    }
 
73
}
 
74
 
 
75
 
 
76
/*!
 
77
 * Transpose a matrix in place.
 
78
 */
 
79
void
 
80
lib3ds_matrix_transpose(float m[4][4]) {
 
81
    int i, j;
 
82
    float swp;
 
83
 
 
84
    for (j = 0; j < 4; j++) {
 
85
        for (i = j + 1; i < 4; i++) {
 
86
            swp = m[j][i];
 
87
            m[j][i] = m[i][j];
 
88
            m[i][j] = swp;
 
89
        }
 
90
    }
 
91
}
 
92
 
 
93
 
 
94
/*!
 
95
 * Add two matrices.
 
96
 */
 
97
void
 
98
lib3ds_matrix_add(float m[4][4], float a[4][4], float b[4][4]) {
 
99
    int i, j;
 
100
 
 
101
    for (j = 0; j < 4; j++) {
 
102
        for (i = 0; i < 4; i++) {
 
103
            m[j][i] = a[j][i] + b[j][i];
 
104
        }
 
105
    }
 
106
}
 
107
 
 
108
 
 
109
/*!
 
110
 * Subtract two matrices.
 
111
 *
 
112
 * \param m Result.
 
113
 * \param a Addend.
 
114
 * \param b Minuend.
 
115
 */
 
116
void
 
117
lib3ds_matrix_sub(float m[4][4], float a[4][4], float b[4][4]) {
 
118
    int i, j;
 
119
 
 
120
    for (j = 0; j < 4; j++) {
 
121
        for (i = 0; i < 4; i++) {
 
122
            m[j][i] = a[j][i] - b[j][i];
 
123
        }
 
124
    }
 
125
}
 
126
 
 
127
 
 
128
/*!
 
129
 * Multiplies a matrix by a second one (m = m * n).
 
130
 */
 
131
void
 
132
lib3ds_matrix_mult(float m[4][4], float a[4][4], float b[4][4]) {
 
133
    float tmp[4][4];
 
134
    int i, j, k;
 
135
    float ab;
 
136
 
 
137
    memcpy(tmp, a, 16 * sizeof(float));
 
138
    for (j = 0; j < 4; j++) {
 
139
        for (i = 0; i < 4; i++) {
 
140
            ab = 0.0f;
 
141
            for (k = 0; k < 4; k++) ab += tmp[k][i] * b[j][k];
 
142
            m[j][i] = ab;
 
143
        }
 
144
    }
 
145
}
 
146
 
 
147
 
 
148
/*!
 
149
 * Multiply a matrix by a scalar.
 
150
 *
 
151
 * \param m Matrix to be set.
 
152
 * \param k Scalar.
 
153
 */
 
154
void
 
155
lib3ds_matrix_scalar(float m[4][4], float k) {
 
156
    int i, j;
 
157
 
 
158
    for (j = 0; j < 4; j++) {
 
159
        for (i = 0; i < 4; i++) {
 
160
            m[j][i] *= k;
 
161
        }
 
162
    }
 
163
}
 
164
 
 
165
 
 
166
static float
 
167
det2x2(
 
168
    float a, float b,
 
169
    float c, float d) {
 
170
    return((a)*(d) - (b)*(c));
 
171
}
 
172
 
 
173
 
 
174
static float
 
175
det3x3(
 
176
    float a1, float a2, float a3,
 
177
    float b1, float b2, float b3,
 
178
    float c1, float c2, float c3) {
 
179
    return(
 
180
              a1*det2x2(b2, b3, c2, c3) -
 
181
              b1*det2x2(a2, a3, c2, c3) +
 
182
              c1*det2x2(a2, a3, b2, b3)
 
183
          );
 
184
}
 
185
 
 
186
 
 
187
/*!
 
188
 * Find determinant of a matrix.
 
189
 */
 
190
float
 
191
lib3ds_matrix_det(float m[4][4]) {
 
192
    float a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4;
 
193
 
 
194
    a1 = m[0][0];
 
195
    b1 = m[1][0];
 
196
    c1 = m[2][0];
 
197
    d1 = m[3][0];
 
198
    a2 = m[0][1];
 
199
    b2 = m[1][1];
 
200
    c2 = m[2][1];
 
201
    d2 = m[3][1];
 
202
    a3 = m[0][2];
 
203
    b3 = m[1][2];
 
204
    c3 = m[2][2];
 
205
    d3 = m[3][2];
 
206
    a4 = m[0][3];
 
207
    b4 = m[1][3];
 
208
    c4 = m[2][3];
 
209
    d4 = m[3][3];
 
210
    return(
 
211
              a1 * det3x3(b2, b3, b4, c2, c3, c4, d2, d3, d4) -
 
212
              b1 * det3x3(a2, a3, a4, c2, c3, c4, d2, d3, d4) +
 
213
              c1 * det3x3(a2, a3, a4, b2, b3, b4, d2, d3, d4) -
 
214
              d1 * det3x3(a2, a3, a4, b2, b3, b4, c2, c3, c4)
 
215
          );
 
216
}
 
217
 
 
218
 
 
219
/*!
 
220
 * Invert a matrix in place.
 
221
 *
 
222
 * \param m Matrix to invert.
 
223
 *
 
224
 * \return LIB3DS_TRUE on success, LIB3DS_FALSE on failure.
 
225
 *
 
226
 * GGemsII, K.Wu, Fast Matrix Inversion
 
227
 */
 
228
int
 
229
lib3ds_matrix_inv(float m[4][4]) {
 
230
    int i, j, k;
 
231
    int pvt_i[4], pvt_j[4];            /* Locations of pivot elements */
 
232
    float pvt_val;               /* Value of current pivot element */
 
233
    float hold;                  /* Temporary storage */
 
234
    float determinat;
 
235
 
 
236
    determinat = 1.0f;
 
237
    for (k = 0; k < 4; k++)  {
 
238
        /* Locate k'th pivot element */
 
239
        pvt_val = m[k][k];          /* Initialize for search */
 
240
        pvt_i[k] = k;
 
241
        pvt_j[k] = k;
 
242
        for (i = k; i < 4; i++) {
 
243
            for (j = k; j < 4; j++) {
 
244
                if (fabs(m[i][j]) > fabs(pvt_val)) {
 
245
                    pvt_i[k] = i;
 
246
                    pvt_j[k] = j;
 
247
                    pvt_val = m[i][j];
 
248
                }
 
249
            }
 
250
        }
 
251
 
 
252
        /* Product of pivots, gives determinant when finished */
 
253
        determinat *= pvt_val;
 
254
        if (fabs(determinat) < LIB3DS_EPSILON) {
 
255
            return(FALSE);  /* Matrix is singular (zero determinant) */
 
256
        }
 
257
 
 
258
        /* "Interchange" rows (with sign change stuff) */
 
259
        i = pvt_i[k];
 
260
        if (i != k) {             /* If rows are different */
 
261
            for (j = 0; j < 4; j++) {
 
262
                hold = -m[k][j];
 
263
                m[k][j] = m[i][j];
 
264
                m[i][j] = hold;
 
265
            }
 
266
        }
 
267
 
 
268
        /* "Interchange" columns */
 
269
        j = pvt_j[k];
 
270
        if (j != k) {            /* If columns are different */
 
271
            for (i = 0; i < 4; i++) {
 
272
                hold = -m[i][k];
 
273
                m[i][k] = m[i][j];
 
274
                m[i][j] = hold;
 
275
            }
 
276
        }
 
277
 
 
278
        /* Divide column by minus pivot value */
 
279
        for (i = 0; i < 4; i++) {
 
280
            if (i != k) m[i][k] /= (-pvt_val) ;
 
281
        }
 
282
 
 
283
        /* Reduce the matrix */
 
284
        for (i = 0; i < 4; i++) {
 
285
            hold = m[i][k];
 
286
            for (j = 0; j < 4; j++) {
 
287
                if (i != k && j != k) m[i][j] += hold * m[k][j];
 
288
            }
 
289
        }
 
290
 
 
291
        /* Divide row by pivot */
 
292
        for (j = 0; j < 4; j++) {
 
293
            if (j != k) m[k][j] /= pvt_val;
 
294
        }
 
295
 
 
296
        /* Replace pivot by reciprocal (at last we can touch it). */
 
297
        m[k][k] = 1.0f / pvt_val;
 
298
    }
 
299
 
 
300
    /* That was most of the work, one final pass of row/column interchange */
 
301
    /* to finish */
 
302
    for (k = 4 - 2; k >= 0; k--) { /* Don't need to work with 1 by 1 corner*/
 
303
        i = pvt_j[k];          /* Rows to swap correspond to pivot COLUMN */
 
304
        if (i != k) {          /* If rows are different */
 
305
            for (j = 0; j < 4; j++) {
 
306
                hold = m[k][j];
 
307
                m[k][j] = -m[i][j];
 
308
                m[i][j] = hold;
 
309
            }
 
310
        }
 
311
 
 
312
        j = pvt_i[k];         /* Columns to swap correspond to pivot ROW */
 
313
        if (j != k)           /* If columns are different */
 
314
            for (i = 0; i < 4; i++) {
 
315
                hold = m[i][k];
 
316
                m[i][k] = -m[i][j];
 
317
                m[i][j] = hold;
 
318
            }
 
319
    }
 
320
    return(TRUE);
 
321
}
 
322
 
 
323
 
 
324
/*!
 
325
 * Apply a translation to a matrix.
 
326
 */
 
327
void
 
328
lib3ds_matrix_translate(float m[4][4], float x, float y, float z) {
 
329
    int i;
 
330
 
 
331
    for (i = 0; i < 3; i++) {
 
332
        m[3][i] += m[0][i] * x + m[1][i] * y + m[2][i] * z;
 
333
    }
 
334
}
 
335
 
 
336
 
 
337
/*!
 
338
 * Apply scale factors to a matrix.
 
339
 */
 
340
void
 
341
lib3ds_matrix_scale(float m[4][4], float x, float y, float z) {
 
342
    int i;
 
343
 
 
344
    for (i = 0; i < 4; i++) {
 
345
        m[0][i] *= x;
 
346
        m[1][i] *= y;
 
347
        m[2][i] *= z;
 
348
    }
 
349
}
 
350
 
 
351
 
 
352
/*!
 
353
 * Apply a rotation about an arbitrary axis to a matrix.
 
354
 */
 
355
void
 
356
lib3ds_matrix_rotate_quat(float m[4][4], float q[4]) {
 
357
    float s, xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz, l;
 
358
    float R[4][4];
 
359
 
 
360
    l = q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3];
 
361
    if (fabs(l) < LIB3DS_EPSILON) {
 
362
        s = 1.0f;
 
363
    } else {
 
364
        s = 2.0f / l;
 
365
    }
 
366
 
 
367
    xs = q[0] * s;
 
368
    ys = q[1] * s;
 
369
    zs = q[2] * s;
 
370
    wx = q[3] * xs;
 
371
    wy = q[3] * ys;
 
372
    wz = q[3] * zs;
 
373
    xx = q[0] * xs;
 
374
    xy = q[0] * ys;
 
375
    xz = q[0] * zs;
 
376
    yy = q[1] * ys;
 
377
    yz = q[1] * zs;
 
378
    zz = q[2] * zs;
 
379
 
 
380
    R[0][0] = 1.0f - (yy + zz);
 
381
    R[1][0] = xy - wz;
 
382
    R[2][0] = xz + wy;
 
383
    R[0][1] = xy + wz;
 
384
    R[1][1] = 1.0f - (xx + zz);
 
385
    R[2][1] = yz - wx;
 
386
    R[0][2] = xz - wy;
 
387
    R[1][2] = yz + wx;
 
388
    R[2][2] = 1.0f - (xx + yy);
 
389
    R[3][0] = R[3][1] = R[3][2] = R[0][3] = R[1][3] = R[2][3] = 0.0f;
 
390
    R[3][3] = 1.0f;
 
391
 
 
392
    lib3ds_matrix_mult(m, m, R);
 
393
}
 
394
 
 
395
 
 
396
/*!
 
397
 * Apply a rotation about an arbitrary axis to a matrix.
 
398
 */
 
399
void
 
400
lib3ds_matrix_rotate(float m[4][4], float angle, float ax, float ay, float az) {
 
401
    float q[4];
 
402
    float axis[3];
 
403
 
 
404
    lib3ds_vector_make(axis, ax, ay, az);
 
405
    lib3ds_quat_axis_angle(q, axis, angle);
 
406
    lib3ds_matrix_rotate_quat(m, q);
 
407
}
 
408
 
 
409
 
 
410
/*!
 
411
 * Compute a camera matrix based on position, target and roll.
 
412
 *
 
413
 * Generates a translate/rotate matrix that maps world coordinates
 
414
 * to camera coordinates.  Resulting matrix does not include perspective
 
415
 * transform.
 
416
 *
 
417
 * \param matrix Destination matrix.
 
418
 * \param pos Camera position
 
419
 * \param tgt Camera target
 
420
 * \param roll Roll angle
 
421
 */
 
422
void
 
423
lib3ds_matrix_camera(float matrix[4][4], float pos[3], float tgt[3], float roll) {
 
424
    float M[4][4];
 
425
    float x[3], y[3], z[3];
 
426
 
 
427
    lib3ds_vector_sub(y, tgt, pos);
 
428
    lib3ds_vector_normalize(y);
 
429
 
 
430
    if (y[0] != 0. || y[1] != 0) {
 
431
        z[0] = 0;
 
432
        z[1] = 0;
 
433
        z[2] = 1.0;
 
434
    } else { /* Special case:  looking straight up or down z axis */
 
435
        z[0] = -1.0;
 
436
        z[1] = 0;
 
437
        z[2] = 0;
 
438
    }
 
439
 
 
440
    lib3ds_vector_cross(x, y, z);
 
441
    lib3ds_vector_cross(z, x, y);
 
442
    lib3ds_vector_normalize(x);
 
443
    lib3ds_vector_normalize(z);
 
444
 
 
445
    lib3ds_matrix_identity(M);
 
446
    M[0][0] = x[0];
 
447
    M[1][0] = x[1];
 
448
    M[2][0] = x[2];
 
449
    M[0][1] = y[0];
 
450
    M[1][1] = y[1];
 
451
    M[2][1] = y[2];
 
452
    M[0][2] = z[0];
 
453
    M[1][2] = z[1];
 
454
    M[2][2] = z[2];
 
455
 
 
456
    lib3ds_matrix_identity(matrix);
 
457
    lib3ds_matrix_rotate(matrix, roll, 0, 1, 0);
 
458
    lib3ds_matrix_mult(matrix, matrix, M);
 
459
    lib3ds_matrix_translate(matrix, -pos[0], -pos[1], -pos[2]);
 
460
}
 
461
 
 
462
 
 
463
 
 
464
 
 
465
 
 
466