Faster than the Speed of Pythagoras

To determine the distance away from a route point, you can use the standard
Pythagorean Theorem, but in order to speed things up, you can toss the sqrt
operation out the door and use the sum of the squares of the lengths instead.
To see what I mean, take a look at the following two lines of code:

float Distance = sqrt(Length1*Length1 + Length2*Length2);
float Distance = Length1*Length1 + Length2*Length2;

Notice that the preceding two lines of code are almost identical, except the second
line omits the sqrt function, making the second line execute much faster. The
downside is that you don’t get the exact length, which really isn’t a problem.

NOTE
The Pythagorean Theorem is probably the most famous theorem in geometry. It states that the
square of the length of the hypotenuse of a right triangle is equal to the sum of the squares of the
lengths of the sides. Basically, it means that the square root of the lengths of two sides (when
both are squared and added together) equals the length of the third side of a right triangle.

For example, imagine that you are measuring the distance between two points and
you want to see whether that distance is less than 40. If the coordinates of the two
points are 0,0 and 30,20, the faster distance calculation will give you a distance of
1,300 (because the length of the two sides are 30 and 20, respectively).

How can you determine the distance now? By calculating the square (the number
times itself) of the distance, that’s how! So, by taking 40 times 40, you get 1,600. By
comparing the distance of 1,300 between the points, you can see that indeed the
distance is less than 1,600 and, thus, less than the original distance of 40 you were
checking.

To get back to what I was originally talking about, you can use the faster method
of distance calculation to determine when a character is close enough to a route
point. Say that you want a route point considered as being touched by a character
if that character comes within so many units from it. Utilizing the faster method of
distance calculation, you can use the following function to determine whether that
is the case:

BOOL TouchedRoutePoint(
  float CharXPos, float CharZPos, // Character coordinates
  float RouteXPos, float RouteZPos, // Route point coordinates
  float Distance) // Distance to check
{
  // Square the distance to adjust for faster distance checking
  Distance *= Distance;

  // Now calculate the distance
  float XDiff = (float)fabs(RouteXPos - CharXPos);
  float ZDiff = (float)fabs(RouteZPos - CharZPos);
  float Dist = XDiff*XDiff + ZDiff*ZDiff;

  // Return results
  if(Dist <= Distance) // Within range being checked
    return TRUE;

  return FALSE; // Out of distance range
}

When calling TouchedRoutePoint with the character coordinates, the coordinates of
the route point, and the distance from the point to check, you will receive a value
of TRUE if the character is within Distance units from the route point coordinates.
A return value of FALSE means that the character is not within Distance units from
the route point.

Walking the Route
 

At long last, you can put everything together and force a character to walk from
one route point to the next. Here’s a small program that takes the five route points
defined previously and puts a character at point one, forcing the character to walk
from point to point forever:

sRoutePoint Route[5] = {
  { -200.0f, -100.0f },
  { 100.0f, -300.0f },
  { 300.0f, -200.0f },
  { 200.0f, 100.0f },
  { 0.0f, 400.0f }
};

long NumRoutePoints = 5;

// Character coordinates and movement variables
float CharXPos = Route[0].XPos;
float CharZPos = Route[0].ZPos;
float MoveX, MoveZ;
float Speed; // Walking speed of character

// Start track to 2nd point
long TargetRoutePoint = 1;
SetupMovement(TargetRoutePoint);

// Loop forever, moving and checking for route points reached
while(1) {
  // Is character within range of route point?
  if(TouchedRoutePoint(TargetRoutePoint, 32.0f) == TRUE) {
    // Move to next route point
    TargetRoutePoint++;
    if(TargetRoutePoint >= NumRoutePoints)
    TargetRoutePoint = 0;
    SetupMovement(TargetRoutePoint);
  }

  // Move character
  CharXPos += MoveX;
  CharZPos += MoveZ;
}

// Function to check if within range of route point
BOOL TouchedRoutePoint(long PointNum, float Distance)
{
  Distance *= Distance;
  float XDiff = (float)fabs(CharXPos - Route[PointNum].XPos);
  float ZDiff = (float)fabs(CharZPos - Route[PointNum].ZPos);
  float Dist = XDiff*XDiff + ZDiff*ZDiff;

  if(Dist <= Distance)
    return TRUE;
 

  return FALSE;
}

// Function to calculate movement variables
void SetupMovement(long PointNum)
{
  float XDiff = (float)fabs(CharXPos - Route[PointNum].XPos);
  float ZDiff = (float)fabs(CharZPos - Route[PointNum].ZPos);
  float Length = sqrt(XDiff*XDiff + ZDiff*ZDiff);

  MoveX = (Route[PointNum].XPos - CharXPos) / Length * Speed;
  MoveZ = (Route[PointNum].ZPos - CharZPos) / Length * Speed;
}