Merge "DO NOT MERGE Remove the rs_quaternion functions until mr1" into honeycomb

extern void __attribute__((overloadable)) rsMatrixTranspose(rs_matrix3x3 *m);

extern void __attribute__((overloadable)) rsMatrixTranspose(rs_matrix2x2 *m);

/////////////////////////////////////////////////////

// quaternion ops

/////////////////////////////////////////////////////

_RS_STATIC 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;

}

_RS_STATIC 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;

}

_RS_STATIC void __attribute__((overloadable))

rsQuaternionMultiply(rs_quaternion *q, float s) {

    q->w *= s;

    q->x *= s;

    q->y *= s;

    q->z *= s;

}

_RS_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;

}

_RS_STATIC void

rsQuaternionAdd(rs_quaternion *q, const rs_quaternion *rhs) {

    q->w *= rhs->w;

    q->x *= rhs->x;

    q->y *= rhs->y;

    q->z *= rhs->z;

}

_RS_STATIC void

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;

}

_RS_STATIC void

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);

}

_RS_STATIC void

rsQuaternionConjugate(rs_quaternion *q) {

    q->x = -q->x;

    q->y = -q->y;

    q->z = -q->z;

}

_RS_STATIC float

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;

}

_RS_STATIC void

rsQuaternionNormalize(rs_quaternion *q) {

    const float len = rsQuaternionDot(q, q);

    if (len != 1) {

        const float recipLen = 1.f / sqrt(len);

        rsQuaternionMultiply(q, recipLen);

    }

}

_RS_STATIC void

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);

}

_RS_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;

    m->m[15] = 1.0f;

}

/////////////////////////////////////////////////////

// utility funcs

/////////////////////////////////////////////////////

__inline__ _RS_STATIC void __attribute__((overloadable, always_inline))

rsExtractFrustumPlanes(const rs_matrix4x4 *modelViewProj,

                         float4 *left, float4 *right,

                         float4 *top, float4 *bottom,

                         float4 *near, float4 *far) {

    // x y z w = a b c d in the plane equation

    left->x = modelViewProj->m[3] + modelViewProj->m[0];

    left->y = modelViewProj->m[7] + modelViewProj->m[4];

    left->z = modelViewProj->m[11] + modelViewProj->m[8];

    left->w = modelViewProj->m[15] + modelViewProj->m[12];

    right->x = modelViewProj->m[3] - modelViewProj->m[0];

    right->y = modelViewProj->m[7] - modelViewProj->m[4];

    right->z = modelViewProj->m[11] - modelViewProj->m[8];

    right->w = modelViewProj->m[15] - modelViewProj->m[12];

    top->x = modelViewProj->m[3] - modelViewProj->m[1];

    top->y = modelViewProj->m[7] - modelViewProj->m[5];

    top->z = modelViewProj->m[11] - modelViewProj->m[9];

    top->w = modelViewProj->m[15] - modelViewProj->m[13];

    bottom->x = modelViewProj->m[3] + modelViewProj->m[1];

    bottom->y = modelViewProj->m[7] + modelViewProj->m[5];

    bottom->z = modelViewProj->m[11] + modelViewProj->m[9];

    bottom->w = modelViewProj->m[15] + modelViewProj->m[13];

    near->x = modelViewProj->m[3] + modelViewProj->m[2];

    near->y = modelViewProj->m[7] + modelViewProj->m[6];

    near->z = modelViewProj->m[11] + modelViewProj->m[10];

    near->w = modelViewProj->m[15] + modelViewProj->m[14];

    far->x = modelViewProj->m[3] - modelViewProj->m[2];

    far->y = modelViewProj->m[7] - modelViewProj->m[6];

    far->z = modelViewProj->m[11] - modelViewProj->m[10];

    far->w = modelViewProj->m[15] - modelViewProj->m[14];

    float len = length(left->xyz);

    *left /= len;

    len = length(right->xyz);

    *right /= len;

    len = length(top->xyz);

    *top /= len;

    len = length(bottom->xyz);

    *bottom /= len;

    len = length(near->xyz);

    *near /= len;

    len = length(far->xyz);

    *far /= len;

}

__inline__ _RS_STATIC bool __attribute__((overloadable, always_inline))

rsIsSphereInFrustum(float4 *sphere,

                      float4 *left, float4 *right,

                      float4 *top, float4 *bottom,

                      float4 *near, float4 *far) {

    float distToCenter = dot(left->xyz, sphere->xyz) + left->w;

    if (distToCenter < -sphere->w) {

        return false;

    }

    distToCenter = dot(right->xyz, sphere->xyz) + right->w;

    if (distToCenter < -sphere->w) {

        return false;

    }

    distToCenter = dot(top->xyz, sphere->xyz) + top->w;

    if (distToCenter < -sphere->w) {

        return false;

    }

    distToCenter = dot(bottom->xyz, sphere->xyz) + bottom->w;

    if (distToCenter < -sphere->w) {

        return false;

    }

    distToCenter = dot(near->xyz, sphere->xyz) + near->w;

    if (distToCenter < -sphere->w) {

        return false;

    }

    distToCenter = dot(far->xyz, sphere->xyz) + far->w;

    if (distToCenter < -sphere->w) {

        return false;

    }

    return true;

}

/////////////////////////////////////////////////////

// int ops

/////////////////////////////////////////////////////