1 /*
2 Copyright (C) 1997-2001 Id Software, Inc.
3
4 This program is free software; you can redistribute it and/or
5 modify it under the terms of the GNU General Public License
6 as published by the Free Software Foundation; either version 2
7 of the License, or (at your option) any later version.
8
9 This program is distributed in the hope that it will be useful,
10 but WITHOUT ANY WARRANTY; without even the implied warranty of
11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
12
13 See the GNU General Public License for more details.
14
15 You should have received a copy of the GNU General Public License
16 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 // m_quad.c
22 //
23
24 #include "shared.h"
25
26 quat_t quatIdentity = {
27 0, 0, 0, 1
28 };
29
30 /*
31 =============================================================================
32
33 QUATERNIONS
34
35 =============================================================================
36 */
37
38 /*
39 ===============
40 Quat_ConcatTransforms
41 ===============
42 */
Quat_ConcatTransforms(quat_t q1,vec3_t v1,quat_t q2,vec3_t v2,quat_t q,vec3_t v)43 void Quat_ConcatTransforms (quat_t q1, vec3_t v1, quat_t q2, vec3_t v2, quat_t q, vec3_t v)
44 {
45 Quat_Multiply (q1, q2, q);
46 Quat_TransformVector (q1, v2, v);
47 v[0] += v1[0]; v[1] += v1[1]; v[2] += v1[2];
48 }
49
50
51 /*
52 ===============
53 Quat_Copy
54 ===============
55 */
Quat_Copy(quat_t q1,quat_t q2)56 void Quat_Copy (quat_t q1, quat_t q2)
57 {
58 q2[0] = q1[0];
59 q2[1] = q1[1];
60 q2[2] = q1[2];
61 q2[3] = q1[3];
62 }
63
64
65 /*
66 ===============
67 Quat_Conjugate
68 ===============
69 */
Quat_Conjugate(quat_t q1,quat_t q2)70 void Quat_Conjugate (quat_t q1, quat_t q2)
71 {
72 q2[0] = -q1[0];
73 q2[1] = -q1[1];
74 q2[2] = -q1[2];
75 q2[3] = q1[3];
76 }
77
78
79 /*
80 ===============
81 Quat_Identity
82 ===============
83 */
Quat_Identity(quat_t q)84 void Quat_Identity (quat_t q)
85 {
86 Quat_Copy (quatIdentity, q);
87 }
88
89
90 /*
91 ===============
92 Quat_Inverse
93 ===============
94 */
Quat_Inverse(quat_t q1,quat_t q2)95 mat_t Quat_Inverse (quat_t q1, quat_t q2)
96 {
97 Quat_Conjugate (q1, q2);
98
99 return Quat_Normalize (q2);
100 }
101
102
103 /*
104 ===============
105 Quat_Normalize
106 ===============
107 */
Quat_Normalize(quat_t q)108 mat_t Quat_Normalize (quat_t q)
109 {
110 mat_t length;
111
112 length = q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3];
113 if (length != 0) {
114 mat_t ilength = 1.0 / sqrt (length);
115 q[0] *= ilength;
116 q[1] *= ilength;
117 q[2] *= ilength;
118 q[3] *= ilength;
119 }
120
121 return length;
122 }
123
124
125 /*
126 ===============
127 Quat_Lerp
128 ===============
129 */
Quat_Lerp(quat_t q1,quat_t q2,mat_t t,quat_t out)130 void Quat_Lerp (quat_t q1, quat_t q2, mat_t t, quat_t out)
131 {
132 quat_t p1;
133 mat_t omega, cosom, sinom, scale0, scale1;
134
135 cosom = q1[0] * q2[0] + q1[1] * q2[1] + q1[2] * q2[2] + q1[3] * q2[3];
136 if (cosom < 0.0) {
137 cosom = -cosom;
138 p1[0] = -q1[0]; p1[1] = -q1[1];
139 p1[2] = -q1[2]; p1[3] = -q1[3];
140 }
141 else {
142 p1[0] = q1[0]; p1[1] = q1[1];
143 p1[2] = q1[2]; p1[3] = q1[3];
144 }
145
146 if (cosom < 1.0 - 0.001) {
147 omega = acos (cosom);
148 sinom = 1.0f / sin (omega);
149 scale0 = sin ((1.0f - t) * omega) * sinom;
150 scale1 = sin (t * omega) * sinom;
151 }
152 else {
153 scale0 = 1.0f - t;
154 scale1 = t;
155 }
156
157 out[0] = scale0 * p1[0] + scale1 * q2[0];
158 out[1] = scale0 * p1[1] + scale1 * q2[1];
159 out[2] = scale0 * p1[2] + scale1 * q2[2];
160 out[3] = scale0 * p1[3] + scale1 * q2[3];
161 }
162
163
164 /*
165 ===============
166 Quat_Matrix3
167 ===============
168 */
Quat_Matrix3(quat_t q,mat3x3_t m)169 void Quat_Matrix3 (quat_t q, mat3x3_t m)
170 {
171 mat_t wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
172
173 x2 = q[0] + q[0]; y2 = q[1] + q[1]; z2 = q[2] + q[2];
174 xx = q[0] * x2; xy = q[0] * y2; xz = q[0] * z2;
175 yy = q[1] * y2; yz = q[1] * z2; zz = q[2] * z2;
176 wx = q[3] * x2; wy = q[3] * y2; wz = q[3] * z2;
177
178 m[0][0] = 1.0f - yy - zz; m[0][1] = xy - wz; m[0][2] = xz + wy;
179 m[1][0] = xy + wz; m[1][1] = 1.0f - xx - zz; m[1][2] = yz - wx;
180 m[2][0] = xz - wy; m[2][1] = yz + wx; m[2][2] = 1.0f - xx - yy;
181 }
182
183
184 /*
185 ===============
186 Quat_Multiply
187 ===============
188 */
Quat_Multiply(quat_t q1,quat_t q2,quat_t out)189 void Quat_Multiply (quat_t q1, quat_t q2, quat_t out)
190 {
191 out[0] = q1[3] * q2[0] + q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1];
192 out[1] = q1[3] * q2[1] + q1[1] * q2[3] + q1[2] * q2[0] - q1[0] * q2[2];
193 out[2] = q1[3] * q2[2] + q1[2] * q2[3] + q1[0] * q2[1] - q1[1] * q2[0];
194 out[3] = q1[3] * q2[3] - q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2];
195 }
196
197
198 /*
199 ===============
200 Quat_TransformVector
201 ===============
202 */
Quat_TransformVector(quat_t q,vec3_t v,vec3_t out)203 void Quat_TransformVector (quat_t q, vec3_t v, vec3_t out)
204 {
205 mat_t wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
206
207 x2 = q[0] + q[0]; y2 = q[1] + q[1]; z2 = q[2] + q[2];
208 xx = q[0] * x2; xy = q[0] * y2; xz = q[0] * z2;
209 yy = q[1] * y2; yz = q[1] * z2; zz = q[2] * z2;
210 wx = q[3] * x2; wy = q[3] * y2; wz = q[3] * z2;
211
212 out[0] = (float)((1.0f - yy - zz) * v[0] + (xy - wz) * v[1] + (xz + wy) * v[2]);
213 out[1] = (float)((xy + wz) * v[0] + (1.0f - xx - zz) * v[1] + (yz - wx) * v[2]);
214 out[2] = (float)((xz - wy) * v[0] + (yz + wx) * v[1] + (1.0f - xx - yy) * v[2]);
215 }
216