2D Collision, Physics, Rigid body dynamics

BlitzMax Forums/BlitzMax Programming/2D Collision, Physics, Rigid body dynamics

Pantheon
 #1
Thanks very much .Alex, i'll have a good look at the code later. Im sure that will help speed up the process of creating the basic library.

altitudems, here is the Arbiter file and a segment of the collision file:

Rem
* Copyright (c) 2006 Erin Catto http://www.gphysics.com"
*
* Permission To use, copy, modify, distribute And sell this software
* And its documentation For any purpose is hereby granted without fee,
* provided that the above copyright notice appear in all copies.
* Erin Catto makes no representations about the suitability 
* of this software For any purpose.  
* It is provided "as is" without express Or implied warranty.
End Rem

'/**
' *
' */
Const inEdge1:Int = 0
Const inEdge2:Int = 1
Const outEdge1:Int = 2
Const outEdge2:Int = 3

'/**
' *
' */
Type TFeaturePair

	Field e:Byte Ptr
	Field key:Int

	Method New( )
		e = Byte Ptr( Varptr( key ) )
	End Method
End Type

'/**
' *
' */
Type TContact

	Field position:TVec2
	Field normal:TVec2
	Field separation:Float
	Field accumulatedNormalImpulse:Float
	Field accumulatedTangentImpulse:Float
	Field massNormal:Float, massTangent:Float
	Field bias:Float
	Field feature:TFeaturePair;
End Type

'/**
' *
' */
Function Arbiter:TArbiter( a:TBody, b:TBody )

	Local nArb:TArbiter = New TArbiter
	nArb.body1 = a
	nArb.body2 = b
	nArb.numContacts = Collide( nArb.contacts[TArbiter.MAX_POINTS], nArb.body1, nArb.body2 )
	nArb.friction = Sqr( nArb.body1.friction * nArb.body2.friction )
	Return nArb
End Function

'/**
' *
' */
Type TArbiter

	Const MAX_POINTS:Int = 2
	
	Field contacts:TContact[MAX_POINTS]
	Field numContacts:Int
	Field body1:TBody
	Field body2:TBody
	Field friction:Float
		
	Method Update( newContacts:TContact[], numNewContacts:Int )
	
		Local i:Int, j:Int, k:Int
		Local mergedContacts:TContact[2]

		For i = 0 To numNewContacts-1
		
			Local cNew:TContact = newContacts[ i ]
			k = -1
			
			For j = 0 To numContacts-1

				Local cOld:TContact = contacts[ j ]
				
				If cNew.feature.key = cOld.feature.key
				
					k = j
					Exit
				EndIf
			Next	

			If k > -1
			
				local c:TContact = mergedContacts[i]
				local cOld:TContact = contacts[k]
				c = cNew
				c.accumulatedNormalImpulse = cOld.accumulatedNormalImpulse
				c.accumulatedTangentImpulse = cOld.accumulatedTangentImpulse
			Else

				mergedContacts[i] = newContacts[i]
			EndIf
		Next

		For i = 0 To numNewContacts-1
		
			contacts[i] = mergedContacts[i]
		Next
		
		numContacts = numNewContacts
	End Method
	
	Method PreStep( inv_dt:Float )
	
		Local k_allowedPenetration:Float = Float( 0.01 )
		Local i:Int

 		For i = 0 To numContacts-1
		
			Local c:TContact = contacts[i]
			Local r1:TVec2 = SubVec2Vec2( c.position, body1.position )
			Local r2:TVec2 = SubVec2Vec2( c.position, body2.position )

			'// Precompute normal mass, tangent mass, And bias.
			Local rn1:Float = Dot( r1, c.normal )
			Local rn2:Float = Dot( r2, c.normal )
			Local kNormal:Float = body1.invMass + body2.invMass
			kNormal :+ body1.invI * (Dot(r1, r1) - rn1 * rn1)
			kNormal :+ body2.invI * (Dot(r2, r2) - rn2 * rn2)
			c.massNormal = 1.0 / kNormal

			Local tangent:TVec2 = CrossVec2Float( c.normal, 1 )
			Local rt1:Float = Dot( r1, tangent )
			Local rt2:Float = Dot( r2, tangent )
			local kTangent:Float = body1.invMass + body2.invMass
			kTangent :+ body1.invI*(Dot(r1, r1)-rt1*rt1)+body2.invI*(Dot(r2, r2)-rt2*rt2)
			c.massTangent = 1 / kTangent

			c.bias = -0.1 * inv_dt * Min(0, c.separation + k_allowedPenetration)
			
			'// Apply normal + friction impulse
			local tVector1:TVec2 = c.normal.MultEx( c.accumulatedNormalImpulse )
			local tVector2:TVec2 = tangent.MultEx( c.accumulatedTangentImpulse )
			Local impulse:TVec2 = AddVec2Vec2( tVector1, tVector2 )

			body1.velocity = AddVec2Vec2( body1.velocity, impulse.MultEx( body1.invMass ) )
			body1.angularVelocity :- CrossVec2Vec2( Impulse, r1 ) * body1.invI
			body2.velocity = AddVec2Vec2( body2.velocity, impulse.MultEx( body2.invMass ) )
			body2.angularVelocity :+ CrossVec2Vec2( Impulse, r2 ) * body2.invI
		Next
	End Method

	Method ApplyImpulse( )
	
		local b1:TBody = body1
		local b2:TBody = body2
	
		local i:int
	
		For i = 0 to numContacts-1
		
			Local c:TContact = contacts[i]
			Local r1:TVec2 = SubVec2Vec2( c.position, body1.position )
			Local r2:TVec2 = SubVec2Vec2( c.position, body2.position )
		
			'// Relative velocity at contact
			local rv:TVec2 = AddVec2Vec2( b2.velocity, CrossFloatVec2( b2.angularVelocity, r2 ) )
			rv = SubVec2Vec2( rv, b1.velocity )
			rv = SubVec2Vec2( rv, CrossFloatVec2( b1.angularVelocity, r1 ) )
			local dv:TVec2 = Vec2( rv.x, rv.y )
	
			'// Compute normal impulse with bias.
			local vn:Float = Dot( dv, c.normal )
			local normalImpulse:Float = c.massNormal * ( -vn + c.bias )
	
			'// Clamp the accumulated impulse
			local oldNormalImpulse:float = c.accumulatedNormalImpulse
			c.accumulatedNormalImpulse = Max( oldNormalImpulse + normalImpulse, 0 )
			normalImpulse = c.accumulatedNormalImpulse - oldNormalImpulse

			'// Apply contact impulse
			local impulse:TVec2 = c.normal.MultEx( normalImpulse )
	
			b1.velocity = SubVec2Vec2( b1.velocity, impulse.multEx( b1.invMass ) )
			b1.angularVelocity :- CrossVec2Vec2( r1, impulse ) * b1.invI
	
			b2.velocity = SubVec2Vec2( b2.velocity, impulse.multEx( b2.invMass ) )
			b2.angularVelocity :+ CrossVec2Vec2( r2, impulse ) * b2.invI

			'// Compute friction impulse
			local maxTangentImpulse:Float = friction * c.accumulatedNormalImpulse
	
			local tangent:TVec2 = CrossVec2Float( c.normal, 1 )
			local vt:Float = Dot( rv, tangent )
			local tangentImpulse:Float = c.massTangent * (-vt)
	
			'// Clamp friction
			Local oldTangentImpulse:float = c.accumulatedTangentImpulse
			c.accumulatedTangentImpulse = ClampFloat(oldTangentImpulse + tangentImpulse, -maxTangentImpulse, maxTangentImpulse)
			tangentImpulse = c.accumulatedTangentImpulse - oldTangentImpulse
	
			'// Apply contact impulse
			impulse = tangent.MultEx( tangentImpulse )
	
			b1.velocity = SubVec2Vec2( b1.velocity, impulse.MultEx( b1.invMass ) )
			b1.angularVelocity :- b1.invI * CrossVec2Vec2( r1, impulse )
	
			b2.velocity = SubVec2Vec2( b2.velocity, impulse.MultEx( b2.invMass ) )
			b2.angularVelocity :+ b2.invI * CrossVec2Vec2( r2, impulse )
			
		Next
	End Method
End Type



Rem
* Copyright (c) 2006 Erin Catto http://www.gphysics.com"
*
* Permission To use, copy, modify, distribute And sell this software
* And its documentation For any purpose is hereby granted without fee,
* provided that the above copyright notice appear in all copies.
* Erin Catto makes no representations about the suitability 
* of this software For any purpose.  
* It is provided "as is" without express Or implied warranty.
End Rem

const FACE_A_X:Int	= 1
const FACE_A_Y:Int	= 2
const FACE_B_X:Int	= 3
const FACE_B_Y:Int	= 4

const NO_EDGE:Int	= 0
const EDGE1:Int		= 1
const EDGE2:Int		= 2
const EDGE3:Int		= 3
const EDGE4:Int		= 4

Type TClipVertex

	field v:TVec2
	field fp:TFeaturePair
	
End Type

Function _Flip( fp:TFeaturePair )
	'Swap( fp.e[ inEdge1 ], fp.e[ inEdge2 ] )
	'Swap( fp.e[outEdge1 ], fp.e[outEdge2 ] )
End Function

Function ClipSegmentToLine:Int( vOut:TClipVertex[], vIn:TClipVertex[], normal:TVec2, Offset:Float, ClipEdge:Int )
		
End Function

Function ComputeIncidentEdge( c:TClipVertex[], h:TVec2, Pos:TVec2, Rot:TMat22, normal:TVec2 )	
	
End Function

Function Collide:int( contacts:TContact, bodyA:TBody, bodyB:TBody )
	
End Function


I also added these lines to the TVec2 class in mathutils. It was just to simplify writing the vector equations.

	Method MultEx:TVec2( a:Float )
		local nVec2:TVec2 = New TVec2
		nVec2.x = x*a
		nVec2.y = y*a
		return nVec2	
	End Method

	Method Neg( )
		x = -x
		y = -y
	End Method


I will try and get collision finished as soon as i can. I also fixed the compilation error in the constructor function, yeah it was a stupid mistake.

I literaly convered the source files line by line so im sure that there will be a few logic mistakes in the above code but they shouldnt be too hard to fix.