Its probaly more fair if I post the code we have so far for others while I am occupied. So here is a complete post of all the files so far. Note: Collide.bmx only needs some vector and matrix equations converted. I've commented out the work still to be completed so it should run, but with major errors however. It should be debugable however.
arbiter.bmx
body.bmx
joint.bmx
world.bmx
mathutils.bmx
main.bmx
collide.bmx
Oh yeah, and merry christmas :)
arbiter.bmx
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
body.bmx
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 '/** ' * ' */ Function Body:TBody() Local b:Tbody = New TBody b.position.Set(0.0, 0.0) b.rotation = 0.0 b.velocity.Set(0.0, 0.0) b.angularVelocity = 0.0 b.force.Set(0.0, 0.0) b.torque = 0.0 b.friction = 0.2 b.width.Set(1.0, 1.0) b.mass = FLT_MAX b.invMass = 0.0 b.I = FLT_MAX b.invI = 0.0 Return New TBody End Function '/** ' * ' */ Type TBody Field position:TVec2 Field rotation:Float Field velocity:TVec2 Field angularVelocity:Float Field force:TVec2 Field torque:Float Field width:TVec2 Field friction:Float Field mass:Float, invMass:Float Field I:Float, invI:Float Method New() position = New TVec2 velocity = New TVec2 force = New TVec2 width = New TVec2 End Method Method Set(w:TVec2, m:Float) position.Set(0.0, 0.0) rotation = 0.0 velocity.Set(0.0, 0.0) angularVelocity = 0.0 force.Set(0.0, 0.0) torque = 0.0 friction = 0.2 width = w mass = m If (mass < FLT_MAX) invMass = 1.0 / mass I = mass * (width.x * width.x + width.y * width.y) / 12.0 invI = 1.0 / I Else invMass = 0.0 I = FLT_MAX invI = 0.0 End If End Method Method AddForce (f:TVec2) force.AddV(f) End Method End Type
joint.bmx
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 Function Joint:TJoint () Return New TJoint End Function Type TJoint Field M:TMat22 Field localAnchor1:TVec2, localAnchor2:TVec2 Field r1:TVec2, r2:TVec2 Field bias:TVec2 Field accumulatedImpulse:TVec2 Field body1:TBody Field body2:TBody Field relaxation:Float Method New() AccumulatedImpulse = New TVec2 Relaxation = 1.0 End Method Method Set (b1:TBody, b2:TBody, anchor:TVec2) body1 = b1 body2 = b2 Local Rot1:TMat22 = Mat22_Angle(body1.rotation) Local Rot2:TMat22 = Mat22_Angle(body2.rotation) Local Rot1T:TMat22 = Rot1.Transpose() Local Rot2T:TMat22 = Rot2.Transpose() localAnchor1 = MultMat22Vec2(Rot1T, SubVec2Vec2(anchor, body1.position)) localAnchor1 = MultMat22Vec2(Rot2T, SubVec2Vec2(anchor, body2.position)) accumulatedImpulse.Set(0.0, 0.0) relaxation = 1.0 End Method Method PreStep(inv_dt:Float) ' Pre-compute anchors, mass matrix, And bias. Local Rot1:TMat22 = Mat22_Angle(body1.rotation) Local Rot2:TMat22 = Mat22_Angle(body2.rotation) r1 = MultMat22Vec2(Rot1, localAnchor1) r2 = MultMat22Vec2(Rot2, localAnchor2) ' deltaV = deltaV0 + K * impulse ' invM = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] ' = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y] ' [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x] Local K1:TMat22 = New TMat22 K1.col1.x = body1.invMass + body2.invMass; K1.col2.x = 0.0 K1.col1.y = 0.0; K1.col2.y = body1.invMass + body2.invMass Local K2:TMat22 K2.col1.x = body1.invI * r1.y * r1.y; K2.col2.x = -body1.invI * r1.x * r1.y K2.col1.y = -body1.invI * r1.x * r1.y; K2.col2.y = body1.invI * r1.x * r1.x Local K3:TMat22 K3.col1.x = body2.invI * r2.y * r2.y; K3.col2.x = -body2.invI * r2.x * r2.y; K3.col1.y = -body2.invI * r2.x * r2.y; K3.col2.y = body2.invI * r2.x * r2.x; Local K:TMat22 = AddMat22Mat22(AddMat22Mat22(K1, K2), K3) M = K.Invert() Local p1:TVec2 = AddVec2Vec2(body1.position, r1) Local p2:TVec2 = AddVec2Vec2(body2.position, r2) Local dp:TVec2 = SubVec2Vec2(p2, p1) bias = MultFloatVec2((-0.1 * inv_dt), dp) 'Apply accumulated impulse. accumulatedImpulse.Mult(relaxation) body1.velocity.SubV(MultFloatVec2(body1.invMass, accumulatedImpulse)) body1.angularVelocity :- body1.invI * CrossVec2Vec2(r1, accumulatedImpulse) body2.velocity.AddV(MultFloatVec2(body2.invMass, accumulatedImpulse)) body2.angularVelocity :+ body2.invI * CrossVec2Vec2(r2, accumulatedImpulse) End Method Method ApplyImpulse() Local dv:TVec2 = AddVec2Vec2(body2.velocity, CrossFloatVec2(body2.angularVelocity, r2)) dv.SubV(SubVec2Vec2(body1.velocity, CrossFloatVec2(body1.angularVelocity, r1))) Local impulse:TVec2 = MultMat22Vec2(M, AddVec2Vec2(dv.Reverse(), bias)) body1.velocity.SubV(MultFloatVec2(body1.invMass,impulse)) body1.angularVelocity :- body1.invI * CrossVec2Vec2(r1, impulse) body2.velocity.AddV(MultFloatVec2(body2.invMass, impulse)) body2.angularVelocity :+ body2.invI * CrossVec2Vec2(r2, impulse) accumulatedImpulse.AddV(impulse) End Method End Type
world.bmx
Rem * Copyright (c) 2006 Erin Catto <a href="http://www.gphysics.com" target="_blank">www.gphysics.com</a> * * 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 Function World:TWorld (gravity:TVec2, iterations:Int) Local w:TWorld = New TWorld w.gravity = gravity w.iterations = iterations Return w End Function Type TWorld Field Bodies:TList Field Joints:TList Field Arbiters:TList Field Gravity:TVec2 Field iterations:Int Method New() Bodies = CreateList() Joints = CreateList() Arbiters = CreateList() Gravity = Vec2(0.0,0.0) End Method Method Add ( obj:Object ) If TBody(obj) Bodies.AddLast(obj) If TJoint(obj) Joints.AddLast(obj) End Method Method Clear() Bodies.Clear Joints.Clear Arbiters.Clear End Method Method BroadPhase() ' O(n^2) broad-phase Local BodiesArray:Object [] = Bodies.ToArray() For Local i:Int = 0 To BodiesArray.Length - 1 Local bi:Tbody = TBody(BodiesArray[i]) For Local j:Int = i + 1 To BodiesArray.Length - 1 Local bj:TBody = TBody(BodiesArray[j]) If (bi.invMass = 0.0 And bj.invMass = 0.0) Continue Local newArb:TArbiter = Arbiter(bi, bj) If (newArb.numContacts > 0) 'Not sure what is being accomplished here: Local arb:TLink = Arbiters.FindLink(newArb) 'ArbIter arb = arbiters.find(newArb) If (arb = Arbiters.LastLink()) Arbiters.AddLast(newArb) Else TArbiter(arb.Value()).Update(newArb.contacts, newArb.numContacts) End If Else Arbiters.Remove(newArb) End If Next Next End Method Method _Step (dt:Float) Local inv_dt:Float If dt > 0.0 Then inv_dt = 1.0 / dt Else inv_dt = 0.0 BroadPhase() Local BodiesArray:Object [] = Bodies.ToArray() For Local i:Int = 0 To BodiesArray.Length -1 Local b:TBody = TBody(BodiesArray[i]) If (b.invMass = 0.0) Continue b.velocity.AddV(MultFloatVec2(dt,(AddVec2Vec2(gravity, MultFloatVec2(b.invMass, b.force))))) b.angularVelocity :+ dt * b.invI * b.torque Next For Local arb:TArbiter = EachIn Arbiters arb.PreStep(inv_dt) Next Local JointsArray:Object [] = Bodies.ToArray() For Local i:Int = 0 To JointsArray.Length - 1 TJoint(JointsArray[i]).PreStep(inv_dt) Next For Local i:Int = 0 To iterations For Local arb:TArbiter = EachIn Arbiters arb.ApplyImpulse() Next For Local j:Int = 0 To JointsArray.Length - 1 TJoint(JointsArray[j]).ApplyImpulse() Next Next For Local i:Int = 0 To BodiesArray.Length - 1 Local b:TBody = TBody(BodiesArray[i]) b.position.AddV(MultFLoatVec2(dt,b.velocity)) b.rotation :+ dt * b.angularVelocity b.force.Set(0.0, 0.0) b.torque = 0.0 Next End Method End Type
mathutils.bmx
Rem * Copyright (c) 2006 Erin Catto <a href="http://www.gphysics.com" target="_blank">www.gphysics.com</a> * * 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 FLT_MAX:Float = 10^37 Function Vec2:TVec2 (_x:Float, _y:Float) 'Constructor Local v:TVec2 = New TVec2 v.X = _x v.Y = _y Return v End Function Type TVec2 Field x:Float, y:Float Method Set(_x:Float, _y:Float) X = _x Y = _y End Method Method Reverse:TVec2() '-() Return Vec2(-X, -Y) End Method Method AddV(v:TVec2) '+= X :+ v.X y :+ v.Y End Method Method SubV(v:TVec2) '-= X :- v.X y :- v.Y End Method Method Mult(a:Float) '*= x :* a y :* a End Method Method Neg( ) x =-x y =-y End Method Method MultEx:TVec2( a:Float ) Local nVec2:TVec2 = New TVec2 nVec2.x = x*a nVec2.y = y*a Return nVec2 End Method Method NegEx:TVec2( ) Local nVec2:TVec2 = New TVec2 nVec2.x =-x nVec2.y =-y Return nVec2 End Method Method Length:Float () Return Sqr(X * X + Y * Y) End Method End Type Function Mat22:TMat22(_col1:TVec2,_col2:TVec2) 'Constructor Local m:TMat22 = New TMat22 m.col1 = _col1 m.col2 = _col2 Return m End Function Function Mat22_Angle:TMat22(angle:Float) 'Constructor from angle Local m:TMat22 = New TMat22 Local c:Float = Float(Cos(angle)), s:Float = Float(Sin(angle)) m.col1.X = c m.col2.X = -s m.col1.Y = s m.col2.Y = c Return m End Function Type TMat22 Field col1:TVec2 Field col2:TVec2 Method New() col1 = New TVec2 col2 = New TVec2 End Method Method Transpose:TMat22() Return Mat22(Vec2(col1.X, col2.X), Vec2(col1.Y, col2.Y)) End Method Method Invert:TMat22() Local a:Float = col1.x, b:Float = col2.x, c:Float = col1.y, d:Float = col2.y Local E:TMat22 Local det:Float = a * d - b * c Assert (det <> 0.0),"Error: Type Mat22>Invert" det = 1.0 / det E.col1.X = det * d E.col2.x = -det * b E.col1.Y = -det * c E.col2.y = det * a Return E End Method End Type Function Dot:Float(a:TVec2, b:TVec2) Return a.X * b.X + a.X * b.X End Function Function CrossVec2Vec2:Float(a:TVec2, b:TVec2) Return a.X * b.Y - a.Y * b.X End Function Function CrossVec2Float:TVec2(a:TVec2, s:Float) Return Vec2(s * a.Y, -s * a.X) End Function Function CrossFloatVec2:TVec2(s:Float, a:TVec2) Return Vec2(-s * a.Y, s * a.X) End Function Function MultMat22Vec2:TVec2 (A:TMat22, v:TVec2) Return Vec2(A.col1.x * v.x + A.col2.x * v.y, A.col1.y * v.x + A.col2.y * v.y) End Function Function AddVec2Vec2:TVec2 (a:TVec2, b:TVec2) Return Vec2(a.x + b.x, a.y + b.y) End Function Function SubVec2Vec2:TVec2 (a:TVec2, b:TVec2) Return Vec2(a.x - b.x, a.y - b.y) End Function Function MultFloatVec2:TVec2 (s:Float, v:TVec2) Return Vec2(s * v.x, s * v.y) End Function Function AddMat22Mat22:TMat22 (A:TMat22, B:TMat22) Return Mat22(AddVec2Vec2(A.col1, B.col1), AddVec2Vec2(A.col2, B.col2)) End Function Function MultMat22Mat22:TMat22 (A:TMat22, B:TMat22) Return Mat22(MultMat22Vec2(A, B.col1), MultMat22Vec2(A, B.col2)) End Function Function AbsoluteVec2:TVec2(a:TVec2) Return Vec2(Abs(a.x), Abs(a.y)) End Function Function AbsoluteMat22:TMat22 (A:TMat22) Return Mat22(AbsoluteVec2(A.col1), AbsoluteVec2(A.col2)) End Function Function Sign:Float(x:Float) Return Sgn(x) End Function Function MinFloat:Float(a:Float, b:Float) If a < b Return a Else Return b End Function Function MaxFloat:Float(a:Float, b:Float) If a > b Return a Else Return b End Function Function ClampFloat:Float(a:Float, low:Float, high:Float) Return MaxFloat(low, MinFloat(a, high)) End Function Function Swap(a:Object, b:Object) Local tmp:Object = a a = b b = tmp End Function ' Random number in range [-1,1] Function Random:Float() Return Float(Rnd(-1,1)) End Function Function Random2:Float(lo:Float, hi:Float) Return Float(Rnd(lo,hi)) End Function
main.bmx
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 SuperStrict Include "Source\MathUtils.bmx" Include "Source\Arbiter.bmx" Include "Source\Body.bmx" Include "Source\Collide.bmx" Include "Source\Joint.bmx" Include "Source\World.bmx" Graphics 800,600 Global DeltaTime:Float = 1.0 / 60.0 Global _World:TWorld = World(Vec2(0.0, -10.0), 10) Global Bomb:TBody '* Demo 1 "A single box" Local Ground:TBody = Body() Ground.Set(Vec2(100.0, 20.0), FLT_MAX) Ground.Position.Set(0.0, -0.5 * Ground.Width.Y) _World.Add(Ground) Local Box:TBody = Body() Box.Set(Vec2(1.0, 1.0), 100.0) Box.position.Set(0.0, 4.0) _World.Add(Box) While Not KeyHit(KEY_ESCAPE) If KeyHit(KEY_SPACE) LaunchBomb End If Cls _World._Step(DeltaTime) For Local b:TBody = EachIn _World.bodies DrawBody(b) Next For Local j:TJoint = EachIn _World.joints DrawJoint(j) Next DrawText("Demo 1",0,0) DrawText("Press Space to Launch the Bomb",0,15) Flip Wend Function DrawBody (body:Tbody) Local R:TMat22 = Mat22_Angle(body.rotation) Local x:TVec2 = body.Position Local h:TVec2 = MultFloatVec2(0.5, body.width) Local v1:TVec2 = AddVec2Vec2(x, MultMat22Vec2(R, Vec2(-h.x, -h.y))) Local v2:TVec2 = AddVec2Vec2(x, MultMat22Vec2(R, Vec2(h.x, -h.y))) Local v3:TVec2 = AddVec2Vec2(x, MultMat22Vec2(R, Vec2(h.x, h.y))) Local v4:TVec2 = AddVec2Vec2(x, MultMat22Vec2(R, Vec2(-h.x, h.y))) If (body = Bomb) SetColor 128,255,128 Else SetColor 200,200,255 End If DrawLine v1.x, v1.y, v2.x, v2.y DrawLine v2.x, v2.y, v3.x, v3.y DrawLine v3.x, v3.y, v4.x, v4.y DrawLine v4.x, v4.y, v1.x, v1.y End Function Function DrawJoint (joint:TJoint) Local b1:TBody = joint.body1 Local b2:TBody = joint.body2 Local R1:TMat22 Mat22_Angle(b1.rotation) Local R2:TMat22 Mat22_Angle(b2.rotation) Local x1:TVec2 = b1.position Local p1:TVec2 = AddVec2Vec2(x1, MultMat22Vec2(R1, joint.localAnchor1)) Local x2:TVec2 = b2.position Local p2:TVec2 = AddVec2Vec2(x2, MultMat22Vec2(R2, joint.localAnchor2)) SetColor 128,128,200 DrawLine x1.x, x1.y, p1.x, p1.y DrawLine p1.x, p1.y, x2.x, x2.y DrawLine x2.x, x2.y, p2.x, p2.y DrawLine p2.x, p2.y, x1.x, x1.y End Function Function LaunchBomb () If (Not bomb) Bomb = Body() Bomb.Set(Vec2(1.0, 1.0), 50.0) Bomb.Friction = 0.2 _World.Add(Bomb) End If Bomb.position.Set(Random2(-15.0, 15.0), 15.0) Bomb.rotation = Random2(-1.5, 1.5) Bomb.velocity = MultFloatVec2(-1.5, Bomb.position) Bomb.angularVelocity = Random2(-20.0, 20.0) End Function
collide.bmx
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 '/** ' * ' */ Type TAxis Const FACE_A_X:Int = 1 Const FACE_A_Y:Int = 2 Const FACE_B_X:Int = 3 Const FACE_B_Y:Int = 4 End Type '/** ' * ' */ 'Type EdgeNumbers Const NO_EDGE:Int = 0 Const EDGE1:Int = 1 Const EDGE2:Int = 2 Const EDGE3:Int = 3 Const EDGE4:Int = 4 'End Type '/** ' * ' */ 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 ) '// Start with no output points Local numOut:Int = 0 '// Calculate the distance of end points to the line Local distance0:Float = Dot( normal, vIn[0].v ) - Offset Local distance1:Float = Dot( normal, vIn[1].v ) - Offset '// If the points are behind the plane If ( distance0 <= 0 ) vOut[numOut] = vIn[0]; If ( distance1 <= 0 ) vOut[numOut+1] = vIn[1]; numOut :+2 '// If the points are on different sides of the plane If ( distance0 * distance1 < 0 ) '// Find intersection point of edge and plane Local interp:Float = distance0 / (distance0 - distance1) vOut[numOut].v = subVec2Vec2( vIn[1].v, vIn[0].v ).MultEx( interp ) vOut[numOut].v = addVec2Vec2( vIn[0].v, vOut[numOut].v ) If ( distance0 > 0 ) vOut[numOut].fp = vIn[0].fp; vOut[numOut].fp.e[inEdge1] = clipEdge vOut[numOut].fp.e[inEdge2] = NO_EDGE Else vOut[numOut].fp = vIn[1].fp vOut[numOut].fp.e[outEdge1] = clipEdge vOut[numOut].fp.e[outEdge2] = NO_EDGE EndIf numOut :+1 EndIf Return numOut End Function '/** ' * ' */ Function ComputeIncidentEdge( c:TClipVertex[], h:TVec2, Pos:TVec2, Rot:TMat22, normal:TVec2 ) '// The normal is from the reference box. Convert it '// to the incident boxe's frame and flip sign. Local RotT:TMat22 = Rot.Transpose( ) Local n:TVec2 = MultMat22Vec2( RotT, normal ) n.Neg( ) Local nAbs:TVec2 = AbsoluteVec2( n ) If ( nAbs.x > nAbs.y ) If ( Sign( n.x ) > 0 ) c[0].v.Set( h.x, -h.y ) c[0].fp.e[inEdge2] = EDGE3 c[0].fp.e[outEdge2] = EDGE4 c[1].v.Set( h.x, h.y ) c[1].fp.e[inEdge2] = EDGE4 c[1].fp.e[outEdge2] = EDGE1 Else c[0].v.Set( -h.x, h.y ) c[0].fp.e[inEdge2] = EDGE1 c[0].fp.e[outEdge2] = EDGE2 c[1].v.Set( -h.x, -h.y ) c[1].fp.e[inEdge2] = EDGE2 c[1].fp.e[outEdge2] = EDGE3 EndIf Else If ( Sign(n.y) > 0 ) c[0].v.Set( h.x, h.y ) c[0].fp.e[inEdge2] = EDGE4 c[0].fp.e[outEdge2] = EDGE1 c[1].v.Set( -h.x, h.y ) c[1].fp.e[inEdge2] = EDGE1 c[1].fp.e[outEdge2] = EDGE2 Else c[0].v.Set( -h.x, -h.y ) c[0].fp.e[inEdge2] = EDGE2 c[0].fp.e[outEdge2] = EDGE3 c[1].v.Set( h.x, -h.y ) c[1].fp.e[inEdge2] = EDGE3 c[1].fp.e[outEdge2] = EDGE4 EndIf EndIf ' c[0].v = Pos + Rot * c[0].v ' c[1].v = Pos + Rot * c[1].v End Function '/** ' * ' */ Function Collide:Int( contacts:TContact, bodyA:TBody, bodyB:TBody ) '// Setup Local hA:TVec2 = bodyA.width.MultEx( 0.5 ) Local hB:TVec2 = bodyB.width.MultEx( 0.5 ) Local posA:TVec2 = bodyA.position Local posB:TVec2 = bodyB.position Local RotA:TMat22 = MAT22_Angle( bodyA.rotation ) Local RotB:TMat22 = MAT22_Angle( bodyB.rotation ) Local RotAT:TMat22 = RotA.Transpose( ) Local RotBT:TMat22 = RotB.Transpose( ) Local a1:TVec2 = RotA.col1 Local a2:TVec2 = RotA.col2 Local b1:TVec2 = RotB.col1 Local b2:TVec2 = RotB.col2 Local dp:TVec2 = SubVec2Vec2( posB, posA ) Local dA:TVec2 = MultMat22Vec2( RotAT, dp ) Local dB:TVec2 = MultMat22Vec2( RotBT, dp ) Local C:TMat22 = MultMat22Mat22( RotAT, RotB ) Local absC:TMat22 = AbsoluteMat22( C ) Local absCT:TMat22 = absC.Transpose() '// Box A faces Local faceA:TVec2 = AbsoluteVec2( dA )' - hA - absC * hB If (faceA.x > 0 Or faceA.y > 0) Return 0 EndIf '// Box B faces Local faceB:TVec2 = AbsoluteVec2( dB )' - absCT * hA - hB If (faceB.x > 0 Or faceB.y > 0) Return 0 EndIf '// Find best axis Local axis:Int Local separation:Float Local normal:TVec2 '// Box A faces axis = TAxis.FACE_A_X separation = faceA.x If ( dA.x > 0 ) normal = RotA.col1 Else normal = RotA.col1.NegEx( ) EndIf If (faceA.y > (1.05 * separation + 0.01 * hA.y)) axis = TAxis.FACE_A_Y separation = faceA.y If (dA.y > 0) normal = RotA.col2 Else normal = RotA.col2.NegEx( ) EndIf EndIf '// Box B faces If (faceB.x > (1.05 * separation + 0.01 * hB.x)) axis = TAxis.FACE_B_X separation = faceB.x If (dB.x > 0) normal = RotB.col1 Else normal = RotB.col1.NegEx( ) EndIf EndIf If (faceB.y > 1.05 * separation + 0.01 * hB.y) axis = TAxis.FACE_B_Y separation = faceB.y; If (dB.y > 0.0) normal = RotB.col2 Else normal = RotB.col2.NegEx( ) EndIf EndIf Rem '// Setup clipping plane data based on the separating axis Local frontNormal:TVec2 Local sideNormal:TVec2 Local incidentEdge:TClipVertex[2] Local front:Float, negSide:Float, posSide:Float Local negEdge:Byte, posEdge:Byte '// Compute the clipping lines And the line segment To be clipped Select (axis) Case TAxis.FACE_A_X frontNormal = normal front = Dot(posA, frontNormal) + hA.x sideNormal = RotA.col2 Float side = Dot(posA, sideNormal) negSide = -side + hA.y posSide = side + hA.y negEdge = EDGE3 posEdge = EDGE1 ComputeIncidentEdge(incidentEdge, hB, posB, RotB, frontNormal) Case TAxis.FACE_A_Y frontNormal = normal front = Dot(posA, frontNormal) + hA.y sideNormal = RotA.col1 Float side = Dot(posA, sideNormal) negSide = -side + hA.x posSide = side + hA.x negEdge = EDGE2 posEdge = EDGE4 ComputeIncidentEdge(incidentEdge, hB, posB, RotB, frontNormal) Case TAxis.FACE_B_X frontNormal = -normal front = Dot(posB, frontNormal) + hB.x sideNormal = RotB.col2 Float side = Dot(posB, sideNormal) negSide = -side + hB.y posSide = side + hB.y negEdge = EDGE3 posEdge = EDGE1 ComputeIncidentEdge( incidentEdge, hA, posA, RotA, frontNormal ) Case TAxis.FACE_B_Y frontNormal = -normal front = Dot(posB, frontNormal) + hB.y sideNormal = RotB.col1 Float side = Dot(posB, sideNormal) negSide = -side + hB.x posSide = side + hB.x negEdge = EDGE2 posEdge = EDGE4 ComputeIncidentEdge( incidentEdge, hA, posA, RotA, frontNormal ) End Select '// clip other face with 5 box planes (1 face plane, 4 edge planes) Local clipPoints1:TClipVertex[2] Local clipPoints2:TClipVertex[2] Local np:Int '// Clip To box side 1 np = ClipSegmentToLine( clipPoints1, incidentEdge,-sideNormal, negSide, negEdge ) If (np < 2) Return 0 EndIf '// Clip To negative box side 1 np = ClipSegmentToLine( clipPoints2, clipPoints1, sideNormal, posSide, posEdge ) If (np < 2) Return 0 EndIf '// Now clipPoints2 contains the clipping points. '// Due To roundoff, it is possible that clipping removes all points. Local numContacts:Int = 0, i:Int For i = 0 To 1 Local separation:Float = Dot( frontNormal, clipPoints2[i].v ) - front If (separation <= 0) contacts[numContacts].separation = separation contacts[numContacts].normal = normal '// slide contact point onto reference face (easy To cull) contacts[numContacts].position = clipPoints2[i].v - separation * frontNormal contacts[numContacts].feature = clipPoints2[i].fp If ((axis = FACE_B_X) Or (axis = FACE_B_Y)) _Flip(contacts[numContacts].feature) EndIf numContacts:+1 EndIf Next Return numContacts End Rem End Function
Oh yeah, and merry christmas :)