C++ MatrixF::mulV方法代码示例

void GFXDrawUtil::_drawSolidCapsule( const GFXStateBlockDesc &desc, const Point3F &center, F32 radius, F32 height, const ColorI &color, const MatrixF *xfm )
{	
   MatrixF mat;
   if ( xfm )
      mat = *xfm;      
   else
      mat = MatrixF::Identity;

   S32 numPoints = sizeof(circlePoints)/sizeof(Point2F);
   GFXVertexBufferHandle<GFXVertexPC> verts(mDevice, numPoints * 2 + 2, GFXBufferTypeVolatile);
   verts.lock();

   for (S32 i=0; i<numPoints + 1; i++)
   {
      S32 imod = i % numPoints;      
      verts[2 * i].point = Point3F( circlePoints[imod].x * radius, circlePoints[imod].y * radius, height );
      verts[2 * i].color = color;
      verts[2 * i + 1].point = Point3F( circlePoints[imod].x * radius, circlePoints[imod].y * radius, -height );
      verts[2 * i + 1].color = color;
   }

   S32 totalNumPnts = numPoints * 2 + 2;

   // Apply xfm if we were passed one.
   for ( U32 i = 0; i < totalNumPnts; i++ )
      mat.mulP( verts[i].point );

   // Apply position offset
   for ( U32 i = 0; i < totalNumPnts; i++ )
      verts[i].point += center;

   verts.unlock();

   mDevice->setStateBlockByDesc( desc );

   mDevice->setVertexBuffer( verts );
   mDevice->setupGenericShaders();

   mDevice->drawPrimitive( GFXTriangleStrip, 0, 2 * numPoints );

   Point3F sphereCenter;
   MatrixF sphereMat;

   if ( xfm )
      sphereMat = *xfm;
   else
      sphereMat = MatrixF::Identity;   

   sphereCenter.set( 0, 0, 0.5f * height );
   mat.mulV( sphereCenter );
   sphereCenter += center;

   drawSphere( desc, radius, sphereCenter, color, true, false, &sphereMat );

   sphereCenter.set( 0, 0, -0.5f * height );
   mat.mulV( sphereCenter );
   sphereCenter += center;

   drawSphere( desc, radius, sphereCenter, color, false, true, &sphereMat );
}

bool Convex::getCollisionInfo(const MatrixF& mat, const Point3F& scale, CollisionList* cList,F32 tol)
{
   PROFILE_SCOPE( Convex_GetCollisionInfo );

   // Making these static prevents needless Vector resizing that occurs
   // in the ConvexFeature constructor.
   static ConvexFeature fa;
   static ConvexFeature fb;

   for ( CollisionStateList* itr = mList.mNext; 
         itr != &mList; 
         itr = itr->mNext) 
   {

      CollisionState* state = itr->mState;

      if (state->mLista != itr)
         state->swap();

      if (state->dist <= tol) 
      {
         fa.reset();
         fb.reset();
         VectorF v;

         // The idea is that we need to scale the matrix, so we need to
         // make a copy of it, before we can pass it in to getFeatures.
         // This is used to scale us for comparison against the other
         // convex, which is correctly scaled.
         MatrixF omat = mat;
         omat.scale(scale);

         MatrixF imat = omat;
         imat.inverse();
         imat.mulV(-state->v,&v);

         getFeatures(omat,v,&fa);

         imat = state->b->getTransform();
         imat.scale(state->b->getScale());

         MatrixF bxform = imat;
         imat.inverse();
         imat.mulV(state->v,&v);

         state->b->getFeatures(bxform,v,&fb);

         fa.collide(fb,cList,tol);
      }
   }

   return (cList->getCount() != 0);
}

void Sun::_renderCorona( ObjectRenderInst *ri, SceneState *state, BaseMatInstance *overrideMat )
{   
   Point3F sunlightPosition = state->getCameraPosition() - mLight->getDirection() * state->getFarPlane() * 0.9f;

   // Calculate Billboard Radius (in world units) to be constant, independent of distance.
   // Takes into account distance, viewport size, and specified size in editor
   F32 BBRadius = (((sunlightPosition - state->getCameraPosition()).len()) / (GFX->getViewport().extent.x / 640.0)) / 2;  
   BBRadius *= mCoronaScale;

   GFXTransformSaver saver;

   if ( state->isReflectPass() )
      GFX->setProjectionMatrix( gClientSceneGraph->getNonClipProjection() );

   GFX->setStateBlock(mCoronaSB);

   // Initialize points with basic info
   Point3F points[4];
   points[0] = Point3F(-BBRadius, 0.0, -BBRadius);
   points[1] = Point3F( BBRadius, 0.0, -BBRadius);
   points[2] = Point3F( BBRadius, 0.0,  BBRadius);
   points[3] = Point3F(-BBRadius, 0.0,  BBRadius);

   // Get info we need to adjust points
   MatrixF camView = GFX->getWorldMatrix();
   camView.inverse();

   // Finalize points
   for(int i = 0; i < 4; i++)
   {
      // align with camera
      camView.mulV(points[i]);
      // offset
      points[i] += sunlightPosition;
   }

   // Draw it
   
   if ( mCoronaUseLightColor )
      PrimBuild::color(mLightColor);
   else
      PrimBuild::color(mCoronaTint);
   
   GFX->setTexture(0, mCoronaTexture);

   PrimBuild::begin( GFXTriangleFan, 4 );
   PrimBuild::texCoord2f(0, 0);
   PrimBuild::vertex3fv(points[0]);
   PrimBuild::texCoord2f(1, 0);
   PrimBuild::vertex3fv(points[1]);
   PrimBuild::texCoord2f(1, 1);
   PrimBuild::vertex3fv(points[2]);
   PrimBuild::texCoord2f(0, 1);
   PrimBuild::vertex3fv(points[3]);
   PrimBuild::end();
}

void ForestConvex::getFeatures( const MatrixF &mat, const VectorF &n, ConvexFeature *cf )
{
   cf->material = 0;
   cf->object = mObject;

   TSShapeInstance *si = mData->getShapeInstance();

   TSShape::ConvexHullAccelerator* pAccel =
      si->getShape()->getAccelerator(mData->getCollisionDetails()[hullId]);
   AssertFatal(pAccel != NULL, "Error, no accel!");

   F32 currMaxDP = mDot(pAccel->vertexList[0], n);
   U32 index = 0;
   U32 i;
   for (i = 1; i < pAccel->numVerts; i++) 
   {
      F32 dp = mDot(pAccel->vertexList[i], n);
      if (dp > currMaxDP) 
      {
         currMaxDP = dp;
         index = i;
      }
   }

   const U8* emitString = pAccel->emitStrings[index];
   U32 currPos = 0;
   U32 numVerts = emitString[currPos++];
   for (i = 0; i < numVerts; i++) 
   {
      cf->mVertexList.increment();
      U32 index = emitString[currPos++];
      mat.mulP(pAccel->vertexList[index], &cf->mVertexList.last());
   }

   U32 numEdges = emitString[currPos++];
   for (i = 0; i < numEdges; i++) 
   {
      U32 ev0 = emitString[currPos++];
      U32 ev1 = emitString[currPos++];
      cf->mEdgeList.increment();
      cf->mEdgeList.last().vertex[0] = ev0;
      cf->mEdgeList.last().vertex[1] = ev1;
   }

   U32 numFaces = emitString[currPos++];
   for (i = 0; i < numFaces; i++) 
   {
      cf->mFaceList.increment();
      U32 plane = emitString[currPos++];
      mat.mulV(pAccel->normalList[plane], &cf->mFaceList.last().normal);
      for (U32 j = 0; j < 3; j++)
         cf->mFaceList.last().vertex[j] = emitString[currPos++];
   }
}

MatrixF PlaneReflector::getFrustumClipProj( MatrixF &modelview )
{
   static MatrixF rotMat(EulerF( static_cast<F32>(M_PI / 2.f), 0.0, 0.0));
   static MatrixF invRotMat(EulerF( -static_cast<F32>(M_PI / 2.f), 0.0, 0.0));


   MatrixF revModelview = modelview;
   revModelview = rotMat * revModelview;  // add rotation to modelview because it needs to be removed from projection

   // rotate clip plane into modelview space
   Point4F clipPlane;
   Point3F pnt = refplane * -(refplane.d + 0.0 );
   Point3F norm = refplane;

   revModelview.mulP( pnt );
   revModelview.mulV( norm );
   norm.normalize();

   clipPlane.set( norm.x, norm.y, norm.z, -mDot( pnt, norm ) );


   // Manipulate projection matrix
   //------------------------------------------------------------------------
   MatrixF proj = GFX->getProjectionMatrix();
   proj.mul( invRotMat );  // reverse rotation imposed by Torque
   proj.transpose();       // switch to row-major order

   // Calculate the clip-space corner point opposite the clipping plane
   // as (sgn(clipPlane.x), sgn(clipPlane.y), 1, 1) and
   // transform it into camera space by multiplying it
   // by the inverse of the projection matrix
   Vector4F	q;
   q.x = sgn(clipPlane.x) / proj(0,0);
   q.y = sgn(clipPlane.y) / proj(1,1);
   q.z = -1.0F;
   q.w = ( 1.0F - proj(2,2) ) / proj(3,2);

   F32 a = 1.0 / (clipPlane.x * q.x + clipPlane.y * q.y + clipPlane.z * q.z + clipPlane.w * q.w);

   Vector4F c = clipPlane * a;

   // CodeReview [ags 1/23/08] Come up with a better way to deal with this.
   if(GFX->getAdapterType() == OpenGL)
      c.z += 1.0f;

   // Replace the third column of the projection matrix
   proj.setColumn( 2, c );
   proj.transpose(); // convert back to column major order
   proj.mul( rotMat );  // restore Torque rotation

   return proj;
}

//----------------------------------------------------------------------------
// Create ring
//----------------------------------------------------------------------------
SplashRing Splash::createRing()
{
   SplashRing ring;
   U32 numPoints = mDataBlock->numSegments + 1;

   Point3F ejectionAxis( 0.0, 0.0, 1.0 );

   Point3F axisx;
   if (mFabs(ejectionAxis.z) < 0.999f)
      mCross(ejectionAxis, Point3F(0, 0, 1), &axisx);
   else
      mCross(ejectionAxis, Point3F(0, 1, 0), &axisx);
   axisx.normalize();

   for( U32 i=0; i<numPoints; i++ )
   {
      F32 t = F32(i) / F32(numPoints);

      AngAxisF thetaRot( axisx, mDataBlock->ejectionAngle * (M_PI / 180.0));
      AngAxisF phiRot(   ejectionAxis, t * (M_PI * 2.0));

      Point3F pointAxis = ejectionAxis;

      MatrixF temp;
      thetaRot.setMatrix(&temp);
      temp.mulP(pointAxis);
      phiRot.setMatrix(&temp);
      temp.mulP(pointAxis);

      Point3F startOffset = axisx;
      temp.mulV( startOffset );
      startOffset *= mDataBlock->startRadius;

      SplashRingPoint point;
      point.position = getPosition() + startOffset;
      point.velocity = pointAxis * mDataBlock->velocity;

      ring.points.push_back( point );
   }

   ring.color = mDataBlock->colors[0];
   ring.lifetime = mDataBlock->ringLifetime;
   ring.elapsedTime = 0.0;
   ring.v = mDataBlock->texFactor * mFmod( mElapsedTime, 1.0 );

   return ring;
}

void ProcessedFFMaterial::_setSecondaryLightInfo(const MatrixF &_objTrans, LightInfo* light)
{
   // set object transform
   MatrixF objTrans = _objTrans;
   objTrans.inverse();

   // fill in secondary light
   //-------------------------
   GFXLightInfo xlatedLight;
   light->setGFXLight(&xlatedLight);

   Point3F lightPos = light->getPosition();
   Point3F lightDir = light->getDirection();
   objTrans.mulP(lightPos);
   objTrans.mulV(lightDir);

   xlatedLight.mPos = lightPos;
   xlatedLight.mDirection = lightDir;

   GFX->setLight(1, &xlatedLight);
}

void ProcessedFFMaterial::_setPrimaryLightInfo(const MatrixF &_objTrans, LightInfo* light, U32 pass)
{
   // Just in case
   GFX->setGlobalAmbientColor(ColorF(0.0f, 0.0f, 0.0f, 1.0f));
   if ( light->getType() == LightInfo::Ambient )
   {
      // Ambient light
      GFX->setGlobalAmbientColor( light->getAmbient() );
      return;
   }

   GFX->setLight(0, NULL);
   GFX->setLight(1, NULL);
   // This is a quick hack that lets us use FF lights
   GFXLightMaterial lightMat;
   lightMat.ambient = ColorF(1.0f, 1.0f, 1.0f, 1.0f);
   lightMat.diffuse = ColorF(1.0f, 1.0f, 1.0f, 1.0f);
   lightMat.emissive = ColorF(0.0f, 0.0f, 0.0f, 0.0f);
   lightMat.specular = ColorF(0.0f, 0.0f, 0.0f, 0.0f);
   lightMat.shininess = 128.0f;
   GFX->setLightMaterial(lightMat);   

   // set object transform
   MatrixF objTrans = _objTrans;
   objTrans.inverse();

   // fill in primary light
   //-------------------------
   GFXLightInfo xlatedLight;
   light->setGFXLight(&xlatedLight);
   Point3F lightPos = light->getPosition();
   Point3F lightDir = light->getDirection();
   objTrans.mulP(lightPos);
   objTrans.mulV(lightDir);

   xlatedLight.mPos = lightPos;
   xlatedLight.mDirection = lightDir;

   GFX->setLight(0, &xlatedLight);
}

void WorldEditorSelection::rotate(const EulerF & rot, const Point3F & center)
{
   // single selections will rotate around own axis, multiple about world
   if(size() == 1)
   {
      SceneObject* object = dynamic_cast< SceneObject* >( at( 0 ) );
      if( object )
      {
         MatrixF mat = object->getTransform();

         Point3F pos;
         mat.getColumn(3, &pos);

         // get offset in obj space
         Point3F offset = pos - center;
         MatrixF wMat = object->getWorldTransform();
         wMat.mulV(offset);

         //
         MatrixF transform(EulerF(0,0,0), -offset);
         transform.mul(MatrixF(rot));
         transform.mul(MatrixF(EulerF(0,0,0), offset));
         mat.mul(transform);

         object->setTransform(mat);
      }
   }
   else
   {
      for( iterator iter = begin(); iter != end(); ++ iter )
      {
         SceneObject* object = dynamic_cast< SceneObject* >( *iter );
         if( !object )
            continue;
            
         MatrixF mat = object->getTransform();

         Point3F pos;
         mat.getColumn(3, &pos);

         // get offset in obj space
         Point3F offset = pos - center;

         MatrixF transform(rot);
         Point3F wOffset;
         transform.mulV(offset, &wOffset);

         MatrixF wMat = object->getWorldTransform();
         wMat.mulV(offset);

         //
         transform.set(EulerF(0,0,0), -offset);

         mat.setColumn(3, Point3F(0,0,0));
         wMat.setColumn(3, Point3F(0,0,0));

         transform.mul(wMat);
         transform.mul(MatrixF(rot));
         transform.mul(mat);
         mat.mul(transform);

         mat.normalize();
         mat.setColumn(3, wOffset + center);

         object->setTransform(mat);
      }
   }

   mCentroidValid = false;
}

void AdvancedLightBinManager::LightMaterialInfo::setLightParameters( const LightInfo *lightInfo, const SceneRenderState* renderState, const MatrixF &worldViewOnly )
{
   MaterialParameters *matParams = matInstance->getMaterialParameters();

   // Set color in the right format, set alpha to the luminance value for the color.
   ColorF col = lightInfo->getColor();

   // TODO: The specularity control of the light
   // is being scaled by the overall lumiance.
   //
   // Not sure if this may be the source of our
   // bad specularity results maybe?
   //

   const Point3F colorToLumiance( 0.3576f, 0.7152f, 0.1192f );
   F32 lumiance = mDot(*((const Point3F *)&lightInfo->getColor()), colorToLumiance );
   col.alpha *= lumiance;

   matParams->setSafe( lightColor, col );
   matParams->setSafe( lightBrightness, lightInfo->getBrightness() );

   switch( lightInfo->getType() )
   {
   case LightInfo::Vector:
      {
         VectorF lightDir = lightInfo->getDirection();
         worldViewOnly.mulV(lightDir);
         lightDir.normalize();
         matParams->setSafe( lightDirection, lightDir );

         // Set small number for alpha since it represents existing specular in
         // the vector light. This prevents a divide by zero.
         ColorF ambientColor = renderState->getAmbientLightColor();
         ambientColor.alpha = 0.00001f;
         matParams->setSafe( lightAmbient, ambientColor );

         // If no alt color is specified, set it to the average of
         // the ambient and main color to avoid artifacts.
         //
         // TODO: Trilight disabled until we properly implement it
         // in the light info!
         //
         //ColorF lightAlt = lightInfo->getAltColor();
         ColorF lightAlt( ColorF::BLACK ); // = lightInfo->getAltColor();
         if ( lightAlt.red == 0.0f && lightAlt.green == 0.0f && lightAlt.blue == 0.0f )
            lightAlt = (lightInfo->getColor() + renderState->getAmbientLightColor()) / 2.0f;

         ColorF trilightColor = lightAlt;
         matParams->setSafe(lightTrilight, trilightColor);
      }
      break;

   case LightInfo::Spot:
      {
         const F32 outerCone = lightInfo->getOuterConeAngle();
         const F32 innerCone = getMin( lightInfo->getInnerConeAngle(), outerCone );
         const F32 outerCos = mCos( mDegToRad( outerCone / 2.0f ) );
         const F32 innerCos = mCos( mDegToRad( innerCone / 2.0f ) );
         Point4F spotParams(  outerCos, 
                              innerCos - outerCos, 
                              mCos( mDegToRad( outerCone ) ), 
                              0.0f );

         matParams->setSafe( lightSpotParams, spotParams );

         VectorF lightDir = lightInfo->getDirection();
         worldViewOnly.mulV(lightDir);
         lightDir.normalize();
         matParams->setSafe( lightDirection, lightDir );
      }
      // Fall through

   case LightInfo::Point:
   {
      const F32 radius = lightInfo->getRange().x;
      matParams->setSafe( lightRange, radius );

      Point3F lightPos;
      worldViewOnly.mulP(lightInfo->getPosition(), &lightPos);
      matParams->setSafe( lightPosition, lightPos );

      // Get the attenuation falloff ratio and normalize it.
      Point3F attenRatio = lightInfo->getExtended<ShadowMapParams>()->attenuationRatio;
      F32 total = attenRatio.x + attenRatio.y + attenRatio.z;
      if ( total > 0.0f )
         attenRatio /= total;

      Point2F attenParams( ( 1.0f / radius ) * attenRatio.y,
                           ( 1.0f / ( radius * radius ) ) * attenRatio.z );

      matParams->setSafe( lightAttenuation, attenParams );
      break;
   }

   default:
      AssertFatal( false, "Bad light type!" );
      break;
   }
}

void AtlasConvex::getFeatures(const MatrixF& mat,const VectorF& n, ConvexFeature* cf)
{
   cf->material = 0;
   cf->object = mObject;

   // For a tetrahedron this is pretty easy.

   //    points...
   S32 firstVert = cf->mVertexList.size();
   cf->mVertexList.increment(); mat.mulP(point[0], &cf->mVertexList.last());
   cf->mVertexList.increment(); mat.mulP(point[1], &cf->mVertexList.last());
   cf->mVertexList.increment(); mat.mulP(point[2], &cf->mVertexList.last());
   cf->mVertexList.increment(); mat.mulP(point[3], &cf->mVertexList.last());

   //    edges...
   cf->mEdgeList.increment(); 
   cf->mEdgeList.last().vertex[0] = firstVert+0;
   cf->mEdgeList.last().vertex[1] = firstVert+1;

   cf->mEdgeList.increment(); 
   cf->mEdgeList.last().vertex[0] = firstVert+1;
   cf->mEdgeList.last().vertex[1] = firstVert+2;

   cf->mEdgeList.increment(); 
   cf->mEdgeList.last().vertex[0] = firstVert+2;
   cf->mEdgeList.last().vertex[1] = firstVert+0;

   cf->mEdgeList.increment(); 
   cf->mEdgeList.last().vertex[0] = firstVert+3;
   cf->mEdgeList.last().vertex[1] = firstVert+0;

   cf->mEdgeList.increment(); 
   cf->mEdgeList.last().vertex[0] = firstVert+3;
   cf->mEdgeList.last().vertex[1] = firstVert+1;

   cf->mEdgeList.increment(); 
   cf->mEdgeList.last().vertex[0] = firstVert+3;
   cf->mEdgeList.last().vertex[1] = firstVert+2;

   //    triangles...
   cf->mFaceList.increment();
   cf->mFaceList.last().normal;
   mat.mulV(normal, &cf->mFaceList.last().normal);
   cf->mFaceList.last().vertex[0] = firstVert+0;
   cf->mFaceList.last().vertex[1] = firstVert+1;
   cf->mFaceList.last().vertex[2] = firstVert+2;

   cf->mFaceList.increment();
   mat.mulV(PlaneF(point[0], point[1], point[3]), &cf->mFaceList.last().normal);
   cf->mFaceList.last().vertex[0] = firstVert+3;
   cf->mFaceList.last().vertex[1] = firstVert+0;
   cf->mFaceList.last().vertex[2] = firstVert+1;

   cf->mFaceList.increment();
   mat.mulV(PlaneF(point[1], point[2], point[3]), &cf->mFaceList.last().normal);
   cf->mFaceList.last().vertex[0] = firstVert+3;
   cf->mFaceList.last().vertex[1] = firstVert+1;
   cf->mFaceList.last().vertex[2] = firstVert+2;

   cf->mFaceList.increment();
   mat.mulV(PlaneF(point[2], point[0], point[3]), &cf->mFaceList.last().normal);
   cf->mFaceList.last().vertex[0] = firstVert+3;
   cf->mFaceList.last().vertex[1] = firstVert+0;
   cf->mFaceList.last().vertex[2] = firstVert+2;

   // All done!
}

//.........这里部分代码省略.........
      // Check if we should mMove, or if we are 'close enough'
      if (mFabs(xDiff) < mMoveTolerance && mFabs(yDiff) < mMoveTolerance) 
      {
         mMoveState = ModeStop;
         throwCallback("onReachDestination");
      }
      else 
      {
         // Build move direction in world space
         if (mIsZero(xDiff))
            movePtr->y = (location.y > mMoveDestination.y) ? -1.0f : 1.0f;
         else
            if (mIsZero(yDiff))
               movePtr->x = (location.x > mMoveDestination.x) ? -1.0f : 1.0f;
            else
               if (mFabs(xDiff) > mFabs(yDiff)) 
               {
                  F32 value = mFabs(yDiff / xDiff);
                  movePtr->y = (location.y > mMoveDestination.y) ? -value : value;
                  movePtr->x = (location.x > mMoveDestination.x) ? -1.0f : 1.0f;
               }
               else 
               {
                  F32 value = mFabs(xDiff / yDiff);
                  movePtr->x = (location.x > mMoveDestination.x) ? -value : value;
                  movePtr->y = (location.y > mMoveDestination.y) ? -1.0f : 1.0f;
               }

         // Rotate the move into object space (this really only needs
         // a 2D matrix)
         Point3F newMove;
         MatrixF moveMatrix;
         moveMatrix.set(EulerF(0.0f, 0.0f, -(rotation.z + movePtr->yaw)));
         moveMatrix.mulV( Point3F( movePtr->x, movePtr->y, 0.0f ), &newMove );
         movePtr->x = newMove.x;
         movePtr->y = newMove.y;

         // Set movement speed.  We'll slow down once we get close
         // to try and stop on the spot...
         if (mMoveSlowdown) 
         {
            F32 speed = mMoveSpeed;
            F32 dist = mSqrt(xDiff*xDiff + yDiff*yDiff);
            F32 maxDist = 5.0f;
            if (dist < maxDist)
               speed *= dist / maxDist;
            movePtr->x *= speed;
            movePtr->y *= speed;

            mMoveState = ModeSlowing;
         }
         else 
         {
            movePtr->x *= mMoveSpeed;
            movePtr->y *= mMoveSpeed;

            mMoveState = ModeMove;
         }

         if (mMoveStuckTestCountdown > 0)
            --mMoveStuckTestCountdown;
         else
         {
            // We should check to see if we are stuck...
            F32 locationDelta = (location - mLastLocation).len();
            if (locationDelta < mMoveStuckTolerance && mDamageState == Enabled) 

//.........这里部分代码省略.........
      // Check if we should mMove, or if we are 'close enough'
      if (mFabs(xDiff) < mMoveTolerance && mFabs(yDiff) < mMoveTolerance) 
      {
         mMoveState = ModeStop;
         onReachDestination();
      }
      else 
      {
         // Build move direction in world space
         if (mIsZero(xDiff))
            movePtr->y = (location.y > mMoveDestination.y) ? -1.0f : 1.0f;
         else
            if (mIsZero(yDiff))
               movePtr->x = (location.x > mMoveDestination.x) ? -1.0f : 1.0f;
            else
               if (mFabs(xDiff) > mFabs(yDiff)) 
               {
                  F32 value = mFabs(yDiff / xDiff);
                  movePtr->y = (location.y > mMoveDestination.y) ? -value : value;
                  movePtr->x = (location.x > mMoveDestination.x) ? -1.0f : 1.0f;
               }
               else 
               {
                  F32 value = mFabs(xDiff / yDiff);
                  movePtr->x = (location.x > mMoveDestination.x) ? -value : value;
                  movePtr->y = (location.y > mMoveDestination.y) ? -1.0f : 1.0f;
               }

         // Rotate the move into object space (this really only needs
         // a 2D matrix)
         Point3F newMove;
         MatrixF moveMatrix;
         moveMatrix.set(EulerF(0.0f, 0.0f, -(rotation.z + movePtr->yaw)));
         moveMatrix.mulV( Point3F( movePtr->x, movePtr->y, 0.0f ), &newMove );
         movePtr->x = newMove.x;
         movePtr->y = newMove.y;

         // Set movement speed.  We'll slow down once we get close
         // to try and stop on the spot...
         if (mMoveSlowdown) 
         {
            F32 speed = mMoveSpeed;
            F32 dist = mSqrt(xDiff*xDiff + yDiff*yDiff);
            F32 maxDist = mMoveTolerance*2;
            if (dist < maxDist)
               speed *= dist / maxDist;
            movePtr->x *= speed;
            movePtr->y *= speed;

            mMoveState = ModeSlowing;
         }
         else 
         {
            movePtr->x *= mMoveSpeed;
            movePtr->y *= mMoveSpeed;

            mMoveState = ModeMove;
         }

         if (mMoveStuckTestCountdown > 0)
            --mMoveStuckTestCountdown;
         else
         {
            // We should check to see if we are stuck...
            F32 locationDelta = (location - mLastLocation).len();
            if (locationDelta < mMoveStuckTolerance && mDamageState == Enabled) 

void TurretShape::getCameraTransform(F32* pos,MatrixF* mat)
{
   // Returns camera to world space transform
   // Handles first person / third person camera position
   if (isServerObject() && mShapeInstance)
      mShapeInstance->animateNodeSubtrees(true);

   if (*pos == 0) {
      getRenderEyeTransform(mat);
      return;
   }

   // Get the shape's camera parameters.
   F32 min,max;
   MatrixF rot;
   Point3F offset;
   getCameraParameters(&min,&max,&offset,&rot);

   // Start with the current eye position
   MatrixF eye;
   getRenderEyeTransform(&eye);

   // Build a transform that points along the eye axis
   // but where the Z axis is always up.
   {
      MatrixF cam(1);
      VectorF x,y,z(0,0,1);
      eye.getColumn(1, &y);
      mCross(y, z, &x);
      x.normalize();
      mCross(x, y, &z);
      z.normalize();
      cam.setColumn(0,x);
      cam.setColumn(1,y);
      cam.setColumn(2,z);
      mat->mul(cam,rot);
   }

   // Camera is positioned straight back along the eye's -Y axis.
   // A ray is cast to make sure the camera doesn't go through
   // anything solid.
   VectorF vp,vec;
   vp.x = vp.z = 0;
   vp.y = -(max - min) * *pos;
   eye.mulV(vp,&vec);

   // Use the camera node as the starting position if it exists.
   Point3F osp,sp;
   if (mDataBlock->cameraNode != -1) 
   {
      mShapeInstance->mNodeTransforms[mDataBlock->cameraNode].getColumn(3,&osp);
      getRenderTransform().mulP(osp,&sp);
   }
   else
      eye.getColumn(3,&sp);

   // Make sure we don't hit ourself...
   disableCollision();
   if (isMounted())
      getObjectMount()->disableCollision();

   // Cast the ray into the container database to see if we're going
   // to hit anything.
   RayInfo collision;
   Point3F ep = sp + vec + offset;
   if (mContainer->castRay(sp, ep,
         ~(WaterObjectType | GameBaseObjectType | DefaultObjectType | sTriggerMask),
         &collision) == true) {

      // Shift the collision point back a little to try and
      // avoid clipping against the front camera plane.
      F32 t = collision.t - (-mDot(vec, collision.normal) / vec.len()) * 0.1;
      if (t > 0.0f)
         ep = sp + offset + (vec * t);
      else
         eye.getColumn(3,&ep);
   }
   mat->setColumn(3,ep);

   // Re-enable our collision.
   if (isMounted())
      getObjectMount()->enableCollision();
   enableCollision();

   // Apply Camera FX.
   mat->mul( gCamFXMgr.getTrans() );
}

void HoverVehicle::updateForces(F32 /*dt*/)
{
    PROFILE_SCOPE( HoverVehicle_UpdateForces );

    Point3F gravForce(0, 0, sHoverVehicleGravity * mRigid.mass * mGravityMod);

    MatrixF currTransform;
    mRigid.getTransform(&currTransform);
    mRigid.atRest = false;

    mThrustLevel = (mForwardThrust * mDataBlock->mainThrustForce    +
                    mReverseThrust * mDataBlock->reverseThrustForce +
                    mLeftThrust    * mDataBlock->strafeThrustForce  +
                    mRightThrust   * mDataBlock->strafeThrustForce);

    Point3F thrustForce = ((Point3F( 0,  1, 0) * (mForwardThrust * mDataBlock->mainThrustForce))    +
                           (Point3F( 0, -1, 0) * (mReverseThrust * mDataBlock->reverseThrustForce)) +
                           (Point3F(-1,  0, 0) * (mLeftThrust    * mDataBlock->strafeThrustForce))  +
                           (Point3F( 1,  0, 0) * (mRightThrust   * mDataBlock->strafeThrustForce)));
    currTransform.mulV(thrustForce);
    if (mJetting)
        thrustForce *= mDataBlock->turboFactor;

    Point3F torque(0, 0, 0);
    Point3F force(0, 0, 0);

    Point3F vel = mRigid.linVelocity;
    F32 baseStabLen = getBaseStabilizerLength();
    Point3F stabExtend(0, 0, -baseStabLen);
    currTransform.mulV(stabExtend);

    StabPoint stabPoints[2];
    stabPoints[0].osPoint = Point3F((mObjBox.minExtents.x + mObjBox.maxExtents.x) * 0.5,
                                    mObjBox.maxExtents.y,
                                    (mObjBox.minExtents.z + mObjBox.maxExtents.z) * 0.5);
    stabPoints[1].osPoint = Point3F((mObjBox.minExtents.x + mObjBox.maxExtents.x) * 0.5,
                                    mObjBox.minExtents.y,
                                    (mObjBox.minExtents.z + mObjBox.maxExtents.z) * 0.5);
    U32 j, i;
    for (i = 0; i < 2; i++) {
        currTransform.mulP(stabPoints[i].osPoint, &stabPoints[i].wsPoint);
        stabPoints[i].wsExtension = stabExtend;
        stabPoints[i].extension   = baseStabLen;
        stabPoints[i].wsVelocity  = mRigid.linVelocity;
    }

    RayInfo rinfo;

    mFloating = true;
    bool reallyFloating = true;
    F32 compression[2] = { 0.0f, 0.0f };
    F32  normalMod[2]  = { 0.0f, 0.0f };
    bool normalSet[2]  = { false, false };
    Point3F normal[2];

    for (j = 0; j < 2; j++) {
        if (getContainer()->castRay(stabPoints[j].wsPoint, stabPoints[j].wsPoint + stabPoints[j].wsExtension * 2.0,
                                    TerrainObjectType |
                                    WaterObjectType, &rinfo))
        {
            reallyFloating = false;

            if (rinfo.t <= 0.5) {
                // Ok, stab is in contact with the ground, let's calc the forces...
                compression[j] = (1.0 - (rinfo.t * 2.0)) * baseStabLen;
            }
            normalSet[j] = true;
            normalMod[j] = rinfo.t < 0.5 ? 1.0 : (1.0 - ((rinfo.t - 0.5) * 2.0));

            normal[j] = rinfo.normal;
        }

        if ( pointInWater( stabPoints[j].wsPoint ) )
            compression[j] = baseStabLen;
    }

    for (j = 0; j < 2; j++) {
        if (compression[j] != 0.0) {
            mFloating = false;

            // Spring force and damping
            Point3F springForce = -stabPoints[j].wsExtension;
            springForce.normalize();
            springForce *= compression[j] * mDataBlock->stabSpringConstant;

            Point3F springDamping = -stabPoints[j].wsExtension;
            springDamping.normalize();
            springDamping *= -getMin(mDot(springDamping, stabPoints[j].wsVelocity), 0.7f) * mDataBlock->stabDampingConstant;

            force += springForce + springDamping;
        }
    }

    // Gravity
    if (reallyFloating == false)
        force += gravForce;
    else
        force += gravForce * mDataBlock->floatingGravMag;

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