1 #include "rs_core.rsh" 2 3 /* Implementation of Core Runtime */ 4 5 6 ///////////////////////////////////////////////////// 7 // Quaternion ops 8 ///////////////////////////////////////////////////// 9 10 #if (defined(RS_VERSION) && (RS_VERSION >= 24)) 11 extern void __attribute__((overloadable)) rsQuaternionAdd(rs_quaternion * q,const rs_quaternion * rhs)12 rsQuaternionAdd(rs_quaternion* q, const rs_quaternion* rhs) { 13 q->w += rhs->w; 14 q->x += rhs->x; 15 q->y += rhs->y; 16 q->z += rhs->z; 17 } 18 19 extern void __attribute__((overloadable)) rsQuaternionConjugate(rs_quaternion * q)20 rsQuaternionConjugate(rs_quaternion* q) { 21 q->x = -q->x; 22 q->y = -q->y; 23 q->z = -q->z; 24 } 25 26 extern float __attribute__((overloadable)) rsQuaternionDot(const rs_quaternion * q0,const rs_quaternion * q1)27 rsQuaternionDot(const rs_quaternion* q0, const rs_quaternion* q1) { 28 return q0->w*q1->w + q0->x*q1->x + q0->y*q1->y + q0->z*q1->z; 29 } 30 31 extern void __attribute__((overloadable)) rsQuaternionGetMatrixUnit(rs_matrix4x4 * m,const rs_quaternion * q)32 rsQuaternionGetMatrixUnit(rs_matrix4x4* m, const rs_quaternion* q) { 33 float xx = q->x * q->x; 34 float xy = q->x * q->y; 35 float xz = q->x * q->z; 36 float xw = q->x * q->w; 37 float yy = q->y * q->y; 38 float yz = q->y * q->z; 39 float yw = q->y * q->w; 40 float zz = q->z * q->z; 41 float zw = q->z * q->w; 42 43 m->m[0] = 1.0f - 2.0f * ( yy + zz ); 44 m->m[4] = 2.0f * ( xy - zw ); 45 m->m[8] = 2.0f * ( xz + yw ); 46 m->m[1] = 2.0f * ( xy + zw ); 47 m->m[5] = 1.0f - 2.0f * ( xx + zz ); 48 m->m[9] = 2.0f * ( yz - xw ); 49 m->m[2] = 2.0f * ( xz - yw ); 50 m->m[6] = 2.0f * ( yz + xw ); 51 m->m[10] = 1.0f - 2.0f * ( xx + yy ); 52 m->m[3] = m->m[7] = m->m[11] = m->m[12] = m->m[13] = m->m[14] = 0.0f; 53 m->m[15] = 1.0f; 54 } 55 56 extern void __attribute__((overloadable)) rsQuaternionLoadRotateUnit(rs_quaternion * q,float rot,float x,float y,float z)57 rsQuaternionLoadRotateUnit(rs_quaternion* q, float rot, float x, float y, float z) { 58 rot *= (float)(M_PI / 180.0f) * 0.5f; 59 float c = cos(rot); 60 float s = sin(rot); 61 62 q->w = c; 63 q->x = x * s; 64 q->y = y * s; 65 q->z = z * s; 66 } 67 68 extern void __attribute__((overloadable)) rsQuaternionSet(rs_quaternion * q,float w,float x,float y,float z)69 rsQuaternionSet(rs_quaternion* q, float w, float x, float y, float z) { 70 q->w = w; 71 q->x = x; 72 q->y = y; 73 q->z = z; 74 } 75 76 extern void __attribute__((overloadable)) rsQuaternionSet(rs_quaternion * q,const rs_quaternion * rhs)77 rsQuaternionSet(rs_quaternion* q, const rs_quaternion* rhs) { 78 q->w = rhs->w; 79 q->x = rhs->x; 80 q->y = rhs->y; 81 q->z = rhs->z; 82 } 83 84 extern void __attribute__((overloadable)) rsQuaternionLoadRotate(rs_quaternion * q,float rot,float x,float y,float z)85 rsQuaternionLoadRotate(rs_quaternion* q, float rot, float x, float y, float z) { 86 const float len = x*x + y*y + z*z; 87 if (len != 1) { 88 const float recipLen = 1.f / sqrt(len); 89 x *= recipLen; 90 y *= recipLen; 91 z *= recipLen; 92 } 93 rsQuaternionLoadRotateUnit(q, rot, x, y, z); 94 } 95 96 extern void __attribute__((overloadable)) rsQuaternionNormalize(rs_quaternion * q)97 rsQuaternionNormalize(rs_quaternion* q) { 98 const float len = rsQuaternionDot(q, q); 99 if (len != 1) { 100 const float recipLen = 1.f / sqrt(len); 101 q->w *= recipLen; 102 q->x *= recipLen; 103 q->y *= recipLen; 104 q->z *= recipLen; 105 } 106 } 107 108 extern void __attribute__((overloadable)) rsQuaternionMultiply(rs_quaternion * q,float scalar)109 rsQuaternionMultiply(rs_quaternion* q, float scalar) { 110 q->w *= scalar; 111 q->x *= scalar; 112 q->y *= scalar; 113 q->z *= scalar; 114 } 115 116 extern void __attribute__((overloadable)) rsQuaternionMultiply(rs_quaternion * q,const rs_quaternion * rhs)117 rsQuaternionMultiply(rs_quaternion* q, const rs_quaternion* rhs) { 118 rs_quaternion qtmp; 119 rsQuaternionSet(&qtmp, q); 120 121 q->w = qtmp.w*rhs->w - qtmp.x*rhs->x - qtmp.y*rhs->y - qtmp.z*rhs->z; 122 q->x = qtmp.w*rhs->x + qtmp.x*rhs->w + qtmp.y*rhs->z - qtmp.z*rhs->y; 123 q->y = qtmp.w*rhs->y + qtmp.y*rhs->w + qtmp.z*rhs->x - qtmp.x*rhs->z; 124 q->z = qtmp.w*rhs->z + qtmp.z*rhs->w + qtmp.x*rhs->y - qtmp.y*rhs->x; 125 rsQuaternionNormalize(q); 126 } 127 128 extern void __attribute__((overloadable)) rsQuaternionSlerp(rs_quaternion * q,const rs_quaternion * q0,const rs_quaternion * q1,float t)129 rsQuaternionSlerp(rs_quaternion* q, const rs_quaternion* q0, const rs_quaternion* q1, float t) { 130 if (t <= 0.0f) { 131 rsQuaternionSet(q, q0); 132 return; 133 } 134 if (t >= 1.0f) { 135 rsQuaternionSet(q, q1); 136 return; 137 } 138 139 rs_quaternion tempq0, tempq1; 140 rsQuaternionSet(&tempq0, q0); 141 rsQuaternionSet(&tempq1, q1); 142 143 float angle = rsQuaternionDot(q0, q1); 144 if (angle < 0) { 145 rsQuaternionMultiply(&tempq0, -1.0f); 146 angle *= -1.0f; 147 } 148 149 float scale, invScale; 150 if (angle + 1.0f > 0.05f) { 151 if (1.0f - angle >= 0.05f) { 152 float theta = acos(angle); 153 float invSinTheta = 1.0f / sin(theta); 154 scale = sin(theta * (1.0f - t)) * invSinTheta; 155 invScale = sin(theta * t) * invSinTheta; 156 } else { 157 scale = 1.0f - t; 158 invScale = t; 159 } 160 } else { 161 rsQuaternionSet(&tempq1, tempq0.z, -tempq0.y, tempq0.x, -tempq0.w); 162 scale = sin(M_PI * (0.5f - t)); 163 invScale = sin(M_PI * t); 164 } 165 166 rsQuaternionSet(q, tempq0.w*scale + tempq1.w*invScale, tempq0.x*scale + tempq1.x*invScale, 167 tempq0.y*scale + tempq1.y*invScale, tempq0.z*scale + tempq1.z*invScale); 168 } 169 #endif // (defined(RS_VERSION) && (RS_VERSION >= 24)) 170