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

Revision 10853, 6.2 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 * Set a quaternion to Identity
23 */
24void
25lib3ds_quat_identity(float c[4]) {
26    c[0] = c[1] = c[2] = 0.0f;
27    c[3] = 1.0f;
28}
29
30
31/*!
32 * Copy a quaternion.
33 */
34void
35lib3ds_quat_copy(float dest[4], float src[4]) {
36    int i;
37    for (i = 0; i < 4; ++i) {
38        dest[i] = src[i];
39    }
40}
41
42
43/*!
44 * Compute a quaternion from axis and angle.
45 *
46 * \param c Computed quaternion
47 * \param axis Rotation axis
48 * \param angle Angle of rotation, radians.
49 */
50void
51lib3ds_quat_axis_angle(float c[4], float axis[3], float angle) {
52    double omega, s;
53    double l;
54
55    l = sqrt(axis[0] * axis[0] + axis[1] * axis[1] + axis[2] * axis[2]);
56    if (l < LIB3DS_EPSILON) {
57        c[0] = c[1] = c[2] = 0.0f;
58        c[3] = 1.0f;
59    } else {
60        omega = -0.5 * angle;
61        s = sin(omega) / l;
62        c[0] = (float)s * axis[0];
63        c[1] = (float)s * axis[1];
64        c[2] = (float)s * axis[2];
65        c[3] = (float)cos(omega);
66    }
67}
68
69
70/*!
71 * Negate a quaternion
72 */
73void
74lib3ds_quat_neg(float c[4]) {
75    int i;
76    for (i = 0; i < 4; ++i) {
77        c[i] = -c[i];
78    }
79}
80
81
82/*!
83 * Compute the conjugate of a quaternion
84 */
85void
86lib3ds_quat_cnj(float c[4]) {
87    int i;
88    for (i = 0; i < 3; ++i) {
89        c[i] = -c[i];
90    }
91}
92
93
94/*!
95 * Multiply two quaternions.
96 *
97 * \param c Result
98 * \param a,b Inputs
99 */
100void
101lib3ds_quat_mul(float c[4], float a[4], float b[4]) {
102    float qa[4], qb[4];
103    lib3ds_quat_copy(qa, a);
104    lib3ds_quat_copy(qb, b);
105    c[0] = qa[3] * qb[0] + qa[0] * qb[3] + qa[1] * qb[2] - qa[2] * qb[1];
106    c[1] = qa[3] * qb[1] + qa[1] * qb[3] + qa[2] * qb[0] - qa[0] * qb[2];
107    c[2] = qa[3] * qb[2] + qa[2] * qb[3] + qa[0] * qb[1] - qa[1] * qb[0];
108    c[3] = qa[3] * qb[3] - qa[0] * qb[0] - qa[1] * qb[1] - qa[2] * qb[2];
109}
110
111
112/*!
113 * Multiply a quaternion by a scalar.
114 */
115void
116lib3ds_quat_scalar(float c[4], float k) {
117    int i;
118    for (i = 0; i < 4; ++i) {
119        c[i] *= k;
120    }
121}
122
123
124/*!
125 * Normalize a quaternion.
126 */
127void
128lib3ds_quat_normalize(float c[4]) {
129    double l, m;
130
131    l = sqrt(c[0] * c[0] + c[1] * c[1] + c[2] * c[2] + c[3] * c[3]);
132    if (fabs(l) < LIB3DS_EPSILON) {
133        c[0] = c[1] = c[2] = 0.0f;
134        c[3] = 1.0f;
135    } else {
136        int i;
137        m = 1.0f / l;
138        for (i = 0; i < 4; ++i) {
139            c[i] = (float)(c[i] * m);
140        }
141    }
142}
143
144
145/*!
146 * Compute the inverse of a quaternion.
147 */
148void
149lib3ds_quat_inv(float c[4]) {
150    double l, m;
151
152    l = sqrt(c[0] * c[0] + c[1] * c[1] + c[2] * c[2] + c[3] * c[3]);
153    if (fabs(l) < LIB3DS_EPSILON) {
154        c[0] = c[1] = c[2] = 0.0f;
155        c[3] = 1.0f;
156    } else {
157        m = 1.0f / l;
158        c[0] = (float)(-c[0] * m);
159        c[1] = (float)(-c[1] * m);
160        c[2] = (float)(-c[2] * m);
161        c[3] = (float)(c[3] * m);
162    }
163}
164
165
166/*!
167 * Compute the dot-product of a quaternion.
168 */
169float
170lib3ds_quat_dot(float a[4], float b[4]) {
171    return(a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + a[3]*b[3]);
172}
173
174
175float
176lib3ds_quat_norm(float c[4]) {
177    return(c[0]*c[0] + c[1]*c[1] + c[2]*c[2] + c[3]*c[3]);
178}
179
180
181void
182lib3ds_quat_ln(float c[4]) {
183    double om, s, t;
184
185    s = sqrt(c[0] * c[0] + c[1] * c[1] + c[2] * c[2]);
186    om = atan2(s, (double)c[3]);
187    if (fabs(s) < LIB3DS_EPSILON) {
188        t = 0.0f;
189    } else {
190        t = om / s;
191    }
192    {
193        int i;
194        for (i = 0; i < 3; ++i) {
195            c[i] = (float)(c[i] * t);
196        }
197        c[3] = 0.0f;
198    }
199}
200
201
202void
203lib3ds_quat_ln_dif(float c[4], float a[4], float b[4]) {
204    float invp[4];
205
206    lib3ds_quat_copy(invp, a);
207    lib3ds_quat_inv(invp);
208    lib3ds_quat_mul(c, invp, b);
209    lib3ds_quat_ln(c);
210}
211
212
213void
214lib3ds_quat_exp(float c[4]) {
215    double om, sinom;
216
217    om = sqrt(c[0] * c[0] + c[1] * c[1] + c[2] * c[2]);
218    if (fabs(om) < LIB3DS_EPSILON) {
219        sinom = 1.0f;
220    } else {
221        sinom = sin(om) / om;
222    }
223    {
224        int i;
225        for (i = 0; i < 3; ++i) {
226            c[i] = (float)(c[i] * sinom);
227        }
228        c[3] = (float)cos(om);
229    }
230}
231
232
233void
234lib3ds_quat_slerp(float c[4], float a[4], float b[4], float t) {
235    double l;
236    double om, sinom;
237    double sp, sq;
238    float flip = 1.0f;
239    int i;
240
241    l = a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3];
242    if (l < 0) {
243        flip = -1.0f;
244        l = -l;
245    }   
246   
247    om = acos(l);
248    sinom = sin(om);
249    if (fabs(sinom) > LIB3DS_EPSILON) {
250        sp = sin((1.0f - t) * om) / sinom;
251        sq = sin(t * om) / sinom;
252    } else {
253        sp = 1.0f - t;
254        sq = t;
255    }
256    sq *= flip;
257    for (i = 0; i < 4; ++i) {
258        c[i] = (float)(sp * a[i] + sq * b[i]);
259    }
260}
261
262
263void
264lib3ds_quat_squad(float c[4], float a[4], float p[4], float q[4], float b[4], float t) {
265    float ab[4];
266    float pq[4];
267
268    lib3ds_quat_slerp(ab, a, b, t);
269    lib3ds_quat_slerp(pq, p, q, t);
270    lib3ds_quat_slerp(c, ab, pq, 2*t*(1 - t));
271}
272
273
274void
275lib3ds_quat_tangent(float c[4], float p[4], float q[4], float n[4]) {
276    float dn[4], dp[4], x[4];
277    int i;
278
279    lib3ds_quat_ln_dif(dn, q, n);
280    lib3ds_quat_ln_dif(dp, q, p);
281
282    for (i = 0; i < 4; i++) {
283        x[i] = -1.0f / 4.0f * (dn[i] + dp[i]);
284    }
285    lib3ds_quat_exp(x);
286    lib3ds_quat_mul(c, q, x);
287}
288
289
290void
291lib3ds_quat_dump(float q[4]) {
292    printf("%f %f %f %f\n", q[0], q[1], q[2], q[3]);
293}
Note: See TracBrowser for help on using the browser.