1336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham#include "rs_core.rsh" 2336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham 3336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham/* Implementation of Core Runtime */ 4336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham 5336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham 6336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham///////////////////////////////////////////////////// 7336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham// Quaternion ops 8336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham///////////////////////////////////////////////////// 9336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham 106651d072438b1ce3cb5c996ca36c15410cb31e07Yang Ni#if (defined(RS_VERSION) && (RS_VERSION >= 24)) 11336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckhamextern void __attribute__((overloadable)) 12336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham rsQuaternionAdd(rs_quaternion* q, const rs_quaternion* rhs) { 13336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham q->w += rhs->w; 14336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham q->x += rhs->x; 15336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham q->y += rhs->y; 16336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham q->z += rhs->z; 17336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham} 18336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham 19336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckhamextern void __attribute__((overloadable)) 20336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham rsQuaternionConjugate(rs_quaternion* q) { 21336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham q->x = -q->x; 22336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham q->y = -q->y; 23336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham q->z = -q->z; 24336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham} 25336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham 26336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckhamextern float __attribute__((overloadable)) 27336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham rsQuaternionDot(const rs_quaternion* q0, const rs_quaternion* q1) { 28336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham return q0->w*q1->w + q0->x*q1->x + q0->y*q1->y + q0->z*q1->z; 29336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham} 30336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham 31336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckhamextern void __attribute__((overloadable)) 32336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham rsQuaternionGetMatrixUnit(rs_matrix4x4* m, const rs_quaternion* q) { 33336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham float xx = q->x * q->x; 34336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham float xy = q->x * q->y; 35336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham float xz = q->x * q->z; 36336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham float xw = q->x * q->w; 37336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham float yy = q->y * q->y; 38336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham float yz = q->y * q->z; 39336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham float yw = q->y * q->w; 40336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham float zz = q->z * q->z; 41336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham float zw = q->z * q->w; 42336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham 43336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham m->m[0] = 1.0f - 2.0f * ( yy + zz ); 44336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham m->m[4] = 2.0f * ( xy - zw ); 45336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham m->m[8] = 2.0f * ( xz + yw ); 46336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham m->m[1] = 2.0f * ( xy + zw ); 47336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham m->m[5] = 1.0f - 2.0f * ( xx + zz ); 48336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham m->m[9] = 2.0f * ( yz - xw ); 49336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham m->m[2] = 2.0f * ( xz - yw ); 50336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham m->m[6] = 2.0f * ( yz + xw ); 51336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham m->m[10] = 1.0f - 2.0f * ( xx + yy ); 52336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham m->m[3] = m->m[7] = m->m[11] = m->m[12] = m->m[13] = m->m[14] = 0.0f; 53336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham m->m[15] = 1.0f; 54336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham} 55336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham 56336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckhamextern void __attribute__((overloadable)) 57336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham rsQuaternionLoadRotateUnit(rs_quaternion* q, float rot, float x, float y, float z) { 58336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham rot *= (float)(M_PI / 180.0f) * 0.5f; 59336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham float c = cos(rot); 60336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham float s = sin(rot); 61336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham 62336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham q->w = c; 63336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham q->x = x * s; 64336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham q->y = y * s; 65336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham q->z = z * s; 66336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham} 67336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham 68336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckhamextern void __attribute__((overloadable)) 69336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham rsQuaternionSet(rs_quaternion* q, float w, float x, float y, float z) { 70336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham q->w = w; 71336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham q->x = x; 72336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham q->y = y; 73336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham q->z = z; 74336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham} 75336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham 76336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckhamextern void __attribute__((overloadable)) 77336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham rsQuaternionSet(rs_quaternion* q, const rs_quaternion* rhs) { 78336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham q->w = rhs->w; 79336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham q->x = rhs->x; 80336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham q->y = rhs->y; 81336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham q->z = rhs->z; 82336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham} 83336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham 84336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckhamextern void __attribute__((overloadable)) 85336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham rsQuaternionLoadRotate(rs_quaternion* q, float rot, float x, float y, float z) { 86336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham const float len = x*x + y*y + z*z; 87336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham if (len != 1) { 88336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham const float recipLen = 1.f / sqrt(len); 89336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham x *= recipLen; 90336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham y *= recipLen; 91336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham z *= recipLen; 92336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham } 93336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham rsQuaternionLoadRotateUnit(q, rot, x, y, z); 94336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham} 95336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham 96336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckhamextern void __attribute__((overloadable)) 97336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham rsQuaternionNormalize(rs_quaternion* q) { 98336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham const float len = rsQuaternionDot(q, q); 99336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham if (len != 1) { 100336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham const float recipLen = 1.f / sqrt(len); 101336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham q->w *= recipLen; 102336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham q->x *= recipLen; 103336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham q->y *= recipLen; 104336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham q->z *= recipLen; 105336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham } 106336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham} 107336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham 108336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckhamextern void __attribute__((overloadable)) 109336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham rsQuaternionMultiply(rs_quaternion* q, float scalar) { 110336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham q->w *= scalar; 111336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham q->x *= scalar; 112336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham q->y *= scalar; 113336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham q->z *= scalar; 114336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham} 115336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham 116336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckhamextern void __attribute__((overloadable)) 117336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham rsQuaternionMultiply(rs_quaternion* q, const rs_quaternion* rhs) { 118336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham rs_quaternion qtmp; 119336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham rsQuaternionSet(&qtmp, q); 120336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham 121336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham q->w = qtmp.w*rhs->w - qtmp.x*rhs->x - qtmp.y*rhs->y - qtmp.z*rhs->z; 122336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham q->x = qtmp.w*rhs->x + qtmp.x*rhs->w + qtmp.y*rhs->z - qtmp.z*rhs->y; 123336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham q->y = qtmp.w*rhs->y + qtmp.y*rhs->w + qtmp.z*rhs->x - qtmp.x*rhs->z; 124336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham q->z = qtmp.w*rhs->z + qtmp.z*rhs->w + qtmp.x*rhs->y - qtmp.y*rhs->x; 125336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham rsQuaternionNormalize(q); 126336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham} 127336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham 128336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckhamextern void __attribute__((overloadable)) 129336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham rsQuaternionSlerp(rs_quaternion* q, const rs_quaternion* q0, const rs_quaternion* q1, float t) { 130336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham if (t <= 0.0f) { 131336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham rsQuaternionSet(q, q0); 132336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham return; 133336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham } 134336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham if (t >= 1.0f) { 135336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham rsQuaternionSet(q, q1); 136336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham return; 137336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham } 138336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham 139336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham rs_quaternion tempq0, tempq1; 140336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham rsQuaternionSet(&tempq0, q0); 141336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham rsQuaternionSet(&tempq1, q1); 142336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham 143336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham float angle = rsQuaternionDot(q0, q1); 144336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham if (angle < 0) { 145336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham rsQuaternionMultiply(&tempq0, -1.0f); 146336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham angle *= -1.0f; 147336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham } 148336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham 149336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham float scale, invScale; 150336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham if (angle + 1.0f > 0.05f) { 151336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham if (1.0f - angle >= 0.05f) { 152336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham float theta = acos(angle); 153336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham float invSinTheta = 1.0f / sin(theta); 154336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham scale = sin(theta * (1.0f - t)) * invSinTheta; 155336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham invScale = sin(theta * t) * invSinTheta; 156336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham } else { 157336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham scale = 1.0f - t; 158336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham invScale = t; 159336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham } 160336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham } else { 161336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham rsQuaternionSet(&tempq1, tempq0.z, -tempq0.y, tempq0.x, -tempq0.w); 162336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham scale = sin(M_PI * (0.5f - t)); 163336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham invScale = sin(M_PI * t); 164336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham } 165336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham 166336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham rsQuaternionSet(q, tempq0.w*scale + tempq1.w*invScale, tempq0.x*scale + tempq1.x*invScale, 167336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham tempq0.y*scale + tempq1.y*invScale, tempq0.z*scale + tempq1.z*invScale); 168336fc4b3fb2df22623d51d9bbb996d11cd69f16cVerena Beckham} 1696651d072438b1ce3cb5c996ca36c15410cb31e07Yang Ni#endif // (defined(RS_VERSION) && (RS_VERSION >= 24)) 170