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
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 :)
