C++ QuatF类代码示例

bool ConvexShape::protectedSetSurface( void *object, const char *index, const char *data )
{
   ConvexShape *shape = static_cast< ConvexShape* >( object );

   QuatF quat;
	Point3F pos;
	//MatrixF mat;

	/*
   dSscanf( data, "%g %g %g %g %g %g %g %g %g %g %g %g %g %g %g %g", 
      &mat[0], &mat[1], &mat[2], &mat[3], 
      &mat[4], &mat[5], &mat[6], &mat[7], 
      &mat[8], &mat[9], &mat[10], &mat[11],
      &mat[12], &mat[13], &mat[14], &mat[15] );
	*/

	dSscanf( data, "%g %g %g %g %g %g %g", &quat.x, &quat.y, &quat.z, &quat.w, &pos.x, &pos.y, &pos.z );

	MatrixF surface;
	quat.setMatrix( &surface );
	surface.setPosition( pos );

   shape->mSurfaces.push_back( surface );   

   return false;
}

void VPathNode::updateWorldData( void )
{
    if ( !mPath )
    {
        setWorldPosition( getLocalPosition() );
        setWorldRotation( getLocalRotation() );
        return;
    }

    // Fetch Path Details.
    const MatrixF &pathTransform = mPath->getTransform();
    const QuatF   &pathRotation( pathTransform );

    // Calculate the World Position.
    Point3F newPosition = getLocalPosition();
    newPosition.convolve( mPath->getScale() );
    pathTransform.mulP( newPosition );

    // Calculate the new Rotation.
    QuatF newRotation;
    newRotation.mul( getLocalRotation(), pathRotation );

    // Apply.
    setWorldPosition( newPosition );
    setWorldRotation( newRotation );
}

void Sim3DAudioEvent::unpack(NetConnection *con, BitStream *bstream)
{
   SimObjectId id = bstream->readInt(DataBlockObjectIdBitSize) + DataBlockObjectIdFirst;
   Sim::findObject(id, mProfile);

   if (bstream->readFlag()) {
      QuatF q;
      q.x = bstream->readFloat(SoundRotBits);
      q.y = bstream->readFloat(SoundRotBits);
      q.z = bstream->readFloat(SoundRotBits);
      F32 value = ((q.x * q.x) + (q.y * q.y) + (q.z * q.z));
// #ifdef __linux
      // Hmm, this should never happen, but it does...
      if ( value > 1.f )
         value = 1.f;
// #endif
      q.w = mSqrt(1.f - value);
      if (bstream->readFlag())
         q.w = -q.w;
      q.setMatrix(&mTransform);
   }
   else
      mTransform.identity();

   Point3F pos;
   bstream->readCompressedPoint(&pos,SoundPosAccuracy);
   mTransform.setColumn(3, pos);
}

//-----------------------------------------------------------------------------
void Rigid::integrate(F32 delta)
{
   // Update Angular position
   F32 angle = angVelocity.len();
   if (angle != 0.0f) {
      QuatF dq;
      F32 sinHalfAngle;
      mSinCos(angle * delta * -0.5f, sinHalfAngle, dq.w);
      sinHalfAngle *= 1.0f / angle;
      dq.x = angVelocity.x * sinHalfAngle;
      dq.y = angVelocity.y * sinHalfAngle;
      dq.z = angVelocity.z * sinHalfAngle;
      QuatF tmp = angPosition;
      angPosition.mul(tmp, dq);
      angPosition.normalize();

      // Rotate the position around the center of mass
      Point3F lp = linPosition - worldCenterOfMass;
      dq.mulP(lp,&linPosition);
      linPosition += worldCenterOfMass;
   }

   // Update angular momentum
   angMomentum = angMomentum + torque * delta;

   // Update linear position, momentum
   linPosition = linPosition + linVelocity * delta;
   linMomentum = linMomentum + force * delta;
   linVelocity = linMomentum * oneOverMass;

   // Update dependent state variables
   updateInertialTensor();
   updateVelocity();
   updateCenterOfMass();
}

Point3F & QuatF::mulP(const Point3F& p, Point3F* r)
{
   QuatF qq;
   QuatF qi = *this;
   QuatF qv( p.x, p.y, p.z, 0.0f);

   qi.inverse();
   qq.mul(qi, qv);
   qv.mul(qq, *this);
   r->set(qv.x, qv.y, qv.z);
   return *r;
}

F32 Rigid::getKineticEnergy()
{
   Point3F w;
   QuatF qmat = angPosition;
   qmat.inverse();
   qmat.mulP(angVelocity,&w);
   const F32* f = invObjectInertia;
   return 0.5f * ((mass * mDot(linVelocity,linVelocity)) +
      w.x * w.x / f[0] +
      w.y * w.y / f[5] +
      w.z * w.z / f[10]);
}

Point3F& m_mul( const Point3F &p, const QuatF &q, Point3F *r )
{
   QuatF qq;
   QuatF qi = q;
   QuatF qv( p.x, p.y, p.z, 0.0f);

   qi.inverse();   
   m_mul(qi, qv, &qq );
   m_mul(qq, q, &qv );
   r->set(qv.x, qv.y, qv.z);
   return ( *r );
}

void TSShapeLoader::generateNodeTransform(AppNode* node, F32 t, bool blend, F32 referenceTime,
                                          QuatF& rot, Point3F& trans, QuatF& srot, Point3F& scale)
{
   MatrixF m1 = getLocalNodeMatrix(node, t);
   if (blend)
   {
      MatrixF m0 = getLocalNodeMatrix(node, referenceTime);
      m1 = m0.inverse() * m1;
   }

   rot.set(m1);
   trans = m1.getPosition();
   srot.identity();        //@todo: srot not supported yet
   scale = m1.getScale();
}

void	PhysShape::setRotation(const QuatF& rot)
{
    MatrixF tr;
    rot.setMatrix(&tr);
    tr.setPosition(getPosition());
    setTransform(tr);
}

void RigidBody::setRenderPosition(const Point3F& pos, const QuatF& rot)
{
	MatrixF mat;
	rot.setMatrix(&mat);
	mat.setColumn(3,pos);
	setRenderTransform(mat);
}

void ConvexShape::unpackUpdate( NetConnection *conn, BitStream *stream )
{
   Parent::unpackUpdate( conn, stream );

   if ( stream->readFlag() )  // TransformMask
   {
      mathRead(*stream, &mObjToWorld);
      mathRead(*stream, &mObjScale);

      setTransform( mObjToWorld );
      setScale( mObjScale );
   }

   if ( stream->readFlag() ) // UpdateMask
   {
      stream->read( &mMaterialName );      

      if ( isProperlyAdded() )
         _updateMaterial();

      mSurfaces.clear();

      const U32 surfCount = stream->readInt( 32 );
      for ( S32 i = 0; i < surfCount; i++ )
      {
         mSurfaces.increment();
         MatrixF &mat = mSurfaces.last();

         QuatF quat;
         Point3F pos;

         mathRead( *stream, &quat );
         mathRead( *stream, &pos ); 

         quat.setMatrix( &mat );
         mat.setPosition( pos );
      }

      if ( isProperlyAdded() )
         _updateGeometry( true );
   }
}

void PxSingleActor::interpolateTick( F32 delta )
{
   Point3F interpPos;
   QuatF interpRot;

   // Interpolate the position based on the delta.
   interpPos.interpolate( mNextPos, mLastPos, delta );

   // Interpolate the rotation based on the delta.
   interpRot.interpolate( mNextRot, mLastRot, delta );

   // Set up the interpolated transform.
   MatrixF interpMat;

   // Set the interpolated position and rotation.
   interpRot.setMatrix( &interpMat );
   interpMat.setPosition( interpPos );

   // Set the transform to the interpolated transform.
   Parent::setTransform( interpMat );
}

void ProximityMine::unpackUpdate( NetConnection* connection, BitStream* stream )
{
   Parent::unpackUpdate( connection, stream );

   // Item::RotationMask
   if ( stream->readFlag() )
   {
      QuatF rot;
      mathRead( *stream, &rot );

      Point3F pos = mObjToWorld.getPosition();
      rot.setMatrix( &mObjToWorld );
      mObjToWorld.setPosition( pos );
   }

   // !mStatic && ( mask & DeployedMask ) && ( mState > Thrown )
   if ( stream->readFlag() )
   {
      mathRead( *stream, &mStickyCollisionPos );
      mathRead( *stream, &mStickyCollisionNormal );

      mAtRest = true;

      setDeployedPos( mStickyCollisionPos, mStickyCollisionNormal );
   }

   // ( mask & ExplosionMask ) && ( mState == Exploded )
   if ( stream->readFlag() )
   {
      // start the explosion visuals on the client
      explode();
   }

   if ( mStatic && mState <= Deployed )
   {
      // static mines are armed immediately
      mState = Deployed;
      mStateTimeout = 0;
   }
}

void TurretShape::unpackUpdate(NetConnection *connection, BitStream *stream)
{
   Parent::unpackUpdate(connection,stream);

   // InitialUpdateMask
   if (stream->readFlag()) {
      mRespawn = stream->readFlag();
   }

   // Item::RotationMask
   if ( stream->readFlag() )
   {
      QuatF rot;
      mathRead( *stream, &rot );

      Point3F pos = mObjToWorld.getPosition();
      rot.setMatrix( &mObjToWorld );
      mObjToWorld.setPosition( pos );
   }

   // controlled by the client?
   if(stream->readFlag())
      return;

   // TurretUpdateMask
   if (stream->readFlag())
   {
      Point3F rot(0.0f, 0.0f, 0.0f);
      stream->read(&rot.x);
      stream->read(&rot.z);
      _setRotation(rot);

      // New delta for client side interpolation
      mTurretDelta.rot = rot;
      mTurretDelta.rotVec = VectorF(0.0f, 0.0f, 0.0f);

      stream->read(&allowManualRotation);
      stream->read(&allowManualFire);
   }
}

void fxShapeReplicator::CreateShapes(void)
{
   F32				HypX, HypY;
   F32				Angle;
   U32				RelocationRetry;
   Point3F			ShapePosition;
   Point3F			ShapeStart;
   Point3F			ShapeEnd;
   Point3F			ShapeScale;
   EulerF			ShapeRotation;
   QuatF			QRotation;
   bool			CollisionResult;
   RayInfo			RayEvent;
   TSShape*		pShape;


   // Don't create shapes if we are hiding replications.
   if (mFieldData.mHideReplications) return;

   // Cannot continue without shapes!
   if (dStrcmp(mFieldData.mShapeFile, "") == 0) return;

   // Check that we can position somewhere!
   if (!(	mFieldData.mAllowOnTerrain ||
      mFieldData.mAllowStatics ||
      mFieldData.mAllowOnWater))
   {
      // Problem ...
      Con::warnf(ConsoleLogEntry::General, "[%s] - Could not place object, All alloweds are off!", getName());

      // Return here.
      return;
   }

   // Check Shapes.
   AssertFatal(mCurrentShapeCount==0,"Shapes already present, this should not be possible!")

      // Check that we have a shape...
      if (!mFieldData.mShapeFile) return;

   // Set Seed.
   RandomGen.setSeed(mFieldData.mSeed);

   // Set shape vector.
   mReplicatedShapes.clear();

   // Add shapes.
   for (U32 idx = 0; idx < mFieldData.mShapeCount; idx++)
   {
      fxShapeReplicatedStatic*	fxStatic;

      // Create our static shape.
      fxStatic = new fxShapeReplicatedStatic();

      // Set the 'shapeName' field.
      fxStatic->setField("shapeName", mFieldData.mShapeFile);

      // Is this Replicator on the Server?
      if (isServerObject())
         // Yes, so stop it from Ghosting. (Hack, Hack, Hack!)
         fxStatic->touchNetFlags(Ghostable, false);
      else
         // No, so flag as ghost object. (Another damn Hack!)
         fxStatic->touchNetFlags(IsGhost, true);

      // Register the Object.
      if (!fxStatic->registerObject())
      {
         // Problem ...
         Con::warnf(ConsoleLogEntry::General, "[%s] - Could not load shape file '%s'!", getName(), mFieldData.mShapeFile);

         // Destroy Shape.
         delete fxStatic;

         // Destroy existing hapes.
         DestroyShapes();

         // Quit.
         return;
      }

      // Get Allocated Shape.
      pShape = fxStatic->getShape();

      // Reset Relocation Retry.
      RelocationRetry = mFieldData.mShapeRetries;

      // Find it a home ...
      do
      {
         // Get the Replicator Position.
         ShapePosition = getPosition();

         // Calculate a random offset
         HypX	= RandomGen.randF(mFieldData.mInnerRadiusX, mFieldData.mOuterRadiusX);
         HypY	= RandomGen.randF(mFieldData.mInnerRadiusY, mFieldData.mOuterRadiusY);
         Angle	= RandomGen.randF(0, (F32)M_2PI);

         // Calcualte the new position.
         ShapePosition.x += HypX * mCos(Angle);
//.........这里部分代码省略.........