Matrix Functions
<source lang="lsl2"> // // ******************************************************************************* // ******************************************************************************* // // 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()
{
}
} </source>