/*-<==>-----------------------------------------------------------------
/
/----------------------------------------------------------------------*/
bool CSphere::hits (const CLine &line, SCALAR &t_hit) {
// Pendiente de implementar correctamente
// REVISADA
VECTOR aux = line.loc - loc;
SCALAR b = aux.dot(line.dir);
SCALAR c = aux.dot(aux) - radius*radius;
SCALAR d = b*b - c;
if (d<0)
return false; // Noqueremos negativos pq no toca linea con esfera
else{
SCALAR tm = -b - sqrt(d); // -b - SQRT(b^2 -4ac)
SCALAR tM = -b + sqrt(d);// -b + SQRT(b^2 -4ac)
if(tm > 0) {
t_hit = tm;
return true;
}
if(tM > 0){
t_hit = tM;
return true;
}
}
return false;
}
/*
--------------------------------------------------------------------------------------------------
- check collision with stairs
--------------------------------------------------------------------------------------------------
*/
bool PlanesPhysicHandler::ColisionWithStair(const AABB & actorBB, const VECTOR &Speed, VECTOR &ModifiedSpeed)
{
float moveX = Speed.x;
float moveZ = Speed.z;
// calculate norm of speed
VECTOR speedNorm = Speed.unit();
float startX = (actorBB.P.x+actorBB.E.x)/2.0f;
float startZ = (actorBB.P.z+actorBB.E.z)/2.0f;
std::vector<StairPlane>::const_iterator it = _stairs.begin();
std::vector<StairPlane>::const_iterator end = _stairs.end();
// for each stairs
for(int i=0; it != end; ++it, ++i)
{
// project point to plane and check if we cross it
float DotProduct=speedNorm.dot(it->Normal);
// Determine If Ray Parallel To Plane
if (abs(DotProduct) > 0.000001f)
{
// Find Distance To Collision Point
float l2=(it->Normal.dot(it->C1-VECTOR(startX, actorBB.P.y, startZ)))/DotProduct;
// Test If Collision Behind Start or after end
if (l2 > 0 && l2 < Speed.length())
{
float collionsX = startX + (speedNorm.x * l2);
float collionsZ = startZ + (speedNorm.z * l2);
if((collionsX >= it->minX) && (collionsX <= it->maxX))
{
if((collionsZ >= it->minZ) && (collionsZ <= it->maxZ))
{
VECTOR spmY(Speed.x, 0, Speed.z);
VECTOR Vt(it->Normal.dot(spmY)*it->Normal);
VECTOR Vn(spmY - Vt);
ModifiedSpeed = Vn;
return true;
}
}
}
}
}
return false;
}
// Assumes: p1,p2 and p3 are given in ellisoid space:
void checkTriangle(CollisionPacket* colPackage,VECTOR p1,VECTOR p2,VECTOR p3)
{
#ifndef __GNUC__
// Make the plane containing this triangle.
PLANE trianglePlane(p1,p2,p3);
// Is triangle front-facing to the velocity vector?
// We only check front-facing triangles
if (trianglePlane.isFrontFacingTo(colPackage->normalizedVelocity) || (! isBackCulling))
{
// Get interval of plane intersection:
double t0, t1;
bool embeddedInPlane = false;
// Calculate the signed distance from sphere
// position to triangle plane
double signedDistToTrianglePlane = trianglePlane.signedDistanceTo(colPackage->basePoint);
// cache this as we're going to use it a few times below:
float normalDotVelocity = trianglePlane.normal.dot(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.
VECTOR collisionPoint;
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)
{
VECTOR planeIntersectionPoint = (colPackage->basePoint-trianglePlane.normal) + colPackage->velocity*t0;
if (checkPointInTriangle(planeIntersectionPoint,p1,p2,p3))
{
foundCollison = true;
t = t0;
collisionPoint = planeIntersectionPoint;
}
}
// 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:
VECTOR velocity = colPackage->velocity;
VECTOR base = colPackage->basePoint;
float velocitySquaredLength = velocity.squaredLength();
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
b = 2.0*(velocity.dot(base-p1));
c = (p1-base).squaredLength() - 1.0;
if (getLowestRoot(a,b,c, t, &newT))
{
t = newT;
foundCollison = true;
collisionPoint = p1;
}
// P2
b = 2.0*(velocity.dot(base-p2));
c = (p2-base).squaredLength() - 1.0;
if (getLowestRoot(a,b,c, t, &newT))
{
t = newT;
foundCollison = true;
collisionPoint = p2;
}
// P3
b = 2.0*(velocity.dot(base-p3));
c = (p3-base).squaredLength() - 1.0;
if (getLowestRoot(a,b,c, t, &newT))
{
t = newT;
foundCollison = true;
collisionPoint = p3;
}
// Check agains edges:
// p1 -> p2:
VECTOR edge = p2-p1;
VECTOR baseToVertex = p1 - base;
float edgeSquaredLength = edge.squaredLength();
float edgeDotVelocity = edge.dot(velocity);
float edgeDotBaseToVertex = edge.dot(baseToVertex);
// Calculate parameters for equation
a = edgeSquaredLength*-velocitySquaredLength + edgeDotVelocity*edgeDotVelocity;
b = edgeSquaredLength*(2*velocity.dot(baseToVertex)) - 2.0*edgeDotVelocity*edgeDotBaseToVertex;
c = edgeSquaredLength*(1-baseToVertex.squaredLength()) + 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;
collisionPoint = p1 + edge*f;
}
}
// p2 -> p3:
edge = p3-p2;
baseToVertex = p2 - base;
edgeSquaredLength = edge.squaredLength();
edgeDotVelocity = edge.dot(velocity);
edgeDotBaseToVertex = edge.dot(baseToVertex);
a = edgeSquaredLength*-velocitySquaredLength + edgeDotVelocity*edgeDotVelocity;
b = edgeSquaredLength*(2*velocity.dot(baseToVertex)) - 2.0*edgeDotVelocity*edgeDotBaseToVertex;
c = edgeSquaredLength*(1-baseToVertex.squaredLength()) + 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;
collisionPoint = p2 + edge*f;
}
}
// p3 -> p1:
edge = p1-p3;
baseToVertex = p3 - base;
edgeSquaredLength = edge.squaredLength();
edgeDotVelocity = edge.dot(velocity);
edgeDotBaseToVertex = edge.dot(baseToVertex);
a = edgeSquaredLength*-velocitySquaredLength + edgeDotVelocity*edgeDotVelocity;
b = edgeSquaredLength*(2*velocity.dot(baseToVertex)) - 2.0*edgeDotVelocity*edgeDotBaseToVertex;
c = edgeSquaredLength*(1-baseToVertex.squaredLength()) + 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;
collisionPoint = p3 + edge*f;
}
}
}
// Set result:
if (foundCollison == true)
{
// distance to collision: 't' is time of collision
float distToCollision = t*colPackage->velocity.length();
// Does this triangle qualify for the closest hit?
if (distToCollision < colPackage->nearestDistance)
{
// Collision information nessesary for sliding
colPackage->nearestDistance = distToCollision;
colPackage->intersectionPoint=collisionPoint;
colPackage->foundCollision = true;
// trigger collection of triangle index when leave
colPackage->triangleindex = -2;
}
}
} // if not backface
#endif
}
double PLANE::signedDistanceTo(VECTOR point)
{
return (point.dot(normal)) + equation[3];
}
