Verlet Physics

Hey everyone,

I got angular constraints working (i think.. dunno exactly how their supposed to act.) The creation is through the function TAngularConstraint.Create(p1,p2,p3,angle) where p1 is the pivot and p2, and p3 are the two particles that you want to separate by a certain angle. The angle is only needed if you dont want them at their rest angle but some other artificial angle. Anyway, without further adue here is the code:

Type TAngularConstraint
	Global list:TList

	Field _angle:Float
	Field _p1:TParticle = Null
	Field _p2:TParticle = Null
	Field _p3:TParticle = Null
	
	Method New()
		If Not list Then list = CreateList()
		list.addlast(Self)
	EndMethod
	
	Method Update()
		Local t1:TVector = _p3.Position.Subtract(_p1.Position)
		Local t2:TVector = _p2.Position.Subtract(_p1.Position)

		Local Angle:Double = t1.AngleBetween(t2)
		
		If Angle<_angle
			Local da:Double = (_angle-Angle)/(2*(1.0/_p2.mass+1.0/_p3.mass))
			If t1.GetAngle()>t2.GetAngle()
				t1.SetAngle(t1.GetAngle() + da/_p3.mass)
				t2.SetAngle(t2.GetAngle() - da/_p2.mass)
			Else
				t1.SetAngle(t1.GetAngle() - da/_p3.mass)
				t2.SetAngle(t2.GetAngle() + da/_p2.mass)
			EndIf
			t1.Add(_p1.Position,True)
			t2.Add(_p1.Position,True)
			
			If _p3.bLocked = False
				_p3.Position = t1
			EndIf
			If _p2.bLocked = False
				_p2.Position = t2
			EndIf
		EndIf
	End Method
	
	Method CalculateAngle:Double()
		Local t1:TVector = _p3.Position.Subtract(_p1.Position)
		Local t2:TVector = _p2.Position.Subtract(_p1.Position)
		
		Return t1.AngleBetween(t2)
	End Method 		
	
	Function Create:TAngularConstraint (p1:TParticle,p2:TParticle,p3:TParticle,angle:Float = 0)
		Local c:TAngularConstraint = New TAngularConstraint
		c._p1 = p1
		c._p2 = p2
		c._p3 = p3
		If angle = 0
			c._angle = c.CalculateAngle()
		Else
			c._angle = angle
		EndIf
		Return c
	End Function
	
	Function DoFrame()
		For Local o:TAngularConstraint = EachIn(list)
			o.Update()
		Next
	End Function
End Type

and an example

Graphics 640,480,0
Local p:TParticle = TParticle.Create(640/2,480/2)
Local p2:TParticle = TParticle.Create(640/2+25,480/2+50)
Local p3:TParticle = TParticle.Create(640/2-25,480/2+50)
TConstraint.Create(p,p2)
TConstraint.Create(p,p3)
p2.mass = 2
Local l:TAngularConstraint = TAngularConstraint.Create(p,p2,p3)
p.Lock()
Local x# =640/2
Local y# =480/2
TPhysicsEngine.Gravity = 70
TPhysicsEngine.Resistance = 0.005
	
While Not KeyDown(KEY_ESCAPE)
	Cls
	DrawText l.CalculateAngle(),10,10
	If MouseDown(1) Then p2.SetPosition(MouseX(),MouseY())
	TPhysicsEngine.DoFrame()
	Flip
Wend