PxQuat slerp(const PxReal t, const PxQuat& left, const PxQuat& right)
{
const PxReal quatEpsilon = (PxReal(1.0e-8f));
PxReal cosine = left.dot(right);
PxReal sign = PxReal(1);
if (cosine < 0)
{
cosine = -cosine;
sign = PxReal(-1);
}
PxReal sine = PxReal(1) - cosine*cosine;
if(sine>=quatEpsilon*quatEpsilon)
{
sine = PxSqrt(sine);
const PxReal angle = PxAtan2(sine, cosine);
const PxReal i_sin_angle = PxReal(1) / sine;
const PxReal leftw = PxSin(angle*(PxReal(1)-t)) * i_sin_angle;
const PxReal rightw = PxSin(angle * t) * i_sin_angle * sign;
return left * leftw + right * rightw;
}
return left;
}
void UPhysicsHandleComponent::UpdateHandleTransform(const FTransform& NewTransform)
{
if(!KinActorData)
{
return;
}
#if WITH_PHYSX
bool bChangedPosition = true;
bool bChangedRotation = true;
PxRigidDynamic* KinActor = KinActorData;
// Check if the new location is worthy of change
PxVec3 PNewLocation = U2PVector(NewTransform.GetTranslation());
PxVec3 PCurrentLocation = KinActor->getGlobalPose().p;
if((PNewLocation - PCurrentLocation).magnitudeSquared() <= 0.01f*0.01f)
{
PNewLocation = PCurrentLocation;
bChangedPosition = false;
}
// Check if the new rotation is worthy of change
PxQuat PNewOrientation = U2PQuat(NewTransform.GetRotation());
PxQuat PCurrentOrientation = KinActor->getGlobalPose().q;
if(!(FMath::Abs(PNewOrientation.dot(PCurrentOrientation)) < (1.f - KINDA_SMALL_NUMBER)))
{
PNewOrientation = PCurrentOrientation;
bChangedRotation = false;
}
// Don't call moveKinematic if it hasn't changed - that will stop bodies from going to sleep.
if (bChangedPosition || bChangedRotation)
{
KinActor->setKinematicTarget(PxTransform(PNewLocation, PNewOrientation));
//LOC_MOD
//PxD6Joint* Joint = (PxD6Joint*) HandleData;
//if(Joint)// && (PNewLocation - PCurrentLocation).magnitudeSquared() > 0.01f*0.01f)
//{
// PxRigidActor* Actor0, *Actor1;
// Joint->getActors(Actor0, Actor1);
// //Joint->setDrivePosition(PxTransform(Actor0->getGlobalPose().transformInv(PNewLocation)));
// Joint->setDrivePosition(PxTransform::createIdentity());
// //Joint->setDriveVelocity(PxVec3(0), PxVec3(0));
//}
}
#endif // WITH_PHYSX
}
void UGripMotionControllerComponent::UpdatePhysicsHandleTransform(const FBPActorGripInformation &GrippedActor, const FTransform& NewTransform)
{
if (!GrippedActor.Actor && !GrippedActor.Component)
return;
FBPActorPhysicsHandleInformation * HandleInfo = GetPhysicsGrip(GrippedActor);
if (!HandleInfo || !HandleInfo->KinActorData)
return;
#if WITH_PHYSX
bool bChangedPosition = true;
bool bChangedRotation = true;
PxRigidDynamic* KinActor = HandleInfo->KinActorData;
PxScene* PScene = GetPhysXSceneFromIndex(HandleInfo->SceneIndex);
SCOPED_SCENE_WRITE_LOCK(PScene);
// Check if the new location is worthy of change
PxVec3 PNewLocation = U2PVector(NewTransform.GetTranslation());
PxVec3 PCurrentLocation = KinActor->getGlobalPose().p;
if ((PNewLocation - PCurrentLocation).magnitudeSquared() <= 0.01f*0.01f)
{
PNewLocation = PCurrentLocation;
bChangedPosition = false;
}
// Check if the new rotation is worthy of change
PxQuat PNewOrientation = U2PQuat(NewTransform.GetRotation());
PxQuat PCurrentOrientation = KinActor->getGlobalPose().q;
if ((FMath::Abs(PNewOrientation.dot(PCurrentOrientation)) > (1.f - SMALL_NUMBER)))
{
PNewOrientation = PCurrentOrientation;
bChangedRotation = false;
}
// Don't call moveKinematic if it hasn't changed - that will stop bodies from going to sleep.
if (bChangedPosition || bChangedRotation)
{
KinActor->setKinematicTarget(PxTransform(PNewLocation, PNewOrientation));
}
#endif // WITH_PHYSX
}