#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];
}