C++ (Cpp) PxMtdNormal Examples

Example #1

File: CollisionConversions.cpp Project: colwalder/unrealengine

static bool ComputeInflatedMTD_Internal(const float MtdInflation, const PxLocationHit& PHit, FHitResult& OutResult, const PxTransform& QueryTM, const PxGeometry& Geom, const PxTransform& PShapeWorldPose)
{
	PxGeometry* InflatedGeom = NULL;

	PxVec3 PxMtdNormal(0.f);
	PxF32 PxMtdDepth = 0.f;
	const PxGeometry& POtherGeom = PHit.shape->getGeometry().any();
	const bool bMtdResult = PxGeometryQuery::computePenetration(PxMtdNormal, PxMtdDepth, Geom, QueryTM, POtherGeom, PShapeWorldPose);
	if (bMtdResult)
	{
		if (PxMtdNormal.isFinite())
		{
			OutResult.ImpactNormal = P2UVector(PxMtdNormal);
			OutResult.PenetrationDepth = FMath::Max(FMath::Abs(PxMtdDepth) - MtdInflation, 0.f) + KINDA_SMALL_NUMBER;
			return true;
		}
		else
		{
			UE_LOG(LogPhysics, Verbose, TEXT("Warning: ComputeInflatedMTD_Internal: MTD returned NaN :( normal: (X:%f, Y:%f, Z:%f)"), PxMtdNormal.x, PxMtdNormal.y, PxMtdNormal.z);
		}
	}

	return false;
}

Example #2

File: CollisionConversions.cpp Project: Tigrouzen/UnrealEngine-4

/** Util to convert an overlapped shape into a sweep hit result, returns whether it was a blocking hit. */
static bool ConvertOverlappedShapeToImpactHit(const PxShape* PShape,  const PxRigidActor* PActor, const FVector& StartLoc, const FVector& EndLoc, FHitResult& OutResult, const PxGeometry& Geom, const PxTransform& QueryTM, const PxFilterData& QueryFilter, bool bReturnPhysMat, uint32 FaceIdx)
{
	OutResult.TraceStart = StartLoc;
	OutResult.TraceEnd = EndLoc;

	SetHitResultFromShapeAndFaceIndex(PShape, PActor, FaceIdx, OutResult, bReturnPhysMat);

	// Time of zero because initially overlapping
	OutResult.Time = 0.f;
	OutResult.bStartPenetrating = true;

	// See if this is a 'blocking' hit
	PxFilterData PShapeFilter = PShape->getQueryFilterData();
	PxSceneQueryHitType::Enum HitType = FPxQueryFilterCallback::CalcQueryHitType(QueryFilter, PShapeFilter);
	OutResult.bBlockingHit = (HitType == PxSceneQueryHitType::eBLOCK); 

	// Return start location as 'safe location'
	OutResult.Location = P2UVector(QueryTM.p);
	OutResult.ImpactPoint = OutResult.Location; // @todo not really sure of a better thing to do here...

	const PxTransform PShapeWorldPose = PxShapeExt::getGlobalPose(*PShape, *PActor); 


	PxTriangleMeshGeometry PTriMeshGeom;
	if(PShape->getTriangleMeshGeometry(PTriMeshGeom))
	{
		PxU32 HitTris[64];
		bool bOverflow = false;
		int32 NumTrisHit = PxMeshQuery::findOverlapTriangleMesh(Geom, QueryTM, PTriMeshGeom, PShapeWorldPose, HitTris, 64, 0, bOverflow);

#if DRAW_OVERLAPPING_TRIS
		TArray<FOverlapResult> Overlaps;
		DrawGeomOverlaps(World, Geom, QueryTM, Overlaps);

		TArray<FBatchedLine> Lines;
		const FLinearColor LineColor = FLinearColor(1.f,0.7f,0.7f);
		const FLinearColor NormalColor = FLinearColor(1.f,1.f,1.f);
		const float Lifetime = 5.f;
#endif // DRAW_OVERLAPPING_TRIS

		// Track the best triangle plane distance
		float BestPlaneDist = -BIG_NUMBER;
		FVector BestPlaneNormal(0,0,1);
		FVector BestPointOnPlane(0,0,0);
		// Iterate over triangles
		for(int32 TriIdx = 0; TriIdx<NumTrisHit; TriIdx++)
		{
			PxTriangle Tri;
			PxMeshQuery::getTriangle(PTriMeshGeom, PShapeWorldPose, HitTris[TriIdx], Tri);

			const FVector A = P2UVector(Tri.verts[0]);
			const FVector B = P2UVector(Tri.verts[1]);
			const FVector C = P2UVector(Tri.verts[2]);

			FVector TriNormal = ((B-A) ^ (C-A));

			// Use a more accurate normalization that avoids InvSqrtEst
			const float TriNormalSize = TriNormal.Size();
			TriNormal = (TriNormalSize >= KINDA_SMALL_NUMBER ? TriNormal/TriNormalSize : FVector::ZeroVector);

			const FPlane TriPlane(A, TriNormal);

			const FVector QueryCenter = P2UVector(QueryTM.p);
			const float DistToPlane = TriPlane.PlaneDot(QueryCenter);

			if(DistToPlane > BestPlaneDist)
			{
				BestPlaneDist = DistToPlane;
				BestPlaneNormal = TriNormal;
				BestPointOnPlane = A;
			}

#if DRAW_OVERLAPPING_TRIS
			Lines.Add(FBatchedLine(A, B, LineColor, Lifetime, 0.1f, SDPG_Foreground));
			Lines.Add(FBatchedLine(B, C, LineColor, Lifetime, 0.1f, SDPG_Foreground));
			Lines.Add(FBatchedLine(C, A, LineColor, Lifetime, 0.1f, SDPG_Foreground));
			Lines.Add(FBatchedLine(A, A+(50.f*TriNormal), NormalColor, Lifetime, 0.1f, SDPG_Foreground));
#endif // DRAW_OVERLAPPING_TRIS
		}

#if DRAW_OVERLAPPING_TRIS
		if ( World->PersistentLineBatcher )
		{
			World->PersistentLineBatcher->DrawLines(Lines);
		}
#endif // DRAW_OVERLAPPING_TRIS

		OutResult.ImpactNormal = BestPlaneNormal;
	}
	else
	{
		// use vector center of shape to query as good direction to move in
		PxGeometry& PGeom = PShape->getGeometry().any();
		PxVec3 PClosestPoint;
		float Distance = PxGeometryQuery::pointDistance(QueryTM.p, PGeom, PShapeWorldPose, &PClosestPoint);

		if(Distance < KINDA_SMALL_NUMBER)
		{
			//UE_LOG(LogCollision, Warning, TEXT("ConvertOverlappedShapeToImpactHit: Query origin inside shape, giving poor MTD."));			
			PClosestPoint = PxShapeExt::getWorldBounds(*PShape, *PActor).getCenter(); 
		}

		OutResult.ImpactNormal = (OutResult.Location - P2UVector(PClosestPoint)).SafeNormal();
	}

	// Compute depenetration vector and distance if possible.
	PxVec3 PxMtdNormal(0.f);
	PxF32 PxMtdDepth = 0.f;
	PxGeometry& POtherGeom = PShape->getGeometry().any();

	const bool bMtdResult = PxGeometryQuery::computePenetration(PxMtdNormal, PxMtdDepth, Geom, QueryTM, POtherGeom, PShapeWorldPose);
	if (bMtdResult)
	{
		const FVector MtdNormal = P2UVector(PxMtdNormal);
		OutResult.Normal = MtdNormal;
		OutResult.PenetrationDepth = FMath::Abs(PxMtdDepth) + KINDA_SMALL_NUMBER; // TODO: why are we getting negative values here from mtd sometimes?
	}
	else
	{
		OutResult.Normal = OutResult.ImpactNormal;
	}

	return OutResult.bBlockingHit;
}