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