C++ Plane::GetDistance方法代码示例

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