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);
PxGeometryType::Enum PGeomType = PHit.shape->getGeometryType();
if(PGeomType == 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 && PGeomType == PxGeometryType::eTRIANGLEMESH)
{
PxTriangleMeshGeometry PTriMeshGeom;
if( PHit.shape->getTriangleMeshGeometry(PTriMeshGeom) &&
PTriMeshGeom.triangleMesh != NULL &&
PHit.faceIndex < PTriMeshGeom.triangleMesh->getNbTriangles() )
{
if (const PxU32* TriangleRemap = PTriMeshGeom.triangleMesh->getTrianglesRemap())
{
OutResult.FaceIndex = TriangleRemap[PHit.faceIndex];
}
}
}
return EConvertQueryResult::Valid;
}
/** 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;
}
