distanceSqToSegment method

num distanceSqToSegment(Vector3 v0, Vector3 v1, [ Vector3? optionalPointOnRay, Vector3? optionalPointOnSegment, ])

Implementation

num distanceSqToSegment(Vector3 v0, Vector3 v1,
    [Vector3? optionalPointOnRay, Vector3? optionalPointOnSegment]) {
  // from http://www.geometrictools.com/GTEngine/Include/Mathematics/GteDistRaySegment.h
  // It returns the min distance between the ray and the segment
  // defined by v0 and v1
  // It can also set two optional targets :
  // - The closest point on the ray
  // - The closest point on the segment

  _segCenter.setFrom(v0).add(v1).scale(0.5);
  _segDir.setFrom(v1).sub(v0).normalize();
  _diff.setFrom(origin).sub(_segCenter);

  final segExtent = v0.distanceTo(v1) * 0.5;
  num a01 = -direction.dot(_segDir);
  final b0 = _diff.dot(direction);
  final b1 = -_diff.dot(_segDir);
  final c = _diff.length2;
  final det = (1 - a01 * a01).abs();
  num s0, s1, sqrDist, extDet;

  if (det > 0) {
    // The ray and segment are not parallel.

    s0 = a01 * b1 - b0;
    s1 = a01 * b0 - b1;
    extDet = segExtent * det;

    if (s0 >= 0) {
      if (s1 >= -extDet) {
        if (s1 <= extDet) {
          // region 0
          // Minimum at interior points of ray and segment.

          final invDet = 1 / det;
          s0 *= invDet;
          s1 *= invDet;
          sqrDist = s0 * (s0 + a01 * s1 + 2 * b0) +
              s1 * (a01 * s0 + s1 + 2 * b1) +
              c;
        } else {
          // region 1

          s1 = segExtent;
          s0 = math.max(0, -(a01 * s1 + b0));
          sqrDist = -s0 * s0 + s1 * (s1 + 2 * b1) + c;
        }
      } else {
        // region 5

        s1 = -segExtent;
        s0 = math.max(0, -(a01 * s1 + b0));
        sqrDist = -s0 * s0 + s1 * (s1 + 2 * b1) + c;
      }
    } else {
      if (s1 <= -extDet) {
        // region 4

        s0 = math.max(0, -(-a01 * segExtent + b0));
        s1 = (s0 > 0)
            ? -segExtent
            : math.min(math.max(-segExtent, -b1), segExtent);
        sqrDist = -s0 * s0 + s1 * (s1 + 2 * b1) + c;
      } else if (s1 <= extDet) {
        // region 3

        s0 = 0;
        s1 = math.min(math.max(-segExtent, -b1), segExtent);
        sqrDist = s1 * (s1 + 2 * b1) + c;
      } else {
        // region 2

        s0 = math.max(0, -(a01 * segExtent + b0));
        s1 = (s0 > 0)
            ? segExtent
            : math.min(math.max(-segExtent, -b1), segExtent);
        sqrDist = -s0 * s0 + s1 * (s1 + 2 * b1) + c;
      }
    }
  } else {
    // Ray and segment are parallel.

    s1 = (a01 > 0) ? -segExtent : segExtent;
    s0 = math.max(0, -(a01 * s1 + b0));
    sqrDist = -s0 * s0 + s1 * (s1 + 2 * b1) + c;
  }

  if (optionalPointOnRay != null) {
    optionalPointOnRay.setFrom(direction).scale(s0).add(origin);
  }

  if (optionalPointOnSegment != null) {
    optionalPointOnSegment.setFrom(_segDir).scale(s1).add(_segCenter);
  }

  return sqrDist;
}