Circle Line Segment Collision Detection & Response

[rain_storm]

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:

Code: [Select]

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 :

Code: [Select]

  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

[Stonemonkey]

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.

[rain_storm]

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

Code: [Select]

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


[rain_storm]

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

[p01]

You could precalc a SQRT LUT too, or use a fast INV_SQRT approximation if your language allows such tricks.

HTH

[rain_storm]

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

[Jim]

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.

You can still do the trick in yabasic, though it doesn't necessarily speed anything up

Code: [Select]

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

[rain_storm]

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

[Jim]

The code I posted *is* a yabasic implementation!  No shifts, no repeat loop.

Jim

[rain_storm]

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?

Code: [Select]

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

[Jim]

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

[Clanky]

wow! Some pretty hard-core math being done here! 

haha. Nice stuff Rain... nice bit of code I've never seen done before in Yabasic.

[rain_storm]

=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

[rain_storm]

 
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
 

[Clanky]

Haha! Nice... *don't ask me about it! have no clue *

Checked out the code you uploaded! That is awesome!!! Feels gamely - K+ for that.
--I had to change the

Code: [Select]

c2 = peek("port2")
to

Code: [Select]

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

Great Though!!!

[rain_storm]

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

[rain_storm]

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.

Code: [Select]

// 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



[rain_storm]

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)

Code: [Select]

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

Code: [Select]

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



[Voltage]

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.

[rain_storm]

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

Code: [Select]

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