C++ PxTransform类代码示例

void FPhysScene::SetKinematicTarget(FBodyInstance * BodyInstance, const FTransform & TargetTransform)
{
	TargetTransform.DiagnosticCheckNaN_All();

#if WITH_PHYSX
	if (PxRigidDynamic * PRigidDynamic = BodyInstance->GetPxRigidDynamic())
	{
#if WITH_SUBSTEPPING
		if (IsSubstepping())
		{
			FPhysSubstepTask * PhysSubStepper = PhysSubSteppers[SceneType(BodyInstance)];
			PhysSubStepper->SetKinematicTarget(BodyInstance, TargetTransform);
		}
		else
#endif
		{
			const PxTransform PNewPose = U2PTransform(TargetTransform);
			check(PNewPose.isValid());

			SCOPED_SCENE_WRITE_LOCK(PRigidDynamic->getScene());
			PRigidDynamic->setKinematicTarget(PNewPose);
		}
	}
#endif
}

void NpRigidDynamic::setGlobalPose(const PxTransform& pose, bool autowake)
{
    NpScene* scene = NpActor::getAPIScene(*this);

#ifdef PX_CHECKED
    if(scene)
        scene->checkPositionSanity(*this, pose, "PxRigidDynamic::setGlobalPose");
#endif

    PX_CHECK_AND_RETURN(pose.isSane(), "NpRigidDynamic::setGlobalPose: pose is not valid.");
    NP_WRITE_CHECK(NpActor::getOwnerScene(*this));

    PxTransform p = pose.getNormalized();

    if(scene)
    {
        updateDynamicSceneQueryShapes(mShapeManager, scene->getSceneQueryManagerFast());
    }

    PxTransform newPose = p;
    newPose.q.normalize(); //AM: added to fix 1461 where users read and write orientations for no reason.

    Scb::Body& b = getScbBodyFast();
    PxTransform body2World = newPose * b.getBody2Actor();
    b.setBody2World(body2World, false);

    if(scene && autowake && !(b.getActorFlags() & PxActorFlag::eDISABLE_SIMULATION))
        wakeUpInternal();
}

static PxTransform computeBoneTransform(PxTransform &rootTransform, Acclaim::Bone &bone, PxVec3* boneFrameData)
{		
	using namespace Acclaim;

	//PxTransform rootTransform(PxVec3(0.0f), PxQuat(PxIdentity));
	PxTransform parentTransform = (bone.mParent) ? 
		computeBoneTransform(rootTransform, *bone.mParent, boneFrameData) : rootTransform;
		
	PxQuat qWorld = EulerAngleToQuat(bone.mAxis);
	PxVec3 offset = bone.mLength * bone.mDirection;
	PxQuat qDelta = PxQuat(PxIdentity);
	PxVec3 boneData = boneFrameData[bone.mID-1];
	
	if (bone.mDOF & BoneDOFFlag::eRX)
		qDelta = PxQuat(Ps::degToRad(boneData.x), PxVec3(1.0f, 0.0f, 0.0f)) * qDelta;
	if (bone.mDOF & BoneDOFFlag::eRY)
		qDelta = PxQuat(Ps::degToRad(boneData.y), PxVec3(0.0f, 1.0f, 0.0f)) * qDelta;
	if (bone.mDOF & BoneDOFFlag::eRZ)
		qDelta = PxQuat(Ps::degToRad(boneData.z), PxVec3(0.0f, 0.0f, 1.0f)) * qDelta;

	PxQuat boneOrientation = qWorld * qDelta * qWorld.getConjugate();

	PxTransform boneTransform(boneOrientation.rotate(offset), boneOrientation);

	return parentTransform.transform(boneTransform);
}

/** Interpolates kinematic actor transform - Assumes caller has obtained writer lock */
void FPhysSubstepTask::InterpolateKinematicActor_AssumesLocked(const FPhysTarget& PhysTarget, FBodyInstance* BodyInstance, float InAlpha)
{
#if WITH_PHYSX
	PxRigidDynamic * PRigidDynamic = BodyInstance->GetPxRigidDynamic_AssumesLocked();
	InAlpha = FMath::Clamp(InAlpha, 0.f, 1.f);

	/** Interpolate kinematic actors */
	if (PhysTarget.bKinematicTarget)
	{
		//It's possible that the actor is no longer kinematic and is now simulating. In that case do nothing
		if (!BodyInstance->IsNonKinematic())
		{
			const FKinematicTarget& KinematicTarget = PhysTarget.KinematicTarget;
			const FTransform& TargetTM = KinematicTarget.TargetTM;
			const FTransform& StartTM = KinematicTarget.OriginalTM;
			FTransform InterTM = FTransform::Identity;

			InterTM.SetLocation(FMath::Lerp(StartTM.GetLocation(), TargetTM.GetLocation(), InAlpha));
			InterTM.SetRotation(FMath::Lerp(StartTM.GetRotation(), TargetTM.GetRotation(), InAlpha));

			const PxTransform PNewPose = U2PTransform(InterTM);
			check(PNewPose.isValid());
			PRigidDynamic->setKinematicTarget(PNewPose);
		}
	}
#endif
}

/** Interpolates kinematic actor transform - Assumes caller has obtained writer lock */
void FPhysSubstepTask::InterpolateKinematicActor(const FPhysTarget & PhysTarget, FBodyInstance * BodyInstance, float Alpha)
{
#if WITH_PHYSX
    PxRigidDynamic * PRigidDynamic = BodyInstance->GetPxRigidDynamic();
    Alpha = FMath::Clamp(Alpha, 0.f, 1.f);

    /** Interpolate kinematic actors */
    if (PhysTarget.bKinematicTarget)
    {
        //We should only be interpolating kinematic actors
        check(!IsRigidDynamicNonKinematic(PRigidDynamic));
        const FKinematicTarget & KinematicTarget = PhysTarget.KinematicTarget;
        const FTransform & TargetTM = KinematicTarget.TargetTM;
        const FTransform & StartTM = KinematicTarget.OriginalTM;
        FTransform InterTM = FTransform::Identity;

        InterTM.SetLocation(FMath::Lerp(StartTM.GetLocation(), TargetTM.GetLocation(), Alpha));
        InterTM.SetRotation(FMath::Lerp(StartTM.GetRotation(), TargetTM.GetRotation(), Alpha));

        const PxTransform PNewPose = U2PTransform(InterTM);
        check(PNewPose.isValid());
        PRigidDynamic->setKinematicTarget(PNewPose);
    }
#endif
}

void SampleVehicle::drawWheels()
{
	const RendererColor colorPurple(255, 0, 255);

	for(PxU32 i=0;i<mVehicleManager.getNbVehicles();i++)
	{
		//Draw a rotating arrow to get an idea of the wheel rotation speed.
		PxVehicleWheels* veh=mVehicleManager.getVehicle(i);
		const PxRigidDynamic* actor=veh->getRigidDynamicActor();
		PxShape* shapeBuffer[PX_MAX_NUM_WHEELS];
		actor->getShapes(shapeBuffer,veh->mWheelsSimData.getNumWheels());
		const PxTransform vehGlobalPose=actor->getGlobalPose();
		const PxU32 numWheels=veh->mWheelsSimData.getNumWheels();
		for(PxU32 j=0;j<numWheels;j++)
		{
			const PxTransform wheelTransform=vehGlobalPose.transform(shapeBuffer[j]->getLocalPose());
			const PxF32 wheelRadius=veh->mWheelsSimData.getWheelData(j).mRadius;
			const PxF32 wheelHalfWidth=veh->mWheelsSimData.getWheelData(j).mWidth*0.5f;
			PxVec3 offset=wheelTransform.q.getBasisVector0()*wheelHalfWidth;
			offset*= (veh->mWheelsSimData.getWheelCentreOffset(j).x > 0) ? 1.0f : -1.0f;
			const PxVec3 arrow=wheelTransform.rotate(PxVec3(0,0,1));
			getDebugRenderer()->addLine(wheelTransform.p+offset, wheelTransform.p+offset+arrow*wheelRadius, colorPurple);
		}
	}
}

bool physx::Gu::computePlane_ConvexMTD(const PxPlane& plane, const PxConvexMeshGeometry& convexGeom, const PxTransform& convexPose, PxSweepHit& hit)
{
	const ConvexMesh* convexMesh = static_cast<const ConvexMesh*>(convexGeom.convexMesh);
	const Cm::FastVertex2ShapeScaling convexScaling(convexGeom.scale);
	PxU32 nbVerts = convexMesh->getNbVerts();
	const PxVec3* PX_RESTRICT verts = convexMesh->getVerts();

	PxVec3 worldPointMin = convexPose.transform(convexScaling * verts[0]);
	PxReal dmin = plane.distance(worldPointMin);
	for(PxU32 i=1;i<nbVerts;i++)
	{
		const PxVec3 worldPoint = convexPose.transform(convexScaling * verts[i]);
		const PxReal d = plane.distance(worldPoint);
		if(dmin > d)
		{
			dmin = d;
			worldPointMin = worldPoint;
		}
	}

	hit.normal		= plane.n;
	hit.distance	= dmin;
	hit.position	= worldPointMin - plane.n * dmin;
	return true;
}

void PxRigidBodyExt::addLocalForceAtLocalPos(PxRigidBody& body, const PxVec3& force, const PxVec3& pos, PxForceMode::Enum mode, bool wakeup)
{
	const PxTransform globalPose = body.getGlobalPose();
	const PxVec3 globalForcePos = globalPose.transform(pos);
	const PxVec3 globalForce = globalPose.rotate(force);

	addForceAtPosInternal(body, globalForce, globalForcePos, mode, wakeup);
}

PxVec3 PxRigidBodyExt::getLocalVelocityAtLocalPos(const PxRigidBody& body, const PxVec3& point)
{
	const PxTransform globalPose = body.getGlobalPose();
	const PxVec3 centerOfMass    = globalPose.transform(body.getCMassLocalPose().p);
	const PxVec3 rpoint          = globalPose.transform(point) - centerOfMass;

	return getVelocityAtPosInternal(body, rpoint);
}

PxVec3 PxRigidBodyExt::getVelocityAtOffset(const PxRigidBody& body, const PxVec3& point)
{
	const PxTransform globalPose = body.getGlobalPose();
	const PxVec3 centerOfMass    = globalPose.rotate(body.getCMassLocalPose().p);
	const PxVec3 rpoint          = point - centerOfMass;

	return getVelocityAtPosInternal(body, rpoint);
}

void NpArticulationJoint::setChildPose(const PxTransform& t)
{
	PX_CHECK_AND_RETURN(t.isSane(), "NpArticulationJoint::setChildPose t is not valid.");

	NP_WRITE_CHECK(getOwnerScene());

	mJoint.setChildPose(mChild->getCMassLocalPose().transformInv(t.getNormalized()));
}

void NpCloth::setTargetPose(const PxTransform& pose)
{
	NP_WRITE_CHECK(NpActor::getOwnerScene(*this));

	PX_CHECK_AND_RETURN(pose.isSane(), "PxCloth::setTargetPose: invalid transform!");

	mCloth.setTargetPose(pose.getNormalized());
	sendPvdSimpleProperties();
}

	void AddConvexElemsToRigidActor_AssumesLocked() const
	{
		float ContactOffsetFactor, MaxContactOffset;
		GetContactOffsetParams(ContactOffsetFactor, MaxContactOffset);

		for (int32 i = 0; i < BodySetup->AggGeom.ConvexElems.Num(); i++)
		{
			const FKConvexElem& ConvexElem = BodySetup->AggGeom.ConvexElems[i];

			if (ConvexElem.ConvexMesh)
			{
				PxTransform PLocalPose;
				bool bUseNegX = CalcMeshNegScaleCompensation(Scale3D, PLocalPose);

				PxConvexMeshGeometry PConvexGeom;
				PConvexGeom.convexMesh = bUseNegX ? ConvexElem.ConvexMeshNegX : ConvexElem.ConvexMesh;
				PConvexGeom.scale.scale = U2PVector(Scale3DAbs * ConvexElem.GetTransform().GetScale3D().GetAbs());
				FTransform ConvexTransform = ConvexElem.GetTransform();
				if (ConvexTransform.GetScale3D().X < 0 || ConvexTransform.GetScale3D().Y < 0 || ConvexTransform.GetScale3D().Z < 0)
				{
					UE_LOG(LogPhysics, Warning, TEXT("AddConvexElemsToRigidActor: [%s] ConvexElem[%d] has negative scale. Not currently supported"), *GetPathNameSafe(BodySetup->GetOuter()), i);
				}
				if (ConvexTransform.IsValid())
				{
					PxTransform PElementTransform = U2PTransform(ConvexTransform * RelativeTM);
					PLocalPose.q *= PElementTransform.q;
					PLocalPose.p = PElementTransform.p;
					PLocalPose.p.x *= Scale3D.X;
					PLocalPose.p.y *= Scale3D.Y;
					PLocalPose.p.z *= Scale3D.Z;

					if (PConvexGeom.isValid())
					{
						PxVec3 PBoundsExtents = PConvexGeom.convexMesh->getLocalBounds().getExtents();

						ensure(PLocalPose.isValid());
						{
							const float ContactOffset = FMath::Min(MaxContactOffset, ContactOffsetFactor * PBoundsExtents.minElement());
							AttachShape_AssumesLocked(PConvexGeom, PLocalPose, ContactOffset);
						}
					}
					else
					{
						UE_LOG(LogPhysics, Warning, TEXT("AddConvexElemsToRigidActor: [%s] ConvexElem[%d] invalid"), *GetPathNameSafe(BodySetup->GetOuter()), i);
					}
				}
				else
				{
					UE_LOG(LogPhysics, Warning, TEXT("AddConvexElemsToRigidActor: [%s] ConvexElem[%d] has invalid transform"), *GetPathNameSafe(BodySetup->GetOuter()), i);
				}
			}
			else
			{
				UE_LOG(LogPhysics, Warning, TEXT("AddConvexElemsToRigidActor: ConvexElem is missing ConvexMesh (%d: %s)"), i, *BodySetup->GetPathName());
			}
		}
	}

void RenderClothActor::update(float deltaTime)
{
	updateRenderShape();

	// update collision shapes
	PxU32 numCollisionSpheres = mCloth.getNbCollisionSpheres();
	PxU32 numPairs = mCloth.getNbCollisionSpherePairs();
	PxU32 numPlanes = mCloth.getNbCollisionPlanes();
	PxU32 numConvexes = mCloth.getNbCollisionConvexes();

	if (numCollisionSpheres == 0 && numConvexes == 0)
		return;

	if(numPlanes != mNumPlanes)
		mPlanes = (PxClothCollisionPlane*)SAMPLE_ALLOC(sizeof(PxClothCollisionPlane) * (mNumPlanes = numPlanes));

	if(numConvexes != mNumConvexes)
		mConvexMasks = (PxU32*)SAMPLE_ALLOC(sizeof(PxU32) * (mNumConvexes = numConvexes));

	mCloth.getCollisionData(mSpheres, mIndexPairs, mPlanes, mConvexMasks);

	PxTransform clothPose = mCloth.getGlobalPose();

	// update all spheres
	for (PxU32 i=0; i < mSphereActors.size(); ++i)
	{
		float r = mSpheres[i].radius*mCapsuleScale;

		mSphereActors[i]->setRendering(true);
		mSphereActors[i]->setTransform(PxTransform(clothPose.transform(mSpheres[i].pos)));
		mSphereActors[i]->setMeshScale(PxMeshScale(PxVec3(r, r, r), PxQuat::createIdentity()));
	}
	
	// capsule needs to be flipped to match PxTransformFromSegment
	PxTransform flip(PxVec3(0.0f), PxQuat(-PxHalfPi, PxVec3(0.0f, 0.0f, 1.0f)));

	// update all capsules
	for (PxU32 i=0; i < mCapsuleActors.size(); ++i)
	{
		PxU32 i0 = mIndexPairs[i*2];
		PxU32 i1 = mIndexPairs[i*2+1];

		PxClothCollisionSphere& sphere0 = mSpheres[i0];
		PxClothCollisionSphere& sphere1 = mSpheres[i1];
		
		// disable individual rendering for spheres belonging to a capsule
		mSphereActors[i0]->setRendering(false);
		mSphereActors[i1]->setRendering(false);

		PxVec3 p0 = clothPose.transform(sphere0.pos);
		PxVec3 p1 = clothPose.transform(sphere1.pos);

		mCapsuleActors[i]->setDimensions((p1-p0).magnitude()*0.5f, mCapsuleScale*sphere0.radius, mCapsuleScale*sphere1.radius);
		mCapsuleActors[i]->setTransform(PxTransformFromSegment(p0, p1)*flip);		
	}	
}

static FVector FindBoxOpposingNormal(const PxLocationHit& PHit, const FVector& TraceDirectionDenorm, const FVector InNormal)
{
	// We require normal info for our algorithm.
	const bool bNormalData = (PHit.flags & PxHitFlag::eNORMAL);
	if (!bNormalData)
	{
		return InNormal;
	}

	PxBoxGeometry PxBoxGeom;
	const bool bReadGeomSuccess = PHit.shape->getBoxGeometry(PxBoxGeom);
	check(bReadGeomSuccess); // This function should only be used for box geometry

	const PxTransform LocalToWorld = PxShapeExt::getGlobalPose(*PHit.shape, *PHit.actor);
	
	// Find which faces were included in the contact normal, and for multiple faces, use the one most opposing the sweep direction.
	const PxVec3 ContactNormalLocal = LocalToWorld.rotateInv(PHit.normal);
	const float* ContactNormalLocalPtr = &ContactNormalLocal.x;
	const PxVec3 TraceDirDenormWorld = U2PVector(TraceDirectionDenorm);
	const float* TraceDirDenormWorldPtr = &TraceDirDenormWorld.x;
	const PxVec3 TraceDirDenormLocal = LocalToWorld.rotateInv(TraceDirDenormWorld);
	const float* TraceDirDenormLocalPtr = &TraceDirDenormLocal.x;

	PxVec3 BestLocalNormal(ContactNormalLocal);
	float* BestLocalNormalPtr = &BestLocalNormal.x;
	float BestOpposingDot = FLT_MAX;

	for (int32 i=0; i < 3; i++)
	{
		// Select axis of face to compare to, based on normal.
		if (ContactNormalLocalPtr[i] > KINDA_SMALL_NUMBER)
		{
			const float TraceDotFaceNormal = TraceDirDenormLocalPtr[i]; // TraceDirDenormLocal.dot(BoxFaceNormal)
			if (TraceDotFaceNormal < BestOpposingDot)
			{
				BestOpposingDot = TraceDotFaceNormal;
				BestLocalNormal = PxVec3(0.f);
				BestLocalNormalPtr[i] = 1.f;
			}
		}
		else if (ContactNormalLocalPtr[i] < -KINDA_SMALL_NUMBER)
		{
			const float TraceDotFaceNormal = -TraceDirDenormLocalPtr[i]; // TraceDirDenormLocal.dot(BoxFaceNormal)
			if (TraceDotFaceNormal < BestOpposingDot)
			{
				BestOpposingDot = TraceDotFaceNormal;
				BestLocalNormal = PxVec3(0.f);
				BestLocalNormalPtr[i] = -1.f;
			}
		}
	}

	// Fill in result
	const PxVec3 WorldNormal = LocalToWorld.rotate(BestLocalNormal);
	return P2UVector(WorldNormal);
}