ISMatrix.c

Go to the documentation of this file.

/*
MIT LICENSE

Copyright (c) 2014-2021 Inertial Sense, Inc. - http://inertialsense.com

Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files(the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions :

The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/

#include <string.h>
#include <stdlib.h>
#include "ISMatrix.h"
#include "data_sets.h"

void LU( const f_t *M, i_t n, f_t *L, f_t *U );
char solve_upper( f_t *result, i_t n, f_t *A, f_t *b );
char solve_lower( f_t *result, i_t n, f_t *A, f_t *b );

void mul_MatMxN( void * result, const void * A_ptr, const void * B_ptr, i_t m, i_t n, i_t p, char transpose_B, char add )
{
        i_t i;
        i_t j;
        i_t k;

        f_t * matOut = (f_t*)result;
        const f_t * A = (const f_t*)A_ptr;
        const f_t * B = (const f_t*)B_ptr;

        for (i = 0; i < m; i++)
        {
                const f_t * A_i = A + i * n;
                f_t * O_i = matOut + i * p;

                for (j = 0; j < p; j++)
                {
                        f_t s = 0;
                        f_t * O_i_j = O_i + j;

                        for (k = 0; k < n; k++)
                        {
                                const f_t * a = A_i + k;
                                const f_t * b;

                                if (is_zero(a))
                                        continue;

                                if (transpose_B)
                                        b = B + j * n + k;
                                else
                                        b = B + k * p + j;

                                if (is_zero(b))
                                        continue;

                                s += *a * *b;
                        }

                        if (add == 0)
                                *O_i_j = s;
                        else if (add > 0)
                                *O_i_j += s;
                        else
                                *O_i_j -= s;
                }
        }
}

#if 0
void addNxM(void * result, const void * A_ptr, const void * B_ptr, i_t m, i_t n, char transpose_B)
{
    i_t i;
    i_t j;
    i_t k;

    f_t * OUT = result;
    const f_t * A = A_ptr;
    const f_t * B = B_ptr;

    for (i = 0; i < m; i++)
    {
        const f_t * A_i = A + i * n;
        f_t * O_i = OUT + i * p;

        for (j = 0; j < p; j++)
        {
//             f_t s = 0;
            f_t * O_i_j = O_i + j;


            for (k = 0; k < n; k++)
            {
                const f_t * a = A_i + k;
                const f_t * b;

//                 if (is_zero(a))
//                     continue;

                if (transpose_B)
                    b = B + j * n + k;
                else
                    b = B + k * p + j;

                if (is_zero(b))
                    continue;

                s += *a * *b;
            }

            *O_i_j = s;
        }
    }
}
#endif


void eye_MatN( f_t *A, i_t n )
{
        zero_MatMxN( A, n, n );

        // Set diagonals to 1
        for( int i=0; i < n; i++ )
                A[i*n + i] = 1;
}


// Compute the LU factorization of the square matrix A
void LU( const f_t *M, i_t n, f_t *L, f_t *U )
{
        int in, kn;

        f_t *A = (f_t*)MALLOC( sizeof( f_t )*n*n );
        if (A == 0) { return; }

        cpy_MatMxN( A, M, n, n );

        for( int k=0; k < n - 1; k++ )
        {
                for( int i=k + 1; i < n; i++ )
                {
                        in = i*n;
                        kn = k*n;
//                      f_t * Ai = A + i*n;

                        A[in + k] = A[in + k] / A[kn + k];

                        for( int j=k + 1; j < n; j++ )
                        {
#ifdef NO_FPU
                                T &             A_i_j( A[in + j] );
                                const T &       A_i_k( A[in + k] );
                                const T &       A_k_j( A[kn + j] );

                                if( is_zero( A_i_k )
                                        || is_zero( A_k_j )
                                        )
                                        continue;

                                A_i_j -= A_i_k * A_k_j;
#else
                                A[in + j] -= A[in + k] * A[kn + j];
#endif
                        }
                }
        }

        eye_MatN( L, n );

        /* Separate the L matrix */
        for( int j=0; j < n - 1; j++ )
                for( int i=j + 1; i < n; i++ )
                        L[i*n + j] = A[i*n + j];

        /* Separate the M matrix */
        zero_MatMxN( U, n, n );

        for( int i=0; i < n; i++ )
                for( int j=i; j < n; j++ )
                        U[i*n + j] = A[i*n + j];
                        
        FREE( A );
}

// Return 0 on success, -1 on numerical error
char solve_upper( f_t *result, i_t n, f_t *A, f_t *b )
{
        for( int i=n - 1; i >= 0; i-- )
        {
                f_t s = b[i];

                // Reference a row
                f_t *A_i = &A[i*n];

                for( int j=i + 1; j < n; ++j )
                {
#ifdef NO_FPU
                        const T &       A_i_j( A_i[j] );
                        const T &       x_j( x[j] );

                        if( is_zero( A_i_j ) || is_zero( x_j ) )
                                continue;

                        s -= A_i_j * x_j;
#else
                        s -= A_i[j] * result[j*n];
#endif
                }

                // Prevent divide by zero
                if( A_i[i]==0.0f )
                        return -1;
                
                result[i*n] = s / A_i[i];
        }
        
        return 0;
}

// Return 0 on success, -1 on numerical error
char solve_lower( f_t *result, i_t n, f_t *A, f_t *b )
{
        for( int i=0; i < n; ++i )
        {
                f_t s = b[i];

                // Reference a row
                f_t *A_i = &A[i*n];

                for( int j=0; j < i; ++j )
                {
#ifdef NO_FPU
                        const T &       A_i_j( A_i[j] );
                        const T &       x_j( x[j] );

                        if( is_zero( A_i_j )|| is_zero( x_j ) )
                                continue;

                        s -= A_i_j * x_j;
#else
                        s -= A_i[j] * result[j*n];
#endif
                }

                // Prevent divide by zero
                if( A_i[i]==0.0f )
                        return -1;

                result[i*n] = s / A_i[i];
        }
        
        return 0;
}


// Return 0 on success, -1 on numerical error
char inv_MatN( f_t *result, const f_t *M, i_t n )
{
        char error              = 0;

        f_t *L                  = (f_t*)MALLOC( sizeof( f_t )*n*n );
        f_t *U                  = (f_t*)MALLOC( sizeof( f_t )*n*n );
        f_t *invL               = (f_t*)MALLOC( sizeof( f_t )*n*n );
        f_t *invU               = (f_t*)MALLOC( sizeof( f_t )*n*n );
        f_t     *identCol       = (f_t*)MALLOC( sizeof( f_t )*n );
        while( L==NULL || U==NULL || invL==NULL || invU==NULL || identCol==NULL ) { /* Error check malloc */ }
        memset( identCol, 0, sizeof( f_t )*n );

        LU( M, n, L, U );

        for( int i=0; i < n; i++ )
        {
                identCol[i] = 1;
                if( solve_upper( &invU[i], n, U, identCol ) ||          // Fill a column
                        solve_lower( &invL[i], n, L, identCol ) )
                {               
                        error = -1;             
                        break;
                }
                identCol[i] = 0;
        }

        // result = invU * invL
        if( !error )
                mul_MatMxN( result, invU, invL, n, n, n, 0, 0 );

        FREE( L );
        FREE( U );
        FREE( invL );
        FREE( invU );
        FREE( identCol );

        return error;
}


void trans_MatMxN( f_t *result, const f_t *M, int m, int n )
{
        i_t i;
        i_t j;

        const f_t * A = (const f_t*)M;

        for( i = 0; i < m; i++ )
        {
                f_t * O_i = result + i;

                for( j = 0; j < n; j++ )
                {
                        // Copy value
                        *O_i = *A;

                        // Increment pointers
                        A++;
                        O_i += m;
                }
        }
}


void mul_Mat3x3_Mat3x3( ixMatrix3 result, const ixMatrix3 m1, const ixMatrix3 m2 )
{
        // Row 1
        result[0] = m1[0]*m2[0] + m1[1]*m2[3] + m1[2]*m2[6];
        result[1] = m1[0]*m2[1] + m1[1]*m2[4] + m1[2]*m2[7];
        result[2] = m1[0]*m2[2] + m1[1]*m2[5] + m1[2]*m2[8];
        // Row 2
        result[3] = m1[3]*m2[0] + m1[4]*m2[3] + m1[5]*m2[6];
        result[4] = m1[3]*m2[1] + m1[4]*m2[4] + m1[5]*m2[7];
        result[5] = m1[3]*m2[2] + m1[4]*m2[5] + m1[5]*m2[8];
        // Row 3
        result[6] = m1[6]*m2[0] + m1[7]*m2[3] + m1[8]*m2[6];
        result[7] = m1[6]*m2[1] + m1[7]*m2[4] + m1[8]*m2[7];
        result[8] = m1[6]*m2[2] + m1[7]*m2[5] + m1[8]*m2[8];
}

void mul_Mat3x3_Mat3x3_d(ixMatrix3d result, const ixMatrix3d m1, const ixMatrix3d m2)
{
    // Row 1
    result[0] = m1[0] * m2[0] + m1[1] * m2[3] + m1[2] * m2[6];
    result[1] = m1[0] * m2[1] + m1[1] * m2[4] + m1[2] * m2[7];
    result[2] = m1[0] * m2[2] + m1[1] * m2[5] + m1[2] * m2[8];
    // Row 2
    result[3] = m1[3] * m2[0] + m1[4] * m2[3] + m1[5] * m2[6];
    result[4] = m1[3] * m2[1] + m1[4] * m2[4] + m1[5] * m2[7];
    result[5] = m1[3] * m2[2] + m1[4] * m2[5] + m1[5] * m2[8];
    // Row 3
    result[6] = m1[6] * m2[0] + m1[7] * m2[3] + m1[8] * m2[6];
    result[7] = m1[6] * m2[1] + m1[7] * m2[4] + m1[8] * m2[7];
    result[8] = m1[6] * m2[2] + m1[7] * m2[5] + m1[8] * m2[8];
}

void mul_Mat3x3_Trans_Mat3x3( ixMatrix3 result, const ixMatrix3 m1, const ixMatrix3 m2 )
{
    // Row 1
    result[0] = m1[0]*m2[0] + m1[3]*m2[3] + m1[6]*m2[6];
    result[1] = m1[0]*m2[1] + m1[3]*m2[4] + m1[6]*m2[7];
    result[2] = m1[0]*m2[2] + m1[3]*m2[5] + m1[6]*m2[8];
    // Row 2
    result[3] = m1[1]*m2[0] + m1[4]*m2[3] + m1[7]*m2[6];
    result[4] = m1[1]*m2[1] + m1[4]*m2[4] + m1[7]*m2[7];
    result[5] = m1[1]*m2[2] + m1[4]*m2[5] + m1[7]*m2[8];
    // Row 3
    result[6] = m1[2]*m2[0] + m1[5]*m2[3] + m1[8]*m2[6];
    result[7] = m1[2]*m2[1] + m1[5]*m2[4] + m1[8]*m2[7];
    result[8] = m1[2]*m2[2] + m1[5]*m2[5] + m1[8]*m2[8];
}

void mul_Mat3x3_Trans_Mat3x3_d(ixMatrix3d result, const ixMatrix3d m1, const ixMatrix3d m2)
{
    // Row 1
    result[0] = m1[0] * m2[0] + m1[3] * m2[3] + m1[6] * m2[6];
    result[1] = m1[0] * m2[1] + m1[3] * m2[4] + m1[6] * m2[7];
    result[2] = m1[0] * m2[2] + m1[3] * m2[5] + m1[6] * m2[8];
    // Row 2
    result[3] = m1[1] * m2[0] + m1[4] * m2[3] + m1[7] * m2[6];
    result[4] = m1[1] * m2[1] + m1[4] * m2[4] + m1[7] * m2[7];
    result[5] = m1[1] * m2[2] + m1[4] * m2[5] + m1[7] * m2[8];
    // Row 3
    result[6] = m1[2] * m2[0] + m1[5] * m2[3] + m1[8] * m2[6];
    result[7] = m1[2] * m2[1] + m1[5] * m2[4] + m1[8] * m2[7];
    result[8] = m1[2] * m2[2] + m1[5] * m2[5] + m1[8] * m2[8];
}

void mul_Mat3x3_Mat3x3_Trans( ixMatrix3 result, const ixMatrix3 m1, const ixMatrix3 m2 )
{
    // Row 1
    result[0] = m1[0]*m2[0] + m1[1]*m2[1] + m1[2]*m2[2];
    result[1] = m1[0]*m2[3] + m1[1]*m2[4] + m1[2]*m2[5];
    result[2] = m1[0]*m2[6] + m1[1]*m2[7] + m1[2]*m2[8];
    // Row 2
    result[3] = m1[3]*m2[0] + m1[4]*m2[1] + m1[5]*m2[2];
    result[4] = m1[3]*m2[3] + m1[4]*m2[4] + m1[5]*m2[5];
    result[5] = m1[3]*m2[6] + m1[4]*m2[7] + m1[5]*m2[8];
    // Row 3
    result[6] = m1[6]*m2[0] + m1[7]*m2[1] + m1[8]*m2[2];
    result[7] = m1[6]*m2[3] + m1[7]*m2[4] + m1[8]*m2[5];
    result[8] = m1[6]*m2[6] + m1[7]*m2[7] + m1[8]*m2[8];
}

void mul_Mat3x3_Mat3x3_Trans_d(ixMatrix3d result, const ixMatrix3d m1, const ixMatrix3d m2)
{
    // Row 1
    result[0] = m1[0] * m2[0] + m1[1] * m2[1] + m1[2] * m2[2];
    result[1] = m1[0] * m2[3] + m1[1] * m2[4] + m1[2] * m2[5];
    result[2] = m1[0] * m2[6] + m1[1] * m2[7] + m1[2] * m2[8];
    // Row 2
    result[3] = m1[3] * m2[0] + m1[4] * m2[1] + m1[5] * m2[2];
    result[4] = m1[3] * m2[3] + m1[4] * m2[4] + m1[5] * m2[5];
    result[5] = m1[3] * m2[6] + m1[4] * m2[7] + m1[5] * m2[8];
    // Row 3
    result[6] = m1[6] * m2[0] + m1[7] * m2[1] + m1[8] * m2[2];
    result[7] = m1[6] * m2[3] + m1[7] * m2[4] + m1[8] * m2[5];
    result[8] = m1[6] * m2[6] + m1[7] * m2[7] + m1[8] * m2[8];
}

void mul_Mat2x2_Vec2x1( ixVector2 result, const ixMatrix2 m, const ixVector2 v )
{
    result[0] = m[0]*v[0] + m[1]*v[1];
    result[1] = m[2]*v[0] + m[3]*v[1];
}

void mul_Mat2x2_Trans_Vec2x1( ixVector2 result, const ixMatrix2 m, const ixVector2 v )
{
    result[0] = m[0]*v[0] + m[2]*v[1];
    result[1] = m[1]*v[0] + m[3]*v[1];
}

void mul_Mat3x3_Vec3x1( ixVector3 result, const ixMatrix3 m, const ixVector3 v )
{
        result[0] = m[0]*v[0] + m[1]*v[1] + m[2]*v[2];
        result[1] = m[3]*v[0] + m[4]*v[1] + m[5]*v[2];
        result[2] = m[6]*v[0] + m[7]*v[1] + m[8]*v[2];
}

void mul_Mat3x3_Trans_Vec3x1( ixVector3 result, const ixMatrix3 m, const ixVector3 v )
{
    result[0] = m[0]*v[0] + m[3]*v[1] + m[6]*v[2];
    result[1] = m[1]*v[0] + m[4]*v[1] + m[7]*v[2];
    result[2] = m[2]*v[0] + m[5]*v[1] + m[8]*v[2];
}

void mul_Mat4x4_Vec4x1( ixVector4 result, const ixMatrix4 m, const ixVector4 v )
{
        result[0] =  m[0]*v[0] +  m[1]*v[1] +  m[2]*v[2] +  m[3]*v[3];
        result[1] =  m[4]*v[0] +  m[5]*v[1] +  m[6]*v[2] +  m[7]*v[3];
        result[2] =  m[8]*v[0] +  m[9]*v[1] + m[10]*v[2] + m[11]*v[3];
        result[3] = m[12]*v[0] + m[13]*v[1] + m[14]*v[2] + m[15]*v[3];
}

void mul_Mat4x4_Trans_Vec4x1( ixVector4 result, const ixMatrix4 m, const ixVector4 v )
{
    result[0] =  m[0]*v[0] + m[4]*v[1] +  m[8]*v[2] + m[12]*v[3];
    result[1] =  m[1]*v[0] + m[5]*v[1] +  m[9]*v[2] + m[13]*v[3];
    result[2] =  m[2]*v[0] + m[6]*v[1] + m[10]*v[2] + m[14]*v[3];
    result[3] =  m[3]*v[0] + m[7]*v[1] + m[11]*v[2] + m[15]*v[3];
}

void neg_Mat3x3(ixMatrix3 result, const ixMatrix3 m)
{
    // Row 1
    result[0] = -m[0];
    result[1] = -m[1];
    result[2] = -m[2];
    // Row 2
    result[3] = -m[3];
    result[4] = -m[4];
    result[5] = -m[5];
    // Row 3
    result[6] = -m[6];
    result[7] = -m[7];
    result[8] = -m[8];
}

void mul_Vec3x1_Vec1x3( ixMatrix3 result, const ixVector3 v1, const  ixVector3 v2 )
{
        // Row 1
        result[0] = v1[0]*v2[0];
        result[1] = v1[0]*v2[1];
        result[2] = v1[0]*v2[2];
        // Row 2
        result[3] = v1[1]*v2[0];
        result[4] = v1[1]*v2[1];
        result[5] = v1[1]*v2[2];
        // Row 3
        result[6] = v1[2]*v2[0];
        result[7] = v1[2]*v2[1];
        result[8] = v1[2]*v2[2];
}

void mul_Vec3_Vec3( ixVector3 result, const ixVector3 v1, const ixVector3 v2 )
{
        result[0] = v1[0] * v2[0];
        result[1] = v1[1] * v2[1];
        result[2] = v1[2] * v2[2];
}

void mul_Vec4_Vec4( ixVector4 result, const ixVector4 v1, const ixVector4 v2 )
{
        result[0] = v1[0] * v2[0];
        result[1] = v1[1] * v2[1];
        result[2] = v1[2] * v2[2];
        result[3] = v1[3] * v2[3];
}

void sqrt_Vec3( ixVector3 result, const ixVector3 v )
{
        result[0] = _SQRT( v[0] );
        result[1] = _SQRT( v[1] );
        result[2] = _SQRT( v[2] );
}

void sqrt_Vec4( ixVector4 result, const ixVector4 v )
{
        result[0] = _SQRT( v[0] );
        result[1] = _SQRT( v[1] );
        result[2] = _SQRT( v[2] );
        result[3] = _SQRT( v[3] );
}

void abs_Vec2( ixVector2 result, const ixVector2 v )
{
        result[0] = _FABS( v[0] );
        result[1] = _FABS( v[1] );
}

void abs_Vec2d(ixVector2d result, const ixVector2d v)
{
        result[0] = fabs(v[0]);
        result[1] = fabs(v[1]);
}

void abs_Vec3( ixVector3 result, const ixVector3 v )
{
        result[0] = _FABS( v[0] );
        result[1] = _FABS( v[1] );
        result[2] = _FABS( v[2] );      
}

void abs_Vec3d(ixVector3d result, const ixVector3d v)
{
        result[0] = fabs(v[0]);
        result[1] = fabs(v[1]);
        result[2] = fabs(v[2]);
}

void abs_Vec4( ixVector4 result, const ixVector4 v )
{
        result[0] = _FABS( v[0] );
        result[1] = _FABS( v[1] );
        result[2] = _FABS( v[2] );
        result[3] = _FABS( v[3] );
}

void abs_Vec4d(ixVector4d result, const ixVector4d v)
{
        result[0] = fabs(v[0]);
        result[1] = fabs(v[1]);
        result[2] = fabs(v[2]);
        result[3] = fabs(v[3]);
}

f_t dot_Vec2_Vec2(const ixVector2 v1, const ixVector2 v2 )
{
        return  v1[0] * v2[0] +
                        v1[1] * v2[1];
}

f_t dot_Vec3_Vec3(const ixVector3 v1, const ixVector3 v2 )
{
        return  v1[0] * v2[0] +
                v1[1] * v2[1] +
                v1[2] * v2[2];
}

double dot_Vec3d_Vec3d(const ixVector3d v1, const ixVector3d v2)
{
    return  v1[0] * v2[0] +
            v1[1] * v2[1] +
            v1[2] * v2[2];
}

f_t dot_Vec4_Vec4(const ixVector4 v1, const ixVector4 v2 )
{
        return  v1[0] * v2[0] +
                v1[1] * v2[1] +
                v1[2] * v2[2] +
                v1[3] * v2[3];
}

//_______________________________________________________________________________________________
//observe that cross product output cannot overwrite cross product input without destroying logic
void cross_Vec3( ixVector3 result, const ixVector3 v1, const ixVector3 v2 )
{
    result[0] = v1[1]*v2[2] - v1[2]*v2[1];
    result[1] = v1[2]*v2[0] - v1[0]*v2[2];
    result[2] = v1[0]*v2[1] - v1[1]*v2[0]; 
}

void crossd_Vec3( ixVector3d result, const ixVector3 v1, const ixVector3 v2 )
{
        result[0] = (double)(v1[1] * v2[2] - v1[2] * v2[1]);
        result[1] = (double)(v1[2] * v2[0] - v1[0] * v2[2]);
        result[2] = (double)(v1[0] * v2[1] - v1[1] * v2[0]);
}

void mul_Vec2_X( ixVector2 result, const ixVector2 v, const f_t x )
{
    result[0] = v[0]*x;
    result[1] = v[1]*x;
}

void mul_Vec2d_X( ixVector2d result, const ixVector2d v, const double x )
{
    result[0] = v[0]*x;
    result[1] = v[1]*x;
}

void mul_Vec3_X( ixVector3 result, const ixVector3 v, const f_t x )
{
        result[0] = v[0]*x;
        result[1] = v[1]*x;
        result[2] = v[2]*x;
}

void mul_Vec3d_X( ixVector3d result, const ixVector3d v, const double x )
{
        result[0] = v[0]*x;
        result[1] = v[1]*x;
        result[2] = v[2]*x;
}

void mul_Vec4_X( ixVector4 result, const ixVector4 v, const f_t x )
{
        result[0] = v[0]*x;
        result[1] = v[1]*x;
        result[2] = v[2]*x;
        result[3] = v[3]*x;
}

void mul_Vec4d_X( ixVector4d result, const ixVector4d v, const double x )
{
        result[0] = v[0] * x;
        result[1] = v[1] * x;
        result[2] = v[2] * x;
        result[3] = v[3] * x;
}

void div_Vec3_X( ixVector3 result, const ixVector3 v, const f_t x )
{
    f_t d = (f_t)1.0/x;
        result[0] = v[0]*d;
        result[1] = v[1]*d;
        result[2] = v[2]*d;
}

void div_Vec4_X( ixVector4 result, const ixVector4 v, const f_t x )
{
    f_t d = (f_t)1.0/x;
        result[0] = v[0]*d;
        result[1] = v[1]*d;
        result[2] = v[2]*d;
        result[3] = v[3]*d;
}
void div_Vec4d_X( ixVector4d result, const ixVector4d v, const double x )
{
        double d = 1.0 / x;
        result[0] = v[0] * d;
        result[1] = v[1] * d;
        result[2] = v[2] * d;
        result[3] = v[3] * d;
}

void add_Vec3_Vec3( ixVector3 result, const ixVector3 v1, const ixVector3 v2 )
{
        result[0] = v1[0] + v2[0];
        result[1] = v1[1] + v2[1];
        result[2] = v1[2] + v2[2];
}

void add_Vec3d_Vec3d( ixVector3d result, const ixVector3d v1, const ixVector3d v2 )
{
    result[0] = v1[0] + v2[0];
    result[1] = v1[1] + v2[1];
    result[2] = v1[2] + v2[2];
}

void add_K1Vec3_K2Vec3(ixVector3 result, const ixVector3 v1, const ixVector3 v2, float k1, float k2)
{
    result[0] = k1 * v1[0] + k2 * v2[0];
    result[1] = k1 * v1[1] + k2 * v2[1];
    result[2] = k1 * v1[2] + k2 * v2[2];
}

void add_Vec4_Vec4( ixVector4 result, const ixVector4 v1, const ixVector4 v2 )
{
        result[0] = v1[0] + v2[0];
        result[1] = v1[1] + v2[1];
        result[2] = v1[2] + v2[2];
        result[3] = v1[3] + v2[3];
}

void add_Vec4d_Vec4d( ixVector4d result, const ixVector4d v1, const ixVector4d v2 )
{
        result[0] = v1[0] + v2[0];
        result[1] = v1[1] + v2[1];
        result[2] = v1[2] + v2[2];
        result[3] = v1[3] + v2[3];
}

void sub_Vec3_Vec3( ixVector3 result, const ixVector3 v1, const ixVector3 v2 )
{
        result[0] = v1[0] - v2[0];
        result[1] = v1[1] - v2[1];
        result[2] = v1[2] - v2[2];
}

void sub_Vec3d_Vec3d( ixVector3d result, const ixVector3d v1, const ixVector3d v2 )
{
    result[0] = v1[0] - v2[0];
    result[1] = v1[1] - v2[1];
    result[2] = v1[2] - v2[2];
}

void sub_Vec4_Vec4( ixVector4 result, const ixVector4 v1, const ixVector4 v2 )
{
        result[0] = v1[0] - v2[0];
        result[1] = v1[1] - v2[1];
        result[2] = v1[2] - v2[2];
        result[3] = v1[3] - v2[3];
}

void div_Vec3_Vec3( ixVector3 result, const ixVector3 v1, const ixVector3 v2 )
{
        result[0] = v1[0] / v2[0];
        result[1] = v1[1] / v2[1];
        result[2] = v1[2] / v2[2];
}

void div_Vec4_Vec4( ixVector4 result, const ixVector4 v1, const ixVector4 v2 )
{
        result[0] = v1[0] / v2[0];
        result[1] = v1[1] / v2[1];
        result[2] = v1[2] / v2[2];
        result[3] = v1[3] / v2[3];
}

void neg_Vec3(ixVector3 result, const ixVector3 v)
{
    result[0] = -v[0];
    result[1] = -v[1];
    result[2] = -v[2];
}

void cpy_MatRxC_MatMxN( f_t *result, i_t r, i_t c, i_t r_offset, i_t c_offset, f_t *A, i_t m, i_t n )
{
        // Ensure source matrix A fits within result matrix
        if( (m + r_offset) > r || (n + c_offset) > c )
                return;

        // Set result pointer to first location
        result += c*r_offset + c_offset;

        int rowSize = sizeof( f_t )*n;
        for( int mi=0; mi<m; mi++ )
        {
                // Copy row
                memcpy( result, A, rowSize );

                // Update to next row
                A               += n;
                result  += c;
        }
}


void transpose_Mat2( ixMatrix2 result, const ixMatrix2 m )
{
    // Row 1
    result[0] = m[0];
    result[1] = m[2];
    // Row 2
    result[2] = m[1];
    result[3] = m[3];
}

void transpose_Mat3( ixMatrix3 result, const ixMatrix3 m )
{
        // Row 1
        result[0] = m[0];
        result[1] = m[3];
        result[2] = m[6];
        // Row 2
        result[3] = m[1];
        result[4] = m[4];
        result[5] = m[7];
        // Row 3
        result[6] = m[2];
        result[7] = m[5];
        result[8] = m[8];
}

void transpose_Mat4( ixMatrix4 result, const ixMatrix4 m )
{
        // Row 1
        result[ 0] = m[ 0];
        result[ 1] = m[ 4];
        result[ 2] = m[ 8];
        result[ 3] = m[12];
        // Row 2
        result[ 4] = m[ 1];
        result[ 5] = m[ 5];
        result[ 6] = m[ 9];
        result[ 7] = m[13];
        // Row 3
        result[ 8] = m[ 2];
        result[ 9] = m[ 6];
        result[10] = m[10];
        result[11] = m[14];
        // Row 4
        result[12] = m[ 3];
        result[13] = m[ 7];
        result[14] = m[11];
        result[15] = m[15];
}

char inv_Mat2(ixMatrix2 result, ixMatrix2 m)
{
    f_t invDet, det = m[0] * m[3] - m[1] * m[2];

    if( det!=0.0f )
        invDet = 1.0f/det;
    else
        return -1;

        // Row 1
    result[0] = m[3] * ( invDet);
        result[1] = m[1] * (-invDet);
        // Row 2
        result[2] = m[2] * (-invDet);
        result[3] = m[0] * ( invDet);

    return 1;
}

char inv_Mat3( ixMatrix3 result, const ixMatrix3 m )
{
        //      | m[0] m[1] m[2] |-1             |   m[8]m[4]-m[7]m[5]  -(m[8]m[1]-m[7]m[2])   m[5]m[1]-m[4]m[2]  |
        //      | m[3] m[4] m[5] |    =  1/det * | -(m[8]m[3]-m[6]m[5])   m[8]m[0]-m[6]m[2]  -(m[5]m[0]-m[3]m[2]) |
        //      | m[6] m[7] m[8] |               |   m[7]m[3]-m[6]m[4]  -(m[7]m[0]-m[6]m[1])   m[4]m[0]-m[3]m[1]  |
        
        f_t invDet, det = m[0]*(m[8]*m[4]-m[7]*m[5]) - m[3]*(m[8]*m[1]-m[7]*m[2]) + m[6]*(m[5]*m[1]-m[4]*m[2]);

        if( det!=0 )
                invDet = 1/det;
        else
                return -1;
                
        // Row 1
        result[0] = invDet * (m[8]*m[4]-m[7]*m[5]);
        result[1] = invDet * (m[7]*m[2]-m[8]*m[1]);  
        result[2] = invDet * (m[5]*m[1]-m[4]*m[2]);
        // Row 2
        result[3] = invDet * (m[6]*m[5]-m[8]*m[3]);
        result[4] = invDet * (m[8]*m[0]-m[6]*m[2]); 
        result[5] = invDet * (m[3]*m[2]-m[5]*m[0]);
        // Row 3
        result[6] = invDet * (m[7]*m[3]-m[6]*m[4]);
        result[7] = invDet * (m[6]*m[1]-m[7]*m[0]);
        result[8] = invDet * (m[4]*m[0]-m[3]*m[1]);
        
        return 0;
}

char inv_Mat4( ixMatrix4 result, const ixMatrix4 m )
{
    f_t inv[16], det;
    int i;

    inv[0] = 
        m[5]  * m[10] * m[15] - 
        m[5]  * m[11] * m[14] - 
        m[9]  * m[6]  * m[15] + 
        m[9]  * m[7]  * m[14] +
        m[13] * m[6]  * m[11] - 
        m[13] * m[7]  * m[10];

    inv[4] = 
       -m[4]  * m[10] * m[15] + 
        m[4]  * m[11] * m[14] + 
        m[8]  * m[6]  * m[15] - 
        m[8]  * m[7]  * m[14] - 
        m[12] * m[6]  * m[11] + 
        m[12] * m[7]  * m[10];

    inv[8] = 
        m[4]  * m[9] * m[15] - 
        m[4]  * m[11] * m[13] - 
        m[8]  * m[5] * m[15] + 
        m[8]  * m[7] * m[13] + 
        m[12] * m[5] * m[11] - 
        m[12] * m[7] * m[9];

    inv[12] = 
       -m[4]  * m[9] * m[14] + 
        m[4]  * m[10] * m[13] +
        m[8]  * m[5] * m[14] - 
        m[8]  * m[6] * m[13] - 
        m[12] * m[5] * m[10] + 
        m[12] * m[6] * m[9];

    inv[1] = 
       -m[1]  * m[10] * m[15] + 
        m[1]  * m[11] * m[14] + 
        m[9]  * m[2] * m[15] - 
        m[9]  * m[3] * m[14] - 
        m[13] * m[2] * m[11] + 
        m[13] * m[3] * m[10];

    inv[5] = 
        m[0]  * m[10] * m[15] - 
        m[0]  * m[11] * m[14] - 
        m[8]  * m[2] * m[15] + 
        m[8]  * m[3] * m[14] + 
        m[12] * m[2] * m[11] - 
        m[12] * m[3] * m[10];

    inv[9] = 
       -m[0]  * m[9] * m[15] + 
        m[0]  * m[11] * m[13] + 
        m[8]  * m[1] * m[15] - 
        m[8]  * m[3] * m[13] - 
        m[12] * m[1] * m[11] + 
        m[12] * m[3] * m[9];

    inv[13] = 
        m[0]  * m[9] * m[14] - 
        m[0]  * m[10] * m[13] - 
        m[8]  * m[1] * m[14] + 
        m[8]  * m[2] * m[13] + 
        m[12] * m[1] * m[10] - 
        m[12] * m[2] * m[9];

    inv[2] = 
        m[1]  * m[6] * m[15] - 
        m[1]  * m[7] * m[14] - 
        m[5]  * m[2] * m[15] + 
        m[5]  * m[3] * m[14] + 
        m[13] * m[2] * m[7] - 
        m[13] * m[3] * m[6];

    inv[6] = 
       -m[0]  * m[6] * m[15] + 
        m[0]  * m[7] * m[14] + 
        m[4]  * m[2] * m[15] - 
        m[4]  * m[3] * m[14] - 
        m[12] * m[2] * m[7] + 
        m[12] * m[3] * m[6];

    inv[10] = 
        m[0]  * m[5] * m[15] - 
        m[0]  * m[7] * m[13] - 
        m[4]  * m[1] * m[15] + 
        m[4]  * m[3] * m[13] + 
        m[12] * m[1] * m[7] - 
        m[12] * m[3] * m[5];

    inv[14] = 
       -m[0]  * m[5] * m[14] + 
        m[0]  * m[6] * m[13] + 
        m[4]  * m[1] * m[14] - 
        m[4]  * m[2] * m[13] - 
        m[12] * m[1] * m[6] + 
        m[12] * m[2] * m[5];

    inv[3] = 
       -m[1] * m[6] * m[11] + 
        m[1] * m[7] * m[10] + 
        m[5] * m[2] * m[11] - 
        m[5] * m[3] * m[10] - 
        m[9] * m[2] * m[7] + 
        m[9] * m[3] * m[6];

    inv[7] = 
        m[0] * m[6] * m[11] - 
        m[0] * m[7] * m[10] - 
        m[4] * m[2] * m[11] + 
        m[4] * m[3] * m[10] + 
        m[8] * m[2] * m[7] - 
        m[8] * m[3] * m[6];

    inv[11] = 
       -m[0] * m[5] * m[11] + 
        m[0] * m[7] * m[9] + 
        m[4] * m[1] * m[11] - 
        m[4] * m[3] * m[9] - 
        m[8] * m[1] * m[7] + 
        m[8] * m[3] * m[5];

    inv[15] = 
        m[0] * m[5] * m[10] - 
        m[0] * m[6] * m[9] - 
        m[4] * m[1] * m[10] + 
        m[4] * m[2] * m[9] + 
        m[8] * m[1] * m[6] - 
        m[8] * m[2] * m[5];

    det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];

    if (det == 0)
        return -1;

    det = (f_t)1.0 / det;

    for (i = 0; i < 16; i++)
        result[i] = inv[i] * det;

    return 0;
}

// Initialize Alpha Filter alpha and beta values
void LPFO0_init_Vec3( sLpfO0 *lpf, f_t dt, f_t cornerFreqHz, const ixVector3 initVal )
{
    f_t dc;

    memset( lpf, 0, sizeof(sLpfO0) );
    cpy_Vec3_Vec3( lpf->v, initVal );

    dc = dt * cornerFreqHz;
    lpf->alpha  = dc / (1.0f + dc);
    lpf->beta   = 1.0f - lpf->alpha;
}

// Low-Pass Alpha Filter
void LPFO0_Vec3( sLpfO0 *lpf, const ixVector3 input )
{
    // v[n+1] = beta*v[n] + alpha*input
        O0_LPF_Vec3( lpf->v, input, lpf->alpha, lpf->beta );
}