Imports System.Runtime.InteropServices

Module Math
	<DllImport("user32.dll")> _
	Private Function GetAsyncKeyState(ByVal vKey As System.Windows.Forms.Keys) As Short
	End Function

	Friend Structure Vector2
		Dim X As Single
		Dim Y As Single
	End Structure

	Friend Structure Vector3
		Dim X As Single
		Dim Y As Single
		Dim Z As Single
	End Structure

	Friend Structure Vector4
		Dim X As Single
		Dim Y As Single
		Dim Z As Single
		Dim W As Single
	End Structure

	Friend Structure Line2
		Dim StartPoint As Vector2
		Dim EndPoint As Vector2
	End Structure

	Friend Structure Line3
		Dim StartPoint As Vector3
		Dim EndPoint As Vector3
	End Structure

	Friend Structure Line4
		Dim StartPoint As Vector4
		Dim EndPoint As Vector4
	End Structure

	Friend Structure Triangle
		Dim A As Vector3
		Dim B As Vector3
		Dim C As Vector3
	End Structure

	Friend Structure Box
		Dim Min As Vector3
		Dim Max As Vector3
	End Structure

	Friend Structure Matrix3
		Dim XAxis As Vector3
		Dim YAxis As Vector3
		Dim ZAxis As Vector3
	End Structure

	Friend Structure Matrix4
		Dim XAxis As Vector4
		Dim YAxis As Vector4
		Dim ZAxis As Vector4
		Dim WAxis As Vector4
	End Structure

	Friend Structure RectUV
		Dim MinU As Single
		Dim MaxU As Single
		Dim MinV As Single
		Dim MaxV As Single
	End Structure

	Friend Const PI As Single = 3.14159265358979
	Friend Const DegreesPerRadian As Single = 57.2957795130823 '180 / PI
	Friend Const RadiansPerDegree As Single = 0.0174532925199433 'PI / 180

	Friend Function KeyDown_Truth(vKey As Long) As Boolean
		KeyDown_Truth = (GetAsyncKeyState(vKey) < 0)
	End Function

	Friend Function KeyDown_ZeroToOne(vKey As Long) As Single
		If GetAsyncKeyState(vKey) < 0 Then
			KeyDown_ZeroToOne = 1
		Else
			KeyDown_ZeroToOne = 0
		End If
	End Function

	Friend Function KeyDown_OneToZero(vKey As Long) As Single
		If GetAsyncKeyState(vKey) < 0 Then
			KeyDown_OneToZero = 0
		Else
			KeyDown_OneToZero = 1
		End If
	End Function

	Friend Function SafeSingleToByte(X As Single) As Byte
		Dim Rounded As Long
		Rounded = X * 255
		If Rounded < 0 Then
			Rounded = 0
		ElseIf Rounded > 255 Then
			Rounded = 255
		End If
		SafeSingleToByte = Rounded
	End Function

	Friend Function GaussianRandom() As Single
		GaussianRandom = ((Rnd() + Rnd() + Rnd() + Rnd() + Rnd() + Rnd() + Rnd() + Rnd() + Rnd() + Rnd()) / 5) - 1
	End Function

	Friend Function GaussianRandom3D(Multiplier As Single) As Vector3
		GaussianRandom3D.X = GaussianRandom() * Multiplier
		GaussianRandom3D.Y = GaussianRandom() * Multiplier
		GaussianRandom3D.Z = GaussianRandom() * Multiplier
	End Function

	Friend Function Min_Float(A As Single, B As Single) As Single
		If A < B Then
			Min_Float = A
		Else
			Min_Float = B
		End If
	End Function

	Friend Function Max_Float(A As Single, B As Single) As Single
		If A > B Then
			Max_Float = A
		Else
			Max_Float = B
		End If
	End Function

	Friend Function Min_Int(A As Integer, B As Integer) As Integer
		If A < B Then
			Min_Int = A
		Else
			Min_Int = B
		End If
	End Function

	Friend Function Max_Int(A As Integer, B As Integer) As Integer
		If A > B Then
			Max_Int = A
		Else
			Max_Int = B
		End If
	End Function

	Friend Function Saturate(X As Single) As Single
		If X > 1 Then
			Saturate = 1
		ElseIf X < 0 Then
			Saturate = 0
		Else
			Saturate = X
		End If
	End Function

	Friend Function Clamp(Min As Single, X As Single, Max As Single) As Single
		If X > Max Then
			Clamp = Max
		ElseIf X < Min Then
			Clamp = Min
		Else
			Clamp = X
		End If
	End Function

	Friend Function Cubic(X As Single) As Single
		Cubic = (3 * (X ^ 2)) - (2 * (X ^ 3))
	End Function

	Friend Function GenerateNormalFromPoints(A As Vector3, B As Vector3, C As Vector3) As Vector3
		GenerateNormalFromPoints = NormalVector3(CrosProduct(ToVector3(A, C), ToVector3(A, B)))
	End Function

	Friend Function MakeRectUV(ByVal MinU As Single, ByVal MaxU As Single, ByVal MinV As Single, ByVal MaxV As Single) As RectUV
		MakeRectUV.MinU = MinU
		MakeRectUV.MaxU = MaxU
		MakeRectUV.MinV = MinV
		MakeRectUV.MaxV = MaxV
	End Function

	Friend Function MakeVector2(X As Single, Y As Single) As Vector2
		MakeVector2.X = X
		MakeVector2.Y = Y
	End Function

	Friend Function MakeVector3(X As Single, Y As Single, Z As Single) As Vector3
		MakeVector3.X = X
		MakeVector3.Y = Y
		MakeVector3.Z = Z
	End Function

	Friend Function MakeVector4(X As Single, Y As Single, Z As Single, W As Single) As Vector4
		MakeVector4.X = X
		MakeVector4.Y = Y
		MakeVector4.Z = Z
		MakeVector4.W = W
	End Function

	Friend Function Vec2_Bezier3(A As Vector2, B As Vector2, C As Vector2, Ratio As Single) As Vector2
		Vec2_Bezier3 = LerpVector2(LerpVector2(A, B, Ratio), LerpVector2(B, C, Ratio), Ratio)
	End Function

	Friend Function Vec3_Bezier3(A As Vector3, B As Vector3, C As Vector3, Ratio As Single) As Vector3
		Vec3_Bezier3 = LerpVector3(LerpVector3(A, B, Ratio), LerpVector3(B, C, Ratio), Ratio)
	End Function

	Friend Function Vec2_Bezier4(A As Vector2, B As Vector2, C As Vector2, D As Vector2, Ratio As Single) As Vector2
		Dim BC As Vector2
		BC = LerpVector2(B, C, Ratio)
		Vec2_Bezier4 = LerpVector2(LerpVector2(LerpVector2(A, B, Ratio), BC, Ratio), LerpVector2(BC, LerpVector2(C, D, Ratio), Ratio), Ratio)
	End Function

	Friend Function Vec3_Bezier4(A As Vector3, B As Vector3, C As Vector3, D As Vector3, Ratio As Single) As Vector3
		Dim BC As Vector3
		BC = LerpVector3(B, C, Ratio)
		Vec3_Bezier4 = LerpVector3(LerpVector3(LerpVector3(A, B, Ratio), BC, Ratio), LerpVector3(BC, LerpVector3(C, D, Ratio), Ratio), Ratio)
	End Function

	Friend Function MakeLine2(StartPoint As Vector2, EndPoint As Vector2) As Line2
		MakeLine2.StartPoint = StartPoint
		MakeLine2.EndPoint = EndPoint
	End Function

	Friend Function MakeLine3(StartPoint As Vector3, EndPoint As Vector3) As Line3
		MakeLine3.StartPoint = StartPoint
		MakeLine3.EndPoint = EndPoint
	End Function

	Friend Function MakeLine4(StartPoint As Vector4, EndPoint As Vector4) As Line4
		MakeLine4.StartPoint = StartPoint
		MakeLine4.EndPoint = EndPoint
	End Function

	Friend Function MakeTriangle(A As Vector3, B As Vector3, C As Vector3) As Triangle
		MakeTriangle.A = A
		MakeTriangle.B = B
		MakeTriangle.C = C
	End Function

	Friend Function DirToAngle(Dir As Vector2) As Single
		If Dir.X <> 0 Or Dir.Y <> 0 Then
			If System.Math.Abs(Dir.X) > System.Math.Abs(Dir.Y) Then
				'X is largest
				If Dir.X > 0 Then
					'Right
					DirToAngle = System.Math.Atan(Dir.Y / Dir.X) + (PI / 2)	'+90
				Else
					'Left
					DirToAngle = System.Math.Atan(Dir.Y / Dir.X) - (PI / 2)	'-90
				End If
			Else
				'Y is largest
				If Dir.Y > 0 Then
					'Down
					If Dir.X > 0 Then
						'Down right
						DirToAngle = System.Math.Atan(-(Dir.X / Dir.Y)) + PI '+180
					Else
						'Down left
						DirToAngle = System.Math.Atan(-(Dir.X / Dir.Y)) - PI '-180
					End If
				Else
					'Up
					DirToAngle = System.Math.Atan(-Dir.X / Dir.Y)
				End If
			End If
		Else
			'When not having a direction, the result is usually multiplied by zero so that it does not matter as long as it does not crash
			DirToAngle = 0
		End If
	End Function

	'-PI <= FixRadianCenter <= PI if it only need 360 degrees to get there
	Friend Function FixRadianCenter(ByVal Radian As Single) As Single
		If Radian > PI Then
			FixRadianCenter = Radian - (PI * 2)
		ElseIf Radian < -PI Then
			FixRadianCenter = Radian + (PI * 2)
		Else
			FixRadianCenter = Radian
		End If
	End Function

	Friend Function MidpointLine2(L As Line2) As Vector2
		MidpointLine2 = MiddleVector2(L.StartPoint, L.EndPoint)
	End Function

	Friend Function Line2Direction(L As Line2) As Vector2
		Line2Direction = NormalVector2(ToVector2(L.StartPoint, L.EndPoint))
	End Function

	Friend Function IntersectLine2(A As Line2, B As Line2) As Vector2
		'On Error Resume Next
		Dim OffsetA As Vector2
		Dim OffsetB As Vector2
		Dim DirA As Vector2
		Dim DirB As Vector2
		Dim VertA As Boolean
		Dim VertB As Boolean
		Dim HoriA As Boolean
		Dim HoriB As Boolean
		OffsetA = ToVector2(A.StartPoint, A.EndPoint)
		OffsetB = ToVector2(B.StartPoint, B.EndPoint)
		VertA = (System.Math.Abs(OffsetA.X) < 0.0001)
		VertB = (System.Math.Abs(OffsetB.X) < 0.0001)
		HoriA = (System.Math.Abs(OffsetA.Y) < 0.0001)
		HoriB = (System.Math.Abs(OffsetB.Y) < 0.0001)
		If (VertA And HoriA) Or (VertB And HoriB) Then
			IntersectLine2 = MiddleVector2(MidpointLine2(A), MidpointLine2(B))
			Debug.Print("IntersectLine2: One of the lines was a point")
			Exit Function
		End If
		DirA = NormalVector2(OffsetA)
		DirB = NormalVector2(OffsetB)
		If DistVector2(DirA, DirB) < 0.001 Then
			IntersectLine2 = MiddleVector2(LerpVector2(A.StartPoint, A.EndPoint, 100000), LerpVector2(B.StartPoint, B.EndPoint, 100000))
			Exit Function
		ElseIf DistVector2(DirA, NegVector2(DirB)) < 0.001 Then
			IntersectLine2 = MiddleVector2(LerpVector2(A.StartPoint, A.EndPoint, 100000), LerpVector2(B.StartPoint, B.EndPoint, -100000))
			Exit Function
		End If

		If VertA Then
			'A is vertical
			IntersectLine2 = LerpVector2(B.StartPoint, B.EndPoint, InverseLerp(B.StartPoint.X, B.EndPoint.X, A.StartPoint.X))
		ElseIf HoriA Then
			'A is horizontal
			IntersectLine2 = LerpVector2(B.StartPoint, B.EndPoint, InverseLerp(B.StartPoint.Y, B.EndPoint.Y, A.StartPoint.Y))
		ElseIf VertB Then
			'B is vertical
			IntersectLine2 = LerpVector2(A.StartPoint, A.EndPoint, InverseLerp(A.StartPoint.X, A.EndPoint.X, B.StartPoint.X))
		ElseIf HoriB Then
			'B is horizontal
			IntersectLine2 = LerpVector2(A.StartPoint, A.EndPoint, InverseLerp(A.StartPoint.Y, A.EndPoint.Y, B.StartPoint.Y))
		Else
			Dim YSA As Single 'Y at X = 0 for line A
			Dim YSB As Single 'Y at X = 0 for line B
			Dim YSC As Single 'Y at X = 0 for line A - B
			Dim YKA As Single 'Y coefficient for line A
			Dim YKB As Single 'Y coefficient for line B
			Dim YKC As Single 'Y coefficient for line A - B
			Dim X As Single
			YKA = (A.EndPoint.Y - A.StartPoint.Y) / (A.EndPoint.X - A.StartPoint.X)
			YKB = (B.EndPoint.Y - B.StartPoint.Y) / (B.EndPoint.X - B.StartPoint.X)
			YSA = A.StartPoint.Y - (YKA * A.StartPoint.X)
			YSB = B.StartPoint.Y - (YKB * B.StartPoint.X)
			'Find X and calculate Y from one of the line X->Y functions
			'C = A - B
			YSC = YSA - YSB
			YKC = YKA - YKB
			'C = 0 gives X for A = B
			X = YSC / -YKC
			'X gives Y
			IntersectLine2 = MakeVector2(X, YSA + (YKA * X))
		End If
	End Function

	Friend Function Vector4ToVector3(A As Vector4) As Vector3
		Vector4ToVector3.X = A.X
		Vector4ToVector3.Y = A.Y
		Vector4ToVector3.Z = A.Z
	End Function

	Friend Function Vector3ToVector4(A As Vector3, W As Single) As Vector4
		Vector3ToVector4.X = A.X
		Vector3ToVector4.Y = A.Y
		Vector3ToVector4.Z = A.Z
		Vector3ToVector4.W = W
	End Function

	Friend Function Vector4ToVector2(A As Vector4) As Vector2
		Vector4ToVector2.X = A.X
		Vector4ToVector2.Y = A.Y
	End Function

	Friend Function Vector3ToVector2(A As Vector3) As Vector2
		Vector3ToVector2.X = A.X
		Vector3ToVector2.Y = A.Y
	End Function

	Friend Function Matrix4ToMatrix3(A As Matrix4) As Matrix3
		Matrix4ToMatrix3.XAxis = Vector4ToVector3(A.XAxis)
		Matrix4ToMatrix3.YAxis = Vector4ToVector3(A.YAxis)
		Matrix4ToMatrix3.ZAxis = Vector4ToVector3(A.ZAxis)
	End Function

	Friend Function PositionFromMatrix4(A As Matrix4) As Vector3
		PositionFromMatrix4 = Vector4ToVector3(A.WAxis)
	End Function

	Friend Function TransposeMatrix3(M As Matrix3) As Matrix3
		TransposeMatrix3.XAxis.X = M.XAxis.X
		TransposeMatrix3.XAxis.Y = M.YAxis.X
		TransposeMatrix3.XAxis.Z = M.ZAxis.X
		TransposeMatrix3.YAxis.X = M.XAxis.Y
		TransposeMatrix3.YAxis.Y = M.YAxis.Y
		TransposeMatrix3.YAxis.Z = M.ZAxis.Y
		TransposeMatrix3.ZAxis.X = M.XAxis.Z
		TransposeMatrix3.ZAxis.Y = M.YAxis.Z
		TransposeMatrix3.ZAxis.Z = M.ZAxis.Z
	End Function

	'Returns 0 when Value = A
	'Returns 0.5 when Value = (A + B) / 2
	'Returns 1 when Value = B
	Friend Function InverseLerp(ByVal A As Single, ByVal B As Single, ByVal Value As Single) As Single
		Dim C As Single
		C = B - A
		If C = 0 Then
			InverseLerp = 0.5
		Else
			InverseLerp = (Value - A) / (B - A)
		End If
	End Function

	'Returns A when Ratio = 0
	'Returns (A + B) / 2 when Ratio = 0.5
	'Returns B when Ratio = 1
	Friend Function Lerp(ByVal A As Single, ByVal B As Single, ByVal Ratio As Single) As Single
		Lerp = (A * (1 - Ratio)) + (B * Ratio)
	End Function

	Friend Function LerpVector2(A As Vector2, B As Vector2, Ratio As Single) As Vector2
		LerpVector2.X = Lerp(A.X, B.X, Ratio)
		LerpVector2.Y = Lerp(A.Y, B.Y, Ratio)
	End Function

	Friend Function LerpVector3(A As Vector3, B As Vector3, Ratio As Single) As Vector3
		LerpVector3.X = Lerp(A.X, B.X, Ratio)
		LerpVector3.Y = Lerp(A.Y, B.Y, Ratio)
		LerpVector3.Z = Lerp(A.Z, B.Z, Ratio)
	End Function

	Friend Function LerpVector4(A As Vector4, B As Vector4, Ratio As Single) As Vector4
		LerpVector4.X = Lerp(A.X, B.X, Ratio)
		LerpVector4.Y = Lerp(A.Y, B.Y, Ratio)
		LerpVector4.Z = Lerp(A.Z, B.Z, Ratio)
		LerpVector4.W = Lerp(A.W, B.W, Ratio)
	End Function

	Friend Function RotationMatrixX(Radians As Single) As Matrix3
		RotationMatrixX.XAxis = MakeVector3(1, 0, 0)
		RotationMatrixX.YAxis = MakeVector3(0, System.Math.Cos(Radians), System.Math.Sin(Radians))
		RotationMatrixX.ZAxis = MakeVector3(0, -System.Math.Sin(Radians), System.Math.Cos(Radians))
	End Function

	Friend Function RotationMatrixY(Radians As Double) As Matrix3
		RotationMatrixY.XAxis = MakeVector3(System.Math.Cos(Radians), 0, -System.Math.Sin(Radians))
		RotationMatrixY.YAxis = MakeVector3(0, 1, 0)
		RotationMatrixY.ZAxis = MakeVector3(System.Math.Sin(Radians), 0, System.Math.Cos(Radians))
	End Function

	Friend Function RotationMatrixZ(Radians As Single) As Matrix3
		RotationMatrixZ.XAxis = MakeVector3(System.Math.Cos(Radians), System.Math.Sin(Radians), 0)
		RotationMatrixZ.YAxis = MakeVector3(-System.Math.Sin(Radians), System.Math.Cos(Radians), 0)
		RotationMatrixZ.ZAxis = MakeVector3(0, 0, 1)
	End Function

	Friend Function RoundDirection(Dir As Vector3) As Vector3
		If Dir.X > 0.92387953251129 Then
			RoundDirection = MakeVector3(1, 0, 0)
		ElseIf Dir.X < -0.92387953251129 Then
			RoundDirection = MakeVector3(-1, 0, 0)
		Else
			If Dir.Z > 0.92387953251129 Then
				RoundDirection = MakeVector3(0, 0, 1)
			ElseIf Dir.Z < -0.92387953251129 Then
				RoundDirection = MakeVector3(0, 0, -1)
			Else
				If Dir.X > 0 Then
					If Dir.Z > 0 Then
						RoundDirection = NormalVector3(MakeVector3(1, 0, 1))
					Else
						RoundDirection = NormalVector3(MakeVector3(1, 0, -1))
					End If
				Else
					If Dir.Z > 0 Then
						RoundDirection = NormalVector3(MakeVector3(-1, 0, 1))
					Else
						RoundDirection = NormalVector3(MakeVector3(-1, 0, -1))
					End If
				End If
			End If
		End If
	End Function

	Friend Function MakeBox_FromVectors(Min As Vector3, Max As Vector3) As Box
		MakeBox_FromVectors.Min.X = Min.X
		MakeBox_FromVectors.Min.Y = Min.Y
		MakeBox_FromVectors.Min.Z = Min.Z
		MakeBox_FromVectors.Max.X = Max.X
		MakeBox_FromVectors.Max.Y = Max.Y
		MakeBox_FromVectors.Max.Z = Max.Z
	End Function

	Friend Function MakeBox_FromArgs(MinX As Single, MinY As Single, MinZ As Single, MaxX As Single, MaxY As Single, MaxZ As Single) As Box
		MakeBox_FromArgs.Min.X = MinX
		MakeBox_FromArgs.Min.Y = MinY
		MakeBox_FromArgs.Min.Z = MinZ
		MakeBox_FromArgs.Max.X = MaxX
		MakeBox_FromArgs.Max.Y = MaxY
		MakeBox_FromArgs.Max.Z = MaxZ
	End Function

	Friend Function MakeMatrix3(XAxis As Vector3, YAxis As Vector3, ZAxis As Vector3) As Matrix3
		MakeMatrix3.XAxis = XAxis
		MakeMatrix3.YAxis = YAxis
		MakeMatrix3.ZAxis = ZAxis
	End Function

	Friend Function MakeMatrix4(XAxis As Vector4, YAxis As Vector4, ZAxis As Vector4, WAxis As Vector4) As Matrix4
		MakeMatrix4.XAxis = XAxis
		MakeMatrix4.YAxis = YAxis
		MakeMatrix4.ZAxis = ZAxis
		MakeMatrix4.WAxis = WAxis
	End Function

	Friend Function MakeUnitMatrix3() As Matrix3
		MakeUnitMatrix3.XAxis = MakeVector3(1, 0, 0)
		MakeUnitMatrix3.YAxis = MakeVector3(0, 1, 0)
		MakeUnitMatrix3.ZAxis = MakeVector3(0, 0, 1)
	End Function

	Friend Function MakeUnitMatrix4() As Matrix4
		MakeUnitMatrix4.XAxis = MakeVector4(1, 0, 0, 0)
		MakeUnitMatrix4.YAxis = MakeVector4(0, 1, 0, 0)
		MakeUnitMatrix4.ZAxis = MakeVector4(0, 0, 1, 0)
		MakeUnitMatrix4.WAxis = MakeVector4(0, 0, 0, 1)
	End Function

	Friend Function CrosProduct(A As Vector3, B As Vector3) As Vector3
		CrosProduct.X = A.Y * B.Z - A.Z * B.Y
		CrosProduct.Y = A.Z * B.X - A.X * B.Z
		CrosProduct.Z = A.X * B.Y - A.Y * B.X
	End Function

	Friend Function DotProduct2(A As Vector2, B As Vector2) As Single
		DotProduct2 = (A.X * B.X) + (A.Y * B.Y)
	End Function

	Friend Function DotProduct3(A As Vector3, B As Vector3) As Single
		DotProduct3 = (A.X * B.X) + (A.Y * B.Y) + (A.Z * B.Z)
	End Function

	Friend Function DistVector2(A As Vector2, B As Vector2) As Single
		DistVector2 = AbsVector2(SubVector2(A, B))
	End Function

	Friend Function DistVector3(A As Vector3, B As Vector3) As Single
		DistVector3 = AbsVector3(SubVector3(A, B))
	End Function

	Friend Function DistVector4(A As Vector4, B As Vector4) As Single
		DistVector4 = AbsVector4(SubVector4(A, B))
	End Function

	Friend Function MulMatMat3(A As Matrix3, B As Matrix3) As Matrix3
		MulMatMat3.XAxis = MulVecMat3(A.XAxis, B)
		MulMatMat3.YAxis = MulVecMat3(A.YAxis, B)
		MulMatMat3.ZAxis = MulVecMat3(A.ZAxis, B)
	End Function

	Friend Function MulVecMat3(V As Vector3, M As Matrix3) As Vector3
		MulVecMat3.X = (V.X * M.XAxis.X) + (V.Y * M.YAxis.X) + (V.Z * M.ZAxis.X)
		MulVecMat3.Y = (V.X * M.XAxis.Y) + (V.Y * M.YAxis.Y) + (V.Z * M.ZAxis.Y)
		MulVecMat3.Z = (V.X * M.XAxis.Z) + (V.Y * M.YAxis.Z) + (V.Z * M.ZAxis.Z)
	End Function

	Friend Function MulMatMat4(A As Matrix4, B As Matrix4) As Matrix4
		MulMatMat4.XAxis = MulVecMat4(A.XAxis, B)
		MulMatMat4.YAxis = MulVecMat4(A.YAxis, B)
		MulMatMat4.ZAxis = MulVecMat4(A.ZAxis, B)
		MulMatMat4.WAxis = MulVecMat4(A.WAxis, B)
	End Function

	Friend Function MulVecMat4(V As Vector4, M As Matrix4) As Vector4
		MulVecMat4.X = (V.X * M.XAxis.X) + (V.Y * M.YAxis.X) + (V.Z * M.ZAxis.X) + (V.W * M.ZAxis.X)
		MulVecMat4.Y = (V.X * M.XAxis.Y) + (V.Y * M.YAxis.Y) + (V.Z * M.ZAxis.Y) + (V.W * M.ZAxis.Y)
		MulVecMat4.Z = (V.X * M.XAxis.Z) + (V.Y * M.YAxis.Z) + (V.Z * M.ZAxis.Z) + (V.W * M.ZAxis.Z)
		MulVecMat4.W = (V.X * M.XAxis.W) + (V.Y * M.YAxis.W) + (V.Z * M.ZAxis.W) + (V.W * M.ZAxis.W)
	End Function

	Friend Function DeterminantMat4(M As Matrix4) As Single
		DeterminantMat4 = _
			M.XAxis.X * M.YAxis.Y * M.ZAxis.Z * M.WAxis.W - M.XAxis.X * M.YAxis.Y * M.ZAxis.W * M.WAxis.Z - M.XAxis.X * M.ZAxis.Y * M.YAxis.Z * M.WAxis.W + M.XAxis.X * M.ZAxis.Y * M.YAxis.W * M.WAxis.Z _
		  + M.XAxis.X * M.WAxis.Y * M.YAxis.Z * M.ZAxis.W - M.XAxis.X * M.WAxis.Y * M.YAxis.W * M.ZAxis.Z - M.YAxis.X * M.XAxis.Y * M.ZAxis.Z * M.WAxis.W + M.YAxis.X * M.XAxis.Y * M.ZAxis.W * M.WAxis.Z _
		  + M.YAxis.X * M.ZAxis.Y * M.XAxis.Z * M.WAxis.W - M.YAxis.X * M.ZAxis.Y * M.XAxis.W * M.WAxis.Z - M.YAxis.X * M.WAxis.Y * M.XAxis.Z * M.ZAxis.W + M.YAxis.X * M.WAxis.Y * M.XAxis.W * M.ZAxis.Z _
		  + M.ZAxis.X * M.XAxis.Y * M.YAxis.Z * M.WAxis.W - M.ZAxis.X * M.XAxis.Y * M.YAxis.W * M.WAxis.Z - M.ZAxis.X * M.YAxis.Y * M.XAxis.Z * M.WAxis.W + M.ZAxis.X * M.YAxis.Y * M.XAxis.W * M.WAxis.Z _
		  + M.ZAxis.X * M.WAxis.Y * M.XAxis.Z * M.YAxis.W - M.ZAxis.X * M.WAxis.Y * M.XAxis.W * M.YAxis.Z - M.WAxis.X * M.XAxis.Y * M.YAxis.Z * M.ZAxis.W + M.WAxis.X * M.XAxis.Y * M.YAxis.W * M.ZAxis.Z _
		  + M.WAxis.X * M.YAxis.Y * M.XAxis.Z * M.ZAxis.W - M.WAxis.X * M.YAxis.Y * M.XAxis.W * M.ZAxis.Z - M.WAxis.X * M.ZAxis.Y * M.XAxis.Z * M.YAxis.W + M.WAxis.X * M.ZAxis.Y * M.XAxis.W * M.YAxis.Z
	End Function

	'If you happend to know the inverse determinant of M from the data that generated M, this is much faster than using InverseMat4_Simple
	'Precondition:
	'   M.XAxis.w = 0
	'   M.YAxis.w = 0
	'   M.ZAxis.w = 0
	'   M.WAxis.w = 1
	'   InvDet = 1 / DeterminantMat4(M)
	'Post condition:
	'   Returns the inverse matrix of M
	'       so that M * Inverse(M) = UnitMatrix and (X * M) * Inverse(M) = X
	Friend Function InverseMat4_Fast(M As Matrix4, InvDet As Single) As Matrix4
		Debug.Assert(System.Math.Abs(M.XAxis.W) < 0.0001)
		Debug.Assert(System.Math.Abs(M.YAxis.W) < 0.0001)
		Debug.Assert(System.Math.Abs(M.ZAxis.W) < 0.0001)
		Debug.Assert(System.Math.Abs(M.WAxis.W - 1) < 0.0001)
		InverseMat4_Fast.XAxis.X = InvDet * (M.YAxis.Y * M.ZAxis.Z - M.YAxis.Z * M.ZAxis.Y)
		InverseMat4_Fast.XAxis.Y = -InvDet * (M.XAxis.Y * M.ZAxis.Z - M.XAxis.Z * M.ZAxis.Y)
		InverseMat4_Fast.XAxis.Z = InvDet * (M.XAxis.Y * M.YAxis.Z - M.XAxis.Z * M.YAxis.Y)
		InverseMat4_Fast.XAxis.W = 0
		InverseMat4_Fast.YAxis.X = -InvDet * (M.YAxis.X * M.ZAxis.Z - M.YAxis.Z * M.ZAxis.X)
		InverseMat4_Fast.YAxis.Y = InvDet * (M.XAxis.X * M.ZAxis.Z - M.XAxis.Z * M.ZAxis.X)
		InverseMat4_Fast.YAxis.Z = -InvDet * (M.XAxis.X * M.YAxis.Z - M.XAxis.Z * M.YAxis.X)
		InverseMat4_Fast.YAxis.W = 0
		InverseMat4_Fast.ZAxis.X = InvDet * (M.YAxis.X * M.ZAxis.Y - M.YAxis.Y * M.ZAxis.X)
		InverseMat4_Fast.ZAxis.Y = -InvDet * (M.XAxis.X * M.ZAxis.Y - M.XAxis.Y * M.ZAxis.X)
		InverseMat4_Fast.ZAxis.Z = InvDet * (M.XAxis.X * M.YAxis.Y - M.XAxis.Y * M.YAxis.X)
		InverseMat4_Fast.ZAxis.W = 0
		InverseMat4_Fast.WAxis.X = -(M.WAxis.X * InverseMat4_Fast.XAxis.X + M.WAxis.Y * InverseMat4_Fast.YAxis.X + M.WAxis.Z * InverseMat4_Fast.ZAxis.X)
		InverseMat4_Fast.WAxis.Y = -(M.WAxis.X * InverseMat4_Fast.XAxis.Y + M.WAxis.Y * InverseMat4_Fast.YAxis.Y + M.WAxis.Z * InverseMat4_Fast.ZAxis.Y)
		InverseMat4_Fast.WAxis.Z = -(M.WAxis.X * InverseMat4_Fast.XAxis.Z + M.WAxis.Y * InverseMat4_Fast.YAxis.Z + M.WAxis.Z * InverseMat4_Fast.ZAxis.Z)
		InverseMat4_Fast.WAxis.W = 1
	End Function

	'A simpler version of InverseMat4_Fast
	'Precondition:
	'   M.XAxis.w = 0
	'   M.YAxis.w = 0
	'   M.ZAxis.w = 0
	'   M.WAxis.w = 1
	'Post condition:
	'   Returns the inverse matrix of M
	'       so that M * Inverse(M) = UnitMatrix and (X * M) * Inverse(M) = X
	Friend Function InverseMat4_Simple(M As Matrix4) As Matrix4
		InverseMat4_Simple = InverseMat4_Fast(M, 1 / DeterminantMat4(M))
	End Function

	Friend Function NormalVector2(V As Vector2) As Vector2
		Dim Dist As Single
		Dist = AbsVector2(V)
		If Dist = 0 Then
			NormalVector2 = MakeVector2(0, 1)
		Else
			NormalVector2 = DivVector2(V, Dist)
		End If
	End Function

	Friend Function NormalVector3(V As Vector3) As Vector3
		Dim Dist As Single
		Dist = AbsVector3(V)
		If Dist = 0 Then
			NormalVector3 = MakeVector3(0, 0, 1)
		Else
			NormalVector3 = DivVector3(V, Dist)
		End If
	End Function

	Friend Function AbsVector2(V As Vector2) As Single
		AbsVector2 = System.Math.Sqrt((V.X ^ 2) + (V.Y ^ 2))
	End Function

	Friend Function AbsVector3(V As Vector3) As Single
		AbsVector3 = System.Math.Sqrt((V.X ^ 2) + (V.Y ^ 2) + (V.Z ^ 2))
	End Function

	Friend Function AbsVector4(V As Vector4) As Single
		AbsVector4 = System.Math.Sqrt((V.X ^ 2) + (V.Y ^ 2) + (V.Z ^ 2) + (V.W ^ 2))
	End Function

	Friend Function AddMatrix3(A As Matrix3, B As Matrix3) As Matrix3
		AddMatrix3.XAxis = AddVector3(A.XAxis, B.XAxis)
		AddMatrix3.YAxis = AddVector3(A.YAxis, B.YAxis)
		AddMatrix3.ZAxis = AddVector3(A.ZAxis, B.ZAxis)
	End Function

	Friend Function SubMatrix3(A As Matrix3, B As Matrix3) As Matrix3
		SubMatrix3.XAxis = SubVector3(A.XAxis, B.XAxis)
		SubMatrix3.YAxis = SubVector3(A.YAxis, B.YAxis)
		SubMatrix3.ZAxis = SubVector3(A.ZAxis, B.ZAxis)
	End Function

	Friend Function NegVector2(A As Vector2) As Vector2
		NegVector2.X = -A.X
		NegVector2.Y = -A.Y
	End Function

	Friend Function NegVector3(A As Vector3) As Vector3
		NegVector3.X = -A.X
		NegVector3.Y = -A.Y
		NegVector3.Z = -A.Z
	End Function

	Friend Function NegVector4(A As Vector4) As Vector4
		NegVector4.X = -A.X
		NegVector4.Y = -A.Y
		NegVector4.Z = -A.Z
		NegVector4.W = -A.W
	End Function

	Friend Function AddVector2(A As Vector2, B As Vector2) As Vector2
		AddVector2.X = A.X + B.X
		AddVector2.Y = A.Y + B.Y
	End Function

	Friend Function AddVector3(A As Vector3, B As Vector3) As Vector3
		AddVector3.X = A.X + B.X
		AddVector3.Y = A.Y + B.Y
		AddVector3.Z = A.Z + B.Z
	End Function

	Friend Function AddVector4(A As Vector4, B As Vector4) As Vector4
		AddVector4.X = A.X + B.X
		AddVector4.Y = A.Y + B.Y
		AddVector4.Z = A.Z + B.Z
		AddVector4.W = A.W + B.W
	End Function

	Friend Function SubVector2(A As Vector2, B As Vector2) As Vector2
		SubVector2.X = A.X - B.X
		SubVector2.Y = A.Y - B.Y
	End Function

	Friend Function SubVector3(A As Vector3, B As Vector3) As Vector3
		SubVector3.X = A.X - B.X
		SubVector3.Y = A.Y - B.Y
		SubVector3.Z = A.Z - B.Z
	End Function

	Friend Function SubVector4(A As Vector4, B As Vector4) As Vector4
		SubVector4.X = A.X - B.X
		SubVector4.Y = A.Y - B.Y
		SubVector4.Z = A.Z - B.Z
		SubVector4.W = A.W - B.W
	End Function

	Friend Function MulVector2(A As Vector2, S As Single) As Vector2
		MulVector2.X = A.X * S
		MulVector2.Y = A.Y * S
	End Function

	Friend Function MulVector3(A As Vector3, S As Single) As Vector3
		MulVector3.X = A.X * S
		MulVector3.Y = A.Y * S
		MulVector3.Z = A.Z * S
	End Function

	Friend Function MulVector4(A As Vector4, S As Single) As Vector4
		MulVector4.X = A.X * S
		MulVector4.Y = A.Y * S
		MulVector4.Z = A.Z * S
		MulVector4.W = A.W * S
	End Function

	Friend Function DivVector2(A As Vector2, S As Single) As Vector2
		DivVector2.X = A.X / S
		DivVector2.Y = A.Y / S
	End Function

	Friend Function DivVector3(A As Vector3, S As Single) As Vector3
		DivVector3.X = A.X / S
		DivVector3.Y = A.Y / S
		DivVector3.Z = A.Z / S
	End Function

	Friend Function DivVector4(A As Vector4, S As Single) As Vector4
		DivVector4.X = A.X / S
		DivVector4.Y = A.Y / S
		DivVector4.Z = A.Z / S
		DivVector4.W = A.W / S
	End Function

	Friend Function ToVector2(A As Vector2, B As Vector2) As Vector2
		ToVector2.X = B.X - A.X
		ToVector2.Y = B.Y - A.Y
	End Function

	Friend Function ToVector3(A As Vector3, B As Vector3) As Vector3
		ToVector3.X = B.X - A.X
		ToVector3.Y = B.Y - A.Y
		ToVector3.Z = B.Z - A.Z
	End Function

	Friend Function ToVector4(A As Vector4, B As Vector4) As Vector4
		ToVector4.X = B.X - A.X
		ToVector4.Y = B.Y - A.Y
		ToVector4.Z = B.Z - A.Z
		ToVector4.W = B.W - A.W
	End Function

	Friend Function MiddleVector2(A As Vector2, B As Vector2) As Vector2
		MiddleVector2.X = (A.X + B.X) / 2
		MiddleVector2.Y = (A.Y + B.Y) / 2
	End Function

	Friend Function MiddleVector3(A As Vector3, B As Vector3) As Vector3
		MiddleVector3.X = (A.X + B.X) / 2
		MiddleVector3.Y = (A.Y + B.Y) / 2
		MiddleVector3.Z = (A.Z + B.Z) / 2
	End Function

	Friend Function MiddleVector4(A As Vector4, B As Vector4) As Vector4
		MiddleVector4.X = (A.X + B.X) / 2
		MiddleVector4.Y = (A.Y + B.Y) / 2
		MiddleVector4.Z = (A.Z + B.Z) / 2
		MiddleVector4.W = (A.W + B.W) / 2
	End Function

	Friend Function ProjectToPlane(V As Vector3, N As Vector3) As Vector3
		ProjectToPlane = SubVector3(V, MulVector3(N, DotProduct3(V, N)))
	End Function

	Friend Function MulMatVec3(M As Matrix3, S As Vector3) As Matrix3
		MulMatVec3.XAxis = MulVector3(M.XAxis, S.X)
		MulMatVec3.YAxis = MulVector3(M.YAxis, S.Y)
		MulMatVec3.ZAxis = MulVector3(M.ZAxis, S.Z)
	End Function

	Friend Function MakeBoxFromTwoPoints(A As Vector3, B As Vector3) As Box
		MakeBoxFromTwoPoints.Min.X = Min_Float(A.X, B.X)
		MakeBoxFromTwoPoints.Max.X = Max_Float(A.X, B.X)
		MakeBoxFromTwoPoints.Min.Y = Min_Float(A.Y, B.Y)
		MakeBoxFromTwoPoints.Max.Y = Max_Float(A.Y, B.Y)
		MakeBoxFromTwoPoints.Min.Z = Min_Float(A.Z, B.Z)
		MakeBoxFromTwoPoints.Max.Z = Max_Float(A.Z, B.Z)
	End Function

	Friend Sub ExtendBoxUsingPoint(P As Vector3, ByRef B As Box)
		'The box B should be expanded as little as it can to include itself and the point P
		If P.X < B.Min.X Then B.Min.X = P.X
		If P.Y < B.Min.Y Then B.Min.Y = P.Y
		If P.Z < B.Min.Z Then B.Min.Z = P.Z
		If P.X > B.Max.X Then B.Max.X = P.X
		If P.Y > B.Max.Y Then B.Max.Y = P.Y
		If P.Z > B.Max.Z Then B.Max.Z = P.Z
		'P should be in the new B
		Debug.Assert(PointTouchesBox(P, B))
	End Sub

	Friend Function BoxesOverlaps(A As Box, B As Box) As Boolean
		BoxesOverlaps = ( _
		  A.Max.X > B.Min.X And _
		  A.Min.X < B.Max.X And _
		  A.Max.Y > B.Min.Y And _
		  A.Min.Y < B.Max.Y And _
		  A.Max.Z > B.Min.Z And _
		  A.Min.Z < B.Max.Z)
	End Function

	Friend Function BoxesTouches(A As Box, B As Box) As Boolean
		BoxesTouches = ( _
		  A.Max.X >= B.Min.X And _
		  A.Min.X <= B.Max.X And _
		  A.Max.Y >= B.Min.Y And _
		  A.Min.Y <= B.Max.Y And _
		  A.Max.Z >= B.Min.Z And _
		  A.Min.Z <= B.Max.Z)
	End Function

	Friend Function PointIsInBox(P As Vector3, B As Box) As Boolean
		PointIsInBox = ( _
		  P.X > B.Min.X And _
		  P.X < B.Max.X And _
		  P.Y > B.Min.Y And _
		  P.Y < B.Max.Y And _
		  P.Z > B.Min.Z And _
		  P.Z < B.Max.Z)
	End Function

	Friend Function PointTouchesBox(P As Vector3, B As Box) As Boolean
		PointTouchesBox = ( _
		  P.X >= B.Min.X And _
		  P.X <= B.Max.X And _
		  P.Y >= B.Min.Y And _
		  P.Y <= B.Max.Y And _
		  P.Z >= B.Min.Z And _
		  P.Z <= B.Max.Z)
	End Function

	Friend Function MoveBox(P As Vector3, B As Box) As Box
		MoveBox = MakeBox_FromArgs( _
		  P.X + B.Min.X, _
		  P.Y + B.Min.Y, _
		  P.Z + B.Min.Z, _
		  P.X + B.Max.X, _
		  P.Y + B.Max.Y, _
		  P.Z + B.Max.Z)
	End Function

	Friend Function GetBoxCenter(B As Box) As Vector3
		GetBoxCenter = MakeVector3( _
		  ((B.Min.X + B.Max.X) / 2), _
		  ((B.Min.Y + B.Max.Y) / 2), _
		  ((B.Min.Z + B.Max.Z) / 2))
	End Function

	'P is moved to the closest point in B
	Friend Function ClosestPointAtBox(P As Vector3, B As Box) As Vector3
		ClosestPointAtBox = P
		If ClosestPointAtBox.X < B.Min.X Then ClosestPointAtBox.X = B.Min.X
		If ClosestPointAtBox.X > B.Max.X Then ClosestPointAtBox.X = B.Max.X
		If ClosestPointAtBox.Y < B.Min.Y Then ClosestPointAtBox.Y = B.Min.Y
		If ClosestPointAtBox.Y > B.Max.Y Then ClosestPointAtBox.Y = B.Max.Y
		If ClosestPointAtBox.Z < B.Min.Z Then ClosestPointAtBox.Z = B.Min.Z
		If ClosestPointAtBox.Z > B.Max.Z Then ClosestPointAtBox.Z = B.Max.Z
	End Function

	Friend Sub Increase(A As Single, B As Single, S As Single)
		A = A + (B * S)
	End Sub

	Friend Sub IncreaseVector2(A As Vector2, B As Vector2, S As Single)
		Increase(A.X, B.X, S)
		Increase(A.Y, B.Y, S)
	End Sub

	Friend Sub IncreaseVector3(A As Vector3, B As Vector3, S As Single)
		Increase(A.X, B.X, S)
		Increase(A.Y, B.Y, S)
		Increase(A.Z, B.Z, S)
	End Sub

	Friend Sub IncreaseVector4(A As Vector4, B As Vector4, S As Single)
		Increase(A.X, B.X, S)
		Increase(A.Y, B.Y, S)
		Increase(A.Z, B.Z, S)
		Increase(A.W, B.W, S)
	End Sub

	Friend Sub MoveCloserLinear(ByRef RefValue As Single, ByVal Target As Single, ByVal MaxMove As Single)
		If RefValue + MaxMove < Target Then
			RefValue = RefValue + MaxMove
		ElseIf RefValue - MaxMove > Target Then
			RefValue = RefValue - MaxMove
		Else
			RefValue = Target
		End If
	End Sub

	Friend Sub MoveCloserSquareDistance(ByRef RefValue As Single, ByVal Target As Single, ByVal Strength As Single, ByVal Iterations As Long)
		Dim I As Long
		For I = 1 To Iterations
			MoveCloserLinear(RefValue, Target, (Target - RefValue) ^ 2 * (Strength / Iterations))
		Next I
	End Sub
End Module
