Collision detection in 3D

Game development specific discussions.
Posts: 34
Joined: Dec 14, 2018 11:11

Collision detection in 3D

Postby thebigh » Apr 06, 2020 22:29

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 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!
Posts: 34
Joined: Dec 14, 2018 11:11

Re: Collision detection in 3D

Postby thebigh » Apr 06, 2020 22:30

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.

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

Posts: 34
Joined: Dec 14, 2018 11:11

Re: Collision detection in 3D

Postby thebigh » Apr 06, 2020 22:30

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.

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!
Posts: 34
Joined: Dec 14, 2018 11:11

Re: Collision detection in 3D

Postby thebigh » Apr 06, 2020 22:32

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):

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
    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
    dim as single cc = 1.0 - aa - bb
    if cc < 0.0 orelse cc > 1.0 then
        return false
    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!
Posts: 34
Joined: Dec 14, 2018 11:11

Re: Collision detection in 3D

Postby thebigh » Apr 06, 2020 22:33

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

                               '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
        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.
Posts: 34
Joined: Dec 14, 2018 11:11

Re: Collision detection in 3D

Postby thebigh » Apr 06, 2020 22:33

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.

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 )
    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
    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-> = coltype  'for debugging
        end if
    end if
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.
Posts: 34
Joined: Dec 14, 2018 11:11

Re: Collision detection in 3D

Postby thebigh » Apr 06, 2020 22:34

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

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
    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 = 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.
Posts: 34
Joined: Dec 14, 2018 11:11

Re: Collision detection in 3D

Postby thebigh » Apr 06, 2020 22:35

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:

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.

Posts: 6692
Joined: Jan 10, 2006 20:30
Location: Scotland

Re: Collision detection in 3D

Postby dodicat » Apr 08, 2020 11:41

Here is a quick little triangle intercept thing.
+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
            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)
    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
    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
    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
    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
    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)+")"
        Locate 12,50
        Print "distance from mouse  ";planedistance(p,m)
    End If
    Sleep 1,1
Loop Until Inkey=Chr(27)

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
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,_
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)
  Next z
End Sub

Sub translate(a() As vector,shift As vector)
  For z As Long=1 To Ubound(a)
  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)
    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
  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)
  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
  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
    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
  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
  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)
    e(5) =vct( 1,-1, 1)
    e(6) =vct(-1,-1, 1)
    e(7) =vct(-1,-1,-1)
    e(8) =vct( 1,-1,-1)
    e(9) =vct( 1, 1, 1)
    e(10)=vct(-1, 1, 1)
    e(11)=vct(-1,-1, 1)
    e(12)=vct( 1,-1, 1)
    e(13)=vct( 1,-1,-1)
    e(15)=vct(-1, 1,-1) 
    e(16)=vct( 1, 1,-1) 
    e(17)=vct(-1, 1, 1) 
    e(18)=vct(-1, 1,-1)   
    e(20)=vct(-1,-1, 1)   
    e(21)=vct( 1, 1,-1) 
    e(22)=vct( 1, 1, 1)   
    e(23)=vct( 1,-1, 1)   
    e(24)=vct( 1,-1,-1)   
    blow(e(),xres/4)            'magnify
    translate(e(),vct(xres/2,yres/2,0)) 'centralize
    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
        Redim Preserve ball(count)
        If count=15 Then Exit For,For
      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, _
                    for n as long=0 to 15
                    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))
    Next a
  End Sub
  Sub setfaces(face() As vector,ve() As vector)
      Dim As Long s,c
        For n As Long=1 To 4: face(n,c)=ve(n+s):Next n
        Loop Until c=6
      End Scope
    End Sub
    Sub drawfaces(face() As vector,fcolour() As Ulong)
        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
        For a As Single=0 To 1 Step .1
        Next a
      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
          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
            _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         
            Var impact=ball(x).velocity-ball(y).velocity
            _dt=(impact dot impulse)
            Var ma=ball(x).mass,mb=ball(y).mass
          End If
        Next y
      Next x
    End Sub
    Sub drawballs(ball() As _object,angle As vector,byref e as long)
      Dim As vector temp,pivot=vct(xres/2,yres/2,0)
      For z As Long=1 To Ubound(ball)
        var v=length(ball(z).velocity)
         e+=.5*ball(z).mass*v*v 'energy tally
      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()
      Dim As Long fps,energy

        t=t+.25:If t>=360 Then t=0
        Draw String(5,5), "Framerate " &fps
        Draw String(5,20), "Energy " &energy
        Sleep regulate(50,fps),1
      Loop Until Inkey=Chr(27)
      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?
Posts: 627
Joined: May 05, 2017 19:59
Location: Germany

Re: Collision detection in 3D

Postby UEZ » Apr 08, 2020 13:39

@dodicat: very nice 3D balls demo.
Posts: 391
Joined: Feb 01, 2007 16:54
Location: usa

Re: Collision detection in 3D

Postby integer » Apr 10, 2020 6:23

Very Good.
The floor shadows are a nice bonus!

Return to “Game Dev”

Who is online

Users browsing this forum: No registered users and 2 guests