C++ NxVec3类代码示例

void DrawForce(NxActor* actor, NxVec3& forceVec, const NxVec3& color)
{
	// Draw only if the force is large enough
	NxReal force = forceVec.magnitude();
	if (force < 0.1)  return;

	forceVec = 3*forceVec/force;

	NxVec3 pos = actor->getCMassGlobalPosition();
	DrawArrow(pos, pos + forceVec, color);
}

// ----------------------------------------------------------------------
void ObjMesh::updateNormals()
{
	mNormals.resize(mVertices.size());
	int i;
	for (i = 0; i < (int)mNormals.size(); i++) 
		mNormals[i].zero();
	NxVec3 n;
	for (i = 0; i < (int)mTriangles.size(); i++) {
		ObjMeshTriangle &mt = mTriangles[i];
		mt.normalNr[0] = mt.vertexNr[0];
		mt.normalNr[1] = mt.vertexNr[1];
		mt.normalNr[2] = mt.vertexNr[2];
		n.cross(mVertices[mt.vertexNr[1]] - mVertices[mt.vertexNr[0]], 
			mVertices[mt.vertexNr[2]] - mVertices[mt.vertexNr[0]]);
		mNormals[mt.normalNr[0]] += n;
		mNormals[mt.normalNr[1]] += n;
		mNormals[mt.normalNr[2]] += n;
	}
	for (i = 0; i < (int)mNormals.size(); i++) 
		mNormals[i].normalize();
}

void SetupGLMatrix(const NxVec3& pos, const NxMat33& orient)
{
	float glmat[16];	//4x4 column major matrix for OpenGL.
	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;

	glMultMatrixf(&(glmat[0]));
}

    void DynamicImage::update()
    {
        


        if (!sleep)
        {
            NxVec3 towardsVector = tarPos - curPos;

            switch (mode)
            {
            case 0:     //Linear
                if (towardsVector.magnitude() < LINEAR_VELOCITY)
                {
                    curPos = tarPos;
                    sleep = true;
                }
                else
                {
                    towardsVector.normalize();
                    curPos = curPos + (towardsVector * LINEAR_VELOCITY);
                }
                break;


            case 1:     //Halfling
                if (towardsVector.magnitude() < THRESHOLD)
                {
                    curPos = tarPos;
                    sleep = true;
                }
                else
                {
                    curPos = curPos + (towardsVector * 0.033f);
                }
                break;

            }
        }
    }

void Jumper::CanopyOpening::updatePhysics(void)
{
    // velocity of base jumper's body
    NxVec3 velocity = _phActor->getLinearVelocity();

    // local coordinate system of base jumper
    NxMat34 pose = _phActor->getGlobalPose();
    NxVec3 x = pose.M.getColumn(0);
    NxVec3 y = pose.M.getColumn(1);
    NxVec3 z = pose.M.getColumn(2);

    // air resistance force
    float AR = _jumper->getVirtues()->getTrackingAirResistance();

    // terminal velocity
    float Vt = sqrt( 9.8f * _phActor->getMass() / AR );
    float It = velocity.magnitude() / Vt;

    // air resistance force
    NxVec3 Far = NxVec3(0,1,0) * getAirResistancePower( velocity.magnitude() / Vt ) * _phActor->getMass() * 9.8f;

    // finalize motion equation    
    _phActor->addForce( Far );

    // linear damping is function of jumper velocity    
    // this is prevents calculation errors due to high speed rates
    float minVel     = 50.0f;
    float minDamping = _initialLD;
    float maxVel     = 70.0f;
    float maxDamping = 2.5f;
    float factor = ( velocity.magnitude() - minVel ) / ( maxVel - minVel );
    factor = factor < 0 ? 0 : ( factor > 1 ? 1 : factor );
    float damping = minDamping * ( factor - 1 ) + maxDamping * ( factor );
    _phActor->setLinearDamping( damping );

    // shallow brake setting
    _canopy->setLeftDeep( 0.0f );
    _canopy->setRightDeep( 0.0f );

}

void Jumper::RunningJump::update(float dt)
{
    updateAnimation( dt );
    _clump->getFrame()->getLTM();

    if( _actionTime < _blendTime ) 
    {
        Vector3f dir = _clump->getFrame()->getAt();
        dir.normalize();
        _clump->getFrame()->setPos( _clump->getFrame()->getPos() + dir * dt * _vel );            
        return;
    }

    if( _phActor->isSleeping() )
    {
        // setup physics
        Matrix4f sampleLTM = Jumper::getCollisionFF( _clump )->getFrame()->getLTM();
        _phActor->setGlobalPose( wrap( sampleLTM ) );
        _phActor->wakeUp();
        NxVec3 velH = wrap( _clump->getFrame()->getAt() );
        velH.normalize();
        velH *= _vel * 0.01f;
        NxVec3 velV = wrap( _clump->getFrame()->getUp() );
        velV.normalize();
        velV *= 1.5f;
        _phActor->setLinearVelocity( velH + velV + wrap(_clump->getFrame()->getAt() * 600.0f * dt)) ;
        _jumper->initOverburdenCalculator( velH + velV );


    }
    else
    {
        _clump->getFrame()->setMatrix( _matrixConversion->convert( wrap( _phActor->getGlobalPose() ) ) );
    }

    if( _clump->getAnimationController()->isEndOfAnimation( 0 ) )
    {
        _endOfAction = true;
    }
}

void PxSingleActor::applyCorrection( const MatrixF& mat, const NxVec3& linVel, const NxVec3& angVel )
{
   // Sometimes the actor hasn't been
   // created yet during the call from unpackUpdate.
   if ( !mActor || !mWorld )
      return;

   mWorld->releaseWriteLock();

   NxVec3 newPos = mat.getPosition();
   NxVec3 currPos = getPosition();

   NxVec3 offset = newPos - currPos;

   // If the difference isn't large enough,
   // just set the new transform, no correction.
   if ( offset.magnitude() > 0.3f )
   {
      // If we're going to set the linear or angular velocity,
      // we do it before we add a corrective force, since it would be overwritten otherwise.
      NxVec3 currLinVel, currAngVel;
      currLinVel = mActor->getLinearVelocity();
      currAngVel = mActor->getAngularVelocity();

      // Scale the corrective force by half,
      // otherwise it will over correct and oscillate.
      NxVec3 massCent = mActor->getCMassGlobalPosition();
      mActor->addForceAtPos( offset, massCent, NX_VELOCITY_CHANGE );

      // If the linear velocity is divergent enough, change to server linear velocity.
      if ( (linVel - currLinVel).magnitude() > 0.3f )
         mActor->setLinearVelocity( linVel );
      // Same for angular.
      if ( (angVel - currAngVel).magnitude() > 0.3f )
         mActor->setAngularVelocity( angVel );   
   }

   Parent::setTransform( mat );
}

// ray-sphere intersection test from Graphics Gems p.388
// **NOTE** There is a bug in this Graphics Gem.  If the origin
// of the ray is *inside* the sphere being tested, it reports the
// wrong intersection location.  This code has a fix for the bug.
bool  SBM_rayIntersectsSphere(const float *point,const float *direction,const float *_center,float radius,float &t)
{
  bool ret = false;

  NxVec3 rayOrigin(point);
  NxVec3 dir(direction);
  NxVec3 center(_center);

	// notation:
	// point E  = rayOrigin
	// point O  = sphere center

	NxVec3 EO = center - rayOrigin;
  NxVec3 V = dir;

  float dist2 = EO.x*EO.x + EO.y*EO.y + EO.z * EO.z;

  float r2 = radius*radius;

  // Bug Fix For Gem, if origin is *inside* the sphere, invert the
  // direction vector so that we get a valid intersection location.
  if ( dist2 < r2 )
    V*=-1;

	float v = EO.dot(V);

	float disc = r2 - (EO.magnitudeSquared() - v*v);

	if (disc > 0.0f)
	{
    t = sqrtf(disc);
		ret = true;
	}

	return ret;
}

void DrawForceAtShape(NxActor* actor, NxShape* shape, NxVec3& forceVec, const NxVec3& color)
{
	// Draw only if the force is large enough
	NxReal force = forceVec.magnitude();
	if (force < 0.1f)  return;

	forceVec = 3*forceVec/force;

	NxVec3 pos;	
	if (bShapeSelectMode && bForceMode)
		pos = shape->getGlobalPosition();
	else
		pos = actor->getCMassGlobalPosition();
	DrawArrow(pos, pos + forceVec, color);
}

void CreateExperiment()
{
	switch (gExperimentType)
	{
	case LANDER:
		  {
			  gScene->releaseActor(*groundPlane); groundPlane=NULL; // will be recreated with the container cell
			  CreateExperimentMaterials();
			  CreateContainerCell(gCellSize);
			  gPhysicsSDK->setParameter(NX_VISUALIZE_ACTOR_AXES,0);
			  gPhysicsSDK->setParameter(NX_VISUALIZE_COLLISION_FNORMALS,0);
			  gPhysicsSDK->setParameter(NX_VISUALIZE_COLLISION_SHAPES,1);
			  gPhysicsSDK->setParameter(NX_VISUALIZE_WORLD_AXES,0);
			  //gLander=CreateLander(NxVec3(0.0f,0.0f,0.0f));
			  gLander=CreateLander(NxVec3(0.0f,gCellSize/2.0f,0.0f));
			  IdentifyGravitators();
			  if (gLander) {
			  ReOrientActor(gLander);
			  SpinActor(gLander,gLandingRoughness*sqrt(gCellSize*gDefaultGravity.magnitude()/(gLander->getMassSpaceInertiaTensor().magnitude())));
			  LaunchActor(gLander,gLandingRoughness*sqrt(gCellSize*gDefaultGravity.magnitude()));
			  gSelectedActor=gLander;
			  gPhysicsSDK->setParameter(NX_VISUALIZE_ACTOR_AXES,gLanderSize.magnitude());
			  isRunning=true;
			  }
			  else {
				  printf("Error: Unable to create lander\a\n");
				  isRunning=false;
			  }
			  break;
		  }
	case LAY_SUBSTRATE:
		{
			gScene->releaseActor(*groundPlane); groundPlane=NULL; // will be recreated with the container cell
			CreateExperimentMaterials();
			CreateContainerCell(gCellSize);
			gPhysicsSDK->setParameter(NX_VISUALIZE_ACTOR_AXES,0);
			gPhysicsSDK->setParameter(NX_VISUALIZE_COLLISION_FNORMALS,0);
			gPhysicsSDK->setParameter(NX_VISUALIZE_COLLISION_SHAPES,1);
			gPhysicsSDK->setParameter(NX_VISUALIZE_WORLD_AXES,0);
			//ThrowStone();
			isRunning=true;
			break;
		}
	default:				
		{printf("Error: unimplemented experiment type\a\n"); isRunning=false; break;}
	}
}

// -----------------------------------------------------------------------------------
bool ObjMesh::rayTriangleIntersection(const NxVec3 &orig, const NxVec3 &dir,
	 									 const ObjMeshTriangle &triangle,
	 									 NxReal &t, NxReal &u, NxReal &v) const
{
	const NxVec3 &a = mVertices[triangle.vertexNr[0]];
	const NxVec3 &b = mVertices[triangle.vertexNr[0]];
	const NxVec3 &c = mVertices[triangle.vertexNr[0]];

	NxVec3 edge1, edge2, tvec, pvec, qvec;
	NxReal det,inv_det;

	edge1 = b - a;
	edge2 = c - a;
	pvec.cross(dir, edge2);

	/* if determinant is near zero, ray lies in plane of triangle */
	det = edge1.dot(pvec);

	if (det == 0.0f)
		return false;
	inv_det = 1.0f / det;

	/* calculate distance from vert0 to ray origin */
	tvec = orig - a;

	/* calculate U parameter and test bounds */
	u = tvec.dot(pvec) * inv_det;
	if (u < 0.0f || u > 1.0f)
	return false;

	/* prepare to test V parameter */
	qvec.cross(tvec, edge1);

	/* calculate V parameter and test bounds */
	v = dir.dot(qvec) * inv_det;
	if (v < 0.0f || u + v > 1.0f)
	return false;

	/* calculate t, ray intersects triangle */
	t = edge2.dot(qvec) * inv_det;

	return true;
}

void ViewUnProject(int xi, int yi, float depth, NxVec3 &v)
{
//We cannot do picking easily on the xbox/PS3 anyway
#if defined(_XBOX)||defined(__CELLOS_LV2__)
	v=NxVec3(0,0,0);
#else
	GLint viewPort[4];
	GLdouble modelMatrix[16];
	GLdouble projMatrix[16];
	glGetIntegerv(GL_VIEWPORT, viewPort);
	glGetDoublev(GL_MODELVIEW_MATRIX, modelMatrix);
	glGetDoublev(GL_PROJECTION_MATRIX, projMatrix);
	yi = viewPort[3] - yi - 1;
	GLdouble wx, wy, wz;
	gluUnProject((GLdouble) xi, (GLdouble) yi, (GLdouble) depth,
	modelMatrix, projMatrix, viewPort, &wx, &wy, &wz);
	v.set((NxReal)wx, (NxReal)wy, (NxReal)wz);
#endif
}

void WindPointer::onEvent(Actor* initiator, unsigned int eventId, void* eventData)
{
    if( eventId == EVENT_CAMERA_IS_ACTUAL )
    {
        // current wind
        NxVec3 wind = _scene->getWindAtPoint( NxVec3( 0,0,0 ) );

        // make signature worldspace position
        Matrix4f cameraPose = _scene->getCamera()->getPose();
        Vector3f cameraPos( cameraPose[3][0], cameraPose[3][1], cameraPose[3][2] );
        Vector3f pos( wind.x, wind.y, wind.z );
        pos.normalize();
        pos *= 10000.0f;
        pos += cameraPos;

        // update signature window
        Vector3f screenPos = Gameplay::iEngine->getDefaultCamera()->projectPosition( pos );
        if( screenPos[2] > 1 )
        {
            _signature->getPanel()->setVisible( false );
            return;
        }
        else
        {
            _signature->getPanel()->setVisible( true );
            gui::Rect oldRect = _signature->getPanel()->getRect();
            gui::Rect newRect(
                int( screenPos[0] ), int( screenPos[1] ),
                int( screenPos[0] ) + oldRect.getWidth(),
                int( screenPos[1] ) + oldRect.getHeight()
            );
            _signature->getPanel()->setRect( newRect );
            _windSpeed->getStaticText()->setText( wstrformat( Gameplay::iLanguage->getUnicodeString(295), wind.magnitude() ).c_str() );
        }        
    }
}

void GetPos(NxVec3 &maxPos, NxVec3 &minPos, NxCloth *cloth)
{
	NxVec3 vetex[2000];
	ofstream outFile("vetex");
	NxReal maxX = -999.0f, minX = 999.0f;
	NxReal maxY = -999.0f, minY = 999.0f;
	NxReal maxZ = -999.0f, minZ = 999.0f;

	memset(vetex, 0, 2000 * sizeof(NxVec3));
	cloth->getPositions(vetex);
	for (int i = 0; i < 2000; i++)
	{
		if (vetex[i].x == 0 && vetex[i].y == 0 || vetex[i].z == 0)              
		{
			//printf("i = %d\n", i); 
			break;
		}
		else
		{
			if (vetex[i].x > maxX)	maxX = vetex[i].x;
			if (vetex[i].y > maxY)	maxY = vetex[i].y;
			if (vetex[i].z > maxZ)	maxZ = vetex[i].z;

			if (vetex[i].x < minX)	minX = vetex[i].x;
			if (vetex[i].y < minY)	minY = vetex[i].y;
			if (vetex[i].z < minZ)	minZ = vetex[i].z;
			//printf("vetex %f %f %f\n", vetex[i].x, vetex[i].y, vetex[i].z);
			//outFile << vetex[i].x << "\t" << vetex[i].y << "\t" << vetex[i].z << endl;
		}
	}
	minPos.setx(minX);
	minPos.sety(minY);
	minPos.setz(minZ);

	maxPos.setx(maxX);
	maxPos.sety(maxY);
	maxPos.setz(maxZ);

	outFile << maxX << endl;
	outFile.close();
}

NxFluid* CreateFluid(const NxVec3& pos, NxU32 sideNum, NxReal distance, NxScene* scene)
{
	float rad = sideNum*distance*0.5;
	for (unsigned i=0; i<sideNum; i++)
		for (unsigned j=0; j<sideNum; j++)
			for (unsigned k=0; k<sideNum; k++)
			{
				NxVec3 p = NxVec3(i*distance,j*distance,k*distance);
				if (p.distance(NxVec3(rad,rad,rad)) < rad)
				{
					p += pos;
					if(gParticleBufferNum< MAX_PARTICLES)
						gParticleBuffer[gParticleBufferNum++] = p;
				}
			}

	NxParticleData particles;

	//particles.maxParticles			= gParticleBufferCap;
	particles.numParticlesPtr		= &gParticleBufferNum;
	particles.bufferPos				= &gParticleBuffer[0].x;
	particles.bufferPosByteStride	= sizeof(NxVec3);

	//Create a fluid descriptor
	NxFluidDesc fluidDesc;
	fluidDesc.maxParticles = MAX_PARTICLES;
	fluidDesc.flags |= NX_FF_COLLISION_TWOWAY;
	fluidDesc.simulationMethod	= NX_F_SPH;
	fluidDesc.restParticlesPerMeter			= 5.0f;
	fluidDesc.motionLimitMultiplier			= 10.0f;
	fluidDesc.restDensity					= 1000.0f;
	fluidDesc.kernelRadiusMultiplier		= 1.6f;
	fluidDesc.stiffness						= 10.0f;
	fluidDesc.viscosity						= 50.0f;
	fluidDesc.damping						= 0.5f;
	fluidDesc.packetSizeMultiplier			= 16;
	fluidDesc.name                          = "fluid";

	fluidDesc.kernelRadiusMultiplier		= KERNEL_RADIUS_MULTIPLIER;
	fluidDesc.restParticlesPerMeter			= REST_PARTICLES_PER_METER;
	fluidDesc.motionLimitMultiplier			= MOTION_LIMIT_MULTIPLIER;
	fluidDesc.packetSizeMultiplier			= PACKET_SIZE_MULTIPLIER;
	fluidDesc.stiffness						= 40;  // 50
	fluidDesc.viscosity						= 22;
	fluidDesc.restDensity					= 1000;
	fluidDesc.damping						= 0;
// There are some API changes since 280 version, Fluid collision coefficients have been renamed, 
// E.g. NxFluidDesc::dynamicCollisionAdhesion is named NxFluidDesc::dynamicFrictionForDynamicShapes. 
#if NX_SDK_VERSION_NUMBER < 280
	fluidDesc.staticCollisionRestitution = 0.162f;
	fluidDesc.staticCollisionAdhesion = 0.146f;
	fluidDesc.dynamicCollisionRestitution = 0.5f;
	fluidDesc.dynamicCollisionAdhesion = 0.5f;
#else 
	fluidDesc.restitutionForStaticShapes = 0.162f;
	fluidDesc.dynamicFrictionForStaticShapes  = 0.146f;
	fluidDesc.restitutionForDynamicShapes = 0.5f;
	fluidDesc.dynamicFrictionForDynamicShapes = 0.5f;
#endif
	fluidDesc.simulationMethod				= NX_F_SPH;  //NX_F_NO_PARTICLE_INTERACTION;

	fluidDesc.initialParticleData			= particles;
	fluidDesc.particlesWriteData			= particles;

	if(!bHardwareFluid)
		fluidDesc.flags &= ~NX_FF_HARDWARE;

    fluid = gScene->createFluid(fluidDesc);
	if(!fluid)
	{
		fluidDesc.flags &= ~NX_FF_HARDWARE;
		bHardwareFluid = false;
		fluid = gScene->createFluid(fluidDesc);
	}
	
	assert(fluid != NULL);
	return fluid;
}