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

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;
}

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;
}

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;
}

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 PxScaleRigidActor(PxRigidActor& actor, PxReal scale, bool scaleMassProps)
{
	PX_CHECK_AND_RETURN(scale > 0,
		"PxScaleRigidActor requires that the scale parameter is greater than zero");

	Ps::InlineArray<PxShape*, 64> shapes;
	shapes.resize(actor.getNbShapes());
	actor.getShapes(shapes.begin(), shapes.size());

	for(PxU32 i=0;i<shapes.size();i++)
	{
		shapes[i]->setLocalPose(scalePosition(shapes[i]->getLocalPose(), scale));		
		PxGeometryHolder h = shapes[i]->getGeometry();

		switch(h.getType())
		{
		case PxGeometryType::eSPHERE:	
			h.sphere().radius *= scale;			
			break;
		case PxGeometryType::ePLANE:
			break;
		case PxGeometryType::eCAPSULE:
			h.capsule().halfHeight *= scale;
			h.capsule().radius *= scale;
			break;
		case PxGeometryType::eBOX:
			h.box().halfExtents *= scale;
			break;
		case PxGeometryType::eCONVEXMESH:
			h.convexMesh().scale.scale *= scale;
			break;
		case PxGeometryType::eTRIANGLEMESH:
			h.triangleMesh().scale.scale *= scale;
			break;
		case PxGeometryType::eHEIGHTFIELD:
			h.heightField().heightScale *= scale;
			h.heightField().rowScale *= scale;
			h.heightField().columnScale *= scale;
			break;
		case PxGeometryType::eINVALID:
		case PxGeometryType::eGEOMETRY_COUNT:
		default:
			PX_ASSERT(0);
		}
		shapes[i]->setGeometry(h.any());
	}

	if(!scaleMassProps)
		return;

	PxRigidDynamic* dynamic = (&actor)->is<PxRigidDynamic>();
	if(!dynamic)
		return;

	PxReal scale3 = scale*scale*scale;
	dynamic->setMass(dynamic->getMass()*scale3);
	dynamic->setMassSpaceInertiaTensor(dynamic->getMassSpaceInertiaTensor()*scale3*scale*scale);
	dynamic->setCMassLocalPose(scalePosition(dynamic->getCMassLocalPose(), scale));
}

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

PxRigidDynamic* Vehicle::createVehicleActor(const PxVehicleChassisData& chassisData,
	PxMaterial** wheelMaterials, PxConvexMesh** wheelConvexMeshes, const PxU32 numWheels,
	PxMaterial** chassisMaterials, PxConvexMesh** chassisConvexMeshes, const PxU32 numChassisMeshes, PxPhysics& physics, PxVec3 initPos)
{
	//We need a rigid body actor for the vehicle.
	//Don't forget to add the actor to the scene after setting up the associated vehicle.
	PxRigidDynamic* vehActor = physics.createRigidDynamic(PxTransform(initPos));
	vehActor->setRigidBodyFlag(PxRigidBodyFlag::eENABLE_CCD, true);

	//Wheel and chassis simulation filter data.
	PxFilterData wheelSimFilterData;
	wheelSimFilterData.word0 = COLLISION_FLAG_WHEEL;
	wheelSimFilterData.word1 = COLLISION_FLAG_WHEEL_AGAINST;
	PxFilterData chassisSimFilterData;
	chassisSimFilterData.word0 = COLLISION_FLAG_CHASSIS;
	chassisSimFilterData.word1 = COLLISION_FLAG_CHASSIS_AGAINST;

	//Wheel and chassis query filter data.
	//Optional: cars don't drive on other cars.
	PxFilterData wheelQryFilterData;
	wheelQryFilterData.word0 = FilterGroup::eWHEEL;
	setupNonDrivableSurface(wheelQryFilterData);
	PxFilterData chassisQryFilterData;
	chassisQryFilterData.word0 = FilterGroup::eVEHICLE;

	setupNonDrivableSurface(chassisQryFilterData);

	//Add all the wheel shapes to the actor.
	for (PxU32 i = 0; i < numWheels; i++)
	{
		PxConvexMeshGeometry geom(wheelConvexMeshes[i]);
		PxShape* wheelShape = vehActor->createShape(geom, *wheelMaterials[i]);
		wheelShape->setQueryFilterData(wheelQryFilterData);
		wheelShape->setSimulationFilterData(wheelSimFilterData);
		wheelShape->setLocalPose(PxTransform(PxIdentity));
	}

	//Add the chassis shapes to the actor.
	for (PxU32 i = 0; i < numChassisMeshes; i++)
	{
		PxShape* chassisShape = vehActor->createShape
			(PxConvexMeshGeometry(chassisConvexMeshes[i]), *chassisMaterials[i]);
		chassisShape->setQueryFilterData(chassisQryFilterData);
		chassisShape->setSimulationFilterData(chassisSimFilterData);
		chassisShape->setLocalPose(PxTransform(PxIdentity));
	}

	vehActor->setMass(chassisData.mMass);
	vehActor->setMassSpaceInertiaTensor(chassisData.mMOI);
	vehActor->setCMassLocalPose(PxTransform(chassisData.mCMOffset, PxQuat(PxIdentity)));

	return vehActor;
}

PxRigidDynamic* SampleParticles::Raygun::createRayCapsule(bool enableCollision, PxFilterData filterData)
{
	PxRigidDynamic* actor = mSample->getPhysics().createRigidDynamic(PxTransform::createIdentity());
	mSample->runtimeAssert(actor, "PxPhysics::createRigidDynamic returned NULL\n");
	PxShape* shape = actor->createShape(PxCapsuleGeometry(0.1f, 150.0f), mSample->getDefaultMaterial());
	mSample->runtimeAssert(shape, "PxRigidDynamic::createShape returned NULL\n");
	shape->setFlag(PxShapeFlag::eSCENE_QUERY_SHAPE, false);
	shape->setFlag(PxShapeFlag::eSIMULATION_SHAPE, enableCollision);
	shape->setSimulationFilterData(filterData);
	actor->setRigidDynamicFlag(PxRigidDynamicFlag::eKINEMATIC, true);
	actor->setMass(1.0f);
	actor->setMassSpaceInertiaTensor(PxVec3(1.0f));
	mSample->getActiveScene().addActor(*actor);	
	return actor;
}

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;
}

//.........这里部分代码省略.........

			//Ackermann steer accuracy
			PxVehicleAckermannGeometryData ackermann;
			ackermann.mAccuracy		  = 0.1f;
			ackermann.mAxleSeparation = wheelCentreOffsets[ 0 ].z - wheelCentreOffsets[ nWheels - 1 ].z;	// Расстояние между центром передней оси и центром задней оси
			ackermann.mFrontWidth	  = wheelCentreOffsets[ 0 ].x - wheelCentreOffsets[ 1		    ].x;	// Расстояние между центральной точке два передних колеса
			ackermann.mRearWidth	  = wheelCentreOffsets[ nWheels - 2 ].x - wheelCentreOffsets[ nWheels - 1 ].x;	// Расстояние между центральной точке два задних колеса
			driveData.setAckermannGeometryData(ackermann);			
			
			PxTriangleMesh * pTriangleMesh = 0;
			D3DXVECTOR3      vPosition;

			if( GameObject * pRoller = GetDetail( WHEEL_LEFT_1ST ) )
			{
				if( pRoller->CreateTriangleMesh( pPhysX ) )
				{
					pRoller->Update( 0.f );					
					pTriangleMesh = pRoller->GetTriangleMesh();
					Position      = pRoller->GetPosition();
				}
			}

			// Нам нужно добавить колеса столкновения форм, их местный позы, материал для колес, и моделирование фильтра для колес
			PxTriangleMeshGeometry WheelGeom( pTriangleMesh );
			
			PxGeometry* wheelGeometries[ nWheels ] = {0};
			PxTransform wheelLocalPoses[ nWheels ];

			for( PxU32 i = 0; i < nWheels; ++i )
			{
				wheelGeometries[ i ] = &WheelGeom;
				wheelLocalPoses[ i ] = PxTransform::createIdentity();
			}
			
			PxMaterial* pMaterial = pPhysX->GetPhysics()->createMaterial( 0.5f, 0.5f, 0.1f );    //коэффициенты трения скольжения и покоя(Dynamic friction,Static friction), коэффициент упругости
			const PxMaterial& wheelMaterial	= *pMaterial;
			PxFilterData wheelCollFilterData;

			wheelCollFilterData.word0 = COLLISION_FLAG_WHEEL;
			wheelCollFilterData.word1 = COLLISION_FLAG_WHEEL_AGAINST;

			// Нам нужно добавить шасси столкновения форм, их местный позы, материала для шасси и моделирования фильтр для шасси.
			//PxBoxGeometry chassisConvexGeom( 1.5f, 0.3f, 4.f );
			PxBoxGeometry chassisConvexGeom( chassisDims.x/2, chassisDims.y/2, chassisDims.z/2 );

			const PxGeometry* chassisGeoms	    = &chassisConvexGeom;
			const PxTransform chassisLocalPoses = PxTransform::createIdentity();
			const PxMaterial& chassisMaterial	= *pMaterial;

			PxFilterData chassisCollFilterData;
			chassisCollFilterData.word0 = COLLISION_FLAG_CHASSIS;
			chassisCollFilterData.word1 = COLLISION_FLAG_CHASSIS_AGAINST;

			// Создание фильтра запроса данных для автомобилей, чтобы машины не пытайтесь ездить на себя.
			PxFilterData vehQryFilterData;			
			SampleVehicleSetupVehicleShapeQueryFilterData( &vehQryFilterData );

			PxRigidDynamic* vehActor = pPhysX->GetPhysics()->createRigidDynamic( PxTransform::createIdentity() );

			//Add all the wheel shapes to the actor.
			for( PxU32 i = 0; i < nWheels; ++i )
			{
				PxShape* wheelShape=vehActor->createShape( *wheelGeometries[ i ], wheelMaterial );
				wheelShape->setQueryFilterData( vehQryFilterData );
				wheelShape->setSimulationFilterData( wheelCollFilterData );
				wheelShape->setLocalPose( wheelLocalPoses[ i ] );
				wheelShape->setFlag( PxShapeFlag::eSIMULATION_SHAPE, true );
			}

			//Add the chassis shapes to the actor			
			PxShape* chassisShape = vehActor->createShape( *chassisGeoms, chassisMaterial );
			chassisShape->setQueryFilterData( vehQryFilterData );
			chassisShape->setSimulationFilterData( chassisCollFilterData );
			chassisShape->setLocalPose( PxTransform( physx::PxVec3( 0, 0, 0 ) ) );
			

			vehActor->setMass( chassisData.mMass );
			vehActor->setMassSpaceInertiaTensor( chassisData.mMOI );
			vehActor->setCMassLocalPose( PxTransform( chassisData.mCMOffset, PxQuat::createIdentity() ) );
			vehActor->setGlobalPose( PxTransform( physx::PxVec3( 0, 8, 0 ), PxQuat::createIdentity() ) );

			PxVehicleDriveTank* pTank = PxVehicleDriveTank::allocate( nWheels );
 			
 			pTank->setup( pPhysX->GetPhysics(), vehActor, *wheelsSimData, driveData, nWheels );			
			pPhysX->AddActorScene( vehActor );
			m_pActor = vehActor;
			pPhysX->AddTank( pTank );

			//Free the sim data because we don't need that any more.
			wheelsSimData->free();
			//pTank->setDriveModel( PxVehicleDriveTank::eDRIVE_MODEL_SPECIAL );
			pTank->setToRestState();			
			pTank->mDriveDynData.setUseAutoGears( true );

			return true;
		}
	}

	return false;
}

bool UGripMotionControllerComponent::SetUpPhysicsHandle(const FBPActorGripInformation &NewGrip)
{
	UPrimitiveComponent *root = NewGrip.Component;
	if(!root)
		root = Cast<UPrimitiveComponent>(NewGrip.Actor->GetRootComponent());
	
	if (!root)
		return false;

	// Needs to be simulating in order to run physics
	root->SetSimulatePhysics(true);
	root->SetEnableGravity(false);

	FBPActorPhysicsHandleInformation * HandleInfo = CreatePhysicsGrip(NewGrip);

#if WITH_PHYSX
	// Get the PxRigidDynamic that we want to grab.
	FBodyInstance* BodyInstance = root->GetBodyInstance(NAME_None/*InBoneName*/);
	if (!BodyInstance)
	{
		return false;
	}

	ExecuteOnPxRigidDynamicReadWrite(BodyInstance, [&](PxRigidDynamic* Actor)
	{
		PxScene* Scene = Actor->getScene();
	
		// Get transform of actor we are grabbing

		FTransform WorldTransform;
		FTransform InverseTransform = this->GetComponentTransform().Inverse();
		WorldTransform = NewGrip.RelativeTransform.GetRelativeTransform(InverseTransform);

		PxVec3 KinLocation = U2PVector(WorldTransform.GetLocation() - (WorldTransform.GetLocation() - root->GetComponentLocation()));
		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 (!HandleInfo->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(0.0f); // 1.0f;
			KinActor->setMassSpaceInertiaTensor(PxVec3(0.0f, 0.0f, 0.0f));// PxVec3(1.0f, 1.0f, 1.0f));
			KinActor->setMaxDepenetrationVelocity(PX_MAX_F32);

			// No bodyinstance
			KinActor->userData = NULL;
			
			// Add to Scene
			Scene->addActor(*KinActor);

			// Save reference to the kinematic actor.
			HandleInfo->KinActorData = KinActor;

			// Create the joint
			PxVec3 LocalHandlePos = GrabbedActorPose.transformInv(KinLocation);
			PxD6Joint* NewJoint = PxD6JointCreate(Scene->getPhysics(), KinActor, PxTransform::createIdentity(), Actor, PxTransform(LocalHandlePos));
			
			if (!NewJoint)
			{
				HandleInfo->HandleData = 0;
			}
			else
			{
				// No constraint instance
				NewJoint->userData = NULL;
				HandleInfo->HandleData = NewJoint;

				// Remember the scene index that the handle joint/actor are in.
				FPhysScene* RBScene = FPhysxUserData::Get<FPhysScene>(Scene->userData);
				const uint32 SceneType = root->BodyInstance.UseAsyncScene(RBScene) ? PST_Async : PST_Sync;
				HandleInfo->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->setDrivePosition(PxTransform(PxVec3(0, 0, 0)));

				NewJoint->setMotion(PxD6Axis::eTWIST, PxD6Motion::eFREE);
				NewJoint->setMotion(PxD6Axis::eSWING1, PxD6Motion::eFREE);
				NewJoint->setMotion(PxD6Axis::eSWING2, PxD6Motion::eFREE);

				//UpdateDriveSettings();
				if (HandleInfo->HandleData != nullptr)
				{
					HandleInfo->HandleData->setDrive(PxD6Drive::eX, PxD6JointDrive(NewGrip.Stiffness, NewGrip.Damping, PX_MAX_F32, PxD6JointDriveFlag::eACCELERATION));
					HandleInfo->HandleData->setDrive(PxD6Drive::eY, PxD6JointDrive(NewGrip.Stiffness, NewGrip.Damping, PX_MAX_F32, PxD6JointDriveFlag::eACCELERATION));
					HandleInfo->HandleData->setDrive(PxD6Drive::eZ, PxD6JointDrive(NewGrip.Stiffness, NewGrip.Damping, PX_MAX_F32, PxD6JointDriveFlag::eACCELERATION));

					HandleInfo->HandleData->setDrive(PxD6Drive::eSLERP, PxD6JointDrive(NewGrip.Stiffness, NewGrip.Damping, PX_MAX_F32, PxD6JointDriveFlag::eACCELERATION));

						//HandleData->setDrive(PxD6Drive::eTWIST, PxD6JointDrive(Stiffness, Damping, PX_MAX_F32, PxD6JointDriveFlag::eACCELERATION));
						//HandleData->setDrive(PxD6Drive::eSWING, PxD6JointDrive(Stiffness, Damping, PX_MAX_F32, PxD6JointDriveFlag::eACCELERATION));
				}
//.........这里部分代码省略.........

//.........这里部分代码省略.........
		PxRigidDynamic*		sphere = PxCreateDynamic(*gPhysicsSDK, spherePos, sphereGeometry, *material, 1.0f);		
							gScene->addActor(*sphere);
	
		//Creating a trigger shape(trigger shape can't collide against any abject, 
		//thus it is made static otherwise it will fall under the effect of gravity) 
		PxTransform		boxPos(PxVec3(-10.0f, 2.10f, 0.0f));												
		PxShape*		boxShape =	gPhysicsSDK->createShape(PxBoxGeometry(PxVec3(3.0f,2.0f,3.0f)),*material);
						
						boxShape->setFlag(PxShapeFlag::eSIMULATION_SHAPE, false);	//flagged to disable shape collision
						boxShape->setFlag(PxShapeFlag::eTRIGGER_SHAPE, true);		//flagged as trigger shape

		PxRigidStatic*	gBox = PxCreateStatic(*gPhysicsSDK, boxPos, *boxShape);
						gScene->addActor(*gBox);														
	}
*/	

		
	{	
		int numberofblocks=1000;
		//PxVec3 offset = PxVec3(0,0,-1);

		PxReal radius=1;
		PxReal height=1;
		PxVec3 offset0 = PxVec3(-height,0,0);
		PxVec3 offset1 = PxVec3(height,0,0);
		PxVec3 initpos = PxVec3(0.0f, 0.0f, 0.0f);



		PxTransform		boxPos1(initpos,PxQuat(PxHalfPi, PxVec3(0.0f, 1.0f, 0.0f)));	//Position and orientation(transform) for box actor 
		PxRigidDynamic	*gBoxOri = NULL;				//Instance of box actor 
		PxCapsuleGeometry	sphereGeometry(4*radius,0.5*height);											//Defining geometry for box actor
		gBoxOri = PxCreateDynamic(*gPhysicsSDK, boxPos1, sphereGeometry, *material, 1.0f);		//Creating rigid static actor
		gBoxOri->setMass(Ma);
		gScene->addActor(*gBoxOri);														//Adding box actor to PhysX scene
		arrayofBodies.push_back(gBoxOri);

		for (PxU32 i=1; i<numberofblocks; i++)
		{
			if (i<numberofblocks-1){
				cout<<numberofblocks<<i<<"\n";
				PxTransform		boxPos1(initpos+PxVec3(0.0f, 0.0f, 2*i*height/*2.0f*/),PxQuat(PxHalfPi, PxVec3(0.0f, 1.0f, 0.0f)));												//Position and orientation(transform) for box actor 
				PxRigidDynamic	*gBox = NULL;				//Instance of box actor 
				PxCapsuleGeometry	sphereGeometry(radius,height);											//Defining geometry for box actor
								gBox = PxCreateDynamic(*gPhysicsSDK, boxPos1, sphereGeometry, *material, 1.0f);		//Creating rigid static actor
								gBox->setMass(Ma);
								gScene->addActor(*gBox);														//Adding box actor to PhysX scene

								// adding some joint to the mix
					//PxSphericalJoint* joint = PxSphericalJointCreate(*gPhysicsSDK, gBoxOri, PxTransform(-offset), gBox, PxTransform(offset));
			//PxSphericalJoint* joint = PxSphericalJointCreate(*gPhysicsSDK, gBoxOri, PxTransform(offset0), gBox, PxTransform(offset1)); //uncomment to use spherical joints
				PxD6Joint* joint = PxD6JointCreate(*gPhysicsSDK, gBoxOri, PxTransform(offset0), gBox, PxTransform(offset1));

				//add some limits to the joint
				joint->setSwingLimit(PxJointLimitCone(1.57f,1.57f,0.1f));
				joint->setConstraintFlag(PxConstraintFlag::eVISUALIZATION, true);
				joint->setMotion(PxD6Axis::eTWIST, PxD6Motion::eLOCKED);
				joint->setMotion(PxD6Axis::eSWING1, PxD6Motion::eLIMITED);
				joint->setMotion(PxD6Axis::eSWING2, PxD6Motion::eLIMITED);
					//joint->setMotion(PxD6Axis::eSWING1, PxD6Motion::eFREE); //free to rotate around y axis

			//add the body to the array before renaming the original box...
				arrayofBodies.push_back(gBox);

				gBoxOri=gBox;
			}

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