root/OpenSceneGraph/trunk/src/osgPlugins/3ds/lib3ds/lib3ds_matrix.c @ 10853

Revision 10853, 10.7 kB (checked in by robert, 4 years ago)

From Sukender,
"Here is our freshly baked 3DS reader/writer (named 'v0.5' to differentiate from previous one). Changes are against trunk rev. 10819.
Short changelog (from rev 10819):
- Added 3DS writer
- Sync'd with latest lib3DS
- Added options, especially "flattenMatrixTransforms" to get the "old" behaviour (else the reader correctly maps to OSG the transforms from the 3DS file).

What should be done:
- Check with pivot points, with and without "flattenMatrixTransforms" option.
- We ran tests on it, but we can never be 100% sure there is no bug. Testing from the community would of course be helpful."

RevLine 
[10853]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 */
26void
27lib3ds_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 */
41void
42lib3ds_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 */
55void
56lib3ds_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 */
64void
65lib3ds_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 */
79void
80lib3ds_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 */
97void
98lib3ds_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 */
116void
117lib3ds_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 */
131void
132lib3ds_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 */
154void
155lib3ds_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
166static float
167det2x2(
168    float a, float b,
169    float c, float d) {
170    return((a)*(d) - (b)*(c));
171}
172
173
174static float
175det3x3(
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 */
190float
191lib3ds_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 */
228int
229lib3ds_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 */
327void
328lib3ds_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 */
340void
341lib3ds_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 */
355void
356lib3ds_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 */
399void
400lib3ds_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 */
422void
423lib3ds_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
Note: See TracBrowser for help on using the browser.