C++ PxRigidDynamic::getGlobalPose方法代码示例

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 defaultCCTInteraction(const PxControllerShapeHit& hit)
{
	PxRigidDynamic* actor = hit.shape->getActor()->is<PxRigidDynamic>();
	if(actor)
	{
		if(actor->getRigidBodyFlags() & PxRigidBodyFlag::eKINEMATIC)
			return;

		if(0)
		{
			const PxVec3 p = actor->getGlobalPose().p + hit.dir * 10.0f;

			PxShape* shape;
			actor->getShapes(&shape, 1);
			PxRaycastHit newHit;
			PxU32 n = PxShapeExt::raycast(*shape, *shape->getActor(), p, -hit.dir, 20.0f, PxHitFlag::ePOSITION, 1, &newHit, false);
			if(n)
			{
				// We only allow horizontal pushes. Vertical pushes when we stand on dynamic objects creates
				// useless stress on the solver. It would be possible to enable/disable vertical pushes on
				// particular objects, if the gameplay requires it.
				const PxVec3 upVector = hit.controller->getUpDirection();
				const PxF32 dp = hit.dir.dot(upVector);
		//		shdfnd::printFormatted("%f\n", fabsf(dp));
				if(fabsf(dp)<1e-3f)
		//		if(hit.dir.y==0.0f)
				{
					const PxTransform globalPose = actor->getGlobalPose();
					const PxVec3 localPos = globalPose.transformInv(newHit.position);
					::addForceAtLocalPos(*actor, hit.dir*hit.length*1000.0f, localPos, PxForceMode::eACCELERATION);
				}
			}
		}

		// We only allow horizontal pushes. Vertical pushes when we stand on dynamic objects creates
		// useless stress on the solver. It would be possible to enable/disable vertical pushes on
		// particular objects, if the gameplay requires it.
		const PxVec3 upVector = hit.controller->getUpDirection();
		const PxF32 dp = hit.dir.dot(upVector);
//		shdfnd::printFormatted("%f\n", fabsf(dp));
		if(fabsf(dp)<1e-3f)
//		if(hit.dir.y==0.0f)
		{
			const PxTransform globalPose = actor->getGlobalPose();
			const PxVec3 localPos = globalPose.transformInv(toVec3(hit.worldPos));
			::addForceAtLocalPos(*actor, hit.dir*hit.length*1000.0f, localPos, PxForceMode::eACCELERATION);
		}
	}
}

void AddRadialForceToPxRigidDynamic(PxRigidDynamic& PRigidDynamic, const FVector& Origin, float Radius, float Strength, uint8 Falloff)
{
#if WITH_PHYSX
	if (!(PRigidDynamic.getRigidDynamicFlags() & PxRigidDynamicFlag::eKINEMATIC))
	{
		float Mass = PRigidDynamic.getMass();
		PxTransform PCOMTransform = PRigidDynamic.getGlobalPose().transform(PRigidDynamic.getCMassLocalPose());
		PxVec3 PCOMPos = PCOMTransform.p; // center of mass in world space
		PxVec3 POrigin = U2PVector(Origin); // origin of radial impulse, in world space
		PxVec3 PDelta = PCOMPos - POrigin; // vector from

		float Mag = PDelta.magnitude(); // Distance from COM to origin, in Unreal scale : @todo: do we still need conversion scale?

		// If COM is outside radius, do nothing.
		if (Mag > Radius)
		{
			return;
		}

		PDelta.normalize();

		// If using linear falloff, scale with distance.
		float ForceMag = Strength;
		if (Falloff == RIF_Linear)
		{
			ForceMag *= (1.0f - (Mag / Radius));
		}

		// Apply force
		PxVec3 PImpulse = PDelta * ForceMag;
		PRigidDynamic.addForce(PImpulse, PxForceMode::eFORCE);
	}
#endif // WITH_PHYSX
}

void AddRadialImpulseToPxRigidDynamic(PxRigidDynamic& PRigidDynamic, const FVector& Origin, float Radius, float Strength, uint8 Falloff, bool bVelChange)
{
#if WITH_PHYSX
	if (!(PRigidDynamic.getRigidDynamicFlags() & PxRigidDynamicFlag::eKINEMATIC))
	{
		float Mass = PRigidDynamic.getMass();
		PxTransform PCOMTransform = PRigidDynamic.getGlobalPose().transform(PRigidDynamic.getCMassLocalPose());
		PxVec3 PCOMPos = PCOMTransform.p; // center of mass in world space
		PxVec3 POrigin = U2PVector(Origin); // origin of radial impulse, in world space
		PxVec3 PDelta = PCOMPos - POrigin; // vector from origin to COM

		float Mag = PDelta.magnitude(); // Distance from COM to origin, in Unreal scale : @todo: do we still need conversion scale?

		// If COM is outside radius, do nothing.
		if (Mag > Radius)
		{
			return;
		}

		PDelta.normalize();

		// Scale by U2PScale here, because units are velocity * mass. 
		float ImpulseMag = Strength;
		if (Falloff == RIF_Linear)
		{
			ImpulseMag *= (1.0f - (Mag / Radius));
		}

		PxVec3 PImpulse = PDelta * ImpulseMag;

		PxForceMode::Enum Mode = bVelChange ? PxForceMode::eVELOCITY_CHANGE : PxForceMode::eIMPULSE;
		PRigidDynamic.addForce(PImpulse, Mode);
	}
#endif // WITH_PHYSX
}

void getSimplePose( PxActor* actor, float* data ) //TODO rework
{
    PxShape* shp[1];
    PxRigidDynamic* rigid = (PxRigidDynamic*)actor;
    rigid->getShapes( shp, PxU32(1) );
    PxMat44 shape_pose = rigid->getGlobalPose(); //(PxShapeExt::getGlobalPose(*shp[0], *rigid));
    for( int i = 0; i < 4; i++ )
        for( int j = 0; j < 4; j++ )
            data[i*4 + j] = shape_pose[j][i];
}

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
}

		void setup(){
			ofSetFrameRate(150);

			w=ofGetScreenWidth();
			h=ofGetScreenHeight();
		        InitPhysX();
        		for(int i=0; i<=300; i++) {
		                if(gScene) StepPhysX();
		                PxVec3 boxPos = gBox->getGlobalPose().p;
                		cout<<"BoxPosition: X:"<<boxPos.x <<" - Y:"<<boxPos.y <<" - Z:"<<boxPos.z<<"\n";
		        }
		}

void 
Spacetime::saveState(void) {
	linearVelocityVector.clear();
	angularVelocityVector.clear();
	globalPoseVector.clear();
	for (int i = 0; i < dynamic_actors.size(); i++) {
		PxRigidDynamic* current = dynamic_actors[i];
		linearVelocityVector.push_back(current->getLinearVelocity());
		angularVelocityVector.push_back(current->getAngularVelocity());
		globalPoseVector.push_back(current->getGlobalPose());
	}
}

void PhysicsSystemPhysX::update(const Time& deltaTime)
{	
	// Step the simulation forward
	mScene->simulate(deltaTime.seconds());

	if (mScene->fetchResults())
	{

		// Now let's push back the transforms into the World
		ComponentManager* colliderBoxManager = getWorld()->getComponentManager<CColliderBox>();
		ComponentManager* transformManager = getWorld()->getComponentManager<CTransform>();
		for (std::size_t i = 0; i < colliderBoxManager->size(); ++i)
		{
			Entity E = colliderBoxManager->getInstanceEntity(i);
			CColliderBox* box = (CColliderBox*)colliderBoxManager->getInstance(i);
			CTransform* transform = (CTransform*)transformManager->getComponentFromEntity(E);

			// Create
			if (box->userData == nullptr)
			{
				PxMaterial* boxMaterial = mPhysics->createMaterial(0.5f, 0.5f, 0.1f);

				PxRigidDynamic* boxActor = mPhysics->createRigidDynamic(PxTransform(transform->position.x, transform->position.y, transform->position.z));
				boxActor->createShape(PxBoxGeometry(10.f, 10.f, 10.f), *boxMaterial);
				PxRigidBodyExt::updateMassAndInertia(*boxActor, 30);
				//boxActor->setLinearVelocity(PxVec3(0, -50, 0));

				mScene->addActor(*boxActor);
				box->userData = boxActor;
			}
			// Update
			else
			{
				PxRigidDynamic* boxActor = (PxRigidDynamic*)box->userData;
				PxTransform pxTransform = boxActor->getGlobalPose();

				transform->position.x = pxTransform.p.x;
				transform->position.y = pxTransform.p.y;
				transform->position.z = pxTransform.p.z;

				transform->rotation.x = pxTransform.q.x;
				transform->rotation.y = pxTransform.q.y;
				transform->rotation.z = pxTransform.q.z;
				transform->rotation.w = pxTransform.q.w;
			}
		}

		syncActors();
	}
}

void ScoreCallback::onTrigger(PxTriggerPair* pairs, PxU32 count)
{
	for (int i = 0; i < count; i++)
	{
		if (pairs[i].otherActor->isRigidDynamic() && pairs[i].otherActor->getName() == "Ball")
		{
			if (pairs[i].status & PxPairFlag::eNOTIFY_TOUCH_FOUND)
			{

				PxRigidDynamic* ball = static_cast<PxRigidDynamic*>(pairs[i].otherActor);

				if (pairs[i].triggerActor->getName() == "BumperHigh" || pairs[i].triggerActor->getName() == "BumperLow")
				{
					PxRigidDynamic* bumper = static_cast<PxRigidDynamic*>(pairs[i].triggerActor);

					if (pairs[i].triggerActor->getName() == "BumperHigh")
						Pinball::AddScoreHigh = true;
					else
						Pinball::AddScoreLow = true;

					Transform bumperPose = bumper->getGlobalPose();
					Transform ballPose = ball->getGlobalPose();
					Vec3 dir = bumperPose.p - ballPose.p;
					dir.normalize();

					dir = Vec3(dir.x, dir.y, dir.z);
					Pinball::BounceBall = true;
					Pinball::BallBounceDirection = dir;
				}
				if (pairs[i].triggerActor->getName() == "SpinnerSwitch")
				{
						Pinball::EnableSpinners = true;
				}
			}
		}
	}
}

PxReal labscale::getRegolithSurface()
{
    PxReal regSurf = -PX_MAX_REAL;
    PxU32 nbActors = gPhysX.mScene->getActors(gPhysX.roles.dynamics,gPhysX.cast,MAX_ACTORS_PER_SCENE);
    while (nbActors--)
    {
        PxRigidDynamic* actor = gPhysX.cast[nbActors]->isRigidDynamic();
        if (actor && strcmp(actor->getName(), "regolith") == 0)
        {
            PxReal y = actor->getGlobalPose().p.y;
            if (y>regSurf) regSurf = y;
        }
    }

    return (regSurf + labscale::regolith.diameter/2.0);
}

void PhysX::AddLocalTorque(PintObjectHandle handle, const Point& local_torque)
{
	PxRigidActor* RigidActor = GetActorFromHandle(handle);
	if(!RigidActor)
	{
		PxShape* Shape = GetShapeFromHandle(handle);
		ASSERT(Shape);
		RigidActor = &Shape->getActor();
	}

	if(RigidActor->getConcreteType()==PxConcreteType::eRIGID_DYNAMIC)
	{
		PxRigidDynamic* RigidDynamic = static_cast<PxRigidDynamic*>(RigidActor);
		const PxVec3 GlobalTorque = RigidDynamic->getGlobalPose().rotate(ToPxVec3(local_torque));
		RigidDynamic->addTorque(GlobalTorque, PxForceMode::eACCELERATION, true);
	}
}

void CSlingshotEngine::loadSlingshot()
{
	Matrix44 transformation2;
	setTranslation(transformation2, Vector3(0.0f, -2.5f, -15.0f));
	Entity* pBird = SceneFactory::CreateBird(transformation2, EntityCategories::BIRD, 1.0f);

	m_pBirdBody = pBird->getSingleComponent<PhysXBody>();
	PxRigidDynamic* pBirdDynamic = m_pBirdBody->getActor()->isRigidDynamic();

	Vector3 arm0Position = PhysXVector::toVector3(m_pArm0Body->getActor()->isRigidActor()->getGlobalPose().p);
	Vector3 arm1Position = PhysXVector::toVector3(m_pArm1Body->getActor()->isRigidActor()->getGlobalPose().p);
	Vector3 birdPosition = PhysXVector::toVector3(pBirdDynamic->getGlobalPose().p);
	Vector3 arm0ToBird = birdPosition - arm0Position;
	Matrix44 arm0LocalFrame;
	setTranslation(arm0LocalFrame, arm0ToBird * 0.5f);
	Vector3 arm1ToBird = birdPosition - arm1Position;
	Matrix44 arm1LocalFrame;
	setTranslation(arm1LocalFrame, arm1ToBird * 0.5f);
	Matrix44 birdLocalFrame;
	setTranslation(birdLocalFrame, arm0ToBird * -0.5f);

	PxPhysics* pPhysics = static_cast<PhysXPhysicsFactory*>(PhysicsFactory::getInstance())->getPhysics();

	m_pArm0Joint = PxD6JointCreate(*pPhysics, m_pArm0Body->getActor()->isRigidActor(),
		PhysXMatrix::toPxTransform(arm0LocalFrame), pBirdDynamic, PhysXMatrix::toPxTransform(birdLocalFrame));

	setTranslation(birdLocalFrame, arm1ToBird * -0.5f);

	m_pArm1Joint = PxD6JointCreate(*pPhysics, m_pArm1Body->getActor()->isRigidActor(),
		PhysXMatrix::toPxTransform(arm1LocalFrame), pBirdDynamic, PhysXMatrix::toPxTransform(birdLocalFrame));

	m_pArm0Joint->setMotion(PxD6Axis::eX, PxD6Motion::eFREE);
	m_pArm0Joint->setMotion(PxD6Axis::eY, PxD6Motion::eFREE);
	m_pArm0Joint->setMotion(PxD6Axis::eZ, PxD6Motion::eFREE);
	m_pArm1Joint->setMotion(PxD6Axis::eX, PxD6Motion::eFREE);
	m_pArm1Joint->setMotion(PxD6Axis::eY, PxD6Motion::eFREE);
	m_pArm1Joint->setMotion(PxD6Axis::eZ, PxD6Motion::eFREE);

	PxD6JointDrive drive(500.0f, -20.0f, PX_MAX_F32);
	m_pArm0Joint->setDrive(PxD6Drive::eX, drive);
	m_pArm0Joint->setDrive(PxD6Drive::eY, drive);
	m_pArm0Joint->setDrive(PxD6Drive::eZ, drive);
	m_pArm1Joint->setDrive(PxD6Drive::eX, drive);
	m_pArm1Joint->setDrive(PxD6Drive::eY, drive);
	m_pArm1Joint->setDrive(PxD6Drive::eZ, drive);
}

void defaultCCTInteraction(const PxControllerShapeHit& hit)
{
	PxRigidDynamic* actor = hit.shape->getActor().is<PxRigidDynamic>();
	if(actor)
	{
		if(actor->getRigidDynamicFlags() & PxRigidDynamicFlag::eKINEMATIC)
			return;

		// We only allow horizontal pushes. Vertical pushes when we stand on dynamic objects creates
		// useless stress on the solver. It would be possible to enable/disable vertical pushes on
		// particular objects, if the gameplay requires it.
		const PxVec3 upVector = hit.controller->getUpDirection();
		const PxF32 dp = hit.dir.dot(upVector);
//		printf("%f\n", fabsf(dp));
		if(fabsf(dp)<1e-3f)
//		if(hit.dir.y==0.0f)
		{
			const PxTransform globalPose = actor->getGlobalPose();
			const PxVec3 localPos = globalPose.transformInv(toVec3(hit.worldPos));
			::addForceAtLocalPos(*actor, hit.dir*hit.length*1000.0f, localPos, PxForceMode::eACCELERATION);
		}
	}
}

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));
			}
//.........这里部分代码省略.........