Vector3 PlaneEdgeIntersection(const Plane& plane, const Vector3& start, const Vector3& end)
{
float start_dist = Vector3::Dot(start, plane.GetNormal()) + plane.GetDistance();
float end_dist = Vector3::Dot(end, plane.GetNormal()) + plane.GetDistance();
Vector3 ab = end - start;
float ab_p = Vector3::Dot(plane.GetNormal(), ab);
if (fabs(ab_p) > 0.0001f)
{
Vector3 p_co = plane.GetNormal() * (-plane.GetDistance());
Vector3 w = start - p_co;
float fac = -Vector3::Dot(plane.GetNormal(), w) / ab_p;
ab = ab * fac;
return start + ab;
}
return start;
}
ZED_BOOL Ray::Intersects( const Plane &p_Plane, ZED_BOOL p_Cull,
ZED_FLOAT32 *p_Length, Vector3 *p_HitPos )
{
Vector3 Normal;
p_Plane.GetNormal( &Normal );
ZED_FLOAT32 Dist = Normal.Dot( m_Direction );
if( Arithmetic::IsZero( Dist ) )
{
return ZED_FALSE;
}
// Check if the plane normal is facing away from the ray's
// direction
if( p_Cull && ( Dist > ZED_Epsilon ) )
{
return ZED_FALSE;
}
ZED_FLOAT32 Origin = -( ( Normal.Dot( m_Origin ) ) +
p_Plane.GetDistance( ) );
ZED_FLOAT32 Length = Origin / Dist;
// Intersection before ray origin
if( Length < ( -ZED_Epsilon ) )
{
return ZED_FALSE;
}
if( p_HitPos )
{
( *p_HitPos ).Copy( m_Origin + ( m_Direction*Length ) );
// As OpenGL uses a forward Z, negate it to get the correct
// result
( *p_HitPos )[ 2 ] = m_Origin[ 2 ]-( m_Direction[ 2 ]*Length );
( *p_HitPos ).Clean( );
}
if( p_Length )
{
( *p_Length ) = Length;
}
return ZED_TRUE;
}
/**
* @brief
* Calculates the side the polygon is on
*/
Polygon::ESide Polygon::GetSide(const Plane &cPlane) const
{
uint32 nNumInFront = 0, nNumBehind = 0;
for (uint32 i=0; i<m_lstVertices.GetNumOfElements(); i++) {
float fDistance = cPlane.GetDistance(m_lstVertices[i]);
if (fDistance > 0.00001)
nNumInFront++;
else if (fDistance < -0.00001)
nNumBehind++;
}
// Return plane side the polygon is on
if (nNumInFront && !nNumBehind)
return InFront;
if (!nNumInFront && nNumBehind)
return Behind;
if (!nNumInFront && !nNumBehind)
return Coinciding;
else
return Spanning;
}
bool Ray::Intersects(const Plane& plane, F32* distance) const {
Vector3f planeNormal = plane.GetNormal();
Vector3f rayEnd = origin + direction * this->distance;
XMFLOAT4 f4PlaneCoefs(planeNormal.x, planeNormal.y, planeNormal.z, plane.GetDistance());
XMFLOAT3 f3RayBegin(origin.v);
XMFLOAT3 f3RayEnd(rayEnd.v);
XMVECTOR vPlaneCoefs = XMLoadFloat4(&f4PlaneCoefs);
XMVECTOR vRayBegin = XMLoadFloat3(&f3RayBegin);
XMVECTOR vRayEnd = XMLoadFloat3(&f3RayEnd);
XMVECTOR vIntersection = XMPlaneIntersectLine(vPlaneCoefs, vRayBegin, vRayEnd);
XMFLOAT3 f3Intersection;
XMStoreFloat3(&f3Intersection, vIntersection);
if (IsNaN(f3Intersection.x) || IsNaN(f3Intersection.y) || IsNaN(f3Intersection.z)) {
return false;
} else {
*distance = Vector3f::Distance(origin, Vector3f(f3Intersection.x, f3Intersection.y, f3Intersection.z));
return true;
}
}
/**
* @brief
* Returns the side of the plane the scene hierarchy node volume is on
*/
char SceneHierarchyNode::GetPlaneSide(const Plane &cPlane) const
{
Vector3 vVertex[8];
uint8 nInFront = 0;
uint8 nInBack = 0;
// Check plane side
m_cAABoundingBox.GetVertices(vVertex);
for (uint8 i=0; i<8; i++) {
if (cPlane.GetDistance(vVertex[i]) > 0.0f)
nInFront++;
else
nInBack++;
}
// Return plane side
if (nInFront == 8)
return 1;
else if (nInBack == 8)
return -1;
else
return 0;
}
void CollisionDetection::BuildCollisionManifold(float dt, PhysicsObject* obj1, PhysicsObject* obj2, CollisionShape* shape1, CollisionShape* shape2, const CollisionData& coldata, Manifold* manifold)
{
if (!manifold)
return;
//Get the required face information for the two shapes around the collision normal
std::list<Vector3> polygon1, polygon2;
Vector3 normal1, normal2;
std::vector<Plane> adjPlanes1, adjPlanes2;
shape1->GetIncidentReferencePolygon(obj1, coldata.normal, &polygon1, &normal1, &adjPlanes1);
shape2->GetIncidentReferencePolygon(obj2, -coldata.normal, &polygon2, &normal2, &adjPlanes2);
//If either shape1 or shape2 returned a single point, then it must be on a curve and thus the only contact is already availble
if (polygon1.size() <= 1)
{
if (polygon1.size() == 1)
{
manifold->AddContact(dt, polygon1.front(), polygon1.front() - coldata.normal * coldata.penetration, coldata.normal, coldata.penetration);
}
}
else if (polygon2.size() == 1)
{
manifold->AddContact(dt, polygon2.front() + coldata.normal * coldata.penetration, polygon2.front(), coldata.normal, coldata.penetration);
}
else
{
//Otherwise use clipping to cut down the incident face to fit inside the reference planes using the surrounding face planes
bool flipped;
std::list<Vector3> *incPolygon;
Vector3 *incNormal;
std::vector<Plane> *refAdjPlanes;
Plane refPlane;
//Get the incident and reference polygons
if (fabs(Vector3::Dot(coldata.normal, normal1)) <= fabs(Vector3::Dot(coldata.normal, normal2)))
{
float planeDist = -Vector3::Dot(normal1, polygon1.front());
refPlane = Plane(normal1, planeDist);
refAdjPlanes = &adjPlanes1;
incPolygon = &polygon2;
incNormal = &normal2;
flipped = false;
}
else
{
float planeDist = -Vector3::Dot(normal2, polygon2.front());
refPlane = Plane(normal2, planeDist);
refAdjPlanes = &adjPlanes2;
incPolygon = &polygon1;
incNormal = &normal1;
flipped = true;
}
//Clip the Polygon
SutherlandHodgesonClipping(*incPolygon, *refAdjPlanes, incPolygon);
//Now we are left with a selection of valid contact points to be used for the manifold
Vector3 startPoint = incPolygon->back();
for (const Vector3& endPoint : *incPolygon)
{
float contact_penetration;
Vector3 globalOnA, globalOnB;
//Calculate distance to ref plane/face
contact_penetration = -fabs(Vector3::Dot(endPoint, refPlane.GetNormal()) + refPlane.GetDistance());
contact_penetration = min(contact_penetration, coldata.penetration);
if (flipped)
{
globalOnA = endPoint - coldata.normal * contact_penetration;
globalOnB = endPoint;
}
else
{
globalOnA = endPoint;
globalOnB = endPoint + coldata.normal * contact_penetration;
}
contact_penetration = -(fabs(contact_penetration) + fabs(coldata.penetration)) * 0.5f;
manifold->AddContact(dt, globalOnA, globalOnB, coldata.normal, contact_penetration);
startPoint = endPoint;
}
}
}
Boole Overlap(const Plane& Surface, const Sphere& Ball)
{
return ( MathTools::Abs( Surface.GetDistance( Ball.Center ) ) <= Ball.Radius );
}
float Vector3::GetDistanceToPlane ( const Plane & aPlane ) const
{
return Dot ( aPlane.GetNormal(), *this ) - aPlane.GetDistance();
}
/**
* @brief
* Splits the polygon
*/
bool Polygon::Split(const Plane &cSplitter, Polygon &cFrontPolygon, Polygon &cBackPolygon)
{
// Init splitted polygons
cFrontPolygon.GetVertexList().Clear();
cBackPolygon.GetVertexList().Clear();
// Call custom init function
CustomInit(cFrontPolygon);
CustomInit(cBackPolygon);
// Loop through vertices
if (m_lstVertices.GetNumOfElements()) {
Vector3 *pV1 = &m_lstVertices[m_lstVertices.GetNumOfElements()-1];
float fV1Distance = cSplitter.GetDistance(*pV1);
for (uint32 i=0; i<m_lstVertices.GetNumOfElements(); i++) {
Vector3 *pV2 = &m_lstVertices[i];
float fV2Distance = cSplitter.GetDistance(*pV2);
Vector3 vDelta = (*pV2)-(*pV1);
float fDistance = fV1Distance/(Math::Abs(fV1Distance)+Math::Abs(fV2Distance));
if (fV2Distance > Math::Epsilon) { // In front of splitter plane
if (fV1Distance < Math::Epsilon) { // Line is splitted
Vector3 vRes = (*pV1)+vDelta*(-fDistance);
cFrontPolygon.GetVertexList().Add(vRes);
cBackPolygon.GetVertexList().Add(vRes);
// Call custom split function
CustomSplit(cFrontPolygon, cBackPolygon, i, -fDistance);
}
cFrontPolygon.GetVertexList().Add(*pV2);
// Call custom add function
CustomAdd(cFrontPolygon, i);
} else if (fV2Distance < Math::Epsilon) { // Behind splitter plane
if (fV1Distance > Math::Epsilon) { // Line is splitted
Vector3 vRes = (*pV1)+vDelta*fDistance;
cFrontPolygon.GetVertexList().Add(vRes);
cBackPolygon.GetVertexList().Add(vRes);
// Call custom split function
CustomSplit(cFrontPolygon, cBackPolygon, i, fDistance);
}
cBackPolygon.GetVertexList().Add(*pV2);
// Call custom add function
CustomAdd(cBackPolygon, i);
} else {
cFrontPolygon.GetVertexList().Add(*pV2);
cBackPolygon.GetVertexList().Add(*pV2);
// Call custom add functions
CustomAdd(cFrontPolygon, i);
CustomAdd(cBackPolygon, i);
}
pV1 = pV2;
fV1Distance = fV2Distance;
}
}
// Compute planes
cFrontPolygon.ComputePlane();
cBackPolygon.ComputePlane();
// Done
return true;
}