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_quaternion.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 * Add two quaternions
70 * @param q destination quaternion to add to
71 * @param rsh right hand side quaternion to add
72 */
73static void
74rsQuaternionAdd(rs_quaternion *q, const rs_quaternion *rhs) {
75    q->w *= rhs->w;
76    q->x *= rhs->x;
77    q->y *= rhs->y;
78    q->z *= rhs->z;
79}
80
81/**
82 * Loads a quaternion that represents a rotation about an arbitrary unit vector
83 * @param q quaternion to set
84 * @param rot angle to rotate by
85 * @param x component of a vector
86 * @param y component of a vector
87 * @param x component of a vector
88 */
89static void
90rsQuaternionLoadRotateUnit(rs_quaternion *q, float rot, float x, float y, float z) {
91    rot *= (float)(M_PI / 180.0f) * 0.5f;
92    float c = cos(rot);
93    float s = sin(rot);
94
95    q->w = c;
96    q->x = x * s;
97    q->y = y * s;
98    q->z = z * s;
99}
100
101/**
102 * Loads a quaternion that represents a rotation about an arbitrary vector
103 * (doesn't have to be unit)
104 * @param q quaternion to set
105 * @param rot angle to rotate by
106 * @param x component of a vector
107 * @param y component of a vector
108 * @param x component of a vector
109 */
110static void
111rsQuaternionLoadRotate(rs_quaternion *q, float rot, float x, float y, float z) {
112    const float len = x*x + y*y + z*z;
113    if (len != 1) {
114        const float recipLen = 1.f / sqrt(len);
115        x *= recipLen;
116        y *= recipLen;
117        z *= recipLen;
118    }
119    rsQuaternionLoadRotateUnit(q, rot, x, y, z);
120}
121
122/**
123 * Conjugates the quaternion
124 * @param q quaternion to conjugate
125 */
126static void
127rsQuaternionConjugate(rs_quaternion *q) {
128    q->x = -q->x;
129    q->y = -q->y;
130    q->z = -q->z;
131}
132
133/**
134 * Dot product of two quaternions
135 * @param q0 first quaternion
136 * @param q1 second quaternion
137 * @return dot product between q0 and q1
138 */
139static float
140rsQuaternionDot(const rs_quaternion *q0, const rs_quaternion *q1) {
141    return q0->w*q1->w + q0->x*q1->x + q0->y*q1->y + q0->z*q1->z;
142}
143
144/**
145 * Normalizes the quaternion
146 * @param q quaternion to normalize
147 */
148static void
149rsQuaternionNormalize(rs_quaternion *q) {
150    const float len = rsQuaternionDot(q, q);
151    if (len != 1) {
152        const float recipLen = 1.f / sqrt(len);
153        rsQuaternionMultiply(q, recipLen);
154    }
155}
156
157/**
158 * Multiply quaternion by another quaternion
159 * @param q destination quaternion
160 * @param rhs right hand side quaternion to multiply by
161 */
162static void __attribute__((overloadable))
163rsQuaternionMultiply(rs_quaternion *q, const rs_quaternion *rhs) {
164    rs_quaternion qtmp;
165    rsQuaternionSet(&qtmp, q);
166
167    q->w = qtmp.w*rhs->w - qtmp.x*rhs->x - qtmp.y*rhs->y - qtmp.z*rhs->z;
168    q->x = qtmp.w*rhs->x + qtmp.x*rhs->w + qtmp.y*rhs->z - qtmp.z*rhs->y;
169    q->y = qtmp.w*rhs->y + qtmp.y*rhs->w + qtmp.z*rhs->x - qtmp.x*rhs->z;
170    q->z = qtmp.w*rhs->z + qtmp.z*rhs->w + qtmp.x*rhs->y - qtmp.y*rhs->x;
171    rsQuaternionNormalize(q);
172}
173
174/**
175 * Performs spherical linear interpolation between two quaternions
176 * @param q result quaternion from interpolation
177 * @param q0 first param
178 * @param q1 second param
179 * @param t how much to interpolate by
180 */
181static void
182rsQuaternionSlerp(rs_quaternion *q, const rs_quaternion *q0, const rs_quaternion *q1, float t) {
183    if (t <= 0.0f) {
184        rsQuaternionSet(q, q0);
185        return;
186    }
187    if (t >= 1.0f) {
188        rsQuaternionSet(q, q1);
189        return;
190    }
191
192    rs_quaternion tempq0, tempq1;
193    rsQuaternionSet(&tempq0, q0);
194    rsQuaternionSet(&tempq1, q1);
195
196    float angle = rsQuaternionDot(q0, q1);
197    if (angle < 0) {
198        rsQuaternionMultiply(&tempq0, -1.0f);
199        angle *= -1.0f;
200    }
201
202    float scale, invScale;
203    if (angle + 1.0f > 0.05f) {
204        if (1.0f - angle >= 0.05f) {
205            float theta = acos(angle);
206            float invSinTheta = 1.0f / sin(theta);
207            scale = sin(theta * (1.0f - t)) * invSinTheta;
208            invScale = sin(theta * t) * invSinTheta;
209        } else {
210            scale = 1.0f - t;
211            invScale = t;
212        }
213    } else {
214        rsQuaternionSet(&tempq1, tempq0.z, -tempq0.y, tempq0.x, -tempq0.w);
215        scale = sin(M_PI * (0.5f - t));
216        invScale = sin(M_PI * t);
217    }
218
219    rsQuaternionSet(q, tempq0.w*scale + tempq1.w*invScale, tempq0.x*scale + tempq1.x*invScale,
220                        tempq0.y*scale + tempq1.y*invScale, tempq0.z*scale + tempq1.z*invScale);
221}
222
223/**
224 * Computes rotation matrix from the normalized quaternion
225 * @param m resulting matrix
226 * @param p normalized quaternion
227 */
228static void rsQuaternionGetMatrixUnit(rs_matrix4x4 *m, const rs_quaternion *q) {
229    float xx = q->x * q->x;
230    float xy = q->x * q->y;
231    float xz = q->x * q->z;
232    float xw = q->x * q->w;
233    float yy = q->y * q->y;
234    float yz = q->y * q->z;
235    float yw = q->y * q->w;
236    float zz = q->z * q->z;
237    float zw = q->z * q->w;
238
239    m->m[0]  = 1.0f - 2.0f * ( yy + zz );
240    m->m[4]  =        2.0f * ( xy - zw );
241    m->m[8]  =        2.0f * ( xz + yw );
242    m->m[1]  =        2.0f * ( xy + zw );
243    m->m[5]  = 1.0f - 2.0f * ( xx + zz );
244    m->m[9]  =        2.0f * ( yz - xw );
245    m->m[2]  =        2.0f * ( xz - yw );
246    m->m[6]  =        2.0f * ( yz + xw );
247    m->m[10] = 1.0f - 2.0f * ( xx + yy );
248    m->m[3]  = m->m[7] = m->m[11] = m->m[12] = m->m[13] = m->m[14] = 0.0f;
249    m->m[15] = 1.0f;
250}
251
252#endif
253
254