/** Loc is an anchor point in the world to guide which part of the infinite plane to draw. */
void DrawDebugSolidPlane(const UWorld* InWorld, FPlane const& P, FVector const& Loc, float Size, FColor const& Color, bool bPersistent, float LifeTime, uint8 DepthPriority)
{
// no debug line drawing on dedicated server
if (GEngine->GetNetMode(InWorld) != NM_DedicatedServer)
{
FVector const ClosestPtOnPlane = Loc - P.PlaneDot(Loc) * P;
FVector U, V;
P.FindBestAxisVectors(U, V);
U *= Size;
V *= Size;
TArray<FVector> Verts;
Verts.AddUninitialized(4);
Verts[0] = ClosestPtOnPlane + U + V;
Verts[1] = ClosestPtOnPlane - U + V;
Verts[2] = ClosestPtOnPlane + U - V;
Verts[3] = ClosestPtOnPlane - U - V;
TArray<int32> Indices;
Indices.AddUninitialized(6);
Indices[0] = 0; Indices[1] = 2; Indices[2] = 1;
Indices[3] = 1; Indices[4] = 2; Indices[5] = 3;
// plane quad
DrawDebugMesh(InWorld, Verts, Indices, Color, bPersistent, LifeTime, DepthPriority);
// arrow indicating normal
DrawDebugDirectionalArrow(InWorld, ClosestPtOnPlane, ClosestPtOnPlane + P * 16.f, 8.f, FColor::White, bPersistent, LifeTime, DepthPriority);
}
}
/**
* Convert Convex Normal to Capsule Normal
* For capsule, we'd like to get normal of capsule rather than convex, so the normal can be smooth when we move character
*
* @param HalfHeight : HalfHeight of the capsule
* @param Radius : Radius of the capsule
* @param OutHit : The result of hit from physX
@ @param PointOnGeom : If not null, use this point and the impact normal to recompute the impact point (which can yield a better normal).
* @result OutHit.Normal is converted to Capsule Normal
*/
static void ConvertConvexNormalToCapsuleNormal(float HalfHeight, float Radius, struct FHitResult& OutHit, const FVector* PointOnGeom)
{
if ( OutHit.Time > 0.f )
{
// Impact point is contact point where it meet
const float ContactZ = OutHit.ImpactPoint.Z;
// Hit.Location is center of the object
const FVector CenterOfCapsule = OutHit.Location;
// We're looking for end of the normal for this capsule
FVector NormalEnd = CenterOfCapsule;
// see if it's upper hemisphere
if (ContactZ > CenterOfCapsule.Z + HalfHeight)
{
// if upper hemisphere, the normal should point to the center of upper hemisphere
NormalEnd.Z = CenterOfCapsule.Z + HalfHeight;
}
else if (ContactZ < CenterOfCapsule.Z - HalfHeight)
{
// if lower hemisphere, the normal should point to the center of lower hemisphere
NormalEnd.Z = CenterOfCapsule.Z - HalfHeight;
}
else
{
// otherwise, normal end is going to be same as contact point
// since it's the tube section of the capsule
NormalEnd.Z = ContactZ;
}
// Recompute ImpactPoint if requested
if (PointOnGeom)
{
const FPlane GeomPlane(*PointOnGeom, OutHit.ImpactNormal);
const float DistToPlane = GeomPlane.PlaneDot(NormalEnd);
if (DistToPlane >= Radius)
{
OutHit.ImpactPoint = NormalEnd - DistToPlane * OutHit.ImpactNormal;
}
}
//DrawDebugLine(ContactPoint, NormalEnd, FColor(0, 255, 0), true);
//DrawDebugLine(NormalEnd, CenterOfCapsule, FColor(0, 0, 255), true);
FVector ContactNormal = (NormalEnd - OutHit.ImpactPoint);
// if ContactNormal is not nearly zero, set it
// otherwise use previous normal
const float Size = ContactNormal.Size();
if (Size >= KINDA_SMALL_NUMBER)
{
ContactNormal /= Size;
OutHit.Normal = ContactNormal;
}
#if !(UE_BUILD_SHIPPING || UE_BUILD_TEST || !WITH_EDITOR)
CheckHitResultNormal(OutHit, TEXT("ConvertConvexNormalToCapsuleNormal"));
#endif
}
}
void vtLevel::GetEdgePlane(uint i, FPlane &plane)
{
vtEdge *edge = m_Edges[i];
int islope = edge->m_iSlope;
float slope = (islope / 180.0f * PIf);
int index = i;
int ring = m_LocalFootprint.WhichRing(index);
FLine3 &loop = m_LocalFootprint[ring];
uint ring_edges = loop.GetSize();
int next = (index+1 == ring_edges) ? 0 : index+1;
// get edge vector
FPoint3 vec = loop[next] - loop[index];
vec.Normalize();
// get perpendicular (upward pointing) vector
FPoint3 perp;
perp.Set(0, 1, 0);
// create rotation matrix to rotate it upward
FMatrix4 mat;
mat.Identity();
mat.AxisAngle(vec, slope);
// create normal
FPoint3 norm;
mat.TransformVector(perp, norm);
plane.Set(loop[index], norm);
}
FPoly FPoly::BuildInfiniteFPoly(const FPlane& InPlane)
{
FVector Axis1, Axis2;
// Find two non-problematic axis vectors.
InPlane.FindBestAxisVectors( Axis1, Axis2 );
// Set up the FPoly.
FPoly EdPoly;
EdPoly.Init();
EdPoly.Normal.X = InPlane.X;
EdPoly.Normal.Y = InPlane.Y;
EdPoly.Normal.Z = InPlane.Z;
EdPoly.Base = EdPoly.Normal * InPlane.W;
EdPoly.Vertices.Add( EdPoly.Base + Axis1*HALF_WORLD_MAX + Axis2*HALF_WORLD_MAX );
EdPoly.Vertices.Add( EdPoly.Base - Axis1*HALF_WORLD_MAX + Axis2*HALF_WORLD_MAX );
EdPoly.Vertices.Add( EdPoly.Base - Axis1*HALF_WORLD_MAX - Axis2*HALF_WORLD_MAX );
EdPoly.Vertices.Add( EdPoly.Base + Axis1*HALF_WORLD_MAX - Axis2*HALF_WORLD_MAX );
return EdPoly;
}
void ConvertQueryImpactHit(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)
{
if (Geom != NULL && PHit.hadInitialOverlap())
{
ConvertOverlappedShapeToImpactHit(PHit.shape, PHit.actor, StartLoc, EndLoc, OutResult, *Geom, QueryTM, QueryFilter, bReturnPhysMat, PHit.faceIndex);
return;
}
SetHitResultFromShapeAndFaceIndex(PHit.shape, PHit.actor, PHit.faceIndex, OutResult, bReturnPhysMat);
// calculate the hit time
const float HitTime = PHit.distance/CheckLength;
// figure out where the the "safe" location for this shape is by moving from the startLoc toward the ImpactPoint
const FVector TraceDir = EndLoc - StartLoc;
const FVector SafeLocationToFitShape = StartLoc + (HitTime * TraceDir);
// Other info
OutResult.Location = SafeLocationToFitShape;
OutResult.ImpactPoint = P2UVector(PHit.position);
OutResult.Normal = P2UVector(PHit.normal);
OutResult.ImpactNormal = OutResult.Normal;
OutResult.TraceStart = StartLoc;
OutResult.TraceEnd = EndLoc;
OutResult.Time = HitTime;
// See if this is a 'blocking' hit
PxFilterData PShapeFilter = PHit.shape->getQueryFilterData();
PxSceneQueryHitType::Enum HitType = FPxQueryFilterCallback::CalcQueryHitType(QueryFilter, PShapeFilter);
OutResult.bBlockingHit = (HitType == PxSceneQueryHitType::eBLOCK);
OutResult.bStartPenetrating = (PxU32)(PHit.hadInitialOverlap());
#if !(UE_BUILD_SHIPPING || UE_BUILD_TEST || !WITH_EDITOR)
CheckHitResultNormal(OutResult, TEXT("Invalid Normal from ConvertQueryImpactHit"), StartLoc, EndLoc, Geom);
#endif
// Special handling for swept-capsule results
if(Geom && Geom->getType() == PxGeometryType::eCAPSULE)
{
const PxCapsuleGeometry* Capsule = static_cast<const PxCapsuleGeometry*>(Geom);
// Compute better ImpactNormal. This is the same as Normal except when we hit convex/trimesh, and then its the most opposing normal of the geom at point of impact.
FVector PointOnGeom(P2UVector(PHit.position));
if (FindGeomOpposingNormal(PHit, OutResult.ImpactNormal, PointOnGeom))
{
ConvertConvexNormalToCapsuleNormal(Capsule->halfHeight, Capsule->radius, OutResult, &PointOnGeom);
}
else
{
ConvertConvexNormalToCapsuleNormal(Capsule->halfHeight, Capsule->radius, OutResult, NULL);
OutResult.ImpactNormal = OutResult.Normal;
}
}
else if (Geom && Geom->getType() == PxGeometryType::eSPHERE)
{
const PxSphereGeometry* Sphere = static_cast<const PxSphereGeometry*>(Geom);
// Compute better ImpactNormal. This is the same as Normal except when we hit convex/trimesh, and then its the most opposing normal of the geom at point of impact.
FVector PointOnGeom(P2UVector(PHit.position));
if (FindGeomOpposingNormal(PHit, OutResult.ImpactNormal, PointOnGeom))
{
const FPlane GeomPlane(PointOnGeom, OutResult.ImpactNormal);
const float DistFromPlane = FMath::Abs(GeomPlane.PlaneDot(OutResult.Location));
if (DistFromPlane >= Sphere->radius)
{
// Use the new (better) impact normal to compute a new impact point by projecting the sphere's location onto the geometry.
OutResult.ImpactPoint = OutResult.Location - DistFromPlane * OutResult.ImpactNormal;
}
// Use the impact point to compute a better sphere surface normal (impact point to center of sphere).
ConvertConvexNormalToSphereNormal(Sphere->radius, OutResult);
}
else
{
ConvertConvexNormalToSphereNormal(Sphere->radius, OutResult);
OutResult.ImpactNormal = OutResult.Normal;
}
}
if( PHit.shape->getGeometryType() == PxGeometryType::eHEIGHTFIELD)
{
// Lookup physical material for heightfields
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;
PxConvexMeshGeometry PConvexMeshGeom;
if( PHit.shape->getTriangleMeshGeometry(PTriMeshGeom) &&
PTriMeshGeom.triangleMesh != NULL &&
PHit.faceIndex < PTriMeshGeom.triangleMesh->getNbTriangles() )
{
OutResult.FaceIndex = PTriMeshGeom.triangleMesh->getTrianglesRemap()[PHit.faceIndex];
}
else
{
OutResult.FaceIndex = INDEX_NONE;
}
}
else
{
OutResult.FaceIndex = INDEX_NONE;
}
}
int32 FPoly::SplitWithPlaneFast
(
const FPlane& Plane,
FPoly* FrontPoly,
FPoly* BackPoly
) const
{
FMemMark MemMark(FMemStack::Get());
enum EPlaneClassification
{
V_FRONT=0,
V_BACK=1
};
EPlaneClassification Status,PrevStatus;
EPlaneClassification* VertStatus = new(FMemStack::Get()) EPlaneClassification[Vertices.Num()];
int32 Front=0,Back=0;
EPlaneClassification* StatusPtr = &VertStatus[0];
for( int32 i=0; i<Vertices.Num(); i++ )
{
float Dist = Plane.PlaneDot(Vertices[i]);
if( Dist >= 0.f )
{
*StatusPtr++ = V_FRONT;
if( Dist > +THRESH_SPLIT_POLY_WITH_PLANE )
Front=1;
}
else
{
*StatusPtr++ = V_BACK;
if( Dist < -THRESH_SPLIT_POLY_WITH_PLANE )
Back=1;
}
}
ESplitType Result;
if( !Front )
{
if( Back ) Result = SP_Back;
else Result = SP_Coplanar;
}
else if( !Back )
{
Result = SP_Front;
}
else
{
// Split.
if( FrontPoly )
{
const FVector *V = Vertices.GetData();
const FVector *W = V + Vertices.Num()-1;
FVector *V1 = FrontPoly->Vertices.GetData();
FVector *V2 = BackPoly ->Vertices.GetData();
StatusPtr = &VertStatus [0];
PrevStatus = VertStatus [Vertices.Num()-1];
for( int32 i=0; i<Vertices.Num(); i++ )
{
Status = *StatusPtr++;
if( Status != PrevStatus )
{
// Crossing.
const FVector& Intersection = FMath::LinePlaneIntersection( *W, *V, Plane );
new(FrontPoly->Vertices) FVector(Intersection);
new(BackPoly->Vertices) FVector(Intersection);
if( PrevStatus == V_FRONT )
{
new(BackPoly->Vertices) FVector(*V);
}
else
{
new(FrontPoly->Vertices) FVector(*V);
}
}
else if( Status==V_FRONT )
{
new(FrontPoly->Vertices) FVector(*V);
}
else
{
new(BackPoly->Vertices) FVector(*V);
}
PrevStatus = Status;
W = V++;
}
FrontPoly->Base = Base;
FrontPoly->Normal = Normal;
FrontPoly->PolyFlags = PolyFlags;
BackPoly->Base = Base;
BackPoly->Normal = Normal;
BackPoly->PolyFlags = PolyFlags;
}
Result = SP_Split;
}
MemMark.Pop();
return Result;
}
