C++ (Cpp) PxVec3::normalizeSafe Examples

Example #1

File: CollisionConversions.cpp Project: colwalder/unrealengine

// Compute depenetration vector and distance if possible with a slightly larger geometry
static bool ComputeInflatedMTD(const float MtdInflation, const PxLocationHit& PHit, FHitResult& OutResult, const PxTransform& QueryTM, const PxGeometry& Geom, const PxTransform& PShapeWorldPose)
{
	switch(Geom.getType())
	{
		case PxGeometryType::eCAPSULE:
		{
			const PxCapsuleGeometry* InCapsule = static_cast<const PxCapsuleGeometry*>(&Geom);
			PxCapsuleGeometry InflatedCapsule(InCapsule->radius + MtdInflation, InCapsule->halfHeight); // don't inflate halfHeight, radius is added all around.
			return ComputeInflatedMTD_Internal(MtdInflation, PHit, OutResult, QueryTM, InflatedCapsule, PShapeWorldPose);
		}

		case PxGeometryType::eBOX:
		{
			const PxBoxGeometry* InBox = static_cast<const PxBoxGeometry*>(&Geom);
			PxBoxGeometry InflatedBox(InBox->halfExtents + PxVec3(MtdInflation));
			return ComputeInflatedMTD_Internal(MtdInflation, PHit, OutResult, QueryTM, InflatedBox, PShapeWorldPose);
		}

		case PxGeometryType::eSPHERE:
		{
			const PxSphereGeometry* InSphere = static_cast<const PxSphereGeometry*>(&Geom);
			PxSphereGeometry InflatedSphere(InSphere->radius + MtdInflation);
			return ComputeInflatedMTD_Internal(MtdInflation, PHit, OutResult, QueryTM, InflatedSphere, PShapeWorldPose);
		}

		case PxGeometryType::eCONVEXMESH:
		{
			// We can't exactly inflate the mesh (not easily), so try jittering it a bit to get an MTD result.
			PxVec3 TraceDir = U2PVector(OutResult.TraceEnd - OutResult.TraceStart);
			TraceDir.normalizeSafe();

			// Try forward (in trace direction)
			PxTransform JitteredTM = PxTransform(QueryTM.p + (TraceDir * MtdInflation), QueryTM.q);
			if (ComputeInflatedMTD_Internal(MtdInflation, PHit, OutResult, JitteredTM, Geom, PShapeWorldPose))
			{
				return true;
			}

			// Try backward (opposite trace direction)
			JitteredTM = PxTransform(QueryTM.p - (TraceDir * MtdInflation), QueryTM.q);
			if (ComputeInflatedMTD_Internal(MtdInflation, PHit, OutResult, JitteredTM, Geom, PShapeWorldPose))
			{
				return true;
			}

			// Try axial directions.
			// Start with -Z because this is the most common case (objects on the floor).
			for (int32 i=2; i >= 0; i--)
			{
				PxVec3 Jitter(0.f);
				Jitter[i] = MtdInflation;

				JitteredTM = PxTransform(QueryTM.p - Jitter, QueryTM.q);
				if (ComputeInflatedMTD_Internal(MtdInflation, PHit, OutResult, JitteredTM, Geom, PShapeWorldPose))
				{
					return true;
				}

				JitteredTM = PxTransform(QueryTM.p + Jitter, QueryTM.q);
				if (ComputeInflatedMTD_Internal(MtdInflation, PHit, OutResult, JitteredTM, Geom, PShapeWorldPose))
				{
					return true;
				}
			}

			return false;
		}

		default:
		{
			return false;
		}
	}
}