Fixes and optimizations of two quaternion functions.

Change-Id: Ie5a3f16d49b716d91b6f68fa389473548cecb80d

1 files changed, 37 insertions, 41 deletions

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 @@ rsQuaternionMultiply(rs_quaternion *q, float s) {
 }
 
 /**
- * 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 @@ rsQuaternionNormalize(rs_quaternion *q) {
 }
 
 /**
+ * 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 @@ rsQuaternionSlerp(rs_quaternion *q, const rs_quaternion *q0, const rs_quaternion
  * @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;
-
-    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;
+    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 - 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;
 }