Verlet Collisions

Monkey Forums/Monkey Programming/Verlet Collisions
Canardian
(Posted 2011) [#1]
I ported a simple Verlet Physics example to Monkey, and managed to get 3 kinds of collisions, but still no collision where the active colliding body would continue rotation. Anyone got any ideas how to make it?

Here's the code so far:
Import mojo

Global CONST_ITERATIONS = 5
Global dh:Float
Global dw:Float
Global MouseForce:Float = 0.1
Global ModelCollisionMode:Int = 0	' 0 = density collision, 1 = throwout collision, 2 = bounce collision
Global ModelDensity:Float = 0.5		' higher value is higher density, 1.9 is max
Global ModelThrowout:Float = 0.05	' higher value is higher throwout, 0.05 is max
Global ModelBounce:Float = 1.82		' higher value is higher bounce, 2.0 is max
Global s:String					' for drawing text

Class Vec2
	Field x:Float
	Field y:Float
End

Class Color
	Field r:Int
	Field g:Int
	Field b:Int
End

Class PointMass Extends Vec2
	Field ox:Float
	Field oy:Float
End

Class Constraint
	Field p1:PointMass
	Field p2:PointMass
	Field length:Float
	Field color:Color
End

Global P:PointMass[7]
Global C:Constraint[8]

Function LinesCollide:Bool(x1:Float,y1:Float,x2:Float,y2:Float,x3:Float,y3:Float,x4:Float,y4:Float)
	Local dpx:Float= x3 - x1 + x4 - x2
	Local dpy:Float= y3 - y1 + y4 - y2
	Local qax:Float= x2 - x1
	Local qay:Float= y2 - y1
	Local qbx:Float= x4 - x3
	Local qby:Float= y4 - y3
	Local d:Float= qay*qbx - qby*qax
	Local la:Float = qbx*dpy - qby*dpx
	Local lb:Float = qax*dpy - qay*dpx
	If Abs(la)<=Abs(d) And Abs(lb)<=Abs(d)
		Return True
	End
	Return False
End

Function check_tri_clock_dir:Int(pt1:PointMass, pt2:PointMass, pt3:PointMass)
	Local test:Float
	test = (((pt2.x - pt1.x)*(pt3.y - pt1.y)) - ((pt3.x - pt1.x)*(pt2.y - pt1.y)))
	If (test > 0) Then
		Return 1
	Else
		If(test < 0) Then
			Return -1
		Else
			Return 0
		End
	End
End

Function check_intersect:Int( l1p1:PointMass, l1p2:PointMass, l2p1:PointMass, l2p2:PointMass)
	Local test1_a:Int, test1_b:Int, test2_a:Int, test2_b:Int
	test1_a = check_tri_clock_dir(l1p1, l1p2, l2p1)
	test1_b = check_tri_clock_dir(l1p1, l1p2, l2p2)
	If (test1_a <> test1_b) Then
		test2_a = check_tri_clock_dir(l2p1, l2p2, l1p1)
		test2_b = check_tri_clock_dir(l2p1, l2p2, l1p2)
		If (test2_a <> test2_b) Then
			Return 1
		End
   	End
	Return 0
End

Class Game Extends App
	Method OnCreate()
		SetUpdateRate 60
		dh=DeviceHeight
		dw=DeviceWidth
		P[0] = CreatePointMass(100,100,0,0)
		P[1] = CreatePointMass(100,150,0,0)
		P[2] = CreatePointMass(150,150,0,0)
		P[3] = CreatePointMass(150,100,0,0)
		P[4] = CreatePointMass(200,50,0,0)
		P[5] = CreatePointMass(180,100,0,0)
		P[6] = CreatePointMass(220,100,0,0)
		C[0] = CreateConstraint(P[0],P[1])
		C[1] = CreateConstraint(P[1],P[2])
		C[2] = CreateConstraint(P[2],P[3])
		C[3] = CreateConstraint(P[0],P[3])
		C[4] = CreateConstraint(P[0],P[2])
		'C[5] = CreateConstraint(P[1],P[3])			' second diagonal is not needed
		C[5] = CreateConstraint(P[4],P[5])
		C[6] = CreateConstraint(P[5],P[6])
		C[7] = CreateConstraint(P[6],P[4])
	End
	Method OnOff:String(a:Bool)
		If a
			Return "ON "
		Else
			Return "OFF"
		End
	End
	Method OnRender()
		Cls
		DrawPointMasses
		DrawConstraints
		'DrawText "tx,ty="+P[4].x+","+P[4].y,0,0
		'DrawText "mx,my="+MouseX+","+MouseY,0,12
		DrawText "Click Mouse to attract points (NOTE: Only triangle has active collision)",0,0
		DrawText "1: Density Collision = "+OnOff(ModelCollisionMode=0),0,12
		DrawText "2: Throwout Collision = "+OnOff(ModelCollisionMode=1),0,24
		DrawText "3: Bounce Collision = "+OnOff(ModelCollisionMode=2),0,36
		DrawText "5/6: Density = "+ModelDensity,0,48
		DrawText "7/8: Throwout = "+ModelThrowout,0,60
		DrawText "9/0: Bounce = "+ModelBounce,0,72
		DrawText s,0,84
	End
	Method OnUpdate()
		UpdatePointMasses
		UpdateConstraints
		CheckCollisions
		If KeyHit(KEY_SPACE)
			For Local p:PointMass=Eachin P
				p.y-=5
			Next
		End
		If KeyHit(KEY_1) ModelCollisionMode=0
		If KeyHit(KEY_2) ModelCollisionMode=1
		If KeyHit(KEY_3) ModelCollisionMode=2
		If KeyDown(KEY_5) ModelDensity+=0.01
		If KeyDown(KEY_6) ModelDensity-=0.01
		If KeyDown(KEY_7) ModelThrowout+=0.01
		If KeyDown(KEY_8) ModelThrowout-=0.01
		If KeyDown(KEY_9) ModelBounce+=0.01
		If KeyDown(KEY_0) ModelBounce-=0.01
		If MouseDown(0)
			Local p:PointMass = FindNearestPoint(MouseX,MouseY)
			If p
				p.x+=(MouseX-p.x)*MouseForce
				p.y+=(MouseY-p.y)*MouseForce
			End
		End
	End
End

Function PointDistance(x1:Float,y1:Float,x2:Float,y2:Float)
	Return Sqrt( ((x1-x2)*(x1-x2)) + ((y1-y2)*(y1-y2)) )
End

Function FindNearestPoint:PointMass(x:Float,y:Float,maxrange:Float=100.0)
	Local result:PointMass=Null
	For Local p:PointMass = Eachin P
		If result=Null
			result=p
		End
		If PointDistance(p.x,p.y,x,y)<PointDistance(result.x,result.y,x,y)
			result=p
		End
	Next
	If PointDistance(result.x,result.y,x,y)<maxrange
		Return result
	Else
		Return Null
	End
End

Function DrawPointMasses()
	SetColor 0,0,255
	For Local p:PointMass = Eachin P
		DrawOval p.x-4,p.y-4,8,8		' draws an oval at the verlet's position
	Next
End

Function CheckCollisions()
	' C5-7 And P4-6 is the triangle model
	Local res:Bool
	Local dx:Float
	Local dy:Float
	Local p:PointMass
	For Local i:Int=5 To 7
		C[i].color.r=0
		For Local j:Int=0 To 3		' constraints of the other models
			If check_intersect(C[i].p1,C[i].p2,C[j].p1,C[j].p2)
			'If LinesCollide(C[i].p1.x,C[i].p1.y,C[i].p2.x,C[i].p2.y,C[j].p1.x,C[j].p1.y,C[j].p2.x,C[j].p2.y)
				res=True

				p=C[i].p1
				If ModelCollisionMode=1 Or ModelCollisionMode=2
					dy = p.oy-p.y
					If ModelCollisionMode=1
						dy *= ModelThrowout
						p.oy += dy
					Else
						dy *= ModelBounce
						p.y += dy
					End
				Else
					dy = p.y-p.oy
					dy *= ModelDensity
					p.oy += dy
				End
				If ModelCollisionMode=1 Or ModelCollisionMode=2
					dx = p.ox-p.x
					If ModelCollisionMode=1
						dx *= ModelThrowout
						p.ox += dx
					Else
						dx *= ModelBounce
						p.x += dx
					End
				Else
					dx = p.x-p.ox
					dx *= ModelDensity
					p.ox += dx
				End

				p=C[i].p2
				If ModelCollisionMode=1 Or ModelCollisionMode=2
					dy = p.oy-p.y
					If ModelCollisionMode=1
						dy *= ModelThrowout
						p.oy += dy
					Else
						dy *= ModelBounce
						p.y += dy
					End
				Else
					dy = p.y-p.oy
					dy *= ModelDensity
					p.oy += dy
				End
				If ModelCollisionMode=1 Or ModelCollisionMode=2
					dx = p.ox-p.x
					If ModelCollisionMode=1
						dx *= ModelThrowout
						p.ox += dx
					Else
						dx *= ModelBounce
						p.x += dx
					End
				Else
					dx = p.x-p.ox
					dx *= ModelDensity
					p.ox += dx
				End
				
				C[i].color.r=255
			End
		Next
	Next
	If res Then s="COLLIDE" Else s=""
End

Function UpdatePointMasses()
	For Local p:PointMass = Eachin P
		Local dx:Float = p.x-p.ox		' gets the velocity
		Local dy:Float = p.y-p.oy+.1	' added gravity here
		p.ox = p.x					' updates ox And oy
		p.oy = p.y
		p.x += dx					' adds velocity To get New x And New y
		p.y += dy
		If p.y > dh
			p.y = dh
			dx = p.x-p.ox
			dx *= .5
			p.ox += dx				' simulates friction
		End
		If p.x > dw
			p.x = dw
		End
		If p.x < 0
			p.x = 0
		End
		If p.y < 0
			p.y = 0
		End
	Next
End

' x And y are coords For the verlet. vx And vy are velocity values For the verlet
Function CreatePointMass:PointMass(x:Float,y:Float,vx:Float,vy:Float)
	Local p:PointMass = New PointMass
	p.x = x
	p.y = y
	p.ox = x - vx			' gives the particle a starting velocity
	p.oy = y - vy
	Return p
End

Function UpdateConstraints()
	For Local cnt:Int=0 To CONST_ITERATIONS-1	' this is necessary with many constraints To solve them correctly
		For Local c:Constraint = Eachin C
			Local dist:Float = Sqrt( ((c.p1.x-c.p2.x)*(c.p1.x-c.p2.x)) + ((c.p1.y-c.p2.y)*(c.p1.y-c.p2.y)) )	'distance formula
			Local diff:Float = dist-c.length	'shows the margin of error the update loop has created so it can be corrected
			Local dx:Float = c.p1.x-c.p2.x	'difference between x's and y's
			Local dy:Float = c.p1.y-c.p2.y
			If c.length > 0 Then	'prevents a divided by 0 error that may occur
				diff /= c.length
			Else
				diff = 0
			End
			dx *= .5
			dy *= .5
			c.p1.x -= diff*dx
			c.p1.y -= diff*dy
			c.p2.x += diff*dx
			c.p2.y += diff*dy
		Next
	Next
End

Function CreateConstraint:Constraint(p1:PointMass,p2:PointMass)
	Local c:Constraint = New Constraint
	c.p1 = p1
	c.p2 = p2
	c.length = Sqrt( ((c.p1.x-c.p2.x)*(c.p1.x-c.p2.x)) + ((c.p1.y-c.p2.y)*(c.p1.y-c.p2.y)) )
	c.color = New Color
	c.color.g = 255
	Return c
End

Function DrawConstraints()
	For Local c:Constraint = Eachin C
		SetColor c.color.r,c.color.g,c.color.b
		DrawLine c.p1.x,c.p1.y,c.p2.x,c.p2.y
	Next
End

Function Main()
	New Game
End