atifly.cpp

游戏编程精粹2第三章源码

   aPosition->y = a*mLocationIntervals[interval].lo.y + b*mLocationIntervals[interval].hi.y + ((a*a*a - a)*mLocationIntervals[interval].lo2.y + (b*b*b - b)*mLocationIntervals[interval].hi2.y) / 6.0f;// * (float32)(mSampleInterval*mSampleInterval) / 6.0f;
   aPosition->z = a*mLocationIntervals[interval].lo.z + b*mLocationIntervals[interval].hi.z + ((a*a*a - a)*mLocationIntervals[interval].lo2.z + (b*b*b - b)*mLocationIntervals[interval].hi2.z) / 6.0f;// * (float32)(mSampleInterval*mSampleInterval) / 6.0f;

   return TRUE;
}

//=============================================================================
bool8 AtiFlyPath::CompileOrientations (void)
{
   int32 i;
   int32 j;
   int32 k;
   float32 tmpf;
   float32 tmpf2;
   int32 numPoints = 0;
   AtiQuaternion *pointArray = NULL;
   AtiQuaternion *tmpD2 = NULL;
   AtiQuaternion *tmpIntD2 = NULL;

   /***********************************/
   /* Make sure we have enough points */
   /***********************************/
   if (mNumFlyPoints < 5)
      return FALSE;

   /********************************************/
   /* Allocate new memory for spline intervals */
   /********************************************/
   AtiFreeArray(mOrientationIntervals);
   AtiMallocArray(mOrientationIntervals, mNumFlyPoints, AtiSplineInterval);

   /*****************************************/
   /* Allocate memory for temporary storage */
   /*****************************************/
   AtiMallocArray(pointArray, mNumFlyPoints+9, AtiQuaternion);
   AtiMallocArray(tmpD2,      mNumFlyPoints+9, AtiQuaternion);
   AtiMallocArray(tmpIntD2,   mNumFlyPoints+9, AtiQuaternion);

   /***************************************************************************/
   /* Build each array of vertices padding by 1 point on each end and process */
   /***************************************************************************/
   mPathTime = 0.0;
   mNumSplineIntervals = 0;
   for (i=0; i<mNumFlyPoints; i++)
   {
      /*******************/
      /* Skip cut points */
      /*******************/
      if (mLocation[i].w != 0.0)
         continue;

      /****************/
      /* Init counter */
      /****************/
      numPoints = 0;

      /**************************/
      /* Pad spline by 4 points */
      /**************************/
      if ((i == 0) && (mPreviousPath->mNumFlyPoints >= 5) && (mPreviousPath->mLocation[mPreviousPath->mNumFlyPoints-1].w == 0.0))
      {
         AtiQuatCopy (&(pointArray[numPoints+3]), &(mPreviousPath->mOrientation[mPreviousPath->mNumFlyPoints-1]));
         if (mPreviousPath->mLocation[mPreviousPath->mNumFlyPoints-2].w == 0.0)
         {
            AtiQuatCopy (&(pointArray[numPoints+2]), &(mPreviousPath->mOrientation[mPreviousPath->mNumFlyPoints-2]));
            if (mPreviousPath->mLocation[mPreviousPath->mNumFlyPoints-3].w == 0.0)
            {
               AtiQuatCopy (&(pointArray[numPoints+1]), &(mPreviousPath->mOrientation[mPreviousPath->mNumFlyPoints-3]));
               if (mPreviousPath->mLocation[mPreviousPath->mNumFlyPoints-4].w == 0.0)
               {
                  AtiQuatCopy (&(pointArray[numPoints+0]), &(mPreviousPath->mOrientation[mPreviousPath->mNumFlyPoints-4]));
               }
               else
               {
                  AtiQuatSlerpShortestPath (&(pointArray[numPoints+0]), &(pointArray[numPoints+1]), &(pointArray[numPoints+2]), -1.0f);
               }
            }
            else
            {
               AtiQuatSlerpShortestPath (&(pointArray[numPoints+1]), &(pointArray[numPoints+2]), &(pointArray[numPoints+3]), -1.0f);
               AtiQuatSlerpShortestPath (&(pointArray[numPoints+0]), &(pointArray[numPoints+1]), &(pointArray[numPoints+2]), -1.0f);
            }
         }
         else
         {
            AtiQuatSlerpShortestPath (&(pointArray[numPoints+2]), &(pointArray[numPoints+3]), &(mOrientation[i]), -1.0f);
            AtiQuatSlerpShortestPath (&(pointArray[numPoints+1]), &(pointArray[numPoints+2]), &(pointArray[numPoints+3]), -1.0f);
            AtiQuatSlerpShortestPath (&(pointArray[numPoints+0]), &(pointArray[numPoints+1]), &(pointArray[numPoints+2]), -1.0f);
         }
      }
      else //Not first point on spline, just create phantom points
      {
         if (i < mNumFlyPoints-1) //If not last point on spline
         {
            AtiQuatSlerpShortestPath (&(pointArray[numPoints+3]), &(mOrientation[i]), &(mOrientation[i+1]), -1.0f);
            AtiQuatSlerpShortestPath (&(pointArray[numPoints+2]), &(mOrientation[i]), &(mOrientation[i+1]), -2.0f);
            AtiQuatSlerpShortestPath (&(pointArray[numPoints+1]), &(mOrientation[i]), &(mOrientation[i+1]), -3.0f);
            AtiQuatSlerpShortestPath (&(pointArray[numPoints+0]), &(mOrientation[i]), &(mOrientation[i+1]), -4.0f);
         }
         else
         {
            if (mNextPath->mNumFlyPoints >= 5)
            {
               AtiQuatSlerpShortestPath (&(pointArray[numPoints+3]), &(mOrientation[i]), &(mNextPath->mOrientation[0]), -1.0f);
               AtiQuatSlerpShortestPath (&(pointArray[numPoints+2]), &(mOrientation[i]), &(mNextPath->mOrientation[0]), -2.0f);
               AtiQuatSlerpShortestPath (&(pointArray[numPoints+1]), &(mOrientation[i]), &(mNextPath->mOrientation[0]), -3.0f);
               AtiQuatSlerpShortestPath (&(pointArray[numPoints+0]), &(mOrientation[i]), &(mNextPath->mOrientation[0]), -4.0f);
            }
            else //No next point...something's wrong
            {
               AtiQuatCopy (&(pointArray[numPoints+3]), &(mOrientation[i]));
               AtiQuatCopy (&(pointArray[numPoints+2]), &(mOrientation[i]));
               AtiQuatCopy (&(pointArray[numPoints+1]), &(mOrientation[i]));
               AtiQuatCopy (&(pointArray[numPoints+0]), &(mOrientation[i]));
            }
         }
      }
      numPoints += 4;

      /*****************************************/
      /* Add all points that aren't cut points */
      /*****************************************/
      for (; i<mNumFlyPoints; i++)
      {
         /******************/
         /* Add this point */
         /******************/
         AtiQuatCopy (&(pointArray[numPoints]), &(mOrientation[i]));
         numPoints++;

         /**********************/
         /* Break if cut point */
         /**********************/
         if (mLocation[i].w != 0.0)
            break;
      }

      /*************************************************/
      /* Add last 4 pad points by either getting them  */
      /* from the next path or creating phantom points */
      /*************************************************/
      if (((i < mNumFlyPoints) && (mLocation[i].w != 0.0)) || ((i >= mNumFlyPoints) && (mNextPath->mNumFlyPoints < 5))) //If cut point OR next path isn't valid
      {
         AtiQuatSlerpShortestPath (&(pointArray[numPoints+0]), &(pointArray[numPoints-2]), &(pointArray[numPoints-1]), 2.0f);
         AtiQuatSlerpShortestPath (&(pointArray[numPoints+1]), &(pointArray[numPoints-2]), &(pointArray[numPoints-1]), 3.0f);
         AtiQuatSlerpShortestPath (&(pointArray[numPoints+2]), &(pointArray[numPoints-2]), &(pointArray[numPoints-1]), 4.0f);
         AtiQuatSlerpShortestPath (&(pointArray[numPoints+3]), &(pointArray[numPoints-2]), &(pointArray[numPoints-1]), 5.0f);
         numPoints += 4;
      }
      else //Look at next path
      {
         /********************************************************************/
         /* This is the last point and smoothly feeding into the next spline */
         /********************************************************************/
         //Add first point to close the spline
         AtiQuatCopy (&(pointArray[numPoints]), &(mNextPath->mOrientation[0]));
         numPoints++;

         //Pad with next point
         AtiQuatCopy (&(pointArray[numPoints+0]), &(mNextPath->mOrientation[1]));
         if (mNextPath->mLocation[1].w == 0.0f)
         {
            AtiQuatCopy (&(pointArray[numPoints+1]), &(mNextPath->mOrientation[2]));
            if (mNextPath->mLocation[2].w == 0.0f)
            {
               AtiQuatCopy (&(pointArray[numPoints+2]), &(mNextPath->mOrientation[3]));
               if (mNextPath->mLocation[3].w == 0.0f)
               {
                  AtiQuatCopy (&(pointArray[numPoints+3]), &(mNextPath->mOrientation[4]));
               }
               else
               {
                  AtiQuatSlerpShortestPath (&(pointArray[numPoints+3]), &(pointArray[numPoints+1]), &(pointArray[numPoints+2]), 2.0f);
               }
            }
            else
            {
               AtiQuatSlerpShortestPath (&(pointArray[numPoints+2]), &(pointArray[numPoints+0]), &(pointArray[numPoints+1]), 2.0f);
               AtiQuatSlerpShortestPath (&(pointArray[numPoints+3]), &(pointArray[numPoints+1]), &(pointArray[numPoints+2]), 2.0f);
            }
         }
         else
         {
            AtiQuatSlerpShortestPath (&(pointArray[numPoints+1]), &(pointArray[numPoints-1]), &(pointArray[numPoints+0]), 2.0f);
            AtiQuatSlerpShortestPath (&(pointArray[numPoints+2]), &(pointArray[numPoints+0]), &(pointArray[numPoints+1]), 2.0f);
            AtiQuatSlerpShortestPath (&(pointArray[numPoints+3]), &(pointArray[numPoints+1]), &(pointArray[numPoints+2]), 2.0f);
         }
         numPoints += 4;
      }

      /*************************************************/
      /* Order quarternions for shortest path rotation */
      /*************************************************/
      for (j=0; j<(numPoints-1); j++)
      {
         if (AtiQuatDot(&(pointArray[j]), &(pointArray[j+1])) < 0.0f)
         {
            pointArray[j+1].x = -pointArray[j+1].x;
            pointArray[j+1].y = -pointArray[j+1].y;
            pointArray[j+1].z = -pointArray[j+1].z;
            pointArray[j+1].w = -pointArray[j+1].w;
         }
      }

      /************************************************/
      /* Move quaternions from S3 space into R4 space */
      /************************************************/
      for (j=0; j<numPoints; j++)
      {
         tmpf = 1.0f / (float32)sqrt(2.0*(1.0 - pointArray[j].w));
         pointArray[j].x *= tmpf;
         pointArray[j].y *= tmpf;
         pointArray[j].z *= tmpf;
         pointArray[j].w = (1.0f - pointArray[j].w) * tmpf;
      }

      /********************************************************/
      /* Create spline interval variables based on pointArray */
      /********************************************************/
      tmpD2[0].x = 0.0f;
      tmpD2[0].y = 0.0f;
      tmpD2[0].z = 0.0f;
      tmpD2[0].w = 0.0f;
      tmpIntD2[0].x = 0.0f;
      tmpIntD2[0].y = 0.0f;
      tmpIntD2[0].z = 0.0f;
      tmpIntD2[0].w = 0.0f;

      //Calculate second derivative for all points except first and last
      for (j=1; j<numPoints-1; j++) //For each point
      {
         for (k=0; k<4; k++) //For x, y, and z
         {
            float32 time  = 0.5f;
            float32 time2 = 1.0f;

            tmpf = (time * tmpD2[j-1].v[k]) + 2.0f;
            tmpD2[j].v[k] = (time - 1.0f) / tmpf;

            tmpf2 = ((pointArray[j+1].v[k] - pointArray[j].v[k]) / (time2)) - 
                    ((pointArray[j].v[k] - pointArray[j-1].v[k]) / (time2));

            tmpIntD2[j].v[k] = ((6.0f * tmpf2) / (time2+time2)) - (time * tmpIntD2[j-1].v[k]);
            tmpIntD2[j].v[k] /= tmpf;
         }
      }

      tmpD2[numPoints-1].x = 0.0f;
      tmpD2[numPoints-1].y = 0.0f;
      tmpD2[numPoints-1].z = 0.0f;
      tmpD2[numPoints-1].w = 0.0f;
      tmpIntD2[numPoints-1].x = 0.0f;
      tmpIntD2[numPoints-1].y = 0.0f;
      tmpIntD2[numPoints-1].z = 0.0f;
      tmpIntD2[numPoints-1].w = 0.0f;

      for (j=numPoints-2; j>=1; j--)
      {
         tmpD2[j].x = (tmpD2[j].x * tmpD2[j+1].x) + tmpIntD2[j].x;
         tmpD2[j].y = (tmpD2[j].y * tmpD2[j+1].y) + tmpIntD2[j].y;
         tmpD2[j].z = (tmpD2[j].z * tmpD2[j+1].z) + tmpIntD2[j].z;
         tmpD2[j].w = (tmpD2[j].w * tmpD2[j+1].w) + tmpIntD2[j].w;
      }

      for (j=4; j<numPoints-5; j++)
      {
         mOrientationIntervals[mNumSplineIntervals].lo.x = pointArray[j].x;
         mOrientationIntervals[mNumSplineIntervals].lo.y = pointArray[j].y;
         mOrientationIntervals[mNumSplineIntervals].lo.z = pointArray[j].z;
         mOrientationIntervals[mNumSplineIntervals].lo.w = pointArray[j].w;

         mOrientationIntervals[mNumSplineIntervals].hi.x = pointArray[j+1].x;
         mOrientationIntervals[mNumSplineIntervals].hi.y = pointArray[j+1].y;
         mOrientationIntervals[mNumSplineIntervals].hi.z = pointArray[j+1].z;
         mOrientationIntervals[mNumSplineIntervals].hi.w = pointArray[j+1].w;

         mOrientationIntervals[mNumSplineIntervals].lo2.x = tmpD2[j].x;
         mOrientationIntervals[mNumSplineIntervals].lo2.y = tmpD2[j].y;
         mOrientationIntervals[mNumSplineIntervals].lo2.z = tmpD2[j].z;
         mOrientationIntervals[mNumSplineIntervals].lo2.w = tmpD2[j].w;

         mOrientationIntervals[mNumSplineIntervals].hi2.x = tmpD2[j+1].x;
         mOrientationIntervals[mNumSplineIntervals].hi2.y = tmpD2[j+1].y;
         mOrientationIntervals[mNumSplineIntervals].hi2.z = tmpD2[j+1].z;
         mOrientationIntervals[mNumSplineIntervals].hi2.w = tmpD2[j+1].w;

         /********************/
         /* Update variables */
         /********************/
         mNumSplineIntervals++;
         mPathTime += mSampleInterval;
      }
   }

   /*********************************/
   /* Free temporary storage memory */
   /*********************************/
   AtiFreeArray(pointArray);
   AtiFreeArray(tmpD2);
   AtiFreeArray(tmpIntD2);

   return TRUE;
}

//=============================================================================
bool8 AtiFlyPath::GetPathOrientation (AtiQuaternion *aOrientation, float64 aTime)
{
   int32 tmpi;
   int32 interval;
   float32 intervalOffset;
   float64 tmpf64;
   float32 a;
   float32 b;
   float32 tmpf;
   AtiQuaternion tmpQuat;

   if (mInFlight == FALSE)
      return FALSE;

   /******************************/
   /* Take care of loop wrapping */
   /******************************/
   tmpf64 = aTime - mStartTime;
   if (tmpf64 >= mPathTime)
   {
      tmpi = (int32)(tmpf64/mPathTime);
      tmpf64 -= mPathTime * (float64)(tmpi);
   }

   /**********************************************************************************/
   /* Figure out which interval this time falls into and calculate normalized offset */
   /**********************************************************************************/
   interval = (int32)(tmpf64/mSampleInterval);
   intervalOffset = (float32)((tmpf64 - (mSampleInterval*(float64)(interval))) / mSampleInterval);

   a = 1.0f - intervalOffset;
   b = intervalOffset;

   tmpQuat.x = a*mOrientationIntervals[interval].lo.x + b*mO