Code: Select all
Structure vec2
x.d
y.d
EndStructure
Macro VEC2_length(vectorA)
Sqr( vectorA\x*vectorA\x + vectorA\y*vectorA\y )
EndMacro
Macro VEC2_normalize(vectorA,vectorResult)
VEC2_tempLength = VEC2_length(vectorA)
If VEC2_tempLength
vectorResult\x = vectorA\x / VEC2_tempLength
vectorResult\y = vectorA\y / VEC2_tempLength
Else
vectorResult\x =0
vectorResult\y=0
EndIf
EndMacro
Macro VEC2_dotProduct(vectorA,vectorB)
(vectorA\x * vectorB\x + vectorA\y * vectorB\y)
EndMacro
Macro VEC2_substract(vectorA,vectorB,vectorResult)
vectorResult\x = vectorA\x - vectorB\x
vectorResult\y = vectorA\y - vectorB\y
EndMacro
Macro VEC2_add(vectorA,vectorB,vectorResult)
vectorResult\x = vectorA\x + vectorB\x
vectorResult\y = vectorA\y + vectorB\y
EndMacro
Macro VEC2_multiply(vectorA,magnitude,vectorResult)
vectorResult\x = vectorA\x * magnitude
vectorResult\y = vectorA\y * magnitude
EndMacro
Macro VEC2_crossVector(vectorA,vectorResult)
;returns a vector perpendicular to v
vectorResult\x = vectorA\y
vectorResult\y = -vectorA\x
EndMacro
Structure pointmass
x.d
y.d
vx.d
vy.d
mass.d
invmass.d
bstatic.i
EndStructure
Structure constraint
*p1.pointmass
*p2.pointmass
dist.d
norm.vec2
enabled.i
EndStructure
Structure collisionInfo
depth.d
normal.vec2
collisionVector.vec2
*ptrEdge.constraint
*ptrPoint.pointmass
collisionCase.i
EndStructure
Structure AABB
left.i
top.i
right.i
bottom.i
EndStructure
Structure poly
points.pointmass[32]
constraints.constraint[64]
aabb.aabb
EndStructure
Structure circle
radius.d
EndStructure
Structure body
type.i
npoints.i
nconstraints.i
cords.poly
mass.d
radius.d
color.i
bstatic.i
friction.d
frictionEdgeOnPoint.d
bcollide.i
sprite.i
flagdelete.i
EndStructure
Structure MousePick
x.i
y.i
*mousepoint.pointmass
*mousebody.body
mode.i
sprite.i
EndStructure
Structure World
box.aabb
Gravity.d
Friction.d
Drag.d
sf.vec2
BackgroundSprite.i
ClearColor.i
ang.d
EndStructure
#Circle = 1
#Polygon = 2
#Line = 3
#FRICTION_EDGEONPOINT_FACTOR = 1.0
Global NewList bodies.body()
Global world.World
Global gmouse.MousePick
Prototype CollisionCallback(*body.body,*body1.body)
Prototype CollisionPresolve(*body.body,*body1.body)
Declare Collisions(*cbCollisionCallback.CollisionCallback)
;XIncludeFile "maze.pbi"
UseJPEGImageDecoder()
UsePNGImageDecoder()
Procedure CreateMouseSprite()
gmouse\sprite =CreateSprite(#PB_Any,9,9)
StartDrawing(SpriteOutput( gmouse\sprite))
Box(0,0,9,9,0);RGB(128,0,0))
LineXY(4,0,4,9,RGB(0,255,0))
LineXY(0,4,9,4,RGB(0,255,0))
StopDrawing()
TransparentSpriteColor(gmouse\sprite,0);RGB(128,0,0))
EndProcedure
Procedure Displaymessage(x,y,msg.s)
Protected sp,timg,fw,fh
Static fontnum
If Not fontnum
fontnum = LoadFont(#PB_Any,"Arial",14)
EndIf
timg = CreateImage(#PB_Any,1,1)
StartDrawing(ImageOutput(timg))
DrawingFont(FontID(fontnum))
fw = TextWidth(msg)
fh = TextHeight(msg)
StopDrawing()
sp = CreateSprite(#PB_Any,fw,fh)
StartDrawing(SpriteOutput(sp))
DrawText(0,0,msg,RGB(0,255,0))
StopDrawing()
TransparentSpriteColor(sp,0)
DisplayTransparentSprite(sp,x,y)
FreeSprite(sp)
FreeImage(timg)
EndProcedure
Procedure InitWorld(left,right,top,bottom,Drag.d,Gravity.d,Friction.d,clearcolor.i=0,backgroundSprite.i=0)
world\box\left = left
world\box\right = right
world\box\top = top
world\box\bottom = bottom
world\Friction = 1.0 - Friction
world\Gravity = Gravity / (5*60)
world\Drag = drag
world\sf\x = (-1 - 1) / (left - right) * 0.5
world\sf\y = (-1 - 1) / (top-bottom) * 0.5
world\clearcolor = clearcolor
world\backgroundSprite = backgroundSprite
InitSprite()
InitKeyboard()
InitMouse()
OpenWindow(0,0,0,world\box\right,world\box\bottom,"test");,#PB_Window_BorderLess)
OpenWindowedScreen(WindowID(0),0,0,world\box\right,world\box\bottom,0,0,0)
CreateMouseSprite()
EndProcedure
Procedure GenConstraints(*body.body)
Protected a,b,c,np,segv.vec2
np = *body\npoints -1
For a = 1 To np
b = a+1
If b > np
b=1
EndIf
*body\cords\constraints[a]\p1 = @*body\cords\points[a]
*body\cords\constraints[a]\p2 = @*body\cords\points[b]
*body\cords\constraints[a]\dist = Sqr( Pow((*body\cords\constraints[a]\p1\x - *body\cords\constraints[a]\p2\x),2) + Pow((*body\cords\constraints[a]\p1\y - *body\cords\constraints[a]\p2\y),2))
*body\cords\constraints[a]\enabled = 1
VEC2_substract(*body\cords\constraints[a]\p2,*body\cords\constraints[a]\p1,segv)
VEC2_normalize(segv,segv)
VEC2_crossVector(segv,*body\cords\constraints[a]\norm)
c= a + np
*body\cords\constraints[c]\p1 = @*body\cords\points[a]
*body\cords\constraints[c]\p2 = @*body\cords\points[0]
*body\cords\constraints[c]\dist = Sqr( Pow((*body\cords\constraints[c]\p1\x - *body\cords\constraints[c]\p2\x),2) + Pow((*body\cords\constraints[c]\p1\y - *body\cords\constraints[c]\p2\y),2))
*body\cords\constraints[c]\enabled = 0
VEC2_substract(*body\cords\constraints[c]\p2,*body\cords\constraints[c]\p1,segv)
VEC2_normalize(segv,segv)
VEC2_crossVector(segv,*body\cords\constraints[c]\norm)
Next
EndProcedure
Procedure NewLine(x1,y1,x2,y2,color,bstatic=0,bcollide=0)
Protected dx.d,dy.d
AddElement(bodies())
bodies()\type = #Line
bodies()\color = color
bodies()\npoints = 3
bodies()\nconstraints = 4
bodies()\mass = 3
bodies()\friction = 1.0
bodies()\frictionEdgeOnPoint = 1.5
bodies()\bstatic = bstatic
bodies()\bcollide = bcollide
dx = (x2-x1) / 2
dy = (y2-y1) / 2
b=1
c=2
bodies()\cords\points[0]\mass = 1
bodies()\cords\points[0]\x = x1 + dx
bodies()\cords\points[0]\y = y1 + dy
bodies()\cords\points[0]\vx = bodies()\cords\points[0]\x
bodies()\cords\points[0]\vy = bodies()\cords\points[0]\y
bodies()\cords\points[1]\mass = 1
bodies()\cords\points[1]\x = x1
bodies()\cords\points[1]\y = y1
bodies()\cords\points[1]\vx = bodies()\cords\points[a]\x
bodies()\cords\points[1]\vy = bodies()\cords\points[a]\y
bodies()\cords\points[2]\mass = 1
bodies()\cords\points[2]\x = x2
bodies()\cords\points[2]\y = y2
bodies()\cords\points[2]\vx = bodies()\cords\points[a]\x
bodies()\cords\points[2]\vy = bodies()\cords\points[a]\y
bodies()\cords\constraints[1]\p1 = bodies()\cords\points[1]
bodies()\cords\constraints[1]\p2 = bodies()\cords\points[2]
bodies()\cords\constraints[1]\dist = Sqr( Pow(( bodies()\cords\constraints[1]\p1\x - bodies()\cords\constraints[1]\p2\x),2) + Pow(( bodies()\cords\constraints[1]\p1\y - bodies()\cords\constraints[1]\p2\y),2))
bodies()\cords\constraints[1]\enabled = 1
bodies()\cords\constraints[2]\p1 = bodies()\cords\points[2]
bodies()\cords\constraints[2]\p2 = bodies()\cords\points[1]
bodies()\cords\constraints[2]\dist = Sqr( Pow(( bodies()\cords\constraints[2]\p1\x - bodies()\cords\constraints[2]\p2\x),2) + Pow(( bodies()\cords\constraints[2]\p1\y - bodies()\cords\constraints[2]\p2\y),2))
bodies()\cords\constraints[2]\enabled = 1
bodies()\cords\constraints[3]\p1 = bodies()\cords\points[2]
bodies()\cords\constraints[3]\p2 = bodies()\cords\points[0]
bodies()\cords\constraints[3]\dist = Sqr( Pow(( bodies()\cords\constraints[3]\p1\x - bodies()\cords\constraints[3]\p2\x),2) + Pow(( bodies()\cords\constraints[3]\p1\y - bodies()\cords\constraints[3]\p2\y),2))
bodies()\cords\constraints[3]\enabled = 0
bodies()\cords\constraints[4]\p1 = bodies()\cords\points[0]
bodies()\cords\constraints[4]\p2 = bodies()\cords\points[1]
bodies()\cords\constraints[4]\dist = Sqr( Pow(( bodies()\cords\constraints[4]\p1\x - bodies()\cords\constraints[4]\p2\x),2) + Pow(( bodies()\cords\constraints[4]\p1\y - bodies()\cords\constraints[4]\p2\y),2))
bodies()\cords\constraints[4]\enabled = 0
bodies()\cords\points[1]\bstatic = 1
bodies()\cords\points[2]\bstatic = 1
ProcedureReturn @bodies()
EndProcedure
Procedure NewPolygon(x.d,y.d,sides,radius.d,color,bstatic=0,bcollide=0)
Protected theta.d,i.i,st.d
st = #PI / sides
AddElement(bodies())
bodies()\type = #polygon
bodies()\color = color
bodies()\npoints = sides+1
bodies()\nconstraints = sides * 2
bodies()\mass = sides+1
bodies()\friction = 1.0
bodies()\frictionEdgeOnPoint = 1.5
bodies()\bstatic = bstatic
bodies()\bcollide = bcollide
For i = 1 To sides
bodies()\cords\points[i]\x = x + Cos(theta) * radius
bodies()\cords\points[i]\y = y + Sin(theta) * radius
bodies()\cords\points[i]\vx = bodies()\cords\points[i]\x
bodies()\cords\points[i]\vy = bodies()\cords\points[i]\y
bodies()\cords\points[i]\mass = 1
theta + (2.0 * st)
Next i
bodies()\cords\points[0]\x = x
bodies()\cords\points[0]\y = y
bodies()\cords\points[0]\vx = bodies()\cords\points[0]\x
bodies()\cords\points[0]\vy = bodies()\cords\points[0]\y
bodies()\cords\points[i]\mass = sides
GenConstraints(@bodies())
ProcedureReturn @bodies()
EndProcedure
Procedure NewCircle(x,y,radius,color,bstatic=0,bcollide=0,sprite=0)
AddElement(bodies())
bodies()\type = #Circle
bodies()\color = color
bodies()\npoints = 1
bodies()\cords\points[0]\x = x
bodies()\cords\points[0]\y = y
bodies()\cords\points[0]\vx = x
bodies()\cords\points[0]\vy = y
bodies()\radius = radius
bodies()\sprite = sprite
bodies()\bcollide = bcollide
If bstatic
bodies()\bstatic = 1
bodies()\cords\points[0]\bstatic = 1
EndIf
bodies()\cords\points[0]\mass = #PI * radius * radius
ProcedureReturn @bodies()
EndProcedure
Procedure DrawBodies()
StartDrawing(ScreenOutput())
;DrawingMode(#PB_2DDrawing_Outlined)
ForEach bodies()
Select bodies()\type
Case #Polygon
*st.vec2 = @bodies()\cords\points[1]
For a = 1 To bodies()\npoints - 2
LineXY(bodies()\cords\points[a]\x,bodies()\cords\points[a]\y,bodies()\cords\points[a+1]\x,bodies()\cords\points[a+1]\y,bodies()\color)
Next
LineXY(bodies()\cords\points[a]\x,bodies()\cords\points[a]\y,*st\x,*st\y,bodies()\color)
Case #Circle
If Not bodies()\sprite
Circle(bodies()\cords\points[0]\x,bodies()\cords\points[0]\y,bodies()\radius,bodies()\color)
EndIf
Case #LIne
For a = 1 To bodies()\npoints - 2
LineXY(bodies()\cords\points[a]\x,bodies()\cords\points[a]\y,bodies()\cords\points[a+1]\x,bodies()\cords\points[a+1]\y,bodies()\color)
Next
EndSelect
Next
StopDrawing()
EndProcedure
Procedure UpDateBodies()
Protected dx.d,dy.d,w,h,cx.d,cy.d
Protected MinDistance.d,Distance.d,mdx.d,mdy.d
ExamineMouse()
gmouse\x = MouseX()
gmouse\y = MouseY()
If MouseButton(#PB_MouseButton_Left)
gmouse\mode = #True
Else
gmouse\mode = #False
gmouse\mousepoint = 0
*lbody = 0
EndIf
MinDistance=999999
gmouse\x = MouseX()
gmouse\y = MouseY()
ForEach bodies()
For a = 0 To bodies()\npoints - 1
If gmouse\mousepoint = 0 And gmouse\mode = #True
mdx = gmouse\x - bodies()\cords\points[a]\x
mdy = gmouse\Y - bodies()\cords\points[a]\y
Distance = Sqr(mdx*mdx+mdy*mdy)
If Distance < MinDistance
MinDistance = Distance
If MinDistance < 20
gmouse\mousepoint = @bodies()\cords\points[a]
EndIf
EndIf
EndIf
If Not bodies()\cords\points[a]\bstatic
dx = (bodies()\cords\points[a]\x - bodies()\cords\points[a]\vx) * world\drag
dy = ((bodies()\cords\points[a]\y - bodies()\cords\points[a]\vy) * world\drag) + world\Gravity
bodies()\cords\points[a]\vx = bodies()\cords\points[a]\x
bodies()\cords\points[a]\vy = bodies()\cords\points[a]\y
bodies()\cords\points[a]\x + dx
bodies()\cords\points[a]\y + dy
If bodies()\cords\points[a]\x - bodies()\radius <= world\box\left
bodies()\cords\points[a]\vx = world\box\left + bodies()\radius
bodies()\cords\points[a]\x = world\box\left + bodies()\radius
bodies()\cords\points[a]\x - dx
bodies()\cords\points[a]\y * world\Friction
ElseIf bodies()\cords\points[a]\x + bodies()\radius > world\box\right
bodies()\cords\points[a]\vx = world\box\right - bodies()\radius
bodies()\cords\points[a]\x = world\box\right - bodies()\radius
bodies()\cords\points[a]\x - dx
bodies()\cords\points[a]\y * world\Friction
EndIf
If bodies()\cords\points[a]\y - bodies()\radius <= world\box\top
bodies()\cords\points[a]\vy = world\box\top + bodies()\radius
bodies()\cords\points[a]\y = world\box\top + bodies()\radius
bodies()\cords\points[a]\x * world\Friction
bodies()\cords\points[a]\y - dy
ElseIf bodies()\cords\points[a]\y + bodies()\radius >= world\box\bottom
bodies()\cords\points[a]\vy = world\box\bottom - bodies()\radius
bodies()\cords\points[a]\y = world\box\bottom - bodies()\radius
bodies()\cords\points[a]\x * world\Friction
bodies()\cords\points[a]\y - dy
EndIf
EndIf
Next
Protected segv.vec2 ,sign.d
Select bodies()\type ;update constriants
Case #Polygon,#line
For c = 1 To bodies()\npoints -1
For b = 1 To bodies()\nconstraints
VEC2_substract(bodies()\cords\constraints[b]\p2,bodies()\cords\constraints[b]\p1,segv)
; dotp.d = VEC2_dotProduct(bodies()\cords\constraints[b]\norm,segv)
; If dotp < 0
; sign= 1
; Else
; sign = 1
; EndIf
delta.d = VEC2_length(segv)
If delta
diff.d = ((delta-bodies()\cords\constraints[b]\dist ) / (2 * delta)) ;* sign ;
EndIf
If Abs(diff ) > 0.0001
If Not bodies()\cords\constraints[b]\p1\bstatic
bodies()\cords\constraints[b]\p1\x + (diff * segv\x )
bodies()\cords\constraints[b]\p1\vx + (diff * segv\x )
bodies()\cords\constraints[b]\p1\y + (diff * segv\y )
bodies()\cords\constraints[b]\p1\vy + (diff * segv\y )
EndIf
If Not bodies()\cords\constraints[b]\p2\bstatic
bodies()\cords\constraints[b]\p2\x - (diff * segv\x )
bodies()\cords\constraints[b]\p2\vx - (diff * segv\x )
bodies()\cords\constraints[b]\p2\y - (diff * segv\y )
bodies()\cords\constraints[b]\p2\vy - (diff * segv\y )
EndIf
EndIf
Next
Next
bodies()\cords\aabb\left = 999999
bodies()\cords\aabb\right = 0
bodies()\cords\aabb\top = 999999
bodies()\cords\aabb\bottom = 0
For i = 1 To bodies()\npoints -1 ;calculate axis aligned bounding box and center
If bodies()\cords\points[i]\x < bodies()\cords\aabb\left
bodies()\cords\aabb\left = bodies()\cords\points[i]\x
EndIf
If bodies()\cords\points[i]\x > bodies()\cords\aabb\right
bodies()\cords\aabb\right = bodies()\cords\points[i]\x
EndIf
If bodies()\cords\points[i]\y < bodies()\cords\aabb\top
bodies()\cords\aabb\top = bodies()\cords\points[i]\y
EndIf
If bodies()\cords\points[i]\y > bodies()\cords\aabb\bottom
bodies()\cords\aabb\bottom = bodies()\cords\points[i]\y
EndIf
Next
Case #circle
bodies()\cords\aabb\left = bodies()\cords\points[0]\x - bodies()\radius
bodies()\cords\aabb\top = bodies()\cords\points[0]\y - bodies()\radius
bodies()\cords\aabb\right = bodies()\cords\points[0]\x + bodies()\radius
bodies()\cords\aabb\bottom = bodies()\cords\points[0]\y + bodies()\radius
EndSelect
Next
If gmouse\mousepoint
gmouse\mousepoint\x = gmouse\x
gmouse\mousepoint\y = gmouse\y
gmouse\mousepoint\vx = gmouse\mousepoint\x
gmouse\mousepoint\vy = gmouse\mousepoint\y
EndIf
EndProcedure
Procedure PolyGonToPolygon(*Body.body,*body1.body)
Protected *t.body,axis.vec2 ,minA.d,maxA.d,minB.d,maxB.d,distance.d
Protected collisionInfo.collisionInfo,mindistance.d, tempVector.vec2
Protected *bodyPoint.body,*BodyEdge.body, dotP .d,coledge.i
minDistance = 999999
*t = *body
For b = 1 To 2
For a = 0 To *t\nconstraints -1
If *t\cords\constraints[a]\enabled = #False
Continue
EndIf
axis\x = *t\cords\constraints[a]\p1\y - *t\cords\constraints[a]\p2\y
axis\y = *t\cords\constraints[a]\p2\x - *t\cords\constraints[a]\p1\x
VEC2_Normalize(axis,axis)
minA = 9999999
maxA = -9999999
For c = 1 To *body\npoints -1
dotP = VEC2_dotProduct(axis,*body\cords\points[c])
If dotP < minA
minA = dotP
EndIf
If dotP > maxA
maxA = dotP
EndIf
Next
minB = 9999999
maxB = -9999999
For c = 1 To *body1\npoints -1
dotP = VEC2_dotProduct(axis,*body1\cords\points[c])
If dotP < minB
minB = dotP
EndIf
If dotP > maxB
maxB = dotP
EndIf
Next
If minA < minB
Distance = maxA - minB
Else
Distance = maxB - minA
EndIf
If distance < 0.0
ProcedureReturn #False
EndIf
If distance < minDistance
minDistance = distance
collisionInfo\normal\x = axis\x
collisionInfo\normal\y = axis\y
collisionInfo\ptrEdge = @*t\cords\constraints[a]
cbody = *t
collisionResult = 1
EndIf
Next
*t = *body1
Next
If collisionResult = #True
If Not Bool(*body\bcollide & *body1\bcollide)
collisionInfo\depth = minDistance
If cbody = *body1
*bodyPoint = *Body
*bodyEdge = *Body1
Else
*bodyPoint = *Body1
*bodyEdge = *Body
EndIf
VEC2_substract(*bodyEdge\cords\points[0],*bodyPoint\cords\points[0], tempVector)
If VEC2_dotproduct(collisionInfo\normal,tempVector) < 0
collisionInfo\normal\x = -collisionInfo\normal\x
collisionInfo\normal\y = -collisionInfo\normal\y
EndIf
VEC2_substract(*BodyEdge\cords\points[0], *bodyPoint\cords\points[1],tempVector)
distance.d = VEC2_dotProduct(collisionInfo\normal,tempVector)
minDistance.d = distance
For a = 1 To *bodyPoint\nPoints
VEC2_substract(*bodyEdge\cords\points[0],*bodyPoint\cords\points[a],tempVector)
distance = VEC2_dotProduct(collisionInfo\normal,tempVector)
If distance <= minDistance
minDistance = Distance
collisionInfo\ptrPoint = @*bodyPoint\cords\points[a]
EndIf
Next
VEC2_multiply(collisionInfo\normal,collisionInfo\depth,collisionInfo\collisionVector)
temp .d
If( Abs( collisionInfo\ptrEdge\p1\x - collisionInfo\ptrEdge\p2\x ) > Abs( collisionInfo\ptrEdge\p1\y - collisionInfo\ptrEdge\p2\y ) )
Temp = ( collisionInfo\ptrPoint\x - collisionInfo\collisionVector\x - collisionInfo\ptrEdge\p1\x )/( collisionInfo\ptrEdge\p2\x - collisionInfo\ptrEdge\p1\x)
Else
Temp = (collisionInfo\ptrPoint\y - collisionInfo\collisionVector\y - collisionInfo\ptrEdge\p1\y )/( collisionInfo\ptrEdge\p2\y - collisionInfo\ptrEdge\p1\y)
EndIf
lambda.d = 1.0 / ( Temp*Temp + ( 1 - Temp)* ( 1 -Temp))
If collisionInfo\normal\x < 0
collisionInfo\normal\x = -collisionInfo\normal\x
EndIf
If collisionInfo\normal\y < 0
collisionInfo\normal\y = -collisionInfo\normal\y
EndIf
edgeMass.d = Temp * collisionInfo\ptrEdge\p2\Mass + ( 1 - Temp ) * collisionInfo\ptrEdge\p1\Mass
invCollisionMass.d = 1/(edgeMass + collisionInfo\ptrPoint\mass )
ratio1.d = collisionInfo\ptrPoint\Mass * invCollisionMass
ratio3.d = edgeMass * invCollisionMass
ratio2.d = ratio1 * lambda
If Not collisionInfo\ptrEdge\p1\bstatic
collisionInfo\ptrEdge\p1\x + collisionInfo\collisionVector\x * ( 1 - Temp ) * ratio2
collisionInfo\ptrEdge\p1\y + collisionInfo\collisionVector\y * ( 1 - Temp ) * ratio2
EndIf
If Not collisionInfo\ptrEdge\p2\bstatic
collisionInfo\ptrEdge\p2\x + collisionInfo\collisionVector\x * Temp * ratio2
collisionInfo\ptrEdge\p2\y + collisionInfo\collisionVector\y * Temp * ratio2
EndIf
If Not collisionInfo\ptrPoint\bstatic
collisionInfo\ptrPoint\x - collisionInfo\collisionVector\x * ratio3
collisionInfo\ptrPoint\y - collisionInfo\collisionVector\y * ratio3
EndIf
If Not collisionInfo\ptrEdge\p1\bstatic
collisionInfo\ptrEdge\p1\vx + (collisionInfo\ptrEdge\p1\x - collisionInfo\ptrEdge\p1\vx) * collisionInfo\normal\y * (*BodyPoint\frictionEdgeOnPoint * *BodyEdge\frictionEdgeOnPoint)
collisionInfo\ptrEdge\p1\vy + (collisionInfo\ptrEdge\p1\y - collisionInfo\ptrEdge\p1\vy) * collisionInfo\normal\x * (*BodyPoint\frictionEdgeOnPoint * *BodyEdge\frictionEdgeOnPoint)
EndIf
If Not collisionInfo\ptrEdge\p2\bstatic
collisionInfo\ptrEdge\p2\vx + (collisionInfo\ptrEdge\p2\x - collisionInfo\ptrEdge\p2\vx) * collisionInfo\normal\y * (*BodyPoint\frictionEdgeOnPoint * *BodyEdge\frictionEdgeOnPoint)
collisionInfo\ptrEdge\p2\vy + (collisionInfo\ptrEdge\p2\y - collisionInfo\ptrEdge\p2\vy) * collisionInfo\normal\x * (*BodyPoint\frictionEdgeOnPoint * *BodyEdge\frictionEdgeOnPoint)
EndIf
If Not collisionInfo\ptrPoint\bstatic
collisionInfo\ptrPoint\vx + (collisionInfo\ptrPoint\x - collisionInfo\ptrPoint\vx) * collisionInfo\normal\y * (*BodyPoint\friction * *BodyEdge\friction)
collisionInfo\ptrPoint\vy + (collisionInfo\ptrPoint\y - collisionInfo\ptrPoint\vy) * collisionInfo\normal\x * (*BodyPoint\friction * *BodyEdge\friction)
EndIf
ProcedureReturn #True
EndIf
EndIf
EndProcedure
Procedure CircleToLine(*circ.body,*line.body)
Protected segv.vec2,seguv.vec2,pt.vec2,n.vec2,projv.vec2,*close.vec2,ptv.vec2,closest.vec2
Protected distv.vec2,distuv.vec2 ,axis.vec2 ,frac.d
Protected min.d,dotp.d,edge.i,len.d
min= 9999999
For a = 1 To *line\nconstraints
If *line\cords\constraints[a]\enabled = #False
Continue
EndIf
VEC2_substract(*circ\cords\points[0],*line\cords\constraints[a]\p1,axis)
len = VEC2_length(axis)
If len < min
min = len
edge = a
EndIf
Next
VEC2_substract(*line\cords\constraints[edge]\p2,*line\cords\constraints[edge]\p1,segv)
VEC2_substract(*circ\cords\points[0],*line\cords\constraints[edge]\p1,ptv)
VEC2_normalize(segv,seguv)
proj.d = VEC2_dotProduct(ptv,seguv)
If proj < 0
*close = *line\cords\constraints[edge]\p1
ElseIf proj > segv
*close = *line\cords\constraints[edge]\p2
Else
VEC2_multiply(seguv,proj,projv)
VEC2_add(*line\cords\constraints[edge]\p1,projv,closest)
*close = @closest
EndIf
VEC2_substract(*circ\cords\points[0],*close,distv)
VEC2_normalize(distv,distuv)
len = VEC2_length(distv)
If Len <= *circ\radius
If Not Bool(*circ\bcollide & *line\bcollide)
fd.d = (*circ\radius - len)
If *circ\bstatic Or *line\bstatic
frac = 1
Else
frac = 0.5
EndIf
VEC2_multiply(distuv,fd,distv)
If Not *circ\bstatic
*circ\cords\points[0]\x + distv\x * frac ;move circle away from line
*circ\cords\points[0]\vx + distv\x * frac
*circ\cords\points[0]\y + distv\y * frac
*circ\cords\points[0]\vy + distv\y * frac
EndIf
If Not *line\bstatic
*line\cords\points[0]\x - distv\x * frac
*line\cords\points[0]\y -distv\y * frac
EndIf
n.vec2
n2.vec2
r1.vec2
r1m.vec2
vz.vec2
r1\x = *circ\cords\points[0]\x - *circ\cords\points[0]\vx
r1\y = *circ\cords\points[0]\y - *circ\cords\points[0]\vy
VEC2_crossVector(seguv,n)
VEC2_normalize(n,n)
dotp = VEC2_dotProduct(r1,n)
VEC2_multiply(n,2,n2)
VEC2_multiply(n2,dotp,r1m)
VEC2_substract(r1m,r1,vz)
If Not *circ\bstatic
*circ\cords\points[0]\vx = *circ\cords\points[0]\x + vz\x
*circ\cords\points[0]\vy = *circ\cords\points[0]\y + vz\y
EndIf
If Not *line\bstatic
*line\cords\points[0]\vx = *line\cords\points[0]\x - vz\x
*line\cords\points[0]\vy = *line\cords\points[0]\y - vz\y
EndIf
ProcedureReturn #True
EndIf
EndIf
EndProcedure
Procedure CircleToCircle(*body.body,*body1.body)
Protected dist.d,dx.d,dy.d,frac.d
dx = *body\cords\points[0]\x - *body1\cords\points[0]\x
dy = *body\cords\points[0]\y - *body1\cords\points[0]\y
dist = Sqr(dx*dx+dy*dy)
t.d = *body\radius + *body1\radius
If dist < t
If Not Bool(*body\bcollide | *body1\bcollide)
fd.d = (dist-t)/dist
If *body\bstatic Or *body1\bstatic
frac = 1
Else
frac=0.5
EndIf
If Not *body\bstatic
*body\cords\points[0]\x - dx*fd*frac ;move bodies appart
*body\cords\points[0]\vx - dx*fd*frac
*body\cords\points[0]\y - dy*fd*frac
*body\cords\points[0]\vy - dy*fd*frac
EndIf
If Not *body1\bstatic
*body1\cords\points[0]\x + dx*fd*frac
*body1\cords\points[0]\vx + dx*fd*frac
*body1\cords\points[0]\y + dy*fd*frac
*body1\cords\points[0]\vy + dy*fd*frac
EndIf
m0.d = *body\cords\points[0]\mass ;get mass and velocities
m1.d = *body1\cords\points[0]\mass
x0.d = *body\cords\points[0]\x - *body\cords\points[0]\vx
x1.d = *body1\cords\points[0]\x- *body1\cords\points[0]\vx
y0.d = *body\cords\points[0]\y - *body\cords\points[0]\vy
y1.d = *body1\cords\points[0]\y - *body1\cords\points[0]\vy
dx = *body\cords\points\x - *body1\cords\points\x ;get normal vector
dy = *body\cords\points\y - *body1\cords\points\y
nx.d = dx / t
ny.d = dy / t
p.d = (2 * (x0 * nx + y0 * ny) - (x1 * nx + y1 * ny)) / (m0 + m1); ;get impulse fraction
dx = x0 - (p * m1 * nx) ;apply impulse
dy = y0 - (p * m1 * ny)
If Not *body\bstatic
*body\cords\points[0]\vx = *body\cords\points[0]\x -dx
*body\cords\points[0]\vy = *body\cords\points[0]\y - dy
EndIf
dx = x1 + (p * m0 * nx)
dy = y1 + (p * m0 * ny)
If Not *body1\bstatic
*body1\cords\points[0]\vx = *body1\cords\points[0]\x -dx
*body1\cords\points[0]\vy = *body1\cords\points[0]\y -dy
EndIf
ProcedureReturn #True
EndIf
ProcedureReturn #True
EndIf
EndProcedure
Procedure Collisions(*cbCollisionCallback.collisioncallback)
Protected *BodyPtr.body
Protected result
ForEach bodies()
*BodyPtr = @bodies()
While NextElement(bodies())
result = #False
If ((*bodyptr\cords\aabb\right < bodies()\cords\aabb\left) Or (*BodyPtr\cords\aabb\left > bodies()\cords\aabb\right)) Or ((*BodyPtr\cords\aabb\bottom < bodies()\cords\aabb\top) Or (*BodyPtr\cords\aabb\top > bodies()\cords\aabb\bottom))
Continue
Else
Select *BodyPtr\type
Case #Circle
If bodies()\type = #Circle
result = CircleToCircle(*BodyPtr,@bodies())
ElseIf bodies()\type = #Polygon Or bodies()\type = #line
result = CircleToLine(*BodyPtr,@bodies())
EndIf
Case #Polygon
If bodies()\type = #Polygon Or bodies()\type = #line
result = PolygonToPolygon(*BodyPtr,@bodies())
ElseIf bodies()\type = #circle
result = CircleToLine(@bodies(),*BodyPtr)
EndIf
Case #Line
If bodies()\type = #Polygon
result = PolygonToPolygon(*BodyPtr,@bodies())
ElseIf bodies()\type = #circle
result = CircleToLine(@bodies(),*BodyPtr)
EndIf
EndSelect
If result
If *cbCollisionCallback
*cbCollisionCallback(*BodyPtr,@bodies())
EndIf
EndIf
EndIf
Wend
ChangeCurrentElement(bodies(),*BodyPtr)
Next
EndProcedure
Procedure ApplyImpulse(*body.body,x,y)
Protected dx.d,dy.d
dx = *body\cords\points[0]\vx - *body\cords\points[0]\x
dy = *body\cords\points[0]\vy - *body\cords\points[0]\y
*body\cords\points[0]\x + (dx + x)
*body\cords\points[0]\y + (dy + y)
EndProcedure
Procedure WorldRotateBodies(*body.body,angle.d)
angle * (#PI/180)
cx = (world\box\left + world\box\right) * 0.5
cy = (world\box\top + world\box\bottom) * 0.5
For a = 0 To *body\npoints
dx.d = *body\cords\points[a]\x - cx
dy.d = *body\cords\points[a]\y - cy
rx.d = dx * Cos(angle) - dy * Sin(angle)
ry.d = dx * Sin(angle) + dy * Cos(angle)
*body\cords\points[a]\x = cx + rx
*body\cords\points[a]\vx = cx + rx
*body\cords\points[a]\y = cy + ry
*body\cords\points[a]\vy = cy + ry
Next
EndProcedure
Prototype ProcessKeys()
Prototype PreUpdate()
Prototype DrawSprites()
Procedure WorldDeleteBodies()
ForEach bodies()
If bodies()\flagdelete
DeleteElement(bodies(),1)
EndIf
Next
EndProcedure
Procedure RunWorld(*cbProcessKeys.ProcessKeys,*cbPreUpdate.PreUpdate,*cbDrawSprites.drawSprites=0,*cbCollisionCallback=0)
Protected numfps,numfpsShown,timer
Repeat
Repeat
EV = WindowEvent()
If EV = #PB_Event_CloseWindow
End
EndIf
Until EV = 0
ExamineKeyboard()
If *cbProcessKeys
*cbProcessKeys()
EndIf
For a = 1 To 5
If *cbPreUpdate
*cbPreUpdate()
EndIf
UpDateBodies()
Collisions(*cbCollisionCallback)
WorldDeleteBodies()
Next
ClearScreen(world\ClearColor)
If world\backgroundsprite
DisplaySprite(world\BackgroundSprite,0,0)
EndIf
If *cbDrawSprites
*cbDrawSprites()
EndIf
DrawBodies()
numfps+1
If ElapsedMilliseconds() >= timer
numfpsShown = numfps
numfps=0
timer = ElapsedMilliseconds()+1000
EndIf
Displaymessage(60,30,"FPS : " + Str(numfpsShown))
DisplayTransparentSprite(gmouse\sprite,gmouse\x-4,gmouse\y-4)
FlipBuffers()
Until KeyboardPushed(#PB_Key_Escape)
EndProcedure