Initial commit

Torque/SDK/engine/game/aiWheeledVehicle.cc

@@ -0,0 +1,343 @@
+//-----------------------------------------------------------------------------
+// Torque Game Engine 
+// Copyright (C) GarageGames.com, Inc.
+//-----------------------------------------------------------------------------
+
+#include "math/mMatrix.h"
+#include "math/mPoint.h"
+#include "core/realComp.h"
+#include "game/aiWheeledVehicle.h"
+
+IMPLEMENT_CO_NETOBJECT_V1(AIWheeledVehicle);
+
+
+EulerF extractEuler(const MatrixF & matrix)
+{
+    const F32 * mat = (const F32*)matrix;
+    
+    EulerF r;   
+    r.x = mAsin(mat[MatrixF::idx(2,1)]);
+
+    if(mCos(r.x) != 0.f)
+    {
+        r.y = mAtan(-mat[MatrixF::idx(2,0)], mat[MatrixF::idx(2,2)]);
+        r.z = mAtan(-mat[MatrixF::idx(0,1)], mat[MatrixF::idx(1,1)]);
+    }
+    else
+    {
+        r.y = 0.f;
+        r.z = mAtan(mat[MatrixF::idx(1,0)], mat[MatrixF::idx(0,0)]);
+    }
+
+    return(r);
+}
+
+AIWheeledVehicle::AIWheeledVehicle() :
+	WheeledVehicle()
+{
+   mMoveDestination.set( 0.0f, 0.0f, 0.0f );
+   mMoveSpeed = 1.0f;
+   mMoveTolerance = 0.25f;
+   mMoveSlowdown = true;
+}
+
+/**
+ * Sets the speed at which this AI moves
+ *
+ * @param speed Speed to move, default player was 10
+ */
+void AIWheeledVehicle::setMoveSpeed( F32 speed )
+{
+   mMoveSpeed = getMax(0.0f, getMin( 1.0f, speed ));
+}
+
+/**
+ * Stops movement for this AI
+ */
+void AIWheeledVehicle::stopMove()
+{
+   mMoveState = ModeStop;
+}
+
+/**
+ * Sets how far away from the move location is considered
+ * "on target"
+ * 
+ * @param tolerance Movement tolerance for error
+ */
+void AIWheeledVehicle::setMoveTolerance( const F32 tolerance )
+{
+   mMoveTolerance = getMax( 0.1f, tolerance );
+}
+
+/**
+ * Sets the location for the bot to run to
+ *
+ * @param location Point to run to
+ */
+void AIWheeledVehicle::setMoveDestination( const Point3F &location, bool slowdown )
+{
+   mMoveDestination = location;
+   mMoveState = ModeMove;
+   mMoveSlowdown = slowdown;
+}
+
+// Build a Triangle .. calculate angle of rotation required to meet target..
+// man there has to be a better way! >:)
+F32 AIWheeledVehicle::getSteeringAngle()
+{
+   // What is our target
+   Point3F desired;
+   desired=mMoveDestination;
+
+   MatrixF mat = getTransform();
+   Point3F center, front;
+   Point3F wFront;   
+   Box3F box = getObjBox();	   
+
+   box.getCenter(&center);
+   front=center;
+   front.y=box.max.y; // should be true for all these objects
+
+   getWorldBox().getCenter(&center);
+   front=center+front;
+
+   Point3F objFront=front;
+   Point3F offset = front - center;
+   EulerF rot;
+   rot=extractEuler(mat);
+   MatrixF transform(rot);
+   transform.mulV(offset, &wFront);
+   front = wFront + center;
+
+   Point3F ftoc;
+   ftoc.x=mFabs(front.x-center.x);
+   ftoc.y=mFabs(front.y-center.y);
+   ftoc.z=mFabs(front.z-center.z);
+   F32 fToc=mSqrt((ftoc.x*ftoc.x)+(ftoc.y*ftoc.y));
+
+   Point3F ltoc;
+   ltoc.x=mFabs(desired.x-center.x);
+   ltoc.y=mFabs(desired.y-center.y);
+   ltoc.z=mFabs(desired.z-center.z);
+   F32 lToc=mSqrt((ltoc.x*ltoc.x)+(ltoc.y*ltoc.y));
+
+   Point3F ftol;
+   ftol.x=mFabs(front.x-desired.x);
+   ftol.y=mFabs(front.y-desired.y);
+   ftol.z=mFabs(front.z-desired.z);
+   F32 fTol=mSqrt((ftol.x*ftol.x)+(ftol.y*ftol.y));
+   
+   F32 myAngle = mAcos(((lToc*lToc) + (fToc * fToc) - (fTol*fTol))/(2*lToc*fToc));
+   Point3F location = getPosition();
+
+   F32 xDiff = desired.x - location.x;
+   F32 yDiff = desired.y - location.y;
+
+   F32 finalYaw=mRadToDeg(myAngle);
+
+   F32 maxSteeringAngle=0;
+   
+	VehicleData *vd= (VehicleData*) getDataBlock();
+	maxSteeringAngle=vd->maxSteeringAngle;
+   
+//   if(finalYaw > 150)
+//      steerState = TurnAround;
+   if(finalYaw < 5 && mLastSteered != 0)
+      steerState = Straight;
+   else if(finalYaw < 5)
+      steerState = SteerNull;
+   else
+   {// Quickly Hack out left or right turn info
+      Point3F rotData=objFront-desired;
+      MatrixF leftM(-rot);
+      Point3F leftP;
+      leftM.mulV(rotData, &leftP);
+      leftP = leftP + desired;
+
+      if(leftP.x<desired.x)
+         steerState=Right;
+      else
+         steerState=Left;
+   }
+   
+   Point2F steering = mSteering;
+
+   F32 steer=0;
+   switch(steerState)
+   {
+      case SteerNull:
+         break;
+      case Left:
+         steer=myAngle < maxSteeringAngle ? -myAngle-steering.x: -maxSteeringAngle-steering.x;
+         mLastSteered=steer;
+         break;
+      case Right:
+         steer=myAngle < maxSteeringAngle ? myAngle-steering.x: maxSteeringAngle-steering.x;
+         mLastSteered=steer;
+         break;
+      case Straight:
+         steer=-steering.x;
+         mLastSteered=0;
+         break;
+      case TurnAround:
+         steer=maxSteeringAngle-steering.x;
+         mLastSteered=steer;
+         break;
+   };
+
+//   Con::printf("AI Steering : %f", steer);
+   return steer;
+}
+
+/**
+ * This method calculates the moves for the AI player
+ *
+ * @param movePtr Pointer to move the move list into
+ */
+bool AIWheeledVehicle::getAIMove(Move *movePtr)
+{
+   *movePtr = NullMove;
+
+   if (!mDisableMove) {
+		// Use the eye as the current position.
+		MatrixF eye;
+		getEyeTransform(&eye);
+		Point3F location = getPosition(); 
+		Point4F rotation;
+		getTransform().getColumn(1, &rotation);
+			
+		// Orient towards our destination.
+		if (mMoveState == ModeMove || mMoveState == ModeReverse) {      
+				movePtr->yaw = getSteeringAngle();
+		}
+
+		// Move towards the destination
+		if (mMoveState == ModeMove) {
+			F32 xDiff = mMoveDestination.x - location.x;
+			F32 yDiff = mMoveDestination.y - location.y;
+
+			// 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 (isZero(xDiff))
+					movePtr->y = (location.y > mMoveDestination.y)? -1 : 1;
+				else
+					if (isZero(yDiff))
+					movePtr->x = (location.x > mMoveDestination.x)? -1 : 1;
+					else
+					if (mFabs(xDiff) > mFabs(yDiff)) {
+						F32 value = mFabs(yDiff / xDiff);
+						movePtr->y = value; // (location.y > mMoveDestination.y)? -value : value;
+						movePtr->x = (location.x > mMoveDestination.x)? -1 : 1;
+					}
+					else {
+						F32 value = mFabs(xDiff / yDiff);
+						movePtr->x = (location.x > mMoveDestination.x)? -value : value;
+						movePtr->y = 1;  // (location.y > mMoveDestination.y)? -1 : 1;
+					}
+
+				// Rotate the move into object space (this really only needs
+				// a 2D matrix)
+				Point3F newMove;
+				MatrixF moveMatrix;
+				moveMatrix.set(EulerF(0, 0, -(rotation.z + movePtr->yaw)));
+				moveMatrix.mulV( Point3F( movePtr->x, movePtr->y, 0 ), &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;
+					if (dist < maxDist)
+					speed *= dist / maxDist;
+					movePtr->x *= speed;
+					movePtr->y *= speed;
+				}
+				else {
+					movePtr->x *= mMoveSpeed;
+					movePtr->y *= mMoveSpeed;
+				}
+
+				// We should check to see if we are stuck...
+				if (location == mLastLocation) {
+					throwCallback("onMoveStuck");
+					mMoveState = ModeStop;
+				}
+			}
+		}
+
+		// Replicate the trigger state into the move so that
+		// triggers can be controlled from scripts.
+
+		for( int i = 0; i < MaxTriggerKeys; i++ )
+			movePtr->trigger[i] = getImageTriggerState(i);
+
+   } else {
+	   mRigid.setAtRest();
+   }
+
+   return true;
+}
+
+/**
+ * Utility function to throw callbacks. Callbacks always occure
+ * on the datablock class.
+ *
+ * @param name Name of script function to call
+ */
+void AIWheeledVehicle::throwCallback( const char *name )
+{
+   Con::executef(getDataBlock(), 2, name, scriptThis());
+}
+
+
+// --------------------------------------------------------------------------------------------
+// Console Functions
+// --------------------------------------------------------------------------------------------
+
+ConsoleMethod( AIWheeledVehicle, stop, void, 2, 2, "()"
+              "Stop moving.")
+{
+   object->stopMove();
+}
+
+ConsoleMethod( AIWheeledVehicle, setMoveSpeed, void, 3, 3, "( float speed )"
+              "Sets the move speed for an AI object.")
+{
+   object->setMoveSpeed( dAtof( argv[2] ) );
+}
+
+ConsoleMethod( AIWheeledVehicle, setMoveTolerance, void, 3, 3, "(float speed)" "Sets the movetolerance")
+{
+	object->setMoveTolerance(dAtof(argv[2]));
+}
+
+ConsoleMethod( AIWheeledVehicle, setMoveDestination, void, 3, 4, "(Point3F goal, bool slowDown=true)"
+              "Tells the AI to move to the location provided.")
+{
+   Point3F v( 0.0f, 0.0f, 0.0f );
+   dSscanf( argv[2], "%f %f %f", &v.x, &v.y, &v.z );
+   bool slowdown = (argc > 3)? dAtob(argv[3]): true;
+   object->setMoveDestination( v, slowdown);
+}
+
+ConsoleMethod( AIWheeledVehicle, getMoveDestination, const char *, 2, 2, "()"
+              "Returns the point the AI is set to move to.")
+{
+   Point3F movePoint = object->getMoveDestination();
+
+   char *returnBuffer = Con::getReturnBuffer( 256 );
+   dSprintf( returnBuffer, 256, "%f %f %f", movePoint.x, movePoint.y, movePoint.z );
+
+   return returnBuffer;
+}
+
+