Donate to e Foundation | Murena handsets with /e/OS | Own a part of Murena! Learn more

Commit f13ada94 authored by Alex Sakhartchouk's avatar Alex Sakhartchouk
Browse files

Fixes and optimizations of two quaternion functions. 

Change-Id: Ie5a3f16d49b716d91b6f68fa389473548cecb80d
parent a744ead9

libs/rs/scriptc/rs_quaternion.rsh

+37 −41
Original line number Diff line number Diff line
@@ -65,19 +65,6 @@ rsQuaternionMultiply(rs_quaternion *q, float s) { 
    q->z *= 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
@@ -167,6 +154,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
@@ -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;

}