Working physics module 🚀

Author: willnode

source/math/Bounds4.h

@@ -26,7 +26,7 @@ namespace Forth
 		void Allocate(const Vector4 &point)
 		{
 			min = Min(min, point);
-			max = Min(max, point);
+			max = Max(max, point);
 		}
 
 		inline Vector4 center(void) const
@@ -66,15 +66,15 @@ namespace Forth
 		}
 
 		/// <summary> Returns a point that either inside or touching the bound </summary>
-		inline Vector4 Clamp(Vector4 &point) const
+		inline Vector4 Clamp(const Vector4 &point) const
 		{
 			Vector4 c = center(), e = extent();
-			point = point - c;
+			Vector4 p = point - c;
 
 			for (int i = 0; i < 4; i++)
-				point[i] = Forth::Clamp(-e[i], e[i], point[i]);
+				p[i] = Forth::Clamp(-e[i], e[i], point[i]);
 
-			return point + c;
+			return p + c;
 		}
 
 		/// <summary> Returns a point that either outside or touching the bound </summary>

source/math/Matrix4.cpp

@@ -87,7 +87,7 @@ namespace Forth
 
 	Matrix4 Euler(const Euler4 &e)
 	{
-		Euler(e.x, e.y, e.z, e.t, e.u, e.v);
+		return Euler(e.x, e.y, e.z, e.t, e.u, e.v);
 	}
 
 	/// <summary>

source/math/Transform4.h

@@ -117,7 +117,7 @@ namespace Forth
 
 		/// <summary> Convert to non-orthogonal matrix by applying a scale </summary>
 		/// <remarks> Do not inverse or multiply any matrix produced using this method. </remarks>
-		Transform4 ToTRS(Vector4 scale)
+		Transform4 ToTRS(const Vector4& scale)
 		{
 			return Transform4(position, rotation * Matrix4(scale));
 		}

source/physics/Common.cpp

@@ -17,16 +17,16 @@ namespace Forth
 		float Common::PENETRATION_SLOP = 0.02f;
 		float Common::MAX_DT = 0.02f;
 
-		float Common::DefMixRestitution(Shape* A, Shape* B)
+		float Common::DefMixRestitution(Shape *A, Shape *B)
 		{
 			return A->restitution > B->restitution ? A->restitution : B->restitution;
 		}
-		float Common::DefMixFriction(Shape* A, Shape* B)
+		float Common::DefMixFriction(Shape *A, Shape *B)
 		{
 			return (A->friction + B->friction) * 0.5f;
 		}
 
-		static Common::ShapeMixCB MixRestitution = Common::DefMixRestitution;
-		static Common::ShapeMixCB MixFriction = Common::DefMixFriction;
+		Common::ShapeMixCB Common::MixRestitution = Common::DefMixRestitution;
+		Common::ShapeMixCB Common::MixFriction = Common::DefMixFriction;
 	} // namespace Physics
 } // namespace Forth

source/physics/Common.h

@@ -2,17 +2,17 @@
 
 #include "../extras/Utils.h"
 #include "../math/Bounds4.h"
-#include "../math/math.h"
 #include "../math/Matrix4.h"
 #include "../math/Plane4.h"
 #include "../math/Ray4.h"
+#include "../math/Tensor4.h"
 #include "../math/Transform4.h"
 #include "../math/Vector4.h"
-#include "../math/Tensor4.h"
-#include <vector>
-#include <stack>
-#include <map>
+#include "../math/math.h"
 #include <algorithm>
+#include <map>
+#include <stack>
+#include <vector>
 
 namespace Forth
 {
@@ -93,20 +93,20 @@ namespace Forth
 			/// </summary>
 			static float MAX_DT;
 
-			typedef float(*ShapeMixCB)(class Shape*, class Shape*);
+			typedef float (*ShapeMixCB)(class Shape *, class Shape *);
 
 			static ShapeMixCB MixRestitution;
 			static ShapeMixCB MixFriction;
 
 			/// <summary>
 			/// Restitution (bounce) mixing. Default is max
 			/// </summary>
-			static float DefMixRestitution(class Shape* A, class Shape* B);
+			static float DefMixRestitution(class Shape *A, class Shape *B);
 
 			/// <summary>
 			/// Friction (slide) mixing. Default is average
 			/// </summary>
-			static float DefMixFriction(class Shape* A, class Shape* B);
+			static float DefMixFriction(class Shape *A, class Shape *B);
 		};
 	} // namespace Physics
 } // namespace Forth

source/physics/broadphase/BroadPhase.cpp

@@ -5,22 +5,16 @@ namespace Forth
 {
 	namespace Physics
 	{
-		BroadPhase::BroadPhase(ContactManager* manager)
-		{
-			Manager = manager;
-
-			PairBuffer = ::std::vector<ContactPair>();
-			MoveBuffer = ::std::vector<int>();
-		}
+		BroadPhase::BroadPhase(ContactManager *manager) : Tree(), Manager(manager), PairBuffer(), MoveBuffer() {}
 
-		void BroadPhase::InsertShape(Shape* shape, const Bounds4 &aabb)
+		void BroadPhase::InsertShape(Shape *shape, const Bounds4 &aabb)
 		{
 			int id = Tree.Insert(aabb, shape);
 			shape->broadPhaseIndex = id;
 			BufferMove(id);
 		}
 
-		void BroadPhase::RemoveShape(Shape* shape)
+		void BroadPhase::RemoveShape(Shape *shape)
 		{
 			Tree.Remove(shape->broadPhaseIndex);
 		}
@@ -45,7 +39,7 @@ namespace Forth
 			for (size_t i = 0; i < PairBuffer.size(); ++i)
 			{
 				// Add contact to manager
-				ContactPair pair = PairBuffer[i];
+				ContactPair &pair = PairBuffer[i];
 				Manager->AddContact(Tree.GetShape(pair.A), Tree.GetShape(pair.B));
 			}
 		}
@@ -75,7 +69,7 @@ namespace Forth
 			int iA = Min(index, CurrentIndex);
 			int iB = Max(index, CurrentIndex);
 
-			PairBuffer.push_back({ iA, iB });
+			PairBuffer.push_back({iA, iB});
 		}
 
 	} // namespace Physics

source/physics/broadphase/BroadPhase.h

@@ -16,7 +16,7 @@ namespace Forth
 		class BroadPhase : public ITreeCallback
 		{
 		  public:
-		  	BroadPhase() {}
+			BroadPhase() {}
 
 			BroadPhase(class ContactManager *manager);
 
@@ -37,7 +37,7 @@ namespace Forth
 
 			::std::vector<int> MoveBuffer;
 
-			DynamicTree Tree = DynamicTree();
+			DynamicTree Tree;
 
 			int CurrentIndex;
 

source/physics/broadphase/DynamicTree.cpp

@@ -58,7 +58,7 @@ namespace Forth
 				int id = stack.top();
 				stack.pop();
 
-				Node n = Nodes[id];
+				Node &n = Nodes[id];
 
 				if (IsIntersecting(aabb, n.aabb))
 				{
@@ -93,7 +93,7 @@ namespace Forth
 				if (id == -1)
 					continue;
 
-				Node n = Nodes[id];
+				Node &n = Nodes[id];
 				if (!n.aabb.Raycast(p0, p1))
 					continue;
 
@@ -120,13 +120,13 @@ namespace Forth
 			// Find the closest leaf
 
 			Vector4 c = Nodes[id].aabb.center();
-			Node N = Nodes[Root], R, L, P;
+			Node &N = Nodes[Root];
 			int n = Root, r, l, p;
 
 			while (N.height > 0)
 			{
-				R = Nodes[r = N.right];
-				L = Nodes[l = N.left];
+				Node &R = Nodes[r = N.right];
+				Node &L = Nodes[l = N.left];
 				if (Compare(c, L.aabb, R.aabb) >= 0)
 				{
 					N = L;
@@ -140,7 +140,7 @@ namespace Forth
 			}
 
 			{
-				P = Nodes[p = AllocateNode()];
+				Node &P = Nodes[p = AllocateNode()];
 				int tmp;
 				// Make P becomes parent of N and ID
 				SWAP(P.left, N.left, tmp)
@@ -168,10 +168,10 @@ namespace Forth
 				return;
 			}
 
-			Node N = Nodes[id], P = Nodes[N.parent], S;
+			Node &N = Nodes[id], &P = Nodes[N.parent];
 			int s = (P.left == id) ? P.right : P.left, p = N.parent, tmp;
 
-			S = Nodes[s];
+			Node &S = Nodes[s];
 			// Make S become part of grandparent N, detach N & P
 			SWAP(S.parent, P.parent, tmp)
 
@@ -192,7 +192,7 @@ namespace Forth
 		{
 			while (index != -1)
 			{
-				Node n = Nodes[index];
+				Node &n = Nodes[index];
 
 				n.height = Max(Nodes[n.left].height, Nodes[n.right].height) + 1;
 				n.aabb = Combine(Nodes[n.left].aabb, Nodes[n.right].aabb);
@@ -202,7 +202,7 @@ namespace Forth
 		}
 		int DynamicTree::AllocateNode()
 		{
-			Node F = Nodes[FreeNode];
+			Node &F = Nodes[FreeNode];
 			int f = FreeNode;
 
 			if (F.next == NodesCap)
@@ -256,6 +256,6 @@ namespace Forth
 			right = r;
 		}
 		DynamicTree::Node::Node() {}
-		DynamicTree::Node::Node(int next) { this->next = next; }
+		DynamicTree::Node::Node(int next) : Node() { this->next = next; }
 	} // namespace Physics
 } // namespace Forth

source/physics/broadphase/DynamicTree.h

@@ -11,7 +11,7 @@ namespace Forth
 	{
 		struct ITreeCallback
 		{
-			virtual void TreeCallback(int id);
+			virtual void TreeCallback(int id)=0;
 		};
 
 		/// Dynamic Bounds4 Tree

source/physics/collision/Algorithm.cpp

@@ -5,6 +5,10 @@ namespace Forth
 {
 	namespace Physics
 	{
+		Vector4 Algorithm::input[Common::MULTICONTACT_COUNT];
+		Vector4 Algorithm::result[Common::MULTICONTACT_COUNT];
+		Vector4 Algorithm::incident[8];
+
 		//--------------------------------------------------------------------------------------------------
 		// Get nearest point from two edges, return true if parallel
 
@@ -72,7 +76,7 @@ namespace Forth
 		//--------------------------------------------------------------------------------------------------
 		// Return (W-axis) oriented box extent & rotation necessary for clipping
 
-		void Algorithm::ComputeReferenceEdgesAndBasis(const Vector4 &eR, const Transform4& rtx, const Vector4 &n, int axis, Matrix4 &basis, Vector4 &e)
+		void Algorithm::ComputeReferenceEdgesAndBasis(const Vector4 &eR, const Transform4 &rtx, const Vector4 &n, int axis, Matrix4 &basis, Vector4 &e)
 		{
 			Vector4 nr = n / rtx.rotation;
 
@@ -141,7 +145,7 @@ namespace Forth
 		// Get the most potential face candidate that Incidents N
 		// That is: The face that most parallel to N
 
-		void Algorithm::ComputeIncidentFace(const Transform4& itx, const Vector4 &e, const Vector4 &n, Vector4 result[])
+		void Algorithm::ComputeIncidentFace(const Transform4 &itx, const Vector4 &e, const Vector4 &n, Vector4 result[])
 		{
 			auto ni = (n / itx.rotation);