Move implementations of quaternion functions into driver/runtime.

Move the implementation of all quaternion functions from rs_math.rsh
driver/runtime. The old implementation remains in place for older
versions, and the move occurs only for an UNRELEASED API level.
Added a new file driver/runtime/rs_quaternion.c to contain the
implementations.

Signed-off-by: Verena Beckham <verena@codeplay.com>

(cherry picked from commit 336fc4b3fb2df22623d51d9bbb996d11cd69f16c)

Change-Id: Ib37fad0a911d05640f7b2b9e88b25cd0053108c1

2 files changed, 171 insertions, 1 deletions

diff --git a/driver/runtime/Android.mk b/driver/runtime/Android.mk
index 1b9f6d1a..8685eccf 100755
--- a/driver/runtime/Android.mk
+++ b/driver/runtime/Android.mk
@@ -28,7 +28,8 @@ clcore_base_files := \
     rs_program.c \
     rs_sample.c \
     rs_sampler.c \
-    rs_convert.c
+    rs_convert.c \
+    rs_quaternion.c
 
 clcore_cflags := -Iframeworks/rs/cpu_ref -DRS_DECLARE_EXPIRED_APIS
 
diff --git a/driver/runtime/rs_quaternion.c b/driver/runtime/rs_quaternion.c
new file mode 100644
index 00000000..8fbfd200
--- /dev/null
+++ b/driver/runtime/rs_quaternion.c
@@ -0,0 +1,169 @@
+#include "rs_core.rsh"
+
+/* Implementation of Core Runtime */
+
+
+/////////////////////////////////////////////////////
+// Quaternion ops
+/////////////////////////////////////////////////////
+
+#if (defined(RS_VERSION) && (RS_VERSION >= UNRELEASED))
+extern void __attribute__((overloadable))
+    rsQuaternionAdd(rs_quaternion* q, const rs_quaternion* rhs) {
+    q->w += rhs->w;
+    q->x += rhs->x;
+    q->y += rhs->y;
+    q->z += rhs->z;
+}
+
+extern void __attribute__((overloadable))
+    rsQuaternionConjugate(rs_quaternion* q) {
+    q->x = -q->x;
+    q->y = -q->y;
+    q->z = -q->z;
+}
+
+extern float __attribute__((overloadable))
+    rsQuaternionDot(const rs_quaternion* q0, const rs_quaternion* q1) {
+    return q0->w*q1->w + q0->x*q1->x + q0->y*q1->y + q0->z*q1->z;
+}
+
+extern void __attribute__((overloadable))
+    rsQuaternionGetMatrixUnit(rs_matrix4x4* m, const rs_quaternion* q) {
+    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;
+}
+
+extern void __attribute__((overloadable))
+    rsQuaternionLoadRotateUnit(rs_quaternion* q, float rot, float x, float y, float z) {
+    rot *= (float)(M_PI / 180.0f) * 0.5f;
+    float c = cos(rot);
+    float s = sin(rot);
+
+    q->w = c;
+    q->x = x * s;
+    q->y = y * s;
+    q->z = z * s;
+}
+
+extern void __attribute__((overloadable))
+    rsQuaternionSet(rs_quaternion* q, float w, float x, float y, float z) {
+    q->w = w;
+    q->x = x;
+    q->y = y;
+    q->z = z;
+}
+
+extern void __attribute__((overloadable))
+    rsQuaternionSet(rs_quaternion* q, const rs_quaternion* rhs) {
+    q->w = rhs->w;
+    q->x = rhs->x;
+    q->y = rhs->y;
+    q->z = rhs->z;
+}
+
+extern void __attribute__((overloadable))
+    rsQuaternionLoadRotate(rs_quaternion* q, float rot, float x, float y, float z) {
+    const float len = x*x + y*y + z*z;
+    if (len != 1) {
+        const float recipLen = 1.f / sqrt(len);
+        x *= recipLen;
+        y *= recipLen;
+        z *= recipLen;
+    }
+    rsQuaternionLoadRotateUnit(q, rot, x, y, z);
+}
+
+extern void __attribute__((overloadable))
+    rsQuaternionNormalize(rs_quaternion* q) {
+    const float len = rsQuaternionDot(q, q);
+    if (len != 1) {
+        const float recipLen = 1.f / sqrt(len);
+        q->w *= recipLen;
+        q->x *= recipLen;
+        q->y *= recipLen;
+        q->z *= recipLen;
+    }
+}
+
+extern void __attribute__((overloadable))
+    rsQuaternionMultiply(rs_quaternion* q, float scalar) {
+    q->w *= scalar;
+    q->x *= scalar;
+    q->y *= scalar;
+    q->z *= scalar;
+}
+
+extern 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);
+}
+
+extern void __attribute__((overloadable))
+    rsQuaternionSlerp(rs_quaternion* q, const rs_quaternion* q0, const rs_quaternion* q1, float t) {
+    if (t <= 0.0f) {
+        rsQuaternionSet(q, q0);
+        return;
+    }
+    if (t >= 1.0f) {
+        rsQuaternionSet(q, q1);
+        return;
+    }
+
+    rs_quaternion tempq0, tempq1;
+    rsQuaternionSet(&tempq0, q0);
+    rsQuaternionSet(&tempq1, q1);
+
+    float angle = rsQuaternionDot(q0, q1);
+    if (angle < 0) {
+        rsQuaternionMultiply(&tempq0, -1.0f);
+        angle *= -1.0f;
+    }
+
+    float scale, invScale;
+    if (angle + 1.0f > 0.05f) {
+        if (1.0f - angle >= 0.05f) {
+            float theta = acos(angle);
+            float invSinTheta = 1.0f / sin(theta);
+            scale = sin(theta * (1.0f - t)) * invSinTheta;
+            invScale = sin(theta * t) * invSinTheta;
+        } else {
+            scale = 1.0f - t;
+            invScale = t;
+        }
+    } else {
+        rsQuaternionSet(&tempq1, tempq0.z, -tempq0.y, tempq0.x, -tempq0.w);
+        scale = sin(M_PI * (0.5f - t));
+        invScale = sin(M_PI * t);
+    }
+
+    rsQuaternionSet(q, tempq0.w*scale + tempq1.w*invScale, tempq0.x*scale + tempq1.x*invScale,
+                        tempq0.y*scale + tempq1.y*invScale, tempq0.z*scale + tempq1.z*invScale);
+}
+#endif // (defined(RS_VERSION) && (RS_VERSION >= UNRELEASED))