Heres what i got but its runnin really slow with gravity <> 0.0 but without gravity it runs at a good pace. It needs some serious optimising to get the speed back up. It seems stable enough though. I think a little repulsive force that cancels out gravity is needed for objects that are at rest on a surface. which brings me back to how do you decide whether two objects are colliding or just in contact with each other. Well its definately more stable and looks better. Its just slower than I had hoped it would be.
open window 640, 512
window origin "cc"
gravity = 0.1
ed = 0.8 // energy NOT lost due to heat / deformation / sound
ew = 0.8 // same as ed but for circle to wall
bouncew = 0.8 // bounce with walls
frictionw = 0.98 // friction with walls
RT = 1.01 // radius tolerence must be above 1.0
polys = 8
dim mass(polys), radi(polys)
dim posX(polys), posY(polys)
dim dirX(polys), dirY(polys)
dim vecX(polys), vecY(polys)
for p = 0 to polys
mass(p) = 1.0
radi(p) = 30
posX(p) = ran(200)-ran(200)
posY(p) = ran(200)-ran(200)
dirX(p) = ran(10)-ran(10)
dirY(p) = ran(10)-ran(10)
next
walls = 3
dim x1(walls), y1(walls), x2(walls), y2(walls)
for w = 0 to walls
read x1(w), y1(w), x2(w), y2(w)
next
data -300,-230, 300,-230
data 300,-230, 300, 230
data 300, 230,-300, 230
data -300, 230,-300,-230
label main
wait 0.01
setdispbuf draw
draw = 1 - draw
setdrawbuf draw
clear window
setrgb 1, 255, 255, 255
T = TIME
text -300,-240, "TIME " + str$(TIME)
for p = 0 to polys
circle posX(p), posY(p), radi(p)
next
for w = 0 to walls
line x1(w),y1(w) to x2(w),y2(w)
next
ctrl = peek("port1")
if (and(ctrl, 16) <> 0) dirY(0) = dirY(0) - 1.0
if (and(ctrl, 32) <> 0) dirX(0) = dirX(0) + 1.0
if (and(ctrl, 64) <> 0) dirY(0) = dirY(0) + 1.0
if (and(ctrl,128) <> 0) dirX(0) = dirX(0) - 1.0
for p = 0 to polys
if (dirX(p) > -0.1) and (dirX(p) < 0.1) dirX(p) = 0.0
if (dirY(p) > -0.1) and (dirY(p) < 0.1) dirY(p) = 0.0
dirY(p) = dirY(p) + gravity
next
//repeat
TIME = 1.0
for p = 0 to polys
vecX(p) = dirX(p)
vecY(p) = dirY(p)
next
setrgb 1, 000, 000, 255
for A = 0 to polys
for w = 0 to walls
NEWTIME = circ2edgeDetect(A, x1(w),y1(w),x2(w),y2(w), 1.0)
if (NEWTIME < TIME) then
TIME = NEWTIME
CA = A
CW = w
CC = 1
endif
next
for B = A to polys
if (A <> B) then
NEWTIME = circ2circDetect(A, B, 1.0)
if (NEWTIME < TIME) then
TIME = NEWTIME
CA = A
CB = B
CC = 2
endif
endif
next
next
if (TIME < 1.0) then
if (CC = 1) circ2edgeRespond(CA, x1(CW),y1(CW),x2(CW),y2(CW), TIME)
if (CC = 2) circ2circRespond(CA, CB, TIME)
endif
for p = 0 to polys
posX(p) = posX(p) + TIME*dirX(p)
posY(p) = posY(p) + TIME*dirY(p)
next
if (TIME < 1.0) then
dirX(CA) = vecX(CA)
dirY(CA) = vecY(CA)
if (CC = 2) then
dirX(CB) = vecX(CB)
dirY(CB) = vecY(CB) - gravity
endif
if (CC = 1) then
dirY(CA) = dirY(CA) - gravity
endif
endif
//until (TIME = 1.0)
goto main
// Swept Circle <-> Static Line Segment
sub circ2edgeDetect(A, x1,y1,x2,y2, T)
// test for overlap at starting positions
local N, px,py,vx,vy,r, denom,numea,ua, xb,yb,d
N = T
px = posX(A)
py = posY(A)
vx = dirX(A)
vy = dirY(A)
r = radi(A)*RT
denom = (x2-x1)*(x2-x1) + (y2-y1)*(y2-y1)
numea = (px-x1)*(x2-x1) + (py-y1)*(y2-y1)
ua = numea / denom
if (ua < 0.0) then ua = 0.0
elsif (ua > 1.0) then ua = 1.0
endif
xb = x1 + ua*(x2-x1)
yb = y1 + ua*(y2-y1)
d = (px-xb)*(px-xb) + (py-yb)*(py-yb)
if (d < r*r) then
d = r / sqrt(d)
posX(A) = xb + (px-xb)*d
posY(A) = yb + (py-yb)*d
endif
// test for collision with vertices
local nx,ny, B,C,D, dx,dy,ax,ay,bx,by
r = radi(A)
nx = px + T*vx
ny = py + T*vy
B = (nx-px)*(nx-px) + (ny-py)*(ny-py)
if (B > 0.0) then
C = 2*((nx-px)*(px-x1) + (ny-py)*(py-y1))
D = C*C - 4*B*((px-x1)*(px-x1) + (py-y1)*(py-y1) - r*r)
if (D > 0.0) N = (-C - sqrt(D)) / (2*B)
if (N <= T) and (N > 0.0) T = N
C = 2*((nx-px)*(px-x2) + (ny-py)*(py-y2))
D = C*C - 4*B*((px-x2)*(px-x2) + (py-y2)*(py-y2) - r*r)
if (D > 0.0) N = (-C - sqrt(D)) / (2*B)
if (N <= T) and (N > 0.0) T = N
endif
// test for collision with line segment
B = r * sig((x1-px)*(y2-py) - (x2-px)*(y1-py)) / sqrt((x2-x1)*(x2-x1) + (y2-y1)*(y2-y1))
C = B * (y2-y1)
D = B * (x2-x1)
ax = x1 - C
ay = y1 + D
bx = x2 - C
by = y2 + D
D = (by-ay)*(nx-px) - (bx-ax)*(ny-py)
if (D = 0.0) return T
D = 1 / D
C = ((nx-px)*(py-ay) - (ny-py)*(px-ax)) * D
if (C >= 0.0) and (C <= 1.0) then
N = ((bx-ax)*(py-ay) - (by-ay)*(px-ax)) * D
if (N <= T) and (N > 0.0) T = N
endif
return T
end sub
sub circ2edgeRespond(A, x1,y1,x2,y2, T)
local px,py,nx,ny,r, C,xi,yi
px = posX(A) + T*dirX(A)
py = posY(A) + T*dirY(A)
nx = px + dirX(A)
ny = py + dirY(A)
r = radi(A)
C = ((px-x1)*(x2-x1) + (py-y1)*(y2-y1)) / ((x2-x1)*(x2-x1) + (y2-y1)*(y2-y1))
if (C < 0.0) C = 0.0
if (C > 1.0) C = 1.0
xi = x1 + C*(x2-x1)
yi = y1 + C*(y2-y1)
C = r / sqrt((xi-px)*(xi-px) + (yi-py)*(yi-py))
x1 = px - (yi-py)*C
y1 = py + (xi-px)*C
x2 = px + (yi-py)*C
y2 = py - (xi-px)*C
C = ((nx-x1)*(x2-x1) + (ny-y1)*(y2-y1)) / ((x2-x1)*(x2-x1) + (y2-y1)*(y2-y1))
xi = x1 + C*(x2-x1)
yi = y1 + C*(y2-y1)
vecX(A) = ((xi-px)*frictionw - (nx-xi)*bouncew)*ew
vecY(A) = ((yi-py)*frictionw - (ny-yi)*bouncew)*ew
end sub
// Swept Circle <-> Swept Circle
sub circ2circDetect(A, B, T)
// test for overlap at starting positions
local N, adj,opp,hyp, vA,vB,vC,vD,vE,c, x,y,xi,yi,ix,iy
adj = posX(A) - posX(B)
opp = posY(A) - posY(B)
hyp = adj*adj + opp*opp
if (hyp < (radi(A)+radi(B))*(radi(A)+radi(B))) then
hyp = 1 / sqrt(hyp)
adj = adj*hyp
opp = opp*hyp
x = (posX(A) + posX(B))*0.5
y = (posY(A) + posY(B))*0.5
posX(A) = x + radi(A)*adj
posY(A) = y + radi(A)*opp
posX(B) = x - radi(B)*adj
posY(B) = y - radi(B)*opp
endif
// test that distance traveled is sufficient for collision
vA = sqrt((posX(B)-posX(A))*(posX(B)-posX(A)) + (posY(B)-posY(A))*(posY(B)-posY(A)))
vB = sqrt((dirX(A)-dirX(B))*(dirX(A)-dirX(B)) + (dirY(A)-dirY(B))*(dirY(A)-dirY(B)))
if (vB < vA-(radi(A)+radi(B))) return T
// test that vectors are moving towards each other
c = ((posX(B)-posX(A))*(dirX(A)-dirX(B)) + (posY(B)-posY(A))*(dirY(A)-dirY(B))) / ((dirX(A)-dirX(B))*(dirX(A)-dirX(B)) + (dirY(A)-dirY(B))*(dirY(A)-dirY(B)))
if (c <= 0.0) return T
// test that circles actually overlap at swept intersection
xi = posX(A) + c*(dirX(A)-dirX(B))
yi = posY(A) + c*(dirY(A)-dirY(B))
vC = sqrt((posX(B)-xi)*(posX(B)-xi) + (posY(B)-yi)*(posY(B)-yi))
if (vC > radi(A)+radi(B)) return T
// refine calculated time of collision
vD = sqrt((posX(A)-xi)*(posX(A)-xi) + (posY(A)-yi)*(posY(A)-yi))
vE = sqrt((radi(A)+radi(B))*(radi(A)+radi(B)) - vC*vC)
ix = xi - vE*(dirX(A)-dirX(B)) / vB
iy = yi - vE*(dirY(A)-dirY(B)) / vB
N = ((ix-posX(A))*(dirX(A)-dirX(B)) + (iy-posY(A))*(dirY(A)-dirY(B))) / ((dirX(A)-dirX(B))*(dirX(A)-dirX(B)) + (dirY(A)-dirY(B))*(dirY(A)-dirY(B)))
if (N <= 0.0) or (N > T) return T
return N
end sub
sub circ2circRespond(A, B, T)
// calculate positions at time of collision
local x1,y1,x2,y2,D,ax,ay, va1,vb1,va2,vb2,vaP1,vaP2
x1 = posX(A) + T*dirX(A)
y1 = posY(A) + T*dirY(A)
x2 = posX(B) + T*dirX(B)
y2 = posY(B) + T*dirY(B)
D = 1 / sqrt((x2-x1)*(x2-x1) + (y2-y1)*(y2-y1))
ax = (x2-x1) * D
ay = (y2-y1) * D
va1 = dirX(A)*ax + dirY(A)*ay
vb1 =-dirX(A)*ay + dirY(A)*ax
va2 = dirX(B)*ax + dirY(B)*ay
vb2 =-dirX(B)*ay + dirY(B)*ax
vaP1 = va1 + (1+ed)*(va2-va1) / (1+mass(A)/mass(B))
vaP2 = va2 + (1+ed)*(va1-va2) / (1+mass(B)/mass(A))
vecX(A) = vaP1*ax - vb1*ay
vecY(A) = vaP1*ay + vb1*ax
vecX(B) = vaP2*ax - vb2*ay
vecY(B) = vaP2*ay + vb2*ax
end sub
