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