0001: /*
0002: * $RCSfile: Distance.java,v $
0003: *
0004: * Copyright (c) 2007 Sun Microsystems, Inc. All rights reserved.
0005: *
0006: * Redistribution and use in source and binary forms, with or without
0007: * modification, are permitted provided that the following conditions
0008: * are met:
0009: *
0010: * - Redistribution of source code must retain the above copyright
0011: * notice, this list of conditions and the following disclaimer.
0012: *
0013: * - Redistribution in binary form must reproduce the above copyright
0014: * notice, this list of conditions and the following disclaimer in
0015: * the documentation and/or other materials provided with the
0016: * distribution.
0017: *
0018: * Neither the name of Sun Microsystems, Inc. or the names of
0019: * contributors may be used to endorse or promote products derived
0020: * from this software without specific prior written permission.
0021: *
0022: * This software is provided "AS IS," without a warranty of any
0023: * kind. ALL EXPRESS OR IMPLIED CONDITIONS, REPRESENTATIONS AND
0024: * WARRANTIES, INCLUDING ANY IMPLIED WARRANTY OF MERCHANTABILITY,
0025: * FITNESS FOR A PARTICULAR PURPOSE OR NON-INFRINGEMENT, ARE HEREBY
0026: * EXCLUDED. SUN MICROSYSTEMS, INC. ("SUN") AND ITS LICENSORS SHALL
0027: * NOT BE LIABLE FOR ANY DAMAGES SUFFERED BY LICENSEE AS A RESULT OF
0028: * USING, MODIFYING OR DISTRIBUTING THIS SOFTWARE OR ITS
0029: * DERIVATIVES. IN NO EVENT WILL SUN OR ITS LICENSORS BE LIABLE FOR
0030: * ANY LOST REVENUE, PROFIT OR DATA, OR FOR DIRECT, INDIRECT, SPECIAL,
0031: * CONSEQUENTIAL, INCIDENTAL OR PUNITIVE DAMAGES, HOWEVER CAUSED AND
0032: * REGARDLESS OF THE THEORY OF LIABILITY, ARISING OUT OF THE USE OF OR
0033: * INABILITY TO USE THIS SOFTWARE, EVEN IF SUN HAS BEEN ADVISED OF THE
0034: * POSSIBILITY OF SUCH DAMAGES.
0035: *
0036: * You acknowledge that this software is not designed, licensed or
0037: * intended for use in the design, construction, operation or
0038: * maintenance of any nuclear facility.
0039: *
0040: * $Revision: 1.4 $
0041: * $Date: 2007/02/09 17:20:05 $
0042: * $State: Exp $
0043: */
0044:
0045: // --------------------------------------------------
0046: //
0047: // Distance routines, ported from:
0048: //
0049: // Magic Software, Inc.
0050: // http://www.magic-software.com
0051: // http://www.wild-magic.com
0052: // Copyright (c) 2004. All Rights Reserved
0053: //
0054: // The Wild Magic Library (WML) source code is supplied under the terms of
0055: // the license agreement http://www.magic-software.com/License/WildMagic.pdf
0056: // and may not be copied or disclosed except in accordance with the terms of
0057: // that agreement.
0058: //
0059: // --------------------------------------------------
0060: package com.sun.j3d.internal;
0061:
0062: import javax.vecmath.*;
0063:
0064: /**
0065: * Utility class used to calculate distance. Contains static methods
0066: * used by picking method to determine intersections.
0067: */
0068:
0069: public class Distance {
0070: /* Threshold factor to determine if two lines are parallel */
0071: static final double FUZZ = 1E-5;
0072:
0073: /* Utility method, for easy switch between distance and squared distance */
0074: private static final double DIST(double in) {
0075: // return Math.sqrt (Math.abs (in));
0076: return Math.abs(in);
0077: }
0078:
0079: /**
0080: * Minimum ray to segment distance.
0081: *
0082: * @param rayorig Origin of the ray
0083: * @param raydir Direction of the ray
0084: * @param segstart Segment start point
0085: * @param segend Segment end point
0086: * @return the square of the minimum distance from the ray to the segment
0087: */
0088: static public double rayToSegment(Point3d rayorig, Vector3d raydir,
0089: Point3d segstart, Point3d segend) {
0090: return rayToSegment(rayorig, raydir, segstart, segend, null,
0091: null, null);
0092: }
0093:
0094: /**
0095: * Minimum ray to segment distance. Returns the square of the distance.
0096: *
0097: * @param rayorig Origin of the ray
0098: *
0099: * @param raydir Direction of the ray
0100: *
0101: * @param segstart Segment start point
0102: *
0103: * @param segend Segment end point
0104: *
0105: * @param rayint If non-null, will be filled with the coordinates of
0106: * the point corresponding to the minimum distance on the ray.
0107: *
0108: * @param segint If non-null, will be filled with the coordinates of
0109: * the point corresponding to the minimum distance on the segment.
0110: *
0111: * @param param An array of two doubles, will be filled with the
0112: * parametric factors used to find the point of shortest distance on
0113: * each primitive (ray = O +sD, with O=origin and
0114: * D=direction). param[0] will contain the parameter for the ray,
0115: * and param[1] the parameter for the segment.
0116: *
0117: * @return the square of the minimum distance from the ray to the
0118: * segment
0119: */
0120: static public double rayToSegment(Point3d rayorig, Vector3d raydir,
0121: Point3d segstart, Point3d segend, Point3d rayint,
0122: Point3d segint, double[] param) {
0123: double s, t;
0124:
0125: Vector3d diff = new Vector3d();
0126: diff.sub(rayorig, segstart);
0127: Vector3d segdir = new Vector3d();
0128: segdir.sub(segend, segstart);
0129: /*
0130: System.out.println (rayorig + "\n" + raydir + "\n" + segstart + "\n" +
0131: segdir);
0132: */
0133: double A = raydir.dot(raydir);//Dot(ray.m,ray.m);
0134: double B = -raydir.dot(segdir);//-Dot(ray.m,seg.m);
0135: double C = segdir.dot(segdir);//Dot(seg.m,seg.m);
0136: double D = raydir.dot(diff);//Dot(ray.m,diff);
0137: double E; // -Dot(seg.m,diff), defer until needed
0138: double F = diff.dot(diff);//Dot(diff,diff);
0139: double det = Math.abs(A * C - B * B); // A*C-B*B = |Cross(M0,M1)|^2 >= 0
0140:
0141: double tmp;
0142:
0143: if (det >= FUZZ) {
0144: // ray and segment are not parallel
0145: E = -segdir.dot(diff);//-Dot(seg.m,diff);
0146: s = B * E - C * D;
0147: t = B * D - A * E;
0148:
0149: if (s >= 0) {
0150: if (t >= 0) {
0151: if (t <= det) { // region 0
0152: // minimum at interior points of ray and segment
0153: double invDet = 1.0f / det;
0154: s *= invDet;
0155: t *= invDet;
0156: if (rayint != null)
0157: rayint.scaleAdd(s, raydir, rayorig);
0158: if (segint != null)
0159: segint.scaleAdd(t, segdir, segstart);
0160: if (param != null) {
0161: param[0] = s;
0162: param[1] = t;
0163: }
0164: return DIST(s * (A * s + B * t + 2 * D) + t
0165: * (B * s + C * t + 2 * E) + F);
0166: } else { // region 1
0167:
0168: t = 1;
0169: if (D >= 0) {
0170: s = 0;
0171: if (rayint != null)
0172: rayint.set(rayorig);
0173: if (segint != null)
0174: segint.set(segend);
0175: if (param != null) {
0176: param[0] = s;
0177: param[1] = t;
0178: }
0179: return DIST(C + 2 * E + F);
0180: } else {
0181: s = -D / A;
0182: if (rayint != null)
0183: rayint.scaleAdd(s, raydir, rayorig);
0184: if (segint != null)
0185: segint.set(segend);
0186: if (param != null) {
0187: param[0] = s;
0188: param[1] = t;
0189: }
0190: return DIST((D + 2 * B) * s + C + 2 * E + F);
0191: }
0192: }
0193: } else { // region 5
0194: t = 0;
0195: if (D >= 0) {
0196: s = 0;
0197: if (rayint != null)
0198: rayint.set(rayorig);
0199: if (segint != null)
0200: segint.set(segstart);
0201: if (param != null) {
0202: param[0] = s;
0203: param[1] = t;
0204: }
0205: return DIST(F);
0206: } else {
0207: s = -D / A;
0208: if (rayint != null)
0209: rayint.scaleAdd(s, raydir, rayorig);
0210: if (segint != null)
0211: segint.set(segstart);
0212: if (param != null) {
0213: param[0] = s;
0214: param[1] = t;
0215: }
0216: return DIST(D * s + F);
0217: }
0218: }
0219: } else {
0220: if (t <= 0) { // region 4
0221: if (D < 0) {
0222: s = -D / A;
0223: t = 0;
0224: if (rayint != null)
0225: rayint.scaleAdd(s, raydir, rayorig);
0226: if (segint != null)
0227: segint.set(segstart);
0228: if (param != null) {
0229: param[0] = s;
0230: param[1] = t;
0231: }
0232: return DIST(D * s + F);
0233: } else {
0234: s = 0;
0235: if (E >= 0) {
0236: t = 0;
0237: if (rayint != null)
0238: rayint.set(rayorig);
0239: if (segint != null)
0240: segint.set(segstart);
0241: if (param != null) {
0242: param[0] = s;
0243: param[1] = t;
0244: }
0245: return DIST(F);
0246: } else if (-E >= C) {
0247: t = 1;
0248: if (rayint != null)
0249: rayint.set(rayorig);
0250: if (segint != null)
0251: segint.set(segend);
0252: if (param != null) {
0253: param[0] = s;
0254: param[1] = t;
0255: }
0256: return DIST(C + 2 * E + F);
0257: } else {
0258: t = -E / C;
0259: if (rayint != null)
0260: rayint.set(rayorig);
0261: if (segint != null)
0262: segint.scaleAdd(t, segdir, segstart);
0263: if (param != null) {
0264: param[0] = s;
0265: param[1] = t;
0266: }
0267: return DIST(E * t + F);
0268: }
0269: }
0270: } else if (t <= det) { // region 3
0271: s = 0;
0272: if (E >= 0) {
0273: t = 0;
0274: if (rayint != null)
0275: rayint.set(rayorig);
0276: if (segint != null)
0277: segint.set(segstart);
0278: if (param != null) {
0279: param[0] = s;
0280: param[1] = t;
0281: }
0282: return DIST(F);
0283: } else if (-E >= C) {
0284: t = 1;
0285: if (rayint != null)
0286: rayint.set(rayorig);
0287: if (segint != null)
0288: segint.set(segend);
0289: if (param != null) {
0290: param[0] = s;
0291: param[1] = t;
0292: }
0293: return DIST(C + 2 * E + F);
0294: } else {
0295: t = -E / C;
0296: if (rayint != null)
0297: rayint.set(rayorig);
0298: if (segint != null)
0299: segint.scaleAdd(t, segdir, segstart);
0300: if (param != null) {
0301: param[0] = s;
0302: param[1] = t;
0303: }
0304: return DIST(E * t + F);
0305: }
0306: } else { // region 2
0307: tmp = B + D;
0308: if (tmp < 0) {
0309: s = -tmp / A;
0310: t = 1;
0311: if (rayint != null)
0312: rayint.scaleAdd(s, raydir, rayorig);
0313: if (segint != null)
0314: segint.set(segend);
0315: if (param != null) {
0316: param[0] = s;
0317: param[1] = t;
0318: }
0319: return DIST(tmp * s + C + 2 * E + F);
0320: } else {
0321: s = 0;
0322: if (E >= 0) {
0323: t = 0;
0324: if (rayint != null)
0325: rayint.set(rayorig);
0326: if (segint != null)
0327: segint.set(segstart);
0328: if (param != null) {
0329: param[0] = s;
0330: param[1] = t;
0331: }
0332: return DIST(F);
0333: } else if (-E >= C) {
0334: t = 1;
0335: if (rayint != null)
0336: rayint.set(rayorig);
0337: if (segint != null)
0338: segint.set(segend);
0339: if (param != null) {
0340: param[0] = s;
0341: param[1] = t;
0342: }
0343: return DIST(C + 2 * E + F);
0344: } else {
0345: t = -E / C;
0346: if (rayint != null)
0347: rayint.set(rayorig);
0348: if (segint != null)
0349: segint.scaleAdd(t, segdir, segstart);
0350: if (param != null) {
0351: param[0] = s;
0352: param[1] = t;
0353: }
0354: return DIST(E * t + F);
0355: }
0356: }
0357: }
0358: }
0359: } else {
0360: // ray and segment are parallel
0361: if (B > 0) {
0362: // opposite direction vectors
0363: t = 0;
0364: if (D >= 0) {
0365: s = 0;
0366: if (rayint != null)
0367: rayint.set(rayorig);
0368: if (segint != null)
0369: segint.set(segstart);
0370: if (param != null) {
0371: param[0] = s;
0372: param[1] = t;
0373: }
0374: return DIST(F);
0375: } else {
0376: s = -D / A;
0377: if (rayint != null)
0378: rayint.scaleAdd(s, raydir, rayorig);
0379: if (segint != null)
0380: segint.set(segstart);
0381: if (param != null) {
0382: param[0] = s;
0383: param[1] = t;
0384: }
0385: return DIST(D * s + F);
0386: }
0387: } else {
0388: // same direction vectors
0389: E = segdir.dot(diff);//-Dot(seg.m,diff);
0390: t = 1;
0391: tmp = B + D;
0392: if (tmp >= 0) {
0393: s = 0;
0394: if (rayint != null)
0395: rayint.set(rayorig);
0396: if (segint != null)
0397: segint.set(segend);
0398: if (param != null) {
0399: param[0] = s;
0400: param[1] = t;
0401: }
0402: return DIST(C + 2 * E + F);
0403: } else {
0404: s = -tmp / A;
0405: if (rayint != null)
0406: rayint.scaleAdd(s, raydir, rayorig);
0407: if (segint != null)
0408: segint.set(segend);
0409: if (param != null) {
0410: param[0] = s;
0411: param[1] = t;
0412: }
0413: return DIST(tmp * s + C + 2 * E + F);
0414: }
0415: }
0416: }
0417: }
0418:
0419: /**
0420: * Minimum ray to ray distance. Returns the square of the distance.
0421: *
0422: * @param ray0orig Origin of ray 0
0423: * @param ray0dir Direction of ray 0
0424: * @param ray1orig Origin of ray 1
0425: * @param ray1dir Direction of ray 1
0426: * @return the square of the minimum distance from the ray to the segment
0427: */
0428: static public double rayToRay(Point3d ray0orig, Vector3d ray0dir,
0429: Point3d ray1orig, Vector3d ray1dir) {
0430: return rayToRay(ray0orig, ray0dir, ray1orig, ray1dir, null,
0431: null, null);
0432: }
0433:
0434: /**
0435: * Minimum ray to ray distance. Returns the square of the distance.
0436: *
0437: * @param ray0orig Origin of ray 0
0438: *
0439: * @param ray0dir Direction of ray 0
0440: *
0441: * @param ray1orig Origin of ray 1
0442: *
0443: * @param ray1dir Direction of ray 1
0444: *
0445: * @param ray0int If non-null, will be filled with the coordinates
0446: * of the point corresponding to the minimum distance on ray 0.
0447: *
0448: * @param ray1int If non-null, will be filled with the coordinates
0449: * of the point corresponding to the minimum distance on ray 1.
0450: *
0451: * @param param An array of two doubles, will be filled with the
0452: * parametric factors used to find the point of shortest distance on
0453: * each primitive (ray = O +sD, with O=origin and
0454: * D=direction). param[0] will contain the parameter for ray0, and
0455: * param[1] the parameter for ray1.
0456: *
0457: * @return the square of the minimum distance from the ray to the segment
0458: */
0459: static public double rayToRay(Point3d ray0orig, Vector3d ray0dir,
0460: Point3d ray1orig, Vector3d ray1dir, Point3d ray0int,
0461: Point3d ray1int, double[] param) {
0462:
0463: double s, t;
0464:
0465: Vector3d diff = new Vector3d();
0466: diff.sub(ray0orig, ray1orig);
0467:
0468: double A = ray0dir.dot(ray0dir); //Dot(ray0.m,ray0.m);
0469: double B = -ray0dir.dot(ray1dir); //-Dot(ray0.m,ray1.m);
0470: double C = ray1dir.dot(ray1dir); //Dot(ray1.m,ray1.m);
0471: double D = ray0dir.dot(diff); //Dot(ray0.m,diff);
0472: double E; // -Dot(ray1.m,diff), defer until needed
0473: double F = diff.dot(diff); //Dot(diff,diff);
0474: double det = Math.abs(A * C - B * B); // A*C-B*B = |Cross(M0,M1)|^2 >= 0
0475: /*
0476: System.out.println (ray0orig + "\n" + ray0dir + "\n" +
0477: ray1orig + "\n" + ray1dir);
0478: System.out.println (A + " " + B + " " + C + " " + D + " " + F + " " + det);
0479: */
0480: if (det >= FUZZ) {
0481: // rays are not parallel
0482: E = -ray1dir.dot(diff); //-Dot(ray1.m,diff);
0483: s = B * E - C * D;
0484: t = B * D - A * E;
0485:
0486: if (s >= 0) {
0487: if (t >= 0) { // region 0 (interior)
0488: // minimum at two interior points of rays
0489: double invDet = 1.0f / det;
0490: s *= invDet;
0491: t *= invDet;
0492: if (ray0int != null)
0493: ray0int.scaleAdd(s, ray0dir, ray0orig);
0494: if (ray1int != null)
0495: ray1int.scaleAdd(t, ray1dir, ray1orig);
0496: if (param != null) {
0497: param[0] = s;
0498: param[1] = t;
0499: }
0500: return DIST(s * (A * s + B * t + 2 * D) + t
0501: * (B * s + C * t + 2 * E) + F);
0502: } else { // region 3 (side)
0503: t = 0;
0504: if (D >= 0) {
0505: s = 0;
0506: if (ray0int != null)
0507: ray0int.set(ray0orig);
0508: if (ray1int != null)
0509: ray1int.set(ray1orig);
0510: if (param != null) {
0511: param[0] = s;
0512: param[1] = t;
0513: }
0514: return DIST(F);
0515: } else {
0516: s = -D / A;
0517: if (ray0int != null)
0518: ray0int.scaleAdd(s, ray0dir, ray0orig);
0519: if (ray1int != null)
0520: ray1int.set(ray1orig);
0521: if (param != null) {
0522: param[0] = s;
0523: param[1] = t;
0524: }
0525: return DIST(D * s + F);
0526: }
0527: }
0528: } else {
0529: if (t >= 0) { // region 1 (side)
0530: s = 0;
0531: if (E >= 0) {
0532: t = 0;
0533: if (ray0int != null)
0534: ray0int.set(ray0orig);
0535: if (ray1int != null)
0536: ray1int.set(ray1orig);
0537: if (param != null) {
0538: param[0] = s;
0539: param[1] = t;
0540: }
0541: return DIST(F);
0542: } else {
0543: t = -E / C;
0544: if (ray0int != null)
0545: ray0int.set(ray0orig);
0546: if (ray1int != null)
0547: ray1int.scaleAdd(t, ray1dir, ray1orig);
0548: if (param != null) {
0549: param[0] = s;
0550: param[1] = t;
0551: }
0552: return DIST(E * t + F);
0553: }
0554: } else { // region 2 (corner)
0555: if (D < 0) {
0556: s = -D / A;
0557: t = 0;
0558: if (ray0int != null)
0559: ray0int.scaleAdd(s, ray0dir, ray0orig);
0560: if (ray1int != null)
0561: ray1int.set(ray1orig);
0562: if (param != null) {
0563: param[0] = s;
0564: param[1] = t;
0565: }
0566: return DIST(D * s + F);
0567: } else {
0568: s = 0;
0569: if (E >= 0) {
0570: t = 0;
0571: if (ray0int != null)
0572: ray0int.set(ray0orig);
0573: if (ray1int != null)
0574: ray1int.set(ray1orig);
0575: if (param != null) {
0576: param[0] = s;
0577: param[1] = t;
0578: }
0579: return DIST(F);
0580: } else {
0581: t = -E / C;
0582: if (ray0int != null)
0583: ray0int.set(ray0orig);
0584: if (ray1int != null)
0585: ray1int.scaleAdd(t, ray1dir, ray1orig);
0586: if (param != null) {
0587: param[0] = s;
0588: param[1] = t;
0589: }
0590: return DIST(E * t + F);
0591: }
0592: }
0593: }
0594: }
0595: } else {
0596: // rays are parallel
0597: if (B > 0) {
0598: // opposite direction vectors
0599: t = 0;
0600: if (D >= 0) {
0601: s = 0;
0602: if (ray0int != null)
0603: ray0int.set(ray0orig);
0604: if (ray1int != null)
0605: ray1int.set(ray1orig);
0606: if (param != null) {
0607: param[0] = s;
0608: param[1] = t;
0609: }
0610: return DIST(F);
0611: } else {
0612: s = -D / A;
0613: if (ray0int != null)
0614: ray0int.scaleAdd(s, ray0dir, ray0orig);
0615: if (ray1int != null)
0616: ray1int.set(ray1orig);
0617: if (param != null) {
0618: param[0] = s;
0619: param[1] = t;
0620: }
0621: return DIST(D * s + F);
0622: }
0623: } else {
0624: // same direction vectors
0625: if (D >= 0) {
0626: E = ray1dir.dot(diff); //-Dot(ray1.m,diff);
0627: s = 0;
0628: t = -E / C;
0629: if (ray0int != null)
0630: ray0int.set(ray0orig);
0631: if (ray1int != null)
0632: ray1int.scaleAdd(t, ray1dir, ray1orig);
0633: if (param != null) {
0634: param[0] = s;
0635: param[1] = t;
0636: }
0637: return DIST(E * t + F);
0638: } else {
0639: s = -D / A;
0640: t = 0;
0641: if (ray0int != null)
0642: ray0int.scaleAdd(s, ray0dir, ray0orig);
0643: if (ray1int != null)
0644: ray1int.set(ray1orig);
0645: if (param != null) {
0646: param[0] = s;
0647: param[1] = t;
0648: }
0649: return DIST(D * s + F);
0650: }
0651: }
0652: }
0653: }
0654:
0655: /**
0656: * Minimum pt to ray distance. Returns the square of the distance.
0657: * @param pt The point
0658: * @param rayorig Origin of the ray
0659: * @param raydir Direction of the ray
0660: * @return the square of the minimum distance between the point and the ray
0661: */
0662: static public double pointToRay(Point3d pt, Point3d rayorig,
0663: Vector3d raydir) {
0664: return pointToRay(pt, rayorig, raydir, null, null);
0665: }
0666:
0667: /**
0668: * Minimum pt to ray distance. Returns the square of the distance.
0669: *
0670: * @param pt The point
0671: *
0672: * @param rayorig Origin of the ray
0673: *
0674: * @param raydir Direction of the ray
0675: *
0676: * @param rayint If non-null, will be filled with the coordinates of
0677: * the point corresponding to the minimum distance on the ray.
0678: *
0679: * @param param An array of one double, will be filled with the
0680: * parametric factors used to find the point of shortest distance on
0681: * the ray (ray = O +sD, with O=origin and D=direction). param[0]
0682: * will contain the parameter for the ray.
0683: *
0684: * @return the square of the minimum distance between the point and the ray
0685: */
0686: static public double pointToRay(Point3d pt, Point3d rayorig,
0687: Vector3d raydir, Point3d rayint, double[] param) {
0688:
0689: double t;
0690:
0691: Vector3d diff = new Vector3d();
0692: diff.sub(pt, rayorig);
0693: t = raydir.dot(diff); //Dot(ray.m,diff);
0694:
0695: if (t <= 0.0) {
0696: t = 0.0; // behind start of ray
0697: if (rayint != null)
0698: rayint.set(rayorig);
0699: if (param != null) {
0700: param[0] = t;
0701: }
0702: } else {
0703: t /= raydir.dot(raydir); //Dot(ray.m,ray.m);
0704: diff.scaleAdd(-t, raydir, diff); // diff = diff - t*ray.m;
0705: if (rayint != null)
0706: rayint.scaleAdd(t, raydir, rayorig);
0707: if (param != null) {
0708: param[0] = t;
0709: }
0710: }
0711: return diff.dot(diff);
0712: }
0713:
0714: /**
0715: * Minimum pt to segment distance. Returns the square of the distance.
0716: */
0717: static public double pointToSegment(Point3d pt, Point3d segstart,
0718: Point3d segend) {
0719: return pointToSegment(pt, segstart, segend, null, null);
0720: }
0721:
0722: /**
0723: * Minimum pt to segment distance. Returns the square of the distance.
0724: */
0725: static public double pointToSegment(Point3d pt, Point3d segstart,
0726: Point3d segend, Point3d segint, double[] param) {
0727:
0728: double t;
0729: Vector3d segdir = new Vector3d();
0730: segdir.sub(segend, segstart);
0731: Vector3d diff = new Vector3d();
0732: diff.sub(pt, segstart);
0733: t = segdir.dot(diff); //Dot(seg.m,diff);
0734:
0735: if (t <= 0.0) {
0736: t = 0.0f;
0737: if (segint != null)
0738: segint.set(segstart);
0739: if (param != null) {
0740: param[0] = t;
0741: }
0742: } else {
0743: double mDotm = segdir.dot(segdir); //Dot(seg.m,seg.m);
0744: if (t >= mDotm) {
0745: t = 1.0f;
0746: diff.sub(segdir);
0747: if (segint != null)
0748: segint.set(segend);
0749: if (param != null) {
0750: param[0] = t;
0751: }
0752: } else {
0753: t /= mDotm;
0754: diff.scaleAdd(-t, segdir, diff); //diff = diff - t*seg.m;
0755: if (segint != null)
0756: segint.scaleAdd(t, segdir, segstart);
0757: if (param != null) {
0758: param[0] = t;
0759: }
0760: }
0761: }
0762: return diff.dot(diff); //DIST(diff);
0763: }
0764:
0765: /**
0766: * Minimum segment to segment distance. Returns the square of the distance.
0767: * @param seg0start the start of segment 0
0768: * @param seg0end the end of segment 0
0769: * @param seg1start the start of segment 1
0770: * @param seg1end the end of segment 1
0771: * @return the square of the minimum distance from segment to segment
0772: */
0773: static public double segmentToSegment(Point3d seg0start,
0774: Point3d seg0end, Point3d seg1start, Point3d seg1end) {
0775: return segmentToSegment(seg0start, seg0end, seg1start, seg1end,
0776: null, null, null);
0777: }
0778:
0779: /**
0780: * Minimum segment to segment distance. Returns the square of the distance.
0781: *
0782: * @param seg0start the start of segment 0
0783: *
0784: * @param seg0end the end of segment 0
0785: *
0786: * @param seg1start the start of segment 1
0787: *
0788: * @param seg1end the end of segment 1
0789: *
0790: * @param seg0int If non-null, will be filled with the coordinates
0791: * of the point corresponding to the minimum distance on segment 0.
0792: *
0793: * @param seg1int If non-null, will be filled with the coordinates
0794: * of the point corresponding to the minimum distance on segment 1.
0795: *
0796: * @param param An array of two doubles, will be filled with the
0797: * parametric factors used to find the point of shortest distance on
0798: * each primitive (segment = O +sD, with O=origin and
0799: * D=direction). param[0] will contain the parameter for segment 0,
0800: * and param[1] the parameter for segment 1.
0801: *
0802: * @return the square of the minimum distance from segment to segment
0803: */
0804: static public double segmentToSegment(Point3d seg0start,
0805: Point3d seg0end, Point3d seg1start, Point3d seg1end,
0806: Point3d seg0int, Point3d seg1int, double[] param) {
0807: double s, t;
0808:
0809: Vector3d diff = new Vector3d();
0810: diff.sub(seg0start, seg1start);
0811:
0812: Vector3d seg0dir = new Vector3d();
0813: seg0dir.sub(seg0end, seg0start);
0814: Vector3d seg1dir = new Vector3d();
0815: seg1dir.sub(seg1end, seg1start);
0816:
0817: double A = seg0dir.dot(seg0dir); //Dot(seg0dir,seg0dir);
0818: double B = -seg0dir.dot(seg1dir); //-Dot(seg0dir,seg1dir);
0819: double C = seg1dir.dot(seg1dir); //Dot(seg1dir,seg1dir);
0820: double D = seg0dir.dot(diff); //Dot(seg0dir,diff);
0821: double E; // -Dot(seg1dir,diff), defer until needed
0822: double F = diff.dot(diff); //Dot(diff,diff);
0823: double det = Math.abs(A * C - B * B); // A*C-B*B = |Cross(M0,M1)|^2 >= 0
0824:
0825: double tmp;
0826:
0827: if (det >= FUZZ) {
0828: // line segments are not parallel
0829: E = -seg1dir.dot(diff); //-Dot(seg1dir,diff);
0830: s = B * E - C * D;
0831: t = B * D - A * E;
0832:
0833: if (s >= 0) {
0834: if (s <= det) {
0835: if (t >= 0) {
0836: if (t <= det) { // region 0 (interior)
0837: // minimum at two interior points of 3D lines
0838: double invDet = 1.0f / det;
0839: s *= invDet;
0840: t *= invDet;
0841: if (seg0int != null)
0842: seg0int.scaleAdd(s, seg0dir, seg0start);
0843: if (seg1int != null)
0844: seg1int.scaleAdd(t, seg1dir, seg1start);
0845: if (param != null) {
0846: param[0] = s;
0847: param[1] = t;
0848: }
0849: return DIST(s * (A * s + B * t + 2 * D) + t
0850: * (B * s + C * t + 2 * E) + F);
0851: } else { // region 3 (side)
0852: t = 1;
0853: tmp = B + D;
0854: if (tmp >= 0) {
0855: s = 0;
0856: if (seg0int != null)
0857: seg0int.set(seg0start);
0858: if (seg1int != null)
0859: seg1int.set(seg1end);
0860: if (param != null) {
0861: param[0] = s;
0862: param[1] = t;
0863: }
0864: return DIST(C + 2 * E + F);
0865: } else if (-tmp >= A) {
0866: s = 1;
0867: if (seg0int != null)
0868: seg0int.set(seg0end);
0869: if (seg1int != null)
0870: seg1int.set(seg1end);
0871: if (param != null) {
0872: param[0] = s;
0873: param[1] = t;
0874: }
0875: return DIST(A + C + F + 2 * (E + tmp));
0876: } else {
0877: s = -tmp / A;
0878: if (seg0int != null)
0879: seg0int.scaleAdd(s, seg0dir,
0880: seg0start);
0881: if (seg1int != null)
0882: seg1int.set(seg1end);
0883: if (param != null) {
0884: param[0] = s;
0885: param[1] = t;
0886: }
0887: return DIST(tmp * s + C + 2 * E + F);
0888: }
0889: }
0890: } else { // region 7 (side)
0891: t = 0;
0892: if (D >= 0) {
0893: s = 0;
0894: if (seg0int != null)
0895: seg0int.set(seg0start);
0896: if (seg1int != null)
0897: seg1int.set(seg1start);
0898: if (param != null) {
0899: param[0] = s;
0900: param[1] = t;
0901: }
0902: return DIST(F);
0903: } else if (-D >= A) {
0904: s = 1;
0905: if (seg0int != null)
0906: seg0int.set(seg0end);
0907: if (seg1int != null)
0908: seg1int.set(seg1start);
0909: if (param != null) {
0910: param[0] = s;
0911: param[1] = t;
0912: }
0913: return DIST(A + 2 * D + F);
0914: } else {
0915: s = -D / A;
0916: if (seg0int != null)
0917: seg0int.scaleAdd(s, seg0dir, seg0start);
0918: if (seg1int != null)
0919: seg1int.set(seg1start);
0920: if (param != null) {
0921: param[0] = s;
0922: param[1] = t;
0923: }
0924: return DIST(D * s + F);
0925: }
0926: }
0927: } else {
0928: if (t >= 0) {
0929: if (t <= det) { // region 1 (side)
0930: s = 1;
0931: tmp = B + E;
0932: if (tmp >= 0) {
0933: t = 0;
0934: if (seg0int != null)
0935: seg0int.set(seg0end);
0936: if (seg1int != null)
0937: seg1int.set(seg1start);
0938: if (param != null) {
0939: param[0] = s;
0940: param[1] = t;
0941: }
0942: return DIST(A + 2 * D + F);
0943: } else if (-tmp >= C) {
0944: t = 1;
0945: if (seg0int != null)
0946: seg0int.set(seg0end);
0947: if (seg1int != null)
0948: seg1int.set(seg1end);
0949: if (param != null) {
0950: param[0] = s;
0951: param[1] = t;
0952: }
0953: return DIST(A + C + F + 2 * (D + tmp));
0954: } else {
0955: t = -tmp / C;
0956: if (seg0int != null)
0957: seg0int.set(seg0end);
0958: if (seg1int != null)
0959: seg1int.scaleAdd(t, seg1dir,
0960: seg1start);
0961: if (param != null) {
0962: param[0] = s;
0963: param[1] = t;
0964: }
0965: return DIST(tmp * t + A + 2 * D + F);
0966: }
0967: } else { // region 2 (corner)
0968: tmp = B + D;
0969: if (-tmp <= A) {
0970: t = 1;
0971: if (tmp >= 0) {
0972: s = 0;
0973: if (seg0int != null)
0974: seg0int.set(seg0start);
0975: if (seg1int != null)
0976: seg1int.set(seg1end);
0977: if (param != null) {
0978: param[0] = s;
0979: param[1] = t;
0980: }
0981: return DIST(C + 2 * E + F);
0982: } else {
0983: s = -tmp / A;
0984: if (seg0int != null)
0985: seg0int.scaleAdd(s, seg0dir,
0986: seg0start);
0987: if (seg1int != null)
0988: seg1int.set(seg1end);
0989: if (param != null) {
0990: param[0] = s;
0991: param[1] = t;
0992: }
0993: return DIST(tmp * s + C + 2 * E + F);
0994: }
0995: } else {
0996: s = 1;
0997: tmp = B + E;
0998: if (tmp >= 0) {
0999: t = 0;
1000: if (seg0int != null)
1001: seg0int.set(seg0end);
1002: if (seg1int != null)
1003: seg1int.set(seg1start);
1004: if (param != null) {
1005: param[0] = s;
1006: param[1] = t;
1007: }
1008: return DIST(A + 2 * D + F);
1009: } else if (-tmp >= C) {
1010: t = 1;
1011: if (seg0int != null)
1012: seg0int.set(seg0end);
1013: if (seg1int != null)
1014: seg1int.set(seg1end);
1015: if (param != null) {
1016: param[0] = s;
1017: param[1] = t;
1018: }
1019: return DIST(A + C + F + 2
1020: * (D + tmp));
1021: } else {
1022: t = -tmp / C;
1023: if (seg0int != null)
1024: seg0int.set(seg0end);
1025: if (seg1int != null)
1026: seg1int.scaleAdd(t, seg1dir,
1027: seg1start);
1028: if (param != null) {
1029: param[0] = s;
1030: param[1] = t;
1031: }
1032: return DIST(tmp * t + A + 2 * D + F);
1033: }
1034: }
1035: }
1036: } else { // region 8 (corner)
1037: if (-D < A) {
1038: t = 0;
1039: if (D >= 0) {
1040: s = 0;
1041: if (seg0int != null)
1042: seg0int.set(seg0start);
1043: if (seg1int != null)
1044: seg1int.set(seg1start);
1045: if (param != null) {
1046: param[0] = s;
1047: param[1] = t;
1048: }
1049: return DIST(F);
1050: } else {
1051: s = -D / A;
1052: if (seg0int != null)
1053: seg0int.scaleAdd(s, seg0dir,
1054: seg0start);
1055: if (seg1int != null)
1056: seg1int.set(seg1start);
1057: if (param != null) {
1058: param[0] = s;
1059: param[1] = t;
1060: }
1061: return DIST(D * s + F);
1062: }
1063: } else {
1064: s = 1;
1065: tmp = B + E;
1066: if (tmp >= 0) {
1067: t = 0;
1068: if (seg0int != null)
1069: seg0int.set(seg0end);
1070: if (seg1int != null)
1071: seg1int.set(seg1start);
1072: if (param != null) {
1073: param[0] = s;
1074: param[1] = t;
1075: }
1076: return DIST(A + 2 * D + F);
1077: } else if (-tmp >= C) {
1078: t = 1;
1079: if (seg0int != null)
1080: seg0int.set(seg0end);
1081: if (seg1int != null)
1082: seg1int.set(seg1end);
1083: if (param != null) {
1084: param[0] = s;
1085: param[1] = t;
1086: }
1087: return DIST(A + C + F + 2 * (D + tmp));
1088: } else {
1089: t = -tmp / C;
1090: if (seg0int != null)
1091: seg0int.set(seg0end);
1092: if (seg1int != null)
1093: seg1int.scaleAdd(t, seg1dir,
1094: seg1start);
1095: if (param != null) {
1096: param[0] = s;
1097: param[1] = t;
1098: }
1099: return DIST(tmp * t + A + 2 * D + F);
1100: }
1101: }
1102: }
1103: }
1104: } else {
1105: if (t >= 0) {
1106: if (t <= det) { // region 5 (side)
1107: s = 0;
1108: if (E >= 0) {
1109: t = 0;
1110: if (seg0int != null)
1111: seg0int.set(seg0start);
1112: if (seg1int != null)
1113: seg1int.set(seg1start);
1114: if (param != null) {
1115: param[0] = s;
1116: param[1] = t;
1117: }
1118: return DIST(F);
1119: } else if (-E >= C) {
1120: t = 1;
1121: if (seg0int != null)
1122: seg0int.set(seg0start);
1123: if (seg1int != null)
1124: seg1int.set(seg1end);
1125: if (param != null) {
1126: param[0] = s;
1127: param[1] = t;
1128: }
1129: return DIST(C + 2 * E + F);
1130: } else {
1131: t = -E / C;
1132: if (seg0int != null)
1133: seg0int.set(seg0start);
1134: if (seg1int != null)
1135: seg1int.scaleAdd(t, seg1dir, seg1start);
1136: if (param != null) {
1137: param[0] = s;
1138: param[1] = t;
1139: }
1140: return DIST(E * t + F);
1141: }
1142: } else { // region 4 (corner)
1143: tmp = B + D;
1144: if (tmp < 0) {
1145: t = 1;
1146: if (-tmp >= A) {
1147: s = 1;
1148: if (seg0int != null)
1149: seg0int.set(seg0end);
1150: if (seg1int != null)
1151: seg1int.set(seg1end);
1152: if (param != null) {
1153: param[0] = s;
1154: param[1] = t;
1155: }
1156: return DIST(A + C + F + 2 * (E + tmp));
1157: } else {
1158: s = -tmp / A;
1159: if (seg0int != null)
1160: seg0int.scaleAdd(s, seg0dir,
1161: seg0start);
1162: if (seg1int != null)
1163: seg1int.set(seg1end);
1164: if (param != null) {
1165: param[0] = s;
1166: param[1] = t;
1167: }
1168: return DIST(tmp * s + C + 2 * E + F);
1169: }
1170: } else {
1171: s = 0;
1172: if (E >= 0) {
1173: t = 0;
1174: if (seg0int != null)
1175: seg0int.set(seg0start);
1176: if (seg1int != null)
1177: seg1int.set(seg1start);
1178: if (param != null) {
1179: param[0] = s;
1180: param[1] = t;
1181: }
1182: return DIST(F);
1183: } else if (-E >= C) {
1184: t = 1;
1185: if (seg0int != null)
1186: seg0int.set(seg0start);
1187: if (seg1int != null)
1188: seg1int.set(seg1end);
1189: if (param != null) {
1190: param[0] = s;
1191: param[1] = t;
1192: }
1193: return DIST(C + 2 * E + F);
1194: } else {
1195: t = -E / C;
1196: if (seg0int != null)
1197: seg0int.set(seg0start);
1198: if (seg1int != null)
1199: seg1int.scaleAdd(t, seg1dir,
1200: seg1start);
1201: if (param != null) {
1202: param[0] = s;
1203: param[1] = t;
1204: }
1205: return DIST(E * t + F);
1206: }
1207: }
1208: }
1209: } else { // region 6 (corner)
1210: if (D < 0) {
1211: t = 0;
1212: if (-D >= A) {
1213: s = 1;
1214: if (seg0int != null)
1215: seg0int.set(seg0end);
1216: if (seg1int != null)
1217: seg1int.set(seg1start);
1218: if (param != null) {
1219: param[0] = s;
1220: param[1] = t;
1221: }
1222: return DIST(A + 2 * D + F);
1223: } else {
1224: s = -D / A;
1225: if (seg0int != null)
1226: seg0int.scaleAdd(s, seg0dir, seg0start);
1227: if (seg1int != null)
1228: seg1int.set(seg1start);
1229: if (param != null) {
1230: param[0] = s;
1231: param[1] = t;
1232: }
1233: return DIST(D * s + F);
1234: }
1235: } else {
1236: s = 0;
1237: if (E >= 0) {
1238: t = 0;
1239: if (seg0int != null)
1240: seg0int.set(seg0start);
1241: if (seg1int != null)
1242: seg1int.set(seg1start);
1243: if (param != null) {
1244: param[0] = s;
1245: param[1] = t;
1246: }
1247: return DIST(F);
1248: } else if (-E >= C) {
1249: t = 1;
1250: if (seg0int != null)
1251: seg0int.set(seg0start);
1252: if (seg1int != null)
1253: seg1int.set(seg1end);
1254: if (param != null) {
1255: param[0] = s;
1256: param[1] = t;
1257: }
1258: return DIST(C + 2 * E + F);
1259: } else {
1260: t = -E / C;
1261: if (seg0int != null)
1262: seg0int.set(seg0start);
1263: if (seg1int != null)
1264: seg1int.scaleAdd(t, seg1dir, seg1start);
1265: if (param != null) {
1266: param[0] = s;
1267: param[1] = t;
1268: }
1269: return DIST(E * t + F);
1270: }
1271: }
1272: }
1273: }
1274: } else {
1275: // line segments are parallel
1276: if (B > 0) {
1277: // direction vectors form an obtuse angle
1278: if (D >= 0) {
1279: s = 0;
1280: t = 0;
1281: if (seg0int != null)
1282: seg0int.set(seg0start);
1283: if (seg1int != null)
1284: seg1int.set(seg1start);
1285: if (param != null) {
1286: param[0] = s;
1287: param[1] = t;
1288: }
1289: return DIST(F);
1290: } else if (-D <= A) {
1291: s = -D / A;
1292: t = 0;
1293: if (seg0int != null)
1294: seg0int.scaleAdd(s, seg0dir, seg0start);
1295: if (seg1int != null)
1296: seg1int.set(seg1start);
1297: if (param != null) {
1298: param[0] = s;
1299: param[1] = t;
1300: }
1301: return DIST(D * s + F);
1302: } else {
1303: E = -seg1dir.dot(diff); //-Dot(seg1dir,diff);
1304: s = 1;
1305: tmp = A + D;
1306: if (-tmp >= B) {
1307: t = 1;
1308: if (seg0int != null)
1309: seg0int.set(seg0end);
1310: if (seg1int != null)
1311: seg1int.set(seg1end);
1312: if (param != null) {
1313: param[0] = s;
1314: param[1] = t;
1315: }
1316: return DIST(A + C + F + 2 * (B + D + E));
1317: } else {
1318: t = -tmp / B;
1319: if (seg0int != null)
1320: seg0int.set(seg0end);
1321: if (seg1int != null)
1322: seg1int.scaleAdd(t, seg1dir, seg1start);
1323: if (param != null) {
1324: param[0] = s;
1325: param[1] = t;
1326: }
1327: return DIST(A + 2 * D + F + t
1328: * (C * t + 2 * (B + E)));
1329: }
1330: }
1331: } else {
1332: // direction vectors form an acute angle
1333: if (-D >= A) {
1334: s = 1;
1335: t = 0;
1336: if (seg0int != null)
1337: seg0int.set(seg0end);
1338: if (seg1int != null)
1339: seg1int.set(seg1start);
1340: if (param != null) {
1341: param[0] = s;
1342: param[1] = t;
1343: }
1344: return DIST(A + 2 * D + F);
1345: } else if (D <= 0) {
1346: s = -D / A;
1347: t = 0;
1348: if (seg0int != null)
1349: seg0int.scaleAdd(s, seg0dir, seg0start);
1350: if (seg1int != null)
1351: seg1int.set(seg1start);
1352: if (param != null) {
1353: param[0] = s;
1354: param[1] = t;
1355: }
1356: return DIST(D * s + F);
1357: } else {
1358: E = -seg1dir.dot(diff); //-Dot(seg1dir,diff);
1359: s = 0;
1360: if (D >= -B) {
1361: t = 1;
1362: if (seg0int != null)
1363: seg0int.set(seg0start);
1364: if (seg1int != null)
1365: seg1int.set(seg1end);
1366: if (param != null) {
1367: param[0] = s;
1368: param[1] = t;
1369: }
1370: return DIST(C + 2 * E + F);
1371: } else {
1372: t = -D / B;
1373: if (seg0int != null)
1374: seg0int.set(seg0start);
1375: if (seg1int != null)
1376: seg1int.scaleAdd(t, seg1dir, seg1start);
1377: if (param != null) {
1378: param[0] = s;
1379: param[1] = t;
1380: }
1381: return DIST(F + t * (2 * E + C * t));
1382: }
1383: }
1384: }
1385: }
1386: }
1387: }
|