lineintersect_utils.cpp

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

/* Copyright (C) Graham Rhodes, 2001. 
 * All rights reserved worldwide.
 *
 * This software is provided "as is" without express or implied
 * warranties. You may freely copy and compile this source into
 * applications you distribute provided that the copyright text
 * below is included in the resulting source code, for example:
 * "Portions Copyright (C) Graham Rhodes, 2001"
 */
/**************************************************************************************
|
|           File: lineintersect_utils.cpp
|
|        Purpose: Implementation of line segment intersection utility functions
|
|     Book Title: Game Programming Gems II
|
|  Chapter Title: Fast, Robust Intersection of 3D Line Segments
|
|         Author: Graham Rhodes
|
|      Revisions: 05-Apr-2001 - GSR. Original.
|
**************************************************************************************/
#include <math.h>
#include "lineintersect_utils.h"
#include <assert.h>

// uncomment the following line to have the code check intermediate results
//#define CHECK_ANSWERS

// uncomment the following line to use Cramer's rule instead of Gaussian elimination
//#define USE_CRAMERS_RULE

#define FMAX(a,b) ((a) > (b) ? (a) : (b))
#define FMIN(a,b) ((a) > (b) ? (b) : (a))
#define FABS(a) ((a) < 0.0f ? -(a) : (a))
#define OUT_OF_RANGE(a) ((a) < 0.0f || (a) > 1.f)

// pragma to get rid of math.h inline function removal warnings.
#pragma warning(disable:4514)

/**************************************************************************
|
|     Method: IntersectLineSegments
|
|    Purpose: Find the nearest point between two finite length line segments
|             or two infinite lines in 3-dimensional space. The function calculates
|             the point on each line/line segment that is closest to the other
|             line/line segment, the midpoint between the nearest points, and
|             the vector between these two points. If the two nearest points
|             are close within a tolerance, a flag is set indicating the lines
|             have a "true" intersection.
|
| 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
|             B1x, B1y, B1z   - Coordinates of first defining point of line/segment B
|             B2x, B2y, B2z   - Coordinates of second defining point of line/segment B
|             infinite_lines  - set to true if lines are to be treated as infinite
|             epsilon         - 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
|             NearestPointX,  - Midpoint between the two nearest points. This can be
|             NearestPointY,    treated as *the* intersection point if nearest points
|             NearestPointZ     are sufficiently close. This corresponds to point P
|                               in the text.
|             NearestVectorX, - Vector between the nearest point on A to the nearest
|                               point on segment B. This vector is normal to both
|                               lines if the lines are infinite, but is not guaranteed
|                               to be normal to both lines if both lines are finite
|                               length.
|           true_intersection - true if the nearest points are close within a small
|                               tolerance.
**************************************************************************/
void IntersectLineSegments(const float A1x, const float A1y, const float A1z,
                           const float A2x, const float A2y, const float A2z, 
                           const float B1x, const float B1y, const float B1z,
                           const float B2x, const float B2y, const float B2z,
                           bool infinite_lines, float epsilon, float &PointOnSegAx,
                           float &PointOnSegAy, float &PointOnSegAz, float &PointOnSegBx,
                           float &PointOnSegBy, float &PointOnSegBz, float &NearestPointX,
                           float &NearestPointY, float &NearestPointZ, float &NearestVectorX,
                           float &NearestVectorY, float &NearestVectorZ, bool &true_intersection)
{
  float temp = 0.f;
  float epsilon_squared = epsilon * epsilon;

// Compute parameters from Equations (1) and (2) in the text
  float Lax = A2x - A1x;
  float Lay = A2y - A1y;
  float Laz = A2z - A1z;
  float Lbx = B2x - B1x;
  float Lby = B2y - B1y;
  float Lbz = B2z - B1z;
// From Equation (15)
  float L11 =  (Lax * Lax) + (Lay * Lay) + (Laz * Laz);
  float L22 =  (Lbx * Lbx) + (Lby * Lby) + (Lbz * Lbz);

// Line/Segment A is degenerate ---- Special Case #1
  if (L11 < epsilon_squared)
  {
    PointOnSegAx = A1x;
    PointOnSegAy = A1y;
    PointOnSegAz = A1z;
    FindNearestPointOnLineSegment(B1x, B1y, B1z, Lbx, Lby, Lbz, A1x, A1y, A1z,
                                  infinite_lines, epsilon, PointOnSegBx, PointOnSegBy,
                                  PointOnSegBz, temp);
  }
// Line/Segment B is degenerate ---- Special Case #1
  else if (L22 < epsilon_squared)
  {
    PointOnSegBx = B1x;
    PointOnSegBy = B1y;
    PointOnSegBz = B1z;
    FindNearestPointOnLineSegment(A1x, A1y, A1z, Lax, Lay, Laz, B1x, B1y, B1z,
                                  infinite_lines, epsilon, PointOnSegAx, PointOnSegAy,
                                  PointOnSegAz, temp);
  }
// Neither line/segment is degenerate
  else
  {
// Compute more parameters from Equation (3) in the text.
    float ABx = B1x - A1x;
    float ABy = B1y - A1y;
    float ABz = B1z - A1z;

// and from Equation (15).
    float L12 = -(Lax * Lbx) - (Lay * Lby) - (Laz * Lbz);

    float DetL = L11 * L22 - L12 * L12;
// Lines/Segments A and B are parallel ---- special case #2.
    if (FABS(DetL) < epsilon)
    {
      FindNearestPointOfParallelLineSegments(A1x, A1y, A1z, A2x, A2y, A2z,
                                             Lax, Lay, Laz,
                                             B1x, B1y, B1z, B2x, B2y, B2z,
                                             Lbx, Lby, Lbz,
                                             infinite_lines, epsilon,
                                             PointOnSegAx, PointOnSegAy, PointOnSegAz,
                                             PointOnSegBx, PointOnSegBy, PointOnSegBz);
    }
// The general case
    else
    {
// from Equation (15)
      float ra = Lax * ABx + Lay * ABy + Laz * ABz;
      float rb = -Lbx * ABx - Lby * ABy - Lbz * ABz;

      float t = (L11 * rb - ra * L12)/DetL; // Equation (12)

#ifdef USE_CRAMERS_RULE
      float s = (L22 * ra - rb * L12)/DetL;
#else
      float s = (ra-L12*t)/L11;             // Equation (13)
#endif // USE_CRAMERS_RULE

#ifdef CHECK_ANSWERS
      float check_ra = s*L11 + t*L12;
      float check_rb = s*L12 + t*L22;
      assert(FABS(check_ra-ra) < epsilon);
      assert(FABS(check_rb-rb) < epsilon);
#endif // CHECK_ANSWERS

// if we are dealing with infinite lines or if parameters s and t both
// lie in the range [0,1] then just compute the points using Equations
// (1) and (2) from the text.
      PointOnSegAx = (A1x + s * Lax);
      PointOnSegAy = (A1y + s * Lay);
      PointOnSegAz = (A1z + s * Laz);
      PointOnSegBx = (B1x + t * Lbx);
      PointOnSegBy = (B1y + t * Lby);
      PointOnSegBz = (B1z + t * Lbz);
// otherwise, at least one of s and t is outside of [0,1] and we have to
// handle this case.
      if (false == infinite_lines && (OUT_OF_RANGE(s) || OUT_OF_RANGE(t)))
      {
        AdjustNearestPoints(A1x, A1y, A1z, Lax, Lay, Laz,
                            B1x, B1y, B1z, Lbx, Lby, Lbz,
                            epsilon, s, t,
                            PointOnSegAx, PointOnSegAy, PointOnSegAz,
                            PointOnSegBx, PointOnSegBy, PointOnSegBz);
      }
    }
  }

  NearestPointX = 0.5f * (PointOnSegAx + PointOnSegBx);
  NearestPointY = 0.5f * (PointOnSegAy + PointOnSegBy);
  NearestPointZ = 0.5f * (PointOnSegAz + PointOnSegBz);

  NearestVectorX = PointOnSegBx - PointOnSegAx;
  NearestVectorY = PointOnSegBy - PointOnSegAy;
  NearestVectorZ = PointOnSegBz - PointOnSegAz;

// optional check to indicate if the lines truly intersect
  true_intersection = (FABS(NearestVectorX) +
                       FABS(NearestVectorY) +
                       FABS(NearestVectorZ)) < epsilon ? true : false;
}

/**************************************************************************
|
|     Method: FindNearestPointOnLineSegment
|
|    Purpose: Given a line (segment) and a point in 3-dimensional space,
|             find the point on the line (segment) that is closest to the
|             point.
|
| Parameters: Input:
|             ------
|             A1x, A1y, A1z   - Coordinates of first defining point of the line/segment
|             Lx, Ly, Lz      - Vector from (A1x, A1y, A1z) to the second defining point
|                               of the line/segment.
|             Bx, By, Bz      - Coordinates of the point
|             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:
|             -------
|             NearestPointX,  - Point on line/segment that is closest to (Bx, By, Bz)
|             NearestPointY,
|             NearestPointZ