/** Function for setting linear motor parameters. */
void FConstraintInstance::SetLinearDriveParams(float InSpring, float InDamping, float InForceLimit)
{
#if WITH_PHYSX
if (bLinearPositionDrive || bLinearVelocityDrive)
{
if (PxD6Joint* Joint = GetUnbrokenJoint())
{
// X-Axis linear drive
const float DriveSpringX = bLinearPositionDrive && bLinearXPositionDrive ? InSpring : 0.0f;
const float DriveDampingX = (bLinearVelocityDrive && LinearVelocityTarget.X != 0.f) ? InDamping : 0.0f;
const float LinearForceLimit = InForceLimit > 0.f ? InForceLimit : PX_MAX_F32;
Joint->setDrive(PxD6Drive::eX, PxD6JointDrive(DriveSpringX, DriveDampingX, LinearForceLimit, bIsAccelerationDrive));
// Y-Axis linear drive
const float DriveSpringY = bLinearPositionDrive && bLinearYPositionDrive ? InSpring : 0.0f;
const float DriveDampingY = (bLinearVelocityDrive && LinearVelocityTarget.Y != 0.f) ? InDamping : 0.0f;
Joint->setDrive(PxD6Drive::eY, PxD6JointDrive(DriveSpringY, DriveDampingY, LinearForceLimit, bIsAccelerationDrive));
// Z-Axis linear drive
const float DriveSpringZ = bLinearPositionDrive && bLinearZPositionDrive ? InSpring : 0.0f;
const float DriveDampingZ = (bLinearVelocityDrive && LinearVelocityTarget.Z != 0.f) ? InDamping : 0.0f;
Joint->setDrive(PxD6Drive::eZ, PxD6JointDrive(DriveSpringZ, DriveDampingZ, LinearForceLimit, bIsAccelerationDrive));
}
}
#endif
LinearDriveSpring = InSpring;
LinearDriveDamping = InDamping;
LinearDriveForceLimit = InForceLimit;
}
/** Function for setting linear motor parameters. */
void FConstraintInstance::SetLinearDriveParams(float InSpring, float InDamping, float InForceLimit)
{
#if WITH_PHYSX
if (bLinearPositionDrive || bLinearVelocityDrive)
{
ExecuteOnUnbrokenJointReadWrite([&] (PxD6Joint* Joint)
{
// X-Axis linear drive
const float DriveSpringX = bLinearPositionDrive && bLinearXPositionDrive ? InSpring : 0.0f;
const float DriveDampingX = bLinearVelocityDrive && bLinearXVelocityDrive ? InDamping : 0.0f;
const float LinearForceLimit = InForceLimit > 0.f ? InForceLimit : PX_MAX_F32;
Joint->setDrive(PxD6Drive::eX, PxD6JointDrive(DriveSpringX, DriveDampingX, LinearForceLimit, bIsAccelerationDrive));
// Y-Axis linear drive
const float DriveSpringY = bLinearPositionDrive && bLinearYPositionDrive ? InSpring : 0.0f;
const float DriveDampingY = bLinearVelocityDrive && bLinearYVelocityDrive ? InDamping : 0.0f;
Joint->setDrive(PxD6Drive::eY, PxD6JointDrive(DriveSpringY, DriveDampingY, LinearForceLimit, bIsAccelerationDrive));
// Z-Axis linear drive
const float DriveSpringZ = bLinearPositionDrive && bLinearZPositionDrive ? InSpring : 0.0f;
const float DriveDampingZ = bLinearVelocityDrive && bLinearZVelocityDrive ? InDamping : 0.0f;
Joint->setDrive(PxD6Drive::eZ, PxD6JointDrive(DriveSpringZ, DriveDampingZ, LinearForceLimit, bIsAccelerationDrive));
});
}
#endif
LinearDriveSpring = InSpring;
LinearDriveDamping = InDamping;
LinearDriveForceLimit = InForceLimit;
}
// D6 joint with a spring maintaining its position
PxJoint* createDampedD6(PxRigidActor* a0, const PxTransform& t0, PxRigidActor* a1, const PxTransform& t1)
{
PxD6Joint* j = PxD6JointCreate(*gPhysics, a0, t0, a1, t1);
j->setMotion(PxD6Axis::eSWING1, PxD6Motion::eFREE);
j->setMotion(PxD6Axis::eSWING2, PxD6Motion::eFREE);
j->setMotion(PxD6Axis::eTWIST, PxD6Motion::eFREE);
j->setDrive(PxD6Drive::eSLERP, PxD6JointDrive(0, 1000, FLT_MAX, true));
return j;
}
/** Function for setting angular motor parameters. */
void FConstraintInstance::SetAngularDriveParams(float InSpring, float InDamping, float InForceLimit)
{
#if WITH_PHYSX
if (bAngularOrientationDrive || bAngularVelocityDrive)
{
if (PxD6Joint* Joint = GetUnbrokenJoint())
{
const float AngularForceLimit = InForceLimit > 0.0f ? InForceLimit : PX_MAX_F32;
const float DriveSpring = bAngularOrientationDrive ? InSpring : 0.0f;
const float DriveDamping = bAngularVelocityDrive ? InDamping : 0.0f;
if (AngularDriveMode == EAngularDriveMode::SLERP)
{
Joint->setDrive(PxD6Drive::eTWIST, PxD6JointDrive());
Joint->setDrive(PxD6Drive::eSWING, PxD6JointDrive());
Joint->setDrive(PxD6Drive::eSLERP, PxD6JointDrive(DriveSpring, DriveDamping, AngularForceLimit, bIsAccelerationDrive));
}
else
{
Joint->setDrive(PxD6Drive::eTWIST, PxD6JointDrive(DriveSpring, DriveDamping, AngularForceLimit, bIsAccelerationDrive));
Joint->setDrive(PxD6Drive::eSWING, PxD6JointDrive(DriveSpring, DriveDamping, AngularForceLimit, bIsAccelerationDrive));
Joint->setDrive(PxD6Drive::eSLERP, PxD6JointDrive());
}
}
}
#endif
AngularDriveSpring = InSpring;
AngularDriveDamping = InDamping;
AngularDriveForceLimit = InForceLimit;
}
/** Function for setting angular motor parameters. */
void FConstraintInstance::SetAngularDriveParams(float InSpring, float InDamping, float InForceLimit)
{
#if WITH_PHYSX
ExecuteOnUnbrokenJointReadWrite([&] (PxD6Joint* Joint)
{
const float AngularForceLimit = InForceLimit > 0.0f ? InForceLimit : PX_MAX_F32;
const float DriveSpring = bAngularOrientationDrive ? InSpring : 0.0f;
const float DriveDamping = bAngularVelocityDrive ? InDamping : 0.0f;
if (AngularDriveMode == EAngularDriveMode::SLERP)
{
Joint->setDrive(PxD6Drive::eTWIST, PxD6JointDrive());
Joint->setDrive(PxD6Drive::eSWING, PxD6JointDrive());
Joint->setDrive(PxD6Drive::eSLERP, (bEnableSwingDrive || bEnableTwistDrive) ? PxD6JointDrive(DriveSpring, DriveDamping, AngularForceLimit, bIsAccelerationDrive) : PxD6JointDrive());
}
else
{
Joint->setDrive(PxD6Drive::eTWIST, bEnableTwistDrive ? PxD6JointDrive(DriveSpring, DriveDamping, AngularForceLimit, bIsAccelerationDrive) : PxD6JointDrive());
Joint->setDrive(PxD6Drive::eSWING, bEnableSwingDrive ? PxD6JointDrive(DriveSpring, DriveDamping, AngularForceLimit, bIsAccelerationDrive) : PxD6JointDrive());
Joint->setDrive(PxD6Drive::eSLERP, PxD6JointDrive());
}
});
#endif
AngularDriveSpring = InSpring;
AngularDriveDamping = InDamping;
AngularDriveForceLimit = InForceLimit;
}
EXPORT void PhysXD6Joint_SetDrive( PhysXD6Joint* _this, PxD6Drive::Enum index, float spring, float damping,
float forceLimit, bool acceleration )
{
_this->GetJoint()->setDrive(index, PxD6JointDrive(spring, damping, forceLimit, acceleration));
}
bool UGripMotionControllerComponent::SetUpPhysicsHandle(const FBPActorGripInformation &NewGrip)
{
UPrimitiveComponent *root = NewGrip.Component;
if(!root)
root = Cast<UPrimitiveComponent>(NewGrip.Actor->GetRootComponent());
if (!root)
return false;
// Needs to be simulating in order to run physics
root->SetSimulatePhysics(true);
root->SetEnableGravity(false);
FBPActorPhysicsHandleInformation * HandleInfo = CreatePhysicsGrip(NewGrip);
#if WITH_PHYSX
// Get the PxRigidDynamic that we want to grab.
FBodyInstance* BodyInstance = root->GetBodyInstance(NAME_None/*InBoneName*/);
if (!BodyInstance)
{
return false;
}
ExecuteOnPxRigidDynamicReadWrite(BodyInstance, [&](PxRigidDynamic* Actor)
{
PxScene* Scene = Actor->getScene();
// Get transform of actor we are grabbing
FTransform WorldTransform;
FTransform InverseTransform = this->GetComponentTransform().Inverse();
WorldTransform = NewGrip.RelativeTransform.GetRelativeTransform(InverseTransform);
PxVec3 KinLocation = U2PVector(WorldTransform.GetLocation() - (WorldTransform.GetLocation() - root->GetComponentLocation()));
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 (!HandleInfo->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(0.0f); // 1.0f;
KinActor->setMassSpaceInertiaTensor(PxVec3(0.0f, 0.0f, 0.0f));// PxVec3(1.0f, 1.0f, 1.0f));
KinActor->setMaxDepenetrationVelocity(PX_MAX_F32);
// No bodyinstance
KinActor->userData = NULL;
// Add to Scene
Scene->addActor(*KinActor);
// Save reference to the kinematic actor.
HandleInfo->KinActorData = KinActor;
// Create the joint
PxVec3 LocalHandlePos = GrabbedActorPose.transformInv(KinLocation);
PxD6Joint* NewJoint = PxD6JointCreate(Scene->getPhysics(), KinActor, PxTransform::createIdentity(), Actor, PxTransform(LocalHandlePos));
if (!NewJoint)
{
HandleInfo->HandleData = 0;
}
else
{
// No constraint instance
NewJoint->userData = NULL;
HandleInfo->HandleData = NewJoint;
// Remember the scene index that the handle joint/actor are in.
FPhysScene* RBScene = FPhysxUserData::Get<FPhysScene>(Scene->userData);
const uint32 SceneType = root->BodyInstance.UseAsyncScene(RBScene) ? PST_Async : PST_Sync;
HandleInfo->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->setDrivePosition(PxTransform(PxVec3(0, 0, 0)));
NewJoint->setMotion(PxD6Axis::eTWIST, PxD6Motion::eFREE);
NewJoint->setMotion(PxD6Axis::eSWING1, PxD6Motion::eFREE);
NewJoint->setMotion(PxD6Axis::eSWING2, PxD6Motion::eFREE);
//UpdateDriveSettings();
if (HandleInfo->HandleData != nullptr)
{
HandleInfo->HandleData->setDrive(PxD6Drive::eX, PxD6JointDrive(NewGrip.Stiffness, NewGrip.Damping, PX_MAX_F32, PxD6JointDriveFlag::eACCELERATION));
HandleInfo->HandleData->setDrive(PxD6Drive::eY, PxD6JointDrive(NewGrip.Stiffness, NewGrip.Damping, PX_MAX_F32, PxD6JointDriveFlag::eACCELERATION));
HandleInfo->HandleData->setDrive(PxD6Drive::eZ, PxD6JointDrive(NewGrip.Stiffness, NewGrip.Damping, PX_MAX_F32, PxD6JointDriveFlag::eACCELERATION));
HandleInfo->HandleData->setDrive(PxD6Drive::eSLERP, PxD6JointDrive(NewGrip.Stiffness, NewGrip.Damping, PX_MAX_F32, PxD6JointDriveFlag::eACCELERATION));
//HandleData->setDrive(PxD6Drive::eTWIST, PxD6JointDrive(Stiffness, Damping, PX_MAX_F32, PxD6JointDriveFlag::eACCELERATION));
//HandleData->setDrive(PxD6Drive::eSWING, PxD6JointDrive(Stiffness, Damping, PX_MAX_F32, PxD6JointDriveFlag::eACCELERATION));
}
}
}
});
#else
return false;
#endif // WITH_PHYSX
return true;
}
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;
}
