2
0
Brian Fiete 4 жил өмнө
parent
commit
39c140f44a

+ 14 - 0
BeefLibs/Beefy2D/src/geom/BoundingBox.bf

@@ -0,0 +1,14 @@
+// This file contains portions of code from the FNA project (github.com/FNA-XNA/FNA),
+// released under the Microsoft Public License
+
+namespace Beefy.geom
+{
+	class BoundingBox
+	{
+		public Vector3 Min;
+		public Vector3 Max;
+		public const int CornerCount = 8;
+
+
+	}
+}

+ 755 - 19
BeefLibs/Beefy2D/src/geom/Matrix4.bf

@@ -55,6 +55,19 @@ namespace Beefy.geom
 		    this.m33 = m33;
 	    }
 
+		public static Matrix4 CreateFromColumnMajor(
+			float m00, float m10, float m20, float m30,
+			float m01, float m11, float m21, float m31,
+			float m02, float m12, float m22, float m32,
+			float m03, float m13, float m23, float m33)
+		{
+			return .(
+				m00, m01, m02, m03,
+				m10, m11, m12, m13,
+				m20, m21, m22, m23,
+				m30, m31, m32, m33);
+		}
+
         public static Matrix4 CreatePerspective(float width, float height, float nearPlaneDistance, float farPlaneDistance)
         {
             Matrix4 matrix;
@@ -190,25 +203,23 @@ namespace Beefy.geom
         public static Matrix4 Multiply(Matrix4 m1, Matrix4 m2)
 	    {
 		    Matrix4 r;
-		    r.m00 = m1.m00 * m2.m00 + m1.m01 * m2.m10 + m1.m02 * m2.m20 + m1.m03 * m2.m30;
-		    r.m01 = m1.m00 * m2.m01 + m1.m01 * m2.m11 + m1.m02 * m2.m21 + m1.m03 * m2.m31;
-		    r.m02 = m1.m00 * m2.m02 + m1.m01 * m2.m12 + m1.m02 * m2.m22 + m1.m03 * m2.m32;
-		    r.m03 = m1.m00 * m2.m03 + m1.m01 * m2.m13 + m1.m02 * m2.m23 + m1.m03 * m2.m33;
-
-		    r.m10 = m1.m10 * m2.m00 + m1.m11 * m2.m10 + m1.m12 * m2.m20 + m1.m13 * m2.m30;
-		    r.m11 = m1.m10 * m2.m01 + m1.m11 * m2.m11 + m1.m12 * m2.m21 + m1.m13 * m2.m31;
-		    r.m12 = m1.m10 * m2.m02 + m1.m11 * m2.m12 + m1.m12 * m2.m22 + m1.m13 * m2.m32;
-		    r.m13 = m1.m10 * m2.m03 + m1.m11 * m2.m13 + m1.m12 * m2.m23 + m1.m13 * m2.m33;
-
-		    r.m20 = m1.m20 * m2.m00 + m1.m21 * m2.m10 + m1.m22 * m2.m20 + m1.m23 * m2.m30;
-		    r.m21 = m1.m20 * m2.m01 + m1.m21 * m2.m11 + m1.m22 * m2.m21 + m1.m23 * m2.m31;
-		    r.m22 = m1.m20 * m2.m02 + m1.m21 * m2.m12 + m1.m22 * m2.m22 + m1.m23 * m2.m32;
-		    r.m23 = m1.m20 * m2.m03 + m1.m21 * m2.m13 + m1.m22 * m2.m23 + m1.m23 * m2.m33;
-
-		    r.m30 = m1.m30 * m2.m00 + m1.m31 * m2.m10 + m1.m32 * m2.m20 + m1.m33 * m2.m30;
-		    r.m31 = m1.m30 * m2.m01 + m1.m31 * m2.m11 + m1.m32 * m2.m21 + m1.m33 * m2.m31;
-		    r.m32 = m1.m30 * m2.m02 + m1.m31 * m2.m12 + m1.m32 * m2.m22 + m1.m33 * m2.m32;
-		    r.m33 = m1.m30 * m2.m03 + m1.m31 * m2.m13 + m1.m32 * m2.m23 + m1.m33 * m2.m33;
+
+			r.m00 = (((m1.m00 * m2.m00) + (m1.m10 * m2.m01)) + (m1.m20 * m2.m02)) + (m1.m30 * m2.m03);
+			r.m10 = (((m1.m00 * m2.m10) + (m1.m10 * m2.m11)) + (m1.m20 * m2.m12)) + (m1.m30 * m2.m13);
+			r.m20 = (((m1.m00 * m2.m20) + (m1.m10 * m2.m21)) + (m1.m20 * m2.m22)) + (m1.m30 * m2.m23);
+			r.m30 = (((m1.m00 * m2.m30) + (m1.m10 * m2.m31)) + (m1.m20 * m2.m32)) + (m1.m30 * m2.m33);
+			r.m01 = (((m1.m01 * m2.m00) + (m1.m11 * m2.m01)) + (m1.m21 * m2.m02)) + (m1.m31 * m2.m03);
+			r.m11 = (((m1.m01 * m2.m10) + (m1.m11 * m2.m11)) + (m1.m21 * m2.m12)) + (m1.m31 * m2.m13);
+			r.m21 = (((m1.m01 * m2.m20) + (m1.m11 * m2.m21)) + (m1.m21 * m2.m22)) + (m1.m31 * m2.m23);
+			r.m31 = (((m1.m01 * m2.m30) + (m1.m11 * m2.m31)) + (m1.m21 * m2.m32)) + (m1.m31 * m2.m33);
+			r.m02 = (((m1.m02 * m2.m00) + (m1.m12 * m2.m01)) + (m1.m22 * m2.m02)) + (m1.m32 * m2.m03);
+			r.m12 = (((m1.m02 * m2.m10) + (m1.m12 * m2.m11)) + (m1.m22 * m2.m12)) + (m1.m32 * m2.m13);
+			r.m22 = (((m1.m02 * m2.m20) + (m1.m12 * m2.m21)) + (m1.m22 * m2.m22)) + (m1.m32 * m2.m23);
+			r.m32 = (((m1.m02 * m2.m30) + (m1.m12 * m2.m31)) + (m1.m22 * m2.m32)) + (m1.m32 * m2.m33);
+			r.m03 = (((m1.m03 * m2.m00) + (m1.m13 * m2.m01)) + (m1.m23 * m2.m02)) + (m1.m33 * m2.m03);
+			r.m13 = (((m1.m03 * m2.m10) + (m1.m13 * m2.m11)) + (m1.m23 * m2.m12)) + (m1.m33 * m2.m13);
+			r.m23 = (((m1.m03 * m2.m20) + (m1.m13 * m2.m21)) + (m1.m23 * m2.m22)) + (m1.m33 * m2.m23);
+			r.m33 = (((m1.m03 * m2.m30) + (m1.m13 * m2.m31)) + (m1.m23 * m2.m32)) + (m1.m33 * m2.m33);
 
 		    return r;
 	    }
@@ -489,5 +500,730 @@ namespace Beefy.geom
                 r20, r21, r22, r23,
                   0, 0, 0, 1);
         }
+
+		public static void Invert(Matrix4 matrix, out Matrix4 result)
+		{
+			float num1 = matrix.m00;
+			float num2 = matrix.m10;
+			float num3 = matrix.m20;
+			float num4 = matrix.m30;
+			float num5 = matrix.m01;
+			float num6 = matrix.m11;
+			float num7 = matrix.m21;
+			float num8 = matrix.m31;
+			float num9 =  matrix.m02;
+			float num10 = matrix.m12;
+			float num11 = matrix.m22;
+			float num12 = matrix.m32;
+			float num13 = matrix.m03;
+			float num14 = matrix.m13;
+			float num15 = matrix.m23;
+			float num16 = matrix.m33;
+			float num17 = (float) ((double) num11 * (double) num16 - (double) num12 * (double) num15);
+			float num18 = (float) ((double) num10 * (double) num16 - (double) num12 * (double) num14);
+			float num19 = (float) ((double) num10 * (double) num15 - (double) num11 * (double) num14);
+			float num20 = (float) ((double) num9 * (double) num16 - (double) num12 * (double) num13);
+			float num21 = (float) ((double) num9 * (double) num15 - (double) num11 * (double) num13);
+			float num22 = (float) ((double) num9 * (double) num14 - (double) num10 * (double) num13);
+			float num23 = (float) ((double) num6 * (double) num17 - (double) num7 * (double) num18 + (double) num8 * (double) num19);
+			float num24 = (float) -((double) num5 * (double) num17 - (double) num7 * (double) num20 + (double) num8 * (double) num21);
+			float num25 = (float) ((double) num5 * (double) num18 - (double) num6 * (double) num20 + (double) num8 * (double) num22);
+			float num26 = (float) -((double) num5 * (double) num19 - (double) num6 * (double) num21 + (double) num7 * (double) num22);
+			float num27 = (float) (1.0 / ((double) num1 * (double) num23 + (double) num2 * (double) num24 + (double) num3 * (double) num25 + (double) num4 * (double) num26));
+			
+			result.m00 = num23 * num27;
+			result.m01 = num24 * num27;
+			result.m02 = num25 * num27;
+			result.m03 = num26 * num27;
+			result.m10 = (float)(-((double) num2 * (double) num17 - (double) num3 * (double) num18 + (double) num4 * (double) num19) * num27);
+			result.m11 = (float) ((double) num1 * (double) num17 - (double) num3 * (double) num20 + (double) num4 * (double) num21) * num27;
+			result.m12 = (float)(-((double) num1 * (double) num18 - (double) num2 * (double) num20 + (double) num4 * (double) num22) * num27);
+			result.m13 = (float) ((double) num1 * (double) num19 - (double) num2 * (double) num21 + (double) num3 * (double) num22) * num27;
+			float num28 = (float) ((double) num7 * (double) num16 - (double) num8 * (double) num15);
+			float num29 = (float) ((double) num6 * (double) num16 - (double) num8 * (double) num14);
+			float num30 = (float) ((double) num6 * (double) num15 - (double) num7 * (double) num14);
+			float num31 = (float) ((double) num5 * (double) num16 - (double) num8 * (double) num13);
+			float num32 = (float) ((double) num5 * (double) num15 - (double) num7 * (double) num13);
+			float num33 = (float) ((double) num5 * (double) num14 - (double) num6 * (double) num13);
+			result.m20 = (float) ((double) num2 * (double) num28 - (double) num3 * (double) num29 + (double) num4 * (double) num30) * num27;
+			result.m21 = (float)(-((double) num1 * (double) num28 - (double) num3 * (double) num31 + (double) num4 * (double) num32) * num27);
+			result.m22 = (float) ((double) num1 * (double) num29 - (double) num2 * (double) num31 + (double) num4 * (double) num33) * num27;
+			result.m23 = (float)(-((double) num1 * (double) num30 - (double) num2 * (double) num32 + (double) num3 * (double) num33) * num27);
+			float num34 = (float) ((double) num7 * (double) num12 - (double) num8 * (double) num11);
+			float num35 = (float) ((double) num6 * (double) num12 - (double) num8 * (double) num10);
+			float num36 = (float) ((double) num6 * (double) num11 - (double) num7 * (double) num10);
+			float num37 = (float) ((double) num5 * (double) num12 - (double) num8 * (double) num9);
+			float num38 = (float) ((double) num5 * (double) num11 - (double) num7 * (double) num9);
+			float num39 = (float) ((double) num5 * (double) num10 - (double) num6 * (double) num9);
+			result.m30 = (float)(-((double) num2 * (double) num34 - (double) num3 * (double) num35 + (double) num4 * (double) num36) * num27);
+			result.m31 = (float) ((double) num1 * (double) num34 - (double) num3 * (double) num37 + (double) num4 * (double) num38) * num27;
+			result.m32 = (float)(-((double) num1 * (double) num35 - (double) num2 * (double) num37 + (double) num4 * (double) num39) * num27);
+			result.m33 = (float) ((double) num1 * (double) num36 - (double) num2 * (double) num38 + (double) num3 * (double) num39) * num27;
+			
+			/*
+			
+			
+		    ///
+		    // Use Laplace expansion theorem to calculate the inverse of a 4x4 matrix
+		    // 
+		    // 1. Calculate the 2x2 determinants needed the 4x4 determinant based on the 2x2 determinants 
+		    // 3. Create the adjugate matrix, which satisfies: A * adj(A) = det(A) * I
+		    // 4. Divide adjugate matrix with the determinant to find the inverse
+		    
+		    float det1, det2, det3, det4, det5, det6, det7, det8, det9, det10, det11, det12;
+		    float detMatrix;
+		    FindDeterminants(ref matrix, out detMatrix, out det1, out det2, out det3, out det4, out det5, out det6, 
+		                     out det7, out det8, out det9, out det10, out det11, out det12);
+		    
+		    float invDetMatrix = 1f / detMatrix;
+		    
+		    Matrix ret; // Allow for matrix and result to point to the same structure
+		    
+		    ret.M11 = (matrix.M22*det12 - matrix.M23*det11 + matrix.M24*det10) * invDetMatrix;
+		    ret.M12 = (-matrix.M12*det12 + matrix.M13*det11 - matrix.M14*det10) * invDetMatrix;
+		    ret.M13 = (matrix.M42*det6 - matrix.M43*det5 + matrix.M44*det4) * invDetMatrix;
+		    ret.M14 = (-matrix.M32*det6 + matrix.M33*det5 - matrix.M34*det4) * invDetMatrix;
+		    ret.M21 = (-matrix.M21*det12 + matrix.M23*det9 - matrix.M24*det8) * invDetMatrix;
+		    ret.M22 = (matrix.M11*det12 - matrix.M13*det9 + matrix.M14*det8) * invDetMatrix;
+		    ret.M23 = (-matrix.M41*det6 + matrix.M43*det3 - matrix.M44*det2) * invDetMatrix;
+		    ret.M24 = (matrix.M31*det6 - matrix.M33*det3 + matrix.M34*det2) * invDetMatrix;
+		    ret.M31 = (matrix.M21*det11 - matrix.M22*det9 + matrix.M24*det7) * invDetMatrix;
+		    ret.M32 = (-matrix.M11*det11 + matrix.M12*det9 - matrix.M14*det7) * invDetMatrix;
+		    ret.M33 = (matrix.M41*det5 - matrix.M42*det3 + matrix.M44*det1) * invDetMatrix;
+		    ret.M34 = (-matrix.M31*det5 + matrix.M32*det3 - matrix.M34*det1) * invDetMatrix;
+		    ret.M41 = (-matrix.M21*det10 + matrix.M22*det8 - matrix.M23*det7) * invDetMatrix;
+		    ret.M42 = (matrix.M11*det10 - matrix.M12*det8 + matrix.M13*det7) * invDetMatrix;
+		    ret.M43 = (-matrix.M41*det4 + matrix.M42*det2 - matrix.M43*det1) * invDetMatrix;
+		    ret.M44 = (matrix.M31*det4 - matrix.M32*det2 + matrix.M33*det1) * invDetMatrix;
+		    
+		    result = ret;
+		    */
+		}
+
+		public static Matrix4 Invert(Matrix4 matrix)
+		{
+		    Invert(matrix, var outMatrix);
+		    return outMatrix;
+		}
+
+		/// <summary>
+		/// Decomposes this matrix to translation, rotation and scale elements. Returns <c>true</c> if matrix can be decomposed; <c>false</c> otherwise.
+		/// </summary>
+		/// <param name="scale">Scale vector as an output parameter.
+		/// <param name="rotation">Rotation quaternion as an output parameter.
+		/// <param name="translation">Translation vector as an output parameter.
+		/// <returns><c>true</c> if matrix can be decomposed; <c>false</c> otherwise.</returns>
+		public bool Decompose(
+			out Vector3 scale,
+			out Quaternion rotation,
+			out Vector3 translation
+		) {
+			translation.mX = m03;
+			translation.mY = m13;
+			translation.mZ = m23;
+
+			float xs = (Math.Sign(m00 * m10 * m20 * m30) < 0) ? -1 : 1;
+			float ys = (Math.Sign(m01 * m11 * m21 * m31) < 0) ? -1 : 1;
+			float zs = (Math.Sign(m02 * m12 * m22 * m32) < 0) ? -1 : 1;
+
+			scale.mX = xs * (float) Math.Sqrt(m00 * m00 + m10 * m10 + m20 * m20);
+			scale.mY = ys * (float) Math.Sqrt(m01 * m01 + m11 * m11 + m21 * m21);
+			scale.mZ = zs * (float) Math.Sqrt(m02 * m02 + m12 * m12 + m22 * m22);
+
+			if (Math.WithinEpsilon(scale.mX, 0.0f) ||
+				Math.WithinEpsilon(scale.mY, 0.0f) ||
+				Math.WithinEpsilon(scale.mZ, 0.0f)	)
+			{
+				rotation = Quaternion.Identity;
+				return false;
+			}
+
+			Matrix4 m1 = Matrix4.CreateFromColumnMajor(
+				m00 / scale.mX, m10 / scale.mX, m20 / scale.mX, 0,
+				m01 / scale.mY, m11 / scale.mY, m21 / scale.mY, 0,
+				m02 / scale.mZ, m12 / scale.mZ, m22 / scale.mZ, 0,
+				0, 0, 0, 1
+			);
+
+			rotation = Quaternion.CreateFromRotationMatrix(m1);
+			return true;
+		}
+
+		/// <summary>
+		/// Returns a determinant of this matrix.
+		/// </summary>
+		/// <returns>Determinant of this matrix</returns>
+		/// <remarks>See more about determinant here - http://en.wikipedia.org/wiki/Determinant.
+		/// </remarks>
+		public float Determinant()
+		{
+			float num18 = (m22 * m33) - (m32 * m23);
+			float num17 = (m12 * m33) - (m32 * m13);
+			float num16 = (m12 * m23) - (m22 * m13);
+			float num15 = (m02 * m33) - (m32 * m03);
+			float num14 = (m02 * m23) - (m22 * m03);
+			float num13 = (m02 * m13) - (m12 * m03);
+			return (
+				(
+					(
+						(m00 * (((m11 * num18) - (m21 * num17)) + (m31 * num16))) -
+						(m10 * (((m01 * num18) - (m21 * num15)) + (m31 * num14)))
+					) + (m20 * (((m01 * num17) - (m11 * num15)) + (m31 * num13)))
+				) - (m30 * (((m01 * num16) - (m11 * num14)) + (m21 * num13)))
+			);
+		}
+
+		/// <summary>
+		/// Creates a new matrix for spherical billboarding that rotates around specified object position.
+		/// </summary>
+		/// <param name="objectPosition">Position of billboard object. It will rotate around that vector.
+		/// <param name="cameraPosition">The camera position.
+		/// <param name="cameraUpVector">The camera up vector.
+		/// <param name="cameraForwardVector">Optional camera forward vector.
+		/// <returns>The matrix for spherical billboarding.</returns>
+		public static Matrix4 CreateBillboard(
+			Vector3 objectPosition,
+			Vector3 cameraPosition,
+			Vector3 cameraUpVector,
+			Nullable<Vector3> cameraForwardVector
+		) {
+			Matrix4 result;
+
+			// Delegate to the other overload of the function to do the work
+			CreateBillboard(
+				objectPosition,
+				cameraPosition,
+				cameraUpVector,
+				cameraForwardVector,
+				out result
+			);
+
+			return result;
+		}
+
+		/// <summary>
+		/// Creates a new matrix for spherical billboarding that rotates around specified object position.
+		/// </summary>
+		/// <param name="objectPosition">Position of billboard object. It will rotate around that vector.
+		/// <param name="cameraPosition">The camera position.
+		/// <param name="cameraUpVector">The camera up vector.
+		/// <param name="cameraForwardVector">Optional camera forward vector.
+		/// <param name="result">The matrix for spherical billboarding as an output parameter.
+		public static void CreateBillboard(
+			Vector3 objectPosition,
+			Vector3 cameraPosition,
+			Vector3 cameraUpVector,
+			Vector3? cameraForwardVector,
+			out Matrix4 result
+		) {
+			Vector3 vector;
+			Vector3 vector2;
+			Vector3 vector3;
+			vector.mX = objectPosition.mX - cameraPosition.mX;
+			vector.mY = objectPosition.mY - cameraPosition.mY;
+			vector.mZ = objectPosition.mZ - cameraPosition.mZ;
+			float num = vector.LengthSquared;
+			if (num < 0.0001f)
+			{
+				vector = cameraForwardVector.HasValue ?
+					-cameraForwardVector.Value :
+					Vector3.Forward;
+			}
+			else
+			{
+				vector *= (float) (1f / ((float) Math.Sqrt((double) num)));
+			}
+			vector3 = Vector3.Cross(cameraUpVector, vector);
+			vector3.Normalize();
+			vector2 = Vector3.Cross(vector, vector3);
+			result.m00 = vector3.mX;
+			result.m10 = vector3.mY;
+			result.m20 = vector3.mZ;
+			result.m30 = 0;
+			result.m01 = vector2.mX;
+			result.m11 = vector2.mY;
+			result.m21 = vector2.mZ;
+			result.m31 = 0;
+			result.m02 = vector.mX;
+			result.m12 = vector.mY;
+			result.m22 = vector.mZ;
+			result.m32 = 0;
+			result.m03 = objectPosition.mX;
+			result.m13 = objectPosition.mY;
+			result.m23 = objectPosition.mZ;
+			result.m33 = 1;
+		}
+
+		/// <summary>
+		/// Creates a new matrix for cylindrical billboarding that rotates around specified axis.
+		/// </summary>
+		/// <param name="objectPosition">Object position the billboard will rotate around.
+		/// <param name="cameraPosition">Camera position.
+		/// <param name="rotateAxis">Axis of billboard for rotation.
+		/// <param name="cameraForwardVector">Optional camera forward vector.
+		/// <param name="objectForwardVector">Optional object forward vector.
+		/// <returns>The matrix for cylindrical billboarding.</returns>
+		public static Matrix4 CreateConstrainedBillboard(
+			Vector3 objectPosition,
+			Vector3 cameraPosition,
+			Vector3 rotateAxis,
+			Nullable<Vector3> cameraForwardVector,
+			Nullable<Vector3> objectForwardVector
+		) {
+			Matrix4 result;
+			CreateConstrainedBillboard(
+				objectPosition,
+				cameraPosition,
+				rotateAxis,
+				cameraForwardVector,
+				objectForwardVector,
+				out result
+			);
+			return result;
+		}
+
+		/// <summary>
+		/// Creates a new matrix for cylindrical billboarding that rotates around specified axis.
+		/// </summary>
+		/// <param name="objectPosition">Object position the billboard will rotate around.
+		/// <param name="cameraPosition">Camera position.
+		/// <param name="rotateAxis">Axis of billboard for rotation.
+		/// <param name="cameraForwardVector">Optional camera forward vector.
+		/// <param name="objectForwardVector">Optional object forward vector.
+		/// <param name="result">The matrix for cylindrical billboarding as an output parameter.
+		public static void CreateConstrainedBillboard(
+			Vector3 objectPosition,
+			Vector3 cameraPosition,
+			Vector3 rotateAxis,
+			Vector3? cameraForwardVector,
+			Vector3? objectForwardVector,
+			out Matrix4 result
+		) {
+			float num;
+			Vector3 vector;
+			Vector3 vector2;
+			Vector3 vector3;
+			vector2.mX = objectPosition.mX - cameraPosition.mX;
+			vector2.mY = objectPosition.mY - cameraPosition.mY;
+			vector2.mZ = objectPosition.mZ - cameraPosition.mZ;
+			float num2 = vector2.LengthSquared;
+			if (num2 < 0.0001f)
+			{
+				vector2 = cameraForwardVector.HasValue ?
+					-cameraForwardVector.Value :
+					Vector3.Forward;
+			}
+			else
+			{
+				vector2 *= (float) (1f / ((float) Math.Sqrt((double) num2)));
+			}
+			Vector3 vector4 = rotateAxis;
+			num = Vector3.Dot(rotateAxis, vector2);
+			if (Math.Abs(num) > 0.9982547f)
+			{
+				if (objectForwardVector.HasValue)
+				{
+					vector = objectForwardVector.Value;
+					num = Vector3.Dot(rotateAxis, vector);
+					if (Math.Abs(num) > 0.9982547f)
+					{
+						num = (
+							(rotateAxis.mX * Vector3.Forward.mX) +
+							(rotateAxis.mY * Vector3.Forward.mY)
+						) + (rotateAxis.mZ * Vector3.Forward.mZ);
+						vector = (Math.Abs(num) > 0.9982547f) ?
+							Vector3.Right :
+							Vector3.Forward;
+					}
+				}
+				else
+				{
+					num = (
+						(rotateAxis.mX * Vector3.Forward.mX) +
+						(rotateAxis.mY * Vector3.Forward.mY)
+					) + (rotateAxis.mZ * Vector3.Forward.mZ);
+					vector = (Math.Abs(num) > 0.9982547f) ?
+						Vector3.Right :
+						Vector3.Forward;
+				}
+				vector3 = Vector3.Cross(rotateAxis, vector);
+				vector3.Normalize();
+				vector = Vector3.Cross(vector3, rotateAxis);
+				vector.Normalize();
+			}
+			else
+			{
+				vector3 = Vector3.Cross(rotateAxis, vector2);
+				vector3.Normalize();
+				vector = Vector3.Cross(vector3, vector4);
+				vector.Normalize();
+			}
+
+			result.m00 = vector3.mX;
+			result.m10 = vector3.mY;
+			result.m20 = vector3.mZ;
+			result.m30 = 0;
+			result.m01 = vector4.mX;
+			result.m11 = vector4.mY;
+			result.m21 = vector4.mZ;
+			result.m31 = 0;
+			result.m02 = vector.mX;
+			result.m12 = vector.mY;
+			result.m22 = vector.mZ;
+			result.m32 = 0;
+			result.m03 = objectPosition.mX;
+			result.m13 = objectPosition.mY;
+			result.m23 = objectPosition.mZ;
+			result.m33 = 1;
+		}
+
+		/// <summary>
+		/// Creates a new matrix which contains the rotation moment around specified axis.
+		/// </summary>
+		/// <param name="axis">The axis of rotation.
+		/// <param name="angle">The angle of rotation in radians.
+		/// <returns>The rotation matrix.</returns>
+		public static Matrix4 CreateFromAxisAngle(Vector3 axis, float angle)
+		{
+			Matrix4 result;
+			CreateFromAxisAngle(axis, angle, out result);
+			return result;
+		}
+
+		/// <summary>
+		/// Creates a new matrix which contains the rotation moment around specified axis.
+		/// </summary>
+		/// <param name="axis">The axis of rotation.
+		/// <param name="angle">The angle of rotation in radians.
+		/// <param name="result">The rotation matrix as an output parameter.
+		public static void CreateFromAxisAngle(
+			Vector3 axis,
+			float angle,
+			out Matrix4 result
+		) {
+			float x = axis.mX;
+			float y = axis.mY;
+			float z = axis.mZ;
+			float num2 = (float) Math.Sin((double) angle);
+			float num = (float) Math.Cos((double) angle);
+			float num11 = x * x;
+			float num10 = y * y;
+			float num9 = z * z;
+			float num8 = x * y;
+			float num7 = x * z;
+			float num6 = y * z;
+			result.m00 = num11 + (num * (1f - num11));
+			result.m10 = (num8 - (num * num8)) + (num2 * z);
+			result.m20 = (num7 - (num * num7)) - (num2 * y);
+			result.m30 = 0;
+			result.m01 = (num8 - (num * num8)) - (num2 * z);
+			result.m11 = num10 + (num * (1f - num10));
+			result.m21 = (num6 - (num * num6)) + (num2 * x);
+			result.m31 = 0;
+			result.m02 = (num7 - (num * num7)) + (num2 * y);
+			result.m12 = (num6 - (num * num6)) - (num2 * x);
+			result.m22 = num9 + (num * (1f - num9));
+			result.m32 = 0;
+			result.m03 = 0;
+			result.m13 = 0;
+			result.m23 = 0;
+			result.m33 = 1;
+		}
+
+		/// <summary>
+		/// Creates a new rotation matrix from a <see cref="Quaternion"/>.
+		/// </summary>
+		/// <param name="quaternion"><see cref="Quaternion"/> of rotation moment.
+		/// <returns>The rotation matrix.</returns>
+		public static Matrix4 CreateFromQuaternion(Quaternion quaternion)
+		{
+			Matrix4 result;
+			CreateFromQuaternion(quaternion, out result);
+			return result;
+		}
+
+		/// <summary>
+		/// Creates a new rotation matrix from a <see cref="Quaternion"/>.
+		/// </summary>
+		/// <param name="quaternion"><see cref="Quaternion"/> of rotation moment.
+		/// <param name="result">The rotation matrix as an output parameter.
+		public static void CreateFromQuaternion(Quaternion quaternion, out Matrix4 result)
+		{
+			float num9 = quaternion.mX * quaternion.mX;
+			float num8 = quaternion.mY * quaternion.mY;
+			float num7 = quaternion.mZ * quaternion.mZ;
+			float num6 = quaternion.mX * quaternion.mY;
+			float num5 = quaternion.mZ * quaternion.mW;
+			float num4 = quaternion.mZ * quaternion.mX;
+			float num3 = quaternion.mY * quaternion.mW;
+			float num2 = quaternion.mY * quaternion.mZ;
+			float num = quaternion.mX * quaternion.mW;
+			result.m00 = 1f - (2f * (num8 + num7));
+			result.m10 = 2f * (num6 + num5);
+			result.m20 = 2f * (num4 - num3);
+			result.m30 = 0f;
+			result.m01 = 2f * (num6 - num5);
+			result.m11 = 1f - (2f * (num7 + num9));
+			result.m21 = 2f * (num2 + num);
+			result.m31 = 0f;
+			result.m02 = 2f * (num4 + num3);
+			result.m12 = 2f * (num2 - num);
+			result.m22 = 1f - (2f * (num8 + num9));
+			result.m32 = 0f;
+			result.m03 = 0f;
+			result.m13 = 0f;
+			result.m23 = 0f;
+			result.m33 = 1f;
+		}
+
+		/// Creates a new rotation matrix from the specified yaw, pitch and roll values.
+		/// @param yaw The yaw rotation value in radians.
+		/// @param pitch The pitch rotation value in radians.
+		/// @param roll The roll rotation value in radians.
+		/// @returns The rotation matrix
+		/// @remarks For more information about yaw, pitch and roll visit http://en.wikipedia.org/wiki/Euler_angles.
+		public static Matrix4 CreateFromYawPitchRoll(float yaw, float pitch, float roll)
+		{
+			Matrix4 matrix;
+			CreateFromYawPitchRoll(yaw, pitch, roll, out matrix);
+			return matrix;
+		}
+
+		/// Creates a new rotation matrix from the specified yaw, pitch and roll values.
+		/// @param yaw The yaw rotation value in radians.
+		/// @param pitch The pitch rotation value in radians.
+		/// @param roll The roll rotation value in radians.
+		/// @param result The rotation matrix as an output parameter.
+		/// @remarks>For more information about yaw, pitch and roll visit http://en.wikipedia.org/wiki/Euler_angles.
+		public static void CreateFromYawPitchRoll(
+			float yaw,
+			float pitch,
+			float roll,
+			out Matrix4 result
+		) {
+			Quaternion quaternion;
+			Quaternion.CreateFromYawPitchRoll(yaw, pitch, roll, out quaternion);
+			CreateFromQuaternion(quaternion, out result);
+		}
+
+		/// Creates a new viewing matrix.
+		/// @param cameraPosition Position of the camera.
+		/// @param cameraTarget Lookup vector of the camera.
+		/// @param cameraUpVector The direction of the upper edge of the camera.
+		/// @returns The viewing matrix.
+		public static Matrix4 CreateLookAt(
+			Vector3 cameraPosition,
+			Vector3 cameraTarget,
+			Vector3 cameraUpVector
+		) {
+			Matrix4 matrix;
+			CreateLookAt(cameraPosition, cameraTarget, cameraUpVector, out matrix);
+			return matrix;
+		}
+
+		/// Creates a new viewing matrix.
+		/// @param cameraPosition Position of the camera.
+		/// @param cameraTarget Lookup vector of the camera.
+		/// @param cameraUpVector The direction of the upper edge of the camera.
+		/// @param result The viewing matrix as an output parameter.
+		public static void CreateLookAt(
+			Vector3 cameraPosition,
+			Vector3 cameraTarget,
+			Vector3 cameraUpVector,
+			out Matrix4 result
+		) {
+			Vector3 vectorA = Vector3.Normalize(cameraPosition - cameraTarget);
+			Vector3 vectorB = Vector3.Normalize(Vector3.Cross(cameraUpVector, vectorA));
+			Vector3 vectorC = Vector3.Cross(vectorA, vectorB);
+			result.m00 = vectorB.mX;
+			result.m10 = vectorC.mX;
+			result.m20 = vectorA.mX;
+			result.m30 = 0f;
+			result.m01 = vectorB.mY;
+			result.m11 = vectorC.mY;
+			result.m21 = vectorA.mY;
+			result.m31 = 0f;
+			result.m02 = vectorB.mZ;
+			result.m12 = vectorC.mZ;
+			result.m22 = vectorA.mZ;
+			result.m32 = 0f;
+			result.m03 = -Vector3.Dot(vectorB, cameraPosition);
+			result.m13 = -Vector3.Dot(vectorC, cameraPosition);
+			result.m23 = -Vector3.Dot(vectorA, cameraPosition);
+			result.m33 = 1f;
+		}
+
+		/// Creates a new projection matrix for orthographic view.
+		/// @param width Width of the viewing volume.
+		/// @param height Height of the viewing volume.
+		/// @param zNearPlane Depth of the near plane.
+		/// @param zFarPlane Depth of the far plane.
+		/// @returns The new projection matrix for orthographic view.</returns>
+		public static Matrix4 CreateOrthographic(
+			float width,
+			float height,
+			float zNearPlane,
+			float zFarPlane
+		) {
+			Matrix4 matrix;
+			CreateOrthographic(width, height, zNearPlane, zFarPlane, out matrix);
+			return matrix;
+		}
+
+		/// Creates a new projection matrix for orthographic view.
+		/// @param width Width of the viewing volume.
+		/// @param height Height of the viewing volume.
+		/// @param zNearPlane Depth of the near plane.
+		/// @param zFarPlane Depth of the far plane.
+		/// @param result The new projection matrix for orthographic view as an output parameter.
+		public static void CreateOrthographic(
+			float width,
+			float height,
+			float zNearPlane,
+			float zFarPlane,
+			out Matrix4 result
+		) {
+			result.m00 = 2f / width;
+			result.m10 = result.m20 = result.m30 = 0f;
+			result.m11 = 2f / height;
+			result.m01 = result.m21 = result.m31 = 0f;
+			result.m22 = 1f / (zNearPlane - zFarPlane);
+			result.m02 = result.m12 = result.m32 = 0f;
+			result.m03 = result.m13 = 0f;
+			result.m23 = zNearPlane / (zNearPlane - zFarPlane);
+			result.m33 = 1f;
+		}
+
+		/// Creates a new projection matrix for customized orthographic view.
+		/// @param left Lower x-value at the near plane.
+		/// @param right Upper x-value at the near plane.
+		/// @param bottom Lower y-coordinate at the near plane.
+		/// @param top Upper y-value at the near plane.
+		/// @param zNearPlane Depth of the near plane.
+		/// @param zFarPlane Depth of the far plane.
+		/// @returns The new projection matrix for customized orthographic view.</returns>
+		public static Matrix4 CreateOrthographicOffCenter(
+			float left,
+			float right,
+			float bottom,
+			float top,
+			float zNearPlane,
+			float zFarPlane
+		) {
+			Matrix4 matrix;
+			CreateOrthographicOffCenter(
+				left,
+				right,
+				bottom,
+				top,
+				zNearPlane,
+				zFarPlane,
+				out matrix
+			);
+			return matrix;
+		}
+
+		/// Creates a new projection matrix for customized orthographic view.
+		/// @param left Lower x-value at the near plane.
+		/// @param right Upper x-value at the near plane.
+		/// @param bottom Lower y-coordinate at the near plane.
+		/// @param top Upper y-value at the near plane.
+		/// @param zNearPlane Depth of the near plane.
+		/// @param zFarPlane Depth of the far plane.
+		/// @param result The new projection matrix for customized orthographic view as an output parameter.
+		public static void CreateOrthographicOffCenter(
+			float left,
+			float right,
+			float bottom,
+			float top,
+			float zNearPlane,
+			float zFarPlane,
+			out Matrix4 result
+		)
+		{
+			result.m00 = (float) (2.0 / ((double) right - (double) left));
+			result.m10 = 0.0f;
+			result.m20 = 0.0f;
+			result.m30 = 0.0f;
+			result.m01 = 0.0f;
+			result.m11 = (float) (2.0 / ((double) top - (double) bottom));
+			result.m21 = 0.0f;
+			result.m31 = 0.0f;
+			result.m02 = 0.0f;
+			result.m12 = 0.0f;
+			result.m22 = (float) (1.0 / ((double) zNearPlane - (double) zFarPlane));
+			result.m32 = 0.0f;
+			result.m03 = (float) (
+				((double) left + (double) right) /
+				((double) left - (double) right)
+			);
+			result.m13 = (float) (
+				((double) top + (double) bottom) /
+				((double) bottom - (double) top)
+			);
+			result.m23 = (float) (
+				(double) zNearPlane /
+				((double) zNearPlane - (double) zFarPlane)
+			);
+			result.m33 = 1.0f;
+		}
+
+		/// Creates a new matrix that flattens geometry into a specified <see cref="Plane"/> as if casting a shadow from a specified light source.
+		/// @param lightDirection A vector specifying the direction from which the light that will cast the shadow is coming.
+		/// @param plane The plane onto which the new matrix should flatten geometry so as to cast a shadow.
+		/// @returns>A matrix that can be used to flatten geometry onto the specified plane from the specified direction.
+		public static Matrix4 CreateShadow(Vector3 lightDirection, Plane plane)
+		{
+			Matrix4 result;
+			result = CreateShadow(lightDirection, plane);
+			return result;
+		}
+
+		/// Creates a new matrix that flattens geometry into a specified <see cref="Plane"/> as if casting a shadow from a specified light source.
+		/// @param lightDirection A vector specifying the direction from which the light that will cast the shadow is coming.
+		/// @param plane The plane onto which the new matrix should flatten geometry so as to cast a shadow.
+		/// @param result A matrix that can be used to flatten geometry onto the specified plane from the specified direction as an output parameter.
+		public static void CreateShadow(
+			Vector3 lightDirection,
+			Plane plane,
+			out Matrix4 result)
+		{
+			float dot = (
+				(plane.Normal.mX * lightDirection.mX) +
+				(plane.Normal.mY * lightDirection.mY) +
+				(plane.Normal.mZ * lightDirection.mZ)
+			);
+			float x = -plane.Normal.mX;
+			float y = -plane.Normal.mY;
+			float z = -plane.Normal.mZ;
+			float d = -plane.D;
+
+			result.m00 = (x * lightDirection.mX) + dot;
+			result.m10 = x * lightDirection.mY;
+			result.m20 = x * lightDirection.mZ;
+			result.m30 = 0;
+			result.m01 = y * lightDirection.mX;
+			result.m11 = (y * lightDirection.mY) + dot;
+			result.m21 = y * lightDirection.mZ;
+			result.m31 = 0;
+			result.m02 = z * lightDirection.mX;
+			result.m12 = z * lightDirection.mY;
+			result.m22 = (z * lightDirection.mZ) + dot;
+			result.m32 = 0;
+			result.m03 = d * lightDirection.mX;
+			result.m13 = d * lightDirection.mY;
+			result.m23 = d * lightDirection.mZ;
+			result.m33 = dot;
+		}
+
+		public override void ToString(System.String strBuffer)
+		{
+			for (int row < 4)
+				for (int col < 4)
+				{
+#unwarn
+					strBuffer.AppendF($"M{row+1}{col+1}:{((float*)&this)[row+col*4]}\n");
+				}
+		}
     }
 }

+ 291 - 0
BeefLibs/Beefy2D/src/geom/Plane.bf

@@ -0,0 +1,291 @@
+// This file contains portions of code from the FNA project (github.com/FNA-XNA/FNA),
+// released under the Microsoft Public License
+
+using System;
+
+namespace Beefy.geom
+{
+	/// <summary>
+	/// Defines the intersection between a <see cref="Plane"/> and a bounding volume.
+	/// </summary>
+	public enum PlaneIntersectionType
+	{
+		/// <summary>
+		/// There is no intersection, the bounding volume is in the negative half space of the plane.
+		/// </summary>
+		Front,
+		/// <summary>
+		/// There is no intersection, the bounding volume is in the positive half space of the plane.
+		/// </summary>
+		Back,
+		/// <summary>
+		/// The plane is intersected.
+		/// </summary>
+		Intersecting
+	}
+
+	public struct Plane
+	{
+		public Vector3 Normal;
+		public float D;
+
+		public this(Vector4 value)
+			: this(Vector3(value.mX, value.mY, value.mZ), value.mW)
+		{
+		}
+
+		public this(Vector3 normal, float d)
+		{
+			Normal = normal;
+			D = d;
+		}
+
+		public this(Vector3 a, Vector3 b, Vector3 c)
+		{
+			Vector3 ab = b - a;
+			Vector3 ac = c - a;
+
+			Vector3 cross = Vector3.Cross(ab, ac);
+			Vector3.Normalize(cross, out Normal);
+			D = -(Vector3.Dot(Normal, a));
+		}
+
+		public this(float a, float b, float c, float d)
+			: this(Vector3(a, b, c), d)
+		{
+
+		}
+
+		public float Dot(Vector4 value)
+		{
+			return (
+				(this.Normal.mX * value.mX) +
+				(this.Normal.mY * value.mY) +
+				(this.Normal.mZ * value.mZ) +
+				(this.D * value.mW)
+			);
+		}
+
+		public void Dot(ref Vector4 value, out float result)
+		{
+			result = (
+				(this.Normal.mX * value.mX) +
+				(this.Normal.mY * value.mY) +
+				(this.Normal.mZ * value.mZ) +
+				(this.D * value.mW)
+			);
+		}
+
+		public float DotCoordinate(Vector3 value)
+		{
+			return (
+				(this.Normal.mX * value.mX) +
+				(this.Normal.mY * value.mY) +
+				(this.Normal.mZ * value.mZ) +
+				this.D
+			);
+		}
+
+		public void DotCoordinate(ref Vector3 value, out float result)
+		{
+			result = (
+				(this.Normal.mX * value.mX) +
+				(this.Normal.mY * value.mY) +
+				(this.Normal.mZ * value.mZ) +
+				this.D
+			);
+		}
+
+		public float DotNormal(Vector3 value)
+		{
+			return (
+				(this.Normal.mX * value.mX) +
+				(this.Normal.mY * value.mY) +
+				(this.Normal.mZ * value.mZ)
+			);
+		}
+
+		public void DotNormal(Vector3 value, out float result)
+		{
+			result = (
+				(this.Normal.mX * value.mX) +
+				(this.Normal.mY * value.mY) +
+				(this.Normal.mZ * value.mZ)
+			);
+		}
+
+		public void Normalize() mut
+		{
+			float length = Normal.Length;
+			float factor = 1.0f / length;
+			Normal = Vector3.Multiply(Normal, factor);
+			D = D * factor;
+		}
+
+		/*public PlaneIntersectionType Intersects(BoundingBox box)
+		{
+			return box.Intersects(this);
+		}
+
+		public void Intersects(ref BoundingBox box, out PlaneIntersectionType result)
+		{
+			box.Intersects(ref this, out result);
+		}
+
+		public PlaneIntersectionType Intersects(BoundingSphere sphere)
+		{
+			return sphere.Intersects(this);
+		}
+
+		public void Intersects(ref BoundingSphere sphere, out PlaneIntersectionType result)
+		{
+			sphere.Intersects(ref this, out result);
+		}
+
+		public PlaneIntersectionType Intersects(BoundingFrustum frustum)
+		{
+			return frustum.Intersects(this);
+		}*/
+
+		#endregion
+
+		#region Internal Methods
+
+		internal PlaneIntersectionType Intersects(ref Vector3 point)
+		{
+			float distance;
+			DotCoordinate(ref point, out distance);
+			if (distance > 0)
+			{
+				return PlaneIntersectionType.Front;
+			}
+			if (distance < 0)
+			{
+				return PlaneIntersectionType.Back;
+			}
+			return PlaneIntersectionType.Intersecting;
+		}
+
+		#endregion
+
+		#region Public Static Methods
+
+		public static Plane Normalize(Plane value)
+		{
+			Plane ret;
+			Normalize(value, out ret);
+			return ret;
+		}
+
+		public static void Normalize(Plane value, out Plane result)
+		{
+			float length = value.Normal.Length;
+			float factor = 1.0f / length;
+			result.Normal = Vector3.Multiply(value.Normal, factor);
+			result.D = value.D * factor;
+		}
+
+		/// <summary>
+		/// Transforms a normalized plane by a matrix.
+		/// </summary>
+		/// <param name="plane">The normalized plane to transform.</param>
+		/// <param name="matrix">The transformation matrix.</param>
+		/// <returns>The transformed plane.</returns>
+		public static Plane Transform(Plane plane, Matrix4 matrix)
+		{
+			Plane result;
+			Transform(plane, matrix, out result);
+			return result;
+		}
+
+		/// <summary>
+		/// Transforms a normalized plane by a matrix.
+		/// </summary>
+		/// <param name="plane">The normalized plane to transform.</param>
+		/// <param name="matrix">The transformation matrix.</param>
+		/// <param name="result">The transformed plane.</param>
+		public static void Transform(
+			Plane plane,
+			Matrix4 matrix,
+			out Plane result
+		) {
+			/* See "Transforming Normals" in
+			 * http://www.glprogramming.com/red/appendixf.html
+			 * for an explanation of how this works.
+			 */
+			Matrix4 transformedMatrix;
+			transformedMatrix = Matrix4.Invert(matrix);
+			transformedMatrix = Matrix4.Transpose(transformedMatrix);
+			Vector4 vector = Vector4(plane.Normal, plane.D);
+			Vector4 transformedVector;
+			Vector4.Transform(
+				vector,
+				transformedMatrix,
+				out transformedVector
+			);
+			result = Plane(transformedVector);
+		}
+
+		/// <summary>
+		/// Transforms a normalized plane by a quaternion rotation.
+		/// </summary>
+		/// <param name="plane">The normalized plane to transform.</param>
+		/// <param name="rotation">The quaternion rotation.</param>
+		/// <returns>The transformed plane.</returns>
+		public static Plane Transform(Plane plane, Quaternion rotation)
+		{
+			Plane result;
+			Transform(plane, rotation, out result);
+			return result;
+		}
+
+		/// <summary>
+		/// Transforms a normalized plane by a quaternion rotation.
+		/// </summary>
+		/// <param name="plane">The normalized plane to transform.</param>
+		/// <param name="rotation">The quaternion rotation.</param>
+		/// <param name="result">The transformed plane.</param>
+		public static void Transform(
+			Plane plane,
+			Quaternion rotation,
+			out Plane result
+		) {
+			result.Normal = Vector3.Transform(
+				plane.Normal,
+				rotation
+			);
+			result.D = plane.D;
+		}
+
+		#endregion
+
+		#region Public Static Operators and Override Methods
+
+		public static bool operator !=(Plane plane1, Plane plane2)
+		{
+			return !plane1.Equals(plane2);
+		}
+
+		public static bool operator ==(Plane plane1, Plane plane2)
+		{
+			return plane1.Equals(plane2);
+		}
+
+		public bool Equals(Plane other)
+		{
+			return (Normal == other.Normal && D == other.D);
+		}
+
+		public int GetHashCode()
+		{
+			return Normal.GetHashCode() ^ D.GetHashCode();
+		}
+
+		public override void ToString(String str)
+		{
+			str.AppendF($"{Normal:{Normal} D:{D}}");
+		}
+
+		#endregion
+	}
+}

+ 47 - 37
BeefLibs/Beefy2D/src/geom/Quaternion.bf

@@ -1,3 +1,6 @@
+// This file contains portions of code from the FNA project (github.com/FNA-XNA/FNA),
+// released under the Microsoft Public License
+
 using System;
 using Beefy.gfx;
 
@@ -189,43 +192,50 @@ namespace Beefy.geom
 
         public static void CreateFromRotationMatrix(ref Matrix4 matrix, out Quaternion result)
         {
-            float num8 = (matrix.m11 + matrix.m22) + matrix.m33;
-            if (num8 > 0f)
-            {
-                float num = (float)Math.Sqrt((double)(num8 + 1f));
-                result.mW = num * 0.5f;
-                num = 0.5f / num;
-                result.mX = (matrix.m23 - matrix.m32) * num;
-                result.mY = (matrix.m31 - matrix.m13) * num;
-                result.mZ = (matrix.m12 - matrix.m21) * num;
-            }
-            else if ((matrix.m11 >= matrix.m22) && (matrix.m11 >= matrix.m33))
-            {
-                float num7 = (float)Math.Sqrt((double)(((1f + matrix.m11) - matrix.m22) - matrix.m33));
-                float num4 = 0.5f / num7;
-                result.mX = 0.5f * num7;
-                result.mY = (matrix.m12 + matrix.m21) * num4;
-                result.mZ = (matrix.m13 + matrix.m31) * num4;
-                result.mW = (matrix.m23 - matrix.m32) * num4;
-            }
-            else if (matrix.m22 > matrix.m33)
-            {
-                float num6 = (float)Math.Sqrt((double)(((1f + matrix.m22) - matrix.m11) - matrix.m33));
-                float num3 = 0.5f / num6;
-                result.mX = (matrix.m21 + matrix.m12) * num3;
-                result.mY = 0.5f * num6;
-                result.mZ = (matrix.m32 + matrix.m23) * num3;
-                result.mW = (matrix.m31 - matrix.m13) * num3;
-            }
-            else
-            {
-                float num5 = (float)Math.Sqrt((double)(((1f + matrix.m33) - matrix.m11) - matrix.m22));
-                float num2 = 0.5f / num5;
-                result.mX = (matrix.m31 + matrix.m13) * num2;
-                result.mY = (matrix.m32 + matrix.m23) * num2;
-                result.mZ = 0.5f * num5;
-                result.mW = (matrix.m12 - matrix.m21) * num2;
-            }
+            float sqrt;
+			float half;
+			float scale = matrix.m00 + matrix.m11 + matrix.m22;
+
+			if (scale > 0.0f)
+			{
+				sqrt = (float) Math.Sqrt(scale + 1.0f);
+				result.mW = sqrt * 0.5f;
+				sqrt = 0.5f / sqrt;
+
+				result.mX = (matrix.m21 - matrix.m12) * sqrt;
+				result.mY = (matrix.m02 - matrix.m20) * sqrt;
+				result.mZ = (matrix.m10 - matrix.m01) * sqrt;
+			}
+			else if ((matrix.m00 >= matrix.m11) && (matrix.m00 >= matrix.m22))
+			{
+				sqrt = (float) Math.Sqrt(1.0f + matrix.m00 - matrix.m11 - matrix.m22);
+				half = 0.5f / sqrt;
+
+				result.mX = 0.5f * sqrt;
+				result.mY = (matrix.m10 + matrix.m01) * half;
+				result.mZ = (matrix.m20 + matrix.m02) * half;
+				result.mW = (matrix.m21 - matrix.m12) * half;
+			}
+			else if (matrix.m11 > matrix.m22)
+			{
+				sqrt = (float) Math.Sqrt(1.0f + matrix.m11 - matrix.m00 - matrix.m22);
+				half = 0.5f/sqrt;
+
+				result.mX = (matrix.m01 + matrix.m10)*half;
+				result.mY = 0.5f*sqrt;
+				result.mZ = (matrix.m12 + matrix.m21)*half;
+				result.mW = (matrix.m02 - matrix.m20)*half;
+			}
+			else
+			{
+				sqrt = (float) Math.Sqrt(1.0f + matrix.m22 - matrix.m00 - matrix.m11);
+				half = 0.5f / sqrt;
+
+				result.mX = (matrix.m02 + matrix.m20) * half;
+				result.mY = (matrix.m12 + matrix.m21) * half;
+				result.mZ = 0.5f * sqrt;
+				result.mW = (matrix.m10 - matrix.m01) * half;
+			}
         }
 
         public static Quaternion CreateFromYawPitchRoll(float yaw, float pitch, float roll)

+ 65 - 2
BeefLibs/Beefy2D/src/geom/Vector3.bf

@@ -1,3 +1,6 @@
+// This file contains portions of code from the FNA project (github.com/FNA-XNA/FNA),
+// released under the Microsoft Public License
+
 using System;
 using System.Collections;
 using System.Text;
@@ -131,7 +134,7 @@ namespace Beefy.geom
         {
 			Vector3 newVec;
             Normalize(vector, out newVec);
-            return vector;
+            return newVec;
         }
 
         public static void Normalize(Vector3 value, out Vector3 result)
@@ -173,7 +176,7 @@ namespace Beefy.geom
             return new Vector2D((float)Math.Cos(angle) * length, (float)Math.Sin(angle) * length);
         }*/
 
-        public static Vector3 Transform(Vector3 vec, Matrix4 matrix)
+        public static Vector3 TransformW(Vector3 vec, Matrix4 matrix)
         {
 			Vector3 result;
             float fInvW = 1.0f / (matrix.m30 * vec.mX + matrix.m31 * vec.mY + matrix.m32 * vec.mZ + matrix.m33);
@@ -181,9 +184,19 @@ namespace Beefy.geom
             result.mX = (matrix.m00 * vec.mX + matrix.m01 * vec.mY + matrix.m02 * vec.mZ + matrix.m03) * fInvW;
             result.mY = (matrix.m10 * vec.mX + matrix.m11 * vec.mY + matrix.m12 * vec.mZ + matrix.m13) * fInvW;
             result.mZ = (matrix.m20 * vec.mX + matrix.m21 * vec.mY + matrix.m22 * vec.mZ + matrix.m23) * fInvW;
+
 			return result;
         }
 
+		public static Vector3 Transform(Vector3 vec, Matrix4 matrix)
+		{
+			Vector3 result;
+			result.mX = (vec.mX * matrix.m00) + (vec.mY * matrix.m01) + (vec.mZ * matrix.m02) + matrix.m03;
+			result.mY = (vec.mX * matrix.m10) + (vec.mY * matrix.m11) + (vec.mZ * matrix.m12) + matrix.m13;
+			result.mZ = (vec.mX * matrix.m20) + (vec.mY * matrix.m21) + (vec.mZ * matrix.m22) + matrix.m23;
+			return result;
+		}
+
         /*public static void Transform(Vector3[] sourceArray, ref Matrix4 matrix, Vector3[] destinationArray)
         {
             //Debug.Assert(destinationArray.Length >= sourceArray.Length, "The destination array is smaller than the source array.");
@@ -199,6 +212,51 @@ namespace Beefy.geom
             }
         }*/
 
+		/// <summary>
+		/// Returns a <see>Vector3</see> pointing in the opposite
+		/// direction of <paramref name="value"/>.
+		/// </summary>
+		/// <param name="value">The vector to negate.</param>
+		/// <returns>The vector negation of <paramref name="value"/>.</returns>
+		public static Vector3 Negate(Vector3 value)
+		{
+		    return .(-value.mX, -value.mY, -value.mZ);
+		}
+
+		/// <summary>
+		/// Stores a <see>Vector3</see> pointing in the opposite
+		/// direction of <paramref name="value"/> in <paramref name="result"/>.
+		/// </summary>
+		/// <param name="value">The vector to negate.</param>
+		/// <param name="result">The vector that the negation of <paramref name="value"/> will be stored in.</param>
+		public static void Negate(Vector3 value, out Vector3 result)
+		{
+		    result.mX = -value.mX;
+		    result.mY = -value.mY;
+		    result.mZ = -value.mZ;
+		}
+
+		/// <summary>
+		/// Creates a new <see cref="Vector3"/> that contains a multiplication of two vectors.
+		/// </summary>
+		/// <param name="value1">Source <see cref="Vector3"/>.</param>
+		/// <param name="value2">Source <see cref="Vector3"/>.</param>
+		/// <returns>The result of the vector multiplication.</returns>
+		public static Vector3 Multiply(Vector3 value1, Vector3 value2)
+		{
+			return .(value1.mX * value2.mX, value1.mY * value2.mY, value1.mZ * value2.mZ);
+		}
+
+		public static Vector3 Multiply(Vector3 value1, float value2)
+		{
+			return .(value1.mX * value2, value1.mY * value2, value1.mZ * value2);
+		}
+
+		public void Normalize() mut
+		{
+		    Normalize(this, out this);
+		}
+
         public static Vector3 Transform(Vector3 vec, Quaternion quat)
         {        
             Matrix4 matrix = quat.ToMatrix();
@@ -234,6 +292,11 @@ namespace Beefy.geom
             return Vector3(vec1.mX - vec2.mX, vec1.mY - vec2.mY, vec1.mZ - vec2.mZ);
         }
 
+		public static Vector3 operator -(Vector3 vec1)
+		{
+		    return Vector3(-vec1.mX, -vec1.mY, -vec1.mZ);
+		}
+
         public static Vector3 operator *(Vector3 vec, float scale)
         {
             return Vector3(vec.mX * scale, vec.mY * scale, vec.mZ * scale);

+ 1435 - 0
BeefLibs/Beefy2D/src/geom/Vector4.bf

@@ -0,0 +1,1435 @@
+// This file contains portions of code from the FNA project (github.com/FNA-XNA/FNA),
+// released under the Microsoft Public License
+
+using System;
+
+namespace Beefy.geom
+{
+	public struct Vector4 : IHashable
+	{
+		#region Public Static Properties
+
+		/// <summary>
+		/// Returns a <see cref="Vector4"/> with components 0, 0, 0, 0.
+		/// </summary>
+		public static Vector4 Zero
+		{
+			get
+			{
+				return zero;
+			}
+		}
+
+		/// <summary>
+		/// Returns a <see cref="Vector4"/> with components 1, 1, 1, 1.
+		/// </summary>
+		public static Vector4 One
+		{
+			get
+			{
+				return unit;
+			}
+		}
+
+		/// <summary>
+		/// Returns a <see cref="Vector4"/> with components 1, 0, 0, 0.
+		/// </summary>
+		public static Vector4 UnitX
+		{
+			get
+			{
+				return unitX;
+			}
+		}
+
+		/// <summary>
+		/// Returns a <see cref="Vector4"/> with components 0, 1, 0, 0.
+		/// </summary>
+		public static Vector4 UnitY
+		{
+			get
+			{
+				return unitY;
+			}
+		}
+
+		/// <summary>
+		/// Returns a <see cref="Vector4"/> with components 0, 0, 1, 0.
+		/// </summary>
+		public static Vector4 UnitZ
+		{
+			get
+			{
+				return unitZ;
+			}
+		}
+
+		/// <summary>
+		/// Returns a <see cref="Vector4"/> with components 0, 0, 0, 1.
+		/// </summary>
+		public static Vector4 UnitW
+		{
+			get
+			{
+				return unitW;
+			}
+		}
+
+		/// <summary>
+		/// The x coordinate of this <see cref="Vector4"/>.
+		/// </summary>
+		public float mX;
+
+		/// <summary>
+		/// The y coordinate of this <see cref="Vector4"/>.
+		/// </summary>
+		public float mY;
+
+		/// <summary>
+		/// The z coordinate of this <see cref="Vector4"/>.
+		/// </summary>
+		public float mZ;
+
+		/// <summary>
+		/// The w coordinate of this <see cref="Vector4"/>.
+		/// </summary>
+		public float mW;
+
+		#endregion
+
+		#region Private Static Fields
+
+		private static Vector4 zero = .(); // Not readonly for performance -flibit
+		private static readonly Vector4 unit = .(1f, 1f, 1f, 1f);
+		private static readonly Vector4 unitX = .(1f, 0f, 0f, 0f);
+		private static readonly Vector4 unitY = .(0f, 1f, 0f, 0f);
+		private static readonly Vector4 unitZ = .(0f, 0f, 1f, 0f);
+		private static readonly Vector4 unitW = .(0f, 0f, 0f, 1f);
+
+		#endregion
+
+		#region Public Constructors
+
+		public this()
+		{
+			mX = 0;
+			mY = 0;
+			mZ = 0;
+			mW = 0;
+		}
+
+		/// <summary>
+		/// Constructs a 3d vector with X, Y, Z and W from four values.
+		/// </summary>
+		/// <param name="x">The x coordinate in 4d-space.</param>
+		/// <param name="y">The y coordinate in 4d-space.</param>
+		/// <param name="z">The z coordinate in 4d-space.</param>
+		/// <param name="w">The w coordinate in 4d-space.</param>
+		public this(float x, float y, float z, float w)
+		{
+			this.mX = x;
+			this.mY = y;
+			this.mZ = z;
+			this.mW = w;
+		}
+
+		/// <summary>
+		/// Constructs a 3d vector with X and Z from <see cref="Vector2"/> and Z and W from the scalars.
+		/// </summary>
+		/// <param name="value">The x and y coordinates in 4d-space.</param>
+		/// <param name="z">The z coordinate in 4d-space.</param>
+		/// <param name="w">The w coordinate in 4d-space.</param>
+		public this(Vector2 value, float z, float w)
+		{
+			this.mX = value.mX;
+			this.mY = value.mY;
+			this.mZ = z;
+			this.mW = w;
+		}
+
+		/// <summary>
+		/// Constructs a 3d vector with X, Y, Z from <see cref="Vector3"/> and W from a scalar.
+		/// </summary>
+		/// <param name="value">The x, y and z coordinates in 4d-space.</param>
+		/// <param name="w">The w coordinate in 4d-space.</param>
+		public this(Vector3 value, float w)
+		{
+			this.mX = value.mX;
+			this.mY = value.mY;
+			this.mZ = value.mZ;
+			this.mW = w;
+		}
+
+		/// <summary>
+		/// Constructs a 4d vector with X, Y, Z and W set to the same value.
+		/// </summary>
+		/// <param name="value">The x, y, z and w coordinates in 4d-space.</param>
+		public this(float value)
+		{
+			this.mX = value;
+			this.mY = value;
+			this.mZ = value;
+			this.mW = value;
+		}
+
+		#endregion
+
+		#region Public Methods
+
+		/// <summary>
+		/// Compares whether current instance is equal to specified <see cref="Vector4"/>.
+		/// </summary>
+		/// <param name="other">The <see cref="Vector4"/> to compare.</param>
+		/// <returns><c>true</c> if the instances are equal; <c>false</c> otherwise.</returns>
+		public bool Equals(Vector4 other)
+		{
+			return (	mX == other.mX &&
+					mY == other.mY &&
+					mZ == other.mZ &&
+					mW == other.mW	);
+		}
+
+		/// <summary>
+		/// Gets the hash code of this <see cref="Vector4"/>.
+		/// </summary>
+		/// <returns>Hash code of this <see cref="Vector4"/>.</returns>
+		public int GetHashCode()
+		{
+			return mW.GetHashCode() + mX.GetHashCode() + mY.GetHashCode() + mZ.GetHashCode();
+		}
+
+		/// <summary>
+		/// Returns the length of this <see cref="Vector4"/>.
+		/// </summary>
+		/// <returns>The length of this <see cref="Vector4"/>.</returns>
+		public float Length()
+		{
+			return (float) Math.Sqrt((mX * mX) + (mY * mY) + (mZ * mZ) + (mW * mW));
+		}
+
+		/// <summary>
+		/// Returns the squared length of this <see cref="Vector4"/>.
+		/// </summary>
+		/// <returns>The squared length of this <see cref="Vector4"/>.</returns>
+		public float LengthSquared()
+		{
+			return (mX * mX) + (mY * mY) + (mZ * mZ) + (mW * mW);
+		}
+
+		/// <summary>
+		/// Turns this <see cref="Vector4"/> to a unit vector with the same direction.
+		/// </summary>
+		public void Normalize() mut
+		{
+			float factor = 1.0f / (float) Math.Sqrt(
+				(mX * mX) +
+				(mY * mY) +
+				(mZ * mZ) +
+				(mW * mW)
+			);
+			mX *= factor;
+			mY *= factor;
+			mZ *= factor;
+			mW *= factor;
+		}
+
+		public override void ToString(String str)
+		{
+			str.AppendF($"({mX}, {mY}, {mZ}, {mW})");
+		}
+
+		/// <summary>
+		/// Performs vector addition on <paramref name="value1"/> and <paramref name="value2"/>.
+		/// </summary>
+		/// <param name="value1">The first vector to add.</param>
+		/// <param name="value2">The second vector to add.</param>
+		/// <returns>The result of the vector addition.</returns>
+		public static Vector4 Add(Vector4 value1, Vector4 value2)
+		{
+			return .(
+				value1.mX + value2.mX,
+				value1.mY + value2.mY,
+				value1.mZ + value2.mZ,
+				value1.mW + value2.mW);
+		}
+
+		/// <summary>
+		/// Performs vector addition on <paramref name="value1"/> and
+		/// <paramref name="value2"/>, storing the result of the
+		/// addition in <paramref name="result"/>.
+		/// </summary>
+		/// <param name="value1">The first vector to add.</param>
+		/// <param name="value2">The second vector to add.</param>
+		/// <param name="result">The result of the vector addition.</param>
+		public static void Add(ref Vector4 value1, ref Vector4 value2, out Vector4 result)
+		{
+			result.mW = value1.mW + value2.mW;
+			result.mX = value1.mX + value2.mX;
+			result.mY = value1.mY + value2.mY;
+			result.mZ = value1.mZ + value2.mZ;
+		}
+
+		/// <summary>
+		/// Creates a new <see cref="Vector4"/> that contains the cartesian coordinates of a vector specified in barycentric coordinates and relative to 4d-triangle.
+		/// </summary>
+		/// <param name="value1">The first vector of 4d-triangle.</param>
+		/// <param name="value2">The second vector of 4d-triangle.</param>
+		/// <param name="value3">The third vector of 4d-triangle.</param>
+		/// <param name="amount1">Barycentric scalar <c>b2</c> which represents a weighting factor towards second vector of 4d-triangle.</param>
+		/// <param name="amount2">Barycentric scalar <c>b3</c> which represents a weighting factor towards third vector of 4d-triangle.</param>
+		/// <returns>The cartesian translation of barycentric coordinates.</returns>
+		public static Vector4 Barycentric(
+			Vector4 value1,
+			Vector4 value2,
+			Vector4 value3,
+			float amount1,
+			float amount2
+		) {
+			return Vector4(
+				Math.Barycentric(value1.mX, value2.mX, value3.mX, amount1, amount2),
+				Math.Barycentric(value1.mY, value2.mY, value3.mY, amount1, amount2),
+				Math.Barycentric(value1.mZ, value2.mZ, value3.mZ, amount1, amount2),
+				Math.Barycentric(value1.mW, value2.mW, value3.mW, amount1, amount2)
+			);
+		}
+
+		/// <summary>
+		/// Creates a new <see cref="Vector4"/> that contains the cartesian coordinates of a vector specified in barycentric coordinates and relative to 4d-triangle.
+		/// </summary>
+		/// <param name="value1">The first vector of 4d-triangle.</param>
+		/// <param name="value2">The second vector of 4d-triangle.</param>
+		/// <param name="value3">The third vector of 4d-triangle.</param>
+		/// <param name="amount1">Barycentric scalar <c>b2</c> which represents a weighting factor towards second vector of 4d-triangle.</param>
+		/// <param name="amount2">Barycentric scalar <c>b3</c> which represents a weighting factor towards third vector of 4d-triangle.</param>
+		/// <param name="result">The cartesian translation of barycentric coordinates as an output parameter.</param>
+		public static void Barycentric(
+			ref Vector4 value1,
+			ref Vector4 value2,
+			ref Vector4 value3,
+			float amount1,
+			float amount2,
+			out Vector4 result
+		) {
+			result.mX = Math.Barycentric(value1.mX, value2.mX, value3.mX, amount1, amount2);
+			result.mY = Math.Barycentric(value1.mY, value2.mY, value3.mY, amount1, amount2);
+			result.mZ = Math.Barycentric(value1.mZ, value2.mZ, value3.mZ, amount1, amount2);
+			result.mW = Math.Barycentric(value1.mW, value2.mW, value3.mW, amount1, amount2);
+		}
+
+		/// <summary>
+		/// Creates a new <see cref="Vector4"/> that contains CatmullRom interpolation of the specified vectors.
+		/// </summary>
+		/// <param name="value1">The first vector in interpolation.</param>
+		/// <param name="value2">The second vector in interpolation.</param>
+		/// <param name="value3">The third vector in interpolation.</param>
+		/// <param name="value4">The fourth vector in interpolation.</param>
+		/// <param name="amount">Weighting factor.</param>
+		/// <returns>The result of CatmullRom interpolation.</returns>
+		public static Vector4 CatmullRom(
+			Vector4 value1,
+			Vector4 value2,
+			Vector4 value3,
+			Vector4 value4,
+			float amount
+		) {
+			return Vector4(
+				Math.CatmullRom(value1.mX, value2.mX, value3.mX, value4.mX, amount),
+				Math.CatmullRom(value1.mY, value2.mY, value3.mY, value4.mY, amount),
+				Math.CatmullRom(value1.mZ, value2.mZ, value3.mZ, value4.mZ, amount),
+				Math.CatmullRom(value1.mW, value2.mW, value3.mW, value4.mW, amount)
+			);
+		}
+
+		/// <summary>
+		/// Creates a new <see cref="Vector4"/> that contains CatmullRom interpolation of the specified vectors.
+		/// </summary>
+		/// <param name="value1">The first vector in interpolation.</param>
+		/// <param name="value2">The second vector in interpolation.</param>
+		/// <param name="value3">The third vector in interpolation.</param>
+		/// <param name="value4">The fourth vector in interpolation.</param>
+		/// <param name="amount">Weighting factor.</param>
+		/// <param name="result">The result of CatmullRom interpolation as an output parameter.</param>
+		public static void CatmullRom(
+			ref Vector4 value1,
+			ref Vector4 value2,
+			ref Vector4 value3,
+			ref Vector4 value4,
+			float amount,
+			out Vector4 result
+		) {
+			result.mX = Math.CatmullRom(value1.mX, value2.mX, value3.mX, value4.mX, amount);
+			result.mY = Math.CatmullRom(value1.mY, value2.mY, value3.mY, value4.mY, amount);
+			result.mZ = Math.CatmullRom(value1.mZ, value2.mZ, value3.mZ, value4.mZ, amount);
+			result.mW = Math.CatmullRom(value1.mW, value2.mW, value3.mW, value4.mW, amount);
+		}
+
+		/// <summary>
+		/// Clamps the specified value within a range.
+		/// </summary>
+		/// <param name="value1">The value to clamp.</param>
+		/// <param name="min">The min value.</param>
+		/// <param name="max">The max value.</param>
+		/// <returns>The clamped value.</returns>
+		public static Vector4 Clamp(Vector4 value1, Vector4 min, Vector4 max)
+		{
+			return Vector4(
+				Math.Clamp(value1.mX, min.mX, max.mX),
+				Math.Clamp(value1.mY, min.mY, max.mY),
+				Math.Clamp(value1.mZ, min.mZ, max.mZ),
+				Math.Clamp(value1.mW, min.mW, max.mW)
+			);
+		}
+
+		/// <summary>
+		/// Clamps the specified value within a range.
+		/// </summary>
+		/// <param name="value1">The value to clamp.</param>
+		/// <param name="min">The min value.</param>
+		/// <param name="max">The max value.</param>
+		/// <param name="result">The clamped value as an output parameter.</param>
+		public static void Clamp(
+			Vector4 value1,
+			Vector4 min,
+			Vector4 max,
+			out Vector4 result
+		) {
+			result.mX = Math.Clamp(value1.mX, min.mX, max.mX);
+			result.mY = Math.Clamp(value1.mY, min.mY, max.mY);
+			result.mZ = Math.Clamp(value1.mZ, min.mZ, max.mZ);
+			result.mW = Math.Clamp(value1.mW, min.mW, max.mW);
+		}
+
+		/// <summary>
+		/// Returns the distance between two vectors.
+		/// </summary>
+		/// <param name="value1">The first vector.</param>
+		/// <param name="value2">The second vector.</param>
+		/// <returns>The distance between two vectors.</returns>
+		public static float Distance(Vector4 value1, Vector4 value2)
+		{
+			return (float) Math.Sqrt(DistanceSquared(value1, value2));
+		}
+
+		/// <summary>
+		/// Returns the distance between two vectors.
+		/// </summary>
+		/// <param name="value1">The first vector.</param>
+		/// <param name="value2">The second vector.</param>
+		/// <param name="result">The distance between two vectors as an output parameter.</param>
+		public static void Distance(ref Vector4 value1, ref Vector4 value2, out float result)
+		{
+			result = (float) Math.Sqrt(DistanceSquared(value1, value2));
+		}
+
+		/// <summary>
+		/// Returns the squared distance between two vectors.
+		/// </summary>
+		/// <param name="value1">The first vector.</param>
+		/// <param name="value2">The second vector.</param>
+		/// <returns>The squared distance between two vectors.</returns>
+		public static float DistanceSquared(Vector4 value1, Vector4 value2)
+		{
+			return (
+				(value1.mW - value2.mW) * (value1.mW - value2.mW) +
+				(value1.mX - value2.mX) * (value1.mX - value2.mX) +
+				(value1.mY - value2.mY) * (value1.mY - value2.mY) +
+				(value1.mZ - value2.mZ) * (value1.mZ - value2.mZ)
+			);
+		}
+
+		/// <summary>
+		/// Returns the squared distance between two vectors.
+		/// </summary>
+		/// <param name="value1">The first vector.</param>
+		/// <param name="value2">The second vector.</param>
+		/// <param name="result">The squared distance between two vectors as an output parameter.</param>
+		public static void DistanceSquared(
+			ref Vector4 value1,
+			ref Vector4 value2,
+			out float result
+		) {
+			result = (
+				(value1.mW - value2.mW) * (value1.mW - value2.mW) +
+				(value1.mX - value2.mX) * (value1.mX - value2.mX) +
+				(value1.mY - value2.mY) * (value1.mY - value2.mY) +
+				(value1.mZ - value2.mZ) * (value1.mZ - value2.mZ)
+			);
+		}
+
+		/// <summary>
+		/// Divides the components of a <see cref="Vector4"/> by the components of another <see cref="Vector4"/>.
+		/// </summary>
+		/// <param name="value1">Source <see cref="Vector4"/>.</param>
+		/// <param name="value2">Divisor <see cref="Vector4"/>.</param>
+		/// <returns>The result of dividing the vectors.</returns>
+		public static Vector4 Divide(Vector4 value1, Vector4 value2)
+		{
+			var value1;
+			value1.mW /= value2.mW;
+			value1.mX /= value2.mX;
+			value1.mY /= value2.mY;
+			value1.mZ /= value2.mZ;
+			return value1;
+		}
+
+		/// <summary>
+		/// Divides the components of a <see cref="Vector4"/> by a scalar.
+		/// </summary>
+		/// <param name="value1">Source <see cref="Vector4"/>.</param>
+		/// <param name="divider">Divisor scalar.</param>
+		/// <returns>The result of dividing a vector by a scalar.</returns>
+		public static Vector4 Divide(Vector4 value1, float divider)
+		{
+			var value1;
+			float factor = 1f / divider;
+			value1.mW *= factor;
+			value1.mX *= factor;
+			value1.mY *= factor;
+			value1.mZ *= factor;
+			return value1;
+		}
+
+		/// <summary>
+		/// Divides the components of a <see cref="Vector4"/> by a scalar.
+		/// </summary>
+		/// <param name="value1">Source <see cref="Vector4"/>.</param>
+		/// <param name="divider">Divisor scalar.</param>
+		/// <param name="result">The result of dividing a vector by a scalar as an output parameter.</param>
+		public static void Divide(ref Vector4 value1, float divider, out Vector4 result)
+		{
+			float factor = 1f / divider;
+			result.mW = value1.mW * factor;
+			result.mX = value1.mX * factor;
+			result.mY = value1.mY * factor;
+			result.mZ = value1.mZ * factor;
+		}
+
+		/// <summary>
+		/// Divides the components of a <see cref="Vector4"/> by the components of another <see cref="Vector4"/>.
+		/// </summary>
+		/// <param name="value1">Source <see cref="Vector4"/>.</param>
+		/// <param name="value2">Divisor <see cref="Vector4"/>.</param>
+		/// <param name="result">The result of dividing the vectors as an output parameter.</param>
+		public static void Divide(
+			ref Vector4 value1,
+			ref Vector4 value2,
+			out Vector4 result
+		) {
+			result.mW = value1.mW / value2.mW;
+			result.mX = value1.mX / value2.mX;
+			result.mY = value1.mY / value2.mY;
+			result.mZ = value1.mZ / value2.mZ;
+		}
+
+		/// <summary>
+		/// Returns a dot product of two vectors.
+		/// </summary>
+		/// <param name="value1">The first vector.</param>
+		/// <param name="value2">The second vector.</param>
+		/// <returns>The dot product of two vectors.</returns>
+		public static float Dot(Vector4 vector1, Vector4 vector2)
+		{
+			return (
+				vector1.mX * vector2.mX +
+				vector1.mY * vector2.mY +
+				vector1.mZ * vector2.mZ +
+				vector1.mW * vector2.mW
+			);
+		}
+
+		/// <summary>
+		/// Returns a dot product of two vectors.
+		/// </summary>
+		/// <param name="value1">The first vector.</param>
+		/// <param name="value2">The second vector.</param>
+		/// <param name="result">The dot product of two vectors as an output parameter.</param>
+		public static void Dot(ref Vector4 vector1, ref Vector4 vector2, out float result)
+		{
+			result = (
+				(vector1.mX * vector2.mX) +
+				(vector1.mY * vector2.mY) +
+				(vector1.mZ * vector2.mZ) +
+				(vector1.mW * vector2.mW)
+			);
+		}
+
+		/// <summary>
+		/// Creates a new <see cref="Vector4"/> that contains hermite spline interpolation.
+		/// </summary>
+		/// <param name="value1">The first position vector.</param>
+		/// <param name="tangent1">The first tangent vector.</param>
+		/// <param name="value2">The second position vector.</param>
+		/// <param name="tangent2">The second tangent vector.</param>
+		/// <param name="amount">Weighting factor.</param>
+		/// <returns>The hermite spline interpolation vector.</returns>
+		public static Vector4 Hermite(
+			Vector4 value1,
+			Vector4 tangent1,
+			Vector4 value2,
+			Vector4 tangent2,
+			float amount
+		) {
+			return Vector4(
+				Math.Hermite(value1.mX, tangent1.mX, value2.mX, tangent2.mX, amount),
+				Math.Hermite(value1.mY, tangent1.mY, value2.mY, tangent2.mY, amount),
+				Math.Hermite(value1.mZ, tangent1.mZ, value2.mZ, tangent2.mZ, amount),
+				Math.Hermite(value1.mW, tangent1.mW, value2.mW, tangent2.mW, amount)
+			);
+		}
+
+		/// <summary>
+		/// Creates a new <see cref="Vector4"/> that contains hermite spline interpolation.
+		/// </summary>
+		/// <param name="value1">The first position vector.</param>
+		/// <param name="tangent1">The first tangent vector.</param>
+		/// <param name="value2">The second position vector.</param>
+		/// <param name="tangent2">The second tangent vector.</param>
+		/// <param name="amount">Weighting factor.</param>
+		/// <param name="result">The hermite spline interpolation vector as an output parameter.</param>
+		public static void Hermite(
+			Vector4 value1,
+			Vector4 tangent1,
+			Vector4 value2,
+			Vector4 tangent2,
+			float amount,
+			out Vector4 result
+		) {
+			result.mW = Math.Hermite(value1.mW, tangent1.mW, value2.mW, tangent2.mW, amount);
+			result.mX = Math.Hermite(value1.mX, tangent1.mX, value2.mX, tangent2.mX, amount);
+			result.mY = Math.Hermite(value1.mY, tangent1.mY, value2.mY, tangent2.mY, amount);
+			result.mZ = Math.Hermite(value1.mZ, tangent1.mZ, value2.mZ, tangent2.mZ, amount);
+		}
+
+		/// <summary>
+		/// Creates a new <see cref="Vector4"/> that contains linear interpolation of the specified vectors.
+		/// </summary>
+		/// <param name="value1">The first vector.</param>
+		/// <param name="value2">The second vector.</param>
+		/// <param name="amount">Weighting value(between 0.0 and 1.0).</param>
+		/// <returns>The result of linear interpolation of the specified vectors.</returns>
+		public static Vector4 Lerp(Vector4 value1, Vector4 value2, float amount)
+		{
+			return Vector4(
+				Math.Lerp(value1.mX, value2.mX, amount),
+				Math.Lerp(value1.mY, value2.mY, amount),
+				Math.Lerp(value1.mZ, value2.mZ, amount),
+				Math.Lerp(value1.mW, value2.mW, amount)
+			);
+		}
+
+		/// <summary>
+		/// Creates a new <see cref="Vector4"/> that contains linear interpolation of the specified vectors.
+		/// </summary>
+		/// <param name="value1">The first vector.</param>
+		/// <param name="value2">The second vector.</param>
+		/// <param name="amount">Weighting value(between 0.0 and 1.0).</param>
+		/// <param name="result">The result of linear interpolation of the specified vectors as an output parameter.</param>
+		public static void Lerp(
+			ref Vector4 value1,
+			ref Vector4 value2,
+			float amount,
+			out Vector4 result
+		) {
+			result.mX = Math.Lerp(value1.mX, value2.mX, amount);
+			result.mY = Math.Lerp(value1.mY, value2.mY, amount);
+			result.mZ = Math.Lerp(value1.mZ, value2.mZ, amount);
+			result.mW = Math.Lerp(value1.mW, value2.mW, amount);
+		}
+
+		/// <summary>
+		/// Creates a new <see cref="Vector4"/> that contains a maximal values from the two vectors.
+		/// </summary>
+		/// <param name="value1">The first vector.</param>
+		/// <param name="value2">The second vector.</param>
+		/// <returns>The <see cref="Vector4"/> with maximal values from the two vectors.</returns>
+		public static Vector4 Max(Vector4 value1, Vector4 value2)
+		{
+			return Vector4(
+				Math.Max(value1.mX, value2.mX),
+				Math.Max(value1.mY, value2.mY),
+				Math.Max(value1.mZ, value2.mZ),
+				Math.Max(value1.mW, value2.mW)
+			);
+		}
+
+		/// <summary>
+		/// Creates a new <see cref="Vector4"/> that contains a maximal values from the two vectors.
+		/// </summary>
+		/// <param name="value1">The first vector.</param>
+		/// <param name="value2">The second vector.</param>
+		/// <param name="result">The <see cref="Vector4"/> with maximal values from the two vectors as an output parameter.</param>
+		public static void Max(ref Vector4 value1, ref Vector4 value2, out Vector4 result)
+		{
+			result.mX = Math.Max(value1.mX, value2.mX);
+			result.mY = Math.Max(value1.mY, value2.mY);
+			result.mZ = Math.Max(value1.mZ, value2.mZ);
+			result.mW = Math.Max(value1.mW, value2.mW);
+		}
+
+		/// <summary>
+		/// Creates a new <see cref="Vector4"/> that contains a minimal values from the two vectors.
+		/// </summary>
+		/// <param name="value1">The first vector.</param>
+		/// <param name="value2">The second vector.</param>
+		/// <returns>The <see cref="Vector4"/> with minimal values from the two vectors.</returns>
+		public static Vector4 Min(Vector4 value1, Vector4 value2)
+		{
+			return Vector4(
+				Math.Min(value1.mX, value2.mX),
+				Math.Min(value1.mY, value2.mY),
+				Math.Min(value1.mZ, value2.mZ),
+				Math.Min(value1.mW, value2.mW)
+			);
+		}
+
+		/// <summary>
+		/// Creates a new <see cref="Vector4"/> that contains a minimal values from the two vectors.
+		/// </summary>
+		/// <param name="value1">The first vector.</param>
+		/// <param name="value2">The second vector.</param>
+		/// <param name="result">The <see cref="Vector4"/> with minimal values from the two vectors as an output parameter.</param>
+		public static void Min(ref Vector4 value1, ref Vector4 value2, out Vector4 result)
+		{
+			result.mX = Math.Min(value1.mX, value2.mX);
+			result.mY = Math.Min(value1.mY, value2.mY);
+			result.mZ = Math.Min(value1.mZ, value2.mZ);
+			result.mW = Math.Min(value1.mW, value2.mW);
+		}
+
+		/// <summary>
+		/// Creates a new <see cref="Vector4"/> that contains a multiplication of two vectors.
+		/// </summary>
+		/// <param name="value1">Source <see cref="Vector4"/>.</param>
+		/// <param name="value2">Source <see cref="Vector4"/>.</param>
+		/// <returns>The result of the vector multiplication.</returns>
+		public static Vector4 Multiply(Vector4 value1, Vector4 value2)
+		{
+			var value1;
+			value1.mW *= value2.mW;
+			value1.mX *= value2.mX;
+			value1.mY *= value2.mY;
+			value1.mZ *= value2.mZ;
+			return value1;
+		}
+
+		/// <summary>
+		/// Creates a new <see cref="Vector4"/> that contains a multiplication of <see cref="Vector4"/> and a scalar.
+		/// </summary>
+		/// <param name="value1">Source <see cref="Vector4"/>.</param>
+		/// <param name="scaleFactor">Scalar value.</param>
+		/// <returns>The result of the vector multiplication with a scalar.</returns>
+		public static Vector4 Multiply(Vector4 value1, float scaleFactor)
+		{
+			var value1;
+			value1.mW *= scaleFactor;
+			value1.mX *= scaleFactor;
+			value1.mY *= scaleFactor;
+			value1.mZ *= scaleFactor;
+			return value1;
+		}
+
+		/// <summary>
+		/// Creates a new <see cref="Vector4"/> that contains a multiplication of <see cref="Vector4"/> and a scalar.
+		/// </summary>
+		/// <param name="value1">Source <see cref="Vector4"/>.</param>
+		/// <param name="scaleFactor">Scalar value.</param>
+		/// <param name="result">The result of the multiplication with a scalar as an output parameter.</param>
+		public static void Multiply(Vector4 value1, float scaleFactor, out Vector4 result)
+		{
+			result.mW = value1.mW * scaleFactor;
+			result.mX = value1.mX * scaleFactor;
+			result.mY = value1.mY * scaleFactor;
+			result.mZ = value1.mZ * scaleFactor;
+		}
+
+		/// <summary>
+		/// Creates a new <see cref="Vector4"/> that contains a multiplication of two vectors.
+		/// </summary>
+		/// <param name="value1">Source <see cref="Vector4"/>.</param>
+		/// <param name="value2">Source <see cref="Vector4"/>.</param>
+		/// <param name="result">The result of the vector multiplication as an output parameter.</param>
+		public static void Multiply(ref Vector4 value1, ref Vector4 value2, out Vector4 result)
+		{
+			result.mW = value1.mW * value2.mW;
+			result.mX = value1.mX * value2.mX;
+			result.mY = value1.mY * value2.mY;
+			result.mZ = value1.mZ * value2.mZ;
+		}
+
+		/// <summary>
+		/// Creates a new <see cref="Vector4"/> that contains the specified vector inversion.
+		/// </summary>
+		/// <param name="value">Source <see cref="Vector4"/>.</param>
+		/// <returns>The result of the vector inversion.</returns>
+		public static Vector4 Negate(Vector4 value)
+		{
+			return Vector4(-value.mX, -value.mY, -value.mZ, -value.mW);
+		}
+
+		/// <summary>
+		/// Creates a new <see cref="Vector4"/> that contains the specified vector inversion.
+		/// </summary>
+		/// <param name="value">Source <see cref="Vector4"/>.</param>
+		/// <param name="result">The result of the vector inversion as an output parameter.</param>
+		public static void Negate(ref Vector4 value, out Vector4 result)
+		{
+			result.mX = -value.mX;
+			result.mY = -value.mY;
+			result.mZ = -value.mZ;
+			result.mW = -value.mW;
+		}
+
+		/// <summary>
+		/// Creates a new <see cref="Vector4"/> that contains a normalized values from another vector.
+		/// </summary>
+		/// <param name="value">Source <see cref="Vector4"/>.</param>
+		/// <returns>Unit vector.</returns>
+		public static Vector4 Normalize(Vector4 vector)
+		{
+			float factor = 1.0f / (float) Math.Sqrt(
+				(vector.mX * vector.mX) +
+				(vector.mY * vector.mY) +
+				(vector.mZ * vector.mZ) +
+				(vector.mW * vector.mW)
+			);
+			return Vector4(
+				vector.mX * factor,
+				vector.mY * factor,
+				vector.mZ * factor,
+				vector.mW * factor
+			);
+		}
+
+		/// <summary>
+		/// Creates a new <see cref="Vector4"/> that contains a normalized values from another vector.
+		/// </summary>
+		/// <param name="value">Source <see cref="Vector4"/>.</param>
+		/// <param name="result">Unit vector as an output parameter.</param>
+		public static void Normalize(Vector4 vector, out Vector4 result)
+		{
+			float factor = 1.0f / (float) Math.Sqrt(
+				(vector.mX * vector.mX) +
+				(vector.mY * vector.mY) +
+				(vector.mZ * vector.mZ) +
+				(vector.mW * vector.mW)
+			);
+			result.mX = vector.mX * factor;
+			result.mY = vector.mY * factor;
+			result.mZ = vector.mZ * factor;
+			result.mW = vector.mW * factor;
+		}
+
+		/// <summary>
+		/// Creates a new <see cref="Vector4"/> that contains cubic interpolation of the specified vectors.
+		/// </summary>
+		/// <param name="value1">Source <see cref="Vector4"/>.</param>
+		/// <param name="value2">Source <see cref="Vector4"/>.</param>
+		/// <param name="amount">Weighting value.</param>
+		/// <returns>Cubic interpolation of the specified vectors.</returns>
+		public static Vector4 SmoothStep(Vector4 value1, Vector4 value2, float amount)
+		{
+			return Vector4(
+				Math.SmoothStep(value1.mX, value2.mX, amount),
+				Math.SmoothStep(value1.mY, value2.mY, amount),
+				Math.SmoothStep(value1.mZ, value2.mZ, amount),
+				Math.SmoothStep(value1.mW, value2.mW, amount)
+			);
+		}
+
+		/// <summary>
+		/// Creates a new <see cref="Vector4"/> that contains cubic interpolation of the specified vectors.
+		/// </summary>
+		/// <param name="value1">Source <see cref="Vector4"/>.</param>
+		/// <param name="value2">Source <see cref="Vector4"/>.</param>
+		/// <param name="amount">Weighting value.</param>
+		/// <param name="result">Cubic interpolation of the specified vectors as an output parameter.</param>
+		public static void SmoothStep(
+			Vector4 value1,
+			Vector4 value2,
+			float amount,
+			out Vector4 result
+		) {
+			result.mX = Math.SmoothStep(value1.mX, value2.mX, amount);
+			result.mY = Math.SmoothStep(value1.mY, value2.mY, amount);
+			result.mZ = Math.SmoothStep(value1.mZ, value2.mZ, amount);
+			result.mW = Math.SmoothStep(value1.mW, value2.mW, amount);
+		}
+
+		/// <summary>
+		/// Creates a new <see cref="Vector4"/> that contains subtraction of on <see cref="Vector4"/> from a another.
+		/// </summary>
+		/// <param name="value1">Source <see cref="Vector4"/>.</param>
+		/// <param name="value2">Source <see cref="Vector4"/>.</param>
+		/// <returns>The result of the vector subtraction.</returns>
+		public static Vector4 Subtract(Vector4 value1, Vector4 value2)
+		{
+			var value1;
+			value1.mW -= value2.mW;
+			value1.mX -= value2.mX;
+			value1.mY -= value2.mY;
+			value1.mZ -= value2.mZ;
+			return value1;
+		}
+
+		/// <summary>
+		/// Creates a new <see cref="Vector4"/> that contains subtraction of on <see cref="Vector4"/> from a another.
+		/// </summary>
+		/// <param name="value1">Source <see cref="Vector4"/>.</param>
+		/// <param name="value2">Source <see cref="Vector4"/>.</param>
+		/// <param name="result">The result of the vector subtraction as an output parameter.</param>
+		public static void Subtract(Vector4 value1, Vector4 value2, out Vector4 result)
+		{
+			result.mW = value1.mW - value2.mW;
+			result.mX = value1.mX - value2.mX;
+			result.mY = value1.mY - value2.mY;
+			result.mZ = value1.mZ - value2.mZ;
+		}
+
+		/// <summary>
+		/// Creates a new <see cref="Vector4"/> that contains a transformation of 2d-vector by the specified <see cref="Matrix"/>.
+		/// </summary>
+		/// <param name="value">Source <see cref="Vector2"/>.</param>
+		/// <param name="matrix">The transformation <see cref="Matrix"/>.</param>
+		/// <returns>Transformed <see cref="Vector4"/>.</returns>
+		public static Vector4 Transform(Vector2 position, Matrix4 matrix)
+		{
+			Vector4 result;
+			Transform(position, matrix, out result);
+			return result;
+		}
+
+		/// <summary>
+		/// Creates a new <see cref="Vector4"/> that contains a transformation of 3d-vector by the specified <see cref="Matrix"/>.
+		/// </summary>
+		/// <param name="value">Source <see cref="Vector3"/>.</param>
+		/// <param name="matrix">The transformation <see cref="Matrix"/>.</param>
+		/// <returns>Transformed <see cref="Vector4"/>.</returns>
+		public static Vector4 Transform(Vector3 position, Matrix4 matrix)
+		{
+			Vector4 result;
+			Transform(position, matrix, out result);
+			return result;
+		}
+
+		/// <summary>
+		/// Creates a new <see cref="Vector4"/> that contains a transformation of 4d-vector by the specified <see cref="Matrix"/>.
+		/// </summary>
+		/// <param name="value">Source <see cref="Vector4"/>.</param>
+		/// <param name="matrix">The transformation <see cref="Matrix"/>.</param>
+		/// <returns>Transformed <see cref="Vector4"/>.</returns>
+		public static Vector4 Transform(Vector4 vector, Matrix4 matrix)
+		{
+			return Transform(vector, matrix);
+		}
+
+		/// <summary>
+		/// Creates a new <see cref="Vector4"/> that contains a transformation of 2d-vector by the specified <see cref="Matrix"/>.
+		/// </summary>
+		/// <param name="value">Source <see cref="Vector2"/>.</param>
+		/// <param name="matrix">The transformation <see cref="Matrix"/>.</param>
+		/// <param name="result">Transformed <see cref="Vector4"/> as an output parameter.</param>
+		public static void Transform(Vector2 position, Matrix4 matrix, out Vector4 result)
+		{
+			result = Vector4(
+				(position.mX * matrix.m00) + (position.mY * matrix.m01) + matrix.m03,
+				(position.mX * matrix.m10) + (position.mY * matrix.m11) + matrix.m13,
+				(position.mX * matrix.m20) + (position.mY * matrix.m21) + matrix.m23,
+				(position.mX * matrix.m30) + (position.mY * matrix.m31) + matrix.m33
+			);
+		}
+
+		/// <summary>
+		/// Creates a new <see cref="Vector4"/> that contains a transformation of 3d-vector by the specified <see cref="Matrix"/>.
+		/// </summary>
+		/// <param name="value">Source <see cref="Vector3"/>.</param>
+		/// <param name="matrix">The transformation <see cref="Matrix"/>.</param>
+		/// <param name="result">Transformed <see cref="Vector4"/> as an output parameter.</param>
+		public static void Transform(Vector3 position, Matrix4 matrix, out Vector4 result)
+		{
+			float x = (
+				(position.mX * matrix.m00) +
+				(position.mY * matrix.m01) +
+				(position.mZ * matrix.m02) +
+				matrix.m03
+			);
+			float y = (
+				(position.mX * matrix.m10) +
+				(position.mY * matrix.m11) +
+				(position.mZ * matrix.m12) +
+				matrix.m13
+			);
+			float z = (
+				(position.mX * matrix.m20) +
+				(position.mY * matrix.m21) +
+				(position.mZ * matrix.m22) +
+				matrix.m23
+			);
+			float w = (
+				(position.mX * matrix.m30) +
+				(position.mY * matrix.m31) +
+				(position.mZ * matrix.m32) +
+				matrix.m33
+			);
+			result.mX = x;
+			result.mY = y;
+			result.mZ = z;
+			result.mW = w;
+		}
+
+		/// <summary>
+		/// Creates a new <see cref="Vector4"/> that contains a transformation of 4d-vector by the specified <see cref="Matrix"/>.
+		/// </summary>
+		/// <param name="value">Source <see cref="Vector4"/>.</param>
+		/// <param name="matrix">The transformation <see cref="Matrix"/>.</param>
+		/// <param name="result">Transformed <see cref="Vector4"/> as an output parameter.</param>
+		public static void Transform(Vector4 vector, Matrix4 matrix, out Vector4 result)
+		{
+			float x = (
+				(vector.mX * matrix.m00) +
+				(vector.mY * matrix.m01) +
+				(vector.mZ * matrix.m02) +
+				(vector.mW * matrix.m03)
+			);
+			float y = (
+				(vector.mX * matrix.m10) +
+				(vector.mY * matrix.m11) +
+				(vector.mZ * matrix.m12) +
+				(vector.mW * matrix.m13)
+			);
+			float z = (
+				(vector.mX * matrix.m20) +
+				(vector.mY * matrix.m21) +
+				(vector.mZ * matrix.m22) +
+				(vector.mW * matrix.m23)
+			);
+			float w = (
+				(vector.mX * matrix.m30) +
+				(vector.mY * matrix.m31) +
+				(vector.mZ * matrix.m32) +
+				(vector.mW * matrix.m33)
+			);
+			result.mX = x;
+			result.mY = y;
+			result.mZ = z;
+			result.mW = w;
+		}
+
+		/// <summary>
+		/// Apply transformation on all vectors within array of <see cref="Vector4"/> by the specified <see cref="Matrix"/> and places the results in an another array.
+		/// </summary>
+		/// <param name="sourceArray">Source array.</param>
+		/// <param name="matrix">The transformation <see cref="Matrix"/>.</param>
+		/// <param name="destinationArray">Destination array.</param>
+		public static void Transform(
+			Vector4[] sourceArray,
+			Matrix4 matrix,
+			Vector4[] destinationArray
+		) {
+			if (sourceArray == null)
+			{
+				Runtime.FatalError("sourceArray");
+			}
+			if (destinationArray == null)
+			{
+				Runtime.FatalError("destinationArray");
+			}
+			if (destinationArray.Count < sourceArray.Count)
+			{
+				Runtime.FatalError(
+					"destinationArray is too small to contain the result."
+				);
+			}
+			for (int i = 0; i < sourceArray.Count; i += 1)
+			{
+				Transform(
+					sourceArray[i],
+					matrix,
+					out destinationArray[i]
+				);
+			}
+		}
+
+		/// <summary>
+		/// Apply transformation on vectors within array of <see cref="Vector4"/> by the specified <see cref="Matrix"/> and places the results in an another array.
+		/// </summary>
+		/// <param name="sourceArray">Source array.</param>
+		/// <param name="sourceIndex">The starting index of transformation in the source array.</param>
+		/// <param name="matrix">The transformation <see cref="Matrix"/>.</param>
+		/// <param name="destinationArray">Destination array.</param>
+		/// <param name="destinationIndex">The starting index in the destination array, where the first <see cref="Vector4"/> should be written.</param>
+		/// <param name="length">The number of vectors to be transformed.</param>
+		public static void Transform(
+			Vector4[] sourceArray,
+			int sourceIndex,
+			Matrix4 matrix,
+			Vector4[] destinationArray,
+			int destinationIndex,
+			int length
+		) {
+			if (sourceArray == null)
+			{
+				Runtime.FatalError("sourceArray");
+			}
+			if (destinationArray == null)
+			{
+				Runtime.FatalError("destinationArray");
+			}
+			if (destinationIndex + length > destinationArray.Count)
+			{
+				Runtime.FatalError(
+					"destinationArray is too small to contain the result."
+				);
+			}
+			if (sourceIndex + length > sourceArray.Count)
+			{
+				Runtime.FatalError(
+					"The combination of sourceIndex and length was greater than sourceArray.Length."
+				);
+			}
+			for (int i = 0; i < length; i += 1)
+			{
+				Transform(
+					sourceArray[i + sourceIndex],
+					matrix,
+					out destinationArray[i + destinationIndex]
+				);
+			}
+		}
+
+		/// <summary>
+		/// Creates a new <see cref="Vector4"/> that contains a transformation of 2d-vector by the specified <see cref="Quaternion"/>.
+		/// </summary>
+		/// <param name="value">Source <see cref="Vector2"/>.</param>
+		/// <param name="rotation">The <see cref="Quaternion"/> which contains rotation transformation.</param>
+		/// <returns>Transformed <see cref="Vector4"/>.</returns>
+		public static Vector4 Transform(Vector2 value, Quaternion rotation)
+		{
+			Vector4 result;
+			Transform(value, rotation, out result);
+			return result;
+		}
+
+		/// <summary>
+		/// Creates a new <see cref="Vector4"/> that contains a transformation of 3d-vector by the specified <see cref="Quaternion"/>.
+		/// </summary>
+		/// <param name="value">Source <see cref="Vector3"/>.</param>
+		/// <param name="rotation">The <see cref="Quaternion"/> which contains rotation transformation.</param>
+		/// <returns>Transformed <see cref="Vector4"/>.</returns>
+		public static Vector4 Transform(Vector3 value, Quaternion rotation)
+		{
+			Vector4 result;
+			Transform(value, rotation, out result);
+			return result;
+		}
+
+		/// <summary>
+		/// Creates a new <see cref="Vector4"/> that contains a transformation of 4d-vector by the specified <see cref="Quaternion"/>.
+		/// </summary>
+		/// <param name="value">Source <see cref="Vector4"/>.</param>
+		/// <param name="rotation">The <see cref="Quaternion"/> which contains rotation transformation.</param>
+		/// <returns>Transformed <see cref="Vector4"/>.</returns>
+		public static Vector4 Transform(Vector4 value, Quaternion rotation)
+		{
+			Vector4 result;
+			Transform(value, rotation, out result);
+			return result;
+		}
+
+		/// <summary>
+		/// Creates a new <see cref="Vector4"/> that contains a transformation of 2d-vector by the specified <see cref="Quaternion"/>.
+		/// </summary>
+		/// <param name="value">Source <see cref="Vector2"/>.</param>
+		/// <param name="rotation">The <see cref="Quaternion"/> which contains rotation transformation.</param>
+		/// <param name="result">Transformed <see cref="Vector4"/> as an output parameter.</param>
+		public static void Transform(
+			Vector2 value,
+			Quaternion rotation,
+			out Vector4 result
+		) {
+			double xx = rotation.mX + rotation.mX;
+			double yy = rotation.mY + rotation.mY;
+			double zz = rotation.mZ + rotation.mZ;
+			double wxx = rotation.mW * xx;
+			double wyy = rotation.mW * yy;
+			double wzz = rotation.mW * zz;
+			double xxx = rotation.mX * xx;
+			double xyy = rotation.mX * yy;
+			double xzz = rotation.mX * zz;
+			double yyy = rotation.mY * yy;
+			double yzz = rotation.mY * zz;
+			double zzz = rotation.mZ * zz;
+			result.mX = (float) (
+				(double) value.mX * (1.0 - yyy - zzz) +
+				(double) value.mY * (xyy - wzz)
+			);
+			result.mY = (float) (
+				(double) value.mX * (xyy + wzz) +
+				(double) value.mY * (1.0 - xxx - zzz)
+			);
+			result.mZ = (float) (
+				(double) value.mX * (xzz - wyy) +
+				(double) value.mY * (yzz + wxx)
+			);
+			result.mW = 1.0f;
+		}
+
+		/// <summary>
+		/// Creates a new <see cref="Vector4"/> that contains a transformation of 3d-vector by the specified <see cref="Quaternion"/>.
+		/// </summary>
+		/// <param name="value">Source <see cref="Vector3"/>.</param>
+		/// <param name="rotation">The <see cref="Quaternion"/> which contains rotation transformation.</param>
+		/// <param name="result">Transformed <see cref="Vector4"/> as an output parameter.</param>
+		public static void Transform(
+			Vector3 value,
+			Quaternion rotation,
+			out Vector4 result
+		) {
+			double xx = rotation.mX + rotation.mX;
+			double yy = rotation.mY + rotation.mY;
+			double zz = rotation.mZ + rotation.mZ;
+			double wxx = rotation.mW * xx;
+			double wyy = rotation.mW * yy;
+			double wzz = rotation.mW * zz;
+			double xxx = rotation.mX * xx;
+			double xyy = rotation.mX * yy;
+			double xzz = rotation.mX * zz;
+			double yyy = rotation.mY * yy;
+			double yzz = rotation.mY * zz;
+			double zzz = rotation.mZ * zz;
+			result.mX = (float) (
+				(double) value.mX * (1.0 - yyy - zzz) +
+				(double) value.mY * (xyy - wzz) +
+				(double) value.mZ * (xzz + wyy)
+			);
+			result.mY = (float) (
+				(double) value.mX * (xyy + wzz) +
+				(double) value.mY * (1.0 - xxx - zzz) +
+				(double) value.mZ * (yzz - wxx)
+			);
+			result.mZ = (float) (
+				(double) value.mX * (xzz - wyy) +
+				(double) value.mY * (yzz + wxx) +
+				(double) value.mZ * (1.0 - xxx - yyy)
+			);
+			result.mW = 1.0f;
+		}
+
+		/// <summary>
+		/// Creates a new <see cref="Vector4"/> that contains a transformation of 4d-vector by the specified <see cref="Quaternion"/>.
+		/// </summary>
+		/// <param name="value">Source <see cref="Vector4"/>.</param>
+		/// <param name="rotation">The <see cref="Quaternion"/> which contains rotation transformation.</param>
+		/// <param name="result">Transformed <see cref="Vector4"/> as an output parameter.</param>
+		public static void Transform(
+			Vector4 value,
+			Quaternion rotation,
+			out Vector4 result
+		) {
+			double xx = rotation.mX + rotation.mX;
+			double yy = rotation.mY + rotation.mY;
+			double zz = rotation.mZ + rotation.mZ;
+			double wxx = rotation.mW * xx;
+			double wyy = rotation.mW * yy;
+			double wzz = rotation.mW * zz;
+			double xxx = rotation.mX * xx;
+			double xyy = rotation.mX * yy;
+			double xzz = rotation.mX * zz;
+			double yyy = rotation.mY * yy;
+			double yzz = rotation.mY * zz;
+			double zzz = rotation.mZ * zz;
+			result.mX = (float) (
+				(double) value.mX * (1.0 - yyy - zzz) +
+				(double) value.mY * (xyy - wzz) +
+				(double) value.mZ * (xzz + wyy)
+			);
+			result.mY = (float) (
+				(double) value.mX * (xyy + wzz) +
+				(double) value.mY * (1.0 - xxx - zzz) +
+				(double) value.mZ * (yzz - wxx)
+			);
+			result.mZ = (float) (
+				(double) value.mX * (xzz - wyy) +
+				(double) value.mY * (yzz + wxx) +
+				(double) value.mZ * (1.0 - xxx - yyy)
+			);
+			result.mW = value.mW;
+		}
+
+		/// <summary>
+		/// Apply transformation on all vectors within array of <see cref="Vector4"/> by the specified <see cref="Quaternion"/> and places the results in an another array.
+		/// </summary>
+		/// <param name="sourceArray">Source array.</param>
+		/// <param name="rotation">The <see cref="Quaternion"/> which contains rotation transformation.</param>
+		/// <param name="destinationArray">Destination array.</param>
+		public static void Transform(
+			Vector4[] sourceArray,
+			Quaternion rotation,
+			Vector4[] destinationArray
+		) {
+			if (sourceArray == null)
+			{
+				Runtime.FatalError("sourceArray");
+			}
+			if (destinationArray == null)
+			{
+				Runtime.FatalError("destinationArray");
+			}
+			if (destinationArray.Count < sourceArray.Count)
+			{
+				Runtime.FatalError(
+					"destinationArray is too small to contain the result."
+				);
+			}
+			for (int i = 0; i < sourceArray.Count; i += 1)
+			{
+				Transform(
+					sourceArray[i],
+					rotation,
+					out destinationArray[i]
+				);
+			}
+		}
+
+		/// <summary>
+		/// Apply transformation on vectors within array of <see cref="Vector4"/> by the specified <see cref="Quaternion"/> and places the results in an another array.
+		/// </summary>
+		/// <param name="sourceArray">Source array.</param>
+		/// <param name="sourceIndex">The starting index of transformation in the source array.</param>
+		/// <param name="rotation">The <see cref="Quaternion"/> which contains rotation transformation.</param>
+		/// <param name="destinationArray">Destination array.</param>
+		/// <param name="destinationIndex">The starting index in the destination array, where the first <see cref="Vector4"/> should be written.</param>
+		/// <param name="length">The number of vectors to be transformed.</param>
+		public static void Transform(
+			Vector4[] sourceArray,
+			int sourceIndex,
+			Quaternion rotation,
+			Vector4[] destinationArray,
+			int destinationIndex,
+			int length
+		) {
+			if (sourceArray == null)
+			{
+				Runtime.FatalError("sourceArray");
+			}
+			if (destinationArray == null)
+			{
+				Runtime.FatalError("destinationArray");
+			}
+			if (destinationIndex + length > destinationArray.Count)
+			{
+				Runtime.FatalError(
+					"destinationArray is too small to contain the result."
+				);
+			}
+			if (sourceIndex + length > sourceArray.Count)
+			{
+				Runtime.FatalError(
+					"The combination of sourceIndex and length was greater than sourceArray.Length."
+				);
+			}
+			for (int i = 0; i < length; i += 1)
+			{
+				Transform(
+					sourceArray[i + sourceIndex],
+					rotation,
+					out destinationArray[i + destinationIndex]
+				);
+			}
+		}
+
+		#endregion
+
+		#region Public Static Operators
+
+		public static Vector4 operator -(Vector4 value)
+		{
+			return Vector4(-value.mX, -value.mY, -value.mZ, -value.mW);
+		}
+
+		public static bool operator ==(Vector4 value1, Vector4 value2)
+		{
+			return (	value1.mX == value2.mX &&
+					value1.mY == value2.mY &&
+					value1.mZ == value2.mZ &&
+					value1.mW == value2.mW	);
+		}
+
+		public static bool operator !=(Vector4 value1, Vector4 value2)
+		{
+			return !(value1 == value2);
+		}
+
+		public static Vector4 operator +(Vector4 value1, Vector4 value2)
+		{
+			var value1;
+			value1.mW += value2.mW;
+			value1.mX += value2.mX;
+			value1.mY += value2.mY;
+			value1.mZ += value2.mZ;
+			return value1;
+		}
+
+		public static Vector4 operator -(Vector4 value1, Vector4 value2)
+		{
+			var value1;
+			value1.mW -= value2.mW;
+			value1.mX -= value2.mX;
+			value1.mY -= value2.mY;
+			value1.mZ -= value2.mZ;
+			return value1;
+		}
+
+		public static Vector4 operator *(Vector4 value1, Vector4 value2)
+		{
+			var value1;
+			value1.mW *= value2.mW;
+			value1.mX *= value2.mX;
+			value1.mY *= value2.mY;
+			value1.mZ *= value2.mZ;
+			return value1;
+		}
+
+		public static Vector4 operator *(Vector4 value1, float scaleFactor)
+		{
+			var value1;
+			value1.mW *= scaleFactor;
+			value1.mX *= scaleFactor;
+			value1.mY *= scaleFactor;
+			value1.mZ *= scaleFactor;
+			return value1;
+		}
+
+		public static Vector4 operator *(float scaleFactor, Vector4 value1)
+		{
+			var value1;
+			value1.mW *= scaleFactor;
+			value1.mX *= scaleFactor;
+			value1.mY *= scaleFactor;
+			value1.mZ *= scaleFactor;
+			return value1;
+		}
+
+		public static Vector4 operator /(Vector4 value1, Vector4 value2)
+		{
+			var value1;
+			value1.mW /= value2.mW;
+			value1.mX /= value2.mX;
+			value1.mY /= value2.mY;
+			value1.mZ /= value2.mZ;
+			return value1;
+		}
+
+		public static Vector4 operator /(Vector4 value1, float divider)
+		{
+			var value1;
+			float factor = 1f / divider;
+			value1.mW *= factor;
+			value1.mX *= factor;
+			value1.mY *= factor;
+			value1.mZ *= factor;
+			return value1;
+		}
+
+		#endregion
+	}
+}

+ 59 - 0
BeefLibs/Beefy2D/src/geom/Viewport.bf

@@ -0,0 +1,59 @@
+// This file contains portions of code from the FNA project (github.com/FNA-XNA/FNA),
+// released under the Microsoft Public License
+
+using Beefy.geom;
+using System.Diagnostics;
+
+namespace Beefy.geom
+{
+	struct Viewport
+	{
+		private int mX;
+		private int mY;
+		private int mWidth;
+		private int mHeight;
+		private float mMinDepth;
+		private float mMaxDepth;
+
+		public this(int x, int y, int width, int height,float minDepth,float maxDepth)
+		{
+		    this.mX = x;
+		    this.mY = y;
+		    this.mWidth = width;
+		    this.mHeight = height;
+		    this.mMinDepth = minDepth;
+		    this.mMaxDepth = maxDepth;
+		}
+
+		/// Unprojects a Vector3 from screen space into world space.
+		/// @param source The Vector3 to unproject.
+		/// @param projection The projection
+		/// @param view The view
+		/// @param world The world
+		public Vector3 Unproject(Vector3 source, Matrix4 projection, Matrix4 view, Matrix4 world)
+		{
+			var source;
+
+		    Matrix4 matrix = Matrix4.Invert(Matrix4.Multiply(Matrix4.Multiply(world, view), projection));
+		    source.mX = (((source.mX - this.mX) / ((float) this.mWidth)) * 2f) - 1f;
+		    source.mY = -((((source.mY - this.mY) / ((float) this.mHeight)) * 2f) - 1f);
+		    source.mZ = (source.mZ - this.mMinDepth) / (this.mMaxDepth - this.mMinDepth);
+		    Vector3 vector = Vector3.Transform(source, matrix);
+		    float a = (((source.mX * matrix.m30) + (source.mY * matrix.m31)) + (source.mZ * matrix.m32)) + matrix.m33;
+		    if (!WithinEpsilon(a, 1f))
+		    {
+		        vector.mX = vector.mX / a;
+		        vector.mY = vector.mY / a;
+		        vector.mZ = vector.mZ / a;
+		    }
+		    return vector;
+
+		}
+
+		private static bool WithinEpsilon(float a, float b)
+		{
+		    float num = a - b;
+		    return ((-1.401298E-45f <= num) && (num <= float.Epsilon));
+		}
+	}
+}

+ 22 - 5
BeefLibs/Beefy2D/src/gfx/Graphics.bf

@@ -442,7 +442,10 @@ namespace Beefy.gfx
         //static unsafe extern void Gfx_DrawIndexedVertices2D(void* vtxData, int vtxCount, int* idxData, int idxCount, float a, float b, float c, float d, float tx, float ty, float z);
 
         [CallingConvention(.Stdcall), CLink]
-        static extern void Gfx_DrawIndexedVertices2D(int32 vertexSize, void* vtxData, int32 vtxCount, uint16* idxData, int32 idxCount, float a, float b, float c, float d, float tx, float ty, float z);
+        static extern void Gfx_DrawIndexedVertices(int32 vertexSize, void* vtxData, int32 vtxCount, uint16* idxData, int32 idxCount);
+
+		[CallingConvention(.Stdcall), CLink]
+		static extern void Gfx_DrawIndexedVertices2D(int32 vertexSize, void* vtxData, int32 vtxCount, uint16* idxData, int32 idxCount, float a, float b, float c, float d, float tx, float ty, float z);
 
         [CallingConvention(.Stdcall), CLink]
         static extern void Gfx_SetShaderConstantData(int32 usageIdx, int32 slotIdx, void* data, int32 size);
@@ -794,14 +797,28 @@ namespace Beefy.gfx
 
         public void DrawIndexedVertices(VertexDefinition vertexDef, void* vertices, int vtxCount, uint16[] indices)
         {
-            Gfx_DrawIndexedVertices2D(vertexDef.mVertexSize, vertices, (int32)vtxCount, indices.CArray(), (int32)indices.Count,
-                mMatrix.a, mMatrix.b, mMatrix.c, mMatrix.d, mMatrix.tx, mMatrix.ty, ZDepth);
+			if (vertexDef.mPosition2DOffset != -1)
+			{
+	            Gfx_DrawIndexedVertices2D(vertexDef.mVertexSize, vertices, (int32)vtxCount, indices.CArray(), (int32)indices.Count,
+	                mMatrix.a, mMatrix.b, mMatrix.c, mMatrix.d, mMatrix.tx, mMatrix.ty, ZDepth);
+			}
+			else
+			{
+				Gfx_DrawIndexedVertices(vertexDef.mVertexSize, vertices, (int32)vtxCount, indices.CArray(), (int32)indices.Count);
+			}
         }
 
         public void DrawIndexedVertices(VertexDefinition vertexDef, void* vertices, int vtxCount, uint16* indices, int idxCount)
         {
-            Gfx_DrawIndexedVertices2D(vertexDef.mVertexSize, vertices, (int32)vtxCount, indices, (int32)idxCount,
-                mMatrix.a, mMatrix.b, mMatrix.c, mMatrix.d, mMatrix.tx, mMatrix.ty, ZDepth);
+			if (vertexDef.mPosition2DOffset != -1)
+			{
+	            Gfx_DrawIndexedVertices2D(vertexDef.mVertexSize, vertices, (int32)vtxCount, indices, (int32)idxCount,
+	                mMatrix.a, mMatrix.b, mMatrix.c, mMatrix.d, mMatrix.tx, mMatrix.ty, ZDepth);
+			}
+			else
+			{
+				Gfx_DrawIndexedVertices(vertexDef.mVertexSize, vertices, (int32)vtxCount, indices, (int32)idxCount);
+			}
         }
 
         public void SetVertexShaderConstantData(int slotIdx, void* data, int size)

+ 45 - 7
BeefLibs/Beefy2D/src/gfx/Model.bf

@@ -48,7 +48,13 @@ namespace Beefy.gfx
 
     #if !STUDIO_CLIENT
     extension ModelDef
-    {        
+    {
+		public enum ModelCreateFlags
+		{
+			None = 0,
+			NoSetRenderState = 1
+		}
+
         public class Animation
         {            
             public void* mNativeModelDefAnimation;
@@ -103,11 +109,20 @@ namespace Beefy.gfx
 		extern static void* Res_OpenModel(char8* fileName, char8* baseDir, void* nativeVertexDef);
 
         [CallingConvention(.Stdcall), CLink]
-        extern static void* ModelDef_CreateModelInstance(void* nativeModel);
+        extern static void* ModelDef_CreateModelInstance(void* nativeModel, ModelCreateFlags flags);
+
+		[CallingConvention(.Stdcall), CLink]
+		extern static void ModelDef_Compact(void* nativeModel);
+
+		[CallingConvention(.Stdcall), CLink]
+		extern static void ModelDef_SetBaseDir(void* nativeModel, char8* baseDir);
 
 		[CallingConvention(.Stdcall), CLink]
 		extern static char8* ModelDef_GetInfo(void* nativeModel);
 
+		[CallingConvention(.Stdcall), CLink]
+		extern static void ModelDef_GetBounds(void* nativeModel, out Vector3 min, out Vector3 max);
+
         [CallingConvention(.Stdcall), CLink]
         extern static float ModelDef_GetFrameRate(void* nativeModel);
 
@@ -123,6 +138,9 @@ namespace Beefy.gfx
 		[CallingConvention(.Stdcall), CLink]
 		extern static void ModelDef_SetTextures(void* nativeModel, int32 meshIdx, int32 primitivesIdx, char8** paths, int32 pathCount);
 
+		[CallingConvention(.Stdcall), CLink]
+		extern static bool ModelDef_RayIntersect(void* nativeModel, Matrix4 worldMtx, Vector3 origin, Vector3 vec, out Vector3 outIntersect, out float outDistance);
+
 		[CallingConvention(.Stdcall), CLink]
 		extern static Span<uint8> Res_SerializeModel(void* nativeModel);
 
@@ -147,20 +165,20 @@ namespace Beefy.gfx
         public static ModelDef LoadModel(String fileName, String baseDir)
         {
 			void* nativeModelDef = null;
-			if (fileName.EndsWith(".bfm", .OrdinalIgnoreCase))
-				nativeModelDef = Res_OpenModel(fileName, baseDir, VertexDef.sVertexDefinition.mNativeVertexDefinition);
+			if (fileName.EndsWith(".gltf", .OrdinalIgnoreCase))
+				nativeModelDef = Res_OpenGLTF(fileName, baseDir, VertexDef.sVertexDefinition.mNativeVertexDefinition);
 			else if (fileName.EndsWith(".fbx", .OrdinalIgnoreCase))
             	nativeModelDef = Res_OpenFBX(fileName, VertexDef.sVertexDefinition.mNativeVertexDefinition);
 			else
-				nativeModelDef = Res_OpenGLTF(fileName, baseDir, VertexDef.sVertexDefinition.mNativeVertexDefinition);
+				nativeModelDef = Res_OpenModel(fileName, baseDir, VertexDef.sVertexDefinition.mNativeVertexDefinition);
             if (nativeModelDef == null)
                 return null;
             return new ModelDef(nativeModelDef);            
         }
 
-        public ModelInstance CreateInstance()
+        public ModelInstance CreateInstance(ModelCreateFlags flags = .None)
         {
-            void* nativeModelInstance = ModelDef_CreateModelInstance(mNativeModelDef);
+            void* nativeModelInstance = ModelDef_CreateModelInstance(mNativeModelDef, flags);
             if (nativeModelInstance == null)
                 return null;
             var modelInstance = new ModelInstance(nativeModelInstance, this);            
@@ -177,6 +195,21 @@ namespace Beefy.gfx
 			str.Append(ModelDef_GetInfo(mNativeModelDef));
 		}
 
+		public void GetBounds(out Vector3 min, out Vector3 max)
+		{
+			ModelDef_GetBounds(mNativeModelDef, out min, out max);
+		}
+
+		public void Compact()
+		{
+			ModelDef_Compact(mNativeModelDef);
+		}
+
+		public void SetBaseDir(StringView baseDir)
+		{
+			ModelDef_SetBaseDir(mNativeModelDef, baseDir.ToScopeCStr!());
+		}
+
 		public void SetTextures(int meshIdx, int primitivesIdx, Span<char8*> paths)
 		{
 			ModelDef_SetTextures(mNativeModelDef, (.)meshIdx, (.)primitivesIdx, paths.Ptr, (.)paths.Length);
@@ -187,6 +220,11 @@ namespace Beefy.gfx
 			var span = Res_SerializeModel(mNativeModelDef);
 			data.AddRange(span);
 		}
+
+		public bool RayIntersect(Matrix4 worldMtx, Vector3 origin, Vector3 vec, out Vector3 outIntersect, out float outDistance)
+		{
+			return ModelDef_RayIntersect(mNativeModelDef, worldMtx, origin, vec, out outIntersect, out outDistance);
+		}	
     }
 
     public class ModelInstance : RenderCmd

+ 28 - 0
BeefLibs/Beefy2D/src/gfx/RenderState.bf

@@ -19,6 +19,12 @@ namespace Beefy.gfx
         Always
     }
 
+	public enum Topology
+	{
+		TriangleList,
+		LineList
+	}
+
 #if !STUDIO_CLIENT
     public class RenderState
     {        
@@ -34,6 +40,9 @@ namespace Beefy.gfx
 		[CallingConvention(.Stdcall), CLink]
 		static extern void RenderState_SetTexWrap(void* renderState, bool texWrap);
 
+		[CallingConvention(.Stdcall), CLink]
+		static extern void RenderState_SetWireframe(void* renderState, bool wireframe);
+
         [CallingConvention(.Stdcall), CLink]
         static extern void RenderState_DisableClip(void* renderState);
 
@@ -46,6 +55,9 @@ namespace Beefy.gfx
         [CallingConvention(.Stdcall), CLink]
         static extern void RenderState_SetDepthWrite(void* nativeRenderState, int32 depthWrite);
 
+		[CallingConvention(.Stdcall), CLink]
+		static extern void RenderState_SetTopology(void* nativeRenderState, int32 topology);
+
         public void* mNativeRenderState;
         public bool mIsFromDefaultRenderState;
 
@@ -115,6 +127,22 @@ namespace Beefy.gfx
 				RenderState_SetTexWrap(mNativeRenderState, value);
 			}
 		}
+
+		public bool Wireframe
+		{
+			set
+			{
+				RenderState_SetWireframe(mNativeRenderState, value);
+			}
+		}
+
+		public Topology Topology
+		{
+			set
+			{
+				RenderState_SetTopology(mNativeRenderState, (.)value);
+			}
+		}
     }
 #else
     public class RenderState

+ 150 - 1
BeefLibs/corlib/src/Math.bf

@@ -24,7 +24,9 @@ namespace System
 
 		private static float[7] sRoundPower10Single = .(
 			1E0f, 1E1f, 1E2f, 1E3f, 1E4f, 1E5f, 1E6f);
-        
+
+		private static float sMachineEpsilonFloat = GetMachineEpsilonFloat();
+
         public const double PI_d = 3.14159265358979323846;
         public const double E_d = 2.7182818284590452354;
 		public const float PI_f = 3.14159265358979323846f;
@@ -48,6 +50,33 @@ namespace System
         public static extern float Floor(float f);
 		public static extern double Floor(double d);
 
+		public static bool WithinEpsilon(float a, float b)
+		{
+			return Math.Abs(a - b) < sMachineEpsilonFloat;
+		}
+
+		/// <summary>
+		/// Find the current machine's Epsilon for the float data type.
+		/// (That is, the largest float, e,  where e == 0.0f is true.)
+		/// </summary>
+		private static float GetMachineEpsilonFloat()
+		{
+			float machineEpsilon = 1.0f;
+			float comparison;
+
+			/* Keep halving the working value of machineEpsilon until we get a number that
+			 * when added to 1.0f will still evaluate as equal to 1.0f.
+			 */
+			repeat
+			{
+				machineEpsilon *= 0.5f;
+				comparison = 1.0f + machineEpsilon;
+			}
+			while (comparison > 1.0f);
+
+			return machineEpsilon;
+		}
+
 		private static float InternalRound(float value, int32 digits, MidpointRounding mode)
 		{
 		    if (Abs(value) < cSingleRoundLimit)
@@ -476,5 +505,125 @@ namespace System
 		{
 			return ((val) + (align - 1)) & ~(align - 1);
 		}
+
+		/// Interpolates between two values using a cubic equation.
+		/// @param name Source value.
+		/// @param name Source value.
+		/// @param name Weighting value.
+		/// @returns Interpolated value.
+		public static float SmoothStep(float value1, float value2, float amount)
+		{
+			/* It is expected that 0 < amount < 1.
+			 * If amount < 0, return value1.
+			 * If amount > 1, return value2.
+			 */
+			float result = Clamp(amount, 0f, 1f);
+			result = Hermite(value1, 0f, value2, 0f, result);
+
+			return result;
+		}
+
+		/// Performs a Hermite spline interpolation.
+		/// @param value1 Source position.
+		/// @param tangent1 Source tangent.
+		/// @param value2 Source position.
+		/// @param tangent2 Source tangent.
+		/// @param amount Weighting factor.
+		/// @returns The result of the Hermite spline interpolation.
+		public static float Hermite(
+			float value1,
+			float tangent1,
+			float value2,
+			float tangent2,
+			float amount
+		) {
+			/* All transformed to double not to lose precision
+			 * Otherwise, for high numbers of param:amount the result is NaN instead
+			 * of Infinity.
+			 */
+			double v1 = value1, v2 = value2, t1 = tangent1, t2 = tangent2, s = amount;
+			double result;
+			double sCubed = s * s * s;
+			double sSquared = s * s;
+
+			if (WithinEpsilon(amount, 0f))
+			{
+				result = value1;
+			}
+			else if (WithinEpsilon(amount, 1f))
+			{
+				result = value2;
+			}
+			else
+			{
+				result = (
+					((2 * v1 - 2 * v2 + t2 + t1) * sCubed) +
+					((3 * v2 - 3 * v1 - 2 * t1 - t2) * sSquared) +
+					(t1 * s) +
+					v1
+				);
+			}
+
+			return (float) result;
+		}
+
+		/// Returns the Cartesian coordinate for one axis of a point that is defined by a
+		/// given triangle and two normalized barycentric (areal) coordinates.
+		/// <param name="value1">
+		/// The coordinate on one axis of vertex 1 of the defining triangle.
+		/// </param>
+		/// <param name="value2">
+		/// The coordinate on the same axis of vertex 2 of the defining triangle.
+		/// </param>
+		/// <param name="value3">
+		/// The coordinate on the same axis of vertex 3 of the defining triangle.
+		/// </param>
+		/// <param name="amount1">
+		/// The normalized barycentric (areal) coordinate b2, equal to the weighting factor
+		/// for vertex 2, the coordinate of which is specified in value2.
+		/// </param>
+		/// @param amount2
+		/// The normalized barycentric (areal) coordinate b3, equal to the weighting factor
+		/// for vertex 3, the coordinate of which is specified in value3.
+		/// </param>
+		/// @returns Cartesian coordinate of the specified point with respect to the axis being used.
+		public static float Barycentric(
+			float value1,
+			float value2,
+			float value3,
+			float amount1,
+			float amount2
+		) {
+			return value1 + (value2 - value1) * amount1 + (value3 - value1) * amount2;
+		}
+
+		/// Performs a Catmull-Rom interpolation using the specified positions.
+		/// @param value1 The first position in the interpolation.
+		/// @param value2">The second position in the interpolation.
+		/// @param value3">The third position in the interpolation.
+		/// @param value4">The fourth position in the interpolation.
+		/// @param name="amount">Weighting factor.
+		/// @returns A position that is the result of the Catmull-Rom interpolation.
+		public static float CatmullRom(
+			float value1,
+			float value2,
+			float value3,
+			float value4,
+			float amount
+		) {
+			/* Using formula from http://www.mvps.org/directx/articles/catmull/
+			 * Internally using doubles not to lose precision.
+			 */
+			double amountSquared = amount * amount;
+			double amountCubed = amountSquared * amount;
+			return (float) (
+				0.5 *
+				(
+					((2.0 * value2 + (value3 - value1) * amount) +
+					((2.0 * value1 - 5.0 * value2 + 4.0 * value3 - value4) * amountSquared) +
+					(3.0 * value2 - value1 - 3.0 * value3 + value4) * amountCubed)
+				)
+			);
+		}
     }
 }

+ 27 - 0
BeefySysLib/BeefySysLib.cpp

@@ -610,6 +610,23 @@ BF_EXPORT void BF_CALLTYPE Gfx_DrawQuads(TextureSegment* textureSegment, Default
 	}
 }
 
+BF_EXPORT void BF_CALLTYPE Gfx_DrawIndexedVertices(int vertexSize, void* vtxData, int vtxCount, uint16* idxData, int idxCount)
+{
+	DrawLayer* drawLayer = gBFApp->mRenderDevice->mCurDrawLayer;
+
+	uint16 idxOfs;
+	void* drawBatchVtxPtr;
+	uint16* drawBatchIdxPtr;
+	gBFApp->mRenderDevice->mCurDrawLayer->AllocIndexed(vtxCount, idxCount, (void**)&drawBatchVtxPtr, &drawBatchIdxPtr, &idxOfs);
+	BF_ASSERT(gBFApp->mRenderDevice->mCurDrawLayer->mCurDrawBatch->mVtxSize == vertexSize);
+
+	uint16* idxPtr = idxData;
+	for (int idxIdx = 0; idxIdx < idxCount; idxIdx++)
+		*(drawBatchIdxPtr++) = *(idxPtr++) + idxOfs;
+
+	memcpy(drawBatchVtxPtr, vtxData, vertexSize * vtxCount);	
+}
+
 BF_EXPORT void BF_CALLTYPE Gfx_DrawIndexedVertices2D(int vertexSize, void* vtxData, int vtxCount, uint16* idxData, int idxCount, float a, float b, float c, float d, float tx, float ty, float z)
 {
 	DrawLayer* drawLayer = gBFApp->mRenderDevice->mCurDrawLayer;
@@ -683,6 +700,11 @@ BF_EXPORT void BF_CALLTYPE RenderState_SetTexWrap(RenderState* renderState, bool
 	renderState->SetTexWrap(texWrap);
 }
 
+BF_EXPORT void BF_CALLTYPE RenderState_SetWireframe(RenderState* renderState, bool wireframe)
+{
+	renderState->SetWireframe(wireframe);
+}
+
 BF_EXPORT void BF_CALLTYPE RenderState_SetClip(RenderState* renderState, float x, float y, float width, float height)
 {	
 	BF_ASSERT((width >= 0) && (height >= 0));
@@ -720,6 +742,11 @@ BF_EXPORT void BF_CALLTYPE RenderState_SetDepthWrite(RenderState* renderState, i
 	renderState->SetWriteDepthBuffer(depthWrite != 0);
 }
 
+BF_EXPORT void BF_CALLTYPE RenderState_SetTopology(RenderState* renderState, int topology)
+{
+	renderState->SetTopology((Topology3D)topology);
+}
+
 BF_EXPORT Shader* BF_CALLTYPE Gfx_LoadShader(const char* fileName, VertexDefinition* vertexDefinition)
 {
 	return gBFApp->mRenderDevice->LoadShader(fileName, vertexDefinition);

+ 4 - 0
BeefySysLib/BeefySysLib.vcxproj

@@ -1936,11 +1936,13 @@ copy /y "$(OutDir)$(TargetName).lib" "$(SolutionDir)\BeefLibs\Beefy2D\dist\"</Co
     <ClCompile Include="util\Heap.cpp" />
     <ClCompile Include="util\Json.cpp" />
     <ClCompile Include="util\MappedFile.cpp" />
+    <ClCompile Include="util\MathUtils.cpp" />
     <ClCompile Include="util\Matrix4.cpp" />
     <ClCompile Include="util\PerfTimer.cpp" />
     <ClCompile Include="util\Point.cpp" />
     <ClCompile Include="util\PolySpline.cpp" />
     <ClCompile Include="util\Quaternion.cpp" />
+    <ClCompile Include="util\Sphere.cpp" />
     <ClCompile Include="util\StackHelper.cpp" />
     <ClCompile Include="util\String.cpp" />
     <ClCompile Include="util\ThreadPool.cpp" />
@@ -2166,6 +2168,7 @@ copy /y "$(OutDir)$(TargetName).lib" "$(SolutionDir)\BeefLibs\Beefy2D\dist\"</Co
     <ClInclude Include="util\Heap.h" />
     <ClInclude Include="util\Json.h" />
     <ClInclude Include="util\MappedFile.h" />
+    <ClInclude Include="util\MathUtils.h" />
     <ClInclude Include="util\Matrix4.h" />
     <ClInclude Include="util\MultiHashSet.h" />
     <ClInclude Include="util\PerfTimer.h" />
@@ -2176,6 +2179,7 @@ copy /y "$(OutDir)$(TargetName).lib" "$(SolutionDir)\BeefLibs\Beefy2D\dist\"</Co
     <ClInclude Include="util\Rect.h" />
     <ClInclude Include="util\SizedArray.h" />
     <ClInclude Include="util\SLIList.h" />
+    <ClInclude Include="util\Sphere.h" />
     <ClInclude Include="util\StackHelper.h" />
     <ClInclude Include="util\String.h" />
     <ClInclude Include="util\ThreadPool.h" />

+ 12 - 0
BeefySysLib/BeefySysLib.vcxproj.filters

@@ -713,6 +713,12 @@
     <ClCompile Include="util\Json.cpp">
       <Filter>src\util</Filter>
     </ClCompile>
+    <ClCompile Include="util\MathUtils.cpp">
+      <Filter>src\util</Filter>
+    </ClCompile>
+    <ClCompile Include="util\Sphere.cpp">
+      <Filter>src\util</Filter>
+    </ClCompile>
   </ItemGroup>
   <ItemGroup>
     <ClInclude Include="Common.h">
@@ -1087,6 +1093,12 @@
     <ClInclude Include="util\Json.h">
       <Filter>src\util</Filter>
     </ClInclude>
+    <ClInclude Include="util\MathUtils.h">
+      <Filter>src\util</Filter>
+    </ClInclude>
+    <ClInclude Include="util\Sphere.h">
+      <Filter>src\util</Filter>
+    </ClInclude>
   </ItemGroup>
   <ItemGroup>
     <CustomBuild Include="third_party\libffi\i686-pc-cygwin\src\x86\win32.asm">

+ 488 - 3
BeefySysLib/gfx/ModelDef.cpp

@@ -6,6 +6,10 @@
 #include "util/TLSingleton.h"
 #include "FileStream.h"
 #include "MemStream.h"
+#include "util/MathUtils.h"
+#include "util/Sphere.h"
+#include "util/HashSet.h"
+#include "util/BeefPerf.h"
 
 #pragma warning(disable:4190)
 
@@ -50,14 +54,30 @@ void Beefy::ModelAnimation::GetJointTranslation(int jointIdx, float frameNum, Mo
 
 //
 
-BF_EXPORT ModelInstance* BF_CALLTYPE ModelDef_CreateModelInstance(ModelDef* modelDef)
+BF_EXPORT ModelInstance* BF_CALLTYPE ModelDef_CreateModelInstance(ModelDef* modelDef, ModelCreateFlags flags)
 {
-	return gBFApp->mRenderDevice->CreateModelInstance(modelDef);
+	return gBFApp->mRenderDevice->CreateModelInstance(modelDef, flags);
+}
+
+BF_EXPORT void ModelDef_Compact(ModelDef* modelDef)
+{
+	modelDef->Compact();
+}
+
+BF_EXPORT void ModelDef_GetBounds(ModelDef* modelDef, Vector3& min, Vector3& max)
+{
+	modelDef->GetBounds(min, max);
+}
+
+BF_EXPORT void ModelDef_SetBaseDir(ModelDef* modelDef, char* baseDIr)
+{
+	modelDef->mLoadDir = baseDIr;
 }
 
 BF_EXPORT const char* BF_CALLTYPE ModelDef_GetInfo(ModelDef* modelDef)
 {
 	String& outString = *gModelDef_TLStrReturn.Get();
+	outString.Clear();
 	for (int meshIdx = 0; meshIdx < (int)modelDef->mMeshes.mSize; meshIdx++)	
 	{
 		auto mesh = modelDef->mMeshes[meshIdx];
@@ -105,6 +125,11 @@ BF_EXPORT void BF_CALLTYPE ModelDef_SetTextures(ModelDef* modelDef, int32 meshId
 		prims.mTexPaths.Add(paths[i]);
 }
 
+BF_EXPORT bool BF_CALLTYPE ModelDef_RayIntersect(ModelDef* modelDef, const Matrix4& worldMtx, const Vector3& origin, const Vector3& vec, Vector3& outIntersect, float& outDistance)
+{
+	return modelDef->RayIntersect(worldMtx, origin, vec, outIntersect, outDistance);
+}
+
 BF_EXPORT void BF_CALLTYPE ModelDefAnimation_GetJointTranslation(ModelAnimation* modelAnimation, int jointIdx, float frame, ModelJointTranslation* outJointTranslation)
 {
 	modelAnimation->GetJointTranslation(jointIdx, frame, outJointTranslation);
@@ -126,6 +151,11 @@ BF_EXPORT void BF_CALLTYPE ModelDefAnimation_Clip(ModelAnimation* modelAnimation
 	modelAnimation->mFrames.RemoveRange(numFrames, modelAnimation->mFrames.Count() - numFrames);
 }
 
+ModelDef::ModelDef()
+{
+	mFlags = Flags_None;
+}
+
 ModelDef::~ModelDef()
 {
 	for (auto& materialInstance : mMaterials)
@@ -134,6 +164,459 @@ ModelDef::~ModelDef()
 	}
 }
 
+void ModelDef::Compact()
+{
+	for (auto& mesh : mMeshes)
+	{
+		for (auto& prims : mesh.mPrimitives)
+		{
+			Array<int> vtxMap;
+			vtxMap.Insert(0, -1, prims.mVertices.mSize);			
+			int newVtxIdx = 0;
+
+			for (uint16 vtxIdx : prims.mIndices)
+			{
+				if (vtxMap[vtxIdx] == -1)				
+					vtxMap[vtxIdx] = newVtxIdx++;
+			}
+
+			if (newVtxIdx >= prims.mVertices.mSize)
+				continue;
+
+			for (uint16& vtxIdx : prims.mIndices)
+				vtxIdx = vtxMap[vtxIdx];
+
+			Array<ModelVertex> oldVertices = prims.mVertices;
+			prims.mVertices.Resize(newVtxIdx);
+			for (int oldVtxIdx = 0; oldVtxIdx < oldVertices.mSize; oldVtxIdx++)
+			{
+				int newVtxIdx = vtxMap[oldVtxIdx];
+				if (newVtxIdx != -1)
+					prims.mVertices[newVtxIdx] = oldVertices[oldVtxIdx];
+			}
+		}
+	}	
+}
+
+void ModelDef::CalcBounds()
+{
+	int vtxCount = 0;
+
+	for (auto& mesh : mMeshes)
+	{
+		for (auto& prims : mesh.mPrimitives)
+		{
+			for (auto& vtx : prims.mVertices)
+			{
+				if (vtxCount == 0)
+				{
+					mBounds.mMin = vtx.mPosition;
+					mBounds.mMax = vtx.mPosition;
+				}
+				else
+				{
+					mBounds.mMin.mX = BF_MIN(mBounds.mMin.mX, vtx.mPosition.mX);
+					mBounds.mMin.mY = BF_MIN(mBounds.mMin.mY, vtx.mPosition.mY);
+					mBounds.mMin.mZ = BF_MIN(mBounds.mMin.mZ, vtx.mPosition.mZ);
+
+					mBounds.mMax.mX = BF_MAX(mBounds.mMax.mX, vtx.mPosition.mX);
+					mBounds.mMax.mY = BF_MAX(mBounds.mMax.mY, vtx.mPosition.mY);
+					mBounds.mMax.mZ = BF_MAX(mBounds.mMax.mZ, vtx.mPosition.mZ);
+				}
+
+				vtxCount++;
+			}
+		}
+	}
+
+	mFlags = (Flags)(mFlags | Flags_HasBounds);
+}
+
+void ModelDef::GetBounds(Vector3& min, Vector3& max)
+{
+	if ((mFlags & Flags_HasBounds) == 0)
+		CalcBounds();
+
+	min = mBounds.mMin;
+	max = mBounds.mMax;	
+}
+
+#define SWAP(x, y) { auto temp = x; x = y; y = temp; }
+#define N (sizeof(A)/sizeof(A[0]))
+
+// Partition using Lomuto partition scheme
+static int partition(float a[], int left, int right, int pIndex)
+{
+	// pick `pIndex` as a pivot from the array
+	float pivot = a[pIndex];
+
+	// Move pivot to end
+	SWAP(a[pIndex], a[right]);
+
+	// elements less than the pivot will be pushed to the left of `pIndex`;
+	// elements more than the pivot will be pushed to the right of `pIndex`;
+	// equal elements can go either way
+	pIndex = left;
+
+	// each time we find an element less than or equal to the pivot, `pIndex`
+	// is incremented, and that element would be placed before the pivot.
+	for (int i = left; i < right; i++)
+	{
+		if (a[i] <= pivot)
+		{
+			SWAP(a[i], a[pIndex]);
+			pIndex++;
+		}
+	}
+
+	// move pivot to its final place
+	SWAP(a[pIndex], a[right]);
+
+	// return `pIndex` (index of the pivot element)
+	return pIndex;
+}
+
+// Returns the k'th smallest element in the list within `left…right`
+// (i.e., `left <= k <= right`). The search space within the array is
+// changing for each round – but the list is still the same size.
+// Thus, `k` does not need to be updated with each round.
+static float quickselect(float A[], int left, int right, int k)
+{
+	// If the array contains only one element, return that element
+	if (left == right) {
+		return A[left];
+	}
+
+	// select `pIndex` between left and right
+	int pIndex = left + rand() % (right - left + 1);
+
+	pIndex = partition(A, left, right, pIndex);
+
+	// The pivot is in its final sorted position
+	if (k == pIndex) {
+		return A[k];
+	}
+
+	// if `k` is less than the pivot index
+	else if (k < pIndex) {
+		return quickselect(A, left, pIndex - 1, k);
+	}
+
+	// if `k` is more than the pivot index
+	else {
+		return quickselect(A, pIndex + 1, right, k);
+	}
+}
+
+void ModelDef::GenerateCollisionData()
+{
+	BP_ZONE("ModelDef::GenerateCollisionData");
+
+	mFlags = (Flags)(mFlags | Flags_HasBVH);
+
+	int statsWorkItrs = 0;
+	int statsWorkTris = 0;
+
+	BF_ASSERT(mBVNodes.IsEmpty());
+			
+	struct _WorkEntry
+	{
+		int mParentNodeIdx;		
+		int mTriWorkIdx;
+		int mTriWorkCount;		
+	};
+
+	Array<int32> triWorkList;
+	
+	int triCount = 0;
+	for (auto& mesh : mMeshes)
+	{
+		for (auto& prims : mesh.mPrimitives)
+		{
+			int startIdx = mBVIndices.mSize;
+			triCount += prims.mIndices.mSize / 3;
+			triWorkList.Reserve(triWorkList.mSize + triCount);
+			mBVIndices.Reserve(mBVIndices.mSize + prims.mIndices.mSize);
+			mBVVertices.Reserve(mBVVertices.mSize + prims.mVertices.mSize);
+
+			for (int triIdx = 0; triIdx < triCount; triIdx++)
+				triWorkList.Add(startIdx / 3 + triIdx);
+
+			for (auto idx : prims.mIndices)
+			{
+				mBVIndices.Add(idx + startIdx);
+			}
+
+			for (auto& vtx : prims.mVertices)
+				mBVVertices.Add(vtx.mPosition);
+		}
+	}
+
+	Array<_WorkEntry> workList;
+	
+ 	_WorkEntry workEntry;
+	workEntry.mParentNodeIdx = -1; 	
+	workEntry.mTriWorkIdx = 0;
+	workEntry.mTriWorkCount = triWorkList.mSize;
+	workList.Add(workEntry);
+
+	Array<Vector3> points;
+	points.Reserve(triWorkList.mSize);	
+	Array<float> centers;
+	centers.Reserve(triWorkList.mSize);
+	Array<int> left;
+	left.Reserve(triWorkList.mSize);
+	Array<int> right;
+	right.Reserve(triWorkList.mSize);	
+	
+	mBVTris.Reserve(triWorkList.mSize * 2);
+
+	while (!workList.IsEmpty())
+	{
+		auto workEntry = workList.back();
+		workList.pop_back();
+
+		statsWorkItrs++;
+		statsWorkTris += workEntry.mTriWorkCount;
+		
+		centers.Clear();
+		left.Clear();
+		right.Clear();
+
+		int nodeIdx = mBVNodes.mSize;
+		mBVNodes.Add(ModelBVNode());
+
+		if (workEntry.mParentNodeIdx != -1)
+		{
+			auto& bvParent = mBVNodes[workEntry.mParentNodeIdx];
+			if (bvParent.mLeft == -1)
+				bvParent.mLeft = nodeIdx;
+			else
+			{
+				BF_ASSERT(bvParent.mRight == -1);
+				bvParent.mRight = nodeIdx;
+			}
+		}
+
+		for (int triIdxIdx = 0; triIdxIdx < workEntry.mTriWorkCount; triIdxIdx++)
+		{
+			bool inLeft = false;
+			bool inRight = false;
+
+			int triIdx = triWorkList.mVals[workEntry.mTriWorkIdx + triIdxIdx];
+			for (int triVtxIdx = 0; triVtxIdx < 3; triVtxIdx++)
+			{
+				int vtxIdx = mBVIndices.mVals[triIdx * 3 + triVtxIdx];
+				bool isNewVtx = false;
+
+				int32* valuePtr = NULL;				
+				auto& vtx = mBVVertices[vtxIdx];
+
+				auto& bvNode = mBVNodes[nodeIdx];
+				if ((triIdxIdx == 0) && (triVtxIdx == 0))
+				{
+					bvNode.mBoundAABB.mMin = vtx;
+					bvNode.mBoundAABB.mMax = vtx;
+				}
+				else
+				{
+					bvNode.mBoundAABB.mMin.mX = BF_MIN(bvNode.mBoundAABB.mMin.mX, vtx.mX);
+					bvNode.mBoundAABB.mMin.mY = BF_MIN(bvNode.mBoundAABB.mMin.mY, vtx.mY);
+					bvNode.mBoundAABB.mMin.mZ = BF_MIN(bvNode.mBoundAABB.mMin.mZ, vtx.mZ);
+
+					bvNode.mBoundAABB.mMax.mX = BF_MAX(bvNode.mBoundAABB.mMax.mX, vtx.mX);
+					bvNode.mBoundAABB.mMax.mY = BF_MAX(bvNode.mBoundAABB.mMax.mY, vtx.mY);
+					bvNode.mBoundAABB.mMax.mZ = BF_MAX(bvNode.mBoundAABB.mMax.mZ, vtx.mZ);
+				}				
+			}
+		}
+
+		//mBVNodes[nodeIdx].mBoundSphere = Sphere::MiniBall(points.mVals, points.mSize);
+
+		bool didSplit = false;
+
+		if (workEntry.mTriWorkCount > 4)
+		{
+			int splitPlane = 0;
+			float splitWidth = 0;
+
+			// Split along widest AABB dimension
+			for (int dimIdx = 0; dimIdx < 3; dimIdx++)
+			{
+				float minVal = 0;
+				float maxVal = 0;
+
+				for (int triIdxIdx = 0; triIdxIdx < workEntry.mTriWorkCount; triIdxIdx++)
+				{
+					int triIdx = triWorkList.mVals[workEntry.mTriWorkIdx + triIdxIdx];
+					for (int triVtxIdx = 0; triVtxIdx < 3; triVtxIdx++)
+					{
+						int vtxIdx = mBVIndices.mVals[triIdx * 3 + triVtxIdx];
+						const Vector3& vtx = mBVVertices.mVals[vtxIdx];
+
+						float coord = ((float*)&vtx)[dimIdx];
+						if ((triIdxIdx == 0) && (triVtxIdx == 0))
+						{
+							minVal = coord;
+							maxVal = coord;
+						}
+						else
+						{
+							minVal = BF_MIN(minVal, coord);
+							maxVal = BF_MAX(maxVal, coord);
+						}
+					}
+				}
+
+				float width = maxVal - minVal;
+				if (width > splitWidth)
+				{
+					splitPlane = dimIdx;
+					splitWidth = width;
+				}
+			}						
+
+			centers.SetSize(workEntry.mTriWorkCount);
+			for (int triIdxIdx = 0; triIdxIdx < workEntry.mTriWorkCount; triIdxIdx++)
+			{
+				int triIdx = triWorkList.mVals[workEntry.mTriWorkIdx + triIdxIdx];
+				float coordAcc = 0;
+				for (int triVtxIdx = 0; triVtxIdx < 3; triVtxIdx++)
+				{
+					int vtxIdx = mBVIndices.mVals[triIdx * 3 + triVtxIdx];
+					const Vector3& vtx = mBVVertices.mVals[vtxIdx];
+					float coord = ((float*)&vtx)[splitPlane];
+					coordAcc += coord;
+				}
+
+				float coordAvg = coordAcc / 3;
+				centers.mVals[triIdxIdx] = coordAvg;
+			}
+
+			float centerCoord = quickselect(centers.mVals, 0, centers.mSize - 1, centers.mSize / 2);
+
+// 			centers.Sort([](float lhs, float rhs) { return lhs < rhs; });
+// 			centerCoord = centers[centers.mSize / 2];						
+
+			for (int triIdxIdx = 0; triIdxIdx < workEntry.mTriWorkCount; triIdxIdx++)
+			{
+				bool inLeft = false;
+				bool inRight = false;
+
+				int triIdx = triWorkList.mVals[workEntry.mTriWorkIdx + triIdxIdx];
+				for (int triVtxIdx = 0; triVtxIdx < 3; triVtxIdx++)
+				{
+					int vtxIdx = mBVIndices.mVals[triIdx * 3 + triVtxIdx];
+					const Vector3& vtx = mBVVertices.mVals[vtxIdx];
+					float coord = ((float*)&vtx)[splitPlane];
+
+					if (coord < centerCoord)
+						inLeft = true;
+					else
+						inRight = true;
+				}
+
+				if (inLeft)
+					left.Add(triIdx);
+				if (inRight)
+					right.Add(triIdx);
+			}
+
+			// Don't split if the split didn't significantly separate the triangles
+			bool doSplit =
+				(left.mSize <= workEntry.mTriWorkCount * 0.85f) &&
+				(right.mSize <= workEntry.mTriWorkCount * 0.85f);
+
+			if (doSplit)
+			{				
+				mBVNodes[nodeIdx].mKind = ModelBVNode::Kind_Branch;
+				mBVNodes[nodeIdx].mLeft = -1;
+				mBVNodes[nodeIdx].mRight = -1;
+						
+				_WorkEntry childWorkEntry;
+								
+				childWorkEntry.mParentNodeIdx = nodeIdx;
+				childWorkEntry.mTriWorkIdx = triWorkList.mSize;
+				childWorkEntry.mTriWorkCount = right.mSize;
+				workList.Add(childWorkEntry);
+				triWorkList.Insert(triWorkList.mSize, right.mVals, right.mSize);											
+
+				childWorkEntry.mParentNodeIdx = nodeIdx;
+				childWorkEntry.mTriWorkIdx = triWorkList.mSize;
+				childWorkEntry.mTriWorkCount = left.mSize;
+				workList.Add(childWorkEntry);
+				triWorkList.Insert(triWorkList.mSize, left.mVals, left.mSize);
+
+				continue;
+			}
+		}
+
+		// Did not split	
+		int triStartIdx = mBVTris.mSize;
+		//mBVTris.Reserve(mBVTris.mSize + workEntry.mTriWorkCount);
+		for (int triIdxIdx = 0; triIdxIdx < workEntry.mTriWorkCount; triIdxIdx++)
+			mBVTris.Add(triWorkList[workEntry.mTriWorkIdx + triIdxIdx]);
+
+		auto& bvNode = mBVNodes[nodeIdx];
+		bvNode.mKind = ModelBVNode::Kind_Leaf;
+		bvNode.mTriStartIdx = triStartIdx;
+		bvNode.mTriCount = workEntry.mTriWorkCount;
+	}
+
+	NOP;
+}
+
+void ModelDef::RayIntersect(ModelBVNode* bvNode, const Matrix4& worldMtx, const Vector3& origin, const Vector3& vec, Vector3& outIntersect, float& outDistance)
+{	
+// 	if (!RayIntersectsCircle(origin, vec, bvNode->mBoundSphere, NULL, NULL, NULL))
+// 		return false;
+	if (!RayIntersectsAABB(origin, vec, bvNode->mBoundAABB, NULL, NULL, NULL))
+		return;
+
+	if (bvNode->mKind == ModelBVNode::Kind_Branch)
+	{
+		bool hadIntersect = false;
+		for (int branchIdx = 0; branchIdx < 2; branchIdx++)		
+			RayIntersect(&mBVNodes[(branchIdx == 0) ? bvNode->mLeft : bvNode->mRight], worldMtx, origin, vec, outIntersect, outDistance);		
+		return;
+	}
+	
+	for (int triIdxIdx = 0; triIdxIdx < bvNode->mTriCount; triIdxIdx++)
+	{
+		int triIdx = mBVTris[bvNode->mTriStartIdx + triIdxIdx];
+
+		Vector3 curIntersect;
+		float curDistance;
+		if (RayIntersectsTriangle(origin, vec, 
+			mBVVertices[mBVIndices[triIdx*3+0]], mBVVertices[mBVIndices[triIdx*3+1]], mBVVertices[mBVIndices[triIdx*3+2]],
+			&curIntersect, &curDistance))
+		{
+			if (curDistance < outDistance)
+			{
+				outIntersect = curIntersect;
+				outDistance = curDistance;
+			}
+		}
+	}		
+}
+
+bool ModelDef::RayIntersect(const Matrix4& worldMtx, const Vector3& origin, const Vector3& vec, Vector3& outIntersect, float& outDistance)
+{
+	if ((mFlags & Flags_HasBounds) == 0)
+		CalcBounds();
+
+	if (!RayIntersectsAABB(origin, vec, mBounds, NULL, NULL, NULL))
+		return false;
+
+	if (mBVNodes.IsEmpty())
+		GenerateCollisionData();
+
+	const float maxDist = 3.40282e+038f;
+	outDistance = maxDist;
+	RayIntersect(&mBVNodes[0], worldMtx, origin, vec, outIntersect, outDistance);	
+	return outDistance != maxDist;
+}
+
 ModelMaterialDef* ModelMaterialDef::CreateOrGet(const StringImpl& prefix, const StringImpl& path)
 {	
 	StringT<128> key = prefix;
@@ -211,7 +694,7 @@ public:
 
 	bool ReadFile(const StringImpl& fileName, const StringImpl& baseDir)
 	{	
-		if (fileName.Contains(':'))
+		if (fileName.StartsWith('@'))
 		{
 			int colon = (int)fileName.IndexOf(':');
 			String addrStr = fileName.Substring(1, colon - 1);
@@ -286,5 +769,7 @@ BF_EXPORT StringView BF_CALLTYPE Res_SerializeModel(ModelDef* modelDef)
 	}
 	
 	String& outString = *gModelDef_TLStrReturn.Get();
+	outString.Clear();
+	outString.Append((char*)ms.GetPtr(), ms.GetSize());
 	return outString;
 }

+ 69 - 5
BeefySysLib/gfx/ModelDef.h

@@ -5,6 +5,8 @@
 #include "util/Vector.h"
 #include "util/Array.h"
 #include "gfx/Texture.h"
+#include "util/Sphere.h"
+#include "util/MathUtils.h"
 #include <vector>
 
 NS_BF_BEGIN;
@@ -151,8 +153,6 @@ class ModelMesh
 {
 public:
 	String mName;	
-	//String mTexFileName;
-	//String mBumpFileName;	
 	Array<ModelPrimitives> mPrimitives;
 };
 
@@ -166,19 +166,83 @@ public:
 	Array<ModelNode*> mChildren;
 };
 
+class ModelBVNode
+{	
+public:
+	enum Kind
+	{
+		Kind_None,
+		Kind_Branch,
+		Kind_Leaf
+	};
+
+public:
+	Sphere mBoundSphere;
+	AABB mBoundAABB;
+
+	union
+	{
+		struct 
+		{
+			int mLeft;
+			int mRight;
+		};
+
+		struct  
+		{
+			int mTriStartIdx;
+			int mTriCount;
+		};
+	};
+
+	Kind mKind;
+
+public:
+	ModelBVNode()
+	{
+		mKind = Kind_None;
+	}
+};
+
 class ModelDef
 {
+public:
+	enum Flags
+	{
+		Flags_None,
+		Flags_HasBounds,
+		Flags_HasBVH,
+	};
+
 public:
 	String mLoadDir;
-	float mFrameRate;
+	float mFrameRate;	
 	Array<ModelMesh> mMeshes;
 	Array<ModelJoint> mJoints;
 	Array<ModelAnimation> mAnims;
 	Array<ModelNode> mNodes;
 	Array<ModelMaterialInstance> mMaterials;
-
-public:
+	
+	Flags mFlags;
+	AABB mBounds;
+	Array<ModelBVNode> mBVNodes;
+	Array<uint16> mBVIndices;
+	Array<Vector3> mBVVertices;
+	Array<int32> mBVTris;
+
+protected:
+	void CalcBounds();
+	void RayIntersect(ModelBVNode* bvNode, const Matrix4& worldMtx, const Vector3& origin, const Vector3& vec, Vector3& outIntersect, float& outDistance);
+	
+public:
+	ModelDef();
 	~ModelDef();
+
+	void Compact();
+	void GetBounds(Vector3& min, Vector3& max);
+
+	void GenerateCollisionData();
+	bool RayIntersect(const Matrix4& worldMtx, const Vector3& origin, const Vector3& vec, Vector3& outIntersect, float& outDistance);
 };
 
 NS_BF_END;

+ 2 - 0
BeefySysLib/gfx/RenderDevice.cpp

@@ -19,9 +19,11 @@ RenderState::RenderState()
 	mWriteDepthBuffer = false;
 	mCullMode = CullMode_None;
 	mDepthFunc = DepthFunc_Always;
+	mTopology = Topology3D_TriangleList;
 	mShader = NULL;	
 	mClipped = false;
 	mTexWrap = false;
+	mWireframe = false;
 }
 
 RenderTarget::RenderTarget()

+ 17 - 1
BeefySysLib/gfx/RenderDevice.h

@@ -143,6 +143,12 @@ enum CullMode : int8
 	CullMode_Back
 };
 
+enum Topology3D : int8
+{
+	Topology3D_TriangleList,
+	Topology3D_LineLine
+};
+
 enum TextureFlag : int8
 {
 	TextureFlag_Additive = 1,
@@ -183,8 +189,10 @@ public:
 	DepthFunc				mDepthFunc;	
 	bool					mClipped;
 	bool					mTexWrap;
+	bool					mWireframe;
 	Rect					mClipRect;
 	CullMode				mCullMode;
+	Topology3D				mTopology;
 
 public:
 	RenderState();
@@ -192,10 +200,12 @@ public:
 
 	virtual void SetShader(Shader* shader) { mShader = shader; }
 	virtual void SetTexWrap(bool wrap) { mTexWrap = wrap; }
+	virtual void SetWireframe(bool wireframe) { mWireframe = wireframe; }
 	virtual void SetClipped(bool clipped) { mClipped = clipped; }
 	virtual void SetClipRect(const Rect& rect) { mClipRect = rect; }
 	virtual void SetWriteDepthBuffer(bool writeDepthBuffer) { mWriteDepthBuffer = writeDepthBuffer; }
 	virtual void SetDepthFunc(DepthFunc depthFunc) { mDepthFunc = depthFunc; }
+	virtual void SetTopology(Topology3D topology) { mTopology = topology; }
 };
 
 class PoolData
@@ -241,6 +251,12 @@ public:
 	}
 };
 
+enum ModelCreateFlags
+{
+	ModelCreateFlags_None = 0,
+	ModelCreateFlags_NoSetRenderState = 1
+};
+
 class RenderDevice
 {
 public:	
@@ -272,7 +288,7 @@ public:
 	virtual void			RemoveRenderWindow(RenderWindow* renderWindow);
 	
 	virtual RenderState*	CreateRenderState(RenderState* srcRenderState);
-	virtual ModelInstance*	CreateModelInstance(ModelDef* modelDef) { return NULL; }
+	virtual ModelInstance*	CreateModelInstance(ModelDef* modelDef, ModelCreateFlags flags) { return NULL; }
 	virtual VertexDefinition* CreateVertexDefinition(VertexDefData* elementData, int numElements);	
 
 	virtual void			FrameStart() = 0;

+ 34 - 45
BeefySysLib/platform/win/DXRenderDevice.cpp

@@ -143,9 +143,9 @@ static int GetBytesPerPixel(DXGI_FORMAT fmt, int& blockSize)
 	case DXGI_FORMAT_BC6H_TYPELESS: return 1;
 	case DXGI_FORMAT_BC6H_UF16: return 1;
 	case DXGI_FORMAT_BC6H_SF16: return 1;
-	case DXGI_FORMAT_BC7_TYPELESS: return 1;
-	case DXGI_FORMAT_BC7_UNORM: return 1;
-	case DXGI_FORMAT_BC7_UNORM_SRGB: return 1;
+	case DXGI_FORMAT_BC7_TYPELESS: blockSize = 4; return 16;
+	case DXGI_FORMAT_BC7_UNORM: blockSize = 4; return 16;
+	case DXGI_FORMAT_BC7_UNORM_SRGB: blockSize = 4; return 16;
 	case DXGI_FORMAT_AYUV: return 1;
 	case DXGI_FORMAT_Y410: return 1;
 	case DXGI_FORMAT_Y416: return 1;
@@ -510,9 +510,17 @@ void DXRenderDevice::PhysSetRenderState(RenderState* renderState)
 	DXRenderState* dxRenderState = (DXRenderState*)renderState;
 	DXShader* dxShader = (DXShader*)renderState->mShader;
 	
-	if ((renderState->mShader != mPhysRenderState->mShader) && (renderState->mShader != NULL))
+	if (renderState->mTopology != mPhysRenderState->mTopology)
 	{
-		mD3DDeviceContext->PSSetSamplers(0, 1, mPhysRenderState->mTexWrap ? &mD3DWrapSamplerState  : &mD3DDefaultSamplerState);
+		D3D_PRIMITIVE_TOPOLOGY topology = D3D_PRIMITIVE_TOPOLOGY_TRIANGLELIST;
+		if (dxRenderState->mTopology == Topology3D_LineLine)
+			topology = D3D_PRIMITIVE_TOPOLOGY_LINELIST;
+		mD3DDeviceContext->IASetPrimitiveTopology(topology);
+	}
+
+	if ((renderState->mShader != mPhysRenderState->mShader) && (renderState->mShader != NULL))
+	{		
+		mD3DDeviceContext->PSSetSamplers(0, 1, renderState->mTexWrap ? &mD3DWrapSamplerState  : &mD3DDefaultSamplerState);
 		mD3DDeviceContext->IASetInputLayout(dxShader->mD3DLayout);
 		mD3DDeviceContext->VSSetShader(dxShader->mD3DVertexShader, NULL, 0);
 		mD3DDeviceContext->PSSetShader(dxShader->mD3DPixelShader, NULL, 0);
@@ -591,6 +599,9 @@ void DXRenderDevice::PhysSetRenderState(RenderState* renderState)
 		setRasterizerState = true;
 	}
 
+	if (renderState->mWireframe != mPhysRenderState->mWireframe)
+		setRasterizerState = true;	
+
 	if (setRasterizerState)
 	{
 		if (dxRenderState->mD3DRasterizerState == NULL)
@@ -599,13 +610,13 @@ void DXRenderDevice::PhysSetRenderState(RenderState* renderState)
 			{
 				D3D11_CULL_NONE,
 				D3D11_CULL_FRONT,
-				D3D11_CULL_BACK				
+				D3D11_CULL_BACK
 			};
 
 			D3D11_RASTERIZER_DESC rasterizerState;
 			rasterizerState.CullMode = cullModes[dxRenderState->mCullMode];
 			//rasterizerState.CullMode = D3D11_CULL_BACK;
-			rasterizerState.FillMode = D3D11_FILL_SOLID;
+			rasterizerState.FillMode = renderState->mWireframe ? D3D11_FILL_WIREFRAME : D3D11_FILL_SOLID;
 			rasterizerState.FrontCounterClockwise = false;
 			rasterizerState.DepthBias = 0;
 			rasterizerState.DepthBiasClamp = 0;
@@ -701,7 +712,7 @@ struct DXModelVertex
 	Vector3 mTangent;
 };
 
-ModelInstance* DXRenderDevice::CreateModelInstance(ModelDef* modelDef)
+ModelInstance* DXRenderDevice::CreateModelInstance(ModelDef* modelDef, ModelCreateFlags flags)
 {
 	DXModelInstance* dxModelInstance = new DXModelInstance(modelDef);
 
@@ -718,16 +729,17 @@ ModelInstance* DXRenderDevice::CreateModelInstance(ModelDef* modelDef)
 	};	
 
 	auto vertexDefinition = CreateVertexDefinition(vertexDefData, sizeof(vertexDefData) / sizeof(vertexDefData[0]));
-	auto renderState = CreateRenderState(mDefaultRenderState);
-	renderState->mShader = LoadShader(gBFApp->mInstallDir + "/shaders/ModelStd", vertexDefinition);	
+	RenderState* renderState = NULL;
+	if ((flags & ModelCreateFlags_NoSetRenderState) == 0)
+	{
+		renderState = CreateRenderState(mDefaultRenderState);
+		renderState->mShader = LoadShader(gBFApp->mInstallDir + "/shaders/ModelStd", vertexDefinition);
+		renderState->mTexWrap = true;
+		renderState->mDepthFunc = DepthFunc_LessEqual;
+		renderState->mWriteDepthBuffer = true;
+	}
 	delete vertexDefinition;
-
-	//renderState->mCullMode = CullMode_Front;
-
-	renderState->mTexWrap = true;
-	renderState->mDepthFunc = DepthFunc_LessEqual;
-	renderState->mWriteDepthBuffer = true;
-
+	
 	dxModelInstance->mRenderState = renderState;
 
 	////
@@ -1052,7 +1064,8 @@ DXModelInstance::~DXModelInstance()
 
 void DXModelInstance::Render(RenderDevice* renderDevice, RenderWindow* renderWindow)
 {	
-	SetRenderState();
+	if (mRenderState != NULL)
+		SetRenderState();
 
 	for (int meshIdx = 0; meshIdx < (int)mDXModelMeshs.size(); meshIdx++)
 	{
@@ -1064,28 +1077,7 @@ void DXModelInstance::Render(RenderDevice* renderDevice, RenderWindow* renderWin
 		for (auto primIdx = 0; primIdx < (int)dxMesh->mPrimitives.size(); primIdx++)
 		{
 			auto dxPrimitives = &dxMesh->mPrimitives[primIdx];
-
-			if (dxPrimitives->mNumIndices == 11904)
-			{
-				NOP;
-			}
-
-			//TODO:
-			if (dxPrimitives->mNumIndices == 48384)
-				continue;
-
-			if (::GetAsyncKeyState('1'))
-			{
-				if (dxPrimitives->mNumIndices != 9417)
-					continue;
-			}
-			else if (::GetAsyncKeyState('2'))
-			{
-				if (dxPrimitives->mNumIndices != 3684)
-					continue;
-			}
 			
-
 			if (dxPrimitives->mTextures.IsEmpty())
 				continue;
 
@@ -1103,14 +1095,12 @@ void DXModelInstance::Render(RenderDevice* renderDevice, RenderWindow* renderWin
 }
 
 void Beefy::DXModelInstance::CommandQueued(DrawLayer* drawLayer)
-{
-#ifndef BF_NO_FBX
+{	
 	mRenderState = drawLayer->mRenderDevice->mCurRenderState;
-
 	BF_ASSERT(mRenderState->mShader->mVertexSize == sizeof(DXModelVertex));
-
 	drawLayer->mCurTextures[0] = NULL;
 
+#ifndef BF_NO_FBX	
 	ModelAnimation* fbxAnim = &mModelDef->mAnims[0];
 
 	Matrix4 jointsMatrices[BF_MAX_NUM_BONES];
@@ -1560,8 +1550,7 @@ bool DXRenderDevice::Init(BFApp* app)
     rasterizerState.AntialiasedLineEnable = false;
 	
 	mD3DDevice->CreateRasterizerState(&rasterizerState, &dxRenderState->mD3DRasterizerState);	
-	mD3DDeviceContext->RSSetState(dxRenderState->mD3DRasterizerState);
-		
+	mD3DDeviceContext->RSSetState(dxRenderState->mD3DRasterizerState);		
 	mD3DDeviceContext->IASetPrimitiveTopology(D3D11_PRIMITIVE_TOPOLOGY_TRIANGLELIST);	
 
 	ID3D11BlendState* g_pBlendState = NULL;

+ 2 - 2
BeefySysLib/platform/win/DXRenderDevice.h

@@ -95,7 +95,7 @@ typedef std::map<String, DXShaderParam*> DXShaderParamMap;
 
 class DXShader : public Shader
 {
-public:	
+public:		
 	ID3D11InputLayout*		mD3DLayout;
 	ID3D11VertexShader*		mD3DVertexShader;
 	ID3D11PixelShader*		mD3DPixelShader;
@@ -302,7 +302,7 @@ public:
 	virtual void			PhysSetRenderWindow(RenderWindow* renderWindow);
 	virtual void			PhysSetRenderTarget(Texture* renderTarget) override;
 	virtual RenderState*	CreateRenderState(RenderState* srcRenderState) override;
-	virtual ModelInstance*	CreateModelInstance(ModelDef* modelDef) override;			
+	virtual ModelInstance*	CreateModelInstance(ModelDef* modelDef, ModelCreateFlags flags) override;
 
 public:
 	DXRenderDevice();

+ 145 - 0
BeefySysLib/util/MathUtils.cpp

@@ -0,0 +1,145 @@
+#include "MathUtils.h"
+#include "Sphere.h"
+
+USING_NS_BF;
+
+bool Beefy::RayIntersectsTriangle(const Vector3& rayOrigin, const Vector3& rayVector, const Vector3& vtx0, const Vector3& vtx1, const Vector3& vtx2, Vector3* outIntersectionPoint, float* outDistance)
+{
+	const float EPSILON = 0.0000001f;	
+	Vector3 edge1, edge2, h, s, q;
+	float a, f, u, v;
+	edge1 = vtx1 - vtx0;
+	edge2 = vtx2 - vtx0;
+	
+	h = Vector3::CrossProduct(rayVector, edge2);//rayVector.crossProduct(edge2);
+	a = Vector3::Dot(edge1, h); //edge1.dotProduct(h);
+	if (a > -EPSILON && a < EPSILON)
+		return false;    // This ray is parallel to this triangle.
+	f = 1.0f / a;
+	s = rayOrigin - vtx0;
+	u = f * Vector3::Dot(s, h); //s.dotProduct(h);
+	if (u < 0.0 || u > 1.0)
+		return false;
+	q = Vector3::CrossProduct(s, edge1); //s.crossProduct(edge1);
+	v = f * Vector3::Dot(rayVector, q); //rayVector.dotProduct(q);
+	if (v < 0.0 || u + v > 1.0)
+		return false;
+	// At this stage we can compute t to find out where the intersection point is on the line.
+	float distance = f * Vector3::Dot(edge2, q); //edge2.dotProduct(q);
+	if (distance > EPSILON) // ray intersection
+	{
+		if (outIntersectionPoint != NULL)
+			*outIntersectionPoint = rayOrigin + rayVector * distance;
+		if (outDistance != NULL)
+			*outDistance = distance;
+		return true;
+	}
+	else // This means that there is a line intersection but not a ray intersection.
+		return false;
+}
+
+bool Beefy::RayIntersectsCircle(const Vector3& rayOrigin, const Vector3& rayVector, const Sphere& sphere, Vector3* outIntersectionPoint, Vector3* outNormal, float* outDistance)
+{
+	Vector3 e = sphere.mCenter - rayOrigin;
+	float rSq = sphere.mRadius * sphere.mRadius;
+
+	float eSq = e.GetMagnitudeSquare();
+	float a = Vector3::Dot(e, rayVector); // ray.direction is assumed to be normalized
+	float bSq = /*sqrtf(*/eSq - (a * a)/*)*/;
+	float f = sqrtf(fabsf((rSq)- /*(b * b)*/bSq));
+
+	// Assume normal intersection!
+	float t = a - f;
+
+	// No collision has happened
+	if (rSq - (eSq - a * a) < 0.0f) 
+	{
+		return false;
+	}
+	// Ray starts inside the sphere
+	else if (eSq < rSq) 
+	{
+		// Just reverse direction
+		t = a + f;
+	}
+		
+	if (outDistance != NULL)
+		*outDistance = t; 
+	if ((outIntersectionPoint != NULL) || (outNormal != NULL))
+	{
+		Vector3 intersect = rayOrigin + rayVector * t;
+		if (outIntersectionPoint != NULL)
+			*outIntersectionPoint = intersect;
+		if (outNormal != NULL)
+			*outNormal = Vector3::Normalize(intersect - sphere.mCenter);
+	}
+
+	return true;
+}
+
+bool Beefy::RayIntersectsAABB(const Vector3& rayOrigin, const Vector3& rayVector, const AABB& aabb, Vector3* outIntersectionPoint, Vector3* outNormal, float* outDistance)
+{
+	const Vector3& min = aabb.mMin;
+	const Vector3& max = aabb.mMax;
+
+	// Any component of direction could be 0!
+	// Address this by using a small number, close to
+	// 0 in case any of directions components are 0
+	float t1 = (min.mX - rayOrigin.mX) / ((fabs(rayVector.mX) < 0.00001f) ? 0.00001f : rayVector.mX);
+	float t2 = (max.mX - rayOrigin.mX) / ((fabs(rayVector.mX) < 0.00001f) ? 0.00001f : rayVector.mX);
+	float t3 = (min.mY - rayOrigin.mY) / ((fabs(rayVector.mY) < 0.00001f) ? 0.00001f : rayVector.mY);
+	float t4 = (max.mY - rayOrigin.mY) / ((fabs(rayVector.mY) < 0.00001f) ? 0.00001f : rayVector.mY);
+	float t5 = (min.mZ - rayOrigin.mZ) / ((fabs(rayVector.mZ) < 0.00001f) ? 0.00001f : rayVector.mZ);
+	float t6 = (max.mZ - rayOrigin.mZ) / ((fabs(rayVector.mZ) < 0.00001f) ? 0.00001f : rayVector.mZ);
+
+	float tmin = fmaxf(fmaxf(fminf(t1, t2), fminf(t3, t4)), fminf(t5, t6));
+	float tmax = fminf(fminf(fmaxf(t1, t2), fmaxf(t3, t4)), fmaxf(t5, t6));
+
+	// if tmax < 0, ray is intersecting AABB
+	// but entire AABB is being it's origin
+	if (tmax < 0) {
+		return false;
+	}
+
+	// if tmin > tmax, ray doesn't intersect AABB
+	if (tmin > tmax) {
+		return false;
+	}
+
+	float t_result = tmin;
+
+	// If tmin is < 0, tmax is closer
+	if (tmin < 0.0f) {
+		t_result = tmax;
+	}
+
+	if ((outIntersectionPoint != NULL) || (outDistance != NULL))
+	{
+		Vector3 hitPoint = rayOrigin + rayVector * t_result;
+		if (outIntersectionPoint != NULL)
+			*outIntersectionPoint = hitPoint;
+		if (outDistance != NULL)
+			*outDistance = (rayOrigin - hitPoint).GetMagnitude();		
+	}
+	
+	if (outNormal != NULL)
+	{
+		static Vector3 normals[] = {
+			Vector3(-1, 0, 0),
+			Vector3(1, 0, 0),
+			Vector3(0, -1, 0),
+			Vector3(0, 1, 0),
+			Vector3(0, 0, -1),
+			Vector3(0, 0, 1)
+		};
+		float t[] = { t1, t2, t3, t4, t5, t6 };
+
+		for (int i = 0; i < 6; ++i) 
+		{
+			if (t_result == t[i])
+				*outNormal = normals[i];
+		}
+	}
+
+	return true;	
+}

+ 21 - 0
BeefySysLib/util/MathUtils.h

@@ -0,0 +1,21 @@
+#pragma once
+
+#include "Vector.h"
+#include "Matrix4.h"
+
+NS_BF_BEGIN
+
+class AABB
+{
+public:
+	Vector3 mMin;
+	Vector3 mMax;
+};
+
+class Sphere;
+
+bool RayIntersectsTriangle(const Vector3& rayOrigin, const Vector3& rayVector, const Vector3& vtx0, const Vector3& vtx1, const Vector3& vtx2, Vector3* outIntersectionPoint, float* distance);
+bool RayIntersectsCircle(const Vector3& rayOrigin, const Vector3& rayVector, const Sphere& sphere, Vector3* outIntersectionPoint, Vector3* outNormal, float* outDistance);
+bool RayIntersectsAABB(const Vector3& rayOrigin, const Vector3& rayVector, const AABB& aabb, Vector3* outIntersectionPoint, Vector3* outNormal, float* outDistance);
+
+NS_BF_END

+ 9 - 0
BeefySysLib/util/Matrix4.h

@@ -109,6 +109,15 @@ public:
 	}
 
 	static Matrix4 CreateTransform(const Vector3& position, const Vector3& scale, const Quaternion& orientation);
+
+	static float Determinant(float m11, float m12, float m13,
+		float m21, float m22, float m23,
+		float m31, float m32, float m33)
+	{
+		return m11 * (m22 * m33 - m32 * m23) -
+			m21 * (m12 * m33 - m32 * m13) +
+			m31 * (m12 * m23 - m22 * m13);
+	}
 };
 
 NS_BF_END;

+ 195 - 0
BeefySysLib/util/Sphere.cpp

@@ -0,0 +1,195 @@
+#include "Sphere.h"
+#include "Matrix4.h"
+
+USING_NS_BF;
+
+static const float radiusEpsilon = 1e-4f;   // NOTE: To avoid numerical inaccuracies
+
+Sphere::Sphere()
+{
+	mRadius = -1;
+}
+
+Sphere::Sphere(const Sphere& S)
+{
+	mRadius = S.mRadius;
+	mCenter = S.mCenter;
+}
+
+Sphere::Sphere(const Vector3& O)
+{
+	mRadius = 0 + radiusEpsilon;
+	mCenter = O;
+}
+
+Sphere::Sphere(const Vector3& O, float R)
+{
+	mRadius = R;
+	mCenter = O;
+}
+
+Sphere::Sphere(const Vector3& O, const Vector3& A)
+{
+	Vector3 a = A - O;
+
+	Vector3 o = a * 0.5f;
+
+	mRadius = o.GetMagnitude() + radiusEpsilon;
+	mCenter = O + o;
+}
+
+Sphere::Sphere(const Vector3& O, const Vector3& A, const Vector3& B)
+{
+	Vector3 a = A - O;
+	Vector3 b = B - O;
+
+	Vector3 axb = Vector3::CrossProduct(a, b);
+
+	float Denominator = 2.0f * Vector3::Dot(axb, axb);
+
+	Vector3 o =
+		((Vector3::CrossProduct(axb, a) * Vector3::Dot(b, b)) +	
+		(Vector3::CrossProduct(b, axb) * Vector3::Dot(a, a))) / Denominator;
+
+	mRadius = o.GetMagnitude() + radiusEpsilon;
+	mCenter = O + o;
+}
+
+Sphere::Sphere(const Vector3& O, const Vector3& A, const Vector3& B, const Vector3& C)
+{
+	Vector3 a = A - O;
+	Vector3 b = B - O;
+	Vector3 c = C - O;
+
+	float Denominator = 2.0f * Matrix4::Determinant(
+		a.mX, a.mY, a.mZ,
+		b.mX, b.mY, b.mZ,
+		c.mX, c.mY, c.mZ);
+
+	Vector3 o = (
+		(Vector3::CrossProduct(a, b) * Vector3::Dot(c, c)) +
+		(Vector3::CrossProduct(c, a) * Vector3::Dot(b, b)) +
+		(Vector3::CrossProduct(b, c) * Vector3::Dot(a, a))) / Denominator;
+
+	mRadius = o.GetMagnitude() + radiusEpsilon;
+	mCenter = O + o;
+}
+
+Sphere& Sphere::operator=(const Sphere& S)
+{
+	mRadius = S.mRadius;
+	mCenter = S.mCenter;
+
+	return *this;
+}
+
+float Sphere::GetDistance(const Vector3& P) const
+{
+	return Vector3::GetDistance(P, mCenter) - mRadius;
+}
+
+float Sphere::GetDistanceSquare(const Vector3& P) const
+{
+	return Vector3::GetDistanceSquare(P, mCenter) - mRadius * mRadius;
+}
+
+float Sphere::GetDistance(const Sphere& S, const Vector3& P)
+{
+	return Vector3::GetDistance(P, S.mCenter) - S.mRadius;
+}
+
+float Sphere::GetDistance(const Vector3& P, const Sphere& S)
+{
+	return Vector3::GetDistance(P, S.mCenter) - S.mRadius;
+}
+
+float Sphere::GetDistanceSquare(const Sphere& S, const Vector3& P)
+{
+	return Vector3::GetDistanceSquare(P, S.mCenter) - S.mRadius * S.mRadius;
+}
+
+float Sphere::GetDistanceSquare(const Vector3& P, const Sphere& S)
+{
+	return Vector3::GetDistanceSquare(P, S.mCenter) - S.mRadius * S.mRadius;
+}
+
+Sphere Sphere::RecurseMini(Vector3* P[], int p, int b)
+{
+	Sphere MB;
+
+	switch (b)
+	{
+	case 0:
+		MB = Sphere();
+		break;
+	case 1:
+		MB = Sphere(*P[-1]);
+		break;
+	case 2:
+		MB = Sphere(*P[-1], *P[-2]);
+		break;
+	case 3:
+		MB = Sphere(*P[-1], *P[-2], *P[-3]);
+		break;
+	case 4:
+		MB = Sphere(*P[-1], *P[-2], *P[-3], *P[-4]);
+		return MB;
+	}
+
+	for (int i = 0; i < p; i++)
+	{
+		if (MB.GetDistanceSquare(*P[i]) > 0)   // Signed square distance to sphere
+		{
+			for (int j = i; j > 0; j--)
+			{
+				Vector3* T = P[j];
+				P[j] = P[j - 1];
+				P[j - 1] = T;
+			}
+
+			MB = RecurseMini(P + 1, i, b + 1);
+		}
+	}
+
+	return MB;
+}
+
+Sphere Sphere::MiniBall(Vector3 P[], int p)
+{
+	Vector3** L = new Vector3* [p];
+
+	for (int i = 0; i < p; i++)
+		L[i] = &P[i];
+
+	Sphere MB = RecurseMini(L, p);
+
+	delete[] L;
+
+	return MB;
+}
+
+Sphere Sphere::SmallBall(Vector3 P[], int p)
+{
+	Vector3 mCenter;
+	float mRadius = -1;
+
+	if (p > 0)
+	{		
+		for (int i = 0; i < p; i++)
+			mCenter += P[i];
+
+		mCenter /= (float)p;
+
+		for (int i = 0; i < p; i++)
+		{
+			float d2 = Vector3::GetDistanceSquare(P[i], mCenter);
+
+			if (d2 > mRadius)
+				mRadius = d2;
+		}
+
+		mRadius = sqrtf(mRadius) + radiusEpsilon;
+	}
+
+	return Sphere(mCenter, mRadius);
+}

+ 43 - 0
BeefySysLib/util/Sphere.h

@@ -0,0 +1,43 @@
+#pragma once
+
+#include "Common.h"
+#include "Vector.h"
+
+NS_BF_BEGIN
+
+class Vector3;
+class Matrix4;
+
+class Sphere
+{
+public:
+	Vector3 mCenter;
+	float mRadius;
+
+	Sphere();
+	Sphere(const Sphere& X);
+	Sphere(const Vector3& O);   // Point-Sphere
+	Sphere(const Vector3& O, float R);   // Center and radius (not squared)
+	Sphere(const Vector3& O, const Vector3& A);   // Sphere through two points
+	Sphere(const Vector3& O, const Vector3& A, const Vector3& B);   // Sphere through three points
+	Sphere(const Vector3& O, const Vector3& A, const Vector3& B, const Vector3& C);   // Sphere through four points
+
+	Sphere& operator=(const Sphere& S);
+
+	float GetDistance(const Vector3& P) const;  // Distance from p to boundary of the Sphere
+	float GetDistanceSquare(const Vector3& P) const;  // Square distance from p to boundary of the Sphere
+
+	static float GetDistance(const Sphere& S, const Vector3& P);  // Distance from p to boundary of the Sphere
+	static float GetDistance(const Vector3& P, const Sphere& S);  // Distance from p to boundary of the Sphere
+
+	static float GetDistanceSquare(const Sphere& S, const Vector3& P);  // Square distance from p to boundary of the Sphere
+	static float GetDistanceSquare(const Vector3& P, const Sphere& S);  // Square distance from p to boundary of the Sphere
+
+	static Sphere MiniBall(Vector3 P[], int p);   // Smallest enclosing sphere
+	static Sphere SmallBall(Vector3 P[], int p);   // Enclosing sphere approximation
+
+private:
+	static Sphere RecurseMini(Vector3* P[], int p, int b = 0);
+};
+
+NS_BF_END

+ 20 - 0
BeefySysLib/util/Vector.cpp

@@ -4,6 +4,13 @@
 
 USING_NS_BF;
 
+Vector3::Vector3()
+{
+	mX = 0;
+	mY = 0;
+	mZ = 0;
+}
+
 Vector3::Vector3(float x, float y, float z)
 {
 	mX = x;
@@ -16,6 +23,11 @@ float Vector3::GetMagnitude() const
 	return sqrtf(mX*mX + mY*mY + mZ*mZ);
 }
 
+float Vector3::GetMagnitudeSquare() const
+{
+	return mX * mX + mY * mY + mZ * mZ;
+}
+
 Vector3 Vector3::Normalize(const Vector3& vec)
 {
 	float mag = vec.GetMagnitude();
@@ -39,6 +51,14 @@ Vector3 Vector3::CrossProduct(const Vector3& vec1, const Vector3& vec2)
 }
 
 Vector3 Vector3::Transform(const Vector3& vec, const Matrix4& matrix)
+{
+	return Vector3(
+		(matrix.m00 * vec.mX + matrix.m01 * vec.mY + matrix.m02 * vec.mZ + matrix.m03),
+		(matrix.m10 * vec.mX + matrix.m11 * vec.mY + matrix.m12 * vec.mZ + matrix.m13),
+		(matrix.m20 * vec.mX + matrix.m21 * vec.mY + matrix.m22 * vec.mZ + matrix.m23));
+}
+
+Vector3 Vector3::TransformW(const Vector3& vec, const Matrix4& matrix)
 {
 	float fInvW = 1.0f / (matrix.m30 * vec.mX + matrix.m31 * vec.mY + matrix.m32 * vec.mZ + matrix.m33);
 

+ 67 - 2
BeefySysLib/util/Vector.h

@@ -36,13 +36,25 @@ public:
 	float mZ;
 
 public:
-	Vector3(float x = 0, float y = 0, float z = 0);
+	Vector3();
+	Vector3(float x, float y, float z);
 
 	float GetMagnitude() const;
+	float GetMagnitudeSquare() const;
 	static Vector3 Normalize(const Vector3& vec);
 	static float Dot(const Vector3& vec1, const Vector3& vec2);
 	static Vector3 CrossProduct(const Vector3& vec1, const Vector3& vec2);
 	
+	static float GetDistance(const Vector3& v0, const Vector3& v1)
+	{
+		return (v0 - v1).GetMagnitude();
+	}
+
+	static float GetDistanceSquare(const Vector3& v0, const Vector3& v1)
+	{
+		return (v0 - v1).GetMagnitudeSquare();
+	}
+
 	bool operator==(const Vector3& check) const
 	{
 		return (mX == check.mX) && (mY == check.mY) && (mZ == check.mZ);			
@@ -54,8 +66,9 @@ public:
 	}
 
 	static Vector3 Transform(const Vector3& vec, const Matrix4& matrix);
+	static Vector3 TransformW(const Vector3& vec, const Matrix4& matrix);
 	static Vector3 Transform(const Vector3& vec, const Quaternion& quat);
-	static Vector3 Transform2(const Vector3& vec, const Quaternion& quat);
+	static Vector3 Transform2(const Vector3& vec, const Quaternion& quat);	
 
 	static Vector3 Scale(const Vector3& vec, float scale)
 	{
@@ -77,6 +90,24 @@ public:
 		return Vector3(mX * scale, mY * scale, mZ * scale);
 	}
 
+	Vector3 operator /(float scale) const
+	{
+		return Vector3(mX / scale, mY / scale, mZ / scale);
+	}
+
+	float operator^(const Vector3& v) // Angle between vectors
+	{
+		return acosf(Dot(*this / this->GetMagnitude(), v / v.GetMagnitude()));
+	}
+	
+	inline Vector3& operator += (const Vector3& vec)
+	{
+		mX += vec.mX;
+		mY += vec.mY;
+		mZ += vec.mZ;
+		return *this;
+	}
+
 	inline Vector3& operator -= (const Vector3& vec)
 	{
 		mX -= vec.mX;
@@ -85,6 +116,40 @@ public:
 		return *this;
 	}
 
+	inline Vector3& operator *= (float num)
+	{
+		mX *= num;
+		mY *= num;
+		mZ *= num;
+		return *this;
+	}
+
+	inline Vector3& operator /= (float num)
+	{
+		mX /= num;
+		mY /= num;
+		mZ /= num;
+		return *this;
+	}
+
+	inline Vector3 operator - (const Vector3& vec) const
+	{
+		Vector3 result;
+		result.mX = mX - vec.mX;
+		result.mY = mY - vec.mY;
+		result.mZ = mZ - vec.mZ;
+		return result;
+	}
+
+	inline Vector3 operator + (const Vector3& vec)
+	{
+		Vector3 result;
+		result.mX = mX + vec.mX;
+		result.mY = mY + vec.mY;
+		result.mZ = mZ + vec.mZ;
+		return result;
+	}
+
 	inline Vector3& operator *= (const Vector3& vec)
 	{
 		mX *= vec.mX;