2D Collision, Physics, Rigid body dynamics

Great, could you maybe use <codebox> instead?

I am only as far along as what you see posted here, I will work on world.h/cpp next.

I think there will have to be some time for getting everything working together. I'm sure it won't run correctly right away. Also I'm not sure if we are on the same page with using pointers. I have been converting them into straight variables unless the are being used to manipulate something outside of the methods/functions scope.

Ok here is a quick port of Joint.h/cpp:

Include "MathUtils.bmx"
Include "Body.bmx"

Function Joint:TJoint ()
	Return New TJoint
End Function

Type TJoint
	Field M:TMat22
	Field localAnchor1:TVec2, localAnchor2:TVec2
	Field r1:TVec2, r2:TVec2
	Field bias:TVec2
	Field accumulatedImpulse:TVec2
	Field body1:TBody
	Field body2:TBody
	Field relaxation:Float
	
	Method New()
		AccumulatedImpulse = New TVec2
		Relaxation = 1.0
	End Method
	
	Method Set (b1:TBody,  b2:TBody, anchor:TVec2)
		body1 = b1
		body2 = b2
	
		Local Rot1:TMat22 = Mat22_Angle(body1.rotation)
		Local Rot2:TMat22 = Mat22_Angle(body2.rotation)
		Local Rot1T:TMat22 = Rot1.Transpose()
		Local Rot2T:TMat22 = Rot2.Transpose()
		localAnchor1 = MultMat22Vec2(Rot1T,  SubVec2Vec2(anchor, body1.position))
		localAnchor1 = MultMat22Vec2(Rot2T,  SubVec2Vec2(anchor, body2.position))
	
		accumulatedImpulse.Set(0.0, 0.0)
		relaxation = 1.0
	End Method

	Method  PreStep(inv_dt:Float)
		' Pre-compute anchors, mass matrix, And bias.
		Local Rot1:TMat22 = Mat22_Angle(body1.rotation)
		Local Rot2:TMat22 = Mat22_Angle(body2.rotation)
		
		r1 = MultMat22Vec2(Rot1, localAnchor1)
		r2 = MultMat22Vec2(Rot2, localAnchor2)
	
		' deltaV = deltaV0 + K * impulse
		' invM = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
		'      = [1/m1+1/m2     0    ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
		'        [    0     1/m1+1/m2]           [-r1.x*r1.y r1.x*r1.x]           [-r1.x*r1.y r1.x*r1.x]
		Local K1:TMat22 = New TMat22
		K1.col1.x = body1.invMass + body2.invMass;	K1.col2.x = 0.0
		K1.col1.y = 0.0; 							K1.col2.y = body1.invMass + body2.invMass
	
		Local K2:TMat22
		K2.col1.x =  body1.invI * r1.y * r1.y;		K2.col2.x = -body1.invI * r1.x * r1.y
		K2.col1.y = -body1.invI * r1.x * r1.y;		K2.col2.y =  body1.invI * r1.x * r1.x
	
		Local K3:TMat22
		K3.col1.x =  body2.invI * r2.y * r2.y;		K3.col2.x = -body2.invI * r2.x * r2.y;
		K3.col1.y = -body2.invI * r2.x * r2.y;		K3.col2.y =  body2.invI * r2.x * r2.x;
	
		Local K:TMat22 = AddMat22Mat22(AddMat22Mat22(K1, K2), K3)
		M = K.Invert()
	
		Local p1:TVec2 = AddVec2Vec2(body1.position, r1)
		Local p2:TVec2 = AddVec2Vec2(body2.position, r2)
		Local dp:TVec2 = SubVec2Vec2(p2, p1)
		bias = MultFloatVec2((-0.1 * inv_dt), dp)
	
	
		'Apply accumulated impulse.
		accumulatedImpulse.Mult(relaxation)
	
		body1.velocity.SubV(MultFloatVec2(body1.invMass, accumulatedImpulse))
		body1.angularVelocity :- body1.invI * CrossVec2Vec2(r1, accumulatedImpulse)
	
		body2.velocity.AddV(MultFloatVec2(body2.invMass, accumulatedImpulse))
		body2.angularVelocity :+ body2.invI * CrossVec2Vec2(r2, accumulatedImpulse)
	End Method
	
	Method ApplyImpulse()
		Local dv:TVec2 = AddVec2Vec2(body2.velocity, CrossFloatVec2(body2.angularVelocity, r2))
		dv.SubV(SubVec2Vec2(body1.velocity, CrossFloatVec2(body1.angularVelocity, r1)))

		Local impulse:TVec2 = MultMat22Vec2(M, AddVec2Vec2(dv.Reverse(), bias))
	
		body1.velocity.SubV(MultFloatVec2(body1.invMass,impulse))
		body1.angularVelocity :-  body1.invI * CrossVec2Vec2(r1, impulse)
	
		body2.velocity.AddV(MultFloatVec2(body2.invMass,  impulse))
		body2.angularVelocity :+ body2.invI * CrossVec2Vec2(r2, impulse)
	
		accumulatedImpulse.AddV(impulse)
	End Method
	
End Type