← Back to branch summary

~ubuntu-branches/ubuntu/gutsy/rss-glx/gutsy

« back to all changes in this revision

Viewing changes to reallyslick/rsMath/rsQuat.cpp

  • Committer: Bazaar Package Importer
  • Author(s): LaMont Jones
  • Date: 2005-11-30 18:21:27 UTC
  • mto: This revision was merged to the branch mainline in revision 4.
  • Revision ID: james.westby@ubuntu.com-20051130182127-5iww7elbiyzej1lk
Tags: upstream-0.8.0
Import upstream version 0.8.0

Show diffs side-by-side

added added

removed removed

Lines of Context:
reallyslick/rsMath/rsQuat.cpp
 
1
/*
 
2
 * Copyright (C) 1999-2005  Terence M. Welsh
 
3
 *
 
4
 * This file is part of rsMath.
 
5
 *
 
6
 * rsMath is free software; you can redistribute it and/or
 
7
 * modify it under the terms of the GNU Lesser General Public
 
8
 * License version 2.1 as published by the Free Software Foundation.
 
9
 *
 
10
 * rsMath 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 General Public License for more details.
 
14
 *
 
15
 * You should have received a copy of the GNU Lesser General Public
 
16
 * License along with this program; if not, write to the Free Software
 
17
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 
18
 */
 
19
 
 
20
 
 
21
 
 
22
#include <rsMath/rsMath.h>
 
23
#include <math.h>
 
24
 
 
25
 
 
26
 
 
27
rsQuat::rsQuat(){
 
28
        q[0] = 0.0f;
 
29
        q[1] = 0.0f;
 
30
        q[2] = 0.0f;
 
31
        q[3] = 1.0f;
 
32
}
 
33
 
 
34
 
 
35
rsQuat::rsQuat(float x, float y, float z, float w){
 
36
        q[0] = x;
 
37
        q[1] = y;
 
38
        q[2] = z;
 
39
        q[3] = w;
 
40
}
 
41
 
 
42
 
 
43
rsQuat::~rsQuat(){
 
44
        return;
 
45
}
 
46
 
 
47
 
 
48
void rsQuat::set(float x, float y, float z, float w){
 
49
        q[0] = x;
 
50
        q[1] = y;
 
51
        q[2] = z;
 
52
        q[3] = w;
 
53
}
 
54
 
 
55
 
 
56
void rsQuat::copy(rsQuat copyquat){
 
57
        q[0] = copyquat[0];
 
58
        q[1] = copyquat[1];
 
59
        q[2] = copyquat[2];
 
60
        q[3] = copyquat[3];
 
61
}
 
62
 
 
63
 
 
64
void rsQuat::make(float a, float x, float y, float z){
 
65
        if(a < RSEPSILON && a > -RSEPSILON){
 
66
                q[0] = 0.0f;
 
67
                q[1] = 0.0f;
 
68
                q[2] = 0.0f;
 
69
                q[3] = 1.0f;
 
70
        }
 
71
        else{
 
72
                a *= 0.5f;
 
73
                float sintheta = sinf(a);
 
74
                q[0] = sintheta * x;
 
75
                q[1] = sintheta * y;
 
76
                q[2] = sintheta * z;
 
77
                q[3] = cosf(a);
 
78
        }
 
79
}
 
80
 
 
81
 
 
82
void rsQuat::make(float a, const rsVec &v){
 
83
        if(a < RSEPSILON && a > -RSEPSILON){
 
84
                q[0] = 0.0f;
 
85
                q[1] = 0.0f;
 
86
                q[2] = 0.0f;
 
87
                q[3] = 1.0f;
 
88
        }
 
89
        else{
 
90
                a *= 0.5f;
 
91
                float sintheta = sinf(a);
 
92
                q[0] = sintheta * v[0];
 
93
                q[1] = sintheta * v[1];
 
94
                q[2] = sintheta * v[2];
 
95
                q[3] = cosf(a);
 
96
        }
 
97
}
 
98
 
 
99
 
 
100
void rsQuat::normalize(){
 
101
        float length = float(sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]));
 
102
 
 
103
        q[0] /= length;
 
104
        q[1] /= length;
 
105
        q[2] /= length;
 
106
        q[3] /= length;
 
107
}
 
108
 
 
109
 
 
110
void rsQuat::preMult(rsQuat &postQuat){
 
111
        // q1q2 = s1v2 + s2v1 + v1xv2, s1s2 - v1.v2
 
112
        float tempx = q[0];
 
113
        float tempy = q[1];
 
114
        float tempz = q[2];
 
115
        float tempw = q[3];
 
116
 
 
117
        q[0] = tempw * postQuat[0] + postQuat[3] * tempx
 
118
                + tempy * postQuat[2] - postQuat[1] * tempz;
 
119
        q[1] = tempw * postQuat[1] + postQuat[3] * tempy
 
120
                + tempz * postQuat[0] - postQuat[2] * tempx;
 
121
        q[2] = tempw * postQuat[2] + postQuat[3] * tempz
 
122
                + tempx * postQuat[1] - postQuat[0] * tempy;
 
123
        q[3] = tempw * postQuat[3]
 
124
                - tempx * postQuat[0]
 
125
                - tempy * postQuat[1]
 
126
                - tempz * postQuat[2];
 
127
}
 
128
 
 
129
 
 
130
void rsQuat::postMult(rsQuat &preQuat){
 
131
        float tempx = q[0];
 
132
        float tempy = q[1];
 
133
        float tempz = q[2];
 
134
        float tempw = q[3];
 
135
 
 
136
        q[0] = preQuat[3] * tempx + tempw * preQuat[0]
 
137
                + preQuat[1] * tempz - tempy * preQuat[2];
 
138
        q[1] = preQuat[3] * tempy + tempw * preQuat[1]
 
139
                + preQuat[2] * tempx - tempz * preQuat[0];
 
140
        q[2] = preQuat[3] * tempz + tempw * preQuat[2]
 
141
                + preQuat[0] * tempy - tempx * preQuat[1];
 
142
        q[3] = preQuat[3] * tempw
 
143
                - preQuat[0] * tempx
 
144
                - preQuat[1] * tempy
 
145
                - preQuat[2] * tempz;
 
146
}
 
147
 
 
148
 
 
149
void rsQuat::toMat(float *mat){
 
150
        float s, xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;
 
151
 
 
152
        // must have an axis
 
153
        if(q[0] == 0.0f && q[1] == 0.0f && q[2] == 0.0f){
 
154
                mat[0] = 1.0f;
 
155
                mat[1] = 0.0f;
 
156
                mat[2] = 0.0f;
 
157
                mat[3] = 0.0f;
 
158
                mat[4] = 0.0f;
 
159
                mat[5] = 1.0f;
 
160
                mat[6] = 0.0f;
 
161
                mat[7] = 0.0f;
 
162
                mat[8] = 0.0f;
 
163
                mat[9] = 0.0f;
 
164
                mat[10] = 1.0f;
 
165
                mat[11] = 0.0f;
 
166
                mat[12] = 0.0f;
 
167
                mat[13] = 0.0f;
 
168
                mat[14] = 0.0f;
 
169
                mat[15] = 1.0f;
 
170
                return;
 
171
        }
 
172
 
 
173
        s = 2.0f / (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
 
174
        xs = q[0] * s;
 
175
        ys = q[1] * s;
 
176
        zs = q[2] * s;
 
177
        wx = q[3] * xs;
 
178
        wy = q[3] * ys;
 
179
        wz = q[3] * zs;
 
180
        xx = q[0] * xs;
 
181
        xy = q[0] * ys;
 
182
        xz = q[0] * zs;
 
183
        yy = q[1] * ys;
 
184
        yz = q[1] * zs;
 
185
        zz = q[2] * zs;
 
186
 
 
187
        mat[0] = 1.0f - yy - zz;
 
188
        mat[1] = xy + wz;
 
189
        mat[2] = xz - wy;
 
190
        mat[3] = 0.0f;
 
191
        mat[4] = xy - wz;
 
192
        mat[5] = 1.0f - xx - zz;
 
193
        mat[6] = yz + wx;
 
194
        mat[7] = 0.0f;
 
195
        mat[8] = xz + wy;
 
196
        mat[9] = yz - wx;
 
197
        mat[10] = 1.0f - xx - yy;
 
198
        mat[11] = 0.0f;
 
199
        mat[12] = 0.0f;
 
200
        mat[13] = 0.0f;
 
201
        mat[14] = 0.0f;
 
202
        mat[15] = 1.0f;
 
203
}
 
204
 
 
205
 
 
206
void rsQuat::fromMat(float* mat){
 
207
        float a, b;
 
208
        int i;
 
209
 
 
210
        a = mat[0] + mat[5] + mat[10];
 
211
        if(a > 0.0){
 
212
                b = float(sqrt(a + 1.0f));
 
213
                q[3] = b * 0.5f;
 
214
                b = 0.5f / b;
 
215
 
 
216
                q[0] = (mat[6] - mat[9]) * b;
 
217
                q[1] = (mat[8] - mat[2]) * b;
 
218
                q[2] = (mat[1] - mat[4]) * b;
 
219
        }
 
220
        else{
 
221
                i = 0;
 
222
                if(mat[5] > mat[0])
 
223
                        i = 1;
 
224
                if(mat[10] > mat[5])
 
225
                        i = 2;
 
226
                
 
227
                if(i==0){
 
228
                        b = float(sqrt(mat[0] - mat[5] - mat[10] + 1.0f));
 
229
                        q[0] *= 0.5f;
 
230
                        b = 0.5f / b;
 
231
                        q[3] = (mat[6] - mat[9]) * b;
 
232
                        q[1] = (mat[1] - mat[4]) * b;
 
233
                        q[2] = (mat[2] - mat[8]) * b;
 
234
                }
 
235
                if(i==1){
 
236
                        b = float(sqrt(mat[5] - mat[10] - mat[0] + 1.0f));
 
237
                        q[1] *= 0.5f;
 
238
                        b = 0.5f / b;
 
239
                        q[3] = (mat[8] - mat[2]) * b;
 
240
                        q[2] = (mat[6] - mat[9]) * b;
 
241
                        q[0] = (mat[4] - mat[1]) * b;
 
242
                }
 
243
                if(i==2){
 
244
                        b = float(sqrt(mat[10] - mat[0] - mat[5] + 1.0f));
 
245
                        q[2] *= 0.5f;
 
246
                        b = 0.5f / b;
 
247
                        q[3] = (mat[1] - mat[4]) * b;
 
248
                        q[0] = (mat[8] - mat[2]) * b;
 
249
                        q[1] = (mat[9] - mat[6]) * b;
 
250
                }
 
251
        }
 
252
}
 
253
 
 
254
 
 
255
void rsQuat::fromEuler(float yaw, float pitch, float roll){
 
256
        float cy, cp, cr, sy, sp, sr, cpcy, spsy;
 
257
 
 
258
        cy = cosf(yaw * 0.5f);
 
259
        cp = cosf(pitch * 0.5f);
 
260
        cr = cosf(roll * 0.5f);
 
261
 
 
262
        sy = sinf(yaw * 0.5f);
 
263
        sp = sinf(pitch * 0.5f);
 
264
        sr = sinf(roll * 0.5f);
 
265
        
 
266
        cpcy = cp * cy;
 
267
        spsy = sp * sy;
 
268
 
 
269
        q[3] = cr * cpcy + sr * spsy;
 
270
        q[0] = sr * cpcy - cr * spsy;
 
271
        q[1] = cr * sp * cy + sr * cp * sy;
 
272
        q[2] = cr * cp * sy - sr * sp * cy;
 
273
}
 
274
 
 
275
 
 
276
void rsQuat::slerp(rsQuat a, rsQuat b, float t){
 
277
        float n, cn, sn, scalea, scaleb;
 
278
 
 
279
        cn = a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3];
 
280
        if((1.0f + cn) > RSEPSILON){
 
281
                if((1.0f - cn) > RSEPSILON){
 
282
                        n = acosf(cn);
 
283
                        sn = sinf(n);
 
284
                        scalea = sinf((1.0f - t) * n) / sn;
 
285
                        scaleb = sinf(t * n) / sn;
 
286
                }
 
287
                else{
 
288
                        scalea = 1.0f - t;
 
289
                        scaleb = t;
 
290
                }
 
291
                q[0] = scalea * a[0] + scaleb * b[0];
 
292
                q[1] = scalea * a[1] + scaleb * b[1];
 
293
                q[2] = scalea * a[2] + scaleb * b[2];
 
294
                q[3] = scalea * a[3] + scaleb * b[3];
 
295
        }
 
296
        else{
 
297
                scalea = sinf((1.0f - t) * RS_PIo2);
 
298
                scaleb = sinf(t * RS_PIo2);
 
299
                q[0] = scalea * a[0] - scaleb * q[1];
 
300
                q[1] = scalea * a[1] + scaleb * q[0];
 
301
                q[2] = scalea * a[2] - scaleb * q[3];
 
302
                q[3] = scalea * a[3] + scaleb * q[2];
 
303
        }
 
304
}