using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
namespace BauzoidNET
.math
{
public class Matrix3
{
public const int M11
= 0;
public const int M21
= 1;
public const int M31
= 2;
public const int M12
= 3;
public const int M22
= 4;
public const int M32
= 5;
public const int M13
= 6;
public const int M23
= 7;
public const int M33
= 8;
public const int MATRIX_SIZE
= 3;
public float[] values
= new float[MATRIX_SIZE
*MATRIX_SIZE
];
public Matrix3
()
{
}
public float get
(int row,
int col
)
{
return values
[col
*MATRIX_SIZE
+ row
];
}
public void set
(int row,
int col,
float value
)
{
values
[col
*MATRIX_SIZE
+row
] = value
;
}
public void zero
()
{
for (int i
= 0; i
< values
.Length; i
++)
values
[i
] = 0
.0f
;
}
public void identity
()
{
zero
();
values
[M11
] = 1
.0f
; values
[M22
] = 1
.0f
; values
[M33
] = 1
.0f
;
}
public bool equals
(Matrix3 other
)
{
for (int i
= 0; i
< values
.Length; i
++)
{
if (this.values[i
] != other
.values[i
])
return false;
}
return true;
}
public bool isSingular
()
{
float determinant
= getDeterminant
();
return ((float)Math
.Abs(determinant
) < MathUtil
.EPSILON);
}
public Matrix3 getTranspose
()
{
Matrix3 result
= new Matrix3
();
result
.values[M11
] = values
[M11
];
result
.values[M12
] = values
[M21
];
result
.values[M13
] = values
[M31
];
result
.values[M21
] = values
[M12
];
result
.values[M22
] = values
[M22
];
result
.values[M23
] = values
[M32
];
result
.values[M31
] = values
[M13
];
result
.values[M32
] = values
[M23
];
result
.values[M33
] = values
[M33
];
return result
;
}
public float getDeterminant
()
{
float result
=
values
[M11
] * (values
[M22
] * values
[M33
] - values
[M23
] * values
[M32
]) -
values
[M21
] * (values
[M12
] * values
[M33
] - values
[M13
] * values
[M32
]) +
values
[M31
] * (values
[M12
] * values
[M23
] - values
[M13
] * values
[M22
]);
return result
;
}
public Matrix3 getInverse
()
{
Matrix3 result
= new Matrix3
();
float determinant
= getDeterminant
();
if (determinant
== 0
.0f
)
{
// singular matrix cannot be inverted
return this;
}
result
.values[M11
] = values
[M22
] * values
[M33
] - values
[M32
] * values
[M23
];
result
.values[M21
] = -(values
[M21
] * values
[M33
] - values
[M23
] * values
[M31
]);
result
.values[M31
] = values
[M21
] * values
[M32
] - values
[M22
] * values
[M31
];
result
.values[M12
] = -(values
[M12
] * values
[M33
] - values
[M32
] * values
[M13
]);
result
.values[M22
] = values
[M11
] * values
[M33
] - values
[M13
] * values
[M31
];
result
.values[M32
] = -(values
[M11
] * values
[M32
] - values
[M12
] * values
[M31
]);
result
.values[M13
] = values
[M12
] * values
[M23
] - values
[M13
] * values
[M22
];
result
.values[M23
] = -(values
[M11
] * values
[M23
] - values
[M13
] * values
[M21
]);
result
.values[M33
] = values
[M11
] * values
[M22
] - values
[M21
] * values
[M12
];
float multiply
= 1
.0f
/ determinant
;
result
.values[M11
] *= multiply
;
result
.values[M21
] *= multiply
;
result
.values[M31
] *= multiply
;
result
.values[M12
] *= multiply
;
result
.values[M22
] *= multiply
;
result
.values[M32
] *= multiply
;
result
.values[M13
] *= multiply
;
result
.values[M23
] *= multiply
;
result
.values[M33
] *= multiply
;
return result
;
}
public void copyTo
(Matrix3 target
)
{
target
.values[M11
] = values
[M11
]; target
.values[M12
] = values
[M12
]; target
.values[M13
] = values
[M13
];
target
.values[M21
] = values
[M21
]; target
.values[M22
] = values
[M22
]; target
.values[M23
] = values
[M23
];
target
.values[M31
] = values
[M31
]; target
.values[M32
] = values
[M32
]; target
.values[M33
] = values
[M33
];
}
public void copyFrom
(Matrix3 source
)
{
values
[M11
] = source
.values[M11
]; values
[M12
] = source
.values[M12
]; values
[M13
] = source
.values[M13
];
values
[M21
] = source
.values[M21
]; values
[M22
] = source
.values[M22
]; values
[M23
] = source
.values[M23
];
values
[M31
] = source
.values[M31
]; values
[M32
] = source
.values[M32
]; values
[M33
] = source
.values[M33
];
}
public void postMultiply
(Matrix3 v
)
{
multiply
(this,
this, v
);
}
public void preMultiply
(Matrix3 v
)
{
multiply
(this, v,
this);
}
static public void multiply
(Matrix3 result, Matrix3 a, Matrix3 b
)
{
float result11
= a
.values[M11
] * b
.values[M11
] + a
.values[M21
] * b
.values[M12
] + a
.values[M31
] * b
.values[M13
];
float result21
= a
.values[M11
] * b
.values[M21
] + a
.values[M21
] * b
.values[M22
] + a
.values[M31
] * b
.values[M23
];
float result31
= a
.values[M11
] * b
.values[M31
] + a
.values[M21
] * b
.values[M32
] + a
.values[M31
] * b
.values[M33
];
float result12
= a
.values[M12
] * b
.values[M11
] + a
.values[M22
] * b
.values[M12
] + a
.values[M32
] * b
.values[M13
];
float result22
= a
.values[M12
] * b
.values[M21
] + a
.values[M22
] * b
.values[M22
] + a
.values[M32
] * b
.values[M23
];
float result32
= a
.values[M12
] * b
.values[M31
] + a
.values[M22
] * b
.values[M32
] + a
.values[M32
] * b
.values[M33
];
float result13
= a
.values[M13
] * b
.values[M11
] + a
.values[M23
] * b
.values[M12
] + a
.values[M33
] * b
.values[M13
];
float result23
= a
.values[M13
] * b
.values[M21
] + a
.values[M23
] * b
.values[M22
] + a
.values[M33
] * b
.values[M23
];
float result33
= a
.values[M13
] * b
.values[M31
] + a
.values[M23
] * b
.values[M32
] + a
.values[M33
] * b
.values[M33
];
result
.values[M11
] = result11
;
result
.values[M21
] = result21
;
result
.values[M31
] = result31
;
result
.values[M12
] = result12
;
result
.values[M22
] = result22
;
result
.values[M32
] = result32
;
result
.values[M13
] = result13
;
result
.values[M23
] = result23
;
result
.values[M33
] = result33
;
}
static public Matrix3 multiply
(Matrix3 a, Matrix3 b
)
{
Matrix3 result
= new Matrix3
();
multiply
(result, a, b
);
return result
;
}
static public Matrix3 createIdentity
()
{
return createScale
(1
.0f, 1
.0f, 1
.0f
);
}
static public Matrix3 createScale
(float sx,
float sy,
float sz
)
{
Matrix3 result
= new Matrix3
();
result
.values[M11
] = sx
;
result
.values[M22
] = sy
;
result
.values[M33
] = sz
;
return result
;
}
static public Matrix3 createScale
(Vector3 scale
)
{
return createScale
(scale
.x, scale
.y, scale
.z);
}
static public Matrix3 createScale
(float s
)
{
return createScale
(s, s, s
);
}
static public Matrix3 createRotationX
(float degrees
)
{
float degreesrad
= MathUtil
.degToRad(degrees
);
float sinvalue
= (float)Math
.Sin(degreesrad
);
float cosvalue
= (float)Math
.Cos(degreesrad
);
Matrix3 matrix
= createIdentity
();
matrix
.values[M22
] = cosvalue
;
matrix
.values[M32
] = -sinvalue
;
matrix
.values[M23
] = sinvalue
;
matrix
.values[M33
] = cosvalue
;
return matrix
;
}
static public Matrix3 createRotationY
(float degrees
)
{
float degreesrad
= MathUtil
.degToRad(degrees
);
float sinvalue
= (float)Math
.Sin(degreesrad
);
float cosvalue
= (float)Math
.Cos(degreesrad
);
Matrix3 matrix
= createIdentity
();
matrix
.values[M11
] = cosvalue
;
matrix
.values[M31
] = sinvalue
;
matrix
.values[M13
] = -sinvalue
;
matrix
.values[M33
] = cosvalue
;
return matrix
;
}
static public Matrix3 createRotationZ
(float degrees
)
{
float degreesrad
= MathUtil
.degToRad(degrees
);
float sinvalue
= (float)Math
.Sin(degreesrad
);
float cosvalue
= (float)Math
.Cos(degreesrad
);
Matrix3 matrix
= createIdentity
();
matrix
.values[M11
] = cosvalue
;
matrix
.values[M21
] = -sinvalue
;
matrix
.values[M12
] = sinvalue
;
matrix
.values[M22
] = cosvalue
;
return matrix
;
}
static public Matrix3 createRotation
(Vector3 axis,
float degrees
)
{
float radangle
= MathUtil
.degToRad(degrees
);
float radcos
= (float)Math
.Cos(radangle
);
float radsin
= (float)Math
.Sin(radangle
);
Matrix3 matrix
= new Matrix3
();
matrix
.values[M11
] = radcos
+ axis
.x * axis
.x * (1 - radcos
);
matrix
.values[M21
] = axis
.z * radsin
+ axis
.y * axis
.x * (1 - radcos
);
matrix
.values[M31
] = -axis
.y * radsin
+ axis
.z * axis
.x * (1 - radcos
);
matrix
.values[M12
] = -axis
.z * radsin
+ axis
.x * axis
.y * (1 - radcos
);
matrix
.values[M22
] = radcos
+ axis
.y * axis
.y * (1 - radcos
);
matrix
.values[M32
] = axis
.x * radsin
+ axis
.z * axis
.y * (1 - radcos
);
matrix
.values[M13
] = axis
.y * radsin
+ axis
.x * axis
.z * (1 - radcos
);
matrix
.values[M23
] = -axis
.x * radsin
+ axis
.y * axis
.z * (1 - radcos
);
matrix
.values[M33
] = radcos
+ axis
.z * axis
.z * (1 - radcos
);
return matrix
;
}
static public Matrix3 createFromVectors
(Vector3 forward, Vector3 up, Vector3 right
)
{
forward
.normalize();
up
.normalize();
right
.normalize();
Matrix3 result
= createIdentity
();
result
.values[M11
] = right
.x;
result
.values[M12
] = right
.y;
result
.values[M13
] = right
.z;
result
.values[M21
] = up
.x;
result
.values[M22
] = up
.y;
result
.values[M23
] = up
.z;
result
.values[M31
] = -forward
.x;
result
.values[M32
] = -forward
.y;
result
.values[M33
] = -forward
.z;
return result
;
}
public Vector3 transform
(Vector3 vector
)
{
return new Vector3
(
values
[M11
] * vector
.x + values
[M21
] * vector
.y + values
[M31
] * vector
.z,
values
[M12
] * vector
.x + values
[M22
] * vector
.y + values
[M32
] * vector
.z,
values
[M13
] * vector
.x + values
[M23
] * vector
.y + values
[M33
] * vector
.z);
}
static public void interpolate
(Matrix3 result, Matrix3 a, Matrix3 b,
float ratio
)
{
result
.values[M11
] = a
.values[M11
] + (b
.values[M11
] - a
.values[M11
]) * ratio
;
result
.values[M21
] = a
.values[M21
] + (b
.values[M21
] - a
.values[M21
]) * ratio
;
result
.values[M31
] = a
.values[M31
] + (b
.values[M31
] - a
.values[M31
]) * ratio
;
result
.values[M12
] = a
.values[M12
] + (b
.values[M12
] - a
.values[M12
]) * ratio
;
result
.values[M22
] = a
.values[M22
] + (b
.values[M22
] - a
.values[M22
]) * ratio
;
result
.values[M32
] = a
.values[M32
] + (b
.values[M32
] - a
.values[M32
]) * ratio
;
result
.values[M13
] = a
.values[M13
] + (b
.values[M13
] - a
.values[M13
]) * ratio
;
result
.values[M23
] = a
.values[M23
] + (b
.values[M23
] - a
.values[M23
]) * ratio
;
result
.values[M33
] = a
.values[M33
] + (b
.values[M33
] - a
.values[M33
]) * ratio
;
}
static public Matrix3 interpolate
(Matrix3 a, Matrix3 b,
float ratio
)
{
Matrix3 result
= createIdentity
();
interpolate
(result, a, b, ratio
);
return result
;
}
public Vector3 getForwardVector
()
{
Vector3 forward
= new Vector3
(-values
[M31
],
-values
[M32
],
-values
[M33
]);
forward
.normalize();
return forward
;
}
public Vector3 getUpVector
()
{
Vector3 up
= new Vector3
(values
[M21
], values
[M22
], values
[M23
]);
up
.normalize();
return up
;
}
public Vector3 getRightVector
()
{
Vector3 right
= new Vector3
(values
[M11
], values
[M12
], values
[M13
]);
right
.normalize();
return right
;
}
public float[] toGLMatrix
()
{
return values
;
}
}
}