Hello FreeBASIC enthusiasts. For a while now I have been wrestling with getting

collision detection working in my attempt to write a 3D game in FreeBASIC and

OpenGL. I've probably deleted and rewritten the sorry thing three or four times

now but, since it now more-or-less works I thought I'd run through how it works

while it's still fresh in my memory. Mods, if you think this thread is in the

wrong forum, feel free to move it.

The algorithm I will go through does two things:

* Collision detection. Have I hit something?

* Collision reaction. How do I respond to hitting an obstruction?

The algorithm is, I think, elegant and surprisingly short at its core; most of

the length comes from bookkeeping. It even automagically handles things like

walking up stairs.

The basis of the idea comes from Kasper Fauerby's guide, which you can get at

http://www.peroxide.dk/papers/collision/collision.pdf. Unfortunately, my

attempts to directly transcribe his C++ code were failures and I was not able

to get his examples to work. It is a neat explanation though, and I recommend

you read through it and refer to it while reading my guide.

Now just a few more preliminaries.

* Some knowledge of linear algebra is necessary.

* I am working in OpenGL's coordinate system, in which the X and Z axes are

east-west and north-south respectively, and Y is up-down. If you're in a

Z-vertical system you'll need to tweak the matrix stuff later on, but aside

from that, nothing is OpenGL specific.

* I use functions vecadd, vecsub, crossprod, dotprod, vecnorm to do the usual

vector operations; vecmul for multiplying a vector by a scalar; and

matmultvec for premultiplying a vector by a 3x3 matrix. I won't give code for

these. They're easy enough to code up and you could use operator overloading

instead if you like; as a matter of personal taste I prefer functions.

* The algorithm works on triangles. If, like mine, your actual world geometry

is made of quads, you'll need to split each quad into tris and feed those

into the algorithm. Just make sure the quads are planar, convex, and have

no adjacent parallel edges and you'll be fine.

The basic idea behind the algorithm is this: we represent the player by a

traxial ellipsoid with the player's coordinates at the center and three radii:

half of Mr. Sphere's width, half his height, and half his depth. This might

be, say, (0.35, 0.90, 0.12)metres for vaguely humanoid proportions. Then we

transform the world temporarily, or at least that part of it you can possibly

collide with, into a new coordinate system in which the ellipsoid becomes a

sphere with radius one.

The reason for this is that, if you collide with something, the collision will

happen at a single point on the sphere's surface. And the line joining the

center of the sphere to the collision point is perpendicular to the tangent

plane to the sphere at the collision point. Of all ellipsoids, only the sphere

has this property and it's crucial because the player will slide along this

tangent plane if he hits an obstruction.

The basic sequence is this:

* Player tries to move

* transform player and world into the fancy new coordinates

* If the player hits something, move up to the obstruction

and slide along it parallel to the tangent plane

* Check again to see if Mr. Sphere hits anything else while sliding

* transform back to the usual coordinates

That's the end of post 1. Read on, and we will finally get to some code!

## Collision detection in 3D

### Re: Collision detection in 3D

From now on I'll call the original space R3 and the scaled one eSpace.

So how do we perform this coordinate transform? Just scale the coordinates of

all the points in the world along the axes of the ellipsoid. That is, if your

original ellipsoid has a radius of 0.5 along the x axis you need to multiply

all the x-coordinates of the objects making up the world by 2 to create a

new world in which the radius of the sphere is 1. Then do similar for the

other two dimensions.

We need to be careful though: just dividing by x, y, and z radii respectively

only works if the ellipsoid has not been rotated from its original orientation.

This is something that Fauerby didn't go into, and probably part of the reason

I couldn't get his code to work right. Or if he did, I couldn't understand what

he did. Anyway. In my game, the player will only ever rotate around his Y axis.

He does not "nod" around the X axis, or "tilt" around his Z axis. This makes

the upcoming matrix manipulations easier, but if you have rotation in all three

dimensions you'll have to do a lot of tedious and annoying trigonometry.

Here is how to make a 3x3 matrix which, when applied to a point in R3 space

gives the point's coordinates in eSpace.

Hopefully that's comprehendable. The Y dimension just gets scaled according to

the Y size of the ellipsoid. The X and Z dimensions pick up contributions from

both the width and the depth of the ellipsoid, in proportions determined by the

compass heading of the player.

You will also need to transform back from eSpace into R3 at the end. This is

merely the matrix inverse of the "to eSpace" matrix, and here it is:

Onwards!

So how do we perform this coordinate transform? Just scale the coordinates of

all the points in the world along the axes of the ellipsoid. That is, if your

original ellipsoid has a radius of 0.5 along the x axis you need to multiply

all the x-coordinates of the objects making up the world by 2 to create a

new world in which the radius of the sphere is 1. Then do similar for the

other two dimensions.

We need to be careful though: just dividing by x, y, and z radii respectively

only works if the ellipsoid has not been rotated from its original orientation.

This is something that Fauerby didn't go into, and probably part of the reason

I couldn't get his code to work right. Or if he did, I couldn't understand what

he did. Anyway. In my game, the player will only ever rotate around his Y axis.

He does not "nod" around the X axis, or "tilt" around his Z axis. This makes

the upcoming matrix manipulations easier, but if you have rotation in all three

dimensions you'll have to do a lot of tedious and annoying trigonometry.

Here is how to make a 3x3 matrix which, when applied to a point in R3 space

gives the point's coordinates in eSpace.

Code: Select all

`function make_to_eSpace_matrix( ellipsoid as point3d, heading as single ) as matrix3d`

'constructs a suitable matrix3d from the player's bounding ellipsoid and

'their heading angle

dim as matrix3d ret

dim as single theta = heading * pion180 'openGL measures angles in degrees, trig functions in radians

with ret

'I wrapped the 3x3 array in a typedef. More verbose, but also clearer

.A(0,0) = cos(theta)/ellipsoid.x : .A(0,1) = 0.0 : .A(0,2) = -sin(theta)/ellipsoid.z

.A(1,0) = 0.0 : .A(1,1) = 1.0/ellipsoid.y : .A(1,2) = 0.0

.A(2,0) = sin(theta)/ellipsoid.x : .A(2,1) = 0.0 : .A(2,2) = cos(theta)/ellipsoid.z

end with

return ret

end function

Hopefully that's comprehendable. The Y dimension just gets scaled according to

the Y size of the ellipsoid. The X and Z dimensions pick up contributions from

both the width and the depth of the ellipsoid, in proportions determined by the

compass heading of the player.

You will also need to transform back from eSpace into R3 at the end. This is

merely the matrix inverse of the "to eSpace" matrix, and here it is:

Code: Select all

`function invert_eSpace_matrix( M as matrix3d ) as matrix3d`

'finds the matrix inverse of a to_e_mat matrix

'faster than a generic matrix inverse because

'this one has entries only on the diagonal and anti-diagonal

'and that itself is a consequence of *my* ellipsoid never having a

'sideways tilt or a forward/backward nod.

dim as matrix3d ret

dim as single denom = M.A(2,2)*M.A(0,0)-M.A(2,0)*M.A(0,2) 'don't worry! singularity impossible!

with ret

.A(0,0) = M.A(2,2)/denom : .A(0,1) = 0.0 : .A(0,2) = -M.A(0,2)/denom

.A(1,0) = 0.0 : .A(1,1)=1.0/M.A(1,1) : .A(1,2) = 0.0

.A(2,0) = -M.A(2,0)/denom : .A(2,1) = 0.0 : .A(2,2) = M.A(0,0)/denom

end with

return ret

end function

Onwards!

### Re: Collision detection in 3D

Now the next stage, and possibly the trickiest: detecting whether Mr. Sphere

has bumped up against a triangle. There are three possible collision types.

You can hit the face of the triangle, an edge, or a corner. Each of these needs

to be checked for.

The first thing to do is pass the algorithm information about the player's

intended movement: that is, the initial position and the requested displacement

vector. Fauerby's paper calls the latter a velocity, but I think that's a bit

misleading. It will typically be the real-world velocity of the player times

the current timestep.

Put another way, in the absence of a collision

final position = initial position + intended displacement

If there is a collision, it will happen at some fraction of the intended path,

in the range [0,1]

final position = initial position + t * intended displacement

+ some amount of sliding around

Now is a good time to introduce a bit of bookkeeping, a UDT containing

information passed into the algorithm and given back by it. Don't worry about

most of the entries for now. Currently we only care about some of them, and I

will get back to the others later, including the reason for the secondary UDT

inside it.

Make an instance of this UDT called colpak, calculate the player's

transformation matrices, use to_e_mat to calculate her coordinates and

requested displacement in eSpace, and store all of this info in colpak.

Now we want to look for the *first* collision of the sphere with any part of

a given triangle. Read on!

has bumped up against a triangle. There are three possible collision types.

You can hit the face of the triangle, an edge, or a corner. Each of these needs

to be checked for.

The first thing to do is pass the algorithm information about the player's

intended movement: that is, the initial position and the requested displacement

vector. Fauerby's paper calls the latter a velocity, but I think that's a bit

misleading. It will typically be the real-world velocity of the player times

the current timestep.

Put another way, in the absence of a collision

final position = initial position + intended displacement

If there is a collision, it will happen at some fraction of the intended path,

in the range [0,1]

final position = initial position + t * intended displacement

+ some amount of sliding around

Now is a good time to introduce a bit of bookkeeping, a UDT containing

information passed into the algorithm and given back by it. Don't worry about

most of the entries for now. Currently we only care about some of them, and I

will get back to the others later, including the reason for the secondary UDT

inside it.

Code: Select all

`type coll_info`

found as boolean 'have we found a collision?

epoint as point3d 'where in eSpace is the collision?

distance as single 'how far does Mr. Sphere have to move to make that collision happen?

id as ubyte '0=face, 1=vert, 2=edge (unnecessary but useful for debugging)

end type

type collision_packet

'this UDT does bookkeeping for the detection and handling of

'collisions in 3d space

'----- Info about the move being requested in real space

r3position as point3d ' player's initial position in R3

r3displace as point3d ' player's requested displacement vector in R3

r3force as point3d ' player's additional displacement due to external forces

'----- Info necessary to convert to/from eSpace

ellipsoid as point3d ' dimensions of player bounding ellipsoid in 3d space

' by definition this is (1,1,1) in eSpace

to_e_mat as matrix3d 'matrix for transforming points in real space to eSpace

from_e_mat as matrix3d 'matrix for transforming points in eSpace back to real space

'----- Info about the move being requested in eSpace

eSpacePos as point3d 'position in eSpace

eSpaceDis as point3d 'displacement in eSpace

'----- Was a collision detected? If so, where and how far away?

coll as coll_info

end type

Make an instance of this UDT called colpak, calculate the player's

transformation matrices, use to_e_mat to calculate her coordinates and

requested displacement in eSpace, and store all of this info in colpak.

Now we want to look for the *first* collision of the sphere with any part of

a given triangle. Read on!

### Re: Collision detection in 3D

If a sphere hits the interior of a triangle, it will do so *before* it ever

strikes an edge or a vertex. Play around with a marble and paper scraps if you

like and you'll see it's true.

Now, a triangle is just a small part of an infinite plane containing its three

corners so first we'll see if the sphere collides with that infinite plane.

Then, if it does, we check if the candidate collision point actually is inside

the triangle.

We seek where the distance between the plane and the sphere is equal to 1.

First create the triangle plane (I assume you have a plane UDT specified

by a normal and a point within the plane):

To get the distance to the plane, use this function. It calculates the distance

between the sphere center and the plane, returning a positive quantity if the

plane normal is pointing in your direction and a negative quantity if it is

pointing away from you.

Now we seek t0, the time at which the sphere touches the plane. Strictly, t0

is not actually a time but a unitless quantity representing what fraction of

the requested move actually gets carried out, but it is sufficiently time-like

in this context. All we have to do is solve this equation:

triPlaneNormal · (eSpacePos + t*eSpaceDis) + Cp = ±1

where Cp is the plane constant for the triangle plane (recall that another

way of defining a plane is by its normal and its distance from the origin

along the normal: this distance is the plane constant Cp). This can be solved

for t0 and t1:

Check out page 12 of Fauerby for a more thorough derivation of this.

Now we've got t0: the fraction of the requested move we get to make. For

a face collision to occur, t0 must be in the range [0,1]. If t0>1 then we are

approaching the plane but will not hit it this turn. If t0<0 then either we are

receding from the plane and won't hit it, or we are embedded in the *infinite*

triPlane. In the latter case there might still be a collision with an edge or

a vertex.

Finally, a function for testing whether our candidate collision point is

inside the triangle.

Just one more subtlety. If triPlaneNormal is perpendicular to eSpaceDis *AND*

te distance between sphere and triPlane>1.0 then no collision can occur because

we are travelling parallel to the plane, and can quit early, before get_t0t1

can whinge about division by zero.

To sum up:

*If our motion is parallel to the triPlane, we can quit early

*Otherwise, if our intersection point with the infinite triPlane

is inside the triangle

*AND t0 is between zero and one, then we have a collision!

strikes an edge or a vertex. Play around with a marble and paper scraps if you

like and you'll see it's true.

Now, a triangle is just a small part of an infinite plane containing its three

corners so first we'll see if the sphere collides with that infinite plane.

Then, if it does, we check if the candidate collision point actually is inside

the triangle.

We seek where the distance between the plane and the sphere is equal to 1.

First create the triangle plane (I assume you have a plane UDT specified

by a normal and a point within the plane):

Code: Select all

`function tri_to_plane( tri() as point3d ) as plane`

'given three points, returns the unique plane containing them

dim as plane ret

ret.origin = tri(0) 'pick one of the given points since by definition it is in the plane

ret.normal = normalise_vector(crossprod(vecsub(tri(1), tri(0)), vecsub(tri(2), tri(0))))

return ret

end function

To get the distance to the plane, use this function. It calculates the distance

between the sphere center and the plane, returning a positive quantity if the

plane normal is pointing in your direction and a negative quantity if it is

pointing away from you.

Code: Select all

`function signed_distance( Q as plane, p as point3d ) as single`

'finds the distance between a point and a plane

dim as double Cp = -dotprod(Q.normal, Q.origin)

return Cp + dotprod(p, Q.normal)

end function

Now we seek t0, the time at which the sphere touches the plane. Strictly, t0

is not actually a time but a unitless quantity representing what fraction of

the requested move actually gets carried out, but it is sufficiently time-like

in this context. All we have to do is solve this equation:

triPlaneNormal · (eSpacePos + t*eSpaceDis) + Cp = ±1

where Cp is the plane constant for the triangle plane (recall that another

way of defining a plane is by its normal and its distance from the origin

along the normal: this distance is the plane constant Cp). This can be solved

for t0 and t1:

Code: Select all

`sub get_t0t1( eSpacePos as point3d, eSpaceDis as point3d, triPlane as plane, t0 as single ptr, t1 as single ptr)`

'gets the two moments at which the sphere is at distance 1 from the plane

'the sign depends on the direction of the plane normal, so exchanging the order

'might be necessary

dim as single aa = ( 1.0 - signed_distance(triPlane,eSpacePos))/dotprod( triPlane.normal, eSpaceDis )

dim as single bb = (-1.0 - signed_distance(triPlane,eSpacePos))/dotprod( triPlane.normal, eSpaceDis )

dim as single swp

if aa>bb then

swp = bb

bb = aa

aa = swp

end if

*t0 = aa

*t1 = bb

end sub

Check out page 12 of Fauerby for a more thorough derivation of this.

Now we've got t0: the fraction of the requested move we get to make. For

a face collision to occur, t0 must be in the range [0,1]. If t0>1 then we are

approaching the plane but will not hit it this turn. If t0<0 then either we are

receding from the plane and won't hit it, or we are embedded in the *infinite*

triPlane. In the latter case there might still be a collision with an edge or

a vertex.

Finally, a function for testing whether our candidate collision point is

inside the triangle.

Code: Select all

`function point_in_tri( p1 as point3d, tri() as point3d ) as boolean`

'this function is for checking whether a given point is inside a given triangle

'Fauerby gives a fancy shmancy fast version of this that relies on bit fiddling

'but that seems to depend both on endianness and coordinates being 32-bit quantities,

'neither of which I feel safe in assuming. This is slower but less sketchy.

dim as single abac = vecnorm( crossprod( vecsub(tri(2),tri(0)), vecsub(tri(1),tri(0)) ) )

dim as single aa = vecnorm( crossprod( vecsub(tri(2),p1), vecsub(tri(1),p1) ) ) / abac

if aa < 0.0 orelse aa > 1.0 then

return false

endif

dim as single bb = vecnorm( crossprod( vecsub(tri(2),p1), vecsub(tri(0),p1) ) ) / abac

if bb < 0.0 orelse bb > 1.0 then

return false

endif

dim as single cc = 1.0 - aa - bb

if cc < 0.0 orelse cc > 1.0 then

return false

endif

return true

end function

Just one more subtlety. If triPlaneNormal is perpendicular to eSpaceDis *AND*

te distance between sphere and triPlane>1.0 then no collision can occur because

we are travelling parallel to the plane, and can quit early, before get_t0t1

can whinge about division by zero.

To sum up:

*If our motion is parallel to the triPlane, we can quit early

*Otherwise, if our intersection point with the infinite triPlane

is inside the triangle

*AND t0 is between zero and one, then we have a collision!

### Re: Collision detection in 3D

If we don't hit the triangle on its face, we might still collide with a corner

or an edge. Detecting a vert collision is relatively easy conceptually, but

comes with a little bit of bookkeeping.

All we need is, for any given vertex, to determine when and if its distance to

the sphere is equal to one. More precisely, if C(t) is the center of the sphere

at "time" t and p is the vertex in question we seek t0 for which |C(t0)-p)|=1.

Or, squaring both sides, (C(t0)-p)·(C(t0)-p)=1.

After some tedious algebra, this comes out to a quadratic equation for t0:

A t0^2 + B t0 + C = 0

where

A = eSpaceDis·eSpaceDis

B = 2(eSpaceDis·(eSpacePos-p))

C = |p-eSpacePos|^2 - 1

This, like all quadratics, will have two roots. One represents the first

collision between sphere and vertex. If we do nothing, the vert will go inside

the sphere and some time later come out again. That is the second root, and

we want to chuck it away. Of course, there may be no real roots at all. This

represents the case that the sphere does not ever collide with the vert.

We also need to discard a potential collision candidate if it happens *after*

any collisions with other previously examined edges or vertices. So, here is a

quadratic equation root finder that intelligently keeps only the best solution.

So much for a collision with a vertex. How about edges? The algebra is a bit

more complicated but conceptually not much worse than a vert. Given two points

defining an edge and another point for the sphere center, we want to know where

if ever the distance between the sphere center and the *infinite* line going

through the edge is equal to 1. If that ever happens, we want to know if that

point actually lies between the two endpoints of the edge. And if so, does that

happen for time between 0 and 1?

Now, if our sphere center p0 = eSpacePos + t*eSpaceDis for some t we're trying

to find and v1, v2 are the vertices defining the edge, then

|(p0-v1)-(p0-v1)·(v2-v1)/|v2-v1|^2 * (v2-v1)| = 1

or, if we let q = p0-v1 and edge = v2-v1 we end up with (work through this!)

q·q - (q·edge)^2 / |edge|^2 = 1

Which will end up being a quadratic equation for t, with coefficients

A = |edge·eSpaceDis|^2 - |edge|^2*|eSpaceDis|^2

B = 2*|edge|^2 * r·eSpaceDis-2*(eSpaceDis·edge)*(edge·r)

C = |edge|^2*(1.0-|r|^2) + (edge·r)^2

where r = eSpacePos - v1. The algebra is heinous.

We feed these into the lowest_positive_root function. If that returns false,

there is either no solution or we previously found a collision with earlier

t0. Either way, we can just continue on. If it returns true, we just need to

check whether the collision with the *infinite* line actually takes place

within the line *segment*, but that turns out to be easy.

If the quantity ((edge·eSpacedDis)*t - (edge·r))/|edge|^2 is between zero and

one, then the candidate collision happens between the two vertices.

If you have followed up to here, congratulations. The worst is behind you.

or an edge. Detecting a vert collision is relatively easy conceptually, but

comes with a little bit of bookkeeping.

All we need is, for any given vertex, to determine when and if its distance to

the sphere is equal to one. More precisely, if C(t) is the center of the sphere

at "time" t and p is the vertex in question we seek t0 for which |C(t0)-p)|=1.

Or, squaring both sides, (C(t0)-p)·(C(t0)-p)=1.

After some tedious algebra, this comes out to a quadratic equation for t0:

A t0^2 + B t0 + C = 0

where

A = eSpaceDis·eSpaceDis

B = 2(eSpaceDis·(eSpacePos-p))

C = |p-eSpacePos|^2 - 1

This, like all quadratics, will have two roots. One represents the first

collision between sphere and vertex. If we do nothing, the vert will go inside

the sphere and some time later come out again. That is the second root, and

we want to chuck it away. Of course, there may be no real roots at all. This

represents the case that the sphere does not ever collide with the vert.

We also need to discard a potential collision candidate if it happens *after*

any collisions with other previously examined edges or vertices. So, here is a

quadratic equation root finder that intelligently keeps only the best solution.

Code: Select all

`const ZERO_TOLERANCE = 1.0e-9 'WE WILL NOT PUT UP WITH ROUNDING ERRORS!!!`

'DIVISION BY ZERO IS VERBOTEN!!!

function lowest_positive_root( A as single, B as single, C as single, maxR as single, rptr as single ptr ) as boolean

'Finding collisions with triangle vertices or edges requires solving a quadratic and

'we always want to obtain the smallest positive of those roots. But we only want to

'return a new smallest positive if this is smaller than the result of collisions with

'previously tested verts or edges. So, we take A,B,C as the quadratic coefficients as usual

'but also track the value of the previous champion maxR and give back the actual root as rptr.

'function returns false if there is no root for this quadratic, or neither unseated the king,

'and true if the king was indeed dethroned.

dim as single det = B*B - 4.0*A*C

if det<0.0 orelse abs(A)<ZERO_TOLERANCE then

return false

end if

dim as single rdet = sqr(det)

dim as single r1 = (-B-rdet)/(2.0*A)

dim as single r2 = (-B+rdet)/(2.0*A)

dim as single temp

if r1>r2 then

temp = r2

r2 = r1

r1 = temp

end if

if r1<maxR andalso r1>0 then

*rptr=r1

return true

end if

return false

end function

So much for a collision with a vertex. How about edges? The algebra is a bit

more complicated but conceptually not much worse than a vert. Given two points

defining an edge and another point for the sphere center, we want to know where

if ever the distance between the sphere center and the *infinite* line going

through the edge is equal to 1. If that ever happens, we want to know if that

point actually lies between the two endpoints of the edge. And if so, does that

happen for time between 0 and 1?

Now, if our sphere center p0 = eSpacePos + t*eSpaceDis for some t we're trying

to find and v1, v2 are the vertices defining the edge, then

|(p0-v1)-(p0-v1)·(v2-v1)/|v2-v1|^2 * (v2-v1)| = 1

or, if we let q = p0-v1 and edge = v2-v1 we end up with (work through this!)

q·q - (q·edge)^2 / |edge|^2 = 1

Which will end up being a quadratic equation for t, with coefficients

A = |edge·eSpaceDis|^2 - |edge|^2*|eSpaceDis|^2

B = 2*|edge|^2 * r·eSpaceDis-2*(eSpaceDis·edge)*(edge·r)

C = |edge|^2*(1.0-|r|^2) + (edge·r)^2

where r = eSpacePos - v1. The algebra is heinous.

We feed these into the lowest_positive_root function. If that returns false,

there is either no solution or we previously found a collision with earlier

t0. Either way, we can just continue on. If it returns true, we just need to

check whether the collision with the *infinite* line actually takes place

within the line *segment*, but that turns out to be easy.

If the quantity ((edge·eSpacedDis)*t - (edge·r))/|edge|^2 is between zero and

one, then the candidate collision happens between the two vertices.

If you have followed up to here, congratulations. The worst is behind you.

### Re: Collision detection in 3D

Finally we can put all this together to test for a collision with a single

given triangle. Here it is. Most of it will be explained in the many comments

I have put in there.

That's it. You can now test the collision of Mr. Sphere with any given

triangle. It's relatively smooth sailing from here. All that remains

is to loop through all the triangles in the world, and to determine how

the player will respond to any collision that happens.

given triangle. Here it is. Most of it will be explained in the many comments

I have put in there.

Code: Select all

`sub check_triangle( tri() as point3d, colpak as collision_packet ptr )`

'only two arguments; most of what we need is in colpak

'the triangle vertices are in eSpace coordinates

dim as plane triPlane = tri_to_plane( tri() )

dim as single NdotDis = dotprod( triPlane.normal, colpak->eSpaceDis )

dim as single t_collision = 1.0 'unitless "time" of collision

dim as single newT, f0

dim as ubyte coltype 'for debugging

dim as boolean collisionFound = false 'have we hit this triangle yet?

dim as point3d collisionPoint

dim as point3d edge, sphereToVert, rr

dim as ubyte i, j

dim as byte normsign

dim as single qA, qB, qC 'for quadratic formula

dim as single edge2, posedge, disedge, edgerr

dim as single posdis = dotprod(colpak->eSpacePos, colpak->eSpaceDis)

dim as single pos2 = dotprod( colpak->eSpacePos, colpak->eSpacePos )

dim as single dis2 = dotprod( colpak->eSpaceDis, colpak->eSpaceDis )

'-----CHECK FOR FACE COLLISION-----

dim as single distToTriPlane = signed_distance( triPlane, colpak->eSpacePos )

if abs(NdotDis)<ZERO_TOLERANCE andalso abs(distToTriPlane)>=1.0 then

return 'we're moving parallel to the triangle, but

'not inside the *infinite* triplane so no collision can happen

end if

normsign = Iif(distToTriPlane<0,+1,-1) 'keeps track of whether the triplane

'normal is facing towards the player

'or away from the player

dim as single t0, t1

get_t0t1 colpak->eSpacePos, colpak->eSpaceDis, triPlane, @t0, @t1

if abs(distToTriPlane)<1.0 then 'We are already inside the infinite TriPlane, but not inside the triangle itself

t0=0.0 'we won't get a face collision, but this MIGHT still portend an edge or

t1=1.0 'vert collision

end if

if t0>1.0 orelse t1<0.0 then

return 'can't reach the plane on this turn, so give up

end if

dim as point3d planeIntersectionPoint = _

vecadd(vecadd( colpak->eSpacePos, vecmult(triPlane.normal,normsign)), vecmult(colpak->eSpaceDis,t0))

if point_in_tri( planeIntersectionPoint, tri() ) andalso t0 > 0 then 'we have a collision

collisionFound = true

t_collision = t0

collisionPoint = planeIntersectionPoint

coltype = 0

goto finalise_colpak 'I won't apologise for using a goto

end if

'-----TEST VERTS AND FACES----

posdis = dotprod(colpak->eSpacePos, colpak->eSpaceDis)

for i = 0 to 2

'this is a little less verbose than in Fauerby because the verts are in an array

'and I depart from his ordering by going vert-edge-vert-edge-vert-edge instead

'of vert-vert-vert-edge-edge-edge

sphereToVert = vecsub(colpak->eSpacePos, tri(i) )

qA = dis2

qB = 2.0*dotprod( colpak->eSpaceDis, sphereToVert )

qC = dotprod( sphereToVert, sphereToVert ) - 1.0

if lowest_positive_root( qA, qB, qC, t_collision, @newT ) then 'lpr will only return true if it finds a positive root

'smaller than t_collision, the size of the smallest previous root

t_collision = newT

collisionFound = true

collisionPoint = tri(i)

coltype = 2

end if

'now edges

j = (i+1) mod 3

edge = vecsub( tri(j), tri(i) )

edge2 = dotprod( edge, edge )

posedge = dotprod( colpak->eSpacePos, edge )

disedge = dotprod( colpak->eSpaceDis, edge )

rr = vecsub(tri(i), colpak->eSpacePos)

edgerr = dotprod(edge, rr)

qA = disedge*disedge - edge2*dis2

qB = 2*edge2*dotprod(colpak->eSpaceDis, rr)-2*disedge*edgerr

qC = edge2*(1.0-dotprod(rr,rr)) + edgerr*edgerr

if lowest_positive_root( qA, qB, qC, t_collision, @newT ) then

f0 = (disedge*newT-edgerr)/edge2

if (f0>=0.0 and f0<=1.0) then

t_collision = newT

collisionFound = true

collisionPoint = vecadd(tri(i), vecmult(edge, f0))

coltype = 1

end if

end if

next i

finalise_colpak:

if collisionFound then

dim as single distToCollision = t_collision*vecnorm(colpak->eSpaceDis)

'does this triangle qualify as closest hit?

'it does if it's the first triangle found to be hit, or closer than the previous champ

if (colpak->coll.found=false) orelse (distToCollision < colpak->coll.distance) then

colpak->coll.distance = distToCollision

colpak->coll.epoint = collisionPoint

colpak->coll.found = true

colpak->coll.id = coltype 'for debugging

end if

end if

'phew!

end sub

That's it. You can now test the collision of Mr. Sphere with any given

triangle. It's relatively smooth sailing from here. All that remains

is to loop through all the triangles in the world, and to determine how

the player will respond to any collision that happens.

### Re: Collision detection in 3D

Just as we used the arguments in lowest_positive_root to keep track of the

first collision among a single triangle's vertices and edges, we will use the

colpak structure to store the information about the earliest collision among

all the triangles tested so far. That goes into colpak.coll.epoint and

colpak.coll.distance, and the final part of check_triangle shows you how to

handle that.

Now the only thing left is to decide how to move if a collision has been found.

We use the fact I mentioned way back in my first post: the tangent plane to a

sphere is perpendicular to its radius. This tangent plane is going to be the

plane we slide along. All we're going to do is move the player up to the

collision and back away just a tiny amount to avoid both embedding her in an

object, or collising with the same edge again and again and again. This last

point is crucial for stairs, as you'll see soon.

We then subtract the the new player position from the point she'd have reached

in the absence of a collision and project that onto the tangent plane.

The projection is a *new move request*, shorter than the original one. That's

right, we are going to call this function recursively. Why not just have one

skid and call it a day? Because it's possible to hit something else on this

shorter move. Maybe the player has gotten wedged into a corner or something

like that.

And the sliding plane itself is simply defined by the path from sphere center

to collision point as its normal, and the collision point as its origin. Simple

huh?

Here is the function you need:

It's a bit lengthy but not too complicated. It loops through every triangle in

the world, finds the earliest collision among them, calculates a slidePlane,

and then requests a new move parallel to it. Then it gets called recursively

in case the new move would collide with something new until the next move is

very very small.

At this point you may see how this algorithm gives us stair climbing for free.

If you walk up to a step, your sphere is most likely going to bump against the

lip (an edge) of the step, and the sliding plane will be at a shallow angle to

the floor. That means sliding up it will take you up over the lip and onto the

step. If that's hard to picture in your mind, play around with a tennis ball

and a matchbox as a visual aid.

We're now almost there.

first collision among a single triangle's vertices and edges, we will use the

colpak structure to store the information about the earliest collision among

all the triangles tested so far. That goes into colpak.coll.epoint and

colpak.coll.distance, and the final part of check_triangle shows you how to

handle that.

Now the only thing left is to decide how to move if a collision has been found.

We use the fact I mentioned way back in my first post: the tangent plane to a

sphere is perpendicular to its radius. This tangent plane is going to be the

plane we slide along. All we're going to do is move the player up to the

collision and back away just a tiny amount to avoid both embedding her in an

object, or collising with the same edge again and again and again. This last

point is crucial for stairs, as you'll see soon.

We then subtract the the new player position from the point she'd have reached

in the absence of a collision and project that onto the tangent plane.

The projection is a *new move request*, shorter than the original one. That's

right, we are going to call this function recursively. Why not just have one

skid and call it a day? Because it's possible to hit something else on this

shorter move. Maybe the player has gotten wedged into a corner or something

like that.

And the sliding plane itself is simply defined by the path from sphere center

to collision point as its normal, and the collision point as its origin. Simple

huh?

Here is the function you need:

Code: Select all

`const SAFETY_MARGIN = 0.00001 'very tiny margin to make sure to prevent embedding myself in an obstacle`

const RECURSE_DEPTH = 5 'only do five recursions. after that, the new requested move will be too

'tiny to bother with

function collideWithWorld( byval epos as point3d, byval edis as point3d, byval recurse as ubyte, colpak as collision_packet ptr, byval tris as tri_list ptr) as point3d

'recursively checks the requested movement for collisions with the world. Why recursive? Because

'sliding along the obstruction might produce new, unforeseen, collisions with other triangles.

if recurse > RECURSE_DEPTH then

return epos 'any more steps will be extremely tiny

end if

colpak->coll.found = false

colpak->eSpacePos = epos

colpak->eSpaceDis = edis

dim as tri_list ptr curr = tris

dim as point3d evert(3) 'the eSpace vertices of the currently tested triangle

dim as ubyte ii

while not curr = NULL 'loop through all the triangles making up the world

'I use a linked list of tris, the details will be different

'in your implementation but conceptually the same

for ii=0 to 2

evert(ii) = matmultvec(colpak->to_e_mat, curr->vert(ii)) 'do the conversion

next ii

check_triangle( evert(), colpak )

curr = curr->nxt

wend

if not colpak->coll.found then 'if no collision, then just move normally

return vecadd( epos, edis )

end if

'if collision was detected then we need to do things

dim as point3d dest = vecadd( epos, edis ) 'prospective sphere destination point

dim as point3d newpos = epos

' only update if we are not within the tolerance on distance

' and if so, only move close to it, not right to it. this helps avoid getting embedded

if colpak->coll.distance >= SAFETY_MARGIN then

dim as point3d V = edis

V=normalise_vector(V)

V = vecmult(V, colpak->coll.distance - SAFETY_MARGIN)

newpos = vecadd( colpak->eSpacePos, V )

V = normalise_vector(V)

colpak->coll.epoint = vecsub(colpak->coll.epoint, vecmult(V, SAFETY_MARGIN) )

end if

'determine the sliding plane. We will produce a new displacement vector parallel to it

'to make the player skid along an obstacle they bumped into

dim as point3d slidePlaneOrigin = colpak->coll.epoint

dim as point3d slidePlaneNormal = vecsub(newpos, colpak->coll.epoint)

dim as plane slidePlane

slidePlane.normal = slidePlaneNormal

slidePlane.origin = slidePlaneOrigin

'project the remainder of the requested move onto the sliding plane.

dim as point3d remainder = vecsub(dest, newpos)

dim as point3d newdis = crossprod( slidePlaneNormal, crossprod( remainder, slidePlaneNormal) )

dim as point3d newdest = vecadd(newpos, newdis)

return collideWithWorld( newpos, newdis, recurse+1, colpak, tris ) 'otherwise check to see if we

'bump into anything else while sliding along

end function

It's a bit lengthy but not too complicated. It loops through every triangle in

the world, finds the earliest collision among them, calculates a slidePlane,

and then requests a new move parallel to it. Then it gets called recursively

in case the new move would collide with something new until the next move is

very very small.

At this point you may see how this algorithm gives us stair climbing for free.

If you walk up to a step, your sphere is most likely going to bump against the

lip (an edge) of the step, and the sliding plane will be at a shallow angle to

the floor. That means sliding up it will take you up over the lip and onto the

step. If that's hard to picture in your mind, play around with a tennis ball

and a matchbox as a visual aid.

We're now almost there.

### Re: Collision detection in 3D

The last thing we need is a wrapper routine for all of this stuff that you can

call from your main game loop. First reset colpak with suitable values:

Some of the details will depend on your particular implementation, but the

essentials are: update colpak with the player's position, reset the

"found collision" flag, and update the matrices for going to and from eSpace.

Now call the algorithm:

You'll see a second invocation of collideWithWorld. It's often convenient to

split the player's motion up into components representing their own movement

and forces (such as gravity or flowing water) imposed from the outside world.

Getting things that happen from the player's own volition out of the way first

is a good idea. For one thing, it'll make it easier to climb steps.

That's about it. Put all those code bits together and you have a working (?)

collision detection and reaction algorithm. It compiles, and when I use it in

my own code I haven't fallen through the floor or gotten glued to a wall, or

launched myself into the stratosphere yet. I hope it will be of use.

Thanks for reading this far. Suggestions for improving the clarity of the

writing, and bug corrections in the code will be gratefully received.

Enjoy!

call from your main game loop. First reset colpak with suitable values:

Code: Select all

`sub reset_colpak( byref colpak as collision_packet, byval player_position as point3d, byval player_velocity as point3d,_`

byval player_angle as angles, byval player_ellipsoid as point3d, byval extforce as point3d, dt as single )

with colpak

.r3position = player_position

.r3displace = vecmult(player_velocity, dt)

.r3force = extforce 'this shouldn't need to be changed often

.ellipsoid = player_ellipsoid 'this shouldn't need to be changed often. mostly just when player is ducking

with .coll

.found = false

.distance = 1000.0 'sentinel values. shouldn't be necessary, but just in case

.epoint = ZERO_3D

end with

.to_e_mat = make_to_eSpace_matrix( player_ellipsoid, player_angle.heading )

.from_e_mat = invert_eSpace_matrix( colpak.to_e_mat )

end with

end sub

Some of the details will depend on your particular implementation, but the

essentials are: update colpak with the player's position, reset the

"found collision" flag, and update the matrices for going to and from eSpace.

Now call the algorithm:

Code: Select all

`sub collideAndSlide( byref colpak as collision_packet ptr, tris as tri_list ptr )`

'Checks for a collision with the triangles making up the world

'and if there is a collision makes the player slide along the

'obstruction. There is a second call, to allow for external forces, which

'is best handled separately. Takes a pointer to the collision_packet

'and the list of triangles as input.

dim as point3d epos = matmultvec( colpak->to_e_mat, colpak->r3position )

dim as point3d edis = matmultvec( colpak->to_e_mat, colpak->r3displace )

'update based on player movement and geometry

dim as point3d final_epos = collideWithWorld( epos, edis, 0, colpak, tris ) 'this gets called recursively, so

'requires its own temp copies of

'position and velocity

'update based on external forces and geometry

colpak->r3position = matmultvec( colpak->from_e_mat, final_epos )

colpak->r3displace = colpak->r3force

colpak->eSpaceDis = matmultvec( colpak->to_e_mat, colpak->r3displace )

final_epos = collideWithWorld( final_epos, colpak->eSpaceDis, 0, colpak, tris )

colpak->eSpacePos = final_epos 'sphere has reached its destination in eSpace

colpak->r3position = matmultvec( colpak->from_e_mat, colpak->eSpacePos ) 'convert back to real coordinates

end sub

You'll see a second invocation of collideWithWorld. It's often convenient to

split the player's motion up into components representing their own movement

and forces (such as gravity or flowing water) imposed from the outside world.

Getting things that happen from the player's own volition out of the way first

is a good idea. For one thing, it'll make it easier to climb steps.

That's about it. Put all those code bits together and you have a working (?)

collision detection and reaction algorithm. It compiles, and when I use it in

my own code I haven't fallen through the floor or gotten glued to a wall, or

launched myself into the stratosphere yet. I hope it will be of use.

Thanks for reading this far. Suggestions for improving the clarity of the

writing, and bug corrections in the code will be gratefully received.

Enjoy!

### Re: Collision detection in 3D

Here is a quick little triangle intercept thing.

+z (mouse wheel forward) =into the screen.

Here is a little plane intercept demo I did a few years ago.(no triangles involved)

As usual the demo via fbc 32 bits is much smoother than fb 64 bits.

Could you provide a little demo for your suggestions perhaps?

+z (mouse wheel forward) =into the screen.

Code: Select all

Screen 20,32,,64

Dim Shared As Integer xres,yres

Screeninfo xres,yres

Type vector

As Single x,y,z

#define vct Type<vector>

#define dot *

#define cross ^

End Type

Type plane

As vector v1,v2,v3

End Type

Operator + (v1 As vector,v2 As vector) As vector

Return vct(v1.x+v2.x,v1.y+v2.y,v1.z+v2.z)

End Operator

Operator -(v1 As vector,v2 As vector) As vector

Return vct(v1.x-v2.x,v1.y-v2.y,v1.z-v2.z)

End Operator

Operator * (f As Single,v1 As vector) As vector 'scalar*vector

Return vct(f*v1.x,f*v1.y,f*v1.z)

End Operator

Operator * (v1 As vector,v2 As vector) As Single 'dot product

Return v1.x*v2.x+v1.y*v2.y+v1.z*v2.z

End Operator

Operator ^ (v1 As vector,v2 As vector) As vector 'cross product

Return vct(v1.y*v2.z-v2.y*v1.z,-(v1.x*v2.z-v2.x*v1.z),v1.x*v2.y-v2.x*v1.y)

End Operator

Function length(v As vector) As Single

Return Sqr(v.x*v.x+v.y*v.y+v.z*v.z)

End Function

Function normalize(v As vector) As vector

Dim n As Single=length(v)

If n=0 Then n=1e-20

Return vct(v.x/n,v.y/n,v.z/n)

End Function

Function inpolygon(p1() As vector,Byval p2 As vector) As Integer 'is a point in a polygon?

#define Winder(L1,L2,p) ((L1.x-L2.x)*(p.y-L2.y)-(p.x-L2.x)*(L1.y-L2.y))

Dim As Integer index,nextindex,k=Ubound(p1)+1,wn

For n As Integer=1 To Ubound(p1)

index=n Mod k:nextindex=(n+1) Mod k

If nextindex=0 Then nextindex=1

If p1(index).y<=p2.y Then

If p1(nextindex).y>p2.y Andalso Winder(p1(index),p1(nextindex),p2)>0 Then wn+=1

Else

If p1(nextindex).y<=p2.y Andalso Winder(p1(index),p1(nextindex),p2)<0 Then wn-=1

End If

Next n

Return wn

End Function

Function planedistance(S As PLANE,p As vector,Byref ip As vector=vct(0,0,0)) As Single

Dim As vector unitcross=normalize((s.v1-s.v2) cross (S.v2-S.v3))

Dim As Single dist=unitcross dot (p-s.v1)'pv

Dim As vector ip1=p+dist*unitcross

Dim As Single d1=length(s.v1-ip1)

unitcross=-1*unitcross

Dim As vector ip2=p+dist*unitcross

Dim As Single d2=length(s.v1-ip2)

If d1 <= d2 Then ip=ip1 Else ip=ip2

Return dist

End Function

Function intriangle(t As plane,p As vector,i As vector=(0,0,0)) As Long

planedistance(t,p,i)

Dim As vector a(1 To 3)={t.v1,t.v2,t.v3}

Return inpolygon(a(),i)

End Function

Function perspective(p As vector,eyepoint As vector) As vector

Dim As Single w=1+(p.z/eyepoint.z)

If w=0 Then w=1e-20

Return vct((p.x-eyepoint.x)/w+eyepoint.x,(p.y-eyepoint.y)/w+eyepoint.y,(p.z-eyepoint.z)/w+eyepoint.z)

End Function

Function drawplane(p As plane) As vector 'returns centroid

Line(p.v1.x,p.v1.y)-(p.v2.x,p.v2.y)

Line(p.v2.x,p.v2.y)-(p.v3.x,p.v3.y)

Line(p.v3.x,p.v3.y)-(p.v1.x,p.v1.y)

Draw String(p.v1.x,p.v1.y),"("+Str(p.v1.x)+","+Str(p.v1.y)+","+Str(p.v1.z)+")"

Draw String(p.v2.x,p.v2.y),"("+Str(p.v2.x)+","+Str(p.v2.y)+","+Str(p.v2.z)+")"

Draw String(p.v3.x,p.v3.y),"("+Str(p.v3.x)+","+Str(p.v3.y)+","+Str(p.v3.z)+")"

Return Type((p.v1.x+p.v2.x+p.v3.x)/3,(p.v1.y+p.v2.y+p.v3.y)/3,(p.v1.z+p.v2.z+p.v3.z)/3)'centroid

End Function

Dim As plane p=Type((20,20,0),(500,300,0),(200,500,0))

Dim As vector v

Dim As vector e=Type(512,768\2,200) 'start eyepoint

Dim As Long mx,my,mw

Do

Getmouse mx,my,mw

Var m=Type<vector>(mx,my,0)

p.v2.z=mw 'a vertex of the triangle has z value=mouse wheel

Dim As plane p3d=Type(perspective(p.v1,e),perspective(p.v2,e),perspective(p.v3,e)) 'for drawing

Screenlock

Cls

e=drawplane(p3d):e.z=500 'new eyepoint for perspective

Locate 10,50

Print "mouse at ";"("+Str(m.x)+","+Str(m.y)+","+Str(m.z)+")"

Var d=intriangle(p,m,v) 'v is actual intercept with plane

Dim As vector pv=perspective(v,e) 'for drawing intercept

Circle(pv.x,pv.y),5 'intercept with plane via perspective

If d Then 'if intercepts the triangle

Draw String(pv.x,pv.y),"("+Str(v.x)+","+Str(v.y)+","+Str(v.z)+")"

Line(m.x,m.y)-(pv.x,pv.y)

Locate 12,50

Print "distance from mouse ";planedistance(p,m)

End If

Screenunlock

Sleep 1,1

Loop Until Inkey=Chr(27)

Sleep

Here is a little plane intercept demo I did a few years ago.(no triangles involved)

Code: Select all

`Screen 20,32,,64`

Dim Shared As Integer xres,yres

Screeninfo xres,yres

'Dim Shared As Any Pointer im

Type vector

As Single x,y,z

#define vct Type<vector>

#define dot *

#define cross ^

End Type

Type plane

As vector v1,v2,v3

End Type

Type _object

As vector position,velocity

As Single mass,radius

As Ulong col

End Type

Operator + (v1 As vector,v2 As vector) As vector

Return vct(v1.x+v2.x,v1.y+v2.y,v1.z+v2.z)

End Operator

Operator -(v1 As vector,v2 As vector) As vector

Return vct(v1.x-v2.x,v1.y-v2.y,v1.z-v2.z)

End Operator

Operator * (f As Single,v1 As vector) As vector 'scalar*vector

Return vct(f*v1.x,f*v1.y,f*v1.z)

End Operator

Operator * (v1 As vector,v2 As vector) As Single 'dot product

Return v1.x*v2.x+v1.y*v2.y+v1.z*v2.z

End Operator

Operator ^ (v1 As vector,v2 As vector) As vector 'cross product

Return vct(v1.y*v2.z-v2.y*v1.z,-(v1.x*v2.z-v2.x*v1.z),v1.x*v2.y-v2.x*v1.y)

End Operator

'subs

Function length(v As vector) As Single

Return Sqr(v.x*v.x+v.y*v.y+v.z*v.z)

End Function

Function normalize(v As vector) As vector

Dim n As Single=length(v)

If n=0 Then n=1e-20

Return vct(v.x/n,v.y/n,v.z/n)

End Function

Function Rotate3D(c As vector,p As vector,Byval angle As vector,scale As vector=Type<vector>(1,1,1)) As vector

angle= (0.0174532925199433)*angle

Dim As Single sx=Sin(angle.x),sy=Sin(angle.y),sz=Sin(angle.z)

Dim As Single cx=Cos(angle.x),cy=Cos(angle.y),cz=Cos(angle.z)

Dim As Single dx=p.x-c.x,dy=p.y-c.y,dz=p.z-c.z

Return Type<vector>((scale.x)*((cy*cz)*dx+(-cx*sz+sx*sy*cz)*dy+(sx*sz+cx*sy*cz)*dz)+c.x,_

(scale.y)*((cy*sz)*dx+(cx*cz+sx*sy*sz)*dy+(-sx*cz+cx*sy*sz)*dz)+c.y,_

(scale.z)*((-sy)*dx+(sx*cy)*dy+(cx*cy)*dz)+c.z)

End Function

Function apply_perspective(p As vector,eyepoint As vector) As vector

Dim As Single w=1+(p.z/eyepoint.z)

If w=0 Then w=1e-20

Return vct((p.x-eyepoint.x)/w+eyepoint.x,(p.y-eyepoint.y)/w+eyepoint.y,(p.z-eyepoint.z)/w+eyepoint.z)

End Function

Sub blow(a() As vector,m As Double)

For z As Long=1 To Ubound(a)

a(z)=m*a(z)

Next z

End Sub

Sub translate(a() As vector,shift As vector)

For z As Long=1 To Ubound(a)

a(z)=a(z)+shift

Next z

End Sub

Sub drawpolygon(p() As vector,i As Long,col As Ulong,flag As String="",im As Any Pointer=0)

Dim k As Long=Ubound(p,1)+1

Dim As Long index,nextindex

Dim As Double xc,yc

For n As Long=1 To Ubound(p,1)

xc=xc+p(n,i).x:yc=yc+p(n,i).y

index=n Mod k:nextindex=(n+1) Mod k

If nextindex=0 Then nextindex=1

Line im,(p(index,i).x,p(index,i).y)-(p(nextindex,i).x,p(nextindex,i).y),col

Next

xc=xc/Ubound(p,1):yc=yc/Ubound(p,1)

If flag="fill" Then Paint (xc,yc),col,col

End Sub

Function planedistance(S As PLANE,p As vector,Byref ip As vector=vct(0,0,0)) As Single

Dim As vector unitcross=normalize((s.v1-s.v2) cross (S.v2-S.v3))

Dim As Single dist=unitcross dot (p-s.v1)'pv

Dim As vector ip1=p+dist*unitcross

Dim As Single d1=length(s.v1-ip1)

unitcross=-1*unitcross

Dim As vector ip2=p+dist*unitcross

Dim As Single d2=length(s.v1-ip2)

If d1 <= d2 Then ip=ip1 Else ip=ip2

Return dist

End Function

Sub PAINTBALL(cx As single,_

cy As single,_

radius As single,_

col As Ulong,_

offsetX As single=0,_

offsetY As single=0,_

e As single=0,_

resolution As single=16,_ '32

im As Any Pointer=0)

#define map(a,b,x,c,d) ((d)-(c))*((x)-(a))/((b)-(a))+(c)

Dim As ubyte r,g,b

Dim As single ox,oy,nx,ny

ox=cx+offsetX*radius

oy=cy+offsetY*radius

Var red=Cast(Ubyte Ptr,@col)[2], green=Cast(Ubyte Ptr,@col)[1],blue=Cast(Ubyte Ptr,@col)[0]

For d As single = radius To 0 Step -radius/resolution

nx=(cx-ox)*(d-radius)/radius + cx

ny=(cy-oy)*(d-radius)/radius + cy

r=map(radius,0,d,.3*red,red)'-red*(d/radius-1)

g=map(radius,0,d,.3*green,green)'-green*(d/radius-1)

b=map(radius,0,d,.3*blue,blue)'-blue*(d/radius-1)

Circle im,(nx,ny),d,Rgb(r,g,b),,,e,F

Next d

'draw string(cx,cy),str

End Sub

Function vradius(b As _object) As Single

Dim As Single d=xres/5

Return ((1-1.5)*(b.position.z+d)/(2*d)+1.485)*b.radius

End Function

Function Regulate(Byval MyFps As Long,Byref fps As Long) As Long

Static As Double timervalue,_lastsleeptime,t3,frames

Var t=Timer

frames+=1

If (t-t3)>=1 Then t3=t:fps=frames:frames=0

Var sleeptime=_lastsleeptime+((1/myfps)-T+timervalue)*1000

If sleeptime<1 Then sleeptime=1

_lastsleeptime=sleeptime

timervalue=T

Return sleeptime

End Function

Sub start(e() As vector,side() As plane,ball() As _object,fcolour() As Ulong)

#define rr(f,l) (Rnd*(l-f)+f)

For z As Long=1 To 10:fcolour(z)=Rgb(Rnd*200,Rnd*200,Rnd*200):Next z'floorboards

e(1) =vct( 1, 1,-1)

e(2) =vct(-1, 1,-1)

e(3) =vct(-1, 1, 1)

e(4) =vct( 1, 1, 1)

side(1)=Type<plane>(e(1),e(2),e(3))

e(5) =vct( 1,-1, 1)

e(6) =vct(-1,-1, 1)

e(7) =vct(-1,-1,-1)

e(8) =vct( 1,-1,-1)

side(2)=Type<plane>(e(5),e(6),e(7))

e(9) =vct( 1, 1, 1)

e(10)=vct(-1, 1, 1)

e(11)=vct(-1,-1, 1)

e(12)=vct( 1,-1, 1)

side(3)=Type<plane>(e(9),e(10),e(11))

e(13)=vct( 1,-1,-1)

e(14)=vct(-1,-1,-1)

e(15)=vct(-1, 1,-1)

e(16)=vct( 1, 1,-1)

side(4)=Type<plane>(e(13),e(14),e(15))

e(17)=vct(-1, 1, 1)

e(18)=vct(-1, 1,-1)

e(19)=vct(-1,-1,-1)

e(20)=vct(-1,-1, 1)

side(5)=Type<plane>(e(17),e(18),e(19))

e(21)=vct( 1, 1,-1)

e(22)=vct( 1, 1, 1)

e(23)=vct( 1,-1, 1)

e(24)=vct( 1,-1,-1)

side(6)=Type<plane>(e(21),e(22),e(23))

blow(e(),xres/4) 'magnify

translate(e(),vct(xres/2,yres/2,0)) 'centralize

side(1)=Type<plane>(e(1),e(2),e(3))

side(2)=Type<plane>(e(5),e(6),e(7))

side(3)=Type<plane>(e(9),e(10),e(11))

side(4)=Type<plane>(e(13),e(14),e(15))

side(5)=Type<plane>(e(17),e(18),e(19))

side(6)=Type<plane>(e(21),e(22),e(23))

Dim As Long count

For x As Long=.3*xres To .7*xres Step 1.3*xres/10

For y As Long=.3*yres To .7*yres Step 1.3*yres/10

For z As Long=1 To Ubound(side)'-1

If Abs(planedistance(side(z),vct(x,y,z*20)))<30 Then Goto skip

Next z

count=count+1

Redim Preserve ball(count)

ball(count).position=vct(x,y,0)

ball(count).radius=25

ball(count).mass=4.18*ball(count).radius^3

ball(count).velocity=vct(rr(-1,1),rr(-1,1),rr(-1,-5))

ball(count).velocity=4*normalize((ball(count).velocity))

If count=15 Then Exit For,For

skip:

Next y

Next x

'colours 0 to 15 in 32 bits ulong

dim as ulong clr(15)={4278190080,4278190250,4278233600,4278233770,4289331200, _

4289331370,4289352960,4289374890,4283782485,4283782655, _

4283826005,4283826175,4294923605,4294923775,4294967125,4294967295}

for n as long=0 to 15

ball(n).col=clr(n)

next n

End Sub

Sub rotatebox(e() As vector,ve() As vector,angle As vector)

For a As Long=1 To Ubound(e)

Static As vector pivot:pivot=vct(xres/2,yres/2,0)

Var temp=rotate3d(pivot,e(a),angle,Type(1,1,1))

ve(a)=apply_perspective(temp,vct(xres/2,yres/2,900))

Next a

End Sub

Sub setfaces(face() As vector,ve() As vector)

Scope

Dim As Long s,c

Do

c=c+1

For n As Long=1 To 4: face(n,c)=ve(n+s):Next n

s=s+4

Loop Until c=6

End Scope

End Sub

Sub drawfaces(face() As vector,fcolour() As Ulong)

Scope

Dim As Ulong col

For a As Long=1 To Ubound(face,2)

Select Case As Const a

Case 2:col=Rgb(200,240,255)'2

Case 3:col=Rgb(50,50,100)

Case 5,6:col=Rgb(100,100,150)

End Select

If a<>4 Then drawpolygon(face(),a,col,"fill")

Next a

Dim As vector back,front

Dim As vector pts(1 To 2)

Dim As Long count

back=face(2,1)-face(1,1)

front=face(3,1)-face(4,1)

drawpolygon(face(),1,Rgb(100,100,150))

For a As Single=0 To 1 Step .1

count=count+1

pts(1)=face(1,1)+a*back

pts(2)=face(4,1)+a*front

Line(pts(1).x,pts(1).y)-(pts(2).x,pts(2).y),Rgb(100,100,150)

Paint(.5*(pts(2).x+pts(1).x)-25,2+.5*(pts(2).y+pts(1).y)),fcolour(count),Rgb(100,100,150)

Next a

drawpolygon(face(),4,Rgb(200,0,0))

End Scope

End Sub

Sub zsort(ball() As _object)

For p1 As Long = 1 To Ubound(ball) - 1

For p2 As Long = p1 + 1 To Ubound(ball)

If vradius(ball(p1))>vradius(ball(p2)) Then

Swap ball(p1),ball(p2)

End If

Next p2

Next p1

End Sub

Sub check_ball_to_plane_collisions(ball() As _object,side() As plane,angle As vector)

Dim As vector closepoint,pivot=vct(xres/2,yres/2,0)

Dim As Single _dt

Dim As Single s=.5+(angle.x)/60

For z As Long=1 To Ubound(ball)

For z2 As Long=1 To Ubound(side)

Var seperation=Abs(planedistance(side(z2),ball(z).position,closepoint))

Var temp=closepoint

temp=rotate3d(pivot,temp,angle,vct(1,1,1))

temp=apply_perspective(temp,vct(xres/2,yres/2,900))

If z2=1 Then Circle(temp.x,temp.y),vradius(ball(z)),Rgba(0,0,0,50),,,s,f 'shadows

If seperation<=ball(z).radius Then

Var impact=-1*ball(z).velocity

Var impulse=normalize(closepoint-ball(z).position)

'try to keep the ball off the plane by an arbitrary amount

ball(z).position=ball(z).position-ball(z).radius*(1/10)*impulse

_dt=(impact dot impulse)

ball(z).velocity=ball(z).velocity +2*_dt*impulse

End If

Next z2

Next z

End Sub

Function DetectBallCollisions( B1 As _object,B2 As _object) As boolean 'avoid using sqr if they are well seperated

Dim As vector diff=B2.position-B1.position

Dim As Long dist=(B2.radius+B1.radius)

If Abs(diff.x) > dist Then Return 0

If Abs(diff.y) > dist Then Return 0

If Abs(diff.z) > dist Then Return 0

If length(diff)<=dist Then Function=1 Else Function=0

End Function

Sub check_ball_to_ball_collisions(ball() As _object)

Dim As Single _dt

For x As Long=1 To Ubound(ball)-1

For y As Long=x+1 To Ubound(ball)

If DetectBallCollisions(ball(x),ball(y)) Then

Var impulse=normalize((ball(x).position-ball(y).position))

'reset overlaps

ball(x).position.x=ball(y).position.x+(ball(y).radius+ball(x).radius)*impulse.x

ball(x).position.y=ball(y).position.y+(ball(y).radius+ball(x).radius)*impulse.y

ball(x).position.z=ball(y).position.z+(ball(y).radius+ball(x).radius)*impulse.z

Var impact=ball(x).velocity-ball(y).velocity

_dt=(impact dot impulse)

Var ma=ball(x).mass,mb=ball(y).mass

ball(x).velocity=ball(x).velocity-_dt*((2*mb/(ma+mb)))*impulse

ball(y).velocity=ball(y).velocity+_dt*((2*ma/(mb+ma)))*impulse

End If

Next y

Next x

End Sub

Sub drawballs(ball() As _object,angle As vector,byref e as long)

e=0

Dim As vector temp,pivot=vct(xres/2,yres/2,0)

For z As Long=1 To Ubound(ball)

ball(z).position=ball(z).position+ball(z).velocity

var v=length(ball(z).velocity)

e+=.5*ball(z).mass*v*v 'energy tally

temp=rotate3d(pivot,ball(z).position,angle,Type(1,1,1))

temp=apply_perspective(temp,vct(xres/2,yres/2,900))

paintball(temp.x,temp.y,vradius(ball(z)),ball(z).col,0,-.5)

Next z

End Sub

Function fbmain As Long

Dim As vector e(1 To 24),ve(1 To 24)

Dim As vector angle

Dim As Ulong fcolour(1 To 10)

Dim As plane side(1 To 6)

Dim As vector face(1 To 4,6)

Dim As Single t,pi=4*Atn(1)

Redim As _object ball()

start(e(),side(),ball(),fcolour())

Dim As Long fps,energy

Do

t=t+.25:If t>=360 Then t=0

angle.z=5*Sin(2*t*pi/180)'roll

angle.x=10*Sin(3*t*pi/180)'yaw

angle.y=12*Sin(2*t*pi/180)'screw

rotatebox(e(),ve(),angle)

setfaces(face(),ve())

Screenlock

Cls

Draw String(5,5), "Framerate " &fps

Draw String(5,20), "Energy " &energy

drawfaces(face(),fcolour())

check_ball_to_ball_collisions(ball())

check_ball_to_plane_collisions(ball(),side(),angle)

zsort(ball())

drawballs(ball(),angle,energy)

Screenunlock

Sleep regulate(50,fps),1

Loop Until Inkey=Chr(27)

Sleep

Return 0

End Function

End fbmain

As usual the demo via fbc 32 bits is much smoother than fb 64 bits.

Could you provide a little demo for your suggestions perhaps?

### Re: Collision detection in 3D

@dodicat: very nice 3D balls demo.

### Re: Collision detection in 3D

@dodicat

Very Good.

The floor shadows are a nice bonus!

Very Good.

The floor shadows are a nice bonus!

### Who is online

Users browsing this forum: No registered users and 2 guests