C++ NxMat33::rotX方法代码示例

NxVec3 ApplyVelocityToActor(NxActor* actor, const NxVec3& velDir, const NxReal velStrength, bool velMode)
{
	NxVec3 velVec = velStrength*velDir;

	if (velMode) 
	{
		actor->moveGlobalPosition(actor->getGlobalPosition() + 0.0001*velStrength*velDir);
	} 
	else 
	{
		NxMat33 orient = actor->getGlobalOrientation();
        NxMat33 m;

		m.id();

		if (velDir == NxVec3(1,0,0))
           m.rotX(0.01);
		else if (velDir == NxVec3(-1,0,0))
           m.rotX(-0.01);
		else if (velDir == NxVec3(0,1,0))
           m.rotY(0.01);
		else if (velDir == NxVec3(0,-1,0))
           m.rotY(-0.01);
		else if (velDir == NxVec3(0,0,1))
           m.rotZ(0.01);
		else if (velDir == NxVec3(0,0,-1))
           m.rotZ(-0.01);

		orient = m * orient;

		actor->moveGlobalOrientation(orient);
	}

	return velVec;
}

void World::SetupAnimalScene()
{
	// Create the objects in the scene
	NxSoftBodyDesc softBodyDesc;
	softBodyDesc.globalPose.t		= NxVec3(0.0f, 3.0f, 0.0f);
	softBodyDesc.particleRadius		= 0.2f;
	softBodyDesc.volumeStiffness	= 0.5f;
	softBodyDesc.stretchingStiffness= 1.0f;
	softBodyDesc.friction			= 1.0f;
	softBodyDesc.attachmentResponseCoefficient = 0.1f;
	softBodyDesc.solverIterations	= 5;

	char *fileName = "froggyNormalized";

	char tetFileName[256], objFileName[256], s[256];
	sprintf(tetFileName, "%s.tet", fileName);
	sprintf(objFileName, "%s.obj", fileName);

	ObjMesh *objMesh = new ObjMesh();
	objMesh->loadFromObjFile(FindMediaFile(objFileName, s));
	gObjMeshes.pushBack(objMesh);

	if(objMesh == NULL)
	{
		printf("objMesh %s\n");
	}

	NxMat33 rot;
	rot.rotX(NxHalfPiF32);

	for (int i = 0; i < 5; i++)
	{
		softBodyDesc.globalPose.t	= NxVec3(0, 3+i*3, 0);
		MySoftBody *softBody		= new MySoftBody(gScene, softBodyDesc, FindMediaFile(tetFileName, s), objMesh);
		assert(softBody);
		if (!softBody->getNxSoftBody())
		{
			printf("Error: unable to create the softbody for the current scene.\n");
			delete softBody;
		}
		else
		{
			gSoftBodies.pushBack(softBody);
			NxActor *caps = MyCreateCapsule(NxVec3(0.0f, 3.0f + i*3.0f, -0.3f), 1.0f, 0.73f, 1.0f);
			caps->userData = (void*)&gInvisible;
			caps->setGlobalOrientation(rot);
			softBody->getNxSoftBody()->attachToShape(caps->getShapes()[0], NX_SOFTBODY_ATTACHMENT_TWOWAY);
		}
	}
}

void World::SetupSoftWheelCarScene()
{
	NxSoftBodyDesc softBodyDesc;
	softBodyDesc.particleRadius = 0.2f;
	softBodyDesc.volumeStiffness = 1.0f;
	softBodyDesc.stretchingStiffness = 1.0f;
	softBodyDesc.friction = 1.0f;
	softBodyDesc.attachmentResponseCoefficient = 0.8f;
	softBodyDesc.tearFactor = 10.1;						// should be more than 1.0, the less, the easier to tear with flag NX_SBF_TEARABLE

	softBodyDesc.flags |= NX_SBF_HARDWARE | NX_SBF_VOLUME_CONSERVATION | NX_SBF_TEARABLE;

	char *fileName = "wheel";
	char tetFileName[256], objFileName[256], s[256];
	sprintf(tetFileName, "%s.tet", fileName);
	sprintf(objFileName, "%s.obj", fileName);

	MySoftBody *softBody;

	NxReal carHeight = 7.5f;
	NxReal stiffness = 0.9f;
	NxMat34 capsulePose = NxMat34(NxMat33(NX_IDENTITY_MATRIX), NxVec3(-4, carHeight, -5.0f));
	printf("capsulePose %f %f %f\n", capsulePose.t.x, capsulePose.t.y, capsulePose.t.z);
	capsulePose.M.rotX(NxHalfPiF32);
	NxActor *caps1 = CreateOrientedCapsule(capsulePose, 7.1f, 1.3f, 1.0f);
	capsulePose.t = NxVec3(4, carHeight, -5.0f);
	NxActor *caps2 = CreateOrientedCapsule(capsulePose, 7.1f, 1.3f, 1.0f);
	
	ObjMesh *objMesh = new ObjMesh();
	objMesh->loadFromObjFile(FindMediaFile(objFileName, s));
	
	NxMat33 rot;
	rot.rotX(NxPiF32);
	softBodyDesc.globalPose.t = NxVec3(4.0f, carHeight, 3.4f);
	softBodyDesc.globalPose.M = rot;
	softBodyDesc.stretchingStiffness = stiffness;
	softBody = new MySoftBody(gScene, softBodyDesc, FindMediaFile(tetFileName,s), objMesh);
	if (!softBody->getNxSoftBody())
	{
		printf("Error: Unable to create Softbody for the current scene.\n");
		delete softBody;
	}
	else
	{
		gObjMeshes.push_back(objMesh);

		// wheel 1 
		softBody->getNxSoftBody()->attachToCollidingShapes(NX_SOFTBODY_ATTACHMENT_TWOWAY);
		gSoftBodies.push_back(softBody);

		softBodyDesc.globalPose.t = NxVec3(-4.0f ,carHeight, 3.4f);
		softBodyDesc.globalPose.M = rot;
		softBodyDesc.stretchingStiffness = stiffness;
		softBody = new MySoftBody(gScene, softBodyDesc, FindMediaFile(tetFileName,s), objMesh);
		softBody->getNxSoftBody()->attachToCollidingShapes(NX_SOFTBODY_ATTACHMENT_TWOWAY);
		gSoftBodies.push_back(softBody);

		softBodyDesc.globalPose.t = NxVec3(4.0f, carHeight, -3.4f);
		softBodyDesc.globalPose.M.id();
		softBodyDesc.stretchingStiffness = stiffness;
		softBody = new MySoftBody(gScene, softBodyDesc, FindMediaFile(tetFileName,s), objMesh);
		softBody->getNxSoftBody()->attachToCollidingShapes(NX_SOFTBODY_ATTACHMENT_TWOWAY);
		gSoftBodies.push_back(softBody);

		softBodyDesc.globalPose.t = NxVec3(-4.0f, carHeight, -3.4f);
		softBodyDesc.globalPose.M.id();
		softBodyDesc.stretchingStiffness = stiffness;
		softBody = new MySoftBody(gScene, softBodyDesc, FindMediaFile(tetFileName,s), objMesh);
		softBody->getNxSoftBody()->attachToCollidingShapes(NX_SOFTBODY_ATTACHMENT_TWOWAY);		// bind soft wheel with capsule colliding with it
		gSoftBodies.push_back(softBody);

		NxActor *box = CreateBox(NxVec3(0,carHeight,0), NxVec3(4.6f, 0.5f, 1.0f), 0,1.0f);
		CreateRevoluteJoint(box, caps1, NxVec3(-4,carHeight,-3.5f), NxVec3(0,0,1), false);
		CreateRevoluteJoint(box, caps2, NxVec3( 4,carHeight,-3.5f), NxVec3(0,0,1), false);

	}
	gSelectedActor = caps1;

}

pRigidBody*pFactory::createBody(CK3dEntity *referenceObject,CK3dEntity *worldReferenceObject,NXU::NxActorDesc *desc,int flags)
{

#ifdef _DEBUG
	assert(referenceObject);
	assert(desc);
#endif 
	
	/************************************************************************/
	/*                                                                      
	



	*/
	/************************************************************************/
	VxVector vpos;
	referenceObject->GetPosition(&vpos);

	VxQuaternion vquat;
	referenceObject->GetQuaternion(&vquat);

	NxQuat nrot  = desc->globalPose.M;
	NxVec3 npos = desc->globalPose.t;

	NxMat33 rotX;
	rotX.rotX( PI / 2.0f );
	NxMat33 rotZ;
	rotZ.rotZ( PI );

	NxMat34 rotMat;
	rotMat.M.multiply( rotZ, rotX );

	NxMat34 posMat( true );

/*	posMat.t.set(  -mTerrain->getPosition().x,
		mTerrain->getPosition().y,
		mTerrain->getPosition().z );
*/
	desc->globalPose.multiply( posMat, rotMat );


	NxQuat nrot2  = desc->globalPose.M;
	NxVec3 npos2 = desc->globalPose.t;

	VxQuaternion nvm  = getFromStream(nrot);

	VxVector nvpos = getFromStream(npos);

	

	for (NxU32 k=0; k<desc->mShapes.size(); k++)
	{
		NXU::NxShapeDesc *shape = desc->mShapes[k];
		NxVec3 locPos = shape->localPose.t;
		NxQuat localQuad = shape->localPose.M;
	}


	int op  = 2; 



	return NULL;
}

void UpdateWheelShapeUserData()
{
    // Look for wheel shapes
    NxU32 nbActors = gScene->getNbActors();
    NxActor** actors = gScene->getActors();
    while (nbActors--)
    {
		NxU32 nbShapes = actors[nbActors]->getNbShapes();
	    NxShape*const* shapes = actors[nbActors]->getShapes();
		while (nbShapes--)
		{
			NxShape* shape = shapes[nbShapes];
			if (shape->getType() == NX_SHAPE_WHEEL)
			{
				NxWheelShape* ws = (NxWheelShape*)shape;
				ShapeUserData* sud = (ShapeUserData*)(shape->userData);
				if (sud)
				{
					// Need to save away roll angle in wheel shape user data
					NxReal rollAngle = sud->wheelShapeRollAngle;
//					rollAngle += ws->getAxleSpeed() * 1.0f/60.0f;
					rollAngle += ws->getAxleSpeed() * gDeltaTime;
					while (rollAngle > NxTwoPi)	//normally just 1x
						rollAngle -= NxTwoPi;
					while (rollAngle < -NxTwoPi)	//normally just 1x
						rollAngle += NxTwoPi;

					// We have the roll angle for the wheel now
					sud->wheelShapeRollAngle = rollAngle;


					NxMat34 pose;
					pose = ws->getGlobalPose();

					NxWheelContactData wcd;
					NxShape* s = ws->getContact(wcd);	

					NxReal r = ws->getRadius();
					NxReal st = ws->getSuspensionTravel();
					NxReal steerAngle = ws->getSteerAngle();

//					NxWheelShapeDesc state;	
//					ws->saveToDesc(state);

					NxVec3 p0;
					NxVec3 dir;
					/*
					getWorldSegmentFast(seg);
					seg.computeDirection(dir);
					dir.normalize();
					*/
					p0 = pose.t;  //cast from shape origin
					pose.M.getColumn(1, dir);
					dir = -dir;	//cast along -Y.
					NxReal castLength = r + st;	//cast ray this long

//					renderer.addArrow(p0, dir, castLength, 1.0f);
	
					//have ground contact?
					// This code is from WheelShape.cpp in SDKs/core/common/src
					// if (contactPosition != NX_MAX_REAL)  
					if (s && wcd.contactForce > -1000)
					{
//						pose.t = p0 + dir * wcd.contactPoint;
//						pose.t -= dir * state.radius;	//go from contact pos to center pos.
						pose.t = wcd.contactPoint;
						pose.t -= dir * r;	//go from contact pos to center pos.

						NxMat33 rot, axisRot;
						rot.rotY(steerAngle);
						axisRot.rotY(0);

//						NxReal rollAngle = ((ShapeUserData*)(wheel->userData))->rollAngle;

						NxMat33 rollRot;
						rollRot.rotX(rollAngle);

						pose.M = rot * pose.M * axisRot * rollRot;

						sud->wheelShapePose = pose;

					}
					else
					{
						pose.t = p0 + dir * st;
						sud->wheelShapePose = pose;
					}
				}
			}
		}
	}
}