Verlet Physics & Collision

Monkey Forums/Monkey Code/Verlet Physics & Collision

NoOdle (Posted 2011) [#1]

Taken from the article here: http://archive.gamedev.net/reference/programming/features/verletPhys/default.asp I have translated the code by Benedikt Bitterli. You can find a download to his source on the last page of the article. There is however a translation error in this code, if you uncomment the line that creates triangles you will see what I mean. Hopefully someone can spot where I went wrong. Hold the upkey to run the simulation or uncomment the keydown lines. Import mojo 'Maximum body/vertex/edgecount the physics simulation can handle Global MAX_BODIES : Int = 512 Global MAX_VERTICES : Int = 1024 Global MAX_EDGES : Int = 1024 'Maximum body/edge count a body can contain Global MAX_BODY_VERTICES : Int = 64 Global MAX_BODY_EDGES : Int = 64 Global World : Physics Function Main() New myApp() End Function Class myApp Extends App Method OnCreate() SetUpdateRate( 60 ) 'creates a world World = New Physics( 0.0, 0.5, 10 ) 'create a few box bodies Local testBody : PhysicsBody Local X : Int Local Y : Int For X = 20 To DeviceWidth() - 150 Step 100 For Y = 50 To DeviceHeight() - 250 Step 100 testBody = New PhysicsBody() testBody.CreateBox( X, Y, 50, 50 ) Next Next 'create a few triangle bodies ' For X = 50 To DeviceWidth - 50 Step 130 ' testBody = New PhysicsBody() ' Local V1 : Vertex = New Vertex( testBody, X, 45 ) ' Local V2 : Vertex = New Vertex( testBody, X + 50, 0 ) ' Local V3 : Vertex = New Vertex( testBody, X + 100, 45 ) ' New Edge( testBody, V1, V2, True ) ' New Edge( testBody, V2, V3, True ) ' New Edge( testBody, V3, V1, True ) ' Next End Method Method OnUpdate() If KeyDown( KEY_UP ) World.Update() Endif End Method Method OnRender() Cls 0, 0, 0 World.Render() End Method End Class Class Vec2 Field X : Float Field Y : Float Method New( vX : Float, vY : Float ) Self.X = vX Self.Y = vY End Method Method GetAngle : Float() Return Atan2( Y, X ) End Method Method Add( _x : Float, _y : Float ) X = X + _x Y = Y + _y End Method Method AddVec( Vec:Vec2 ) If Vec = Null Then Return X = X + Vec.X Y = Y + Vec.Y End Method Method SubtractVec( Vec : Vec2 ) If Vec = Null Return X = X - Vec.X Y = Y - Vec.Y End Method Method Multiply( _x : Float, _y : Float) X = X * _x Y = Y * _y End Method Method MultiplyVec( Vec : Vec2 ) If Vec = Null Return X = X * Vec.X Y = Y * Vec.Y End Method Method Divide( _x : Float, _y : Float ) If _x = 0 Or _y = 0 Return X = X / _x Y = Y / _y End Method Method DotProduct : Float( Vec : Vec2 ) Return X * Vec.X + Y * Vec.Y End Method Method GetLength : Float() Return Sqrt( X * X + Y * Y ) End Method Method Normalize() Local Lent : Float = 1.0 / Self.GetLength() X = X * Lent Y = Y * Lent End Method End Class Class Physics Global CollisionInfoE : Edge Global CollisionInfoV : Vertex Global CollisionInfoNormal : Vec2 Global CollisionInfoDepth : Float Field Gravity : Vec2 Field BodyCount : Int Field VertexCount : Int Field EdgeCount : Int Field Vertices : Vertex[ MAX_VERTICES ] Field Edges : Edge[ MAX_EDGES ] Field Bodies : PhysicsBody[ MAX_BODIES ] Field Timestep : Float Field Iterations : Int Method UpdateForces : Void() Local i : Int For i = 0 To Self.VertexCount - 1 Vertices[ i ].Acceleration = Gravity Next End Method Method UpdateVerlet : Void() Local i : Int For i = 0 To VertexCount - 1 Local V : Vertex = Vertices[ i ] Local TempX : Float = V.Position.X Local TempY : Float = V.Position.Y V.Position.X = V.Position.X + ( V.Position.X - V.OldPosition.X + V.Acceleration.X * Timestep * Timestep ) V.Position.Y = V.Position.Y + ( V.Position.Y - V.OldPosition.Y + V.Acceleration.Y * Timestep * Timestep ) V.OldPosition.X = TempX V.OldPosition.Y = TempY Next End Method Method UpdateEdges : Void() Local i : Int For i = 0 To EdgeCount - 1 Local E : Edge = Edges[ i ] Local V1V2 : Vec2 = New Vec2( E.V2.Position.X, E.V2.Position.Y ) V1V2.SubtractVec( E.V1.Position ) Local V1V2Length : Float = V1V2.GetLength() Local Diff : Float = V1V2Length - E.Length V1V2.Normalize() E.V1.Position.X = E.V1.Position.X + V1V2.X * Diff * 0.5 E.V1.Position.Y = E.V1.Position.Y + V1V2.Y * Diff * 0.5 E.V2.Position.X = E.V2.Position.X - V1V2.X * Diff * 0.5 E.V2.Position.Y = E.V2.Position.Y - V1V2.Y * Diff * 0.5 Next End Method Method IterateCollisions : Void() Local i : Int For i = 0 To Iterations - 1 Local t : Int For t = 0 To VertexCount - 1 Local Pos : Vec2 = Vertices[ t ].Position Pos.X = Max( Min( Pos.X, Float(DeviceWidth())), 0.0 ) Pos.Y = Max( Min( Pos.Y, Float(DeviceHeight())), 0.0 ) Next 'Edge Correction Step Self.UpdateEdges() 'Recalculate the centres Local c : Int For c = 0 To BodyCount - 1 Bodies[ c ].CalculateCenter() Next 'Iterate trough all bodies Local B1 : Int Local B2 : Int For B1 = 0 To BodyCount - 1 For B2 = 0 To BodyCount - 1 If B1 <> B2 'test bounding boxes If Self.BodiesOverlap( Bodies[ B1 ], Bodies[ B2 ] ) 'if there is a collision respond to it If Self.DetectCollision( Bodies[ B1 ], Bodies[ B2 ] ) Self.ProcessCollision() Endif Endif Endif Next Next Next End Method Method DetectCollision : Bool( B1 : PhysicsBody, B2 : PhysicsBody ) Local MinDistance : Float = 10000.0 Local i : Int For i = 0 To B1.EdgeCount + B2.EdgeCount - 2 Local E : Edge If( i < B1.EdgeCount ) E = B1.Edges[ i ] Else E = B2.Edges[ i - B1.EdgeCount ] Endif 'This will skip edges that lie totally inside the bodies, as they don't matter. 'The boundary flag has To be set manually And defaults To True If( Not E.Boundary ) Continue Endif 'Calculate the perpendicular To this edge And normalize it Local Axis : Vec2 = New Vec2( E.V1.Position.Y, E.V2.Position.X ) Axis.X = Axis.X - E.V2.Position.Y Axis.Y = Axis.Y - E.V1.Position.X Axis.Normalize() 'Project both bodies onto the perpendicular Local MinA : Float[ 1 ] Local MinB : Float[ 1 ] Local MaxA : Float[ 1 ] Local MaxB : Float[ 1 ] B1.ProjectToAxis( Axis, MinA, MaxA ) B2.ProjectToAxis( Axis, MinB, MaxB ) 'Calculate the distance between the two intervals Local Distance : Float = IntervalDistance( MinA, MaxA, MinB, MaxB ) 'If the intervals don't overlap, return, since there is no collision If Distance > 0.0 Return False Elseif Abs( Distance ) < MinDistance MinDistance = Abs( Distance ) CollisionInfoNormal = Axis CollisionInfoE = E Endif Next CollisionInfoDepth = MinDistance 'Ensure that the body containing the collision edge lies in B2 And the one conatining the collision vertex in B1 If( CollisionInfoE.Parent <> B2 ) Local Temp : PhysicsBody = B2 B2 = B1 B1 = Temp Endif 'This is needed To make sure that the collision normal is pointing at B1 Local SignVec : Vec2 = New Vec2( CollisionInfoNormal.X, CollisionInfoNormal.Y ) Local CenterDiff : Vec2 = New Vec2( B1.Center.X, B1.Center.Y ) CenterDiff.SubtractVec( B2.Center ) Local SignFloat : Float = SignVec.X * CenterDiff.X + SignVec.Y * CenterDiff.Y Local Sign : Int = Sgn( SignFloat ) 'Remember that the line equation is N*( R - R0 ). We choose B2->Center as R0; the normal N is given by the collision normal If Sign <> 1 'Revert the collision normal If it points away from B1 CollisionInfoNormal.X = -CollisionInfoNormal.X CollisionInfoNormal.Y = -CollisionInfoNormal.Y Endif Local CollisionVector : Vec2 = New Vec2( CollisionInfoNormal.X, CollisionInfoNormal.Y ) CollisionVector.Multiply( CollisionInfoDepth, CollisionInfoDepth ) 'Initialize the smallest distance To a large value Local SmallestD : Float = 10000.0 Local Distance : Float For i = 0 To B1.VertexCount - 1 'Measure the distance of the vertex from the line using the line equation Local cDist : Vec2 = New Vec2( B1.Vertices[ i ].Position.X, B1.Vertices[ i ].Position.Y ) cDist.SubtractVec( B2.Center ) Distance = CollisionInfoNormal.X * cDist.X + CollisionInfoNormal.Y * cDist.Y 'If the measured distance is smaller than the smallest distance reported so far, set the smallest distance And the collision vertex If Distance < SmallestD SmallestD = Distance CollisionInfoV = B1.Vertices[ i ] Endif Next 'There is no separating axis. Report a collision! Return True End Method Method ProcessCollision : Void() Local E1 : Vertex = CollisionInfoE.V1 Local E2 : Vertex = CollisionInfoE.V2 Local CollisionVector : Vec2 = New Vec2( CollisionInfoNormal.X * CollisionInfoDepth, CollisionInfoNormal.Y * CollisionInfoDepth ) Local T : Float If Abs( E1.Position.X - E2.Position.X ) > Abs( E1.Position.Y - E2.Position.Y ) T = ( CollisionInfoV.Position.X - CollisionVector.X - E1.Position.X ) / ( E2.Position.X - E1.Position.X ) Else T = ( CollisionInfoV.Position.Y - CollisionVector.Y - E1.Position.Y ) / ( E2.Position.Y - E1.Position.Y ) Endif Local Lambda : Float = 1.0 / ( T * T + ( 1 - T ) * ( 1 - T ) ) E1.Position.X = E1.Position.X - CollisionVector.X * ( 1 - T ) * 0.5 * Lambda E1.Position.Y = E1.Position.Y - CollisionVector.Y * ( 1 - T ) * 0.5 * Lambda E2.Position.X = E2.Position.X - CollisionVector.X * T * 0.5 * Lambda E2.Position.Y = E2.Position.Y - CollisionVector.Y * T * 0.5 * Lambda CollisionInfoV.Position.X = CollisionInfoV.Position.X + CollisionVector.X * 0.5 CollisionInfoV.Position.Y = CollisionInfoV.Position.Y + CollisionVector.Y * 0.5 End Method Method IntervalDistance : Float( MinA : Float[], MaxA : Float[], MinB : Float[], MaxB : Float[] ) If MinA[ 0 ] < MinB[ 0 ] Return MinB[ 0 ] - MaxA[ 0 ] Else Return MinA[ 0 ] - MaxB[ 0 ] Endif End Method 'Used For optimization To test If the bounding boxes of two bodies overlap Method BodiesOverlap : Bool( B1 : PhysicsBody, B2 : PhysicsBody ) Return ( B1.MinX <= B2.MaxX ) And ( B1.MinY <= B2.MaxY ) And ( B1.MaxX >= B2.MinX ) And ( B2.MaxY >= B1.MinY ) End Method Method Update() Self.UpdateForces() Self.UpdateVerlet() Self.IterateCollisions() End Method Method Render() Local i : Int For i = 0 To EdgeCount - 1 DrawLine( Edges[ i ].V1.Position.X, Edges[ i ].V1.Position.Y, Edges[ i ].V2.Position.X, Edges[ i ].V2.Position.Y ) Next ' For i = 0 To VertexCount - 1 ' DrawOval( Vertices[ i ].Position.X - 2, Vertices[ i ].Position.Y - 2, 4, 4 ) ' Next End Method 'Adds New elements To the simulation Method AddBody( Body : PhysicsBody ) Self.Bodies[ Self.BodyCount ] = Body Self.BodyCount = Self.BodyCount + 1 End Method Method AddEdge( E : Edge ) Self.Edges[ Self.EdgeCount ] = E Self.EdgeCount = Self.EdgeCount + 1 End Method Method AddVertex( V : Vertex ) Self.Vertices[ Self.VertexCount ] = V Self.VertexCount = Self.VertexCount + 1 End Method Method FindVertex : Vertex( X : Int, Y : Int ) End Method Method New( GravitationX : Float, GravitationY : Float, pIterations : Int ) BodyCount = 0 VertexCount = 0 EdgeCount = 0 Timestep = 1.0 Gravity = New Vec2( GravitationX, GravitationY ) Iterations = pIterations End Method End Class Class PhysicsBody 'Center of mass Field Center : Vec2 'Min/max coordinates of the bounding box Field MinX : Int Field MinY : Int Field MaxX : Int Field MaxY : Int Field VertexCount : Int Field EdgeCount : Int Field Vertices : Vertex[ MAX_BODY_VERTICES ] Field Edges : Edge[ MAX_BODY_EDGES ] 'The constructor is empty Method New() Self.VertexCount = 0 Self.EdgeCount = 0 World.AddBody( Self ) End Method Method AddEdge : Void( E : Edge ) Self.Edges[ Self.EdgeCount ] = E Self.EdgeCount = Self.EdgeCount + 1 End Method Method AddVertex : Void( V : Vertex ) Self.Vertices[ Self.VertexCount ] = V Self.VertexCount = Self.VertexCount + 1 End Method Method ProjectToAxis( Axis : Vec2, MinV : Float[], MaxV : Float[] ) Local DotP : Float = Axis.DotProduct( Vertices[ 0 ].Position ) MinV[ 0 ] = DotP MaxV[ 0 ] = DotP Local i : Int For i = 0 To VertexCount - 1 DotP = Axis.DotProduct( Vertices[ i ].Position ) MinV[ 0 ] = Min( DotP, MinV[ 0 ]) MaxV[ 0 ] = Max( DotP, MaxV[ 0 ]) Next End Method 'Calculates the center of mass Method CalculateCenter : Void() Center = New Vec2( 0.0, 0.0 ) MinX = 10000.0 MinY = 10000.0 MaxX = -10000.0 MaxY = -10000.0 Local i : Int For i = 0 To Self.VertexCount - 1 Center.AddVec( Vertices[ i ].Position ) MinX = Min( Float( MinX ), Vertices[ i ].Position.X ) MinY = Min( Float( MinY ), Vertices[ i ].Position.Y ) MaxX = Max( Float( MaxX ), Vertices[ i ].Position.X ) MaxY = Max( Float( MaxY ), Vertices[ i ].Position.Y ) Next Center.Divide( VertexCount, VertexCount ) End Method 'Helper Function To create a box primitive Method CreateBox( X : Int, Y : Int, Width : Int, Height : Int ) Local V1 : Vertex = New Vertex( Self, X, Y ) Local V2 : Vertex = New Vertex( Self, X + Width, Y ) Local V3 : Vertex = New Vertex( Self, X + Width, Y + Height ) Local V4 : Vertex = New Vertex( Self, X, Y + Height ) New Edge( Self, V1, V2, True ) New Edge( Self, V2, V3, True ) New Edge( Self, V3, V4, True ) New Edge( Self, V4, V1, True ) New Edge( Self, V1, V3, False ) New Edge( Self, V2, V4, False ) End Method End Class '================================================================================= ' Vertex '================================================================================= Class Vertex Field Position : Vec2 Field OldPosition : Vec2 Field Acceleration : Vec2 Field Parent : PhysicsBody 'Constructor Method New( Body : PhysicsBody, PosX : Float, PosY : Float ) Self.Position = New Vec2( PosX, PosY ) Self.OldPosition = New Vec2( PosX, PosY ) Self.Parent = Body '//Add the vertex To the given body And To the physics simulator Body.AddVertex( Self ) World.AddVertex( Self ) End Method End Class '================================================================================= ' EDGE '================================================================================= Class Edge Field V1 : Vertex Field V2 : Vertex Field Length : Float 'Value used For optimization - see Physics::DetectCollision For more information Field Boundary : Bool Field Parent : PhysicsBody 'Constructor Method New( Body : PhysicsBody, pV1 : Vertex, pV2 : Vertex, pBoundary : Bool = True ) 'Set boundary vertices Self.V1 = pV1 Self.V2 = pV2 'Calculate the original length Local this : Vec2 = New Vec2( pV2.Position.X, pV2.Position.Y ) this.SubtractVec( pV1.Position ) Self.Length = this.GetLength() Self.Boundary = pBoundary Self.Parent = Body 'Add the edge To the given body And To the physics simulator Body.AddEdge( Self ) World.AddEdge( Self ) End Method End Class

matt (Posted 2011) [#2]

Do you mean the triangle shooting off a bit at the start? If you decrease the step of the triangle generation from 130 to 120 (so they're a little closer together) it doesn't do that. Other than that it works for me, nice one.

NoOdle (Posted 2011) [#3]

The shooting off at the start is fine as thats the response to force the triangles into the screen area. The problem I've observed is the triangles floating unrealistically instead of sliding off.

matt (Posted 2011) [#4]

Ah right, I now see what you mean.

RENGAC (Posted 2013) [#5]

I know that this is a very old post, but it's very useful for me. The solid objects seems that there is not any friction between them, as they slides a lot. Modifing the number of iterations changes a lot the simulation results. Any of you have delved more deeper into verlet physics?