Verlet Physics

SSS

Hey everyone! I posted this in the maxphysics thread. However, since I know a lot of you don't frequent it I'm posting it here aswell. Anyway, I've tried to design several physics systems ad-hoc before and they've failed in one way or another. This one seems to work at the moment and I'll be adding collisions and whatever else one adds to a physics system. Anyway, I used the advanced character physics article to write this. I hope you guys like what i've done so far. I definately plan on improving it. Anyone can use it in whatever you guys want (even though its still new and not so good.) I know that it's not such a large accomplishment but I have been trying to get this working for a while so I'm pretty happy with the results :D.

Here's the vector library on which the code runs:

Strict
Import BRL.Basic
Type TVector
	'VARIABLES
	Field _x:Double, _y:Double
	
	'METHODS
	Method New()
		_x = 0
		_y = 0
	End Method
	
	Method Set(x:Double,y:Double)
		_x=x
		_y=y
	End Method
	
	Method Get(x:Double Var, y:Double Var)
		x = _x
		y = _y
	End Method
	
	Method Multiply:TVector(factor:Double)
		_x:*factor
		_y:*factor
		Return Self
	End Method
	
	Method Add:TVector(v:TVector,bself=False)
		If bself = False
			Return TVector.Create(_x+v._x,_y+v._y)
		Else
			_x:+v._x
			_y:+v._y
			Return Self 
		EndIf
	End Method
	
	Method Subtract:TVector(v:TVector,bself=False)
		If bself = False
			Return TVector.Create(_x-v._x,_y-v._y)
		Else
			_x:-v._x
			_y:-v._y
			Return Self
		EndIf
	End Method
	
	Method Copy:TVector()
		Return Create(_x,_y)
	End Method
	
	Method DotProduct:Double(v:TVector)
		Return _x*v._x+_y*v._y
	End Method
	
	Method AngleBetween:Double(v:TVector)
		Return ACos(DotProduct(v)/(Magnitude()*v.Magnitude()))
	EndMethod
	
	Method SetAngle(angle:Double)
		Local mag:Double = Magnitude()
		_x = Cos(angle)*mag
		_y = -Sin(angle)*mag
	End Method
	
	Method GetAngle:Double()
		Return ATan2(-_y,_x)
	End Method
	
	Method SetMagnitude(mag:Double)
		Normalise()
		_x:*mag
		_y:*mag
	End Method
	
	Method Magnitude:Double()
		Return Sqr(_x^2+_y^2)
	End Method
	
	Method MagSquared:Double()
		Return _x^2+_y^2
	End Method
	
	Method Normalise:TVector(bself=True)
		Local m:Double = Magnitude()
		If bself = False
			Return TVector.Create(_x/m,_y/m)
		Else
			_x:/m
			_y:/m
		EndIf
	End Method

	Method PrintVector()
		Print "(" + _x + "," + _y + ")"
	End Method
	
	Method Draw(xoff,yoff)
		DrawLine xoff,yoff,xoff+_x,yoff+_y
	End Method

	'FUNCTIONS
	Function Create:TVector(x:Double,y:Double)
		Local o:TVector = New TVector
		o._x=x
		o._y=y
		Return o
	End Function
	
	Function FromTo:TVector(v1:TVector,v2:TVector)
		Return Create(v2._x-v1._x,v2._y-v2._x)
	End Function
End Type

Here's the actual system:

Strict
Framework BRL.Max2D
Import BRL.D3D7Max2D
Import BRL.Basic
Import BRL.System
Import BRL.Timer
Import BRL.LinkedList
Import "vector.bmx"

Type TParticle
	Global list:TList
	Global bHidden=False
	
	Field oldposition:TVector
	Field position:TVector
	Field acceleration:TVector
	Field mass#
	Field bLocked
	
	Method New()
		If Not list Then list = CreateList()
		ListAddLast(list,Self)
		position = TVector.Create(0,0)
		oldposition = TVector.Create(0,0)
		acceleration = TVector.Create(0,0)
		mass# = 1
		bLocked = False
	End Method
	
	Method Push(Fx#,Fy#)
		acceleration.Add(TVector.Create(Fx/mass,Fy/mass),True)
	End Method
	
	Method Update()
		If bLocked = False Then VerletIntegrator()
		acceleration.set(0,0)
	End Method
	
	Method VerletIntegrator:TVector(update = True)
		If update = True
			Local p:TVector = position.Copy()
			Local op:TVector = oldposition.Copy()
			Local accel:TVector = acceleration.Copy()
			p.Multiply(2-TPhysicsEngine.Resistance)
			op.Multiply(1-TPhysicsEngine.Resistance)
			p.Subtract(op,True)
			accel.Multiply(TPhysicsEngine.DeltaTime()^2)
			p.Add(accel,True)
			oldposition = position.Copy()
			position = p.Copy()
			Return Null
		Else
			Local p:TVector = position.Copy()
			Local op:TVector = oldposition.Copy()
			Local accel:TVector = acceleration.Copy()
			p.Multiply(2-TPhysicsEngine.Resistance)
			p.Subtract(op.Multiply(1-TPhysicsEngine.Resistance),True)
			p.Add(accel.Multiply(TPhysicsEngine.DeltaTime()^2))
			Return p
		EndIf
	End Method
	
	Method Lock()
		bLocked = True
	End Method
		
	Method Unlock()
		blocked = False
	End Method
	
	Method GetVelocity:TVector()
		Local v:TVector = VerletIntegrator(False).Subtract(oldposition)
		v.Multiply(1/Float(2*TPhysicsEngine.DeltaTime()))
		Return v
	End Method
	
	Method SetPosition(x#,y#)
		position._x=x
		position._y=y
		oldposition = position.Copy()
	End Method
	
	Method GetMomentumX:Float()
		Return GetVelocity()._x*mass
	End Method
	
	Method GetMomentumY:Float()
		Return GetVelocity()._y*mass
	End Method
	
	Method Render()
		DrawRect position._x-5,position._y-5,10,10
	End Method
	
	Function Create:TParticle(x#,y#,mass#=1,vx#=0,vy#=0)
		Local o:TParticle = New TParticle
		o.SetPosition(x,y)
		o.mass = mass
		o.acceleration.set(vx,vy)
		Return o
	End Function
	
	Function DoFrame()
		For Local o:TParticle = EachIn(list)
			o.Update()
			If bHidden = False Then o.Render()
		Next
	End Function
End Type

Type TConstraint
	Global list:TList
	Global bHidden=False

	Field _distance:Float
	Field _p1:TParticle = Null
	Field _p2:TParticle = Null
	
	Method New()
		If Not list Then list = CreateList()
		list.addlast(Self)
	EndMethod
	
	Method Update()
		Local vdistance:TVector
		Local ndistance#
		Local difference#
		Local invmass1# = 1.0/_p1.mass
		Local invmass2# = 1.0/_p2.mass
		
		vdistance = _p1.position.Subtract(_p2.position)
		ndistance = vdistance.Magnitude()
		difference = (ndistance-_distance)/(ndistance*(invmass1+invmass2))
		If _p1.bLocked = False
			_p1.position._x=_p1.position._x-vdistance._x*invmass1*difference
			_p1.position._y=_p1.position._y-vdistance._y*invmass1*difference
		EndIf
		If _p2.bLocked = False
			_p2.position._x=_p2.position._x+vdistance._x*invmass2*difference
			_p2.position._y=_p2.position._y+vdistance._y*invmass2*difference
		EndIf
	End Method
	
	Method Render()
		DrawLine _p1.position._x,_p1.position._y,_p2.position._x,_p2.position._y
	End Method	
	
	Function Create:TConstraint(p1:TParticle,p2:TParticle,dist:Float = 0)
		Local c:TConstraint = New TConstraint
		c._p1 = p1
		c._p2 = p2
		If dist = 0
			c._distance = Sqr((p1.position._x-p2.position._x)^2+(p1.position._y-p2.position._y)^2)
		Else
			c._distance = dist
		EndIf
		Return c
	End Function
	
	Function DoFrame()
		For Local o:TConstraint = EachIn(list)
			o.Update()
			If bHidden = False Then o.Render()
		Next
	End Function
End Type

	
	

Type TPhysicsEngine
	Global Gravity:Float = 0.0
	Global Resistance:Float = 0.0
	Global LastTime = 0
	Global ThisTime = 0
	
	Function DoFrame()
		If ThisTime = 0 And LastTime = 0
			ThisTime = MilliSecs()
			LastTime = MilliSecs()
		EndIf
		
		ThisTime = MilliSecs()
		If TParticle.list
			For Local p:TParticle = EachIn(TParticle.list)
				p.acceleration._y:+Gravity
				p.Update()
				p.Render()
			Next
		EndIf
		
		If TConstraint.list
			For Local c:TConstraint = EachIn(TConstraint.list)
				c.Render()
				c.Update()
			Next
		EndIf
		LastTime = ThisTime
	End Function
	
	Function DeltaTime#()
		Return Float(ThisTime-LastTime)/1000.0
	End Function
End Type

And here's a small example:

Graphics 640,480,0

Local p:TParticle = TParticle.Create(640/2,480/2)
Local p2:TParticle = TParticle.Create(640/2,480/2+50)
Local p3:TParticle = TParticle.Create(640/2,480/2+100)
Local p4:TParticle = TParticle.Create(640/2,480/2+150)
Local p5:TParticle = TParticle.Create(640/2,480/2+200)
Local p6:TParticle = TParticle.Create(640/2-25,480/2+225)
Local p7:TParticle = TParticle.Create(640/2+25,480/2+225)
TConstraint.Create(p,p2)
TConstraint.Create(p2,p3)
TConstraint.Create(p3,p4)
TConstraint.Create(p4,p5)
TConstraint.Create(p5,p6)
TConstraint.Create(p6,p7)
TConstraint.Create(p5,p7)
p.Lock()
Local x# =640/2
Local y# =480/2
TPhysicsEngine.Gravity = 70
TPhysicsEngine.Resistance = 0.005
	
While Not KeyDown(KEY_ESCAPE)
	Cls
	If MouseDown(1) Then p5.SetPosition(MouseX(),MouseY())
	TPhysicsEngine.DoFrame()
	Flip
Wend