C++ NxQuat类代码示例

NxWheelShape* AddWheelToActor(NxActor* actor, NxWheelDesc* wheelDesc)
{
	NxWheelShapeDesc wheelShapeDesc;

	// Create a shared car wheel material to be used by all wheels
	if (!wsm)
	{
		NxMaterialDesc m;
		m.flags |= NX_MF_DISABLE_FRICTION;
		wsm = gScene->createMaterial(m);
	}
	wheelShapeDesc.materialIndex = wsm->getMaterialIndex();

	wheelShapeDesc.localPose.t = wheelDesc->position;
	NxQuat q;
	q.fromAngleAxis(90, NxVec3(0,1,0));
	wheelShapeDesc.localPose.M.fromQuat(q);

	NxReal heightModifier = (wheelDesc->wheelSuspension + wheelDesc->wheelRadius) / wheelDesc->wheelSuspension;

	wheelShapeDesc.suspension.spring = wheelDesc->springRestitution*heightModifier;
	wheelShapeDesc.suspension.damper = wheelDesc->springDamping*heightModifier;
	wheelShapeDesc.suspension.targetValue = wheelDesc->springBias*heightModifier;

	wheelShapeDesc.radius = wheelDesc->wheelRadius;
	wheelShapeDesc.suspensionTravel = wheelDesc->wheelSuspension; 
	wheelShapeDesc.inverseWheelMass = 0.1;	//not given!? TODO

//	wheelShapeDesc.lateralTireForceFunction.stiffnessFactor *= wheelDesc->frictionToSide;
//	wheelShapeDesc.longitudalTireForceFunction.stiffnessFactor *= wheelDesc->frictionToFront;

    NxWheelShape* wheelShape = NULL;
	wheelShape = static_cast<NxWheelShape *>(actor->createShape(wheelShapeDesc));
	return wheelShape;
}

NxFluidEmitter* CreateFluidEmitter(const NxReal dimX, const NxReal dimY)
{
	fluid = CreateFluid();
	assert(fluid);

	NxQuat q;
	q.fromAngleAxis(90,NxVec3(1,0,0));
	NxMat34 mat;
	mat.M.fromQuat(q);
	mat.t = NxVec3(0,4.5,0);
	// Create emitter
	NxFluidEmitterDesc emitterDesc;
	emitterDesc.setToDefault();
	emitterDesc.frameShape = NULL;
	emitterDesc.dimensionX = dimX;
	emitterDesc.dimensionY = dimY;
	emitterDesc.relPose = mat;
	emitterDesc.rate = 100;
	emitterDesc.randomAngle = 0.0f;
	emitterDesc.randomPos = NxVec3(0.0f,0.0f,0.0f);
	emitterDesc.fluidVelocityMagnitude = 2.5f;
	emitterDesc.repulsionCoefficient = 0.02f;
	emitterDesc.maxParticles = 0;
	emitterDesc.particleLifetime = 0.0f;
	emitterDesc.type = NX_FE_CONSTANT_PRESSURE;
	emitterDesc.shape = NX_FE_ELLIPSE;
	return fluid->createEmitter(emitterDesc);
}

void PhysXShape::setLocalOrintation(const math::quaternion&v)
{
	NxMat33 nm;
	NxQuat q;
	q.setWXYZ(v.w,v.x,v.y,v.z);
	nm.fromQuat(q);
	m_nxShape->setLocalOrientation(nm);
}

void SetupAttachmentScene()
{
    sprintf(gTitleString, "Attachment Demo");

	// Create objects in scene
	groundPlane = CreateGroundPlane();
	NxActor* box1 = CreateBox(NxVec3(-7,12.25,0), NxVec3(2.5,1,1), 0);
	NxActor* box2 = CreateBox(NxVec3(0,12.25,0), NxVec3(2.5,1,1), 0);
	NxActor* box3 = CreateBox(NxVec3(7,12.25,0), NxVec3(2.5,1,1), 0);	

	NxActor* attachedBox = CreateBox(NxVec3(-7.2,4.5,1.6), NxVec3(1.25,1,1), 1);
	NxActor* attachedSphere = CreateSphere(NxVec3(-0.25,4.0,2.0), 1.3, 1);
	NxActor* attachedCapsule = CreateCapsule(NxVec3(9.0,5.5,2.0),2.0, 1, 1); 

	NxReal damping = 0.3;
	attachedBox->setAngularDamping(damping);
	attachedBox->setLinearDamping(damping);
	attachedSphere->setAngularDamping(damping);
	attachedSphere->setLinearDamping(damping);
	attachedCapsule->setAngularDamping(damping);
	attachedCapsule->setLinearDamping(damping);

	NxQuat q;
	q.fromAngleAxis(90,NxVec3(0,0,1));
	attachedCapsule->setGlobalOrientationQuat(q);

	// Cloth
	NxClothDesc clothDesc;
	clothDesc.globalPose.M.rotX(1.3);
	clothDesc.thickness = 0.3;	
	clothDesc.attachmentResponseCoefficient = 1;
	clothDesc.flags |= NX_CLF_BENDING;
	clothDesc.flags |= NX_CLF_BENDING_ORTHO;
	clothDesc.flags |= NX_CLF_DAMPING | NX_CLF_VISUALIZATION;

	if (gHardwareCloth)
		clothDesc.flags |= NX_CLF_HARDWARE;

	// Cloth attaching to sphere
	clothDesc.globalPose.t = NxVec3(0.75,5,2);
	MyCloth* regularCloth1 = new MyCloth(gScene, clothDesc, 2, 8, 0.4);
	regularCloth1->getNxCloth()->attachToCollidingShapes(NX_CLOTH_ATTACHMENT_TWOWAY);
	gCloths.push_back(regularCloth1);

	// Cloth attaching to box
	clothDesc.globalPose.t = NxVec3(-6.2,5,2);
	MyCloth* regularCloth2 = new MyCloth(gScene, clothDesc, 2, 8, 0.4);
	regularCloth2->getNxCloth()->attachToCollidingShapes(NX_CLOTH_ATTACHMENT_TWOWAY);
	gCloths.push_back(regularCloth2);

	// Cloth attaching to capsule
	clothDesc.globalPose.t = NxVec3(8.0,5,2);
	clothDesc.attachmentTearFactor = 2.0;
	MyCloth* regularCloth3 = new MyCloth(gScene, clothDesc, 2, 8, 0.4);
	regularCloth3->getNxCloth()->attachToShape(box3->getShapes()[0], NX_CLOTH_ATTACHMENT_TEARABLE);
	regularCloth3->getNxCloth()->attachToShape(attachedCapsule->getShapes()[0], NX_CLOTH_ATTACHMENT_TWOWAY);
	gCloths.push_back(regularCloth3);
}

NxQuat getFrom(VxQuaternion source)
{
	NxQuat result;
	result.setx(-source.x);
	result.sety(-source.y);
	result.setz(-source.z);
	result.setw(source.w);
	return result;
}

/***********************************************************
get direction vector
***********************************************************/
LbaVec3 LbaQuaternion::GetDirection(const LbaVec3 &vec)
{
	NxVec3 dir(vec.x, vec.y, vec.z);
	NxQuat current;
	current.setXYZW(X, Y, Z, W);

	current.rotate(dir);

	return LbaVec3(dir.x, dir.y, dir.z);
}

/***********************************************************
add rotation to quaternion
***********************************************************/
void LbaQuaternion::AddRotation(float angle, LbaVec3 vec)
{
	NxQuat q(angle, NxVec3(vec.x, vec.y, vec.z));
	NxQuat current;
	current.setXYZW(X, Y, Z, W);

	current = q * current;

	X = current.x;
	Y = current.y;
	Z = current.z;
	W = current.w;
}

	void NxQuat::rotate(Point & v) const
		{
		NxQuat myInverse;
		myInverse.x = -x;
		myInverse.y = -y;
		myInverse.z = -z;
		myInverse.w =  w;

		NxQuat left;
		left.multiply(*this,v);
		v.x = left.w*myInverse.x + myInverse.w*left.x + left.y*myInverse.z - myInverse.y*left.z;
		v.y = left.w*myInverse.y + myInverse.w*left.y + left.z*myInverse.x - myInverse.z*left.x;
		v.z = left.w*myInverse.z + myInverse.w*left.z + left.x*myInverse.y - myInverse.x*left.y;
		}

void Vehicle::resetCarPos()
{

	NxVec3 vec3(mOriginalPos.x, mOriginalPos.y, mOriginalPos.z);
	NxQuat quat;
	quat.setWXYZ(mOriginalQuat.w, mOriginalQuat.x, mOriginalQuat.y, mOriginalQuat.z);

	mActor->setGlobalPose(NxMat34(quat, vec3));

	vec3.zero();
	mActor->setLinearVelocity(vec3);
	mActor->setAngularVelocity(vec3);

}

	void rotate(btVector3 & v) const
	{
	NxQuat myInverse;
	myInverse.x = -x;
	myInverse.y = -y;
	myInverse.z = -z;
	myInverse.w =  w;

	NxQuat left;
	left.multiply(*this,v);
	float vx = left.w*myInverse.x + myInverse.w*left.x + left.y*myInverse.z - myInverse.y*left.z;
	float vy = left.w*myInverse.y + myInverse.w*left.y + left.z*myInverse.x - myInverse.z*left.x;
	float vz = left.w*myInverse.z + myInverse.w*left.z + left.x*myInverse.y - myInverse.x*left.y;
	v.setValue(vx, vy, vz);
	}

void NxVehicle::_computeLocalVelocity()
{
	_computeMostTouchedActor();
	NxVec3 relativeVelocity;
	if (_mostTouchedActor == NULL || !_mostTouchedActor->isDynamic())
	{
		relativeVelocity = _bodyActor->getLinearVelocity();
	} else {
		relativeVelocity = _bodyActor->getLinearVelocity() - _mostTouchedActor->getLinearVelocity();
	}
	NxQuat rotation = _bodyActor->getGlobalOrientationQuat();
	NxQuat global2Local;
	_localVelocity = relativeVelocity;
	rotation.inverseRotate(_localVelocity);
	//printf("Velocity: %2.3f %2.3f %2.3f\n", _localVelocity.x, _localVelocity.y, _localVelocity.z);
}

void NxVehicle::standUp()
{
	NxVec3 pos = getActor()->getGlobalPosition() + NxVec3(0,2,0);
	NxQuat rot = getActor()->getGlobalOrientationQuat();
	NxVec3 front(1,0,0);
	rot.rotate(front);
	front.y = 0;
	front.normalize();

	NxReal dotproduct  = front.x;

	NxReal angle = NxMath::sign(-front.z) * NxMath::acos(dotproduct);

	rot.fromAngleAxis(NxMath::radToDeg(angle), NxVec3(0,1,0));
	getActor()->setGlobalPosition(pos);
	getActor()->setGlobalOrientationQuat(rot);
	getActor()->setLinearVelocity(NxVec3(0,0,0));
	getActor()->setAngularVelocity(NxVec3(0,0,0));
}

void pVehicle::_computeLocalVelocity()
{
	_computeMostTouchedActor();
	NxVec3 relativeVelocity;
	if (_mostTouchedActor == NULL || !_mostTouchedActor->isDynamic())
	{
		relativeVelocity = getActor()->getLinearVelocity();
	} else {
		relativeVelocity = getActor()->getLinearVelocity() - _mostTouchedActor->getLinearVelocity();
	}
	NxQuat rotation = getActor()->getGlobalOrientationQuat();
	NxQuat global2Local;
	_localVelocity = relativeVelocity;
	rotation.inverseRotate(_localVelocity);
	char master[512];

	//sprintf(master,"Velocity: %2.3f %2.3f %2.3f\n", _localVelocity.x, _localVelocity.y, _localVelocity.z);
	//OutputDebugString(master);
}

Vector3 StillDesign::PhysX::Math::QuatToEuler( NxQuat q )
{
	q.normalize();
	
	float sqw = q.w * q.w;
	float sqx = q.x * q.x;
	float sqy = q.y * q.y;
	float sqz = q.z * q.z;
	
	float test = q.x*q.y + q.z*q.w;
	
	Vector3 rotation = Vector3( 0, 0, 0 );
	
	// Singularity at north pole
	if( test >= 0.499999999999999999f )
	{
		rotation.Y = 2.0f * atan2( q.x, q.w );
		rotation.Z = (float)System::Math::PI;
		rotation.X = 0.0f;
		
		return rotation;
	}
	if( test <= -0.499999999999999999f )
	{
		rotation.Y = -2.0f * atan2( q.x, q.w );
		rotation.Z = -((float)System::Math::PI / 2.0f);
		rotation.X = 0.0f;
		
		return rotation;
	}
	
	rotation.Y = atan2(2*q.y*q.w-2*q.x*q.z , sqx - sqy - sqz + sqw);
	rotation.Z = asin(2*test);
	rotation.X = atan2(2*q.x*q.w-2*q.y*q.z , -sqx + sqy - sqz + sqw);
	
	return rotation;
}

void Vehicle::loadScene(const std::string &fileName)
{

	//车的父节点
	mBaseCarNode			= mSceneMgr->getRootSceneNode()->createChildSceneNode(fileName + "BaseCarNode");

	//DotSceneLoader* dsl = new DotSceneLoader();//rel
	//dsl->parseDotScene(fileName + ".scene", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, mSceneMgr, mBaseCarNode);

	mVehicleRenderable = new VehicleRenderable(mSceneMgr, mBaseCarNode);
	mVehicleRenderable->load(fileName + ".vrf");

	//车身节点
	mBodyNode				= static_cast<Ogre::SceneNode*>(mBaseCarNode->getChild(fileName + "Body"));

	//轮子节点
	mWheels[TOP_LEFT].mSceneNode	= static_cast<Ogre::SceneNode*>(mBaseCarNode->getChild(fileName + "LTWheel"));
	mWheels[TOP_LEFT].mName			= fileName + "LTWheel";

	mWheels[TOP_RIGHT].mSceneNode	= static_cast<Ogre::SceneNode*>(mBaseCarNode->getChild(fileName + "RTWheel"));
	mWheels[TOP_RIGHT].mName		= fileName + "RTWheel";

	mWheels[Bottom_LEFT].mSceneNode	= static_cast<Ogre::SceneNode*>(mBaseCarNode->getChild(fileName + "LBWheel"));
	mWheels[Bottom_LEFT].mName		= fileName + "LBWheel";

	mWheels[Bottom_RIGHT].mSceneNode  = static_cast<Ogre::SceneNode*> (mBaseCarNode->getChild(fileName + "RBWheel"));
	mWheels[Bottom_RIGHT].mName		  = fileName + "RBWheel";

	//设置整体参数
	NxBodyDesc bodyDes;
	//bodyDes.wakeUpCounter	= 1E8;
	bodyDes.mass			= 12000;//mVehicleInfo.gMass;
	//bodyDes.massLocalPose.t	= NxVec3(mVehicleInfo.gMassLocalPose.x, mVehicleInfo.gMassLocalPose.y, mVehicleInfo.gMassLocalPose.z);

	bodyDes.angularDamping	= 0.03f;
	bodyDes.linearDamping	= 0.08f;

	//写入车身的shapeDesc
	mBoundingBox		= mBodyNode->getAttachedObject(0)->getBoundingBox();
	NxBoxShapeDesc boxDes;
	NxVec3 dimen		(mBoundingBox.getHalfSize().x, mBoundingBox.getHalfSize().y, mBoundingBox.getHalfSize().z);
	NxVec3 localPos		(mBodyNode->getPosition().x, mBodyNode->getPosition().y, mBodyNode->getPosition().z);

	////车身到轮子的距离
	//NxReal d1 = NxMath::abs(mBodyNode->getPosition().y - mWheels[0].mSceneNode->getPosition().y);
	////包围盒半径之和
	//NxReal d2 = mBoundingBox.getHalfSize().y + mWheels[0].mSceneNode->getAttachedObject(0)->getBoundingBox().getHalfSize().y;
	////判断轮子是否与车身的包围盒重叠
	//if(d1 < d2)
	//{
	//	//设置为最大适合高度
	//	dimen.y = (mBoundingBox.getSize().y - (d2 - d1)) / 2.0f;
	//}
	boxDes.dimensions.set(dimen);
	boxDes.localPose.t	= localPos;

	NxActorDesc actorDesc;
	actorDesc.body			= &bodyDes;
	actorDesc.globalPose.t	= NxVec3(mOriginalPos.x, mOriginalPos.y, mOriginalPos.z);
	NxQuat quat;
	quat.setWXYZ(mOriginalQuat.w, mOriginalQuat.x, mOriginalQuat.y, mOriginalQuat.z);
	actorDesc.globalPose.M.fromQuat(quat);
	actorDesc.shapes.pushBack(&boxDes);
	mActor		= mNxScene->createActor(actorDesc);
	mActor->setCMassOffsetLocalPosition(NxVec3(0, -mBoundingBox.getSize().y, 0));
	mBodyShape  = static_cast<NxBoxShape*>(mActor->getShapes()[0]);

	//写入轮子的shapeDesc

	createWheelShapeDesc(&mWheels[TOP_LEFT], true);
	createWheelShapeDesc(&mWheels[TOP_RIGHT], true);
	createWheelShapeDesc(&mWheels[Bottom_LEFT], false);
	createWheelShapeDesc(&mWheels[Bottom_RIGHT], false);

	//mWheels[TOP_LEFT].mWheelDesc.localPose.t = NxVec3(mBoundingBox.getHalfSize().x, 2, 0);
	//mWheels[TOP_RIGHT].mWheelDesc.localPose.t = NxVec3(-mBoundingBox.getHalfSize().x, 2, 0);
	//mWheels[Bottom_LEFT].mWheelDesc.localPose.t = NxVec3(mBoundingBox.getHalfSize().x, -mBoundingBox.getHalfSize().y, 0);
	//mWheels[Bottom_RIGHT].mWheelDesc.localPose.t = NxVec3(-mBoundingBox.getHalfSize().x, -mBoundingBox.getHalfSize().y, 0);

	//创建轮子
	mWheels[TOP_LEFT].mWheel = static_cast<NxWheelShape*>(mActor->createShape(mWheels[TOP_LEFT].mWheelDesc));
	mWheels[TOP_RIGHT].mWheel = static_cast<NxWheelShape*>(mActor->createShape(mWheels[TOP_RIGHT].mWheelDesc));
	mWheels[Bottom_LEFT].mWheel	= static_cast<NxWheelShape*>(mActor->createShape(mWheels[Bottom_LEFT].mWheelDesc));
	mWheels[Bottom_RIGHT].mWheel = static_cast<NxWheelShape*>(mActor->createShape(mWheels[Bottom_RIGHT].mWheelDesc));

	NxMaterial* mat = mNxScene->getMaterialFromIndex(0);
	mat->setFrictionCombineMode(NX_CM_MULTIPLY);
	mat->setStaticFriction(300.2f);
	mat->setDynamicFriction(100.5f);
}