Does anyone have a working implementation of collision+response based on this article?

My engine is in C while the source code is in C++, but i'm relatively confident I did a "decent" job of the porting although there may be some "gotchas" I'm overlooking.

Collisions *do* seem to be "registering"... But it seems like the push-back is "under-calculated"... Like, I hit a floor and as i go down through the floor, there seems to be some upward normal velocity so it moves through the floor slowly until it gets all the way through then resorts back to faster velocity. I'm thinking this might have something to do with an ellipsoid-space conversion I may be missing, or something like that. If you see nothing obviously wrong with this source portion and would be willing to talk with me one-on-one for linking the full source and help on setting up the proper build environment, etc. then please respond here and leave a PM too as having notes both places will increase likelihood i'll see it eventually. I'm really stuck on this and I might even consider paying a small stipend for the turnout of something that works properly within my engine, unless you'd be willing to just lend a helping hand.

I do have an ambitious aim/goal and vision for my entire game project so if you help out fixing this issue I think I will be at that stage allowing me to start implementing most of my gameplay ideas and get the ball rolling so if you'd like to get involved and try to follow/aide development then we can talk further

It's a game engine "from scratch" (not OGRE or Irrlicht or whatnot)...

Here's a small demo of the basics so far:

Here's the code I'm having issue with:

#include <math.h>
#include <stdbool.h>

extern obj* main_map;

inline void vectorize( double* restrict cache, double* restrict a, double* restrict b ){
	cache[0] = b[0] - a[0];
	cache[1] = b[1] - a[1];
	cache[2] = b[2] - a[2];
}

inline double dot( double* restrict a, double* restrict b ){
	return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
}

inline void cross( double* restrict cache, double* restrict a, double* restrict b ){
	cache[0] = a[1] * b[2] - a[2] * b[1];
	cache[1] = a[2] * b[0] - a[0] * b[2];
	cache[2] = a[0] * b[1] - a[1] * b[0];
}

inline double magnitude( double* restrict a ){
	return sqrtf( a[0] * a[0] + a[1] * a[1] + a[2] * a[2] );
}

inline void add( double* restrict cache, double* restrict a, double* /*restrict*/ b ){
	cache[0] = a[0] + b[0];
	cache[1] = a[1] + b[1];
	cache[2] = a[2] + b[2];
}

inline void multiply( double* /*restrict*/ cache, double a, double* restrict b ){
	cache[0] = b[0] * a;
	cache[1] = b[1] * a;
	cache[2] = b[2] * a;
}



typedef struct{

double eRadius[3]; // ellipsoid radius
// Information about the move being requested: (in R3)
double R3Velocity[3];
double R3Position[3];
// Information about the move being requested: (in eSpace)
double velocity[3];
double normalizedVelocity[3];
double basePoint[3];
// Hit information
bool foundCollision;
double nearestDistance;
double intersectionPoint[3];
} CollisionPacket;

typedef struct {
float equation[4];
double origin[3];
double normal[3];
} PLANE;


void new_PLANE( PLANE* PLANE_inst, double* origin, double* normal) {

PLANE_inst->normal[0] = normal[0]; PLANE_inst->normal[1] = normal[1]; PLANE_inst->normal[2] = normal[2];
PLANE_inst->origin[0] = origin[0]; PLANE_inst->origin[1] = origin[1]; PLANE_inst->origin[2] = origin[2];
PLANE_inst->equation[0] = normal[0];
PLANE_inst->equation[1] = normal[1];
PLANE_inst->equation[2] = normal[2];
PLANE_inst->equation[3] = -(normal[0]*origin[0]+normal[1]*origin[1]
+normal[2]*origin[2]);

}
// Construct from triangle:
void new_PLANE2(PLANE* PLANE_inst, double* p1, double* p2, double* p3)
{
double p1p2[3]; vectorize( p1p2, p1, p2);
double p1p3[3]; vectorize( p1p3, p1, p3);
cross( PLANE_inst->normal, p1p2, p1p3 );
double normalmag = magnitude( PLANE_inst->normal );
PLANE_inst->normal[0] /= normalmag; PLANE_inst->normal[1] /= normalmag; PLANE_inst->normal[2] /= normalmag;
PLANE_inst->origin[0] = p1[0]; PLANE_inst->origin[1] = p1[1]; PLANE_inst->origin[2] = p1[2];
PLANE_inst->equation[0] = PLANE_inst->normal[0];
PLANE_inst->equation[1] = PLANE_inst->normal[1];
PLANE_inst->equation[2] = PLANE_inst->normal[2];
PLANE_inst->equation[3] = -(PLANE_inst->normal[0]*PLANE_inst->origin[0]+PLANE_inst->normal[1]*PLANE_inst->origin[1]
+PLANE_inst->normal[2]*PLANE_inst->origin[2]);
}

bool isFrontFacingTo( PLANE* PLANE_inst, double* direction) {


double dot_ = dot(PLANE_inst->normal, direction);
return (dot_ <= 0);
}
double signedDistanceTo( PLANE* PLANE_inst,  double* point) {
return dot(point,PLANE_inst->normal) + PLANE_inst->equation[3];
}

typedef unsigned int uint32;
#define in(a) *((uint32*)&a)
bool checkPointInTriangle( double* point,
double* pa, double* pb, double* pc)
{
double e10[3]; vectorize( e10, pa, pb );
double e20[3]; vectorize(e20, pa, pc);
float a = dot(e10,e10);
float b = dot(e10,e20);
float c = dot(e20,e20);
float ac_bb=(a*c)-(b*b);
double vp[3]={point[0]-pa[0], point[1]-pa[1], point[2]-pa[2]};
float d = dot(vp,e10);
float e = dot(vp,e20);
float x = (d*c)-(e*b);
float y = (e*a)-(d*b);
float z = x+y-ac_bb;
return (( in(z)& ~(in(x)|in(y)) ) & 0x80000000);
}

bool getLowestRoot(float a, float b, float c, float maxR,
float* root) {
// Check if a solution exists
float determinant = b*b - 4.0f*a*c;
// If determinant is negative it means no solutions.
if (determinant < 0.0f) return false;
// calculate the two roots: (if determinant == 0 then
// x1==x2 but let’s disregard that slight optimization)
float sqrtD = sqrt(determinant);
float r1 = (-b - sqrtD) / (2*a);
float r2 = (-b + sqrtD) / (2*a);
// Sort so x1 <= x2
if (r1 > r2) {
float temp = r2;
r2 = r1;
r1 = temp;
}
// Get lowest root:
if (r1 > 0 && r1 < maxR) {
*root = r1;
return true;
}
// It is possible that we want x2 - this can happen
// if x1 < 0
if (r2 > 0 && r2 < maxR) {
*root = r2;
return true;
}
// No (valid) solutions
return false;
}

// Assumes: p1,p2 and p3 are given in ellisoid space:
void checkTriangle(CollisionPacket* colPackage,
double* p1, double* p2, double* p3)
{

p1[0] = p1[0] / colPackage->eRadius[0]; p1[1] = p1[1] / colPackage->eRadius[1]; p1[2] = p1[2] / colPackage->eRadius[2];
p2[0] = p2[0] / colPackage->eRadius[0]; p2[1] = p2[1] / colPackage->eRadius[1]; p2[2] = p2[2] / colPackage->eRadius[2];
p3[0] = p3[0] / colPackage->eRadius[0]; p3[1] = p3[1] / colPackage->eRadius[1]; p3[2] = p3[2] / colPackage->eRadius[2];

// Make the plane containing this triangle.
PLANE trianglePlane; new_PLANE2(&trianglePlane,p1,p2,p3);
// Is triangle front-facing to the velocity vector?
// We only check front-facing triangles
// (your choice of course)
if (isFrontFacingTo(&trianglePlane,
colPackage->normalizedVelocity)) {
// Get interval of plane intersection:
double t0, t1;
bool embeddedInPlane = false;
// Calculate the signed distance from sphere
// position to triangle plane
double signedDistToTrianglePlane =
signedDistanceTo(&trianglePlane,colPackage->basePoint);
// cache this as we’re going to use it a few times below:
float normalDotVelocity =
dot(trianglePlane.normal,colPackage->velocity);
// if sphere is travelling parrallel to the plane:
if (normalDotVelocity == 0.0f) {
if (fabs(signedDistToTrianglePlane) >= 1.0f) {
// Sphere is not embedded in plane.
// No collision possible:
return;
}
else {
// sphere is embedded in plane.
// It intersects in the whole range [0..1]
embeddedInPlane = true;
t0 = 0.0;
t1 = 1.0;
}
}
else {
// N dot D is not 0. Calculate intersection interval:
t0=(-1.0-signedDistToTrianglePlane)/normalDotVelocity;
t1=( 1.0-signedDistToTrianglePlane)/normalDotVelocity;
// Swap so t0 < t1
if (t0 > t1) {
double temp = t1;
t1 = t0;
t0 = temp;
}
// Check that at least one result is within range:
if (t0 > 1.0f || t1 < 0.0f) {
// Both t values are outside values [0,1]
// No collision possible:
return;
}
// Clamp to [0,1]
if (t0 < 0.0) t0 = 0.0;
if (t1 < 0.0) t1 = 0.0;
if (t0 > 1.0) t0 = 1.0;
if (t1 > 1.0) t1 = 1.0;
}
// OK, at this point we have two time values t0 and t1
// between which the swept sphere intersects with the
// triangle plane. If any collision is to occur it must
// happen within this interval.
double collisionPoint[3];
bool foundCollison = false;
float t = 1.0;
// First we check for the easy case - collision inside
// the triangle. If this happens it must be at time t0
// as this is when the sphere rests on the front side
// of the triangle plane. Note, this can only happen if
// the sphere is not embedded in the triangle plane.
if (!embeddedInPlane) {
double a[3]; vectorize( a, trianglePlane.normal, colPackage->basePoint );
double b[3]; multiply( b, t0, colPackage->velocity );
double planeIntersectionPoint[3]; add( planeIntersectionPoint, a, b );

if (checkPointInTriangle(planeIntersectionPoint,
p1,p2,p3))
{
foundCollison = true;
t = t0;
collisionPoint[0] = planeIntersectionPoint[0]; collisionPoint[1] = planeIntersectionPoint[1]; collisionPoint[2] = planeIntersectionPoint[2];
}
}
// if we haven’t found a collision already we’ll have to
// sweep sphere against points and edges of the triangle.
// Note: A collision inside the triangle (the check above)
// will always happen before a vertex or edge collision!
// This is why we can skip the swept test if the above
// gives a collision!
if (foundCollison == false) {
// some commonly used terms:
double* velocity = colPackage->velocity;
double* base = colPackage->basePoint;
float velocitySquaredLength = magnitude( velocity ) * magnitude( velocity );
float a,b,c; // Params for equation
float newT;
// For each vertex or edge a quadratic equation have to
// be solved. We parameterize this equation as
// a*t^2 + b*t + c = 0 and below we calculate the
// parameters a,b and c for each test.
// Check against points:
a = velocitySquaredLength;
// P1
double p1base[3]; vectorize( p1base, p1, base );
b = 2.0*(dot(velocity,p1base));
double basep1[3]; vectorize( basep1, base, p1 );
c = magnitude( basep1 ) * magnitude( basep1 ) - 1.0;
if (getLowestRoot(a,b,c, t, &newT)) {
t = newT;
foundCollison = true;
collisionPoint[0] = p1[0]; collisionPoint[1] = p1[1]; collisionPoint[2] = p1[2];
}
// P2
double p2base[3]; vectorize( p2base, p2, base );
b = 2.0*(dot(velocity,p2base));
double basep2[3]; vectorize( basep2, base, p2 );
c = magnitude( basep2 ) * magnitude( basep2 ) - 1.0;
if (getLowestRoot(a,b,c, t, &newT)) {
t = newT;
foundCollison = true;
collisionPoint[0] = p2[0]; collisionPoint[1] = p2[1]; collisionPoint[2] = p2[2];
}
// P3
double p3base[3]; vectorize( p3base, p3, base );
b = 2.0*(dot(velocity,p2base));
double basep3[3]; vectorize( basep3, base, p3 );
c = magnitude( basep3 ) * magnitude( basep3 ) - 1.0;
if (getLowestRoot(a,b,c, t, &newT)) {
t = newT;
foundCollison = true;
collisionPoint[0] = p3[0]; collisionPoint[1] = p3[1]; collisionPoint[2] = p3[2];
}
// Check agains edges:
// p1 -> p2:
double edge[3]; vectorize( edge, p1, p2 );
double baseToVertex[3]; vectorize( baseToVertex, base, p1 );
float edgeSquaredLength = magnitude( edge ) * magnitude( edge );
float edgeDotVelocity = dot(edge,velocity);
float edgeDotBaseToVertex = dot(edge,baseToVertex);
// Calculate parameters for equation
a = edgeSquaredLength*-velocitySquaredLength +
edgeDotVelocity*edgeDotVelocity;
b = edgeSquaredLength*(2*dot(velocity,baseToVertex))-
2.0*edgeDotVelocity*edgeDotBaseToVertex;
c = edgeSquaredLength*(1-magnitude( baseToVertex ) * magnitude( baseToVertex ))+
edgeDotBaseToVertex*edgeDotBaseToVertex;
// Does the swept sphere collide against infinite edge?
if (getLowestRoot(a,b,c, t, &newT)) {
// Check if intersection is within line segment:
float f=(edgeDotVelocity*newT-edgeDotBaseToVertex)/
edgeSquaredLength;
if (f >= 0.0 && f <= 1.0) {
// intersection took place within segment.
t = newT;
foundCollison = true;
double temp[3]; multiply( temp, f, edge );
add( collisionPoint, p1, temp );
}
}
// p2 -> p3:
vectorize( edge, p2, p3 );
vectorize( baseToVertex, base, p2 );
edgeSquaredLength = magnitude( edge ) * magnitude( edge );
edgeDotVelocity = dot(edge,velocity);
edgeDotBaseToVertex = dot(edge,baseToVertex);
a = edgeSquaredLength*-velocitySquaredLength +
edgeDotVelocity*edgeDotVelocity;
b = edgeSquaredLength*(2*dot(velocity,baseToVertex))-
2.0*edgeDotVelocity*edgeDotBaseToVertex;
c = edgeSquaredLength*(1-magnitude( baseToVertex ) * magnitude( baseToVertex ))+
edgeDotBaseToVertex*edgeDotBaseToVertex;
if (getLowestRoot(a,b,c, t, &newT)) {
float f=(edgeDotVelocity*newT-edgeDotBaseToVertex)/
edgeSquaredLength;
if (f >= 0.0 && f <= 1.0) {
t = newT;
foundCollison = true;
double temp[3]; multiply( temp, f, edge );
add( collisionPoint, p2, temp  );
}
}
// p3 -> p1:
vectorize( edge, p3, p1 );
vectorize( baseToVertex, base, p3 );
edgeSquaredLength = magnitude( edge ) * magnitude( edge );
edgeDotVelocity = dot(edge, velocity);
edgeDotBaseToVertex = dot(edge, baseToVertex);
a = edgeSquaredLength*-velocitySquaredLength +
edgeDotVelocity*edgeDotVelocity;
b = edgeSquaredLength*(2*dot(velocity, baseToVertex))-
2.0*edgeDotVelocity*edgeDotBaseToVertex;
c = edgeSquaredLength*(1-magnitude(baseToVertex) * magnitude(baseToVertex))+
edgeDotBaseToVertex*edgeDotBaseToVertex;
if (getLowestRoot(a,b,c, t, &newT)) {
float f=(edgeDotVelocity*newT-edgeDotBaseToVertex)/
edgeSquaredLength;
if (f >= 0.0 && f <= 1.0) {
t = newT;
foundCollison = true;
double temp[3]; multiply( temp, f, edge );
add( collisionPoint, p3, temp  );

}
}
}
// Set result:
if (foundCollison == true) {
// distance to collision: ’t’ is time of collision
float distToCollision = t*magnitude(colPackage->velocity);
// Does this triangle qualify for the closest hit?
// it does if it’s the first hit or the closest
if (colPackage->foundCollision == false ||
distToCollision < colPackage->nearestDistance) {
// Collision information nessesary for sliding
colPackage->nearestDistance = distToCollision;
colPackage->intersectionPoint[0]=collisionPoint[0]; colPackage->intersectionPoint[1]=collisionPoint[1]; colPackage->intersectionPoint[2]=collisionPoint[2];
colPackage->foundCollision = true;
}
}
} // if not backface
}

int collisionRecursionDepth;



// Set this to match application scale..
const float unitsPerMeter = 6/1.778f;
double* collideWithWorld( double* pos,
double* vel, CollisionPacket* collisionPackage)
{
// All hard-coded distances in this function is
// scaled to fit the setting above..
float unitScale = unitsPerMeter / 100.0f;
float veryCloseDistance = 0.005f * unitScale;
// do we need to worry?
if (collisionRecursionDepth>5)
return pos;
// Ok, we need to worry:
collisionPackage->velocity[0] = vel[0]; collisionPackage->velocity[1] = vel[1]; collisionPackage->velocity[2] = vel[2];
collisionPackage->normalizedVelocity[0] = vel[0];collisionPackage->normalizedVelocity[1] = vel[1];collisionPackage->normalizedVelocity[2] = vel[2];
double normalizedVelocitymag = magnitude( collisionPackage->normalizedVelocity );
collisionPackage->normalizedVelocity[0] /= normalizedVelocitymag;collisionPackage->normalizedVelocity[1] /= normalizedVelocitymag;collisionPackage->normalizedVelocity[2] /= normalizedVelocitymag;
collisionPackage->basePoint[0] = pos[0]; collisionPackage->basePoint[1] = pos[1]; collisionPackage->basePoint[2] = pos[2];
collisionPackage->foundCollision = false;
// Check for collision (calls the collision routines)
// Application specific!!
//world->checkCollision(collisionPackage);


	int i = 0;

	while( i < main_map->faces.length ){

		double p1[3] = {main_map->faces.floats[i + 0], main_map->faces.floats[i + 1], main_map->faces.floats[i + 2]};
		double p2[3] = {main_map->faces.floats[i + 3], main_map->faces.floats[i + 4], main_map->faces.floats[i + 5]};
		double p3[3] = {main_map->faces.floats[i + 6], main_map->faces.floats[i + 7], main_map->faces.floats[i + 8]};

		checkTriangle( collisionPackage, p1, p2, p3);



		i += 9;
	}

// If no collision we just move along the velocity
if (collisionPackage->foundCollision == false) {
double* ret = malloc( sizeof( double ) * 3 ); add( ret, pos, vel );
return ret;
}
// *** Collision occured ***
// The original destination point
double destinationPoint[3]; add( destinationPoint, pos, vel );
double* newBasePoint = malloc( sizeof( double ) * 3 ); newBasePoint[0]=pos[0];newBasePoint[1]=pos[1];newBasePoint[2]=pos[2];
// only update if we are not already very close
// and if so we only move very close to intersection..not
// to the exact spot.
if (collisionPackage->nearestDistance>=veryCloseDistance)
{
double V[3] = {vel[0],vel[1],vel[2]};

double Vmag = magnitude( V );
V[0] /= Vmag; V[1] /= Vmag; V[2] /= Vmag;
V[0] *= collisionPackage->nearestDistance-veryCloseDistance;V[1] *= collisionPackage->nearestDistance-veryCloseDistance;V[2] *= collisionPackage->nearestDistance-veryCloseDistance;

add( newBasePoint, collisionPackage->basePoint, V );
// Adjust polygon intersection point (so sliding
// plane will be unaffected by the fact that we
// move slightly less than collision tells us)

Vmag = magnitude( V );
V[0] /= Vmag; V[1] /= Vmag; V[2] /= Vmag;

collisionPackage->intersectionPoint[0] -= veryCloseDistance * V[0];collisionPackage->intersectionPoint[1] -= veryCloseDistance * V[1];collisionPackage->intersectionPoint[2] -= veryCloseDistance * V[2];
}
// Determine the sliding plane
double*slidePlaneOrigin = collisionPackage->intersectionPoint;
double slidePlaneNormal[3]; vectorize( slidePlaneNormal, collisionPackage->intersectionPoint, newBasePoint );
double slidePlaneNormalmag = magnitude( slidePlaneNormal );
slidePlaneNormal[0] /= slidePlaneNormalmag; slidePlaneNormal[1] /= slidePlaneNormalmag; slidePlaneNormal[2] /= slidePlaneNormalmag;

PLANE slidingPlane; new_PLANE(&slidingPlane, slidePlaneOrigin,slidePlaneNormal);
// Again, sorry about formatting.. but look carefully ;)
double temp[3]; multiply( temp, signedDistanceTo(&slidingPlane,destinationPoint), slidePlaneNormal );
double newDestinationPoint[3]; vectorize( newDestinationPoint,temp,destinationPoint );



// Generate the slide vector, which will become our new
// velocity vector for the next iteration
double* newVelocityVector = malloc( sizeof( double ) * 3 ); vectorize( newVelocityVector, collisionPackage->intersectionPoint, newDestinationPoint );
// Recurse:
// dont recurse if the new velocity is very small
if (magnitude( newVelocityVector ) < veryCloseDistance) {
return newBasePoint;
}
collisionRecursionDepth++;
return collideWithWorld(newBasePoint,newVelocityVector, collisionPackage);
}




void collideAndSlide( double* vel,
double* gravity)
{

// Do collision detection:
CollisionPacket collisionPackage;

collisionPackage.eRadius[0]=1.5;collisionPackage.eRadius[1]=3;collisionPackage.eRadius[2]=.5;

collisionPackage.R3Position[0] = x_position; collisionPackage.R3Position[1] = y_position; collisionPackage.R3Position[2] = z_position;

collisionPackage.R3Velocity[0] = vel[0]; collisionPackage.R3Velocity[1] = vel[1]; collisionPackage.R3Velocity[2] = vel[2];

// calculate position and velocity in eSpace
double eSpacePosition[3] = {collisionPackage.R3Position[0]/collisionPackage.eRadius[0],collisionPackage.R3Position[1]/collisionPackage.eRadius[1],collisionPackage.R3Position[2]/collisionPackage.eRadius[2]};




double eSpaceVelocity[3] = {collisionPackage.R3Velocity[0]/collisionPackage.eRadius[0],collisionPackage.R3Velocity[1]/collisionPackage.eRadius[2],collisionPackage.R3Velocity[2]/collisionPackage.eRadius[2]};
// Iterate until we have our final position.
collisionRecursionDepth = 0;
double* finalPosition = collideWithWorld(eSpacePosition,
eSpaceVelocity, &collisionPackage);
// Add gravity pull:
// To remove gravity uncomment from here .....
// Set the new R3 position (convert back from eSpace to R3
collisionPackage.R3Position[0] = finalPosition[0]*collisionPackage.eRadius[0];collisionPackage.R3Position[1] = finalPosition[1]*collisionPackage.eRadius[1];collisionPackage.R3Position[2] = finalPosition[2]*collisionPackage.eRadius[2];
collisionPackage.R3Velocity[0] = gravity[0]; collisionPackage.R3Velocity[1] = gravity[1]; collisionPackage.R3Velocity[2] = gravity[2];
eSpaceVelocity[0] = gravity[0]/collisionPackage.eRadius[0];eSpaceVelocity[1] = gravity[1]/collisionPackage.eRadius[1];eSpaceVelocity[2] = gravity[2]/collisionPackage.eRadius[2];
collisionRecursionDepth = 0;

finalPosition = collideWithWorld(finalPosition,
eSpaceVelocity, &collisionPackage);

// ... to here
// Convert final result back to R3:
finalPosition[0] = finalPosition[0]*collisionPackage.eRadius[0];finalPosition[1] = finalPosition[1]*collisionPackage.eRadius[1];finalPosition[2] = finalPosition[2]*collisionPackage.eRadius[2];
// Move the entity (application specific function)
//MoveTo(finalPosition);

x_position = finalPosition[0]; y_position = finalPosition[1]; z_position = finalPosition[2]; 
}