rs_quaternion.rsh revision 044e2ee36ffe6520570a7f0207d75a8fce8b8e91 
1/*
2 * Copyright (C) 2011 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 *      http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17/** @file rs_matrix.rsh
18 *  \brief Quaternion routines
19 *
20 *
21 */
22
23#ifndef __RS_QUATERNION_RSH__
24#define __RS_QUATERNION_RSH__
25
26
27/**
28 * Set the quaternion components
29 * @param w component
30 * @param x component
31 * @param y component
32 * @param z component
33 */
34static void __attribute__((overloadable))
35rsQuaternionSet(rs_quaternion *q, float w, float x, float y, float z) {
36    q->w = w;
37    q->x = x;
38    q->y = y;
39    q->z = z;
40}
41
42/**
43 * Set the quaternion from another quaternion
44 * @param q destination quaternion
45 * @param rhs source quaternion
46 */
47static void __attribute__((overloadable))
48rsQuaternionSet(rs_quaternion *q, const rs_quaternion *rhs) {
49    q->w = rhs->w;
50    q->x = rhs->x;
51    q->y = rhs->y;
52    q->z = rhs->z;
53}
54
55/**
56 * Multiply quaternion by a scalar
57 * @param q quaternion to multiply
58 * @param s scalar
59 */
60static void __attribute__((overloadable))
61rsQuaternionMultiply(rs_quaternion *q, float s) {
62    q->w *= s;
63    q->x *= s;
64    q->y *= s;
65    q->z *= s;
66}
67
68/**
69 * Multiply quaternion by another quaternion
70 * @param q destination quaternion
71 * @param rhs right hand side quaternion to multiply by
72 */
73static void __attribute__((overloadable))
74rsQuaternionMultiply(rs_quaternion *q, const rs_quaternion *rhs) {
75    q->w = -q->x*rhs->x - q->y*rhs->y - q->z*rhs->z + q->w*rhs->w;
76    q->x =  q->x*rhs->w + q->y*rhs->z - q->z*rhs->y + q->w*rhs->x;
77    q->y = -q->x*rhs->z + q->y*rhs->w + q->z*rhs->x + q->w*rhs->y;
78    q->z =  q->x*rhs->y - q->y*rhs->x + q->z*rhs->w + q->w*rhs->z;
79}
80
81/**
82 * Add two quaternions
83 * @param q destination quaternion to add to
84 * @param rsh right hand side quaternion to add
85 */
86static void
87rsQuaternionAdd(rs_quaternion *q, const rs_quaternion *rhs) {
88    q->w *= rhs->w;
89    q->x *= rhs->x;
90    q->y *= rhs->y;
91    q->z *= rhs->z;
92}
93
94/**
95 * Loads a quaternion that represents a rotation about an arbitrary unit vector
96 * @param q quaternion to set
97 * @param rot angle to rotate by
98 * @param x component of a vector
99 * @param y component of a vector
100 * @param x component of a vector
101 */
102static void
103rsQuaternionLoadRotateUnit(rs_quaternion *q, float rot, float x, float y, float z) {
104    rot *= (float)(M_PI / 180.0f) * 0.5f;
105    float c = cos(rot);
106    float s = sin(rot);
107
108    q->w = c;
109    q->x = x * s;
110    q->y = y * s;
111    q->z = z * s;
112}
113
114/**
115 * Loads a quaternion that represents a rotation about an arbitrary vector
116 * (doesn't have to be unit)
117 * @param q quaternion to set
118 * @param rot angle to rotate by
119 * @param x component of a vector
120 * @param y component of a vector
121 * @param x component of a vector
122 */
123static void
124rsQuaternionLoadRotate(rs_quaternion *q, float rot, float x, float y, float z) {
125    const float len = x*x + y*y + z*z;
126    if (len != 1) {
127        const float recipLen = 1.f / sqrt(len);
128        x *= recipLen;
129        y *= recipLen;
130        z *= recipLen;
131    }
132    rsQuaternionLoadRotateUnit(q, rot, x, y, z);
133}
134
135/**
136 * Conjugates the quaternion
137 * @param q quaternion to conjugate
138 */
139static void
140rsQuaternionConjugate(rs_quaternion *q) {
141    q->x = -q->x;
142    q->y = -q->y;
143    q->z = -q->z;
144}
145
146/**
147 * Dot product of two quaternions
148 * @param q0 first quaternion
149 * @param q1 second quaternion
150 * @return dot product between q0 and q1
151 */
152static float
153rsQuaternionDot(const rs_quaternion *q0, const rs_quaternion *q1) {
154    return q0->w*q1->w + q0->x*q1->x + q0->y*q1->y + q0->z*q1->z;
155}
156
157/**
158 * Normalizes the quaternion
159 * @param q quaternion to normalize
160 */
161static void
162rsQuaternionNormalize(rs_quaternion *q) {
163    const float len = rsQuaternionDot(q, q);
164    if (len != 1) {
165        const float recipLen = 1.f / sqrt(len);
166        rsQuaternionMultiply(q, recipLen);
167    }
168}
169
170/**
171 * Performs spherical linear interpolation between two quaternions
172 * @param q result quaternion from interpolation
173 * @param q0 first param
174 * @param q1 second param
175 * @param t how much to interpolate by
176 */
177static void
178rsQuaternionSlerp(rs_quaternion *q, const rs_quaternion *q0, const rs_quaternion *q1, float t) {
179    if (t <= 0.0f) {
180        rsQuaternionSet(q, q0);
181        return;
182    }
183    if (t >= 1.0f) {
184        rsQuaternionSet(q, q1);
185        return;
186    }
187
188    rs_quaternion tempq0, tempq1;
189    rsQuaternionSet(&tempq0, q0);
190    rsQuaternionSet(&tempq1, q1);
191
192    float angle = rsQuaternionDot(q0, q1);
193    if (angle < 0) {
194        rsQuaternionMultiply(&tempq0, -1.0f);
195        angle *= -1.0f;
196    }
197
198    float scale, invScale;
199    if (angle + 1.0f > 0.05f) {
200        if (1.0f - angle >= 0.05f) {
201            float theta = acos(angle);
202            float invSinTheta = 1.0f / sin(theta);
203            scale = sin(theta * (1.0f - t)) * invSinTheta;
204            invScale = sin(theta * t) * invSinTheta;
205        } else {
206            scale = 1.0f - t;
207            invScale = t;
208        }
209    } else {
210        rsQuaternionSet(&tempq1, tempq0.z, -tempq0.y, tempq0.x, -tempq0.w);
211        scale = sin(M_PI * (0.5f - t));
212        invScale = sin(M_PI * t);
213    }
214
215    rsQuaternionSet(q, tempq0.w*scale + tempq1.w*invScale, tempq0.x*scale + tempq1.x*invScale,
216                        tempq0.y*scale + tempq1.y*invScale, tempq0.z*scale + tempq1.z*invScale);
217}
218
219/**
220 * Computes rotation matrix from the normalized quaternion
221 * @param m resulting matrix
222 * @param p normalized quaternion
223 */
224static void rsQuaternionGetMatrixUnit(rs_matrix4x4 *m, const rs_quaternion *q) {
225    float x2 = 2.0f * q->x * q->x;
226    float y2 = 2.0f * q->y * q->y;
227    float z2 = 2.0f * q->z * q->z;
228    float xy = 2.0f * q->x * q->y;
229    float wz = 2.0f * q->w * q->z;
230    float xz = 2.0f * q->x * q->z;
231    float wy = 2.0f * q->w * q->y;
232    float wx = 2.0f * q->w * q->x;
233    float yz = 2.0f * q->y * q->z;
234
235    m->m[0] = 1.0f - y2 - z2;
236    m->m[1] = xy - wz;
237    m->m[2] = xz + wy;
238    m->m[3] = 0.0f;
239
240    m->m[4] = xy + wz;
241    m->m[5] = 1.0f - x2 - z2;
242    m->m[6] = yz - wx;
243    m->m[7] = 0.0f;
244
245    m->m[8] = xz - wy;
246    m->m[9] = yz - wx;
247    m->m[10] = 1.0f - x2 - y2;
248    m->m[11] = 0.0f;
249
250    m->m[12] = 0.0f;
251    m->m[13] = 0.0f;
252    m->m[14] = 0.0f;
253    m->m[15] = 1.0f;
254}
255
256#endif
257
258