C++ PxRigidDynamic类代码示例

PxRigidDynamic* PxCreateKinematic(PxPhysics& sdk, 
								  const PxTransform& transform, 
								  const PxGeometry& geometry, 
								  PxMaterial& material,
								  PxReal density,
								  const PxTransform& shapeOffset)
{
	PX_CHECK_AND_RETURN_NULL(transform.isValid(), "PxCreateKinematic: transform is not valid.");
	PX_CHECK_AND_RETURN_NULL(shapeOffset.isValid(), "PxCreateKinematic: shapeOffset is not valid.");

	bool isDynGeom = isDynamicGeometry(geometry);
	if(isDynGeom && density <= 0.0f)
	    return NULL;

	PxShape* shape;
	PxRigidDynamic* actor = setShape(sdk.createRigidDynamic(transform), geometry, material, shapeOffset, shape);
	
	if(actor)
	{
		actor->setRigidDynamicFlag(PxRigidDynamicFlag::eKINEMATIC, true);

		if(isDynGeom)
			PxRigidBodyExt::updateMassAndInertia(*actor, density);
		else		
		{
			shape->setFlag(PxShapeFlag::eSIMULATION_SHAPE, false);
			actor->setMass(1);
			actor->setMassSpaceInertiaTensor(PxVec3(1,1,1));
		}
	}
	return actor;
}

void PhysX3::ApplyActionAtPoint(PintObjectHandle handle, PintActionType action_type, const Point& action, const Point& pos)
{
	PxRigidActor* RigidActor = GetActorFromHandle(handle);
	if(!RigidActor)
	{
		PxShape* Shape = GetShapeFromHandle(handle);
		ASSERT(Shape);
#ifdef SUPPORT_SHARED_SHAPES
		RigidActor = Shape->getActor();
#else
		RigidActor = &Shape->getActor();
#endif
	}

	if(RigidActor->getConcreteType()==PxConcreteType::eRIGID_DYNAMIC)
	{
		PxRigidDynamic* RigidDynamic = static_cast<PxRigidDynamic*>(RigidActor);
		if(!(RigidDynamic->getRigidBodyFlags() & PxRigidBodyFlag::eKINEMATIC))
		{
			PxForceMode::Enum mode;
			if(action_type==PINT_ACTION_FORCE)
				mode = PxForceMode::eFORCE;
			else if(action_type==PINT_ACTION_IMPULSE)
				mode = PxForceMode::eIMPULSE;
			else ASSERT(0);

			PxRigidBodyExt::addForceAtPos(*RigidDynamic, ToPxVec3(action), ToPxVec3(pos), mode);
		}
	}
}

	void SetupShapesUserData(const VehicleDescriptor &vd)
	{
		PxRigidDynamic *a = vd.vehicle->mActor;
		if (!a)
			return;

		PxU32 numShapes = a->getNbShapes();
		r3d_assert(numShapes <= vd.numWheels + vd.numHullParts);
		
		for (PxU32 i = 0; i < numShapes; ++i)
		{
			PxShape *s = 0;
			a->getShapes(&s, 1, i);
			if (!s)
				continue;

			if (i < vd.numWheels)
			{
				s->userData = reinterpret_cast<void*>(vd.wheelBonesRemapIndices[i]);
			}
			else
			{
				s->userData = reinterpret_cast<void*>(vd.hullBonesRemapIndices[i - vd.numWheels]);
			}
		}
	}

void 
PhysXRigidManager::update(const physx::PxActiveTransform * activeTransforms, physx::PxU32 nbActiveTransforms, float timeStep, physx::PxVec3 gravity) {
	for (PxU32 i = 0; i < nbActiveTransforms; ++i) {
		if (activeTransforms[i].userData != NULL) {
			std::string *n = static_cast<std::string*>(activeTransforms[i].userData);
			if (rigidBodies.find(*n) != rigidBodies.end()) {
				PxRigidDynamic * actor = rigidBodies[*n].info.actor->is<PxRigidDynamic>();
				if (rigidBodies[*n].rollingFriction >= 0.0f) {
					float mass = actor->getMass();
					float force = mass * gravity.magnitude() * rigidBodies[*n].rollingFriction;
					float actorMotion = (mass * actor->getLinearVelocity().magnitude()) / ((float)rigidBodies[*n].rollingFrictionTimeStamp * timeStep);
					if (force <= actorMotion) {
						rigidBodies[*n].rollingFrictionTimeStamp++;
						PxVec3 forceVec = -(actor->getLinearVelocity().getNormalized() * force);
						actor->addForce(forceVec);
					}
					else {
						rigidBodies[*n].rollingFrictionTimeStamp = 1;
						//actor->setLinearVelocity(actor->getLinearVelocity() / 1.5f);
						//actor->setAngularVelocity(actor->getAngularVelocity() / 1.5f);
						//actor->setLinearVelocity(PxVec3(0.0f));
						//actor->setAngularVelocity(PxVec3(0.0f));
					}
				}
				PxMat44 mat(activeTransforms[i].actor2World);
				mat.scale(PxVec4(PxVec3(rigidBodies[*n].scalingFactor), 1.0f));
				//getMatFromPhysXTransform(activeTransforms[i].actor2World, rigidBodies[*n].extInfo.transform);
				getMatFromPhysXTransform(mat, rigidBodies[*n].info.extInfo.transform);
			}
		}
	}
}

/**
 @brief 
 @date 2013-12-03
*/
void CEvc::pickup()
{
	PxU32 width;
	PxU32 height;
	mApplication.getPlatform()->getWindowSize(width, height);
	mPicking->moveCursor(width/2,height/2);
	mPicking->lazyPick();
	PxActor *actor = mPicking->letGo();
	//PxRigidDynamic *rigidActor = static_cast<PxRigidDynamic*>(actor->is<PxRigidDynamic>());
	PxRigidDynamic *rigidActor = (PxRigidDynamic*)actor;
	if (rigidActor)
	{
		const PxVec3 pos = getCamera().getPos() + (getCamera().getViewDir()*10.f);
		const PxVec3 vel = getCamera().getViewDir() * 20.f;

		rigidActor->addForce( getCamera().getViewDir()*g_pDbgConfig->force );

		PxU32 nbShapes = rigidActor->getNbShapes();
		if(!nbShapes)
			return;

		PxShape** shapes = (PxShape**)SAMPLE_ALLOC(sizeof(PxShape*)*nbShapes);
		PxU32 nb = rigidActor->getShapes(shapes, nbShapes);
		PX_ASSERT(nb==nbShapes);
		for(PxU32 i=0;i<nbShapes;i++)
		{
			RenderBaseActor *renderActor = getRenderActor(rigidActor, shapes[i]);
			if (renderActor)
			{
				renderActor->setRenderMaterial(mManagedMaterials[ 1]);
			}
		}
		SAMPLE_FREE(shapes);
	}
}

PxRigidDynamic* PxCreateKinematic(PxPhysics& sdk, 
								  const PxTransform& transform, 
								  PxShape& shape,
								  PxReal density)
{
	PX_CHECK_AND_RETURN_NULL(transform.isValid(), "PxCreateKinematic: transform is not valid.");

	bool isDynGeom = isDynamicGeometry(shape.getGeometryType());
	if(isDynGeom && density <= 0.0f)
	    return NULL;

	PxRigidDynamic* actor = sdk.createRigidDynamic(transform);	
	if(actor)
	{
		actor->setRigidBodyFlag(PxRigidBodyFlag::eKINEMATIC, true);
		if(!isDynGeom)
			shape.setFlag(PxShapeFlag::eSIMULATION_SHAPE, false);

		actor->attachShape(shape);

		if(isDynGeom)
			PxRigidBodyExt::updateMassAndInertia(*actor, density);
		else		
		{
			actor->setMass(1.f);
			actor->setMassSpaceInertiaTensor(PxVec3(1.f,1.f,1.f));
		}
	}

	return actor;
}

/** 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
}

PxActor* World::createRigidBody(const PxGeometry& geometry, float mass, const ofVec3f& pos, const ofQuaternion& rot, float density)
{
	assert(inited);
	
	PxTransform transform;
	toPx(pos, transform.p);
	toPx(rot, transform.q);
	
	PxActor *actor;
	
	if (mass > 0)
	{
		PxRigidDynamic* rigid = PxCreateDynamic(*physics, transform, geometry, *defaultMaterial, density);
		rigid->setMass(mass);
		rigid->setLinearDamping(0.25);
		rigid->setAngularDamping(0.25);
		
		actor = rigid;
	}
	else
	{
		PxRigidStatic *rigid = PxCreateStatic(*physics, transform, geometry, *defaultMaterial);
		actor = rigid;
	}
	
	scene->addActor(*actor);
	
	return actor;
}

/** 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 Apex::LoadDynamicTriangleMesh(int numVerts, PxVec3* verts, ObjectInfo info)
{
	PxRigidDynamic* meshActor = mPhysics->createRigidDynamic(PxTransform::createIdentity());
	PxShape* meshShape, *convexShape;
	if(meshActor)
	{
		//meshActor->setRigidDynamicFlag(PxRigidDynamicFlag::eKINEMATIC, true);

		PxTriangleMeshDesc meshDesc;
		meshDesc.points.count           = numVerts;
		meshDesc.points.stride          = sizeof(PxVec3);
		meshDesc.points.data            = verts;

		//meshDesc.triangles.count        = numInds/3.;
		//meshDesc.triangles.stride       = 3*sizeof(int);
		//meshDesc.triangles.data         = inds;

		PxToolkit::MemoryOutputStream writeBuffer;
		bool status = mCooking->cookTriangleMesh(meshDesc, writeBuffer);
		if(!status)
			return;

		PxToolkit::MemoryInputData readBuffer(writeBuffer.getData(), writeBuffer.getSize());

		PxTriangleMeshGeometry triGeom;
		triGeom.triangleMesh = mPhysics->createTriangleMesh(readBuffer);
		//triGeom.scale = PxMeshScale(PxVec3(info.sx,info.sy,info.sz),physx::PxQuat::createIdentity());
		
		meshShape = meshActor->createShape(triGeom, *defaultMaterial);
		//meshShape->setLocalPose(PxTransform(PxVec3(info.x,info.y,info.z)));
		meshShape->setFlag(PxShapeFlag::eUSE_SWEPT_BOUNDS, true);

		PxConvexMeshDesc convexDesc;
		convexDesc.points.count     = numVerts;
		convexDesc.points.stride    = sizeof(PxVec3);
		convexDesc.points.data      = verts;
		convexDesc.flags            = PxConvexFlag::eCOMPUTE_CONVEX;

		if(!convexDesc.isValid())
			return;
		PxToolkit::MemoryOutputStream buf;
		if(!mCooking->cookConvexMesh(convexDesc, buf))
			return;
		PxToolkit::MemoryInputData input(buf.getData(), buf.getSize());
		PxConvexMesh* convexMesh = mPhysics->createConvexMesh(input);
		PxConvexMeshGeometry convexGeom = PxConvexMeshGeometry(convexMesh);
		convexShape = meshActor->createShape(convexGeom, *defaultMaterial);
		//convexShape->setLocalPose(PxTransform(PxVec3(info.x,info.y,info.z)));
		//convexShape->setFlag(PxShapeFlag::eSIMULATION_SHAPE, false);

		
		convexShape->setFlag(PxShapeFlag::eSIMULATION_SHAPE, true);
		meshShape->setFlag(PxShapeFlag::eSIMULATION_SHAPE, false);
		meshActor->setRigidDynamicFlag(PxRigidDynamicFlag::eKINEMATIC, false);

		meshActor->setGlobalPose(PxTransform(PxVec3(info.x,info.y,info.z), PxQuat(info.ry, PxVec3(0.0f,1.0f,0.0f))));
		mScene[mCurrentScene]->addActor(*meshActor);
		dynamicActors.push_back(meshActor);
	}
}

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 labscale::ControlFillBoxExperiment()
{
    // Pour regolith, one by one every second
    int nbGrains = gPhysX.mScene->getNbActors(gPhysX.roles.dynamics) - 1;
    static PxReal poured_time = 0;
    if ((gSim.codeTime - poured_time) > (1/labscale::reg_box.pourRate) && nbGrains < labscale::regolith.nbGrains)
    {
        // Pour a grain
        PxRigidDynamic* grain = CreateRegolithGrain();
        RandLaunchActor(grain,2);
        PxVec3 v = grain->getLinearVelocity();
        grain->setLinearVelocity(PxVec3(v.x,0,v.z));

        // Reset timer
        poured_time = gSim.codeTime;
    }

    // When done save scene and stop
    if (nbGrains >= labscale::regolith.nbGrains && CountSleepers() == gPhysX.mScene->getNbActors(gPhysX.roles.dynamics))
    {
        gSim.isRunning = false;
        SaveSceneToRepXDump();
    }

    // Hack if rebooting (F10 was pressed) - not really important
    if ((gSim.codeTime - poured_time) < 0) poured_time = gSim.codeTime;
}

void PhysXRigidManager::setImpulse(std::string scene, float * impulse) {
	if (rigidBodies.find(scene) != rigidBodies.end()) {
		PxRigidDynamic * actor = rigidBodies[scene].info.actor->is<PxRigidDynamic>();
		if (actor) {
			actor->addForce(PxVec3(impulse[0], impulse[1], impulse[2]), PxForceMode::eIMPULSE);
		}
	}
}

void PhysXRigidManager::setInertiaTensor(std::string name, float * value) {
	if (rigidBodies.find(name) != rigidBodies.end()) {
		PxRigidDynamic * dyn = rigidBodies[name].info.actor->is<PxRigidDynamic>();
		if (dyn) {
			dyn->setMassSpaceInertiaTensor(PxVec3(value[0], value[1], value[2]));
		}
	}
}