lineintersect_utils.cpp

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

|             parameter       - Parametric coordinate of the nearest point along the
|                               line/segment. parameter = 0 at (A1x, A1y, A1z) and
|                               parameter = 1 at the second defining point of the line/
|                               segmetn
**************************************************************************/
void FindNearestPointOnLineSegment(const float A1x, const float A1y, const float A1z,
                                   const float Lx, const float Ly, const float Lz,
                                   const float Bx, const float By, const float Bz,
                                   bool infinite_line, float epsilon_squared, float &NearestPointX,
                                   float &NearestPointY, float &NearestPointZ,
                                   float &parameter)
{
// Line/Segment is degenerate --- special case #1
  float D = Lx * Lx + Ly * Ly + Lz * Lz;
  if (D < epsilon_squared)
  {
    NearestPointX = A1x;
    NearestPointY = A1y;
    NearestPointZ = A1z;
    return;
  }

  float ABx = Bx - A1x;
  float ABy = By - A1y;
  float ABz = Bz - A1z;

// parameter is computed from Equation (20).
  parameter = (Lx * ABx + Ly * ABy + Lz * ABz) / D;

  if (false == infinite_line) parameter = FMAX(0.0f, FMIN(1.0f, parameter));

  NearestPointX = A1x + parameter * Lx;
  NearestPointY = A1y + parameter * Ly;
  NearestPointZ = A1z + parameter * Lz;
  return;
}

/**************************************************************************
|
|     Method: FindNearestPointOfParallelLineSegments
|
|    Purpose: Given two lines (segments) that are known to be parallel, find
|             a representative point on each that is nearest to the other. If
|             the lines are considered to be finite then it is possible that there
|             is one true point on each line that is nearest to the other. This
|             code properly handles this case.
|
|             This is the most difficult line intersection case to handle, since
|             there is potentially a family, or locus of points on each line/segment
|             that are nearest to the other.
| Parameters: Input:
|             ------
|             A1x, A1y, A1z   - Coordinates of first defining point of line/segment A
|             A2x, A2y, A2z   - Coordinates of second defining point of line/segment A
|             Lax, Lay, Laz   - Vector from (A1x, A1y, A1z) to the (A2x, A2y, A2z).
|             B1x, B1y, B1z   - Coordinates of first defining point of line/segment B
|             B2x, B2y, B2z   - Coordinates of second defining point of line/segment B
|             Lbx, Lby, Lbz   - Vector from (B1x, B1y, B1z) to the (B2x, B2y, B2z).
|             infinite_lines  - set to true if lines are to be treated as infinite
|             epsilon_squared - tolerance value to be used to check for degenerate
|                               and parallel lines, and to check for true intersection.
|
|             Output:
|             -------
|             PointOnSegAx,   - Coordinates of the point on segment A that are nearest
|             PointOnSegAy,     to segment B. This corresponds to point C in the text.
|             PointOnSegAz
|             PointOnSegBx,   - Coordinates of the point on segment B that are nearest
|             PointOnSegBy,     to segment A. This corresponds to point D in the text.
|             PointOnSegBz

**************************************************************************/
void FindNearestPointOfParallelLineSegments(float A1x, float A1y, float A1z,
                                            float A2x, float A2y, float A2z,
                                            float Lax, float Lay, float Laz,
                                            float B1x, float B1y, float B1z,
                                            float B2x, float B2y, float B2z,
                                            float Lbx, float Lby, float Lbz,
                                            bool infinite_lines, float epsilon_squared,
                                            float &PointOnSegAx, float &PointOnSegAy, float &PointOnSegAz,
                                            float &PointOnSegBx, float &PointOnSegBy, float &PointOnSegBz)
{
  float s[2], temp;
  FindNearestPointOnLineSegment(A1x, A1y, A1z, Lax, Lay, Laz, B1x, B1y, B1z,
                                true, epsilon_squared, PointOnSegAx, PointOnSegAy, PointOnSegAz, s[0]);
  if (true == infinite_lines)
  {
    PointOnSegBx = B1x;
    PointOnSegBy = B1y;
    PointOnSegBz = B1z;
  }
  else
  {
    float tp[3];
    FindNearestPointOnLineSegment(A1x, A1y, A1z, Lax, Lay, Laz, B2x, B2y, B2z,
                                  true, epsilon_squared, tp[0], tp[1], tp[2], s[1]);
    if (s[0] < 0.f && s[1] < 0.f)
    {
      PointOnSegAx = A1x;
      PointOnSegAy = A1y;
      PointOnSegAz = A1z;
      if (s[0] < s[1])
      {
        PointOnSegBx = B2x;
        PointOnSegBy = B2y;
        PointOnSegBz = B2z;
      }
      else
      {
        PointOnSegBx = B1x;
        PointOnSegBy = B1y;
        PointOnSegBz = B1z;
      }
    }
    else if (s[0] > 1.f && s[1] > 1.f)
    {
      PointOnSegAx = A2x;
      PointOnSegAy = A2y;
      PointOnSegAz = A2z;
      if (s[0] < s[1])
      {
        PointOnSegBx = B1x;
        PointOnSegBy = B1y;
        PointOnSegBz = B1z;
      }
      else
      {
        PointOnSegBx = B2x;
        PointOnSegBy = B2y;
        PointOnSegBz = B2z;
      }
    }
    else
    {
      temp = 0.5f*(FMAX(0.0f, FMIN(1.0f, s[0])) + FMAX(0.0f, FMIN(1.0f, s[1])));
      PointOnSegAx = (A1x + temp * Lax);
      PointOnSegAy = (A1y + temp * Lay);
      PointOnSegAz = (A1z + temp * Laz);
      FindNearestPointOnLineSegment(B1x, B1y, B1z, Lbx, Lby, Lbz,
                                    PointOnSegAx, PointOnSegAy, PointOnSegAz, true,
                                    epsilon_squared, PointOnSegBx, PointOnSegBy, PointOnSegBz, temp);
    }
  }
}

/**************************************************************************
|
|     Method: AdjustNearestPoints
|
|    Purpose: Given nearest point information for two infinite lines, adjust
|             to model finite line segments.
|
| Parameters: Input:
|             ------
|             A1x, A1y, A1z   - Coordinates of first defining point of line/segment A
|             Lax, Lay, Laz   - Vector from (A1x, A1y, A1z) to the (A2x, A2y, A2z).
|             B1x, B1y, B1z   - Coordinates of first defining point of line/segment B
|             Lbx, Lby, Lbz   - Vector from (B1x, B1y, B1z) to the (B2x, B2y, B2z).
|             epsilon_squared - tolerance value to be used to check for degenerate
|                               and parallel lines, and to check for true intersection.
|             s               - parameter representing nearest point on infinite line A
|             t               - parameter representing nearest point on infinite line B
|
|             Output:
|             -------
|             PointOnSegAx,   - Coordinates of the point on segment A that are nearest
|             PointOnSegAy,     to segment B. This corresponds to point C in the text.
|             PointOnSegAz
|             PointOnSegBx,   - Coordinates of the point on segment B that are nearest
|             PointOnSegBy,     to segment A. This corresponds to point D in the text.
|             PointOnSegBz
**************************************************************************/
void AdjustNearestPoints(float A1x, float A1y, float A1z,
                         float Lax, float Lay, float Laz,
                         float B1x, float B1y, float B1z,
                         float Lbx, float Lby, float Lbz,
                         float epsilon_squared, float s, float t,
                         float &PointOnSegAx, float &PointOnSegAy, float &PointOnSegAz,
                         float &PointOnSegBx, float &PointOnSegBy, float &PointOnSegBz)
{
// handle the case where both parameter s and t are out of range
  if (OUT_OF_RANGE(s) && OUT_OF_RANGE(t))
  {
    s = FMAX(0.0f, FMIN(1.0f, s));
    PointOnSegAx = (A1x + s * Lax);
    PointOnSegAy = (A1y + s * Lay);
    PointOnSegAz = (A1z + s * Laz);
    FindNearestPointOnLineSegment(B1x, B1y, B1z, Lbx, Lby, Lbz, PointOnSegAx,
                                  PointOnSegAy, PointOnSegAz, true, epsilon_squared,
                                  PointOnSegBx, PointOnSegBy, PointOnSegBz, t);
    if (OUT_OF_RANGE(t))
    {
      t = FMAX(0.0f, FMIN(1.0f, t));
      PointOnSegBx = (B1x + t * Lbx);
      PointOnSegBy = (B1y + t * Lby);
      PointOnSegBz = (B1z + t * Lbz);
      FindNearestPointOnLineSegment(A1x, A1y, A1z, Lax, Lay, Laz, PointOnSegBx,
                                    PointOnSegBy, PointOnSegBz, false, epsilon_squared,
                                    PointOnSegAx, PointOnSegAy, PointOnSegAz, s);
      FindNearestPointOnLineSegment(B1x, B1y, B1z, Lbx, Lby, Lbz, PointOnSegAx,
                                    PointOnSegAy, PointOnSegAz, false, epsilon_squared,
                                    PointOnSegBx, PointOnSegBy, PointOnSegBz, t);
    }
  }
// otherwise, handle the case where the parameter for only one segment is
// out of range
  else if (OUT_OF_RANGE(s))
  {
    s = FMAX(0.0f, FMIN(1.0f, s));
    PointOnSegAx = (A1x + s * Lax);
    PointOnSegAy = (A1y + s * Lay);
    PointOnSegAz = (A1z + s * Laz);
    FindNearestPointOnLineSegment(B1x, B1y, B1z, Lbx, Lby, Lbz, PointOnSegAx,
                                  PointOnSegAy, PointOnSegAz, false, epsilon_squared,
                                  PointOnSegBx, PointOnSegBy, PointOnSegBz, t);
  }
  else if (OUT_OF_RANGE(t))
  {
    t = FMAX(0.0f, FMIN(1.0f, t));
    PointOnSegBx = (B1x + t * Lbx);
    PointOnSegBy = (B1y + t * Lby);
    PointOnSegBz = (B1z + t * Lbz);
    FindNearestPointOnLineSegment(A1x, A1y, A1z, Lax, Lay, Laz, PointOnSegBx,
                                  PointOnSegBy, PointOnSegBz, false, epsilon_squared,
                                  PointOnSegAx, PointOnSegAy, PointOnSegAz, s);
  }
  else
  {
    assert(0);
  }
}