Dark Bit Factory & Gravity
PROGRAMMING => Other languages => Yabasic => Topic started by: rain_storm on November 04, 2007
-
Here is a nice little routine that will allow for circles of any radius length and moving at any speed to collide with a finite line segment. Not only does this routine detect the collision it goes one further and repositions the circle so that it just touches the line segment. Use it freely, I got my inspiration for this routine from the following tutorial, I didn't just cut and paste it's my own interpretation of the solution.
http://www.harveycartel.org/metanet/tutorials/tutorialA.html
I'm very happy with how it works but I want to optimise it to the maximum and was wondering if anybody can figure out a way to remove the square roots. I know that this can be done easily within if statements by simply squaring the other value in the comparison ex:
if (b > sqrt(a)) rem this is equivelent to below
if (b*b > a) rem this is equivelent to above
Can this trick be done to the values themselves? I cant seem to get it to work. Any ways here is the code for anyone who is interested
Edit :
bx = x2 - x1
by = y2 - y1
B = sqrt(bx*bx + by*by)
cs = r * by / B
sn = r * bx / B
Thats the square root I'm trying to eliminate. would it be possible if I squared bx,by then divided by B without a square root in sight
-
I don't think you'll be able to get rid of the square root like that. There's a method of 1/sqr(x) http://dbfinteractive.com/index.php?topic=1981.0 but I doubt it can be done in yabasic but in your example I'd calculate:
B=r/sqrt(bx*bx+by*by)
cs=by*B
sn=bx*B
as divides can be expensive too, doing it that way removes one of them and a couple of mults.
Cheers, Fryer.
-
Thats clever, I'm gonna use it. Now I hunted out an example of a simple square root algorithm and I used it to compute the root but its taking a few loops to calculate it and its got multiply's and divides so I wont use it in the final piece but it has thrown up an interesting conundrum. No matter how large bx and by are the square root never seems to change? I have no idea why this is but it looks like I can use a constant value instead of sqrt(bx*bx + by*by) dont ask me to explain it I just stumbled onto this a few moments ago. here is a version that prints out the square root axproximation that my new found algorithm calculates check it for yourself the value of the square root never changes
open window 640, 512
window origin "cc"
radi = 30.00
posX =-100.0
posY =-100.0
newX = 100.0
newY = 100.0
x1 =-100.000
y1 = 100.000
x2 = 100.000
y2 =-100.000
while (0 = 0)
setdispbuf draw
draw = 1 - draw
setdrawbuf draw
clear window
line x1,y1 to x2,y2
line posX,posY to newX,newY
fill circle posX, posY, radi
circle newX, newY, radi
fill circle xi, yi, radi
c1 = peek("port1")
c2 = peek("port2")
if (and(c1, 16) <> 0) posY = posY - 1
if (and(c1, 32) <> 0) posX = posX + 1
if (and(c1, 64) <> 0) posY = posY + 1
if (and(c1,128) <> 0) posX = posX - 1
if (and(c2, 16) <> 0) newY = newY - 1
if (and(c2, 32) <> 0) newX = newX + 1
if (and(c2, 64) <> 0) newY = newY + 1
if (and(c2,128) <> 0) newX = newX - 1
circ2seg(posX,posY,newX,newY,radi, x1,y1,x2,y2)
wend
sub circ2seg(px,py,nx,ny,r, x1,y1,x2,y2)
local r2,dx,dy,ax,ay,A,B,det,t,cp,cs,sn,denom,ua,ub
xi = nx
yi = ny
r2 = r*r
dx = xi - px
dy = yi - py
A = dx*dx + dy*dy
if (A > 0.0) then
B = 2*(dx*(px-x1) + dy*(py-y1))
det = B*B - 4*A*((px-x1)*(px-x1) + (py-y1)*(py-y1) - r2)
if (det > 0.0) then
t = (-B - sqrt(det)) / (2*A)
ax = t*dx
ay = t*dy
if (A > ax*ax + ay*ay) then
xi = px + ax
yi = py + ay
dx = xi - px
dy = yi - py
A = dx*dx + dy*dy
endif
endif
B = 2*(dx*(px-x2) + dy*(py-y2))
det = B*B - 4*A*((px-x2)*(px-x2) + (py-y2)*(py-y2) - r2)
if (det > 0.0) then
t = (-B - sqrt(det)) / (2*A)
ax = t*dx
ay = t*dy
if (A > ax*ax + ay*ay) then
xi = px + ax
yi = py + ay
dx = xi - px
dy = yi - py
A = dx*dx + dy*dy
endif
endif
endif
bx = x2 - x1
by = y2 - y1
B = root(bx*bx + by*by)
cp = r * sig((x1-px)*(y2-py) - (x2-px)*(y1-py)) / B
cs = cp * by
sn = cp * bx
x1 = x1 - cs
y1 = y1 + sn
x2 = x2 - cs
y2 = y2 + sn
ax = x2 - x1
ay = y2 - y1
denom = ay*dx - ax*dy
if (denom = 0.0) return 0
ua = (ax*(py-y1) - ay*(px-x1)) / denom
if (ua < 0.0) or (ua > 1.0) return 0
ub = ((xi-px)*(py-y1) - (yi-py)*(px-x1)) / denom
if (ub >= 0.0) and (ub <= 1.0) then
ax = ua*(xi-px)
ay = ua*(yi-py)
if (A > ax*ax + ay*ay) then
xi = px + ua*(xi-px)
yi = py + ua*(yi-py)
endif
endif
end sub
sub root(num)
count = 0
nx = num
repeat
x = nx
nx = 0.5*(x + num/x)
count = count + 1
until (abs(x - nx) < 100.0)
text 000,-220, "Input " + str$(num)
text 000,-200, "Loops " + str$(count)
text 000,-180, "Result " + str$(nx)
return nx
end sub
-
I figured it out its because the root algorithm is only a very rough estimate but it has proven that if I use surfaces that are at fixed angles I can precalculate the roots of each angle and plug them into the code instead of using the sqrt function
-
You could precalc a SQRT LUT (http://www.atari-forum.com/viewtopic.php?t=4984&highlight=newton+root) too, or use a fast INV_SQRT approximation (http://hhttp://betterexplained.com/articles/understanding-quakes-fast-inverse-square-root/) if your language allows such tricks.
HTH
-
Something along the lines of precalc would be what I need as Yabasic doesnt have shifts so inverse sqrt will not be possible however the look up table is perfect as I only want walls that are angles of 45 and maybe 22.5 degrees off true right angles
-
1/sqrt(x) is far easier to calculate than sqrt(x), as p01 points out. But the quake method can't buy you anything in yabasic since you don't have pointers and you don't have float (only double) and you don't have integers. Apart from that, it'll work just great. :P
You can still do the trick in yabasic, though it doesn't necessarily speed anything up
x = 99
store = x
guess = 1/x
x = guess
x = x*(1.5 - guess*x*x)
x = x*(1.5 - guess*x*x)
x = x*(1.5 - guess*x*x)
x = x*(1.5 - guess*x*x)
x = x*(1.5 - guess*x*x)
x = x*(1.5 - guess*x*x)
print x
print 1/sqrt(store)
The problem is calculating 'guess'. It needs to be sufficiently close to the right answer for the solution to work. Too far away, and it won't iterate properly (wrong answer). In quake they use a trick based on how floating point numbers are represented in memory - we can't really use that in yabasic. 1/x is a good guess, but it's slower than other guesses, eg if x starts out under 2 then x*0.5 is a good guess.
The second problem is how many
x = x*(1.5 - guess*x*x)
you need.
If your guess is good, you only need one or two to get an excellent result.
Be interesting to see if you can use it!
Jim
-
it may very well be possible but I suspect all benefits would be lost in a Yabasic implementation. I could substitute the shifts with multiplys and mask out whatever bits are neccessary but its the repeat until loop that will break the bank. I will definately be looking into this one more closely though as it is one hell of an optimisation, I plan on getting my head around it asap
-
The code I posted *is* a yabasic implementation! No shifts, no repeat loop.
Jim
-
But the quake method can't buy you anything in yabasic since you don't have pointers and you don't have float (only double) and you don't have integers
There must be an equivelent trick for doubles. but what is the importance of the pointer in all this is it so that the bits can be used as either a float or an integer?
int i = *(int*)&x; // store floating-point bits in integer
i = 0x5f3759d5 - (i >> 1); // initial guess for Newton's method
x = *(float*)&i; // convert new bits into float
what happens to the fractional part of the number in the first line of the above if x = 1.3 does i = 1.0? or is i the same bit pattern as x but represented as an integer
-
There must be an equivelent trick for doubles.
Yes, there is, but you still couldn't use it in yabasic.
but what is the importance of the pointer in all this is it so that the bits can be used as either a float or an integer?
Yes, the idea is that a float is represented like this M*2^E, where M is the fraction, and E is the power of 2. Just like when we use 3x10^8. But having the float as an integer means they can easily split the number apart in to M and E and work on them separately. That part is pretty much impossible on yabasic.
The magic number shifts the bits of M and E around to make a good guess.
Jim
-
wow! Some pretty hard-core math being done here! :boxer:
haha. Nice stuff Rain... nice bit of code I've never seen done before in Yabasic.
-
=D Thanks CLANKY, Heres a version that test for a grid of such line segments it will take a few minutes before it happens but the circle will pass through a wall eventually. this is because the sliding feature is not in there yet and sometimes that can cause problems. When I can get my head around this inverse square root stuff I will turn my attention to that
-
:inspired:
I know how to eliminate those unneccessary sqrt's I should be simply detecting collisions in one sub-routine which should return t as a function of time. Then if t < 1.0 then I should call the collision response routine. This is by far the best solution I can come up with since I dont need any sqrt's to detect the collisions and as it stands every detected collision is responded to which is very wasteful if further collisions are detected. Man why didn't I just it that way to begin with
:whack:
-
Haha! Nice... *don't ask me about it! have no clue :P*
Checked out the code you uploaded! That is awesome!!! Feels gamely - K+ for that.
--I had to change the
c2 = peek("port2")
to
c2 = peek("port1")
Not sure if you meant to have it as PORT2, but I'm using a lap top so yea... can't use the second players arrows easily :P
Great Though!!!
-
Sorry bout that I noramlly use port2 because on my computer peeking port1 behaves more like inkey$ it only registers the last button pressed but peeking port2 acts as expected allowing diagonal motion. I usually switch it back when I post code but I guese I forgot that time
-
Finally I have it finished and I avoid sqrt unless it cannot be avoided. I have also optimised it further but it now requires that the normalised vectors of radius length be passed in as extra parameters.
for the detection routine you should pass in the normalised movement vector of the circle for the response routine its the normalised vector of the line. I have included a normalise routine that takes care of this.
circ2segDetect should be called for all likely collideable surfaces if the returned result is lower than your current frametime use the new result and record which tyle it was. after all likely tyles have been checked
you should call circ2segRespond which will refine the time of collision. now all thats left is to move everything by dirX*frametime, dirY*frametime. sliding has not yet been included but Im working on it.
// Normalised vector of radius length is required
sub normalise(x1,y1,x2,y2,r)
xn = r*(y1-y2) / sqrt((x2-x1)*(x2-x1) + (y2-y1)*(y2-y1))
yn = r*(x2-x1) / sqrt((x2-x1)*(x2-x1) + (y2-y1)*(y2-y1))
end sub
// fast circle collision detection
sub circ2segDetect(px,py,nx,ny,r, x1,y1,x2,y2,xn,yn)
local denom,nume_a,nume_b
px = px + xn
py = py + yn
nx = nx + xn
ny = ny + yn
line px,py to nx,ny
denom = (y2-y1)*(nx-px) - (x2-x1)*(ny-py)
if (denom <> 0.0) then
nume_a = ((x2-x1)*(py-y1) - (y2-y1)*(px-x1)) / denom
nume_b = ((nx-px)*(py-y1) - (ny-py)*(px-x1)) / denom
if (nume_a <= 0.0) return 1.0
if (nume_b >= 0.0) and (nume_a < 1.0) return nume_a
endif
px = px - xn - xn
py = py - yn - yn
nx = nx - xn - xn
ny = ny - yn - yn
line px,py to nx,ny
denom = (y2-y1)*(nx-px) - (x2-x1)*(ny-py)
if (denom <> 0.0) then
nume_a = ((x2-x1)*(py-y1) - (y2-y1)*(px-x1)) / denom
nume_b = ((nx-px)*(py-y1) - (ny-py)*(px-x1)) / denom
if (nume_a <= 0.0) return 1.0
if (nume_b <= 1.0) and (nume_a < 1.0) return nume_a
endif
// This code is only likely to be executed if the
// destination is really close to the line
px = px + xn
py = py + yn
nx = nx + xn
ny = ny + yn
ax = x2 - x1
ay = y2 - y1
A = ax*ax + ay*ay
A = A + A
if (A <= 0.0000001) return 0
bx = x1 - nx
by = y1 - ny
B = ax*bx + ay*by
B = B + B
C = bx*bx + by*by - r*r
C = C + C
det = B*B - A*C
if (det < 0) return 1
// This will only be executed if the circle intersects line
t = -B / A
ax = x1 + t*ax
ay = y1 + t*ay
A = (ax-px)*(ax-px) + (ay-py)*(ay-py)
B = (nx-px)*(nx-px) + (ny-py)*(ny-py)
return sqrt(B/A)
end sub
// fast circle collision response
sub circ2segRespond(px,py,nx,ny,r, x1,y1,x2,y2,xn,yn)
local denom,nume_a,nume_b,nume_c,ax,ay,ix,iy,A,B,C,e,a,t,t2
t = 1.0
px = px + xn
py = py + yn
nx = nx + xn
ny = ny + yn
denom = (y2-y1)*(nx-px) - (x2-x1)*(ny-py)
if (denom <> 0.0) then
nume_a = ((x2-x1)*(py-y1) - (y2-y1)*(px-x1)) / denom
nume_b = ((nx-px)*(py-y1) - (ny-py)*(px-x1)) / denom
if (nume_a <= 0.0) nume_a = 0.0
if (nume_a >= 1.0) nume_a = 1.0
if (nume_b >= 0.0) and (nume_b <= 1.0) t = nume_a
endif
px = px - xn
py = py - yn
nx = nx - xn
ny = ny - yn
ax = nx - px
ay = ny - py
A = ax*ax + ay*ay
nume_c = ((x1-px)*ax + (y1-py)*ay) / A
if (nume_c >= 0.0) then
ix = px + nume_c*ax
iy = py + nume_c*ay
if ((x1-ix)*(x1-ix) + (y1-iy)*(y1-iy) <= r*r) then
bx = px - x1
by = py - y1
B = ax*bx + ay*by
B = B + B
C = bx*bx + by*by - r*r
C = C + C
det = B*B - (A+A)*C
if (det >= 0.0) then
t2 = (-B - sqrt(det)) / (A+A)
if (t2 <= t) t = t2
endif
endif
endif
nume_c = ((x2-px)*ax + (y2-py)*ay) / A
if (nume_c >= 0.0) then
ix = px + nume_c*ax
iy = py + nume_c*ay
if ((x2-ix)*(x2-ix) + (y2-iy)*(y2-iy) <= r*r) then
bx = px - x2
by = py - y2
B = ax*bx + ay*by
B = B + B
C = bx*bx + by*by - r*r
C = C + C
det = B*B - (A+A)*C
if (det >= 0.0) then
t2 = (-B - sqrt(det)) / (A+A)
if (t2 <= t) t = t2
endif
endif
endif
return t
end sub
open window 640, 512
window origin "cc"
px =-100
py =-100
r = 30
x1 =-100
y1 = 100
x2 = 100
y2 =-100
while (0 = 0)
wait 0.01
setdispbuf draw
draw = 1 - draw
setdrawbuf draw
clear window
circle px,py,r
circle nx,ny,r
circle xi,yi,r
line px,py to nx,ny
line x1,y1 to x2,y2
nx1 = x1
ny1 = y1
nx2 = x2
ny2 = y2
c = peek("port1")
if (and(c, 16) <> 0) dy = dy - 1
if (and(c, 32) <> 0) dx = dx + 1
if (and(c, 64) <> 0) dy = dy + 1
if (and(c,128) <> 0) dx = dx - 1
if (and(c, 4096) <> 0) ny2 = y2 - 1
if (and(c, 8192) <> 0) nx2 = x2 + 1
if (and(c,16384) <> 0) ny2 = y2 + 1
if (and(c,32768) <> 0) nx2 = x2 - 1
c = peek("port2")
if (and(c, 16) <> 0) py = py - 1
if (and(c, 32) <> 0) px = px + 1
if (and(c, 64) <> 0) py = py + 1
if (and(c,128) <> 0) px = px - 1
if (and(c, 4096) <> 0) ny1 = y1 - 1
if (and(c, 8192) <> 0) nx1 = x1 + 1
if (and(c,16384) <> 0) ny1 = y1 + 1
if (and(c,32768) <> 0) nx1 = x1 - 1
nx = px + dx
ny = py + dy
normalise(px,py,nx,ny,r)
c = circ2segDetect(px,py,nx,ny,r, x1,y1,x2,y2,xn,yn)
if (c < 1.0) then
normalise(x1,y1,x2,y2,r)
t = circ2segRespond(px,py,nx,ny,r, x1,y1,x2,y2,xn,yn)
xi = px + dx*t
yi = py + dy*t
endif
wend
-
Here are a lot of static collision detection routines its got a whole slew of primitives so feel free to dive in here and use whatever you find useful. These are just the routines though as there is too many to show an example of how the work but they are all tested and work rather well. They all return true / false in the event of a collision or lack thereof. To speed them up a little make sure that your primitives have their vertices arranged in clockwise order. Some of the routines (with triangle and quads etc.) are dependent on other routines (line intersections mostly)
sub point2point(x0,y0, x1,y1)
if (x0 = x1) and (y0 = y1) return 1
return 0
end sub
sub point2circle(x0,y0, x1,y1,r)
if (abs(x0-x1)^2 + abs(y0-y1)^2 < r^2) return 1
return 0
end sub
sub point2line(x0,y0, x1,y1,x2,y2)
if ((x1-x0)*(y2-y0) - (x2-x0)*(y1-y0) = 0) return 1
return 0
end sub
sub point2segment(x0,y0, x1,y1,x2,y2)
if ((x1-x0)*(y2-y0) - (x2-x0)*(y1-y0) = 0) then // Crossproduct
if (x0 > x1) and (x0 < x2) return 1 // Boolean logic
if (x0 > x2) and (x0 < x1) return 1 // Boolean logic
endif
return 0
end sub
sub point2box(x0,y0, x1,y1,x2,y2)
if (x0 > x1) and (x0 < x2) then
if (y0 > y1) and (y0 < y2) return 1
if (y0 > y2) and (y0 < y1) return 1
elsif (x0 > x2) and (x0 < x1) then
if (y0 > y1) and (y0 < y2) return 1
if (y0 > y2) and (y0 < y1) return 1
endif
return 0
end sub
sub point2tri(x0,y0, x1,y1,x2,y2,x3,y3)
if ((x1-x0)*(y3-y0)-(x3-x0)*(y1-y0) < 0) then
if ((x2-x0)*(y1-y0)-(x1-x0)*(y2-y0) < 0) then
if ((x3-x0)*(y2-y0)-(x2-x0)*(y3-y0) < 0) then
return 1
endif
endif
endif
if ((x1-x0)*(y2-y0)-(x2-x0)*(y1-y0) < 0) then
if ((x2-x0)*(y3-y0)-(x3-x0)*(y2-y0) < 0) then
if ((x3-x0)*(y1-y0)-(x1-x0)*(y3-y0) < 0) then
return 1
endif
endif
endif
return 0
end sub
sub point2quad(x0,y0, x1,y1,x2,y2,x3,y3,x4,y4)
if ((x1-x0)*(y4-y0)-(x4-x0)*(y1-y0) < 0) then
if ((x2-x0)*(y1-y0)-(x1-x0)*(y2-y0) < 0) then
if ((x3-x0)*(y2-y0)-(x2-x0)*(y3-y0) < 0) then
if ((x4-x0)*(y3-y0)-(x3-x0)*(y4-y0) < 0) then
return 1
endif
endif
endif
endif
if ((x1-x0)*(y2-y0)-(x2-x0)*(y1-y0) < 0) then
if ((x2-x0)*(y3-y0)-(x3-x0)*(y2-y0) < 0) then
if ((x3-x0)*(y4-y0)-(x4-x0)*(y3-y0) < 0) then
if ((x4-x0)*(y1-y0)-(x1-x0)*(y4-y0) < 0) then
return 1
endif
endif
endif
endif
return 0
end sub
sub line2point(x0,y0,x1,y1, x2,y2)
local dx,dy,cl
dx = x1 - x0
dy = y1 - y0
cl = ((x2-x0)*dx + (y2-y0)*dy) / (dx*dx + dy*dy)
xi = (x0 + cl*dx)
yi = (y0 + cl*dy)
dx = x2 - xi
dy = y2 - yi
if (abs(dx)^2 + abs(dy)^2 <= 1) return 1
return 0
end sub
sub line2circle(x0,y0,x1,y1, x2,y2,r2)
local dx,dy,cl
dx = x1 - x0
dy = y1 - y0
cl = ((x2-x0)*dx + (y2-y0)*dy) / (dx*dx + dy*dy)
xi = x0 + cl*dx
yi = y0 + cl*dy
if ((x2-xi)^2 + (y2-yi)^2 < r2^2) return 1
return 0
end sub
sub line2line(x0,y0,x1,y1, x2,y2,x3,y3)
local a1,a2,c1,c2
a1 = (y1-y0) / (x1-x0)
a2 = (y3-y2) / (x3-x2)
di = 1 / (a2 - a1)
c1 = y0 - a1*x0
c2 = y2 - a2*x2
xi = (c1 - c2) * di
yi = (a2*c1 - a1*c2) * di
end sub
sub line2segment(x0,y0,x1,y1, x2,y2,x3,y3)
local a1,a2,c1,c2
a1 = (y1-y0) / (x1-x0)
a2 = (y3-y2) / int(x3-x2)
di = 1 / (a2 - a1)
c1 = y0 - a1*x0
c2 = y2 - a2*x2
xi = (c1 - c2) * di
yi = (a2*c1 - a1*c2) * di
if (xi > x2) and (xi < x3) then
if (yi > y2) and (yi < y3) return 1
if (yi > y3) and (yi < y2) return 1
endif
if (xi > x3) and (xi < x2) then
if (yi > y2) and (yi < y3) return 1
if (yi > y3) and (yi < y2) return 1
endif
return 0
end sub
sub line2box(x0,y0,x1,y1, x2,y2,x3,y3)
if (line2segment(x0,y0,x1,y1, x2,y2,x3,y3) = 1) return 1
if (line2segment(x0,y0,x1,y1, x3,y2,x2,y3) = 1) return 1
return 0
end sub
sub line2tri(x0,y0,x1,y1, x2,y2,x3,y3,x4,y4)
if (line2segment(x0,y0,x1,y1, x2,y2,x3,y3) = 1) return 1
if (line2segment(x0,y0,x1,y1, x3,y3,x4,y4) = 1) return 1
if (line2segment(x0,y0,x1,y1, x4,y4,x2,y2) = 1) return 1
return 0
end sub
sub line2quad(x0,y0,x1,y1, x2,y2,x3,y3,x4,y4,x5,y5)
if (line2segment(x0,y0,x1,y1, x2,y2,x4,y4) = 1) return 1
if (line2segment(x0,y0,x1,y1, x3,y3,x5,y5) = 1) return 1
return 0
end sub
sub circle2point(x0,y0,r0, x1,y1)
if (abs(x0-x1)^2 + abs(y0-y1)^2 < r0^2) return 1
return 0
end sub
sub circle2circle(x0,y0,r0, x1,y1,r1)
if (abs(x0-x1)^2 + abs(y0-y1)^2 < (r0+r1)^2) return 1
return 0
end sub
sub circle2line(x0,y0,r0, x1,y1,x2,y2)
local dx,dy,cl
dx = x2 - x1
dy = y2 - y1
cl = ((x0-x1)*dx + (y0-y1)*dy) / (dx*dx + dy*dy)
xi = x1 + cl*dx
yi = y1 + cl*dy
if ((x0-xi)^2 + (y0-yi)^2 < r0^2) return 1
return 0
end sub
sub circle2segment(x0,y0,r0, x1,y1,x2,y2)
local dx,dy,cl
dx = x2-x1
dy = y2-y1
cl = ((x0-x1)*dx + (y0-y1)*dy) / (abs(dx)^2 + abs(dy)^2)
if (cl < 0.0) then
cl = 0.0
elsif (cl > 1.0) then
cl = 1.0
endif
xi = x1 + cl*dx
yi = y1 + cl*dy
if (abs(x0 - xi)^2 + abs(y0 - yi)^2 <= r0^2) return 1
return 0
end sub
sub circle2box(x0,y0,r0, x1,y1,x2,y2, z0,z1,z2)
local s2c
if (x0 < x1) then
s2c = abs(x0-x1)^2
elsif (x0 > x2) then
s2c = abs(x0-x2)^2
endif
if (y0 < y1) then
s2c = abs(y0-y1)^2 + s2c
elsif (y0 > y2) then
s2c = abs(y0-y2)^2 + s2c
endif
if (s2c <= r0^2) return 1
return 0
end sub
sub circle2tri(x0,y0,r0, x1,y1,x2,y2,x3,y3)
if (circle2segment(x0,y0,r0, x1,y1,x2,y2) = 1) return 1
if (circle2segment(x0,y0,r0, x2,y2,x3,y3) = 1) return 1
if (circle2segment(x0,y0,r0, x3,y3,x1,y1) = 1) return 1
if ((x1-x0)*(y3-y0)-(x3-x0)*(y1-y0) < 0) then
if ((x2-x0)*(y1-y0)-(x1-x0)*(y2-y0) < 0) then
if ((x3-x0)*(y2-y0)-(x2-x0)*(y3-y0) < 0) then
return 1
endif
endif
endif
if ((x1-x0)*(y2-y0)-(x2-x0)*(y1-y0) < 0) then
if ((x2-x0)*(y3-y0)-(x3-x0)*(y2-y0) < 0) then
if ((x3-x0)*(y1-y0)-(x1-x0)*(y3-y0) < 0) then
return 1
endif
endif
endif
return 0
end sub
sub circle2quad(x0,y0,r0, x1,y1,x2,y2,x3,y3,x4,y4)
if (circle2segment(x0,y0,r0, x1,y1,x2,y2) = 1) return 1
if (circle2segment(x0,y0,r0, x2,y2,x3,y3) = 1) return 1
if (circle2segment(x0,y0,r0, x3,y3,x4,y4) = 1) return 1
if (circle2segment(x0,y0,r0, x4,y4,x1,y1) = 1) return 1
if ((x1-x0)*(y4-y0)-(x4-x0)*(y1-y0) < 0) then
if ((x2-x0)*(y1-y0)-(x1-x0)*(y2-y0) < 0) then
if ((x3-x0)*(y2-y0)-(x2-x0)*(y3-y0) < 0) then
if ((x4-x0)*(y3-y0)-(x3-x0)*(y4-y0) < 0) then
return 1
endif
endif
endif
endif
if ((x1-x0)*(y2-y0)-(x2-x0)*(y1-y0) < 0) then
if ((x2-x0)*(y3-y0)-(x3-x0)*(y2-y0) < 0) then
if ((x3-x0)*(y4-y0)-(x4-x0)*(y3-y0) < 0) then
if ((x4-x0)*(y1-y0)-(x1-x0)*(y4-y0) < 0) then
return 1
endif
endif
endif
endif
end sub
sub segment2point(x0,y0,x1,y1, x2,y2)
local dx,dy,cl
dx = x1 - x0
dy = y1 - y0
cl = ((x2-x0)*dx + (y2-y0)*dy) / (dx*dx + dy*dy)
xi = (x0 + cl*dx)
yi = (y0 + cl*dy)
dx = x2 - xi
dy = y2 - yi
if (abs(dx)^2 + abs(dy)^2 <= 1) then
if (xi >= x0) and (xi <= x1) return 1
if (xi >= x1) and (xi <= x2) return 1
endif
return 0
end sub
sub segment2circle(x0,y0,x1,y1, x2,y2,r2)
local dx,dy,cl
dx = x1-x0
dy = y1-y0
cl = ((x2-x0)*dx + (y2-y0)*dy) / (abs(dx)^2 + abs(dy)^2)
if (cl < 0.0) then
cl = 0.0
elsif (cl > 1.0) then
cl = 1.0
endif
xi = x0 + cl*dx
yi = y0 + cl*dy
if (abs(x2 - xi)^2 + abs(y2 - yi)^2 <= r2^2) return 1
return 0
end sub
sub segment2line(x0,y0,x1,y1, x2,y2,x3,y3)
local denom, nume_a, nume_b
denom = ((y3 - y2)*(x1 - x0)) - ((x3 - x2)*(y1 - y0))
nume_a = ((x3 - x2)*(y0 - y2)) - ((y3 - y2)*(x0 - x2))
nume_b = ((x1 - x0)*(y0 - y2)) - ((y1 - y0)*(x0 - x2))
if (denom = 0.0) then
if (nume_a = 0.0) and (nume_b = 0.0) return 0
return 0
endif
local ua, ub
ua = nume_a / denom
ub = nume_b / denom
if (ua >= 0.0) and (ua <= 1.0) then
xi = x0 + ua*(x1 - x0)
yi = y0 + ua*(y1 - y0)
return 1
endif
return 0
end sub
sub segment2segment(x0,y0,x1,y1, x2,y2,x3,y3)
local denom, nume_a, nume_b
denom = ((y3 - y2)*(x1 - x0)) - ((x3 - x2)*(y1 - y0))
nume_a = ((x3 - x2)*(y0 - y2)) - ((y3 - y2)*(x0 - x2))
nume_b = ((x1 - x0)*(y0 - y2)) - ((y1 - y0)*(x0 - x2))
if (denom = 0.0) then
if (nume_a = 0.0) and (nume_b = 0.0) return 0
return 0
endif
local ua, ub
ua = nume_a / denom
ub = nume_b / denom
if (ua >= 0.0) and (ua <= 1.0) and (ub >= 0.0) and (ub <= 1.0) then
xi = x0 + ua*(x1 - x0)
yi = y0 + ua*(y1 - y0)
return 1
endif
return 0
end sub
sub segment2box(x0,y0,x1,y1, x2,y2,x3,y3)
if (segment2segment(x0,y0,x1,y1, x2,y2,x3,y2) = 1) return 1
if (segment2segment(x0,y0,x1,y1, x3,y2,x3,y3) = 1) return 1
if (segment2segment(x0,y0,x1,y1, x3,y3,x2,y3) = 1) return 1
if (segment2segment(x0,y0,x1,y1, x2,y3,x2,y2) = 1) return 1
return 0
end sub
sub segment2tri(x0,y0,x1,y1, x2,y2,x3,y3,x4,y4)
if (segment2segment(x0,y0,x1,y1, x2,y2,x3,y3) = 1) return 1
if (segment2segment(x0,y0,x1,y1, x3,y3,x4,y4) = 1) return 1
if (segment2segment(x0,y0,x1,y1, x4,y4,x2,y2) = 1) return 1
return 0
end sub
sub segment2quad(x0,y0,x1,y1, x2,y2,x3,y3,x4,y4,x5,y5)
if (segment2segment(x0,y0,x1,y1, x2,y2,x3,y3) = 1) return 1
if (segment2segment(x0,y0,x1,y1, x3,y3,x4,y4) = 1) return 1
if (segment2segment(x0,y0,x1,y1, x4,y4,x5,y5) = 1) return 1
if (segment2segment(x0,y0,x1,y1, x5,y5,x2,y2) = 1) return 1
return 0
end sub
sub box2point(x0,y0,x1,y1, x2,y2)
if (x2 >= x0) and (x2 <= x1) then
if (y2 >= y0) and (y2 <= y1) return 1
if (y2 >= y1) and (y2 <= y0) return 1
elsif (x2 >= x1) and (x2 <= x0) then
if (y2 >= y0) and (y2 <= y1) return 1
if (y2 >= y1) and (y2 <= y0) return 1
endif
return 0
end sub
sub box2circle(x0,y0,x1,y1, x2,y2,r2)
local s2c
if (x2 < x0) then
s2c = abs(x2-x0)^2
elsif (x2 > x1) then
s2c = abs(x2-x1)^2
endif
if (y2 < y0) then
s2c = abs(y2-y0)^2 + s2c
elsif (y2 > y1) then
s2c = abs(y2-y1)^2 + s2c
endif
if (s2c <= r2^2) return 1
s2c = 0
if (x2 > x0) then
s2c = abs(x2-x0)^2
elsif (x2 < x1) then
s2c = abs(x2-x1)^2
endif
if (y2 > y0) then
s2c = abs(y2-y0)^2 + s2c
elsif (y2 < y1) then
s2c = abs(y2-y1)^2 + s2c
endif
if (s2c <= r2^2) return 1
return 0
end sub
sub box2line(x0,y0,x1,y1, x2,y2,x3,y3) end sub
sub box2segment(x0,y0,x1,y1, x2,y2,x3,y3) end sub
sub box2box(x0,y0,x1,y1, x2,y2,x3,y3)
if (x0 < x2) and (x0 < x3) and (x1 < x2) and (x1 < x3) return 0
if (x0 > x2) and (x0 > x3) and (x1 > x2) and (x1 > x3) return 0
if (y0 < y2) and (y0 < y3) and (y1 < y2) and (y1 < y3) return 0
if (y0 > y2) and (y0 > y3) and (y1 > y2) and (y1 > y3) return 0
if (x2 < x0) and (x2 < x1) and (x3 < x0) and (x3 < x1) return 0
if (x2 > x0) and (x2 > x1) and (x3 > x0) and (x3 > x1) return 0
if (y2 < y0) and (y2 < y1) and (y3 < y0) and (y3 < y1) return 0
if (y2 > y0) and (y2 > y1) and (y3 > y0) and (y3 > y1) return 0
return 1
end sub
// eof
Edit I think I got the sliding feature working now but its still having trouble with acute angles if the circle starts really close to the edge
open window 640, 512
window origin "cc"
px =-100
py = 100
x1 =-100
y1 =-100
x2 = 100
y2 = 100
x3 = 100
y3 =-100
r = 30.0
epsilon = 1.0
bounce = 0.001
while (0 = 0)
wait 0.01
c = peek("port1")
if (and(c, 16) <> 0) dy = dy - 1
if (and(c, 32) <> 0) dx = dx + 1
if (and(c, 64) <> 0) dy = dy + 1
if (and(c,128) <> 0) dx = dx - 1
c = peek("port2")
if (and(c, 16) <> 0) py = py - 1
if (and(c, 32) <> 0) px = px + 1
if (and(c, 64) <> 0) py = py + 1
if (and(c,128) <> 0) px = px - 1
nx = px + dx
ny = py + dy
setdispbuf draw
draw = 1 - draw
setdrawbuf draw
clear window
triangle x1,y1 to x2,y2 to x3,y3
circle px,py,r
x = nx
y = ny
curtime = 1.0
newtime = circ2seg(px,py,nx,ny,r, x1,y1,x2,y2)
if (newtime < curtime) then
curtime = newtime
xb = ix
yb = iy
endif
newtime = circ2seg(px,py,nx,ny,r, x2,y2,x3,y3)
if (newtime < curtime) then
curtime = newtime
xb = ix
yb = iy
endif
newtime = circ2seg(px,py,nx,ny,r, x3,y3,x1,y1)
if (newtime < curtime) then
curtime = newtime
xb = ix
yb = iy
endif
if (curtime < 1.0) then
xa = px + curtime*(nx-px)
ya = py + curtime*(ny-py)
fill box xa-2,ya-2 to xa+2,ya+2
fill box xb-2,yb-2 to xb+2,yb+2
// normalise x1,y1,x2,y2
A = r / sqrt((xb-xa)*(xb-xa) + (yb-ya)*(yb-ya))
B = A*(yb-ya)
C = A*(xb-xa)
ax = xa - B
ay = ya + C
bx = xa + B
by = ya - C
setrgb 1, 000, 255, 000
line ax,ay to bx,by
setrgb 1, 255, 255, 255
cl = ((nx-ax)*(bx-ax) + (ny-ay)*(by-ay)) / ((bx-ax)*(bx-ax) + (by-ay)*(by-ay))
vx = ax + cl*(bx-ax)
vy = ay + cl*(by-ay)
circle vx,vy,r
endif
nx = px + curtime*dx
ny = py + curtime*dy
line px,py to nx,ny
circle nx,ny,r
wend
sub circ2seg(px,py,nx,ny,r, x1,y1,x2,y2)
local denoma,denomb, nume_a,nume_b,nume_c,nume_d, A,B,C,D, t,nt
t = 1.0
r = r + epsilon
// normalise x1,y1,x2,y2
A = r / sqrt((x2-x1)*(x2-x1) + (y2-y1)*(y2-y1))
B = A*(y2-y1)
C = A*(x2-x1)
ax = px + B
ay = py - C
bx = nx + B
by = ny - C
// project ray onto line x1,y1,x2,y2
denomb = 1 / ((y2-y1)*(bx-ax) - (x2-x1)*(by-ay))
nume_d = denomb*((x2-x1)*(ay-y1) - (y2-y1)*(ax-x1))
if (nume_d < 0.0) return t
nume_c = denomb*((bx-ax)*(ay-y1) - (by-ay)*(ax-x1))
if (nume_c < 0.0) then
// project ray onto circle x1,y1,r
A = (nx-px)*(nx-px) + (ny-py)*(ny-py)
B = (nx-px)*(px-x1) + (ny-py)*(py-y1)
C = (px-x1)*(px-x1) + (py-y1)*(py-y1) - r*r
D = (B+B)*(B+B) - (A+A)*(C+C)
if (D >= 0) then
ix = x1
iy = y1
return (-(B+B) - sqrt(D)) / (A+A)
endif
elsif (nume_c > 1.0) then
// project ray onto circle x2,y2,r
A = (nx-px)*(nx-px) + (ny-py)*(ny-py)
B = (nx-px)*(px-x2) + (ny-py)*(py-y2)
C = (px-x2)*(px-x2) + (py-y2)*(py-y2) - r*r
D = (B+B)*(B+B) - (A+A)*(C+C)
if (D >= 0) then
ix = x2
iy = y2
return (-(B+B) - sqrt(D)) / (A+A)
endif
else
ix = x1 + nume_c*(x2-x1)
iy = y1 + nume_c*(y2-y1)
return nume_d
endif
return t
end sub
-
Hey rain_storm,
Would you mind if I converted this to FB? I'm working on collision detection and response (sliding) at the moment, and this may be very useful.
-
Sure but here is a more updated one with sliding there is just one glitch that needs to be fixed before optimising the hell out of this. sometimes where there are more surfaces and especially at acute angles the circle can be embedded in the surface which will cause the routine to fail to detect a collision as the algorithm assumes that it is dealing with a circle that is always on the outside of the polygon before the collision. You will see what I mean when this is brought to a grid of collideable surfaces
open window 640, 512
window origin "cc"
bounce = 0.10
friction = 0.90
epsilon = 10.00 // tolerence
gravity = 0.00
px =-150.0
py = 000.0
r = 30.0
walls = 3.0
dim x1(walls), y1(walls)
dim x2(walls), y2(walls)
for w = 0 to walls
read x1(w), y1(w), x2(w), y2(w)
next
data 100,-100,-100,-100
data 100, 100, 100,-100
data -100, 100, 100, 100
data -100,-100,-100, 100
while (0 = 0)
wait 0.01
setdispbuf draw
draw = 1 - draw
setdrawbuf draw
clear window
ctrl = peek("port1")
if (and(ctrl, 16) <> 0) dy = dy - 1.0
if (and(ctrl, 32) <> 0) dx = dx + 1.0
if (and(ctrl, 64) <> 0) dy = dy + 1.0
if (and(ctrl,128) <> 0) dx = dx - 1.0
ctrl = peek("port2")
if (and(ctrl, 16) <> 0) py = py - 1.0
if (and(ctrl, 32) <> 0) px = px + 1.0
if (and(ctrl, 64) <> 0) py = py + 1.0
if (and(ctrl,128) <> 0) px = px - 1.0
nx = px + dx
ny = py + dy
xd = dx
yd = dy
curtime = 1.0
for w = 0 to walls
newtime = circ2edge(px,py,nx,ny,r, x1(w),y1(w),x2(w),y2(w),curtime)
if (newtime < curtime) then
curtime = newtime
xd = vx
yd = vy
tx = ix + xd
ty = iy + yd
endif
line x1(w),y1(w) to x2(w),y2(w)
next
if (curtime < 1.0) then
circle ix,iy,r
circle tx,ty,r
endif
circle px,py,r
circle nx,ny,r
wend
sub circ2edge(px,py,nx,ny,r, x1,y1,x2,y2,t)
// escape if circle is moving towards negative side of edge
if ((x1-px)*(y2-py) - (x2-px)*(y1-py) < 0.0) return t
// normalise edge
local ax,ay,bx,by, adj,opp,hyp
r = r + epsilon
adj = x2 - x1
opp = y2 - y1
hyp = r / sqrt(adj*adj + opp*opp)
ax = px + opp*hyp
ay = py - adj*hyp
bx = nx + opp*hyp
by = ny - adj*hyp
// escape if circle is moving away from edge
local denom, nume_a
denom = (y2-y1)*(bx-ax) - (x2-x1)*(by-ay)
nume_a = ((x2-x1)*(ay-y1) - (y2-y1)*(ax-x1)) / denom
if (nume_a < 0.0) return t
// find closest point on polygon to px,py,nx,ny
local nume_b, nt
nume_b = ((bx-ax)*(ay-y1) - (by-ay)*(ax-x1)) / denom
if (nume_b < 0.0) then nume_b = 0.0
elsif (nume_b > 1.0) then nume_b = 1.0
endif
xi = x1 + nume_b*(x2-x1)
yi = y1 + nume_b*(y2-y1)
// cast ray px,py,nx,ny on to circle xi,yi,r
local A,B,C,D, cx,cy,dx,dy
A = (nx-px)*(nx-px) + (ny-py)*(ny-py)
B = (nx-px)*(px-xi) + (ny-py)*(py-yi)
C = (px-xi)*(px-xi) + (py-yi)*(py-yi) - r*r
D = (B+B)*(B+B) - (A+A)*(C+C)
if (D < 0) return t
if (D > 0) D = sqrt(D)
nt = (-(B+B) - D) / (A+A)
if (nt > t) return t
local slide
ix = px + nt*(nx-px)
iy = py + nt*(ny-py)
// calculate sliding plane normal
a = ix - xi
o = iy - yi
h = 1/sqrt(a*a + o*o)
a = a * h
o = o * h
cx = ix
cy = iy
dx = ix - o*r
dy = iy + a*r
slide = ((nx-cx)*(dx-cx) + (ny-cy)*(dy-cy)) / ((dx-cx)*(dx-cx) + (dy-cy)*(dy-cy))
d = sqrt((nx-ix)*(nx-ix) + (ny-iy)*(ny-iy))
// calculate new motion vector
// a*d*bounce , o*d*bounce this should be the normal depth below surface no the actual depth
// as it stands we are creating energy that did not exist before
vx = (dx-cx)*slide*friction + a*d*bounce
vy = (dy-cy)*slide*friction + o*d*bounce
return nt
end sub