Calculate target bearing

#1 Are you thinking of dot products?

#2 You need to:
1. calculate the target and firer coordinates in world coordinates (the position field)
2. bearing vector in world coordinates = target minus firer positions
3. rotate the bearing vector into the firers object coordinates.
4. place a vector down the center of each firing arc (in firer object coordinates: e.g.: "0 1 0" for dead ahead), and define the maximum angle from that firing arc center to the farthest angle of traverse off center (the "half cone angle" of the firing arc)
5. calculate the angle between the bearing vector to the center of the firing arc vector (in object coordinates). If this is less than the half cone angle, then you are in that arc.

See the Geometric Interpretation section of en.wikipedia.org/wiki/Dot_product for info on calculating the angle between two vectors using the dot product.


It may sound like a lot in an English description rather than mathematical formulas, but it is just:
a vector subtraction,
multiply it by a matrix,
then perform a dot product and a scalar comparison for each firing arc you want to check (Store the cosine of the half cone angle so you don't have to perform the arc cosine)

#3 I've been playing with the dot product (found it in guiShapenameHUD of all places). This is so required, I'm surprised there is no resource already. I'll report on this and post any code that comes out of this (If anyone else has code, feel free to post).

#4 Not the Final update:
I ripped the code out of GUIShapeBaseHUD and created a standalone function that takes two shapes and calculates the target bearing.

OK, I thought this was going to be the final update, but I guess not.

My function:

S32 ShapeBase::getBearing(ShapeBase* base, ShapeBase* target)
{
...
   return bearing;
}

needs a console function.

I tried:

ConsoleFunction (ShapeBase, getBearing, 3, 3 ,"")
{
   return object->getBearing();
}

to no avail. (Added shapeBase* base and ShapeBase* target to body of function and returns 0. The getBearing function works great, I just want to call it from script.


Any ideas on this? I looked through the existing code and not really anything on passing one or two shapes to a console function.

#5 Shouldn't you be using a console Method?


ConsoleMethod (ShapeBase, getBearing, S32, 3, 3 ,""){ return object->getBearing();}

#6 Hmm... my mistake. I AM using ConsoleMethod. I just need to know how to call it.

Here is the base function in full:

S32 ShapeBase::getBearing(ShapeBase* base, ShapeBase* target)
{
   Point3F shapePos;
   Point3F basePos;

   VectorF baseDir;
   VectorF shapeDir;
	   
   MatrixF ctlMat = base->getRenderTransform();
   MatrixF srtMat = target->getRenderTransform();
	   
   ctlMat.getColumn(3, &basePos);
   ctlMat.getColumn(1, &baseDir);
   srtMat.getColumn(3, &shapePos);
   
   shapeDir = shapePos - basePos;
   
   shapeDir.normalize();
   return abs((S32)(mDot(shapeDir, baseDir) * 360) % 360);
}

#7 I would look through shapebase.cc for similar ConsoleMethod definitions:

ConsoleMethod( ShapeBase, mountObject, bool, 4, 4, "( ShapeBase object, int slot )"
              "Mount ourselves on an object in the specified slot.")
{
   ShapeBase *target;
   if (Sim::findObject(argv[2],target)) {
      S32 node = -1;
      dSscanf(argv[3],"%d",&node);
      if (node >= 0 && node < ShapeBaseData::NumMountPoints)
         object->mountObject(target,node);
      return true;
   }
   return false;
}

This one takes two parameters, one of which is a script object id.

#8 I'm confused about your last line:

return abs((S32)(mDot(shapeDir, baseDir) * 360) % 360);

mDot returns a dot product = the cosine of the angle between the two unit vectors
For the positive angle between the vectors expressed in integer degrees, something like this may work:

F32 angle = mAcos(mClampF(mDot(shapeDir, baseDir)),-1.0f, 1.0f); // ranged 0..PI radians
return (S32)mFloor(mRadToDeg(angle) + 0.5); // rounds to nearest degree

The clamp protects against a low probability but fatal condition where the arc cosine crashes by being fed a value outside the range -1 to 1.

Warning: I didn't compile this code or test it.

#9 Personally i would skip the clamp() - shapeDir was normalized in the line right before, and basedir is taken from the rotation matrix of the object, and if that's not unit length you've got bigger problems on your hands.

#10 I was curious to see why I was terrified of not clamping. (See test routine below.)

==>acostest(1000000);
===== acosTest results:
Probability random: 0.084 after normalize: 0.266 failed after clamp: 0.000

I took 1 million random vectors (generated by random pitch and yaw angles and converting to a 3 vector). This should be similar to the row or column of a rotation matrix. I then took the dot product of this vector with themselves. (Admitedly, probably a worst case. But hey, how often does a character try to face directly at a target anyway? ;) ) This should result in a dot product value of 1.0.

8.4% of the time it results in something like 1.0000001 which when input to the mAcos returns an indefinite.

26.6% of the time if it passes the first test, it still failed if you normalize it!

(Note: this doesn't necessarily say that normalization itself is much worse than non-normalized. I should have just tested for that case alone rather than combining the conditions. )

In the 266000 some cases where a normalization results in a vec dot vec being slightly larger than 1.0, NONE slipped through the mClampF. (Which is good. ;) )

In short, if you don't want the occasional idefinite creeping into your running program:
always range check basic and trigonometric functions that require it. (sqrt(-1), arccos, arcsin, atan2(0,0), etc.)

Note: this uses my own Pseudo Random Number generator. You will have to replace this macro with your own, or Torque's method.

// returns:
// 0 if passes test
// 1 if fails from sin,cos setup
// 2 if fails only after normalize, 
// 3 if fails after normalize AND if mClampF fails to keep it within +/- 1.0
S32 doAcosTest(void) 
{
   // do two random yaw+pitch unit vectors
   // dot product is it > 1.0? 
   // renormalize both vectors. 
   // dot product is it > 1.0? 
   // if a failure is found, note the inputs.

   Point3F vec0;
   Point3F vec0n;
   F32 yawAng;
   F32 pitchAng;
   F32 cosP;

   yawAng  = mDegToRad(PRN_GET_UNIFORM_IN_RANGE(0.0f,  360.0f));
   pitchAng= mDegToRad(PRN_GET_UNIFORM_IN_RANGE(-90.0f, 90.0f));

   // from my version of getVectorFromAngles
   cosP = mCos(pitchAng);
   vec0.x = mSin(yawAng) * cosP;
   vec0.y = mCos(yawAng) * cosP;
   vec0.z = mSin(pitchAng);

   F32 dot = mDot(vec0,vec0);
   if (dot > 1.0f) {
      F32 ang = mAcos(dot);

      return 1;
   } else {
      vec0n = vec0;
      vec0n.normalizeSafe();
      dot =  mDot(vec0n,vec0n);
      if (dot > 1.0) {
         F32 clampDot = mClampF(dot,-1.0f,1.0f);
         if (clampDot > 1.0f || clampDot < -1.0f) {
            return 3;
         } else {
            return 2;
         }
      }
   }

   return 0;
}

ConsoleFunction( acosTest, void, 2, 2, "test num" )
{
   S32 num = argc == 2 ? dAtoi(argv[1]) : 10000;

   S32 numFailures1 = 0;
   S32 numFailures2 = 0;
   S32 numFailures3 = 0;
   for (S32 i=0;i<num;i++) {
      switch (doAcosTest()) {
      case 0:
         // NOP
         break;
      case 1:
         numFailures1++;
         break;
      case 2:
         numFailures2++;
         break;
      case 3:
         numFailures3++;
         break;
      }
   }

   F32 probability1 = (F32)numFailures1 / (F32)num;
   F32 probability2 = (F32)(numFailures2 + numFailures3) / (F32)num;
   F32 probability3 = (F32)numFailures3 / (F32)num;
   
   Con::printf("===== acosTest results:");
   char buffer[1024];
   dSprintf(buffer, 1024, "Probability  random: %7.3f     after normalize: %7.3f      failed after clamp: %7.3f ",probability1,probability2,probability3);
   Con::printf(buffer);

}

#11 Oh crap, you're right!
wow, that's totally surprising to me.

i would have expected the following snippet to produce a valid float 100% of the time,
but in fact it produces NAN about 25% of the time!

// vec0n     = a moderately well-conditioned vector in some "random" direction.
    vec0n.normalizeSafe();
    F32 dot   = mDot(vec0n, vec0n);
    F32 acosd = mAcos(dot);

here's a modification of your test which is a bit easier for me to grok:

S32 doAcosTestB(void) 
{
   Point3F vec0;
   Point3F vec0n;

   // choose a random point in a cube centered on the origin and project it onto the unit sphere
   vec0.x    = mRandF(-1.0f, 1.0f);
   vec0.y    = mRandF(-1.0f, 1.0f);
   vec0.z    = mRandF(-1.0f, 1.0f);

   vec0n     = vec0;
   vec0n.normalizeSafe();

   F32 dot   = mDot(vec0n, vec0n);

   F32 acosd = mAcos(dot);

   if (!(acosd <= 5.0f) && !(acosd > 5.0f))
   {
//    Con::printf("NAN! %f %f", acosd, dot);
      return 0;
   }

   return 1;
}

ConsoleFunction( acosTest, void, 3, 3, "count" )
{
   S32 num  = dAtoi(argv[1]);
   S32 pass = 0;

   for (S32 n = 0; n < num; n++)
   {
      pass += doAcosTestB();
   }

   Con::printf("pass = %d   NAN = %d   percent NAN = %1.3f", pass, num - pass, (F32)(num - pass) / (F32)num);
}

#12 I changed my getBearing Function to match the clamped version (the retruned results were identical to my kludge.) However, I didn't notice thats not exactly what I need.

I need the values to go from 0.0 to 359.9 degrees. This function gives the offset from 0 (so there is no way to distinguish between the Port and Starborad Arcs). For the Forward and Aft Arcs, the function works great, as it is symmetrical.

#13 I placed the code in AIPlayer where it worked a lot better. I still need to figure out a pure bearing to target function. Once complete I think a resource is needed.

#14 Viewing the javascript source to this page might be illuminating.

#15 Here is my ConsoleFunction that works:

ConsoleMethod( AIFlyingVehicle, getAimBearing, F32, 3, 3, "Gets the object bearing the AI is targeting.")
{
	AIFlyingVehicle *ai = static_cast<AIFlyingVehicle *>( object );
	GameBase* obj = ai->getAimObject();
	F32 aimBearing; // = ai->getAimBearing();
	Point3F shapePos;
	Point3F basePos;
    VectorF baseDir;
    VectorF shapeDir;
	MatrixF ctlMat = ai->getRenderTransform();
    MatrixF srtMat = obj->getRenderTransform();
	   
    ctlMat.getColumn(3, &basePos);
    ctlMat.getColumn(1, &baseDir);
    srtMat.getColumn(3, &shapePos);
   
    shapeDir = shapePos - basePos;
    shapeDir.normalize(); 
    aimBearing = mFloor(mRadToDeg(mAcos(mClampF(mDot(shapeDir, baseDir), -1.0f, 1.0f)) + 0.5)); // ranged 0..PI radians
    return aimBearing;
}

#16 I still have to figure out/ integrate the Javascript "fix" for it to work 100%.

#17 Fixing my own mess.

F32 ShapeBase::getBearing(ShapeBase* base, ShapeBase* target)
{
   Point3F targPos;
   Point3F basePos;
   VectorF baseDir;
   VectorF targDir;
   
   Point3F forward;
   Point3F up;
   Point3F right;
   
   F32 bearing; 	   
   
   MatrixF baseMat = base->getRenderTransform();
   MatrixF targMat = target->getRenderTransform();
	   
   baseMat.getColumn(2, &up);
   baseMat.getColumn(3, &basePos);
   
   baseMat.getColumn(1, &baseDir);
   targMat.getColumn(3, &targPos);

   baseDir.normalize();
   up.normalize();
  
   mCross( baseDir, up, right );
   right.normalize();

   targPos.z = 0; // We don't care about the Z-Axis for bearing.
   basePos.z = 0;

   targDir = targPos - basePos;
   targDir.normalize();

   F32 dotTurn = mDot( right, targDir );

   if (!isZero(targDir.x) || !isZero(targDir.y)) 
   {
      if (dotTurn < 0) bearing = -mRadToDeg(dotTurn);
	  else { bearing = 360.0f - mRadToDeg(dotTurn); }; 
   }
   else
   {
   	  bearing = 0.0f;
   }
  
   return bearing;
}