10f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines/* 20f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * Copyright (C) 2011 The Android Open Source Project 30f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * 40f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * Licensed under the Apache License, Version 2.0 (the "License"); 50f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * you may not use this file except in compliance with the License. 60f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * You may obtain a copy of the License at 70f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * 80f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * http://www.apache.org/licenses/LICENSE-2.0 90f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * 100f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * Unless required by applicable law or agreed to in writing, software 110f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * distributed under the License is distributed on an "AS IS" BASIS, 120f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 130f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * See the License for the specific language governing permissions and 140f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * limitations under the License. 150f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines */ 160f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 170f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines/** @file rs_quaternion.rsh 180f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * \brief Quaternion routines 190f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * 200f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * 210f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines */ 220f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 230f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines#ifndef __RS_QUATERNION_RSH__ 240f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines#define __RS_QUATERNION_RSH__ 250f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 260f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 270f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines/** 280f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * Set the quaternion components 290f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * @param w component 300f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * @param x component 310f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * @param y component 320f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * @param z component 330f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines */ 340f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hinesstatic void __attribute__((overloadable)) 350f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen HinesrsQuaternionSet(rs_quaternion *q, float w, float x, float y, float z) { 360f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines q->w = w; 370f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines q->x = x; 380f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines q->y = y; 390f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines q->z = z; 400f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines} 410f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 420f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines/** 430f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * Set the quaternion from another quaternion 440f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * @param q destination quaternion 450f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * @param rhs source quaternion 460f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines */ 470f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hinesstatic void __attribute__((overloadable)) 480f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen HinesrsQuaternionSet(rs_quaternion *q, const rs_quaternion *rhs) { 490f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines q->w = rhs->w; 500f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines q->x = rhs->x; 510f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines q->y = rhs->y; 520f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines q->z = rhs->z; 530f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines} 540f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 550f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines/** 560f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * Multiply quaternion by a scalar 570f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * @param q quaternion to multiply 580f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * @param s scalar 590f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines */ 600f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hinesstatic void __attribute__((overloadable)) 610f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen HinesrsQuaternionMultiply(rs_quaternion *q, float s) { 620f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines q->w *= s; 630f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines q->x *= s; 640f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines q->y *= s; 650f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines q->z *= s; 660f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines} 670f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 680f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines/** 690f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * Add two quaternions 700f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * @param q destination quaternion to add to 710f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * @param rsh right hand side quaternion to add 720f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines */ 730f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hinesstatic void 740f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen HinesrsQuaternionAdd(rs_quaternion *q, const rs_quaternion *rhs) { 750f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines q->w *= rhs->w; 760f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines q->x *= rhs->x; 770f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines q->y *= rhs->y; 780f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines q->z *= rhs->z; 790f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines} 800f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 810f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines/** 820f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * Loads a quaternion that represents a rotation about an arbitrary unit vector 830f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * @param q quaternion to set 840f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * @param rot angle to rotate by 850f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * @param x component of a vector 860f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * @param y component of a vector 870f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * @param x component of a vector 880f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines */ 890f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hinesstatic void 900f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen HinesrsQuaternionLoadRotateUnit(rs_quaternion *q, float rot, float x, float y, float z) { 910f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines rot *= (float)(M_PI / 180.0f) * 0.5f; 920f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines float c = cos(rot); 930f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines float s = sin(rot); 940f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 950f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines q->w = c; 960f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines q->x = x * s; 970f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines q->y = y * s; 980f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines q->z = z * s; 990f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines} 1000f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 1010f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines/** 1020f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * Loads a quaternion that represents a rotation about an arbitrary vector 1030f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * (doesn't have to be unit) 1040f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * @param q quaternion to set 1050f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * @param rot angle to rotate by 1060f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * @param x component of a vector 1070f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * @param y component of a vector 1080f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * @param x component of a vector 1090f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines */ 1100f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hinesstatic void 1110f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen HinesrsQuaternionLoadRotate(rs_quaternion *q, float rot, float x, float y, float z) { 1120f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines const float len = x*x + y*y + z*z; 1130f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines if (len != 1) { 1140f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines const float recipLen = 1.f / sqrt(len); 1150f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines x *= recipLen; 1160f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines y *= recipLen; 1170f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines z *= recipLen; 1180f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines } 1190f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines rsQuaternionLoadRotateUnit(q, rot, x, y, z); 1200f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines} 1210f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 1220f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines/** 1230f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * Conjugates the quaternion 1240f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * @param q quaternion to conjugate 1250f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines */ 1260f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hinesstatic void 1270f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen HinesrsQuaternionConjugate(rs_quaternion *q) { 1280f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines q->x = -q->x; 1290f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines q->y = -q->y; 1300f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines q->z = -q->z; 1310f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines} 1320f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 1330f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines/** 1340f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * Dot product of two quaternions 1350f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * @param q0 first quaternion 1360f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * @param q1 second quaternion 1370f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * @return dot product between q0 and q1 1380f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines */ 1390f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hinesstatic float 1400f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen HinesrsQuaternionDot(const rs_quaternion *q0, const rs_quaternion *q1) { 1410f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines return q0->w*q1->w + q0->x*q1->x + q0->y*q1->y + q0->z*q1->z; 1420f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines} 1430f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 1440f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines/** 1450f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * Normalizes the quaternion 1460f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * @param q quaternion to normalize 1470f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines */ 1480f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hinesstatic void 1490f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen HinesrsQuaternionNormalize(rs_quaternion *q) { 1500f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines const float len = rsQuaternionDot(q, q); 1510f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines if (len != 1) { 1520f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines const float recipLen = 1.f / sqrt(len); 1530f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines rsQuaternionMultiply(q, recipLen); 1540f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines } 1550f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines} 1560f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 1570f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines/** 1580f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * Multiply quaternion by another quaternion 1590f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * @param q destination quaternion 1600f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * @param rhs right hand side quaternion to multiply by 1610f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines */ 1620f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hinesstatic void __attribute__((overloadable)) 1630f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen HinesrsQuaternionMultiply(rs_quaternion *q, const rs_quaternion *rhs) { 1640f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines rs_quaternion qtmp; 1650f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines rsQuaternionSet(&qtmp, q); 1660f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 1670f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines q->w = qtmp.w*rhs->w - qtmp.x*rhs->x - qtmp.y*rhs->y - qtmp.z*rhs->z; 1680f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines q->x = qtmp.w*rhs->x + qtmp.x*rhs->w + qtmp.y*rhs->z - qtmp.z*rhs->y; 1690f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines q->y = qtmp.w*rhs->y + qtmp.y*rhs->w + qtmp.z*rhs->x - qtmp.x*rhs->z; 1700f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines q->z = qtmp.w*rhs->z + qtmp.z*rhs->w + qtmp.x*rhs->y - qtmp.y*rhs->x; 1710f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines rsQuaternionNormalize(q); 1720f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines} 1730f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 1740f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines/** 1750f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * Performs spherical linear interpolation between two quaternions 1760f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * @param q result quaternion from interpolation 1770f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * @param q0 first param 1780f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * @param q1 second param 1790f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * @param t how much to interpolate by 1800f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines */ 1810f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hinesstatic void 1820f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen HinesrsQuaternionSlerp(rs_quaternion *q, const rs_quaternion *q0, const rs_quaternion *q1, float t) { 1830f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines if (t <= 0.0f) { 1840f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines rsQuaternionSet(q, q0); 1850f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines return; 1860f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines } 1870f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines if (t >= 1.0f) { 1880f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines rsQuaternionSet(q, q1); 1890f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines return; 1900f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines } 1910f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 1920f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines rs_quaternion tempq0, tempq1; 1930f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines rsQuaternionSet(&tempq0, q0); 1940f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines rsQuaternionSet(&tempq1, q1); 1950f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 1960f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines float angle = rsQuaternionDot(q0, q1); 1970f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines if (angle < 0) { 1980f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines rsQuaternionMultiply(&tempq0, -1.0f); 1990f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines angle *= -1.0f; 2000f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines } 2010f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 2020f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines float scale, invScale; 2030f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines if (angle + 1.0f > 0.05f) { 2040f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines if (1.0f - angle >= 0.05f) { 2050f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines float theta = acos(angle); 2060f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines float invSinTheta = 1.0f / sin(theta); 2070f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines scale = sin(theta * (1.0f - t)) * invSinTheta; 2080f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines invScale = sin(theta * t) * invSinTheta; 2090f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines } else { 2100f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines scale = 1.0f - t; 2110f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines invScale = t; 2120f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines } 2130f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines } else { 2140f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines rsQuaternionSet(&tempq1, tempq0.z, -tempq0.y, tempq0.x, -tempq0.w); 2150f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines scale = sin(M_PI * (0.5f - t)); 2160f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines invScale = sin(M_PI * t); 2170f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines } 2180f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 2190f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines rsQuaternionSet(q, tempq0.w*scale + tempq1.w*invScale, tempq0.x*scale + tempq1.x*invScale, 2200f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines tempq0.y*scale + tempq1.y*invScale, tempq0.z*scale + tempq1.z*invScale); 2210f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines} 2220f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 2230f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines/** 2240f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * Computes rotation matrix from the normalized quaternion 2250f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * @param m resulting matrix 2260f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines * @param p normalized quaternion 2270f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines */ 2280f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hinesstatic void rsQuaternionGetMatrixUnit(rs_matrix4x4 *m, const rs_quaternion *q) { 2290f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines float xx = q->x * q->x; 2300f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines float xy = q->x * q->y; 2310f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines float xz = q->x * q->z; 2320f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines float xw = q->x * q->w; 2330f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines float yy = q->y * q->y; 2340f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines float yz = q->y * q->z; 2350f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines float yw = q->y * q->w; 2360f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines float zz = q->z * q->z; 2370f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines float zw = q->z * q->w; 2380f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 2390f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m->m[0] = 1.0f - 2.0f * ( yy + zz ); 2400f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m->m[4] = 2.0f * ( xy - zw ); 2410f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m->m[8] = 2.0f * ( xz + yw ); 2420f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m->m[1] = 2.0f * ( xy + zw ); 2430f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m->m[5] = 1.0f - 2.0f * ( xx + zz ); 2440f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m->m[9] = 2.0f * ( yz - xw ); 2450f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m->m[2] = 2.0f * ( xz - yw ); 2460f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m->m[6] = 2.0f * ( yz + xw ); 2470f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m->m[10] = 1.0f - 2.0f * ( xx + yy ); 2480f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m->m[3] = m->m[7] = m->m[11] = m->m[12] = m->m[13] = m->m[14] = 0.0f; 2490f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines m->m[15] = 1.0f; 2500f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines} 2510f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 2520f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines#endif 2530f6f72e19db852cc253fd2fc05459abdf8d5c3afStephen Hines 254