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 */ 34 static void __attribute__((overloadable)) 35 rsQuaternionSet(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 */ 47 static void __attribute__((overloadable)) 48 rsQuaternionSet(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 */ 60 static void __attribute__((overloadable)) 61 rsQuaternionMultiply(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 */ 73 static void 74 rsQuaternionAdd(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 */ 89 static void 90 rsQuaternionLoadRotateUnit(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 */ 110 static void 111 rsQuaternionLoadRotate(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 */ 126 static void 127 rsQuaternionConjugate(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 */ 139 static float 140 rsQuaternionDot(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 */ 148 static void 149 rsQuaternionNormalize(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 */ 162 static void __attribute__((overloadable)) 163 rsQuaternionMultiply(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 */ 181 static void 182 rsQuaternionSlerp(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 */ 228 static 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
