distance.java

JAVA3D矩陈的相关类

   *
   * @param ray1dir Direction of ray 1
   *
   * @param ray0int If non-null, will be filled with the coordinates
   * of the point corresponding to the minimum distance on ray 0.
   *
   * @param ray1int If non-null, will be filled with the coordinates
   * of the point corresponding to the minimum distance on ray 1.
   *
   * @param param An array of two doubles, will be filled with the
   * parametric factors used to find the point of shortest distance on
   * each primitive (ray = O +sD, with O=origin and
   * D=direction). param[0] will contain the parameter for ray0, and
   * param[1] the parameter for ray1.
   *
   * @return the square of the minimum distance from the ray to the segment
   */
  static public double rayToRay (Point3d ray0orig, 
				 Vector3d ray0dir,
				 Point3d ray1orig, 
				 Vector3d ray1dir,
				 Point3d ray0int,
				 Point3d ray1int,
				 double[] param) {

    double s, t;

    Vector3d diff = new Vector3d();
    diff.sub (ray0orig, ray1orig);

    double A = ray0dir.dot (ray0dir); //Dot(ray0.m,ray0.m);
    double B = -ray0dir.dot (ray1dir); //-Dot(ray0.m,ray1.m);
    double C = ray1dir.dot (ray1dir); //Dot(ray1.m,ray1.m);
    double D = ray0dir.dot (diff); //Dot(ray0.m,diff);
    double E;  // -Dot(ray1.m,diff), defer until needed
    double F = diff.dot (diff); //Dot(diff,diff);
    double det = Math.abs(A*C-B*B);  // A*C-B*B = |Cross(M0,M1)|^2 >= 0
    /*
      System.out.println (ray0orig + "\n" + ray0dir + "\n" +
      ray1orig + "\n" + ray1dir);
      System.out.println (A + " " + B + " " + C + " " + D + " " + F + " " + det);
      */
    if (det >= FUZZ) {
      // rays are not parallel
      E = -ray1dir.dot (diff); //-Dot(ray1.m,diff);
      s = B*E-C*D;
      t = B*D-A*E;

      if (s >= 0) {
	if (t >= 0) { // region 0 (interior)
	  // minimum at two interior points of rays
	  double invDet = 1.0f/det;
	  s *= invDet;
	  t *= invDet;
	  if (ray0int != null) ray0int.scaleAdd (s, ray0dir, ray0orig);
	  if (ray1int != null) ray1int.scaleAdd (t, ray1dir, ray1orig);
	  if (param != null) { param[0] = s; param[1] = t; }
	  return DIST(s*(A*s+B*t+2*D)+t*(B*s+C*t+2*E)+F);
	}
	else { // region 3 (side)
	  t = 0;
	  if (D >= 0) {
	    s = 0;
	    if (ray0int != null) ray0int.set (ray0orig);
	    if (ray1int != null) ray1int.set (ray1orig);
	    if (param != null) { param[0] = s; param[1] = t; }
	    return DIST(F);
	  }
	  else {
	    s = -D/A;
	    if (ray0int != null) ray0int.scaleAdd (s, ray0dir, ray0orig);
	    if (ray1int != null) ray1int.set (ray1orig);
	    if (param != null) { param[0] = s; param[1] = t; }
	    return DIST(D*s+F);
	  }
	}
      }
      else {
	if (t >= 0) {  // region 1 (side)
	  s = 0;
	  if (E >= 0) {
	    t = 0;
	    if (ray0int != null) ray0int.set (ray0orig);
	    if (ray1int != null) ray1int.set (ray1orig);
	    if (param != null) { param[0] = s; param[1] = t; }
	    return DIST(F);
	  }
	  else {
	    t = -E/C;
	    if (ray0int != null) ray0int.set (ray0orig);
	    if (ray1int != null) ray1int.scaleAdd (t, ray1dir, ray1orig);
	    if (param != null) { param[0] = s; param[1] = t; }
	    return DIST(E*t+F);
	  }
	}
	else { // region 2 (corner)
	  if (D < 0) {
	    s = -D/A;
	    t = 0;
	    if (ray0int != null) ray0int.scaleAdd (s, ray0dir, ray0orig);
	    if (ray1int != null) ray1int.set (ray1orig);
	    if (param != null) { param[0] = s; param[1] = t; }
	    return DIST(D*s+F);
	  }
	  else {
	    s = 0;
	    if (E >= 0) {
	      t = 0;
	      if (ray0int != null) ray0int.set (ray0orig);
	      if (ray1int != null) ray1int.set (ray1orig);
	      if (param != null) { param[0] = s; param[1] = t; }
	      return DIST(F);
	    }
	    else {
	      t = -E/C;
	      if (ray0int != null) ray0int.set (ray0orig);
	      if (ray1int != null) ray1int.scaleAdd (t, ray1dir, ray1orig);
	      if (param != null) { param[0] = s; param[1] = t; }
	      return DIST(E*t+F);
	    }
	  }
	}
      }
    }
    else {
      // rays are parallel
      if (B > 0) {
	// opposite direction vectors
	t = 0;
	if (D >= 0) {
	  s = 0;
	  if (ray0int != null) ray0int.set (ray0orig);
	  if (ray1int != null) ray1int.set (ray1orig);
	  if (param != null) { param[0] = s; param[1] = t; }
	  return DIST(F);
	}
	else {
	  s = -D/A;
	  if (ray0int != null) ray0int.scaleAdd (s, ray0dir, ray0orig);
	  if (ray1int != null) ray1int.set (ray1orig);
	  if (param != null) { param[0] = s; param[1] = t; }
	  return DIST(D*s+F);
	}
      }
      else {
	// same direction vectors
	if (D >= 0) {
	  E = ray1dir.dot (diff); //-Dot(ray1.m,diff);
	  s = 0;
	  t = -E/C;
	  if (ray0int != null) ray0int.set (ray0orig);
	  if (ray1int != null) ray1int.scaleAdd (t, ray1dir, ray1orig);
	  if (param != null) { param[0] = s; param[1] = t; }
	  return DIST(E*t+F);
	}
	else {
	  s = -D/A;
	  t = 0;
	  if (ray0int != null) ray0int.scaleAdd (s, ray0dir, ray0orig);
	  if (ray1int != null) ray1int.set (ray1orig);
	  if (param != null) { param[0] = s; param[1] = t; }
	  return DIST(D*s+F);
	}
      }
    }
  }

  /**
   * Minimum pt to ray distance. Returns the square of the distance.
   * @param pt The point
   * @param rayorig Origin of the ray
   * @param raydir Direction of the ray
   * @return the square of the minimum distance between the point and the ray
   */
  static public double pointToRay (Point3d pt, 
				   Point3d rayorig, 
				   Vector3d raydir) {
    return pointToRay (pt, rayorig, raydir, null, null);
  }

  /**
   * Minimum pt to ray distance. Returns the square of the distance.
   *
   * @param pt The point
   *
   * @param rayorig Origin of the ray
   *
   * @param raydir Direction of the ray
   *
   * @param rayint If non-null, will be filled with the coordinates of
   * the point corresponding to the minimum distance on the ray.
   *
   * @param param An array of one double, will be filled with the
   * parametric factors used to find the point of shortest distance on
   * the ray (ray = O +sD, with O=origin and D=direction). param[0]
   * will contain the parameter for the ray.
   *
   * @return the square of the minimum distance between the point and the ray
   */
  static public double pointToRay (Point3d pt, 
				   Point3d rayorig, 
				   Vector3d raydir,
				   Point3d rayint,
				   double[] param) {

    double t;

    Vector3d diff = new Vector3d();
    diff.sub (pt, rayorig);
    t = raydir.dot (diff); //Dot(ray.m,diff);

    if (t <= 0.0) {
      t = 0.0; // behind start of ray
      if (rayint != null) rayint.set (rayorig);
      if (param != null) { param[0] = t; }
    } else {
      t /= raydir.dot (raydir); //Dot(ray.m,ray.m);
      diff.scaleAdd (-t, raydir, diff); // diff = diff - t*ray.m;
      if (rayint != null) rayint.scaleAdd (t, raydir, rayorig);
      if (param != null) { param[0] = t; }
    }
    return diff.dot(diff);
  }

  /**
   * Minimum pt to segment distance. Returns the square of the distance.
   */
  static public double pointToSegment (Point3d pt, 
				       Point3d segstart, 
				       Point3d segend) {
    return pointToSegment (pt, segstart, segend, null, null);
  }

  /**
   * Minimum pt to segment distance. Returns the square of the distance.
   */
  static public double pointToSegment (Point3d pt, 
				       Point3d segstart, 
				       Point3d segend,
				       Point3d segint,
				       double[] param) {

    double t;
    Vector3d segdir = new Vector3d ();
    segdir.sub (segend, segstart);
    Vector3d diff = new Vector3d();
    diff.sub (pt,segstart);
    t = segdir.dot (diff); //Dot(seg.m,diff);

    if (t <= 0.0) {
      t = 0.0f;
      if (segint != null) segint.set (segstart);
      if (param != null) { param[0] = t; }
    } 
    else {
      double mDotm = segdir.dot (segdir); //Dot(seg.m,seg.m);
      if (t >= mDotm) {
	t = 1.0f;
	diff.sub (segdir);
	if (segint != null) segint.set (segend);
	if (param != null) { param[0] = t; }
      }
      else {
	t /= mDotm;
	diff.scaleAdd (-t, segdir, diff); //diff = diff - t*seg.m;
	if (segint != null) segint.scaleAdd (t, segdir, segstart);
	if (param != null) { param[0] = t; }
      }
    }
    return diff.dot(diff); //DIST(diff);
  }


  /**
   * Minimum segment to segment distance. Returns the square of the distance.
   * @param seg0start the start of segment 0
   * @param seg0end the end of segment 0
   * @param seg1start the start of segment 1
   * @param seg1end the end of segment 1
   * @return the square of the minimum distance from segment to segment    
   */
  static public double segmentToSegment (Point3d seg0start,
					 Point3d seg0end,
					 Point3d seg1start, 
					 Point3d seg1end) {
    return segmentToSegment (seg0start, seg0end, seg1start, seg1end, 
			     null, null, null);
  }

  /**
   * Minimum segment to segment distance. Returns the square of the distance.
   *
   * @param seg0start the start of segment 0
   *
   * @param seg0end the end of segment 0
   *
   * @param seg1start the start of segment 1
   *
   * @param seg1end the end of segment 1
   *
   * @param seg0int If non-null, will be filled with the coordinates
   * of the point corresponding to the minimum distance on segment 0.
   *
   * @param seg1int If non-null, will be filled with the coordinates
   * of the point corresponding to the minimum distance on segment 1.
   *
   * @param param An array of two doubles, will be filled with the
   * parametric factors used to find the point of shortest distance on
   * each primitive (segment = O +sD, with O=origin and
   * D=direction). param[0] will contain the parameter for segment 0,
   * and param[1] the parameter for segment 1.
   *
   * @return the square of the minimum distance from segment to segment
   */
  static public double segmentToSegment (Point3d seg0start,
					 Point3d seg0end,
					 Point3d seg1start, 
					 Point3d seg1end,
					 Point3d seg0int,
					 Point3d seg1int,
					 double[] param) {
    double s,t;

    Vector3d diff = new Vector3d();
    diff.sub (seg0start,seg1start);
    
    Vector3d seg0dir = new Vector3d();
    seg0dir.sub (seg0end, seg0start);
    Vector3d seg1dir = new Vector3d();
    seg1dir.sub (seg1end, seg1start);

    double A = seg0dir.dot (seg0dir); //Dot(seg0dir,seg0dir);
    double B = -seg0dir.dot (seg1dir); //-Dot(seg0dir,seg1dir);
    double C = seg1dir.dot (seg1dir); //Dot(seg1dir,seg1dir);
    double D = seg0dir.dot (diff); //Dot(seg0dir,diff);
    double E;  // -Dot(seg1dir,diff), defer until needed
    double F = diff.dot (diff); //Dot(diff,diff);
    double det = Math.abs(A*C-B*B);  // A*C-B*B = |Cross(M0,M1)|^2 >= 0

    double tmp;

    if (det >= FUZZ) {
      // line segments are not parallel
      E = -seg1dir.dot (diff); //-Dot(seg1dir,diff);
      s = B*E-C*D;
      t = B*D-A*E;
        
      if (s >= 0) {
	if (s <= det) {
	  if (t >= 0) {
	    if (t <= det) {  // region 0 (interior)
	      // minimum at two interior points of 3D lines
	      double invDet = 1.0f/det;
	      s *= invDet;
	      t *= invDet;
	      if (seg0int != null) seg0int.scaleAdd (s, seg0dir, seg0start);
	      if (seg1int != null) seg1int.scaleAdd (t, seg1dir, seg1start);
	      if (param != null) { param[0] = s; param[1] = t; }
	      return DIST(s*(A*s+B*t+2*D)+t*(B*s+C*t+2*E)+F);
	    }
	    else { // region 3 (side)
	      t = 1;
	      tmp = B+D;
	      if (tmp >= 0) {
		s = 0;
		if (seg0int != null) seg0int.set (seg0start);