Merge pull request #27 from Winster332/fix/weld-joint

Bugfix weld-joint from C++

Author: codingben

src/box2dx/Box2D.NetStandard/Common/Mat33.cs

@@ -64,7 +64,10 @@ internal Vector3 Solve33(Vector3 b)
 		{
 			float det = Vector3.Dot(ex, Vector3.Cross(ey, ez));
 			//Debug.Assert(det != 0.0f);
-			det = 1.0f / det;
+			if (det != 0.0f)
+			{
+				det = 1.0f / det;
+			}
 			var x = new Vector3();
 			x.X = det * Vector3.Dot(b, Vector3.Cross(ey, ez));
 			x.Y = det * Vector3.Dot(ex, Vector3.Cross(b, ez));
@@ -82,14 +85,17 @@ internal Vector2 Solve22(Vector2 b)
 			float a11 = ex.X, a12 = ey.X, a21 = ex.Y, a22 = ey.Y;
 			float det = a11 * a22 - a12 * a21;
 			//Debug.Assert(det != 0.0f);
-			det = 1.0f / det;
+			if (det != 0.0f)
+			{
+				det = 1.0f / det;
+			}
 			var x = new Vector2();
 			x.X = det * (a22 * b.X - a12 * b.Y);
 			x.Y = det * (a11 * b.Y - a21 * b.X);
 			return x;
 		}
 
-		internal void GetInverse22(Mat33 M)
+		internal Mat33 GetInverse22(Mat33 M)
 		{
 			float a = ex.X, b = ey.X, c = ex.Y, d = ey.Y;
 			float det = a * d - b * c;
@@ -107,9 +113,11 @@ internal void GetInverse22(Mat33 M)
 			M.ez.X = 0.0f;
 			M.ez.Y = 0.0f;
 			M.ez.Z = 0.0f;
+
+			return M;
 		}
 
-		internal void GetSymInverse33(Mat33 M)
+		internal Mat33 GetSymInverse33(Mat33 M)
 		{
 			float det = Vector3.Dot(ex, Vector3.Cross(ey, ez));
 			if (det != 0.0f)
@@ -132,6 +140,8 @@ internal void GetSymInverse33(Mat33 M)
 			M.ez.X = M.ex.Z;
 			M.ez.Y = M.ey.Z;
 			M.ez.Z = det * (a11 * a22 - a12 * a12);
+
+			return M;
 		}
 	}
 }

src/box2dx/Box2D.NetStandard/Dynamics/Joints/Joint.cs

@@ -35,6 +35,7 @@ Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx
 using Box2D.NetStandard.Dynamics.Joints.Prismatic;
 using Box2D.NetStandard.Dynamics.Joints.Pulley;
 using Box2D.NetStandard.Dynamics.Joints.Revolute;
+using Box2D.NetStandard.Dynamics.Joints.Weld;
 using Box2D.NetStandard.Dynamics.Joints.Wheel;
 using Box2D.NetStandard.Dynamics.World;
 using Box2D.NetStandard.Dynamics.World.Callbacks;
@@ -198,7 +199,8 @@ internal static Joint Create(JointDef def)
 				       RevoluteJointDef d  => new RevoluteJoint(d),
 				       PulleyJointDef d    => new PulleyJoint(d),
 				       GearJointDef d      => new GearJoint(d),
-				       WheelJointDef d     => new WheelJoint(d)
+				       WheelJointDef d     => new WheelJoint(d),
+				       WeldJointDef d     => new WeldJoint(d)
 			       };
 		}
 

src/box2dx/Box2D.NetStandard/Dynamics/Joints/Weld/WeldJoint.cs

@@ -122,7 +122,7 @@ internal override void InitVelocityConstraints(in SolverData data)
 
 			if (Stiffness > 0.0f)
 			{
-				K.GetInverse22(m_mass);
+				m_mass = K.GetInverse22(m_mass);
 
 				float invM = iA + iB;
 
@@ -143,13 +143,13 @@ internal override void InitVelocityConstraints(in SolverData data)
 			}
 			else if (K.ez.Z == 0.0f)
 			{
-				K.GetInverse22(m_mass);
+				m_mass = K.GetInverse22(m_mass);
 				m_gamma = 0.0f;
 				m_bias = 0.0f;
 			}
 			else
 			{
-				K.GetSymInverse33(m_mass);
+				m_mass = K.GetSymInverse33(m_mass);
 				m_gamma = 0.0f;
 				m_bias = 0.0f;
 			}