// D6 joint with a spring maintaining its position
PxJoint* createDampedD6(PxPhysics* gPhysics, PxRigidActor* a0, const PxTransform& t0, PxRigidActor* a1, const PxTransform& t1)
{
PxD6Joint* j = PxD6JointCreate(*gPhysics, a0, t0, a1, t1);
j->setMotion(PxD6Axis::eSWING1, PxD6Motion::eLOCKED);
j->setMotion(PxD6Axis::eSWING2, PxD6Motion::eFREE);
j->setMotion(PxD6Axis::eTWIST, PxD6Motion::eLOCKED);
j->setDrive(PxD6Drive::eSLERP, PxD6JointDrive(0, 1000, FLT_MAX, true));
return j;
}
/** Function for turning linear velocity drive on and off. */
void FConstraintInstance::SetLinearVelocityDrive(bool bEnableXDrive, bool bEnableYDrive, bool bEnableZDrive)
{
#if WITH_PHYSX
PxD6Joint* Joint = (PxD6Joint*)ConstraintData;
if (Joint && !(Joint->getConstraintFlags()&PxConstraintFlag::eBROKEN))
{
// Get the current drives
PxD6JointDrive CurrentDriveX = Joint->getDrive(PxD6Drive::eX);
PxD6JointDrive CurrentDriveY = Joint->getDrive(PxD6Drive::eY);
PxD6JointDrive CurrentDriveZ = Joint->getDrive(PxD6Drive::eZ);
CurrentDriveX.damping = bEnableXDrive && FMath::Abs(LinearVelocityTarget.X) > 0.0f ? LinearDriveDamping : 0.0f;
CurrentDriveY.damping = bEnableYDrive && FMath::Abs(LinearVelocityTarget.Y) > 0.0f ? LinearDriveDamping : 0.0f;
CurrentDriveZ.damping = bEnableZDrive && FMath::Abs(LinearVelocityTarget.Z) > 0.0f ? LinearDriveDamping : 0.0f;
Joint->setDrive(PxD6Drive::eX, CurrentDriveX);
Joint->setDrive(PxD6Drive::eY, CurrentDriveY);
Joint->setDrive(PxD6Drive::eZ, CurrentDriveZ);
}
#endif
bLinearVelocityDrive = bEnableXDrive || bEnableYDrive || bEnableZDrive;
}
/** Function for turning angular velocity drive on and off. */
void FConstraintInstance::SetAngularVelocityDrive(bool bEnableSwingDrive, bool bEnableTwistDrive)
{
#if WITH_PHYSX
PxD6Joint* Joint = (PxD6Joint*)ConstraintData;
if (Joint && !(Joint->getConstraintFlags()&PxConstraintFlag::eBROKEN))
{
// Get the current drives
PxD6JointDrive CurrentDriveSwing = Joint->getDrive(PxD6Drive::eSWING);
PxD6JointDrive CurrentDriveTwist = Joint->getDrive(PxD6Drive::eTWIST);
PxD6JointDrive CurrentDriveSlerp = Joint->getDrive(PxD6Drive::eSLERP);
const bool bSlerp = AngularDriveMode == EAngularDriveMode::SLERP;
CurrentDriveSwing.damping = !bSlerp && bEnableSwingDrive ? AngularDriveDamping : 0.0f;
CurrentDriveTwist.damping = !bSlerp && bEnableTwistDrive ? AngularDriveDamping : 0.0f;
CurrentDriveSlerp.damping = bSlerp && (bEnableSwingDrive && bEnableTwistDrive) ? AngularDriveDamping : 0.0f;
Joint->setDrive(PxD6Drive::eSWING, CurrentDriveSwing);
Joint->setDrive(PxD6Drive::eTWIST, CurrentDriveTwist);
Joint->setDrive(PxD6Drive::eSLERP, CurrentDriveSlerp);
bAngularVelocityDrive = bEnableSwingDrive || bEnableTwistDrive;
}
#endif
}
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;
}
