PxRigidDynamic* PxCloneDynamic(PxPhysics& physicsSDK, 
							   const PxTransform& transform,
							   const PxRigidDynamic& from)
{
	PxRigidDynamic* to = physicsSDK.createRigidDynamic(transform);
	if(!to)
		return NULL;

	copyStaticProperties(*to, from);

	to->setRigidDynamicFlags(from.getRigidDynamicFlags());

	to->setMass(from.getMass());
	to->setMassSpaceInertiaTensor(from.getMassSpaceInertiaTensor());
	to->setCMassLocalPose(from.getCMassLocalPose());

	to->setLinearVelocity(from.getLinearVelocity());
	to->setAngularVelocity(from.getAngularVelocity());

	to->setLinearDamping(from.getAngularDamping());
	to->setAngularDamping(from.getAngularDamping());

	to->setMaxAngularVelocity(from.getMaxAngularVelocity());

	PxU32 posIters, velIters;
	from.getSolverIterationCounts(posIters, velIters);
	to->setSolverIterationCounts(posIters, velIters);

	to->setSleepThreshold(from.getSleepThreshold());

	to->setContactReportThreshold(from.getContactReportThreshold());

	return to;
}
void 
Spacetime::restoreState(void) {
	for (int i = 0; i < dynamic_actors.size(); i++) {
		PxRigidDynamic *current = dynamic_actors[i];
		current->setLinearVelocity(linearVelocityVector[i]);
		current->setAngularVelocity(angularVelocityVector[i]);
		current->setGlobalPose(globalPoseVector[i]);
	}
}
PxRigidDynamic* CloneDynamic(PxPhysics& physicsSDK, 
							   const PxTransform& transform,
							   const PxRigidDynamic& from,
							   NxMirrorScene::MirrorFilter &mirrorFilter)
{
	PxRigidDynamic* to = physicsSDK.createRigidDynamic(transform);
	if(!to)
		return NULL;

	if ( !copyStaticProperties(*to, from, mirrorFilter) )
	{
		to->release();
		to = NULL;
		return NULL;
	}

	to->setRigidDynamicFlags(from.getRigidDynamicFlags());

	to->setMass(from.getMass());
	to->setMassSpaceInertiaTensor(from.getMassSpaceInertiaTensor());
	to->setCMassLocalPose(from.getCMassLocalPose());

	if ( !(to->getRigidDynamicFlags() & PxRigidDynamicFlag::eKINEMATIC) )
	{
		to->setLinearVelocity(from.getLinearVelocity());
		to->setAngularVelocity(from.getAngularVelocity());
	}

	to->setLinearDamping(from.getAngularDamping());
	to->setAngularDamping(from.getAngularDamping());

	to->setMaxAngularVelocity(from.getMaxAngularVelocity());

	PxU32 posIters, velIters;
	from.getSolverIterationCounts(posIters, velIters);
	to->setSolverIterationCounts(posIters, velIters);

	to->setSleepThreshold(from.getSleepThreshold());

	to->setContactReportThreshold(from.getContactReportThreshold());

	return to;
}
void
Spacetime::setState(matrix<double> stateVector) {
	std::vector<PxQuat> theta;
	for (int i = 0; i < joints.size(); i++) {
		PxQuat q = PxQuat::createIdentity();
		if (i == 0) {
			if (DOF > X) { q *= PxQuat(stateVector((i)*DOF+X,0), PxVec3(1,0,0)); }
			if (DOF > Y) { q *= PxQuat(stateVector((i)*DOF+Y,0), PxVec3(0,1,0)); }
			if (DOF > Z) { q *= PxQuat(stateVector((i)*DOF+Z,0), PxVec3(0,0,1)); }
		} else {
			if (DOF > X) { q *= PxQuat(stateVector((i)*DOF+X,0), PxVec3(1,0,0)) * theta[(i-1)*DOF+X]; }
			if (DOF > Y) { q *= PxQuat(stateVector((i)*DOF+Y,0), PxVec3(0,1,0)) * theta[(i-1)*DOF+Y]; }
			if (DOF > Z) { q *= PxQuat(stateVector((i)*DOF+Z,0), PxVec3(0,0,1)) * theta[(i-1)*DOF+Z]; }
		}
		theta.push_back(q);
	}
	dynamic_actors[0]->setGlobalPose(PxTransform(root, PxQuat::createIdentity()));
	PxVec3 lastJointPos = dynamic_actors[0]->getGlobalPose().p + PxVec3(0,0.5,0);
	PxQuat lastJointRot = dynamic_actors[0]->getGlobalPose().q;
	for (int i = 0; i < joints.size(); i++) {
		PxRigidDynamic *current = dynamic_actors[i+1];
		PxVec3 t = theta[i].rotate(-joint_local_positions[i]);
		PxVec3 gPos = lastJointPos + t;
		current->setGlobalPose(PxTransform(gPos, theta[i]));
		lastJointPos = lastJointPos + 2*t;
	}
	for (int i = 0; i < joints.size(); i++) {
		PxRigidDynamic *current = dynamic_actors[i+1];
		PxVec3 angularVelocity;
		if (DOF > X) { angularVelocity[X] = stateVector(joints.size()*DOF + i*DOF+X,0); }
		else		 { angularVelocity[X] = 0.0; }
		if (DOF > Y) { angularVelocity[Y] = stateVector(joints.size()*DOF + i*DOF+Y,0); }
		else		 { angularVelocity[Y] = 0.0; }
		if (DOF > Z) { angularVelocity[Z] = stateVector(joints.size()*DOF + i*DOF+Z,0); }
		else		 { angularVelocity[Z] = 0.0; }
		current->setAngularVelocity(angularVelocity);
		current->setLinearVelocity(PxVec3(0,0,0));
	}
}
Example #5
0
void SampleSubmarine::explode(PxRigidActor* actor, const PxVec3& explosionPos, const PxReal explosionStrength)
{
	size_t numRenderActors = mRenderActors.size();
	for(PxU32 i = 0; i < numRenderActors; i++)
	{
		if(&(mRenderActors[i]->getPhysicsShape()->getActor()) == actor)
		{
			PxShape* shape = mRenderActors[i]->getPhysicsShape();
			PxTransform pose = PxShapeExt::getGlobalPose(*shape);
			
			PxGeometryHolder geom = shape->getGeometry();

			// create new actor from shape (to split compound)
			PxRigidDynamic* newActor = mPhysics->createRigidDynamic(pose);
			if(!newActor) fatalError("createRigidDynamic failed!");

			PxShape* newShape = newActor->createShape(geom.any(), *mMaterial);
			newShape->userData = mRenderActors[i];
			mRenderActors[i]->setPhysicsShape(newShape);
			
			newActor->setActorFlag(PxActorFlag::eVISUALIZATION, true);
			newActor->setLinearDamping(10.5f);
			newActor->setAngularDamping(0.5f);
			PxRigidBodyExt::updateMassAndInertia(*newActor, 1.0f);
			mScene->addActor(*newActor);
			mPhysicsActors.push_back(newActor);
			
			PxVec3 explosion = pose.p - explosionPos;
			PxReal len = explosion.normalize();
			explosion *= (explosionStrength / len);
			newActor->setLinearVelocity(explosion);
			newActor->setAngularVelocity(PxVec3(1,2,3));

		}
	}

	removeActor(actor);
}
void PxVehicleCopyDynamicsData(const PxVehicleCopyDynamicsMap& wheelMap, const PxVehicleWheels& src, PxVehicleWheels* trg)
{
	PX_CHECK_AND_RETURN(trg, "PxVehicleCopyDynamicsData requires that trg is a valid vehicle pointer");

	PX_CHECK_AND_RETURN(src.getVehicleType() == trg->getVehicleType(), "PxVehicleCopyDynamicsData requires that both src and trg are the same type of vehicle");

#ifdef PX_CHECKED
	{
		const PxU32 numWheelsSrc = src.mWheelsSimData.getNbWheels();
		const PxU32 numWheelsTrg = trg->mWheelsSimData.getNbWheels();
		PxU8 copiedWheelsSrc[PX_MAX_NB_WHEELS];
		PxMemZero(copiedWheelsSrc, sizeof(PxU8) * PX_MAX_NB_WHEELS);
		PxU8 setWheelsTrg[PX_MAX_NB_WHEELS];
		PxMemZero(setWheelsTrg, sizeof(PxU8) * PX_MAX_NB_WHEELS);
		for(PxU32 i = 0; i < PxMin(numWheelsSrc, numWheelsTrg); i++)
		{
			const PxU32 srcWheelId = wheelMap.sourceWheelIds[i];
			PX_CHECK_AND_RETURN(srcWheelId < numWheelsSrc, "PxVehicleCopyDynamicsData - wheelMap contains illegal source wheel id");
			PX_CHECK_AND_RETURN(0 == copiedWheelsSrc[srcWheelId], "PxVehicleCopyDynamicsData - wheelMap contains illegal source wheel id");
			copiedWheelsSrc[srcWheelId] = 1;

			const PxU32 trgWheelId = wheelMap.targetWheelIds[i];
			PX_CHECK_AND_RETURN(trgWheelId < numWheelsTrg, "PxVehicleCopyDynamicsData - wheelMap contains illegal target wheel id");
			PX_CHECK_AND_RETURN(0 == setWheelsTrg[trgWheelId], "PxVehicleCopyDynamicsData - wheelMap contains illegal target wheel id");
			setWheelsTrg[trgWheelId]=1;
		}
	}
#endif


	const PxU32 numWheelsSrc = src.mWheelsSimData.getNbWheels();
	const PxU32 numWheelsTrg = trg->mWheelsSimData.getNbWheels();

	//Set all dynamics data on the target to zero.
	//Be aware that setToRestState sets the rigid body to 
	//rest so set the momentum back after calling setToRestState.
	PxRigidDynamic* actorTrg = trg->getRigidDynamicActor();
	PxVec3 linVel = actorTrg->getLinearVelocity();
	PxVec3 angVel = actorTrg->getAngularVelocity();
	switch(src.getVehicleType())
	{
	case PxVehicleTypes::eDRIVE4W:
		((PxVehicleDrive4W*)trg)->setToRestState();
		break;
	case PxVehicleTypes::eDRIVENW:
		((PxVehicleDriveNW*)trg)->setToRestState();
		break;
	case PxVehicleTypes::eDRIVETANK:
		((PxVehicleDriveTank*)trg)->setToRestState();
		break;
	case PxVehicleTypes::eNODRIVE:
		((PxVehicleNoDrive*)trg)->setToRestState();
		break;
	default:
		break;
	}
	actorTrg->setLinearVelocity(linVel);
	actorTrg->setAngularVelocity(angVel);


	//Keep a track of the wheels on trg that have their dynamics data set as a copy from src.
	PxU8 setWheelsTrg[PX_MAX_NB_WHEELS];
	PxMemZero(setWheelsTrg, sizeof(PxU8) * PX_MAX_NB_WHEELS);

	//Keep a track of the average wheel rotation speed of all enabled wheels on src.
	PxU32 numEnabledWheelsSrc = 0;
	PxF32 accumulatedWheelRotationSpeedSrc = 0.0f;

	//Copy wheel dynamics data from src wheels to trg wheels.
	//Track the target wheels that have been given dynamics data from src wheels.
	//Compute the accumulated wheel rotation speed of all enabled src wheels.
	const PxU32 numMappedWheels = PxMin(numWheelsSrc, numWheelsTrg);
	for(PxU32 i = 0; i < numMappedWheels; i++)
	{
		const PxU32 srcWheelId = wheelMap.sourceWheelIds[i];
		const PxU32 trgWheelId = wheelMap.targetWheelIds[i];

		trg->mWheelsDynData.copy(src.mWheelsDynData, srcWheelId, trgWheelId);

		setWheelsTrg[trgWheelId] = 1;

		if(!src.mWheelsSimData.getIsWheelDisabled(srcWheelId))
		{
			numEnabledWheelsSrc++;
			accumulatedWheelRotationSpeedSrc += src.mWheelsDynData.getWheelRotationSpeed(srcWheelId);
		}
	}

	//Compute the average wheel rotation speed of src.
	PxF32 averageWheelRotationSpeedSrc = 0;
	if(numEnabledWheelsSrc > 0)
	{
		averageWheelRotationSpeedSrc = (accumulatedWheelRotationSpeedSrc/ (1.0f * numEnabledWheelsSrc));
	}

	//For wheels on trg that have not had their dynamics data copied from src just set
	//their wheel rotation speed to the average wheel rotation speed.
	for(PxU32 i = 0; i < numWheelsTrg; i++)
	{
		if(0 == setWheelsTrg[i] && !trg->mWheelsSimData.getIsWheelDisabled(i))
		{
			trg->mWheelsDynData.setWheelRotationSpeed(i, averageWheelRotationSpeedSrc);
		}
	}

	//Copy the engine rotation speed/gear states/autobox states/etc.
	switch(src.getVehicleType())
	{
	case PxVehicleTypes::eDRIVE4W:
	case PxVehicleTypes::eDRIVENW:
	case PxVehicleTypes::eDRIVETANK:
		{
			const PxVehicleDriveDynData& driveDynDataSrc = ((const PxVehicleDrive&)src).mDriveDynData;
			PxVehicleDriveDynData* driveDynDataTrg = &((PxVehicleDrive*)trg)->mDriveDynData;
			*driveDynDataTrg = driveDynDataSrc;
		}
		break;
	default:
		break;
	}
}