C++ NxMat33类代码示例

void KeyboardUpCallback(unsigned char key, int x, int y)
{
	gKeys[key] = false;

	switch (key)
	{
		case 'p': { bPause = !bPause; 
					if (bPause)
						hud.SetDisplayString(1, "Paused - Hit \"p\" to Unpause", 0.3f, 0.55f);
					else
						hud.SetDisplayString(1, "", 0.0f, 0.0f);	
					getElapsedTime(); 
					break; }
		case 'x': { bShadows = !bShadows; break; }
		case 'b': { bDebugWireframeMode = !bDebugWireframeMode; break; }
		case 'c': { 
					// Reset the box
					NxMat33 m;
					m.id();
					box1->setGlobalOrientation(m);
					box1->setGlobalPosition(NxVec3(5,3.5,0));
					box1->setLinearVelocity(NxVec3(0,0,0));
					box1->setAngularVelocity(NxVec3(0,0,0));

					ChangeKernel();
					break; 
				  }
		case 27 : { exit(0); break; }
		default : { break; }
	}
}

Matrix3x3 CPhysicsActor::GetGlobalOrientation()
{
	Matrix3x3 orientation;
	NxMat33 tmp = m_Actor->getGlobalOrientation();
	tmp.getColumnMajor( orientation.GetMatrix() );
	return orientation;
}

void CCTDebugData::addAABB(const NxBounds3& bounds, NxU32 color, bool renderFrame)
{
	// Reuse OBB code...
	NxVec3 center;	bounds.getCenter(center);
	NxVec3 extents;	bounds.getExtents(extents);
	NxMat33 id;	id.id();
	addOBB(NxBox(center, extents, id), color, renderFrame);
}

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 SweepTest::FindTouchedCCTs(	NxU32 nb_boxes, const NxExtendedBounds3* boxes, const void** box_user_data,
									NxU32 nb_capsules, const NxExtendedCapsule* capsules, const void** capsule_user_data,
									const NxExtendedBounds3& world_box)
{
	NxExtendedVec3 Origin;	// Will be TouchedGeom::mOffset
	world_box.getCenter(Origin);

	// Find touched boxes, i.e. other box controllers
	for(NxU32 i=0;i<nb_boxes;i++)
	{
		if(!world_box.intersect(boxes[i]))
			continue;

		TouchedUserBox* UserBox = (TouchedUserBox*)reserve(mGeomStream, sizeof(TouchedUserBox)/sizeof(NxU32));
		UserBox->mType		= TOUCHED_USER_BOX;
		UserBox->mUserData	= box_user_data[i];
		UserBox->mOffset	= Origin;
		UserBox->mBox		= boxes[i];
	}

	// Find touched capsules, i.e. other capsule controllers
	NxExtendedVec3 Center;
	NxVec3 Extents;
	world_box.getCenter(Center);
	world_box.getExtents(Extents);
	NxMat33 Idt;
	Idt.id();
	for(NxU32 i=0;i<nb_capsules;i++)
	{
		// Do a quick AABB check first, to avoid calling the SDK too much
		const NxF32 r = capsules[i].radius;
		if((capsules[i].p0.x - r > world_box.max.x) || (world_box.min.x > capsules[i].p1.x + r)) continue;
		if((capsules[i].p0.y - r > world_box.max.y) || (world_box.min.y > capsules[i].p1.y + r)) continue;
		if((capsules[i].p0.z - r > world_box.max.z) || (world_box.min.z > capsules[i].p1.z + r)) continue;

		// Do a box-capsule intersect, or skip it? => better to skip it, not really useful now
/*		NxCapsule tmp;
		tmp.radius = capsules[i].radius;
		tmp.p0.x = float(capsules[i].p0.x);
		tmp.p0.y = float(capsules[i].p0.y);
		tmp.p0.z = float(capsules[i].p0.z);
		tmp.p1.x = float(capsules[i].p1.x);
		tmp.p1.y = float(capsules[i].p1.y);
		tmp.p1.z = float(capsules[i].p1.z);
		float d2 = gUtilLib->NxSegmentOBBSqrDist(tmp, NxVec3(float(Center.x), float(Center.y), float(Center.z)), 
											Extents, 
											Idt, NULL, NULL);
		if(d2<capsules[i].radius*capsules[i].radius)*/
		{
			TouchedUserCapsule* UserCapsule = (TouchedUserCapsule*)reserve(mGeomStream, sizeof(TouchedUserCapsule)/sizeof(NxU32));
			UserCapsule->mType		= TOUCHED_USER_CAPSULE;
			UserCapsule->mUserData	= capsule_user_data[i];
			UserCapsule->mOffset	= Origin;
			UserCapsule->mCapsule	= capsules[i];
		}
	}
}

void pWheel1::setAngle(float angle) 
{
	_angle = angle;

	NxReal Cos, Sin;
	NxMath::sinCos(_angle, Sin, Cos);
	NxMat33 wheelOrientation = wheelCapsule->getLocalOrientation();
	wheelOrientation.setColumn(0,  NxVec3( Cos, 0, Sin ));
	wheelOrientation.setColumn(2,  NxVec3( Sin, 0,-Cos ));
	setWheelOrientation(wheelOrientation);

}

Box::operator NxBox( Box box )
{
	NxBox newBox;
		newBox.center = Math::Vector3ToNxVec3( box.Center );
		newBox.extents = Math::Vector3ToNxVec3( box.Extents );
		
		NxQuat q = Math::QuaternionNxQuat( box.Rotation );
		NxMat33 rot;
		rot.fromQuat( q );
		newBox.rot = rot;
		
	return newBox;
}

NxForceFieldLinearKernel* CreateFFLinearKernel()
{
	// Create a Linear kernel
	NxMat33 m;
	m.id();

	NxForceFieldLinearKernelDesc lKernelDesc;
	lKernelDesc.positionMultiplier = m;
	lKernelDesc.positionTarget = NxVec3(0, 3.5, 0);

	gLinearKernel = gScene->createForceFieldLinearKernel(lKernelDesc);
	return gLinearKernel;
}

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 TickCar()
{
	NxReal steeringAngle = gSteeringValue * gMaxSteeringAngle;

	NxArray<CarWheelContact>::iterator i = wheelContactPoints.begin();
	while(i != wheelContactPoints.end())
	{
		CarWheelContact& cwc = *i;

		WheelShapeUserData* wheelData = (WheelShapeUserData *)(cwc.wheel->userData);

		//apply to powered wheels only.
		if (wheelData->frontWheel)
	    {
			//steering:
			NxMat33 wheelOrientation = cwc.wheel->getLocalOrientation();
			wheelOrientation.setColumn(0,  NxVec3(NxMath::cos(steeringAngle), 0,  NxMath::sin(steeringAngle) ));
			wheelOrientation.setColumn(2,  NxVec3(NxMath::sin(steeringAngle), 0, -NxMath::cos(steeringAngle) ));
			cwc.wheel->setLocalOrientation(wheelOrientation);

			if (frontWheelIsPowered)
			{
				//get the world space orientation:
				wheelOrientation = cwc.wheel->getGlobalOrientation();
				NxVec3 steeringDirection;
				wheelOrientation.getColumn(0, steeringDirection);

				//the power direction of the front wheel is the wheel's axis as it is steered.
				if (gMotorForce)
				{
					cwc.car->addForceAtPos(steeringDirection * gMotorForce,cwc.contactPoint);
				}
			}
		}
		if (!wheelData->frontWheel && rearWheelIsPowered)
		{
			//get the orientation of this car:
			NxMat33 m = cwc.car->getGlobalOrientation();
			NxVec3 carForwardAxis;
			m.getColumn(0, carForwardAxis);
			//the power direction of the rear wheel is always the car's length axis.
			cwc.car->addForceAtPos(carForwardAxis * gMotorForce,cwc.contactPoint);
		}
		i++;
	}

	wheelContactPoints.clear();
}

NxPhysicsForceField::NxPhysicsForceField( NxPhysicsActor * Actor )//   NxPhysicsShape * Shape )
{

	//NxShape * mShape = Shape->GetNxShape();

	NxForceFieldLinearKernelDesc lKernelDesc;
	//constant force of 10 forward, which is then animated in the update() method
	lKernelDesc.constant = NxVec3(0, 0, 0);

	//lKernelDesc.constant = NxVec3(20, 0, 0);

	//The forces do not depend on where the objects are positioned
	NxMat33 m; m.zero();

	m(0,0) = -100; //radial force 
	m(0,1) = -100; //radial force 
	m(0,2) = -100; //radial force 

	lKernelDesc.positionMultiplier = m;
	lKernelDesc.noise = NxVec3(0.0,0.5,0.0); //adds a random noise on the forces to make the objects a little more chaotic

	//Set target velocity along the main axis to 20
	lKernelDesc.velocityTarget = NxVec3(10,10,10);  //20

	//Acts with a force relative to the current velocity to reach the target velocities. 0 means that those components won't be affected
	m.diagonal(NxVec3(0,0,0)); 
	lKernelDesc.velocityMultiplier = m;

	NxForceFieldLinearKernel * linearKernel = Actor->GetNxActor()->getScene().createForceFieldLinearKernel(lKernelDesc);

	//Create the forcefield with the Custom kernel
	NxForceFieldDesc fieldDesc1;
	fieldDesc1.kernel = linearKernel;
	fieldDesc1.coordinates = NX_FFC_SPHERICAL;//  NX_FFC_CARTESIAN;
	fieldDesc1.fluidType = NX_FF_TYPE_OTHER;//NX_FF_TYPE_GRAVITATIONAL;
	fieldDesc1.clothType = NX_FF_TYPE_OTHER;//NX_FF_TYPE_GRAVITATIONAL;
	fieldDesc1.softBodyType = NX_FF_TYPE_OTHER;//NX_FF_TYPE_GRAVITATIONAL;
	fieldDesc1.rigidBodyType = NX_FF_TYPE_OTHER;//NX_FF_TYPE_GRAVITATIONAL;

	// The field's pose is relative to the actor's pose and relative to the world frame if field is null.
	fieldDesc1.actor = Actor->GetNxActor();

	fieldDesc1.flags = NX_FFF_VOLUMETRIC_SCALING_FLUID | NX_FFF_VOLUMETRIC_SCALING_CLOTH | NX_FFF_VOLUMETRIC_SCALING_SOFTBODY | NX_FFF_VOLUMETRIC_SCALING_RIGIDBODY;

	mForceField = Actor->GetNxActor()->getScene().createForceField(fieldDesc1);

}

/*===========================================================*/
bool CLaserBase::HitCheck( float *Distance )
{

	// レイを飛ばす場所
	NxVec3 orig = m_pd3d->GetCamPos();

	// レイの飛ばす方向
	NxVec3 dir;

	NxMat33 m;
	CMatrix mRot;
	mRot.SetRotate( m_pd3d->GetCamRot() );
	
	NxF64 Tmp[3][3];
	CDPConverter::SetMatrix( Tmp , &mRot );
	m.setColumnMajor( Tmp );
    m.getColumn( 2 , dir );

	NxRay ray( orig , dir );
	NxRaycastHit hit;
	NxReal dist;
	
	// レイ発射
	NxShape* closestShape = m_pPhysX->GetScene()->raycastClosestShape( ray , NX_ALL_SHAPES , hit , ( 1<<CPhysX::CG_DEFAULT ) | ( 1<<CPhysX::CG_HEAVY_OBJECT ) | ( 1<<CPhysX::CG_STATIC ) );
	
	if( closestShape )
	{
		m_pHitActor = &hit.shape->getActor();
		NxVec3& worldImpact = hit.worldImpact;
		dist = hit.distance;
		*Distance = hit.distance;

		CRaycastLine rl( ray.orig , worldImpact , 0xffffffff );
		RayPos = rl;

		return true;
	} 
	else 
	{
		dist = 10000;
		*Distance = 10000;
		CRaycastLine rl( ray.orig , dist*dir , 0xffffffff );
		RayPos = rl;
		return false;
	}

}

	void Update ( void )
	{
		NxVec3 forceDir ( -1, 0, 0 );
		NxReal forceStrength = 100;
		NxVec3 forceVec = forceStrength*forceDir;
		this->tractor->addForce(forceVec);

		g_iValue = 10;

		for (int i=0; i<2; i++)
		{
			NxMat34 pose   = frontWheels [ i ].wheel->getGlobalPose ( );
			NxMat33 orient = pose.M;
			NxVec3  pos    = pose.t;

			float glmat[16];
			orient.getColumnMajorStride4(&(glmat[0]));
			pos.get(&(glmat[12]));
			glmat[3] = glmat[7] = glmat[11] = 0.0f;
			glmat[15] = 1.0f;

			SetWorldMatrix ( g_iValue, ( D3DXMATRIX* ) &glmat );
			sObject* pObject = dbGetObject ( g_iValue++ );
			pObject->position.vecPosition = D3DXVECTOR3 ( glmat [ 12 ], glmat [ 13 ], glmat [ 14 ] );
		}

		for (int i=0; i<2; i++)
		{
			NxMat34 pose   = steerWheels [ i ].wheel.wheel->getGlobalPose ( );
			NxMat33 orient = pose.M;
			NxVec3  pos    = pose.t;

			float glmat[16];
			orient.getColumnMajorStride4(&(glmat[0]));
			pos.get(&(glmat[12]));
			glmat[3] = glmat[7] = glmat[11] = 0.0f;
			glmat[15] = 1.0f;

			SetWorldMatrix ( g_iValue, ( D3DXMATRIX* ) &glmat );
			sObject* pObject = dbGetObject ( g_iValue++ );
			pObject->position.vecPosition = D3DXVECTOR3 ( glmat [ 12 ], glmat [ 13 ], glmat [ 14 ] );
		}

		//dbPositionObject ( g_iValue, glmat [ 12 ], glmat [ 13 ], glmat [ 14 ] );

	
	}

void PhysUpdate ( void )
{
	int iObject = 1;

    // Render all the actors in the scene
    int nbActors = gScene->getNbActors();
    NxActor** actors = gScene->getActors();

	while (nbActors--)
    {
        NxActor* actor = *actors++;

		// Get an OpenGL transform (float[16]) from a Novodex shape’s global pose     
		// (NxMat34)
		NxShape* shape = NULL;

		NxMat34 pose = actor->getGlobalPose();

		NxMat33 orient = pose.M;
		NxVec3 pos = pose.t;	

		float glmat[16];    // 4x4 column major OpenGL matrix
		orient.getColumnMajorStride4(&(glmat[0]));
		pos.get(&(glmat[12]));

		// clear the elements we don't need:
		glmat[3] = glmat[7] = glmat[11] = 0.0f;
		glmat[15] = 1.0f;

		{
			sPhysObject* pPhys = ( sPhysObject* ) actor->userData;
			
			if ( pPhys )
			{
				SetWorldMatrix ( pPhys->iID, ( D3DXMATRIX* ) &glmat );

				sObject* pObject = dbGetObject ( pPhys->iID );

				pObject->position.vecPosition = D3DXVECTOR3 ( glmat [ 12 ], glmat [ 13 ], glmat [ 14 ] );
			}
		}
    }
}