Matrix Functions

//
// *******************************************************************************
// *******************************************************************************
//
// Developed by KyleFlynn.
//
// This is free and open source code, protected by the GPLv3 license.
// http://www.gnu.org/licenses/gpl-3.0.html
// Accepting this code is conditioned upon accepting this license.
//
//
// The following are some core linear algebra functions.
// They are primarily used to develop and use 3D transformation matrices.
// Note that ALL matrices herein are assumed to be 3x3 matrices.
// And all of these 3x3 matrices are assumed to be stored as a 3 element list of vectors.
// In all cases, the vectors are assumed to be column (not row) vectors.
//
// For all purposes herein, rows are numbered 1,2,3 and columns are also numbered 1,2,3 (both 1 based).
//
// As a last note, there are certainly faster ways to perform these function.
// However, the following imposes an excellent organization when performing
// complex 3D transformations.
//
// The available functions are:
//
//      list    MakeMatrix(float r1c1, float r1c2, float r1c3, r2c1, ...
//      float   GetMatrixElement(list m, integer row, integer col)
//      list    SetMatrixElement(list m, integer row, integer col, float value) // Returns a new matrix with the new element set.
//      list    MultMatrices(list m1, list m2) 
//      list    DivideMatrices(list m1, list m2)
//      list    BackDivideMatrices(list m1, list m2) 
//      list    InvertMatrix(list m)
//      list    TransposeMatrix(list m)  
//      integer MatrixIsZero(list m) 
//      integer MatrixIsOrthogonal(list m)
//      integer MatrixIsNormal(list m)
//
//      vector  SetVectorElement(vector v, integer row, float value) // Returns a new vector with the new element set.
//      float   GetVectorElement(vector v, integer row)
//      list    MatrixFrom3ColVectors(vector v1, vector v2, vector v3) 
//      list    ZeroMatrix()
//      list    UnitMatrix()
//      vector  MakeVector(float x, float y, float z)
//      string  VectorString(vector v)          // For debugging.
//      string  MatrixString(list m)            // For debugging.
//
// *******************************************************************************
// *******************************************************************************
//

list MakeMatrix(float r1c1, float r1c2, float r1c3,  
                float r2c1, float r2c2, float r2c3,  
                float r3c1, float r3c2, float r3c3) 
{
    return [<r1c1, r2c1, r3c1>, <r1c2, r2c2, r3c2>, <r1c3, r2c3, r3c3>];
}

float GetMatrixElement(list m, integer row, integer col)
{
    return GetVectorElement(llList2Vector(m, col - 1), row);
}

list SetMatrixElement(list m, integer row, integer col, float value) // Returns a new matrix with the new element set.
{
    return llListReplaceList(m, [SetVectorElement(llList2Vector(m, col - 1), row, value)], col - 1, col - 1);
}

float GetVectorElement(vector v, integer row)
{
         if (row == 1) return v.x;
    else if (row == 2) return v.y;
    else if (row == 3) return v.z;
    //
    return 0;
}

vector SetVectorElement(vector v, integer row, float value) // Returns a new vector with the new element set.
{
         if (row == 1) v.x = value;
    else if (row == 2) v.y = value;
    else if (row == 3) v.z = value;
    //
    return v;
}

list ZeroMatrix()
{
    return MakeMatrix(0, 0, 0,
                      0, 0, 0,
                      0, 0, 0);
}

list UnitMatrix()
{
    return MakeMatrix(1, 0, 0,
                      0, 1, 0,
                      0, 0, 1);
}

vector MakeVector(float x, float y, float z)
{
    return <x, y, z>;
}

list  MatrixFrom3ColVectors(vector v1, vector v2, vector v3) 
{
    return [v1, v2, v3];
}

list DivideMatrices(list m1, list m2)
{
    // In Matlab, this is equivalent to:     X = m1 / m2;
    return MultMatrices(m1, InvertMatrix(m2));
}

list BackDivideMatrices(list m1, list m2) 
{
    // In Matlab, this is equivalent to:     X = m1 \ m2;
    return MultMatrices(InvertMatrix(m1), m2);
}

list MultMatrices(list m1, list m2) 
{
    // A matrix generalization of the vector dot product.
    // This procedure specifically multiplies 3x3 matrices, and returns a 3x3 matrix.
    integer r1Ptr;
    integer c1Ptr;
    integer c2Ptr;
    list m3 = ZeroMatrix();
    //
    for (r1Ptr = 1; r1Ptr <= 3; r1Ptr++)
    {    
        for (c2Ptr = 1; c2Ptr <=3; c2Ptr++)
        {
            for (c1Ptr = 1; c1Ptr <=3; c1Ptr++)
            {
                // This uses column ptr for row, but that's okay.
                m3 = SetMatrixElement(m3, r1Ptr, c2Ptr, GetMatrixElement(m3, r1Ptr, c2Ptr) + GetMatrixElement(m1, r1Ptr, c1Ptr) * GetMatrixElement(m2, c1Ptr, c2Ptr));
            }
        }
    }
    //
    return m3;
}

list InvertMatrix(list m)
{
    // Don't forget that InvertMatrix(m)=TransposeMatrix(m) if m is orthonormal.
    // And TransposeMatrix(m) would be much faster.
    //
    // This ONLY works with 3x3 matrices.
    integer rPtr;
    integer cPtr;
    integer ptr;
    integer rPtr2;
    integer cPtr2;
    float PivotMax;
    float dNorm;
    float dSwap;
    integer swpCnt;
    //
    for (ptr = 1; ptr <= 3; ptr++)
    {
        // Search max pivot.
        rPtr2 = ptr;
        cPtr2 = ptr;
        PivotMax = 0;
        for (rPtr = ptr; rPtr <= 3; rPtr++)
        {
            for (cPtr = ptr; cPtr <= 3; cPtr++)
            {            
                if (llFabs(GetMatrixElement(m, rPtr, cPtr)) > PivotMax)
                {
                    rPtr2 = rPtr;
                    cPtr2 = cPtr;
                    PivotMax = llFabs(GetMatrixElement(m, rPtr, cPtr));
                }
            }
        }
        // Swap rows and columns.
        if (rPtr2 > ptr)
        {
            // Swap row.
            for (cPtr = 1; cPtr <= 3; cPtr++)
            {
                dSwap = GetMatrixElement(m, rPtr2, cPtr);
                m = SetMatrixElement(m, rPtr2, cPtr, GetMatrixElement(m, ptr, cPtr));
                m = SetMatrixElement(m, ptr, cPtr, dSwap);
            }
            swpCnt++;
            SetSwapDiag(swpCnt, 1, ptr);
            SetSwapDiag(swpCnt, 2, rPtr2);
            SetSwapDiag(swpCnt, 3, 1);
        }
        if (cPtr2 > ptr)
        {
            // Swap column.
            for (rPtr = 1; rPtr <= 3; rPtr++)
            {
                dSwap = GetMatrixElement(m, rPtr, cPtr2);
                m = SetMatrixElement(m, rPtr, cPtr2, GetMatrixElement(m, rPtr, ptr));
                m = SetMatrixElement(m, rPtr, ptr, dSwap);
            }
            swpCnt++;
            SetSwapDiag(swpCnt, 1, ptr);
            SetSwapDiag(swpCnt, 2, cPtr2);
            SetSwapDiag(swpCnt, 3, 2);
        }
        //
        // Check pivot 0.
        if (llFabs(GetMatrixElement(m, ptr, ptr)) <= 0)
        {
            llSay(DEBUG_CHANNEL, "Matrix inversion error, script is stopped.");
            rPtr = 1 / 0; // Crash the script.
        }
        //
        // Normalization.
        dNorm = GetMatrixElement(m, ptr, ptr);
        m = SetMatrixElement(m, ptr, ptr, 1);
        for (cPtr = 1; cPtr <= 3; cPtr++)
        {
            m = SetMatrixElement(m, ptr, cPtr, GetMatrixElement(m, ptr, cPtr) / dNorm);
        }
        // Linear reduction.
        for (rPtr = 1; rPtr <= 3; rPtr++)
        {
            if (rPtr != ptr && GetMatrixElement(m, rPtr, ptr) != 0)
            {
                dNorm = GetMatrixElement(m, rPtr, ptr);
                m = SetMatrixElement(m, rPtr, ptr, 0);
                for (cPtr = 1; cPtr <= 3; cPtr++)
                {
                    m = SetMatrixElement(m, rPtr, cPtr, GetMatrixElement(m, rPtr, cPtr) - dNorm * GetMatrixElement(m, ptr, cPtr));
                }
            }
        }
    }
    //
    // Un-Scramble rows.
    for (ptr = swpCnt; ptr >= 1; ptr--)
    {
        if (GetSwapDiag(ptr, 3) == 1)
        {
            // Swap column.
            for (rPtr = 1; rPtr <=3; rPtr++)
            {
                dSwap = GetMatrixElement(m, rPtr, GetSwapDiag(ptr, 2));
                m = SetMatrixElement(m, rPtr, GetSwapDiag(ptr, 2), GetMatrixElement(m, rPtr, GetSwapDiag(ptr, 1)));
                m = SetMatrixElement(m, rPtr, GetSwapDiag(ptr, 1), dSwap);
            }
        }
        else if (GetSwapDiag(ptr, 3) == 2)
        {
            // Swap row.
            for (cPtr = 1; cPtr <= 3; cPtr++)
            {
                dSwap = GetMatrixElement(m, GetSwapDiag(ptr, 2), cPtr);
                m = SetMatrixElement(m, GetSwapDiag(ptr, 2), cPtr, GetMatrixElement(m, GetSwapDiag(ptr, 1), cPtr));
                m = SetMatrixElement(m, GetSwapDiag(ptr, 1), cPtr, dSwap);
            }
        }
    }
    //
    return m;
}

list TransposeMatrix(list m)  
{
    list m2 = ZeroMatrix();
    //
    m2 = SetMatrixElement(m2, 1, 2, GetMatrixElement(m, 2, 1));
    m2 = SetMatrixElement(m2, 1, 3, GetMatrixElement(m, 3, 1));
    m2 = SetMatrixElement(m2, 2, 1, GetMatrixElement(m, 1, 2));
    m2 = SetMatrixElement(m2, 2, 3, GetMatrixElement(m, 3, 2));
    m2 = SetMatrixElement(m2, 3, 1, GetMatrixElement(m, 1, 3));
    m2 = SetMatrixElement(m2, 3, 2, GetMatrixElement(m, 2, 3));
    //
    // Don't need to transpose the diagonal elements.
    m2 = SetMatrixElement(m2, 1, 1, GetMatrixElement(m, 1, 1));
    m2 = SetMatrixElement(m2, 2, 2, GetMatrixElement(m, 2, 2));
    m2 = SetMatrixElement(m2, 3, 3, GetMatrixElement(m, 3, 3));
    //
    return m2;
}

integer MatrixIsZero(list m) 
{
    return (GetMatrixElement(m, 1, 1) == 0 && GetMatrixElement(m, 2, 1) == 0 && GetMatrixElement(m, 3, 1) == 0 && 
            GetMatrixElement(m, 1, 2) == 0 && GetMatrixElement(m, 2, 2) == 0 && GetMatrixElement(m, 3, 2) == 0 && 
            GetMatrixElement(m, 1, 3) == 0 && GetMatrixElement(m, 2, 3) == 0 && GetMatrixElement(m, 3, 3) == 0);
}

integer MatrixIsOrthogonal(list m)
{
    float epsilon = .00002; // Our allowed tolerance.
    //
    vector v1 = llList2Vector(m, 0);
    vector v2 = llList2Vector(m, 1);
    vector v3 = llList2Vector(m, 2);
    //
    return llFabs(llFabs(llRot2Angle(llRotBetween(v1, v2))) - 90) < epsilon &&
           llFabs(llFabs(llRot2Angle(llRotBetween(v1, v3))) - 90) < epsilon &&
           llFabs(llFabs(llRot2Angle(llRotBetween(v2, v3))) - 90) < epsilon;
}

integer MatrixIsNormal(list m)
{
    float epsilon = .00002; // Our allowed tolerance.
    //
    vector v1 = llList2Vector(m, 0);
    vector v2 = llList2Vector(m, 1);
    vector v3 = llList2Vector(m, 2);
    //
    return llFabs(v1.x * v1.x + v1.y * v1.y + v1.z * v1.z - 1) < epsilon &&
           llFabs(v2.x * v2.x + v2.y * v2.y + v2.z * v2.z - 1) < epsilon &&
           llFabs(v3.x * v3.x + v3.y * v3.y + v3.z * v3.z - 1) < epsilon;
}

string VectorString(vector v)  
// Primarily used for debugging purposes.
{
    return "[ " + Float2StringFormat(v.x, 5, TRUE) + ", " + Float2StringFormat(v.y, 5, TRUE) + ", " + Float2StringFormat(v.z, 5, TRUE) + " ]";
}

string MatrixString(list m)
// Primarily used for debugging purposes.
{
    return "\n[ " + Float2StringFormat(GetMatrixElement(m, 1, 1), 5, TRUE) + ", " + Float2StringFormat(GetMatrixElement(m, 1, 2), 5, TRUE) + ", " + Float2StringFormat(GetMatrixElement(m, 1, 3), 5, TRUE) + " ;" + "\n" + 
             "  " + Float2StringFormat(GetMatrixElement(m, 2, 1), 5, TRUE) + ", " + Float2StringFormat(GetMatrixElement(m, 2, 2), 5, TRUE) + ", " + Float2StringFormat(GetMatrixElement(m, 2, 3), 5, TRUE) + " ;" + "\n" + 
             "  " + Float2StringFormat(GetMatrixElement(m, 3, 1), 5, TRUE) + ", " + Float2StringFormat(GetMatrixElement(m, 3, 2), 5, TRUE) + ", " + Float2StringFormat(GetMatrixElement(m, 3, 3), 5, TRUE) + " ]";
}

string Float2StringFormat(float num, integer places, integer rnd) 
// Allows string output of a float in a tidy text format.
// rnd (rounding) should be set to TRUE for rounding, FALSE for no rounding
{ 
    if (rnd) 
    {
        float f = llPow(10.0, places);
        integer i = llRound(llFabs(num) * f);
        string s = "00000" + (string)i; // Number of 0s is (value of max places - 1).
        if (num < 0.0) return "-" + (string)( (integer)(i / f) ) + "." + llGetSubString(s, -places, -1);
        return (string)((integer)(i / f)) + "." + llGetSubString(s, -places, -1);
    }
    if (!places) return (string)((integer)num );
    if ((places = (places - 7 - (places < 1))) & 0x80000000) return llGetSubString((string)num, 0, places);
    return (string)num;
}

// These globals are just used in SetSwapDiag & GetSwapDiag, which are used only in InvertMatrix.
// They emulate a 6x3 array of integers.
integer iSwapDiag11;
integer iSwapDiag21;
integer iSwapDiag31;
integer iSwapDiag41;
integer iSwapDiag51;
integer iSwapDiag61;
integer iSwapDiag12;
integer iSwapDiag22;
integer iSwapDiag32;
integer iSwapDiag42;
integer iSwapDiag52;
integer iSwapDiag62;
integer iSwapDiag13;
integer iSwapDiag23;
integer iSwapDiag33;
integer iSwapDiag43;
integer iSwapDiag53;
integer iSwapDiag63;
//

SetSwapDiag(integer i, integer j, integer value)
// Just used for the InvertMatrix function.
{
         if (i == 1) 
    { 
             if (j == 1) iSwapDiag11 = value;
        else if (j == 2) iSwapDiag12 = value;
        else if (j == 3) iSwapDiag13 = value;
    }
    else if (i == 2)
    {
             if (j == 1) iSwapDiag21 = value;
        else if (j == 2) iSwapDiag22 = value;
        else if (j == 3) iSwapDiag23 = value;
    }
    else if (i == 3)
    {
             if (j == 1) iSwapDiag31 = value;
        else if (j == 2) iSwapDiag32 = value;
        else if (j == 3) iSwapDiag33 = value;
    }
    else if (i == 4)
    {
             if (j == 1) iSwapDiag41 = value;
        else if (j == 2) iSwapDiag42 = value;
        else if (j == 3) iSwapDiag43 = value;
    }
    else if (i == 5)
    {
             if (j == 1) iSwapDiag51 = value;
        else if (j == 2) iSwapDiag52 = value;
        else if (j == 3) iSwapDiag53 = value;
    }
    else if (i == 6)
    {
             if (j == 1) iSwapDiag61 = value;
        else if (j == 2) iSwapDiag62 = value;
        else if (j == 3) iSwapDiag63 = value;
    }
}

integer GetSwapDiag(integer i, integer j)
// Just used for the InvertMatrix function.
{
    integer i1;
    integer i2;
    integer i3;
    //
         if (i == 1) { i1 = iSwapDiag11; i2 = iSwapDiag12; i3 = iSwapDiag13; }
    else if (i == 2) { i1 = iSwapDiag21; i2 = iSwapDiag22; i3 = iSwapDiag23; }
    else if (i == 3) { i1 = iSwapDiag31; i2 = iSwapDiag32; i3 = iSwapDiag33; }
    else if (i == 4) { i1 = iSwapDiag41; i2 = iSwapDiag42; i3 = iSwapDiag43; }
    else if (i == 5) { i1 = iSwapDiag51; i2 = iSwapDiag52; i3 = iSwapDiag53; }
    else if (i == 6) { i1 = iSwapDiag61; i2 = iSwapDiag62; i3 = iSwapDiag63; }
    //
         if (j == 1) return i1;
    else if (j == 2) return i2;
    else if (j == 3) return i3;
    return 0; // Needed for compiler.
}

default
{
    on_rez(integer iStartParam)
    {
        llResetScript();
    }
    state_entry()
    {
    
        list m1;
        list m2;
        list m3;
        list m4;
        //
        m1 = MakeMatrix(1,2,3,4,5,6,7,8,9);
        m2 = MakeMatrix(2.2, 3.3, 4.4, 
                        5.5, 6.6, 5.5, 
                        4.4, 3.2, 2.1);
        m3 = MultMatrices(m1, m2);
        llSay(0, MatrixString(m3));
        
        m4 = DivideMatrices(m3, m2);
        llSay(0, MatrixString(m4)); // m4 should take us back to m1.
        
        // With the above, the MultMatrices and DivideMatrices functions are tested, 
        // along with InvertMatrix via divide.  This is the core of these procedures.
        
        
    
    
    }
    touch_start(integer iNumDetected)
    {
    }
    timer()
    {
    }
}