Merge "Fixes and optimizations of two quaternion functions."
diff --git a/libs/rs/scriptc/rs_quaternion.rsh b/libs/rs/scriptc/rs_quaternion.rsh
index 23945ae..4e08d2f 100644
--- a/libs/rs/scriptc/rs_quaternion.rsh
+++ b/libs/rs/scriptc/rs_quaternion.rsh
@@ -66,19 +66,6 @@
}
/**
- * Multiply quaternion by another quaternion
- * @param q destination quaternion
- * @param rhs right hand side quaternion to multiply by
- */
-static void __attribute__((overloadable))
-rsQuaternionMultiply(rs_quaternion *q, const rs_quaternion *rhs) {
- q->w = -q->x*rhs->x - q->y*rhs->y - q->z*rhs->z + q->w*rhs->w;
- q->x = q->x*rhs->w + q->y*rhs->z - q->z*rhs->y + q->w*rhs->x;
- q->y = -q->x*rhs->z + q->y*rhs->w + q->z*rhs->x + q->w*rhs->y;
- q->z = q->x*rhs->y - q->y*rhs->x + q->z*rhs->w + q->w*rhs->z;
-}
-
-/**
* Add two quaternions
* @param q destination quaternion to add to
* @param rsh right hand side quaternion to add
@@ -168,6 +155,23 @@
}
/**
+ * Multiply quaternion by another quaternion
+ * @param q destination quaternion
+ * @param rhs right hand side quaternion to multiply by
+ */
+static void __attribute__((overloadable))
+rsQuaternionMultiply(rs_quaternion *q, const rs_quaternion *rhs) {
+ rs_quaternion qtmp;
+ rsQuaternionSet(&qtmp, q);
+
+ q->w = qtmp.w*rhs->w - qtmp.x*rhs->x - qtmp.y*rhs->y - qtmp.z*rhs->z;
+ q->x = qtmp.w*rhs->x + qtmp.x*rhs->w + qtmp.y*rhs->z - qtmp.z*rhs->y;
+ q->y = qtmp.w*rhs->y + qtmp.y*rhs->w + qtmp.z*rhs->x - qtmp.x*rhs->z;
+ q->z = qtmp.w*rhs->z + qtmp.z*rhs->w + qtmp.x*rhs->y - qtmp.y*rhs->x;
+ rsQuaternionNormalize(q);
+}
+
+/**
* Performs spherical linear interpolation between two quaternions
* @param q result quaternion from interpolation
* @param q0 first param
@@ -222,34 +226,26 @@
* @param p normalized quaternion
*/
static void rsQuaternionGetMatrixUnit(rs_matrix4x4 *m, const rs_quaternion *q) {
- float x2 = 2.0f * q->x * q->x;
- float y2 = 2.0f * q->y * q->y;
- float z2 = 2.0f * q->z * q->z;
- float xy = 2.0f * q->x * q->y;
- float wz = 2.0f * q->w * q->z;
- float xz = 2.0f * q->x * q->z;
- float wy = 2.0f * q->w * q->y;
- float wx = 2.0f * q->w * q->x;
- float yz = 2.0f * q->y * q->z;
+ float xx = q->x * q->x;
+ float xy = q->x * q->y;
+ float xz = q->x * q->z;
+ float xw = q->x * q->w;
+ float yy = q->y * q->y;
+ float yz = q->y * q->z;
+ float yw = q->y * q->w;
+ float zz = q->z * q->z;
+ float zw = q->z * q->w;
- m->m[0] = 1.0f - y2 - z2;
- m->m[1] = xy - wz;
- m->m[2] = xz + wy;
- m->m[3] = 0.0f;
-
- m->m[4] = xy + wz;
- m->m[5] = 1.0f - x2 - z2;
- m->m[6] = yz - wx;
- m->m[7] = 0.0f;
-
- m->m[8] = xz - wy;
- m->m[9] = yz - wx;
- m->m[10] = 1.0f - x2 - y2;
- m->m[11] = 0.0f;
-
- m->m[12] = 0.0f;
- m->m[13] = 0.0f;
- m->m[14] = 0.0f;
+ m->m[0] = 1.0f - 2.0f * ( yy + zz );
+ m->m[4] = 2.0f * ( xy - zw );
+ m->m[8] = 2.0f * ( xz + yw );
+ m->m[1] = 2.0f * ( xy + zw );
+ m->m[5] = 1.0f - 2.0f * ( xx + zz );
+ m->m[9] = 2.0f * ( yz - xw );
+ m->m[2] = 2.0f * ( xz - yw );
+ m->m[6] = 2.0f * ( yz + xw );
+ m->m[10] = 1.0f - 2.0f * ( xx + yy );
+ m->m[3] = m->m[7] = m->m[11] = m->m[12] = m->m[13] = m->m[14] = 0.0f;
m->m[15] = 1.0f;
}