bool BasicPrimitiveTests::IntersectingSegmentAgainstPlane(const LineSegment & segment, const Plane & plane, float & rtn_t)
{
/*
Main Idea:
-> Subsitutes line equation into plane to find intersection
-> Tests if intersection is within segment endpoints
Let:
-> Line Segment S:
-> S(t) = A + t * (B-A)
-> for 0 <= t <= 1
-> Plane P:
-> dot(n, X) = d
Solve for "t" of intersection:
-> t = ( d - dot(n, A) / dot(n, B-A) )
-> Return intersection only if:
-> 0 <= t <= 1
*/
rtn_t = ( plane.GetD() - plane.GetNormal().dot(segment.GetPointA()) ) / plane.GetNormal().dot(segment.GetPointB() - segment.GetPointA());
return (rtn_t <= 1.0f && rtn_t >= 0.0f);
}
bool Ray::GetIntersection(const Plane &plane, float &t) const
{
real denom = glm::dot(plane.GetNormal(), mDirection);
if(!denom) {
return false;
}
real numer = -(glm::dot(plane.GetNormal(), mOrigin) + plane.GetD());
t = numer / denom;
if(t >= 0) {
return true;
}
return false;
}
bool Ray::IntersectPlane( Plane a_plane )
{
v3 normal = a_plane.GetNormal();
const float vdotn = dot( m_dir, normal );
if ( fabs(vdotn) <= EPSILON )
return false;
return NewHit( - dot( m_pos - m_pos, normal ) / vdotn, normal );
}
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;
}
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;
}
}
void Camera::ViewportToPlaneVertex(float x, float y, IntersectionPlane p, Vector3& v) const
{
v = Vector3::Zero;
// unit length dir
Vector3 dir;
GetDirection(dir);
// persp/ortho pos
Vector3 pos;
ViewportToWorldVertex(x, y, pos);
// re-use v to be our camera normal
ViewportToWorldNormal(x, y, v);
// extend to clip distance
v *= FarClipDistance;
// Pick ray from our starting location
Line line = Line (pos, pos + v);
// plane to pick against
Plane plane = Plane::Null;
// compute plane
switch (p)
{
case IntersectionPlanes::Viewport:
{
switch (m_ProjectionMode)
{
case ProjectionModes::Perspective:
{
// Planar intersections of the ray with the direction plane (camera plane)
plane = Plane (pos + (dir * m_Offset), dir);
break;
}
case ProjectionModes::Orthographic:
{
// Planar intersections of the ray with the direction plane (camera plane)
plane = Plane (Vector3::Zero, dir);
break;
}
}
break;
}
case IntersectionPlanes::Ground:
{
plane = Plane (Vector3::Zero, UpVector);
break;
}
}
plane.GetNormal(v);
// shouldn't happen
if (v != Vector3::Zero)
{
// Planar intersections of the ray with the direction plane (camera plane)
line.IntersectsPlane(plane, &v);
}
}
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;
}
}
}
float Vector3::GetDistanceToPlane ( const Plane & aPlane ) const
{
return Dot ( aPlane.GetNormal(), *this ) - aPlane.GetDistance();
}