1044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams/* 2044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * Copyright (C) 2011 The Android Open Source Project 3044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * 4044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * Licensed under the Apache License, Version 2.0 (the "License"); 5044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * you may not use this file except in compliance with the License. 6044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * You may obtain a copy of the License at 7044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * 8044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * http://www.apache.org/licenses/LICENSE-2.0 9044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * 10044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * Unless required by applicable law or agreed to in writing, software 11044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * distributed under the License is distributed on an "AS IS" BASIS, 12044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * See the License for the specific language governing permissions and 14044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * limitations under the License. 15044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams */ 16044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams 171f5754133a22d0d899f6890963a777739ab2aac6Robert Ly/** @file rs_quaternion.rsh 18044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * \brief Quaternion routines 19044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * 20044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * 21044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams */ 22044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams 23044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams#ifndef __RS_QUATERNION_RSH__ 24044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams#define __RS_QUATERNION_RSH__ 25044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams 26044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams 27044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams/** 28044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * Set the quaternion components 29044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * @param w component 30044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * @param x component 31044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * @param y component 32044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * @param z component 33044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams */ 34044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Samsstatic void __attribute__((overloadable)) 35044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason SamsrsQuaternionSet(rs_quaternion *q, float w, float x, float y, float z) { 36044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams q->w = w; 37044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams q->x = x; 38044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams q->y = y; 39044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams q->z = z; 40044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams} 41044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams 42044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams/** 43044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * Set the quaternion from another quaternion 44044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * @param q destination quaternion 45044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * @param rhs source quaternion 46044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams */ 47044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Samsstatic void __attribute__((overloadable)) 48044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason SamsrsQuaternionSet(rs_quaternion *q, const rs_quaternion *rhs) { 49044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams q->w = rhs->w; 50044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams q->x = rhs->x; 51044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams q->y = rhs->y; 52044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams q->z = rhs->z; 53044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams} 54044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams 55044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams/** 56044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * Multiply quaternion by a scalar 57044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * @param q quaternion to multiply 58044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * @param s scalar 59044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams */ 60044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Samsstatic void __attribute__((overloadable)) 61044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason SamsrsQuaternionMultiply(rs_quaternion *q, float s) { 62044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams q->w *= s; 63044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams q->x *= s; 64044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams q->y *= s; 65044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams q->z *= s; 66044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams} 67044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams 68044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams/** 69044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * Add two quaternions 70044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * @param q destination quaternion to add to 71044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * @param rsh right hand side quaternion to add 72044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams */ 73044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Samsstatic void 74044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason SamsrsQuaternionAdd(rs_quaternion *q, const rs_quaternion *rhs) { 75044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams q->w *= rhs->w; 76044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams q->x *= rhs->x; 77044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams q->y *= rhs->y; 78044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams q->z *= rhs->z; 79044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams} 80044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams 81044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams/** 82044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * Loads a quaternion that represents a rotation about an arbitrary unit vector 83044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * @param q quaternion to set 84044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * @param rot angle to rotate by 85044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * @param x component of a vector 86044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * @param y component of a vector 87044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * @param x component of a vector 88044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams */ 89044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Samsstatic void 90044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason SamsrsQuaternionLoadRotateUnit(rs_quaternion *q, float rot, float x, float y, float z) { 91044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams rot *= (float)(M_PI / 180.0f) * 0.5f; 92044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams float c = cos(rot); 93044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams float s = sin(rot); 94044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams 95044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams q->w = c; 96044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams q->x = x * s; 97044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams q->y = y * s; 98044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams q->z = z * s; 99044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams} 100044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams 101044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams/** 102044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * Loads a quaternion that represents a rotation about an arbitrary vector 103044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * (doesn't have to be unit) 104044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * @param q quaternion to set 105044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * @param rot angle to rotate by 106044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * @param x component of a vector 107044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * @param y component of a vector 108044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * @param x component of a vector 109044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams */ 110044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Samsstatic void 111044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason SamsrsQuaternionLoadRotate(rs_quaternion *q, float rot, float x, float y, float z) { 112044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams const float len = x*x + y*y + z*z; 113044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams if (len != 1) { 114044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams const float recipLen = 1.f / sqrt(len); 115044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams x *= recipLen; 116044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams y *= recipLen; 117044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams z *= recipLen; 118044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams } 119044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams rsQuaternionLoadRotateUnit(q, rot, x, y, z); 120044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams} 121044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams 122044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams/** 123044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * Conjugates the quaternion 124044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * @param q quaternion to conjugate 125044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams */ 126044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Samsstatic void 127044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason SamsrsQuaternionConjugate(rs_quaternion *q) { 128044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams q->x = -q->x; 129044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams q->y = -q->y; 130044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams q->z = -q->z; 131044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams} 132044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams 133044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams/** 134044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * Dot product of two quaternions 135044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * @param q0 first quaternion 136044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * @param q1 second quaternion 137044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * @return dot product between q0 and q1 138044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams */ 139044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Samsstatic float 140044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason SamsrsQuaternionDot(const rs_quaternion *q0, const rs_quaternion *q1) { 141044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams return q0->w*q1->w + q0->x*q1->x + q0->y*q1->y + q0->z*q1->z; 142044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams} 143044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams 144044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams/** 145044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * Normalizes the quaternion 146044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * @param q quaternion to normalize 147044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams */ 148044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Samsstatic void 149044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason SamsrsQuaternionNormalize(rs_quaternion *q) { 150044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams const float len = rsQuaternionDot(q, q); 151044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams if (len != 1) { 152044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams const float recipLen = 1.f / sqrt(len); 153044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams rsQuaternionMultiply(q, recipLen); 154044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams } 155044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams} 156044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams 157044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams/** 158bd7b1a9612b3f5119af42ed7ec4a88929a2d0d10Alex Sakhartchouk * Multiply quaternion by another quaternion 159bd7b1a9612b3f5119af42ed7ec4a88929a2d0d10Alex Sakhartchouk * @param q destination quaternion 160bd7b1a9612b3f5119af42ed7ec4a88929a2d0d10Alex Sakhartchouk * @param rhs right hand side quaternion to multiply by 161bd7b1a9612b3f5119af42ed7ec4a88929a2d0d10Alex Sakhartchouk */ 162bd7b1a9612b3f5119af42ed7ec4a88929a2d0d10Alex Sakhartchoukstatic void __attribute__((overloadable)) 163bd7b1a9612b3f5119af42ed7ec4a88929a2d0d10Alex SakhartchoukrsQuaternionMultiply(rs_quaternion *q, const rs_quaternion *rhs) { 164bd7b1a9612b3f5119af42ed7ec4a88929a2d0d10Alex Sakhartchouk rs_quaternion qtmp; 165bd7b1a9612b3f5119af42ed7ec4a88929a2d0d10Alex Sakhartchouk rsQuaternionSet(&qtmp, q); 166bd7b1a9612b3f5119af42ed7ec4a88929a2d0d10Alex Sakhartchouk 167bd7b1a9612b3f5119af42ed7ec4a88929a2d0d10Alex Sakhartchouk q->w = qtmp.w*rhs->w - qtmp.x*rhs->x - qtmp.y*rhs->y - qtmp.z*rhs->z; 168bd7b1a9612b3f5119af42ed7ec4a88929a2d0d10Alex Sakhartchouk q->x = qtmp.w*rhs->x + qtmp.x*rhs->w + qtmp.y*rhs->z - qtmp.z*rhs->y; 169bd7b1a9612b3f5119af42ed7ec4a88929a2d0d10Alex Sakhartchouk q->y = qtmp.w*rhs->y + qtmp.y*rhs->w + qtmp.z*rhs->x - qtmp.x*rhs->z; 170bd7b1a9612b3f5119af42ed7ec4a88929a2d0d10Alex Sakhartchouk q->z = qtmp.w*rhs->z + qtmp.z*rhs->w + qtmp.x*rhs->y - qtmp.y*rhs->x; 171bd7b1a9612b3f5119af42ed7ec4a88929a2d0d10Alex Sakhartchouk rsQuaternionNormalize(q); 172bd7b1a9612b3f5119af42ed7ec4a88929a2d0d10Alex Sakhartchouk} 173bd7b1a9612b3f5119af42ed7ec4a88929a2d0d10Alex Sakhartchouk 174bd7b1a9612b3f5119af42ed7ec4a88929a2d0d10Alex Sakhartchouk/** 175044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * Performs spherical linear interpolation between two quaternions 176044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * @param q result quaternion from interpolation 177044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * @param q0 first param 178044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * @param q1 second param 179044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * @param t how much to interpolate by 180044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams */ 181044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Samsstatic void 182044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason SamsrsQuaternionSlerp(rs_quaternion *q, const rs_quaternion *q0, const rs_quaternion *q1, float t) { 183044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams if (t <= 0.0f) { 184044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams rsQuaternionSet(q, q0); 185044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams return; 186044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams } 187044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams if (t >= 1.0f) { 188044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams rsQuaternionSet(q, q1); 189044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams return; 190044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams } 191044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams 192044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams rs_quaternion tempq0, tempq1; 193044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams rsQuaternionSet(&tempq0, q0); 194044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams rsQuaternionSet(&tempq1, q1); 195044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams 196044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams float angle = rsQuaternionDot(q0, q1); 197044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams if (angle < 0) { 198044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams rsQuaternionMultiply(&tempq0, -1.0f); 199044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams angle *= -1.0f; 200044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams } 201044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams 202044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams float scale, invScale; 203044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams if (angle + 1.0f > 0.05f) { 204044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams if (1.0f - angle >= 0.05f) { 205044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams float theta = acos(angle); 206044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams float invSinTheta = 1.0f / sin(theta); 207044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams scale = sin(theta * (1.0f - t)) * invSinTheta; 208044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams invScale = sin(theta * t) * invSinTheta; 209044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams } else { 210044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams scale = 1.0f - t; 211044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams invScale = t; 212044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams } 213044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams } else { 214044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams rsQuaternionSet(&tempq1, tempq0.z, -tempq0.y, tempq0.x, -tempq0.w); 215044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams scale = sin(M_PI * (0.5f - t)); 216044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams invScale = sin(M_PI * t); 217044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams } 218044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams 219044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams rsQuaternionSet(q, tempq0.w*scale + tempq1.w*invScale, tempq0.x*scale + tempq1.x*invScale, 220044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams tempq0.y*scale + tempq1.y*invScale, tempq0.z*scale + tempq1.z*invScale); 221044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams} 222044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams 223044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams/** 224044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * Computes rotation matrix from the normalized quaternion 225044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * @param m resulting matrix 226044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams * @param p normalized quaternion 227044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams */ 228044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Samsstatic void rsQuaternionGetMatrixUnit(rs_matrix4x4 *m, const rs_quaternion *q) { 229bd7b1a9612b3f5119af42ed7ec4a88929a2d0d10Alex Sakhartchouk float xx = q->x * q->x; 230bd7b1a9612b3f5119af42ed7ec4a88929a2d0d10Alex Sakhartchouk float xy = q->x * q->y; 231bd7b1a9612b3f5119af42ed7ec4a88929a2d0d10Alex Sakhartchouk float xz = q->x * q->z; 232bd7b1a9612b3f5119af42ed7ec4a88929a2d0d10Alex Sakhartchouk float xw = q->x * q->w; 233bd7b1a9612b3f5119af42ed7ec4a88929a2d0d10Alex Sakhartchouk float yy = q->y * q->y; 234bd7b1a9612b3f5119af42ed7ec4a88929a2d0d10Alex Sakhartchouk float yz = q->y * q->z; 235bd7b1a9612b3f5119af42ed7ec4a88929a2d0d10Alex Sakhartchouk float yw = q->y * q->w; 236bd7b1a9612b3f5119af42ed7ec4a88929a2d0d10Alex Sakhartchouk float zz = q->z * q->z; 237bd7b1a9612b3f5119af42ed7ec4a88929a2d0d10Alex Sakhartchouk float zw = q->z * q->w; 238bd7b1a9612b3f5119af42ed7ec4a88929a2d0d10Alex Sakhartchouk 239bd7b1a9612b3f5119af42ed7ec4a88929a2d0d10Alex Sakhartchouk m->m[0] = 1.0f - 2.0f * ( yy + zz ); 240bd7b1a9612b3f5119af42ed7ec4a88929a2d0d10Alex Sakhartchouk m->m[4] = 2.0f * ( xy - zw ); 241bd7b1a9612b3f5119af42ed7ec4a88929a2d0d10Alex Sakhartchouk m->m[8] = 2.0f * ( xz + yw ); 242bd7b1a9612b3f5119af42ed7ec4a88929a2d0d10Alex Sakhartchouk m->m[1] = 2.0f * ( xy + zw ); 243bd7b1a9612b3f5119af42ed7ec4a88929a2d0d10Alex Sakhartchouk m->m[5] = 1.0f - 2.0f * ( xx + zz ); 244bd7b1a9612b3f5119af42ed7ec4a88929a2d0d10Alex Sakhartchouk m->m[9] = 2.0f * ( yz - xw ); 245bd7b1a9612b3f5119af42ed7ec4a88929a2d0d10Alex Sakhartchouk m->m[2] = 2.0f * ( xz - yw ); 246bd7b1a9612b3f5119af42ed7ec4a88929a2d0d10Alex Sakhartchouk m->m[6] = 2.0f * ( yz + xw ); 247bd7b1a9612b3f5119af42ed7ec4a88929a2d0d10Alex Sakhartchouk m->m[10] = 1.0f - 2.0f * ( xx + yy ); 248bd7b1a9612b3f5119af42ed7ec4a88929a2d0d10Alex Sakhartchouk m->m[3] = m->m[7] = m->m[11] = m->m[12] = m->m[13] = m->m[14] = 0.0f; 249044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams m->m[15] = 1.0f; 250044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams} 251044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams 252044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams#endif 253044e2ee36ffe6520570a7f0207d75a8fce8b8e91Jason Sams 254