114c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham#include "rs_core.rsh" 214c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham 314c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham/* Implementation of Core Runtime */ 414c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham 514c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham 614c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham///////////////////////////////////////////////////// 714c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham// Quaternion ops 814c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham///////////////////////////////////////////////////// 914c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham 1014c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham#if (defined(RS_VERSION) && (RS_VERSION >= UNRELEASED)) 1114c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckhamextern void __attribute__((overloadable)) 1214c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham rsQuaternionAdd(rs_quaternion* q, const rs_quaternion* rhs) { 1314c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham q->w += rhs->w; 1414c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham q->x += rhs->x; 1514c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham q->y += rhs->y; 1614c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham q->z += rhs->z; 1714c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham} 1814c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham 1914c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckhamextern void __attribute__((overloadable)) 2014c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham rsQuaternionConjugate(rs_quaternion* q) { 2114c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham q->x = -q->x; 2214c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham q->y = -q->y; 2314c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham q->z = -q->z; 2414c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham} 2514c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham 2614c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckhamextern float __attribute__((overloadable)) 2714c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham rsQuaternionDot(const rs_quaternion* q0, const rs_quaternion* q1) { 2814c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham return q0->w*q1->w + q0->x*q1->x + q0->y*q1->y + q0->z*q1->z; 2914c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham} 3014c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham 3114c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckhamextern void __attribute__((overloadable)) 3214c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham rsQuaternionGetMatrixUnit(rs_matrix4x4* m, const rs_quaternion* q) { 3314c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham float xx = q->x * q->x; 3414c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham float xy = q->x * q->y; 3514c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham float xz = q->x * q->z; 3614c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham float xw = q->x * q->w; 3714c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham float yy = q->y * q->y; 3814c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham float yz = q->y * q->z; 3914c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham float yw = q->y * q->w; 4014c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham float zz = q->z * q->z; 4114c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham float zw = q->z * q->w; 4214c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham 4314c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham m->m[0] = 1.0f - 2.0f * ( yy + zz ); 4414c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham m->m[4] = 2.0f * ( xy - zw ); 4514c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham m->m[8] = 2.0f * ( xz + yw ); 4614c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham m->m[1] = 2.0f * ( xy + zw ); 4714c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham m->m[5] = 1.0f - 2.0f * ( xx + zz ); 4814c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham m->m[9] = 2.0f * ( yz - xw ); 4914c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham m->m[2] = 2.0f * ( xz - yw ); 5014c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham m->m[6] = 2.0f * ( yz + xw ); 5114c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham m->m[10] = 1.0f - 2.0f * ( xx + yy ); 5214c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham m->m[3] = m->m[7] = m->m[11] = m->m[12] = m->m[13] = m->m[14] = 0.0f; 5314c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham m->m[15] = 1.0f; 5414c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham} 5514c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham 5614c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckhamextern void __attribute__((overloadable)) 5714c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham rsQuaternionLoadRotateUnit(rs_quaternion* q, float rot, float x, float y, float z) { 5814c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham rot *= (float)(M_PI / 180.0f) * 0.5f; 5914c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham float c = cos(rot); 6014c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham float s = sin(rot); 6114c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham 6214c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham q->w = c; 6314c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham q->x = x * s; 6414c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham q->y = y * s; 6514c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham q->z = z * s; 6614c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham} 6714c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham 6814c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckhamextern void __attribute__((overloadable)) 6914c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham rsQuaternionSet(rs_quaternion* q, float w, float x, float y, float z) { 7014c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham q->w = w; 7114c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham q->x = x; 7214c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham q->y = y; 7314c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham q->z = z; 7414c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham} 7514c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham 7614c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckhamextern void __attribute__((overloadable)) 7714c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham rsQuaternionSet(rs_quaternion* q, const rs_quaternion* rhs) { 7814c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham q->w = rhs->w; 7914c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham q->x = rhs->x; 8014c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham q->y = rhs->y; 8114c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham q->z = rhs->z; 8214c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham} 8314c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham 8414c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckhamextern void __attribute__((overloadable)) 8514c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham rsQuaternionLoadRotate(rs_quaternion* q, float rot, float x, float y, float z) { 8614c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham const float len = x*x + y*y + z*z; 8714c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham if (len != 1) { 8814c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham const float recipLen = 1.f / sqrt(len); 8914c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham x *= recipLen; 9014c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham y *= recipLen; 9114c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham z *= recipLen; 9214c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham } 9314c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham rsQuaternionLoadRotateUnit(q, rot, x, y, z); 9414c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham} 9514c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham 9614c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckhamextern void __attribute__((overloadable)) 9714c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham rsQuaternionNormalize(rs_quaternion* q) { 9814c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham const float len = rsQuaternionDot(q, q); 9914c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham if (len != 1) { 10014c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham const float recipLen = 1.f / sqrt(len); 10114c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham q->w *= recipLen; 10214c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham q->x *= recipLen; 10314c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham q->y *= recipLen; 10414c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham q->z *= recipLen; 10514c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham } 10614c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham} 10714c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham 10814c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckhamextern void __attribute__((overloadable)) 10914c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham rsQuaternionMultiply(rs_quaternion* q, float scalar) { 11014c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham q->w *= scalar; 11114c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham q->x *= scalar; 11214c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham q->y *= scalar; 11314c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham q->z *= scalar; 11414c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham} 11514c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham 11614c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckhamextern void __attribute__((overloadable)) 11714c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham rsQuaternionMultiply(rs_quaternion* q, const rs_quaternion* rhs) { 11814c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham rs_quaternion qtmp; 11914c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham rsQuaternionSet(&qtmp, q); 12014c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham 12114c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham q->w = qtmp.w*rhs->w - qtmp.x*rhs->x - qtmp.y*rhs->y - qtmp.z*rhs->z; 12214c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham q->x = qtmp.w*rhs->x + qtmp.x*rhs->w + qtmp.y*rhs->z - qtmp.z*rhs->y; 12314c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham q->y = qtmp.w*rhs->y + qtmp.y*rhs->w + qtmp.z*rhs->x - qtmp.x*rhs->z; 12414c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham q->z = qtmp.w*rhs->z + qtmp.z*rhs->w + qtmp.x*rhs->y - qtmp.y*rhs->x; 12514c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham rsQuaternionNormalize(q); 12614c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham} 12714c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham 12814c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckhamextern void __attribute__((overloadable)) 12914c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham rsQuaternionSlerp(rs_quaternion* q, const rs_quaternion* q0, const rs_quaternion* q1, float t) { 13014c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham if (t <= 0.0f) { 13114c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham rsQuaternionSet(q, q0); 13214c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham return; 13314c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham } 13414c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham if (t >= 1.0f) { 13514c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham rsQuaternionSet(q, q1); 13614c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham return; 13714c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham } 13814c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham 13914c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham rs_quaternion tempq0, tempq1; 14014c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham rsQuaternionSet(&tempq0, q0); 14114c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham rsQuaternionSet(&tempq1, q1); 14214c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham 14314c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham float angle = rsQuaternionDot(q0, q1); 14414c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham if (angle < 0) { 14514c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham rsQuaternionMultiply(&tempq0, -1.0f); 14614c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham angle *= -1.0f; 14714c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham } 14814c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham 14914c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham float scale, invScale; 15014c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham if (angle + 1.0f > 0.05f) { 15114c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham if (1.0f - angle >= 0.05f) { 15214c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham float theta = acos(angle); 15314c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham float invSinTheta = 1.0f / sin(theta); 15414c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham scale = sin(theta * (1.0f - t)) * invSinTheta; 15514c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham invScale = sin(theta * t) * invSinTheta; 15614c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham } else { 15714c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham scale = 1.0f - t; 15814c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham invScale = t; 15914c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham } 16014c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham } else { 16114c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham rsQuaternionSet(&tempq1, tempq0.z, -tempq0.y, tempq0.x, -tempq0.w); 16214c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham scale = sin(M_PI * (0.5f - t)); 16314c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham invScale = sin(M_PI * t); 16414c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham } 16514c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham 16614c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham rsQuaternionSet(q, tempq0.w*scale + tempq1.w*invScale, tempq0.x*scale + tempq1.x*invScale, 16714c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham tempq0.y*scale + tempq1.y*invScale, tempq0.z*scale + tempq1.z*invScale); 16814c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham} 16914c8aa51782107afb6f84187233f5ac8f6da0373Verena Beckham#endif // (defined(RS_VERSION) && (RS_VERSION >= UNRELEASED)) 170