Exemple #1
0
	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
}