Swimming AI Bots and setMoveDestination

%transform=$player.getTransform();
%Spos=getWord(%transform,0) SPC getWord(%transform,1) SPC getWord(%transform,2);

%obj.setMoveDestination(%Spos);

bool AIBot::getAIMove(Move *movePtr)
{
   *movePtr = NullMove;

   // Use the eye as the current position.
   MatrixF eye;
   getEyeTransform(&eye);
   Point3F location = eye.getPosition();
   Point3F rotation = getRotation();

   // Orient towards the aim point, aim object, or towards
   // our destination.
   if (mAimObject || mAimLocationSet || mMoveState == ModeMove) {

      // Update the aim position if we're aiming for an object
      if (mAimObject)
         mAimLocation = mAimObject->getPosition() + mAimOffset;
      else
         if (!mAimLocationSet)
            mAimLocation = mMoveDestination;

      Point3F vDiff = mAimLocation - location;
	  vDiff.normalize();
	  mOrientation.normalize();
			
	  mOrientation.interpolate(mOrientation,vDiff,mTurningSpeed);

	  F32 xDiff = mOrientation.x;
	  F32 yDiff = mOrientation.y;
	  
      if (!isZero(xDiff) || !isZero(yDiff)) {

         // First do Yaw
         // use the cur yaw between -Pi and Pi
         F32 curYaw = rotation.z;
         while (curYaw > M_2PI)
            curYaw -= M_2PI;
         while (curYaw < -M_2PI)
            curYaw += M_2PI;

         // find the yaw offset
         F32 newYaw = mAtan( xDiff, yDiff );
         F32 yawDiff = newYaw - curYaw;

         // make it between 0 and 2PI
         if( yawDiff < 0.0f )
            yawDiff += M_2PI;
         else if( yawDiff >= M_2PI )
            yawDiff -= M_2PI;

         // now make sure we take the short way around the circle
         if( yawDiff > M_PI )
            yawDiff -= M_2PI;
         else if( yawDiff < -M_PI )
            yawDiff += M_2PI;

         movePtr->yaw = yawDiff;

         // Next do pitch.
         if (!mAimObject && !mAimLocationSet) {
            // Level out if were just looking at our next way point.
            Point3F headRotation = getHeadRotation();
            movePtr->pitch = -headRotation.x;
         }
         else {
            // This should be adjusted to run from the
            // eye point to the object's center position. Though this
            // works well enough for now.
            F32 vertDist = mAimLocation.z - location.z;
            F32 horzDist = mSqrt(xDiff * xDiff + yDiff * yDiff);
            F32 newPitch = mAtan( horzDist, vertDist ) - ( M_PI / 2.0f );
            if (mFabs(newPitch) > 0.01) {
               Point3F headRotation = getHeadRotation();
               movePtr->pitch = newPitch - headRotation.x;
            }
         }
      }
   }
   else {
      // Level out if we're not doing anything else
      Point3F headRotation = getHeadRotation();
      movePtr->pitch = -headRotation.x;
   }

   // Move towards the destination
   if (mMoveState == ModeMove) {
      F32 xDiff = mMoveDestination.x - location.x;
      F32 yDiff = mMoveDestination.y - location.y;
      F32 zDiff = mMoveDestination.z - location.z;

      // Check if we should mMove, or if we are 'close enough'
      if (mFabs(xDiff) < mMoveTolerance && mFabs(yDiff) < mMoveTolerance  && mFabs(zDiff) < mMoveTolerance) { //  && mFabs(zDiff) < mMoveTolerance
         mMoveState = ModeStop;
         throwCallback("onReachDestination");
      }
      else {
         // Build move direction in world space
		//====================================================================================
		// Movement Calculation with Z Axis
				if (isZero(xDiff))
				{
					if (mFabs(yDiff)>mFabs(zDiff))
					{
						F32 value = mFabs(zDiff / yDiff);
						movePtr->z = (location.z > mMoveDestination.z)? -value : value;
						movePtr->y = (location.y > mMoveDestination.y)? -1 : 1;
					}
					else
					{
						F32 value = mFabs(yDiff / zDiff);
						movePtr->y = (location.y > mMoveDestination.y)? -value : value;
						movePtr->z = (location.z > mMoveDestination.z)? -1 : 1;
					}
				}
				else
				if (isZero(yDiff))
				{
					if (mFabs(xDiff)>mFabs(zDiff))
					{
						F32 value = mFabs(zDiff / xDiff);
						movePtr->z = (location.z > mMoveDestination.z)? -value : value;
						movePtr->x = (location.x > mMoveDestination.x)? -1 : 1;
					}
					else
					{
						F32 value = mFabs(xDiff / zDiff);
						movePtr->x = (location.x > mMoveDestination.x)? -value : value;
						movePtr->z = (location.z > mMoveDestination.z)? -1 : 1;
					}
				}
				else
				if (isZero(zDiff))
				{
					if (mFabs(yDiff)>mFabs(xDiff))
					{
						F32 value = mFabs(xDiff / yDiff);
						movePtr->x = (location.x > mMoveDestination.x)? -value : value;
						movePtr->y = (location.y > mMoveDestination.y)? -1 : 1;
					}
					else
					{
						F32 value = mFabs(yDiff / xDiff);
						movePtr->y = (location.y > mMoveDestination.y)? -value : value;
						movePtr->x = (location.x > mMoveDestination.x)? -1 : 1;
					}
				}
				else
				if ( mFabs(xDiff)>=mFabs(yDiff) && mFabs(xDiff)>=mFabs(zDiff) )
				{
					F32 value1 = mFabs(yDiff / xDiff);
					F32 value2 = mFabs(zDiff / xDiff);

					movePtr->y = (location.y > mMoveDestination.y)? -value1 : value1;
					movePtr->z = (location.z > mMoveDestination.z)? -value2 : value2;
					movePtr->x = (location.x > mMoveDestination.x)? -1 : 1;
				}
				else
				if ((mFabs(yDiff)>=mFabs(xDiff)) && (mFabs(yDiff)>=mFabs(zDiff)))
				{
					F32 value1 = mFabs(xDiff / yDiff);
					F32 value2 = mFabs(zDiff / yDiff);

					movePtr->x = (location.x > mMoveDestination.x)? -value1 : value1;
					movePtr->z = (location.z > mMoveDestination.z)? -value2 : value2;
					movePtr->y = (location.y > mMoveDestination.y)? -1 : 1;
				}
				else
				if ((mFabs(zDiff)>=mFabs(xDiff)) && (mFabs(zDiff)>=mFabs(yDiff)))
				{
					F32 value1 = mFabs(xDiff / zDiff);
					F32 value2 = mFabs(yDiff / zDiff);

					movePtr->x = (location.x > mMoveDestination.x)? -value1 : value1;
					movePtr->y = (location.y > mMoveDestination.y)? -value2 : value2;
					movePtr->z = (location.z > mMoveDestination.z)? -1 : 1;
				}	
        // Rotate the move into object space 
         Point3F newMove;
         MatrixF moveMatrix;
         moveMatrix.set(EulerF(0, 0, 0));
         moveMatrix.mulV( Point3F( movePtr->x, movePtr->y, movePtr->z ), &newMove ); 
         movePtr->x = -newMove.x;
         movePtr->y = -newMove.y;
	 movePtr->z = -newMove.z;

         // 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;
            if (dist < maxDist)
               speed *= dist / maxDist;
            movePtr->x *= speed;
            movePtr->y *= speed;
	    movePtr->z *= speed;
         }
		 else {
            movePtr->x *= mMoveSpeed;
            movePtr->y *= mMoveSpeed;
	    movePtr->z *= mMoveSpeed;
		 }

         // We should check to see if we are stuck...
         if (location == mLastLocation) {
            throwCallback("onMoveStuck");
            mMoveState = ModeStop;
         }
      }
   }

   if (mAimObject) {
      MatrixF eyeMat;
      getEyeTransform(&eyeMat);
      eyeMat.getColumn(3,&location);
      Point3F targetLoc = mAimObject->getBoxCenter();

      RayInfo dummy;
      if (getContainer()->castRay( location, targetLoc,
            InteriorObjectType | StaticShapeObjectType | StaticObjectType |
            TerrainObjectType, &dummy)) {
         if (mTargetInLOS) {
            throwCallback( "onTargetExitLOS" );
            mTargetInLOS = false;
         }
      }
      else
         if (!mTargetInLOS) {
            throwCallback( "onTargetEnterLOS" );
            mTargetInLOS = true;
         }
   }

   for( int i = 0; i < MaxTriggerKeys; i++ )
      movePtr->trigger[i] = getImageTriggerState(i);

   return true;
}