bool FPhysScene::GetKinematicTarget_AssumesLocked(const FBodyInstance* BodyInstance, FTransform& OutTM) const
{
#if WITH_PHYSX
if (PxRigidDynamic * PRigidDynamic = BodyInstance->GetPxRigidDynamic_AssumesLocked())
{
#if WITH_SUBSTEPPING
uint32 BodySceneType = SceneType_AssumesLocked(BodyInstance);
if (IsSubstepping(BodySceneType))
{
FPhysSubstepTask * PhysSubStepper = PhysSubSteppers[BodySceneType];
return PhysSubStepper->GetKinematicTarget_AssumesLocked(BodyInstance, OutTM);
}
else
#endif
{
PxTransform POutTM;
bool validTM = PRigidDynamic->getKinematicTarget(POutTM);
if (validTM)
{
OutTM = P2UTransform(POutTM);
return true;
}
}
}
#endif
return false;
}
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;
}
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;
}
/** 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;
}
void UDestructibleComponent::UpdateDestructibleChunkTM(const TArray<const PxRigidActor*>& ActiveActors)
{
//We want to consolidate the transforms so that we update each destructible component once by passing it an array of chunks to update.
//This helps avoid a lot of duplicated work like marking render dirty, computing inverse world component, etc...
TMap<UDestructibleComponent*, TArray<FUpdateChunksInfo> > ComponentUpdateMapping;
//prepare map to update destructible components
TArray<PxShape*> Shapes;
for (const PxRigidActor* RigidActor : ActiveActors)
{
if (const FDestructibleChunkInfo* DestructibleChunkInfo = FPhysxUserData::Get<FDestructibleChunkInfo>(RigidActor->userData))
{
if (GApexModuleDestructible->owns(RigidActor) && DestructibleChunkInfo->OwningComponent.IsValid())
{
Shapes.AddUninitialized(RigidActor->getNbShapes());
int32 NumShapes = RigidActor->getShapes(Shapes.GetData(), Shapes.Num());
for (int32 ShapeIdx = 0; ShapeIdx < Shapes.Num(); ++ShapeIdx)
{
PxShape* Shape = Shapes[ShapeIdx];
int32 ChunkIndex;
if (NxDestructibleActor* DestructibleActor = GApexModuleDestructible->getDestructibleAndChunk(Shape, &ChunkIndex))
{
const physx::PxMat44 ChunkPoseRT = DestructibleActor->getChunkPose(ChunkIndex);
const physx::PxTransform Transform(ChunkPoseRT);
if (UDestructibleComponent* DestructibleComponent = Cast<UDestructibleComponent>(FPhysxUserData::Get<UPrimitiveComponent>(DestructibleActor->userData)))
{
if (DestructibleComponent->IsRegistered())
{
TArray<FUpdateChunksInfo>& UpdateInfos = ComponentUpdateMapping.FindOrAdd(DestructibleComponent);
FUpdateChunksInfo* UpdateInfo = new (UpdateInfos)FUpdateChunksInfo(ChunkIndex, P2UTransform(Transform));
}
}
}
}
Shapes.Empty(Shapes.Num()); //we want to keep largest capacity array to avoid reallocs
}
}
}
//update each component
for (auto It = ComponentUpdateMapping.CreateIterator(); It; ++It)
{
UDestructibleComponent* DestructibleComponent = It.Key();
TArray<FUpdateChunksInfo>& UpdateInfos = It.Value();
if (DestructibleComponent->IsFracturedOrInitiallyStatic())
{
DestructibleComponent->SetChunksWorldTM(UpdateInfos);
}
else
{
//if we haven't fractured it must mean that we're simulating a destructible and so we should update our ComponentToWorld based on the single rigid body
DestructibleComponent->SyncComponentToRBPhysics();
}
}
}
bool UDestructibleComponent::DoCustomNavigableGeometryExport(FNavigableGeometryExport& GeomExport) const
{
#if WITH_APEX
if (ApexDestructibleActor == NULL)
{
return false;
}
NxDestructibleActor* DestrActor = const_cast<NxDestructibleActor*>(ApexDestructibleActor);
const FTransform ComponentToWorldNoScale(ComponentToWorld.GetRotation(), ComponentToWorld.GetTranslation(), FVector(1.f));
TArray<PxShape*> Shapes;
Shapes.AddUninitialized(8);
PxRigidDynamic** PActorBuffer = NULL;
PxU32 PActorCount = 0;
if (DestrActor->acquirePhysXActorBuffer(PActorBuffer, PActorCount
, NxDestructiblePhysXActorQueryFlags::Static
| NxDestructiblePhysXActorQueryFlags::Dormant
| NxDestructiblePhysXActorQueryFlags::Dynamic))
{
uint32 ShapesExportedCount = 0;
while (PActorCount--)
{
const PxRigidDynamic* PActor = *PActorBuffer++;
if (PActor != NULL)
{
const FTransform PActorGlobalPose = P2UTransform(PActor->getGlobalPose());
const PxU32 ShapesCount = PActor->getNbShapes();
if (ShapesCount > PxU32(Shapes.Num()))
{
Shapes.AddUninitialized(ShapesCount - Shapes.Num());
}
const PxU32 RetrievedShapesCount = PActor->getShapes(Shapes.GetData(), Shapes.Num());
PxShape* const* ShapePtr = Shapes.GetData();
for (PxU32 ShapeIndex = 0; ShapeIndex < RetrievedShapesCount; ++ShapeIndex, ++ShapePtr)
{
if (*ShapePtr != NULL)
{
const PxTransform LocalPose = (*ShapePtr)->getLocalPose();
FTransform LocalToWorld = P2UTransform(LocalPose);
LocalToWorld.Accumulate(PActorGlobalPose);
switch((*ShapePtr)->getGeometryType())
{
case PxGeometryType::eCONVEXMESH:
{
PxConvexMeshGeometry Geometry;
if ((*ShapePtr)->getConvexMeshGeometry(Geometry))
{
++ShapesExportedCount;
// @todo address Geometry.scale not being used here
GeomExport.ExportPxConvexMesh(Geometry.convexMesh, LocalToWorld);
}
}
break;
case PxGeometryType::eTRIANGLEMESH:
{
// @todo address Geometry.scale not being used here
PxTriangleMeshGeometry Geometry;
if ((*ShapePtr)->getTriangleMeshGeometry(Geometry))
{
++ShapesExportedCount;
if ((Geometry.triangleMesh->getTriangleMeshFlags()) & PxTriangleMeshFlag::eHAS_16BIT_TRIANGLE_INDICES)
{
GeomExport.ExportPxTriMesh16Bit(Geometry.triangleMesh, LocalToWorld);
}
else
{
GeomExport.ExportPxTriMesh32Bit(Geometry.triangleMesh, LocalToWorld);
}
}
}
default:
{
UE_LOG(LogPhysics, Log, TEXT("UDestructibleComponent::DoCustomNavigableGeometryExport(): unhandled PxGeometryType, %d.")
, int32((*ShapePtr)->getGeometryType()));
}
break;
}
}
}
}
}
ApexDestructibleActor->releasePhysXActorBuffer();
INC_DWORD_STAT_BY(STAT_Navigation_DestructiblesShapesExported, ShapesExportedCount);
}
#endif // WITH_APEX
// we don't want a regular geometry export
return false;
}
void UPhysicsHandleComponent::GrabComponent(UPrimitiveComponent* InComponent, FName InBoneName, FVector Location, bool bConstrainRotation)
{
// If we are already holding something - drop it first.
if(GrabbedComponent != NULL)
{
ReleaseComponent();
}
if(!InComponent)
{
return;
}
#if WITH_PHYSX
// Get the PxRigidDynamic that we want to grab.
FBodyInstance* BodyInstance = InComponent->GetBodyInstance(InBoneName);
if (!BodyInstance)
{
return;
}
PxRigidDynamic* Actor = BodyInstance->GetPxRigidDynamic();
if (!Actor)
return;
// Get the scene the PxRigidDynamic we want to grab is in.
PxScene* Scene = Actor->getScene();
check(Scene);
// Get transform of actor we are grabbing
PxVec3 KinLocation = U2PVector(Location);
PxTransform GrabbedActorPose = Actor->getGlobalPose();
PxTransform KinPose(KinLocation, GrabbedActorPose.q);
// set target and current, so we don't need another "Tick" call to have it right
TargetTransform = CurrentTransform = P2UTransform(KinPose);
// If we don't already have a handle - make one now.
if (!HandleData)
{
// Create kinematic actor we are going to create joint with. This will be moved around with calls to SetLocation/SetRotation.
PxRigidDynamic* KinActor = Scene->getPhysics().createRigidDynamic(KinPose);
KinActor->setRigidDynamicFlag(PxRigidDynamicFlag::eKINEMATIC, true);
KinActor->setMass(1.0f);
KinActor->setMassSpaceInertiaTensor(PxVec3(1.0f, 1.0f, 1.0f));
// No bodyinstance
KinActor->userData = NULL;
// Add to Scene
Scene->addActor(*KinActor);
// Save reference to the kinematic actor.
KinActorData = KinActor;
// Create the joint
PxVec3 LocalHandlePos = GrabbedActorPose.transformInv(KinLocation);
PxD6Joint* NewJoint = PxD6JointCreate(Scene->getPhysics(), KinActor, PxTransform::createIdentity(), Actor, PxTransform(LocalHandlePos));
if(!NewJoint)
{
HandleData = 0;
}
else
{
// No constraint instance
NewJoint->userData = NULL;
HandleData = NewJoint;
// Remember the scene index that the handle joint/actor are in.
FPhysScene* RBScene = FPhysxUserData::Get<FPhysScene>(Scene->userData);
const uint32 SceneType = InComponent->BodyInstance.UseAsyncScene() ? PST_Async : PST_Sync;
SceneIndex = RBScene->PhysXSceneIndex[SceneType];
// Setting up the joint
NewJoint->setMotion(PxD6Axis::eX, PxD6Motion::eFREE);
NewJoint->setMotion(PxD6Axis::eY, PxD6Motion::eFREE);
NewJoint->setMotion(PxD6Axis::eZ, PxD6Motion::eFREE);
NewJoint->setDrive(PxD6Drive::eX, PxD6JointDrive(LinearStiffness, LinearDamping, PX_MAX_F32, PxD6JointDriveFlag::eACCELERATION));
NewJoint->setDrive(PxD6Drive::eY, PxD6JointDrive(LinearStiffness, LinearDamping, PX_MAX_F32, PxD6JointDriveFlag::eACCELERATION));
NewJoint->setDrive(PxD6Drive::eZ, PxD6JointDrive(LinearStiffness, LinearDamping, PX_MAX_F32, PxD6JointDriveFlag::eACCELERATION));
NewJoint->setDrivePosition(PxTransform(PxVec3(0,0,0)));
NewJoint->setMotion(PxD6Axis::eTWIST, PxD6Motion::eFREE);
NewJoint->setMotion(PxD6Axis::eSWING1, PxD6Motion::eFREE);
NewJoint->setMotion(PxD6Axis::eSWING2, PxD6Motion::eFREE);
bRotationConstrained = bConstrainRotation;
if (bRotationConstrained)
{
NewJoint->setDrive(PxD6Drive::eSLERP, PxD6JointDrive(AngularStiffness, AngularDamping, PX_MAX_F32, PxD6JointDriveFlag::eACCELERATION));
//NewJoint->setDrive(PxD6Drive::eTWIST, PxD6JointDrive(AngularStiffness, AngularDamping, PX_MAX_F32, PxD6JointDriveFlag::eACCELERATION));
//NewJoint->setDrive(PxD6Drive::eSWING, PxD6JointDrive(AngularStiffness, AngularDamping, PX_MAX_F32, PxD6JointDriveFlag::eACCELERATION));
//PosJointDesc.setGlobalAxis(NxVec3(0,0,1));
}
}
}
#endif // WITH_PHYSX
GrabbedComponent = InComponent;
GrabbedBoneName = InBoneName;
}
