C++ FVector::IsNormalized方法代码示例

void DrawDebugDirectionalArrow(const UWorld* InWorld, FVector const& LineStart, FVector const& LineEnd, float ArrowSize, FColor const& Color, bool bPersistentLines, float LifeTime, uint8 DepthPriority)
{
	// no debug line drawing on dedicated server
	if (GEngine->GetNetMode(InWorld) != NM_DedicatedServer)
	{
		if (ArrowSize <= 0)
		{
			ArrowSize = 10.f;
		}

		DrawDebugLine(InWorld, LineStart, LineEnd, Color, bPersistentLines, LifeTime);

		FVector Dir = (LineEnd-LineStart);
		Dir.Normalize();
		FVector Up(0, 0, 1);
		FVector Right = Dir ^ Up;
		if (!Right.IsNormalized())
		{
			Dir.FindBestAxisVectors(Up, Right);
		}
		FVector Origin = FVector::ZeroVector;
		FMatrix TM;
		// get matrix with dir/right/up
		TM.SetAxes(&Dir, &Right, &Up, &Origin);

		// since dir is x direction, my arrow will be pointing +y, -x and -y, -x
		float ArrowSqrt = FMath::Sqrt(ArrowSize);
		FVector ArrowPos;
		DrawDebugLine(InWorld, LineEnd, LineEnd + TM.TransformPosition(FVector(-ArrowSqrt, ArrowSqrt, 0)), Color, bPersistentLines, LifeTime, DepthPriority);
		DrawDebugLine(InWorld, LineEnd, LineEnd + TM.TransformPosition(FVector(-ArrowSqrt, -ArrowSqrt, 0)), Color, bPersistentLines, LifeTime, DepthPriority);
	}
}

static FVector FindConvexMeshOpposingNormal(const PxLocationHit& PHit, const FVector& TraceDirectionDenorm, const FVector InNormal)
{
	if (IsInvalidFaceIndex(PHit.faceIndex))
	{
		return InNormal;
	}

	PxConvexMeshGeometry PConvexMeshGeom;
	bool bSuccess = PHit.shape->getConvexMeshGeometry(PConvexMeshGeom);
	check(bSuccess);	//should only call this function when we have a convex mesh

	if (PConvexMeshGeom.convexMesh)
	{
		check(PHit.faceIndex < PConvexMeshGeom.convexMesh->getNbPolygons());

		const PxU32 PolyIndex = PHit.faceIndex;
		PxHullPolygon PPoly;
		bool bSuccessData = PConvexMeshGeom.convexMesh->getPolygonData(PolyIndex, PPoly);
		if (bSuccessData)
		{
			// Account for non-uniform scale in local space normal.
			const PxVec3 PPlaneNormal(PPoly.mPlane[0], PPoly.mPlane[1], PPoly.mPlane[2]);
			const PxVec3 PLocalPolyNormal = TransformNormalToShapeSpace(PConvexMeshGeom.scale, PPlaneNormal.getNormalized());

			// Convert to world space
			const PxTransform PShapeWorldPose = PxShapeExt::getGlobalPose(*PHit.shape, *PHit.actor);
			const PxVec3 PWorldPolyNormal = PShapeWorldPose.rotate(PLocalPolyNormal);
			const FVector OutNormal = P2UVector(PWorldPolyNormal);

#if !(UE_BUILD_SHIPPING || UE_BUILD_TEST)
			if (!OutNormal.IsNormalized())
			{
				UE_LOG(LogPhysics, Warning, TEXT("Non-normalized Normal (Hit shape is ConvexMesh): %s (LocalPolyNormal:%s)"), *OutNormal.ToString(), *P2UVector(PLocalPolyNormal).ToString());
				UE_LOG(LogPhysics, Warning, TEXT("WorldTransform \n: %s"), *P2UTransform(PShapeWorldPose).ToString());
			}
#endif
			return OutNormal;
		}
	}

	return InNormal;
}

void ConvertQueryImpactHit(const UWorld* World, const PxLocationHit& PHit, FHitResult& OutResult, float CheckLength, const PxFilterData& QueryFilter, const FVector& StartLoc, const FVector& EndLoc, const PxGeometry* const Geom, const PxTransform& QueryTM, bool bReturnFaceIndex, bool bReturnPhysMat)
{
	SCOPE_CYCLE_COUNTER(STAT_ConvertQueryImpactHit);

	checkSlow(PHit.flags & PxHitFlag::eDISTANCE);
	const bool bInitialOverlap = PHit.hadInitialOverlap();
	if (bInitialOverlap && Geom != nullptr)
	{
		ConvertOverlappedShapeToImpactHit(World, PHit, StartLoc, EndLoc, OutResult, *Geom, QueryTM, QueryFilter, bReturnPhysMat);
		return;
	}

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

	// calculate the hit time
	const float HitTime = PHit.distance/CheckLength;
	OutResult.Time = HitTime;
	OutResult.Distance = PHit.distance;

	// figure out where the the "safe" location for this shape is by moving from the startLoc toward the ImpactPoint
	const FVector TraceStartToEnd = EndLoc - StartLoc;
	const FVector SafeLocationToFitShape = StartLoc + (HitTime * TraceStartToEnd);
	OutResult.Location = SafeLocationToFitShape;

	const bool bUsePxPoint = ((PHit.flags & PxHitFlag::ePOSITION) && !bInitialOverlap);
	OutResult.ImpactPoint = bUsePxPoint ? P2UVector(PHit.position) : StartLoc;
	
	// Caution: we may still have an initial overlap, but with null Geom. This is the case for RayCast results.
	const bool bUsePxNormal = ((PHit.flags & PxHitFlag::eNORMAL) && !bInitialOverlap);
	FVector Normal = bUsePxNormal ? P2UVector(PHit.normal).GetSafeNormal() : -TraceStartToEnd.GetSafeNormal();
	OutResult.Normal = Normal;
	OutResult.ImpactNormal = Normal;

	OutResult.TraceStart = StartLoc;
	OutResult.TraceEnd = EndLoc;


#if ENABLE_CHECK_HIT_NORMAL
	CheckHitResultNormal(OutResult, TEXT("Invalid Normal from ConvertQueryImpactHit"), StartLoc, EndLoc, Geom);
#endif // ENABLE_CHECK_HIT_NORMAL

	if (bUsePxNormal && !Normal.IsNormalized())
	{
		// TraceStartToEnd should never be zero, because of the length restriction in the raycast and sweep tests.
		Normal = -TraceStartToEnd.GetSafeNormal();
		OutResult.Normal = Normal;
		OutResult.ImpactNormal = Normal;
	}

	const PxGeometryType::Enum SweptGeometryType = Geom ? Geom->getType() : PxGeometryType::eINVALID;
	OutResult.ImpactNormal = FindGeomOpposingNormal(SweptGeometryType, PHit, TraceStartToEnd, Normal);
	
	// Fill in Actor, Component, material, etc.
	SetHitResultFromShapeAndFaceIndex(PHit.shape, PHit.actor, PHit.faceIndex, OutResult, bReturnPhysMat);

	if( PHit.shape->getGeometryType() == PxGeometryType::eHEIGHTFIELD)
	{
		// Lookup physical material for heightfields
		if (bReturnPhysMat && PHit.faceIndex != InvalidQueryHit.faceIndex)
		{
			PxMaterial* HitMaterial = PHit.shape->getMaterialFromInternalFaceIndex(PHit.faceIndex);
			if (HitMaterial != NULL)
			{
				OutResult.PhysMaterial = FPhysxUserData::Get<UPhysicalMaterial>(HitMaterial->userData);
			}
		}
	}
	else
	if(bReturnFaceIndex && PHit.shape->getGeometryType() == PxGeometryType::eTRIANGLEMESH)
	{
		PxTriangleMeshGeometry PTriMeshGeom;
		if(	PHit.shape->getTriangleMeshGeometry(PTriMeshGeom) && 
			PTriMeshGeom.triangleMesh != NULL &&
			PHit.faceIndex < PTriMeshGeom.triangleMesh->getNbTriangles() )
		{
			OutResult.FaceIndex	= PTriMeshGeom.triangleMesh->getTrianglesRemap()[PHit.faceIndex];
		}
	}
}

static FVector FindTriMeshOpposingNormal(const PxLocationHit& PHit, const FVector& TraceDirectionDenorm, const FVector InNormal)
{
	if (IsInvalidFaceIndex(PHit.faceIndex))
	{
		return InNormal;
	}

	PxTriangleMeshGeometry PTriMeshGeom;
	bool bSuccess = PHit.shape->getTriangleMeshGeometry(PTriMeshGeom);
	check(bSuccess);	//this function should only be called when we have a trimesh

	if (PTriMeshGeom.triangleMesh)
	{
		check(PHit.faceIndex < PTriMeshGeom.triangleMesh->getNbTriangles());

		const PxU32 TriIndex = PHit.faceIndex;
		const void* Triangles = PTriMeshGeom.triangleMesh->getTriangles();

		// Grab triangle indices that we hit
		int32 I0, I1, I2;

		if (PTriMeshGeom.triangleMesh->getTriangleMeshFlags() & PxTriangleMeshFlag::eHAS_16BIT_TRIANGLE_INDICES)
		{
			PxU16* P16BitIndices = (PxU16*)Triangles;
			I0 = P16BitIndices[(TriIndex * 3) + 0];
			I1 = P16BitIndices[(TriIndex * 3) + 1];
			I2 = P16BitIndices[(TriIndex * 3) + 2];
		}
		else
		{
			PxU32* P32BitIndices = (PxU32*)Triangles;
			I0 = P32BitIndices[(TriIndex * 3) + 0];
			I1 = P32BitIndices[(TriIndex * 3) + 1];
			I2 = P32BitIndices[(TriIndex * 3) + 2];
		}

		// Get verts we hit (local space)
		const PxVec3* PVerts = PTriMeshGeom.triangleMesh->getVertices();
		const PxVec3 V0 = PVerts[I0];
		const PxVec3 V1 = PVerts[I1];
		const PxVec3 V2 = PVerts[I2];

		// Find normal of triangle (local space), and account for non-uniform scale
		const PxVec3 PTempNormal = ((V1 - V0).cross(V2 - V0)).getNormalized();
		const PxVec3 PLocalTriNormal = TransformNormalToShapeSpace(PTriMeshGeom.scale, PTempNormal);

		// Convert to world space
		const PxTransform PShapeWorldPose = PxShapeExt::getGlobalPose(*PHit.shape, *PHit.actor);
		const PxVec3 PWorldTriNormal = PShapeWorldPose.rotate(PLocalTriNormal);
		FVector OutNormal = P2UVector(PWorldTriNormal);

		if (PTriMeshGeom.meshFlags & PxMeshGeometryFlag::eDOUBLE_SIDED)
		{
			//double sided mesh so we need to consider direction of query
			const float sign = FVector::DotProduct(OutNormal, TraceDirectionDenorm) > 0.f ? -1.f : 1.f;
			OutNormal *= sign;
		}

#if !(UE_BUILD_SHIPPING || UE_BUILD_TEST)
		if (!OutNormal.IsNormalized())
		{
			UE_LOG(LogPhysics, Warning, TEXT("Non-normalized Normal (Hit shape is TriangleMesh): %s (V0:%s, V1:%s, V2:%s)"), *OutNormal.ToString(), *P2UVector(V0).ToString(), *P2UVector(V1).ToString(), *P2UVector(V2).ToString());
			UE_LOG(LogPhysics, Warning, TEXT("WorldTransform \n: %s"), *P2UTransform(PShapeWorldPose).ToString());
		}
#endif
		return OutNormal;
	}

	return InNormal;
}

EConvertQueryResult ConvertQueryImpactHit(const UWorld* World, const PxLocationHit& PHit, FHitResult& OutResult, float CheckLength, const PxFilterData& QueryFilter, const FVector& StartLoc, const FVector& EndLoc, const PxGeometry* const Geom, const PxTransform& QueryTM, bool bReturnFaceIndex, bool bReturnPhysMat)
{
	SCOPE_CYCLE_COUNTER(STAT_ConvertQueryImpactHit);

#if WITH_EDITOR
	if(bReturnFaceIndex && World->IsGameWorld())
	{
		if(!ensure(UPhysicsSettings::Get()->bSuppressFaceRemapTable == false))
		{
			UE_LOG(LogPhysics, Error, TEXT("A scene query is relying on face indices, but bSuppressFaceRemapTable is true."));
			bReturnFaceIndex = false;
		}
	}
#endif

	checkSlow(PHit.flags & PxHitFlag::eDISTANCE);
	const bool bInitialOverlap = PHit.hadInitialOverlap();
	if (bInitialOverlap && Geom != nullptr)
	{
		ConvertOverlappedShapeToImpactHit(World, PHit, StartLoc, EndLoc, OutResult, *Geom, QueryTM, QueryFilter, bReturnPhysMat);
		return EConvertQueryResult::Valid;
	}

	// See if this is a 'blocking' hit
	const PxFilterData PShapeFilter = PHit.shape->getQueryFilterData();
	const PxQueryHitType::Enum HitType = FPxQueryFilterCallback::CalcQueryHitType(QueryFilter, PShapeFilter);
	OutResult.bBlockingHit = (HitType == PxQueryHitType::eBLOCK); 
	OutResult.bStartPenetrating = bInitialOverlap;

	// calculate the hit time
	const float HitTime = PHit.distance/CheckLength;
	OutResult.Time = HitTime;
	OutResult.Distance = PHit.distance;

	// figure out where the the "safe" location for this shape is by moving from the startLoc toward the ImpactPoint
	const FVector TraceStartToEnd = EndLoc - StartLoc;
	const FVector SafeLocationToFitShape = StartLoc + (HitTime * TraceStartToEnd);
	OutResult.Location = SafeLocationToFitShape;

	const bool bUsePxPoint = ((PHit.flags & PxHitFlag::ePOSITION) && !bInitialOverlap);
	if (bUsePxPoint && !PHit.position.isFinite())
	{
#if ENABLE_NAN_DIAGNOSTIC
		SetHitResultFromShapeAndFaceIndex(PHit.shape, PHit.actor, PHit.faceIndex, OutResult, bReturnPhysMat);
		UE_LOG(LogCore, Error, TEXT("ConvertQueryImpactHit() NaN details:\n>> Actor:%s (%s)\n>> Component:%s\n>> Item:%d\n>> BoneName:%s\n>> Time:%f\n>> Distance:%f\n>> Location:%s\n>> bIsBlocking:%d\n>> bStartPenetrating:%d"),
			*GetNameSafe(OutResult.GetActor()), OutResult.Actor.IsValid() ? *OutResult.GetActor()->GetPathName() : TEXT("no path"),
			*GetNameSafe(OutResult.GetComponent()), OutResult.Item, *OutResult.BoneName.ToString(),
			OutResult.Time, OutResult.Distance, *OutResult.Location.ToString(), OutResult.bBlockingHit ? 1 : 0, OutResult.bStartPenetrating ? 1 : 0);
#endif // ENABLE_NAN_DIAGNOSTIC

		OutResult.Reset();
		logOrEnsureNanError(TEXT("ConvertQueryImpactHit() received NaN/Inf for position: %.2f %.2f %.2f"), PHit.position.x, PHit.position.y, PHit.position.z);
		return EConvertQueryResult::Invalid;
	}

	OutResult.ImpactPoint = bUsePxPoint ? P2UVector(PHit.position) : StartLoc;
	
	// Caution: we may still have an initial overlap, but with null Geom. This is the case for RayCast results.
	const bool bUsePxNormal = ((PHit.flags & PxHitFlag::eNORMAL) && !bInitialOverlap);
	if (bUsePxNormal && !PHit.normal.isFinite())
	{
#if ENABLE_NAN_DIAGNOSTIC
		SetHitResultFromShapeAndFaceIndex(PHit.shape, PHit.actor, PHit.faceIndex, OutResult, bReturnPhysMat);
		UE_LOG(LogCore, Error, TEXT("ConvertQueryImpactHit() NaN details:\n>> Actor:%s (%s)\n>> Component:%s\n>> Item:%d\n>> BoneName:%s\n>> Time:%f\n>> Distance:%f\n>> Location:%s\n>> bIsBlocking:%d\n>> bStartPenetrating:%d"),
			*GetNameSafe(OutResult.GetActor()), OutResult.Actor.IsValid() ? *OutResult.GetActor()->GetPathName() : TEXT("no path"),
			*GetNameSafe(OutResult.GetComponent()), OutResult.Item, *OutResult.BoneName.ToString(),
			OutResult.Time, OutResult.Distance, *OutResult.Location.ToString(), OutResult.bBlockingHit ? 1 : 0, OutResult.bStartPenetrating ? 1 : 0);
#endif // ENABLE_NAN_DIAGNOSTIC

		OutResult.Reset();
		logOrEnsureNanError(TEXT("ConvertQueryImpactHit() received NaN/Inf for normal: %.2f %.2f %.2f"), PHit.normal.x, PHit.normal.y, PHit.normal.z);
		return EConvertQueryResult::Invalid;
	}

	FVector Normal = bUsePxNormal ? P2UVector(PHit.normal).GetSafeNormal() : -TraceStartToEnd.GetSafeNormal();
	OutResult.Normal = Normal;
	OutResult.ImpactNormal = Normal;

	OutResult.TraceStart = StartLoc;
	OutResult.TraceEnd = EndLoc;


#if ENABLE_CHECK_HIT_NORMAL
	CheckHitResultNormal(OutResult, TEXT("Invalid Normal from ConvertQueryImpactHit"), StartLoc, EndLoc, Geom);
#endif // ENABLE_CHECK_HIT_NORMAL

	if (bUsePxNormal && !Normal.IsNormalized())
	{
		// TraceStartToEnd should never be zero, because of the length restriction in the raycast and sweep tests.
		Normal = -TraceStartToEnd.GetSafeNormal();
		OutResult.Normal = Normal;
		OutResult.ImpactNormal = Normal;
	}

	const PxGeometryType::Enum SweptGeometryType = Geom ? Geom->getType() : PxGeometryType::eINVALID;
	OutResult.ImpactNormal = FindGeomOpposingNormal(SweptGeometryType, PHit, TraceStartToEnd, Normal);

	// Fill in Actor, Component, material, etc.
	SetHitResultFromShapeAndFaceIndex(PHit.shape, PHit.actor, PHit.faceIndex, OutResult, bReturnPhysMat);

//.........这里部分代码省略.........

/** Util to find the normal of the face that we hit */
static bool FindGeomOpposingNormal(const PxLocationHit& PHit, FVector& OutNormal, FVector& OutPointOnGeom)
{
	PxTriangleMeshGeometry PTriMeshGeom;
	PxConvexMeshGeometry PConvexMeshGeom;
	if(	PHit.shape->getTriangleMeshGeometry(PTriMeshGeom) && 
		PTriMeshGeom.triangleMesh != NULL &&
		PHit.faceIndex < PTriMeshGeom.triangleMesh->getNbTriangles() )
	{
		const int32 TriIndex = PHit.faceIndex;
		const void* Triangles = PTriMeshGeom.triangleMesh->getTriangles();

		// Grab triangle indices that we hit
		int32 I0, I1, I2;

		if(PTriMeshGeom.triangleMesh->getTriangleMeshFlags() & PxTriangleMeshFlag::eHAS_16BIT_TRIANGLE_INDICES) 
		{
			PxU16* P16BitIndices = (PxU16*)Triangles;
			I0 = P16BitIndices[(TriIndex*3)+0];
			I1 = P16BitIndices[(TriIndex*3)+1];
			I2 = P16BitIndices[(TriIndex*3)+2];
		}
		else
		{
			PxU32* P32BitIndices = (PxU32*)Triangles;
			I0 = P32BitIndices[(TriIndex*3)+0];
			I1 = P32BitIndices[(TriIndex*3)+1];
			I2 = P32BitIndices[(TriIndex*3)+2];
		}

		// Get verts we hit (local space)
		const PxVec3* PVerts = PTriMeshGeom.triangleMesh->getVertices();
		const PxVec3 V0 = PVerts[I0];
		const PxVec3 V1 = PVerts[I1];
		const PxVec3 V2 = PVerts[I2];

		// Find normal of triangle, and convert into world space
		PxVec3 PLocalTriNormal = ((V1 - V0).cross(V2 - V0)).getNormalized();
		TransformNormalToShapeSpace(PTriMeshGeom.scale, PLocalTriNormal, PLocalTriNormal);
	
		const PxTransform PShapeWorldPose = PxShapeExt::getGlobalPose(*PHit.shape, *PHit.actor); 

		const PxVec3 PWorldTriNormal = PShapeWorldPose.rotate(PLocalTriNormal);
		OutNormal = P2UVector(PWorldTriNormal);
		
		if (PTriMeshGeom.scale.isIdentity())
		{
			OutPointOnGeom = P2UVector(PShapeWorldPose.transform(V0));
		}

#if !(UE_BUILD_SHIPPING || UE_BUILD_TEST)
		if (!OutNormal.IsNormalized())
		{
			UE_LOG(LogPhysics, Warning, TEXT("Non-normalized Normal (Hit shape is TriangleMesh): %s (V0:%s, V1:%s, V2:%s)"), *OutNormal.ToString(), *P2UVector(V0).ToString(), *P2UVector(V1).ToString(), *P2UVector(V2).ToString());
			UE_LOG(LogPhysics, Warning, TEXT("WorldTransform \n: %s"), *P2UTransform(PShapeWorldPose).ToString());
		}
#endif
		return true;
	}
	else if(PHit.shape->getConvexMeshGeometry(PConvexMeshGeom) && 
		PConvexMeshGeom.convexMesh != NULL &&
		PHit.faceIndex < PConvexMeshGeom.convexMesh->getNbPolygons() )
	{
		const int32 PolyIndex = PHit.faceIndex;
		PxHullPolygon PPoly;
		bool bSuccess = PConvexMeshGeom.convexMesh->getPolygonData(PolyIndex, PPoly);
		if(bSuccess)
		{
			PxVec3 PPlaneNormal(PPoly.mPlane[0], PPoly.mPlane[1], PPoly.mPlane[2]);

			// Convert to local space, taking scale into account.
			// TODO: can we just change the hit normal within the physx code where we choose the most opposing normal, and avoid doing this here again?
			PxVec3 PLocalPolyNormal;
			TransformNormalToShapeSpace(PConvexMeshGeom.scale, PPlaneNormal.getNormalized(), PLocalPolyNormal);			
			const PxTransform PShapeWorldPose = PHit.actor->getGlobalPose() * PHit.shape->getLocalPose(); 

			const PxVec3 PWorldPolyNormal = PShapeWorldPose.rotate(PLocalPolyNormal);
			OutNormal = P2UVector(PWorldPolyNormal);

#if !(UE_BUILD_SHIPPING || UE_BUILD_TEST)
			if (!OutNormal.IsNormalized())
			{
				UE_LOG(LogPhysics, Warning, TEXT("Non-normalized Normal (Hit shape is ConvexMesh): %s (LocalPolyNormal:%s)"), *OutNormal.ToString(), *P2UVector(PLocalPolyNormal).ToString());
				UE_LOG(LogPhysics, Warning, TEXT("WorldTransform \n: %s"), *P2UTransform(PShapeWorldPose).ToString());
			}
#endif
			return true;
		}
	}

	return false;
}