/* SCE CONFIDENTIAL
PLAYSTATION(R)3 Programmer Tool Runtime Library 154.001
* Copyright (C) 2006 Sony Computer Entertainment Inc.
* All Rights Reserved.
*/

#ifndef _VECTORMATH_QUAT_SOA_C_H
#define _VECTORMATH_QUAT_SOA_C_H

#ifdef __cplusplus
extern "C" {
#endif /* __cplusplus */

/*-----------------------------------------------------------------------------
 * Definitions
 */
#ifndef _VECTORMATH_INTERNAL_FUNCTIONS
#define _VECTORMATH_INTERNAL_FUNCTIONS

#endif

static inline void vmathSoaQCopy( VmathSoaQuat *result, const VmathSoaQuat *quat )
{
    result->x = quat->x;
    result->y = quat->y;
    result->z = quat->z;
    result->w = quat->w;
}

static inline void vmathSoaQMakeFromElems( VmathSoaQuat *result, vec_float4 _x, vec_float4 _y, vec_float4 _z, vec_float4 _w )
{
    result->x = _x;
    result->y = _y;
    result->z = _z;
    result->w = _w;
}

static inline void vmathSoaQMakeFromV3Scalar( VmathSoaQuat *result, const VmathSoaVector3 *xyz, vec_float4 _w )
{
    vmathSoaQSetXYZ( result, xyz );
    vmathSoaQSetW( result, _w );
}

static inline void vmathSoaQMakeFromV4( VmathSoaQuat *result, const VmathSoaVector4 *vec )
{
    result->x = vec->x;
    result->y = vec->y;
    result->z = vec->z;
    result->w = vec->w;
}

static inline void vmathSoaQMakeFromScalar( VmathSoaQuat *result, vec_float4 scalar )
{
    result->x = scalar;
    result->y = scalar;
    result->z = scalar;
    result->w = scalar;
}

static inline void vmathSoaQMakeFromAos( VmathSoaQuat *result, const VmathQuat *quat )
{
    vec_uchar16 shuffle_xxxx = (vec_uchar16)spu_splats((int)0x00010203);
    vec_uchar16 shuffle_yyyy = (vec_uchar16)spu_splats((int)0x04050607);
    vec_uchar16 shuffle_zzzz = (vec_uchar16)spu_splats((int)0x08090a0b);
    vec_uchar16 shuffle_wwww = (vec_uchar16)spu_splats((int)0x0c0d0e0f);
    vec_float4 vec128 = quat->vec128;
    result->x = spu_shuffle( vec128, vec128, shuffle_xxxx );
    result->y = spu_shuffle( vec128, vec128, shuffle_yyyy );
    result->z = spu_shuffle( vec128, vec128, shuffle_zzzz );
    result->w = spu_shuffle( vec128, vec128, shuffle_wwww );
}

static inline void vmathSoaQMakeFrom4Aos( VmathSoaQuat *result, const VmathQuat *quat0, const VmathQuat *quat1, const VmathQuat *quat2, const VmathQuat *quat3 )
{
    vec_float4 tmp0, tmp1, tmp2, tmp3;
    tmp0 = spu_shuffle( quat0->vec128, quat2->vec128, _VECTORMATH_SHUF_XAYB );
    tmp1 = spu_shuffle( quat1->vec128, quat3->vec128, _VECTORMATH_SHUF_XAYB );
    tmp2 = spu_shuffle( quat0->vec128, quat2->vec128, _VECTORMATH_SHUF_ZCWD );
    tmp3 = spu_shuffle( quat1->vec128, quat3->vec128, _VECTORMATH_SHUF_ZCWD );
    result->x = spu_shuffle( tmp0, tmp1, _VECTORMATH_SHUF_XAYB );
    result->y = spu_shuffle( tmp0, tmp1, _VECTORMATH_SHUF_ZCWD );
    result->z = spu_shuffle( tmp2, tmp3, _VECTORMATH_SHUF_XAYB );
    result->w = spu_shuffle( tmp2, tmp3, _VECTORMATH_SHUF_ZCWD );
}

static inline void vmathSoaQMakeIdentity( VmathSoaQuat *result )
{
    vmathSoaQMakeFromElems( result, spu_splats(0.0f), spu_splats(0.0f), spu_splats(0.0f), spu_splats(1.0f) );
}

static inline void vmathSoaQLerp( VmathSoaQuat *result, vec_float4 t, const VmathSoaQuat *quat0, const VmathSoaQuat *quat1 )
{
    VmathSoaQuat tmpQ_0, tmpQ_1;
    vmathSoaQSub( &tmpQ_0, quat1, quat0 );
    vmathSoaQScalarMul( &tmpQ_1, &tmpQ_0, t );
    vmathSoaQAdd( result, quat0, &tmpQ_1 );
}

static inline void vmathSoaQSlerp( VmathSoaQuat *result, vec_float4 t, const VmathSoaQuat *unitQuat0, const VmathSoaQuat *unitQuat1 )
{
    VmathSoaQuat start, tmpQ_0, tmpQ_1;
    vec_float4 recipSinAngle, scale0, scale1, cosAngle, angle;
    vec_uint4 selectMask;
    cosAngle = vmathSoaQDot( unitQuat0, unitQuat1 );
    selectMask = (vec_uint4)spu_cmpgt( spu_splats(0.0f), cosAngle );
    cosAngle = spu_sel( cosAngle, negatef4( cosAngle ), selectMask );
    vmathSoaQSetX( &start, spu_sel( unitQuat0->x, negatef4( unitQuat0->x ), selectMask ) );
    vmathSoaQSetY( &start, spu_sel( unitQuat0->y, negatef4( unitQuat0->y ), selectMask ) );
    vmathSoaQSetZ( &start, spu_sel( unitQuat0->z, negatef4( unitQuat0->z ), selectMask ) );
    vmathSoaQSetW( &start, spu_sel( unitQuat0->w, negatef4( unitQuat0->w ), selectMask ) );
    selectMask = (vec_uint4)spu_cmpgt( spu_splats(_VECTORMATH_SLERP_TOL), cosAngle );
    angle = acosf4( cosAngle );
    recipSinAngle = recipf4( sinf4( angle ) );
    scale0 = spu_sel( spu_sub( spu_splats(1.0f), t ), spu_mul( sinf4( spu_mul( spu_sub( spu_splats(1.0f), t ), angle ) ), recipSinAngle ), selectMask );
    scale1 = spu_sel( t, spu_mul( sinf4( spu_mul( t, angle ) ), recipSinAngle ), selectMask );
    vmathSoaQScalarMul( &tmpQ_0, &start, scale0 );
    vmathSoaQScalarMul( &tmpQ_1, unitQuat1, scale1 );
    vmathSoaQAdd( result, &tmpQ_0, &tmpQ_1 );
}

static inline void vmathSoaQSquad( VmathSoaQuat *result, vec_float4 t, const VmathSoaQuat *unitQuat0, const VmathSoaQuat *unitQuat1, const VmathSoaQuat *unitQuat2, const VmathSoaQuat *unitQuat3 )
{
    VmathSoaQuat tmp0, tmp1;
    vmathSoaQSlerp( &tmp0, t, unitQuat0, unitQuat3 );
    vmathSoaQSlerp( &tmp1, t, unitQuat1, unitQuat2 );
    vmathSoaQSlerp( result, spu_mul( spu_mul( spu_splats(2.0f), t ), spu_sub( spu_splats(1.0f), t ) ), &tmp0, &tmp1 );
}

static inline void vmathSoaQGet4Aos( const VmathSoaQuat *quat, VmathQuat *result0, VmathQuat *result1, VmathQuat *result2, VmathQuat *result3 )
{
    vec_float4 tmp0, tmp1, tmp2, tmp3;
    tmp0 = spu_shuffle( quat->x, quat->z, _VECTORMATH_SHUF_XAYB );
    tmp1 = spu_shuffle( quat->y, quat->w, _VECTORMATH_SHUF_XAYB );
    tmp2 = spu_shuffle( quat->x, quat->z, _VECTORMATH_SHUF_ZCWD );
    tmp3 = spu_shuffle( quat->y, quat->w, _VECTORMATH_SHUF_ZCWD );
    vmathQMakeFrom128( result0, spu_shuffle( tmp0, tmp1, _VECTORMATH_SHUF_XAYB ) );
    vmathQMakeFrom128( result1, spu_shuffle( tmp0, tmp1, _VECTORMATH_SHUF_ZCWD ) );
    vmathQMakeFrom128( result2, spu_shuffle( tmp2, tmp3, _VECTORMATH_SHUF_XAYB ) );
    vmathQMakeFrom128( result3, spu_shuffle( tmp2, tmp3, _VECTORMATH_SHUF_ZCWD ) );
}

static inline void vmathSoaQSetXYZ( VmathSoaQuat *result, const VmathSoaVector3 *vec )
{
    result->x = vec->x;
    result->y = vec->y;
    result->z = vec->z;
}

static inline void vmathSoaQGetXYZ( VmathSoaVector3 *result, const VmathSoaQuat *quat )
{
    vmathSoaV3MakeFromElems( result, quat->x, quat->y, quat->z );
}

static inline void vmathSoaQSetX( VmathSoaQuat *result, vec_float4 _x )
{
    result->x = _x;
}

static inline vec_float4 vmathSoaQGetX( const VmathSoaQuat *quat )
{
    return quat->x;
}

static inline void vmathSoaQSetY( VmathSoaQuat *result, vec_float4 _y )
{
    result->y = _y;
}

static inline vec_float4 vmathSoaQGetY( const VmathSoaQuat *quat )
{
    return quat->y;
}

static inline void vmathSoaQSetZ( VmathSoaQuat *result, vec_float4 _z )
{
    result->z = _z;
}

static inline vec_float4 vmathSoaQGetZ( const VmathSoaQuat *quat )
{
    return quat->z;
}

static inline void vmathSoaQSetW( VmathSoaQuat *result, vec_float4 _w )
{
    result->w = _w;
}

static inline vec_float4 vmathSoaQGetW( const VmathSoaQuat *quat )
{
    return quat->w;
}

static inline void vmathSoaQSetElem( VmathSoaQuat *result, int idx, vec_float4 value )
{
    *(&result->x + idx) = value;
}

static inline vec_float4 vmathSoaQGetElem( const VmathSoaQuat *quat, int idx )
{
    return *(&quat->x + idx);
}

static inline void vmathSoaQAdd( VmathSoaQuat *result, const VmathSoaQuat *quat0, const VmathSoaQuat *quat1 )
{
    result->x = spu_add( quat0->x, quat1->x );
    result->y = spu_add( quat0->y, quat1->y );
    result->z = spu_add( quat0->z, quat1->z );
    result->w = spu_add( quat0->w, quat1->w );
}

static inline void vmathSoaQSub( VmathSoaQuat *result, const VmathSoaQuat *quat0, const VmathSoaQuat *quat1 )
{
    result->x = spu_sub( quat0->x, quat1->x );
    result->y = spu_sub( quat0->y, quat1->y );
    result->z = spu_sub( quat0->z, quat1->z );
    result->w = spu_sub( quat0->w, quat1->w );
}

static inline void vmathSoaQScalarMul( VmathSoaQuat *result, const VmathSoaQuat *quat, vec_float4 scalar )
{
    result->x = spu_mul( quat->x, scalar );
    result->y = spu_mul( quat->y, scalar );
    result->z = spu_mul( quat->z, scalar );
    result->w = spu_mul( quat->w, scalar );
}

static inline void vmathSoaQScalarDiv( VmathSoaQuat *result, const VmathSoaQuat *quat, vec_float4 scalar )
{
    result->x = divf4( quat->x, scalar );
    result->y = divf4( quat->y, scalar );
    result->z = divf4( quat->z, scalar );
    result->w = divf4( quat->w, scalar );
}

static inline void vmathSoaQNeg( VmathSoaQuat *result, const VmathSoaQuat *quat )
{
    result->x = negatef4( quat->x );
    result->y = negatef4( quat->y );
    result->z = negatef4( quat->z );
    result->w = negatef4( quat->w );
}

static inline vec_float4 vmathSoaQDot( const VmathSoaQuat *quat0, const VmathSoaQuat *quat1 )
{
    vec_float4 result;
    result = spu_mul( quat0->x, quat1->x );
    result = spu_add( result, spu_mul( quat0->y, quat1->y ) );
    result = spu_add( result, spu_mul( quat0->z, quat1->z ) );
    result = spu_add( result, spu_mul( quat0->w, quat1->w ) );
    return result;
}

static inline vec_float4 vmathSoaQNorm( const VmathSoaQuat *quat )
{
    vec_float4 result;
    result = spu_mul( quat->x, quat->x );
    result = spu_add( result, spu_mul( quat->y, quat->y ) );
    result = spu_add( result, spu_mul( quat->z, quat->z ) );
    result = spu_add( result, spu_mul( quat->w, quat->w ) );
    return result;
}

static inline vec_float4 vmathSoaQLength( const VmathSoaQuat *quat )
{
    return sqrtf4( vmathSoaQNorm( quat ) );
}

static inline void vmathSoaQNormalize( VmathSoaQuat *result, const VmathSoaQuat *quat )
{
    vec_float4 lenSqr, lenInv;
    lenSqr = vmathSoaQNorm( quat );
    lenInv = rsqrtf4( lenSqr );
    result->x = spu_mul( quat->x, lenInv );
    result->y = spu_mul( quat->y, lenInv );
    result->z = spu_mul( quat->z, lenInv );
    result->w = spu_mul( quat->w, lenInv );
}

static inline void vmathSoaQMakeRotationArc( VmathSoaQuat *result, const VmathSoaVector3 *unitVec0, const VmathSoaVector3 *unitVec1 )
{
    VmathSoaVector3 tmpV3_0, tmpV3_1;
    vec_float4 cosHalfAngleX2, recipCosHalfAngleX2;
    cosHalfAngleX2 = sqrtf4( spu_mul( spu_splats(2.0f), spu_add( spu_splats(1.0f), vmathSoaV3Dot( unitVec0, unitVec1 ) ) ) );
    recipCosHalfAngleX2 = recipf4( cosHalfAngleX2 );
    vmathSoaV3Cross( &tmpV3_0, unitVec0, unitVec1 );
    vmathSoaV3ScalarMul( &tmpV3_1, &tmpV3_0, recipCosHalfAngleX2 );
    vmathSoaQMakeFromV3Scalar( result, &tmpV3_1, spu_mul( cosHalfAngleX2, spu_splats(0.5f) ) );
}

static inline void vmathSoaQMakeRotationAxis( VmathSoaQuat *result, vec_float4 radians, const VmathSoaVector3 *unitVec )
{
    VmathSoaVector3 tmpV3_0;
    vec_float4 s, c, angle;
    angle = spu_mul( radians, spu_splats(0.5f) );
    sincosf4( angle, &s, &c );
    vmathSoaV3ScalarMul( &tmpV3_0, unitVec, s );
    vmathSoaQMakeFromV3Scalar( result, &tmpV3_0, c );
}

static inline void vmathSoaQMakeRotationX( VmathSoaQuat *result, vec_float4 radians )
{
    vec_float4 s, c, angle;
    angle = spu_mul( radians, spu_splats(0.5f) );
    sincosf4( angle, &s, &c );
    vmathSoaQMakeFromElems( result, s, spu_splats(0.0f), spu_splats(0.0f), c );
}

static inline void vmathSoaQMakeRotationY( VmathSoaQuat *result, vec_float4 radians )
{
    vec_float4 s, c, angle;
    angle = spu_mul( radians, spu_splats(0.5f) );
    sincosf4( angle, &s, &c );
    vmathSoaQMakeFromElems( result, spu_splats(0.0f), s, spu_splats(0.0f), c );
}

static inline void vmathSoaQMakeRotationZ( VmathSoaQuat *result, vec_float4 radians )
{
    vec_float4 s, c, angle;
    angle = spu_mul( radians, spu_splats(0.5f) );
    sincosf4( angle, &s, &c );
    vmathSoaQMakeFromElems( result, spu_splats(0.0f), spu_splats(0.0f), s, c );
}

static inline void vmathSoaQMul( VmathSoaQuat *result, const VmathSoaQuat *quat0, const VmathSoaQuat *quat1 )
{
    vec_float4 tmpX, tmpY, tmpZ, tmpW;
    tmpX = spu_sub( spu_add( spu_add( spu_mul( quat0->w, quat1->x ), spu_mul( quat0->x, quat1->w ) ), spu_mul( quat0->y, quat1->z ) ), spu_mul( quat0->z, quat1->y ) );
    tmpY = spu_sub( spu_add( spu_add( spu_mul( quat0->w, quat1->y ), spu_mul( quat0->y, quat1->w ) ), spu_mul( quat0->z, quat1->x ) ), spu_mul( quat0->x, quat1->z ) );
    tmpZ = spu_sub( spu_add( spu_add( spu_mul( quat0->w, quat1->z ), spu_mul( quat0->z, quat1->w ) ), spu_mul( quat0->x, quat1->y ) ), spu_mul( quat0->y, quat1->x ) );
    tmpW = spu_sub( spu_sub( spu_sub( spu_mul( quat0->w, quat1->w ), spu_mul( quat0->x, quat1->x ) ), spu_mul( quat0->y, quat1->y ) ), spu_mul( quat0->z, quat1->z ) );
    vmathSoaQMakeFromElems( result, tmpX, tmpY, tmpZ, tmpW );
}

static inline void vmathSoaQRotate( VmathSoaVector3 *result, const VmathSoaQuat *quat, const VmathSoaVector3 *vec )
{
    vec_float4 tmpX, tmpY, tmpZ, tmpW;
    tmpX = spu_sub( spu_add( spu_mul( quat->w, vec->x ), spu_mul( quat->y, vec->z ) ), spu_mul( quat->z, vec->y ) );
    tmpY = spu_sub( spu_add( spu_mul( quat->w, vec->y ), spu_mul( quat->z, vec->x ) ), spu_mul( quat->x, vec->z ) );
    tmpZ = spu_sub( spu_add( spu_mul( quat->w, vec->z ), spu_mul( quat->x, vec->y ) ), spu_mul( quat->y, vec->x ) );
    tmpW = spu_add( spu_add( spu_mul( quat->x, vec->x ), spu_mul( quat->y, vec->y ) ), spu_mul( quat->z, vec->z ) );
    result->x = spu_add( spu_sub( spu_add( spu_mul( tmpW, quat->x ), spu_mul( tmpX, quat->w ) ), spu_mul( tmpY, quat->z ) ), spu_mul( tmpZ, quat->y ) );
    result->y = spu_add( spu_sub( spu_add( spu_mul( tmpW, quat->y ), spu_mul( tmpY, quat->w ) ), spu_mul( tmpZ, quat->x ) ), spu_mul( tmpX, quat->z ) );
    result->z = spu_add( spu_sub( spu_add( spu_mul( tmpW, quat->z ), spu_mul( tmpZ, quat->w ) ), spu_mul( tmpX, quat->y ) ), spu_mul( tmpY, quat->x ) );
}

static inline void vmathSoaQConj( VmathSoaQuat *result, const VmathSoaQuat *quat )
{
    vmathSoaQMakeFromElems( result, negatef4( quat->x ), negatef4( quat->y ), negatef4( quat->z ), quat->w );
}

static inline void vmathSoaQSelect( VmathSoaQuat *result, const VmathSoaQuat *quat0, const VmathSoaQuat *quat1, vec_uint4 select1 )
{
    result->x = spu_sel( quat0->x, quat1->x, select1 );
    result->y = spu_sel( quat0->y, quat1->y, select1 );
    result->z = spu_sel( quat0->z, quat1->z, select1 );
    result->w = spu_sel( quat0->w, quat1->w, select1 );
}

#ifdef _VECTORMATH_DEBUG

static inline void vmathSoaQPrint( const VmathSoaQuat *quat )
{
    VmathQuat vec0, vec1, vec2, vec3;
    vmathSoaQGet4Aos( quat, &vec0, &vec1, &vec2, &vec3 );
    spu_printf("slot 0:\n");
    vmathQPrint( &vec0 );
    spu_printf("slot 1:\n");
    vmathQPrint( &vec1 );
    spu_printf("slot 2:\n");
    vmathQPrint( &vec2 );
    spu_printf("slot 3:\n");
    vmathQPrint( &vec3 );
}

static inline void vmathSoaQPrints( const VmathSoaQuat *quat, const char *name )
{
    VmathQuat vec0, vec1, vec2, vec3;
    spu_printf( "%s:\n", name );
    vmathSoaQGet4Aos( quat, &vec0, &vec1, &vec2, &vec3 );
    spu_printf("slot 0:\n");
    vmathQPrint( &vec0 );
    spu_printf("slot 1:\n");
    vmathQPrint( &vec1 );
    spu_printf("slot 2:\n");
    vmathQPrint( &vec2 );
    spu_printf("slot 3:\n");
    vmathQPrint( &vec3 );
}

#endif

#ifdef __cplusplus
}
#endif /* __cplusplus */

#endif
