0001: /*
0002: * $RCSfile: PickResult.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.6 $
0041: * $Date: 2007/02/09 17:20:26 $
0042: * $State: Exp $
0043: */
0044:
0045: package com.sun.j3d.utils.picking;
0046:
0047: import javax.vecmath.*;
0048: import javax.media.j3d.*;
0049: import java.util.ArrayList;
0050: import com.sun.j3d.utils.geometry.Primitive;
0051: import com.sun.j3d.internal.*;
0052:
0053: /**
0054: * Stores information about a pick hit.
0055: * Detailed information about the pick and each intersection of the PickShape
0056: * with the picked Node can be inquired. The PickResult is constructed with
0057: * basic information and more detailed information is generated as needed. The
0058: * additional information is only available if capability bits on the scene
0059: * graph Nodes are set properly;
0060: * <A HREF="PickTool.html#setCapabilities(javax.media.j3d.Node, int)">
0061: * <code>PickTool.setCapabilties(Node, int)</code></A>
0062: * can
0063: * be used to ensure correct capabilites are set. Inquiring data which is not
0064: * available due to capabilties not being set will generate a
0065: * <code>CapabilityNotSet</code> exception.
0066: * <p>
0067: * A PickResult can be used to calculate intersections on Node which is not part
0068: * of a live scene graph using the constructor which takes a local to VWorld
0069: * transformation for the Node.
0070: * <p>
0071: * Pick hits on TriangleStrip primitives will store the triangle points in the
0072: * PickIntersection with
0073: * the verticies in counter-clockwise order. For triangles which start with
0074: * an odd numbered vertex this will be the the opposite of the
0075: * order of the points in the TriangleStrip.
0076: * This way the triangle in
0077: * the PickIntersection will display the same was as the triangle in the
0078: * strip.
0079: * <p>
0080: * If the Shape3D being picked has multiple geometry arrays, the arrays are
0081: * stored in the PickResult and referred to by a geometry index.
0082: * <p>
0083: * If the Shape3D refers to a CompressedGeometry, the geometry is decompressed
0084: * into an array of Shape3D nodes which can be inquired. The geometry
0085: * NodeComponents for the Shape3D nodes are stored and used as if the Shape3D
0086: * had multiple geometries. If there are multiple CompressedGeometries on the
0087: * Shape3D, the decompressed Shape3Ds and GeometryArrays will be stored
0088: * sequentially.
0089: * <p>
0090: * The intersection point for Morph nodes cannot be calculated using the
0091: * displayed geometry
0092: * due to limitations in the current Java3D core API (the current
0093: * geometry of the the Morph cannot be inquired). Instead
0094: * the geometry at index 0 in the Morph is used. This limitation may
0095: * be eliminated in a future release of Java3D.
0096: */
0097: public class PickResult {
0098:
0099: /* OPEN ISSUES:
0100: -- getInterpolatedTextureCoordinates uses the depricated API faor
0101: getTextureCoordinate(), need to update.
0102: -- Bounds tests don't fill in any picking info.
0103: -- Can't do any intersections with the PickPoint shape.
0104: */
0105:
0106: // Externally used constants
0107: /**
0108: * Flag to pass to
0109: * <CODE>getNode(int)</CODE>
0110: * to return a
0111: * <code>Shape3D</code> node from
0112: * the <code>SceneGraphPath</code>.
0113: */
0114: public static final int SHAPE3D = 0x1;
0115:
0116: /**
0117: * Flag to pass to
0118: * <CODE>getNode(int)</CODE>
0119: * to return a
0120: * <code>Morph</code> node from
0121: * the <code>SceneGraphPath</code>.
0122: */
0123: public static final int MORPH = 0x2;
0124:
0125: /**
0126: * Flag to pass to
0127: * <CODE>getNode(int)</CODE>
0128:
0129: * to return a
0130: * <code>Primitive</code> node from
0131: * the <code>SceneGraphPath</code>.
0132: */
0133: public static final int PRIMITIVE = 0x4;
0134:
0135: /**
0136: * Flag to pass to
0137: * <CODE>getNode(int)</CODE>
0138: * to return a
0139: * <code>Link</code> node from
0140: * the <code>SceneGraphPath</code>.
0141: */
0142: public static final int LINK = 0x8;
0143:
0144: /**
0145: * Flag to pass to
0146: * <CODE>getNode(int)</CODE>
0147: * to return a
0148: * <code>Group</code> node from
0149: * the <code>SceneGraphPath</code>.
0150: */
0151: public static final int GROUP = 0x10;
0152:
0153: /**
0154: * Flag to pass to
0155: * <CODE>getNode(int)</CODE>
0156: * to return a
0157: * <code>TransformGroup</code> node from
0158: * the <code>SceneGraphPath</code>.
0159: */
0160: public static final int TRANSFORM_GROUP = 0x20;
0161:
0162: /**
0163: * Flag to pass to
0164: * <CODE>getNode(int)</CODE>
0165: * to return a
0166: * <code>BranchGroup</code> node from
0167: * the <code>SceneGraphPath</code>.
0168: */
0169: public static final int BRANCH_GROUP = 0x40;
0170:
0171: /**
0172: * Flag to pass to
0173: * <CODE>getNode(int)</CODE>
0174: * to return a
0175: * <code>Switch</code> node from
0176: * the <code>SceneGraphPath</code>.
0177: */
0178: public static final int SWITCH = 0x80;
0179:
0180: /* =================== ATTRIBUTES ======================= */
0181: static boolean debug = false;
0182:
0183: /** if true, find only the first intersection */
0184: private boolean firstIntersectOnly = false;
0185:
0186: /** Stored SceneGraphPath */
0187: private SceneGraphPath pickedSceneGraphPath = null;
0188:
0189: /** Picked node: shape3d, text3d, etc. */
0190: private Node pickedNode = null;
0191:
0192: /** GeometryArray(s) of the picked node */
0193: private GeometryArray[] geometryArrays = null;
0194:
0195: /** Shape3Ds from CompressedGeometry on the picked node */
0196: private Shape3D[] compressGeomShape3Ds = null;
0197:
0198: /** Transform to World Coordinates */
0199: private Transform3D localToVWorld = null;
0200:
0201: /** the pick shape to use for intersections */
0202: private PickShape pickShape = null;
0203: /* data derived from the pick shape */
0204: private int pickShapeType = -1;
0205: private Vector3d pickShapeDir = null;
0206: private Point3d pickShapeStart = null;
0207: private Point3d pickShapeEnd = null;
0208: private Bounds pickShapeBounds = null;
0209:
0210: static final Point3d zeroPnt = new Point3d();
0211:
0212: /** ArrayList to store intersection results
0213: * Used in PickTool
0214: */
0215: ArrayList intersections = null;
0216:
0217: // internal constants used for intersections
0218: static final double FUZZ = 1E-6; /* fuzziness factor used to determine
0219: if two lines are parallel */
0220: static final int PICK_SHAPE_RAY = 1;
0221: static final int PICK_SHAPE_SEGMENT = 2;
0222: static final int PICK_SHAPE_POINT = 3;
0223: static final int PICK_SHAPE_BOUNDING_BOX = 4;
0224: static final int PICK_SHAPE_BOUNDING_SPHERE = 5;
0225: static final int PICK_SHAPE_BOUNDING_POLYTOPE = 6;
0226: static final int PICK_SHAPE_CYLINDER = 7;
0227: static final int PICK_SHAPE_CONE = 8;
0228:
0229: static final double EPS = 1.0e-13;
0230:
0231: /* =================== METHODS ======================= */
0232:
0233: /** Default constructor. */
0234: PickResult() {
0235: }
0236:
0237: /** Construct a PickResult using a SceneGraphPath
0238: @param sgp SceneGraphPath associated with this PickResult
0239: @param ps The pickShape to intersect against
0240: */
0241: public PickResult(SceneGraphPath sgp, PickShape ps) {
0242: pickedSceneGraphPath = sgp;
0243: pickedNode = sgp.getObject();
0244: localToVWorld = sgp.getTransform();
0245: pickShape = ps;
0246: initPickShape();
0247: }
0248:
0249: /** Construct a PickResult using the Node and localToVWorld transform
0250: @param pn The picked node.
0251: @param l2vw The local to VWorld transformation for the node
0252: @param ps The PickShape to intersect against
0253: @throws IllegalArgumentException If the node is not a Morph or Shape3D.
0254: */
0255: public PickResult(Node pn, Transform3D l2vw, PickShape ps) {
0256: if ((pn instanceof Shape3D) || (pn instanceof Morph)) {
0257: pickedNode = pn;
0258: localToVWorld = l2vw;
0259: pickShape = ps;
0260: initPickShape();
0261: } else {
0262: throw new IllegalArgumentException();
0263: }
0264: }
0265:
0266: void initPickShape() {
0267: if (pickShape instanceof PickRay) {
0268: if (pickShapeStart == null)
0269: pickShapeStart = new Point3d();
0270: if (pickShapeDir == null)
0271: pickShapeDir = new Vector3d();
0272: ((PickRay) pickShape).get(pickShapeStart, pickShapeDir);
0273: pickShapeType = PICK_SHAPE_RAY;
0274: } else if (pickShape instanceof PickSegment) {
0275: if (pickShapeStart == null)
0276: pickShapeStart = new Point3d();
0277: if (pickShapeEnd == null)
0278: pickShapeEnd = new Point3d();
0279: if (pickShapeDir == null)
0280: pickShapeDir = new Vector3d();
0281: ((PickSegment) pickShape).get(pickShapeStart, pickShapeEnd);
0282: pickShapeDir.set(pickShapeEnd.x - pickShapeStart.x,
0283: pickShapeEnd.y - pickShapeStart.y, pickShapeEnd.z
0284: - pickShapeStart.z);
0285: pickShapeType = PICK_SHAPE_SEGMENT;
0286: } else if (pickShape instanceof PickBounds) {
0287: pickShapeBounds = ((PickBounds) pickShape).get();
0288: if (pickShapeBounds instanceof BoundingBox)
0289: pickShapeType = PICK_SHAPE_BOUNDING_BOX;
0290: else if (pickShapeBounds instanceof BoundingSphere)
0291: pickShapeType = PICK_SHAPE_BOUNDING_SPHERE;
0292: else if (pickShapeBounds instanceof BoundingPolytope)
0293: pickShapeType = PICK_SHAPE_BOUNDING_POLYTOPE;
0294: } else if (pickShape instanceof PickPoint) {
0295: throw new RuntimeException(
0296: "PickPoint doesn't make sense for geometry-based picking. Java 3D doesn't have spatial information of the surface. Should use PickBounds with BoundingSphere and set radius to a epsilon tolerance.");
0297: } else if (pickShape instanceof PickCylinder) {
0298: pickShapeType = PICK_SHAPE_CYLINDER;
0299: } else if (pickShape instanceof PickCone) {
0300: pickShapeType = PICK_SHAPE_CONE;
0301: } else {
0302: throw new RuntimeException(
0303: "PickShape not supported for intersection");
0304: }
0305: }
0306:
0307: /** Get the SceneGraphPath. This will be null if the non SceneGraphPath
0308: * constructor was used.
0309: */
0310: public SceneGraphPath getSceneGraphPath() {
0311: /* Q: should this return a copy */
0312: return pickedSceneGraphPath;
0313: }
0314:
0315: /** Get the localToVworld transform for the Node
0316: */
0317: public Transform3D getLocalToVworld() {
0318: return localToVWorld;
0319: }
0320:
0321: /** Get the GeometryArray at index 0 for the picked node
0322: */
0323: public GeometryArray getGeometryArray() {
0324: if (geometryArrays == null) {
0325: storeGeometry();
0326: }
0327: return geometryArrays[0];
0328: }
0329:
0330: /** Get the array of GeometryArrays for the picked node
0331: */
0332: public GeometryArray[] getGeometryArrays() {
0333: if (geometryArrays == null) {
0334: storeGeometry();
0335: }
0336: return geometryArrays;
0337: }
0338:
0339: /** Get the number of GeometryArrays for the picked node
0340: */
0341: public int numGeometryArrays() {
0342: if (geometryArrays == null) {
0343: storeGeometry();
0344: }
0345: return geometryArrays.length;
0346: }
0347:
0348: /** Get the number of Shape3Ds that came from decompressing a
0349: CompressedGeometry on the picked node.
0350: */
0351: public int numCompressedGeometryShape3Ds() {
0352: if (geometryArrays == null) {
0353: storeGeometry();
0354: }
0355: if (compressGeomShape3Ds == null) {
0356: return 0;
0357: } else {
0358: return compressGeomShape3Ds.length;
0359: }
0360: }
0361:
0362: /** Get the array of Shape3Ds that came from decompressing a
0363: CompressedGeometry on the picked node.
0364: */
0365: public Shape3D[] getCompressedGeometryShape3Ds() {
0366: if (geometryArrays == null) {
0367: storeGeometry();
0368: }
0369: if (compressGeomShape3Ds == null) {
0370: return null;
0371: } else {
0372: return compressGeomShape3Ds;
0373: }
0374: }
0375:
0376: /** Get the PickShape used for intersections
0377: */
0378: public PickShape getPickShape() {
0379: return pickShape;
0380: }
0381:
0382: /** Set the PickResult to find only the first intersection of the PickShape
0383: * with the Node. The default is <code>false</code> (all intersections are
0384: * found)
0385: */
0386: public void setFirstIntersectOnly(boolean flag) {
0387: firstIntersectOnly = flag;
0388: }
0389:
0390: /** Return the "first intersection only" value. */
0391: public boolean getFirstPickEnable() {
0392: return firstIntersectOnly;
0393: }
0394:
0395: /** Returns the number of PickIntersections in the PickResult.
0396: @return the number of intersections
0397: */
0398: public int numIntersections() {
0399: if (intersections == null) {
0400: generateIntersections();
0401: }
0402: return intersections.size();
0403: }
0404:
0405: /** Returns a specific PickIntersection object
0406: @param index the index number
0407: @return the PickIntersection referenced by the index number
0408: */
0409: public PickIntersection getIntersection(int index) {
0410: if (intersections == null) {
0411: generateIntersections();
0412: }
0413: return (PickIntersection) intersections.get(index);
0414: }
0415:
0416: /** Gets the PickIntersection in this PickResult that is closest to a point
0417: @param pt the point to use for distance calculations
0418: @return the closest PickIntersection object
0419: */
0420: public PickIntersection getClosestIntersection(Point3d pt) {
0421: PickIntersection pi = null;
0422: PickIntersection curPi = null;
0423: Point3d curPt = null;
0424: double minDist = Double.MAX_VALUE;
0425: double curDist = 0.0;
0426:
0427: if (pt == null)
0428: return null;
0429:
0430: if (intersections == null) {
0431: generateIntersections();
0432: }
0433:
0434: for (int i = 0; i < intersections.size(); i++) {
0435: if ((null != (curPi = getIntersection(i)))
0436: && (null != (curPt = curPi.getPointCoordinatesVW()))) {
0437: curDist = pt.distance(curPt);
0438: if (curDist < minDist) {
0439: pi = curPi;
0440: minDist = curDist;
0441: }
0442: }
0443: }
0444: return pi;
0445: }
0446:
0447: /**
0448: Returns String representation
0449: @return string representation of this object
0450: */
0451: public String toString() {
0452: String rt = new String("PickResult: sgp:"
0453: + pickedSceneGraphPath + "\n");
0454: if (pickedNode != null)
0455: rt += " node:" + pickedNode;
0456:
0457: // TODO: catch cap not set exceptions and return no intersection info
0458: if (intersections == null) {
0459: generateIntersections();
0460: }
0461:
0462: if (intersections.size() > 0) {
0463: for (int i = 0; i < intersections.size(); i++) {
0464: rt += "\n";
0465: rt += ((PickIntersection) intersections.get(i))
0466: .toString2();
0467: }
0468: }
0469:
0470: return rt;
0471: }
0472:
0473: /** Store the geometry for the node in this PickResult */
0474: private void storeGeometry() {
0475: if (pickedNode instanceof Morph) {
0476: geometryArrays = new GeometryArray[1];
0477: geometryArrays[0] = (GeometryArray) ((Morph) pickedNode)
0478: .getGeometryArray(0);
0479: } else if (pickedNode instanceof Shape3D) {
0480: Shape3D shape = ((Shape3D) pickedNode);
0481: ArrayList geoArrays = new ArrayList();
0482: for (int k = 0; k < shape.numGeometries(); k++) {
0483: Geometry geometry = shape.getGeometry(k);
0484: if (geometry instanceof CompressedGeometry) {
0485: Shape3D[] sa = ((CompressedGeometry) geometry)
0486: .decompress();
0487: // System.out.println ("Decompressed geometry has "+sa.length+
0488: // " Shape3Ds");
0489: if (sa != null) {
0490: for (int j = 0; j < sa.length; j++) {
0491: for (int i = 0; i < sa[j].numGeometries(); i++) {
0492: geoArrays.add(sa[j].getGeometry(i));
0493: }
0494: }
0495: }
0496: if (compressGeomShape3Ds == null) {
0497: // just save the new one
0498: compressGeomShape3Ds = sa;
0499: } else {
0500: // append the the new one on the end of the old array
0501: Shape3D[] save = compressGeomShape3Ds;
0502: int newLength = save.length + sa.length;
0503: compressGeomShape3Ds = new Shape3D[newLength];
0504: System.arraycopy(save, 0, compressGeomShape3Ds,
0505: 0, save.length);
0506: System.arraycopy(sa, 0, compressGeomShape3Ds,
0507: save.length, sa.length);
0508: }
0509: } else if (geometry instanceof GeometryArray) {
0510: geoArrays.add(geometry);
0511: }
0512: }
0513: geometryArrays = new GeometryArray[geoArrays.size()];
0514: for (int i = 0; i < geoArrays.size(); i++) {
0515: geometryArrays[i] = (GeometryArray) geoArrays.get(i);
0516: }
0517: }
0518: if (geometryArrays == null) {
0519: if (pickedNode instanceof Shape3D) {
0520: Shape3D shape = (Shape3D) pickedNode;
0521: }
0522: throw new RuntimeException(
0523: "Type of the picked node is not supported");
0524: }
0525: }
0526:
0527: /** Get the picked node */
0528: public Node getObject() {
0529: // get node from scenegraphpath
0530: if (pickedNode == null) {
0531: storeNode();
0532: }
0533: return pickedNode;
0534: }
0535:
0536: /** Set the picked node */
0537: void setObject(Node n) {
0538: pickedNode = n;
0539: }
0540:
0541: /** Get the first node of a certain type up the SceneGraphPath
0542: @param flags the type of node we are interested in
0543: @return a Node object
0544: */
0545: public Node getNode(int flags) {
0546: if (pickedNode == null) {
0547: storeNode();
0548: }
0549: if ((pickedNode instanceof Shape3D) && ((flags & SHAPE3D) != 0)) {
0550: if (debug)
0551: System.out.println("Shape3D found");
0552: return pickedNode;
0553: } else if ((pickedNode instanceof Morph)
0554: && ((flags & MORPH) != 0)) {
0555: if (debug)
0556: System.out.println("Morph found");
0557: return pickedNode;
0558: } else {
0559: if (pickedSceneGraphPath == null) {
0560: return null;
0561: }
0562: for (int j = pickedSceneGraphPath.nodeCount() - 1; j >= 0; j--) {
0563: Node pNode = pickedSceneGraphPath.getNode(j);
0564: if (debug)
0565: System.out.println("looking at node " + pNode);
0566:
0567: if ((pNode instanceof Primitive)
0568: && ((flags & PRIMITIVE) != 0)) {
0569: if (debug)
0570: System.out.println("Primitive found");
0571: return pNode;
0572: } else if ((pNode instanceof Link)
0573: && ((flags & LINK) != 0)) {
0574: if (debug)
0575: System.out.println("Link found");
0576: return pNode;
0577: } else if ((pNode instanceof Switch)
0578: && ((flags & SWITCH) != 0)) {
0579: if (debug)
0580: System.out.println("Switch found");
0581: return pNode;
0582: } else if ((pNode instanceof TransformGroup)
0583: && ((flags & TRANSFORM_GROUP) != 0)) {
0584: if (debug)
0585: System.out.println("xform group found");
0586: return pNode;
0587: } else if ((pNode instanceof BranchGroup)
0588: && ((flags & BRANCH_GROUP) != 0)) {
0589: if (debug)
0590: System.out.println("Branch group found");
0591: return pNode;
0592: } else if ((pNode instanceof Group)
0593: && ((flags & GROUP) != 0)) {
0594: if (debug)
0595: System.out.println("Group found");
0596: return pNode;
0597: }
0598: }
0599: }
0600: return null; // should not be reached
0601: }
0602:
0603: /** Extract the picked node from the SceneGraphPath */
0604: void storeNode() {
0605: if (pickedSceneGraphPath == null) {
0606: throw new RuntimeException("SceneGraphPath missing");
0607: }
0608: pickedNode = pickedSceneGraphPath.getObject();
0609: }
0610:
0611: /** Fill in the intersections of the Node with the PickShape */
0612: boolean generateIntersections() {
0613: if (geometryArrays == null) {
0614: storeGeometry();
0615: }
0616: intersections = new ArrayList();
0617: int hits = 0;
0618:
0619: for (int i = 0; i < geometryArrays.length; i++) {
0620: if (intersect(i, firstIntersectOnly)) {
0621: if (firstIntersectOnly) {
0622: return true;
0623: } else {
0624: hits++;
0625: }
0626: }
0627: }
0628: return (hits > 0);
0629: }
0630:
0631: /* Takes a GeometryArray object, determines what actual type
0632: * it is (RTTI) and casts it to call the appropriate intersect method.
0633: */
0634: final boolean intersect(int geomIndex, boolean firstpick) {
0635: int offset;
0636: GeometryArray geom = geometryArrays[geomIndex];
0637: int numPts = geom.getVertexCount();
0638: double[] doubleData = null;
0639: float[] floatData = null;
0640: Point3d[] p3dData = null;
0641: Point3f[] p3fData = null;
0642: int vformat = geom.getVertexFormat();
0643: int stride;
0644: boolean retFlag = false;
0645:
0646: if ((vformat & GeometryArray.BY_REFERENCE) == 0) {
0647: doubleData = new double[numPts * 3];
0648: geom.getCoordinates(0, doubleData);
0649: } else {
0650: if ((vformat & GeometryArray.INTERLEAVED) == 0) {
0651: doubleData = geom.getCoordRefDouble();
0652: // If data was set as float then ..
0653: if (doubleData == null) {
0654: floatData = geom.getCoordRefFloat();
0655: if (floatData == null) {
0656: p3fData = geom.getCoordRef3f();
0657: if (p3fData == null) {
0658: p3dData = geom.getCoordRef3d();
0659: }
0660: }
0661: }
0662: } else {
0663: floatData = geom.getInterleavedVertices();
0664: }
0665: }
0666:
0667: Point3d[] pnts = new Point3d[numPts];
0668:
0669: /*
0670: System.out.println("geomIndex : " + geomIndex);
0671: System.out.println("numPts : " + numPts);
0672: System.out.println("firstpick : " + firstpick);
0673: System.out.println("localToVWorld : ");
0674: System.out.println(localToVWorld);
0675: */
0676:
0677: if (debug) {
0678: System.out.println("localToVWorld = " + localToVWorld);
0679: }
0680: if ((vformat & GeometryArray.INTERLEAVED) == 0) {
0681: if (doubleData != null) {
0682: offset = 0;
0683: for (int i = 0; i < numPts; i++) {
0684:
0685: // Need to transform each pnt by localToVWorld.
0686: pnts[i] = new Point3d();
0687: pnts[i].x = doubleData[offset++];
0688: pnts[i].y = doubleData[offset++];
0689: pnts[i].z = doubleData[offset++];
0690:
0691: localToVWorld.transform(pnts[i]);
0692: }
0693: } else if (floatData != null) { // by reference and float data is defined ..
0694: offset = 0;
0695: for (int i = 0; i < numPts; i++) {
0696:
0697: // Need to transform each pnt by localToVWorld.
0698: pnts[i] = new Point3d();
0699: pnts[i].x = floatData[offset++];
0700: pnts[i].y = floatData[offset++];
0701: pnts[i].z = floatData[offset++];
0702:
0703: localToVWorld.transform(pnts[i]);
0704: }
0705: } else if (p3fData != null) {
0706: for (int i = 0; i < numPts; i++) {
0707:
0708: // Need to transform each pnt by localToVWorld.
0709: pnts[i] = new Point3d();
0710: pnts[i].set(p3fData[i]);
0711: localToVWorld.transform(pnts[i]);
0712: }
0713: } else { // p3dData
0714: for (int i = 0; i < numPts; i++) {
0715:
0716: // Need to transform each pnt by localToVWorld.
0717: pnts[i] = new Point3d();
0718: pnts[i].set(p3dData[i]);
0719: localToVWorld.transform(pnts[i]);
0720: }
0721: }
0722: }
0723: // Its an interleaved type ..
0724: else {
0725: offset = 0;
0726: if ((vformat & GeometryArray.COLOR_3) == GeometryArray.COLOR_3) {
0727: offset += 3;
0728: } else if ((vformat & GeometryArray.COLOR_4) == GeometryArray.COLOR_4) {
0729: offset += 4;
0730: }
0731: if ((vformat & GeometryArray.NORMALS) != 0)
0732: offset += 3;
0733: if ((vformat & GeometryArray.TEXTURE_COORDINATE_2) == GeometryArray.TEXTURE_COORDINATE_2) {
0734: offset += 2 * geom.getTexCoordSetCount();
0735: } else if ((vformat & GeometryArray.TEXTURE_COORDINATE_3) == GeometryArray.TEXTURE_COORDINATE_3) {
0736: offset += 3 * geom.getTexCoordSetCount();
0737: }
0738: stride = offset + 3; // for the vertices .
0739: for (int i = 0; i < numPts; i++) {
0740:
0741: // Need to transform each pnt by localToVWorld.
0742: pnts[i] = new Point3d();
0743: pnts[i].x = floatData[offset];
0744: pnts[i].y = floatData[offset + 1];
0745: pnts[i].z = floatData[offset + 2];
0746:
0747: localToVWorld.transform(pnts[i]);
0748: offset += stride;
0749: }
0750: }
0751:
0752: PickIntersection pi = new PickIntersection(this , geom);
0753:
0754: if (geom instanceof PointArray) {
0755: retFlag = intersectPA((PointArray) geom, geomIndex, pnts,
0756: firstpick, pi);
0757: } else if (geom instanceof IndexedPointArray) {
0758: pi.iGeom = (IndexedGeometryArray) geom;
0759: retFlag = intersectIPA((IndexedPointArray) geom, geomIndex,
0760: pnts, firstpick, pi);
0761: } else if (geom instanceof LineArray) {
0762: retFlag = intersectLA((LineArray) geom, geomIndex, pnts,
0763: firstpick, pi);
0764: } else if (geom instanceof LineStripArray) {
0765: retFlag = intersectLSA((LineStripArray) geom, geomIndex,
0766: pnts, firstpick, pi);
0767: } else if (geom instanceof IndexedLineArray) {
0768: pi.iGeom = (IndexedGeometryArray) geom;
0769: retFlag = intersectILA((IndexedLineArray) geom, geomIndex,
0770: pnts, firstpick, pi);
0771: } else if (geom instanceof IndexedLineStripArray) {
0772: pi.iGeom = (IndexedGeometryArray) geom;
0773: retFlag = intersectILSA((IndexedLineStripArray) geom,
0774: geomIndex, pnts, firstpick, pi);
0775: } else if (geom instanceof TriangleArray) {
0776: retFlag = intersectTA((TriangleArray) geom, geomIndex,
0777: pnts, firstpick, pi);
0778: } else if (geom instanceof TriangleStripArray) {
0779: retFlag = intersectTSA((TriangleStripArray) geom,
0780: geomIndex, pnts, firstpick, pi);
0781: } else if (geom instanceof TriangleFanArray) {
0782: retFlag = intersectTFA((TriangleFanArray) geom, geomIndex,
0783: pnts, firstpick, pi);
0784: } else if (geom instanceof IndexedTriangleArray) {
0785: pi.iGeom = (IndexedGeometryArray) geom;
0786: retFlag = intersectITA((IndexedTriangleArray) geom,
0787: geomIndex, pnts, firstpick, pi);
0788: } else if (geom instanceof IndexedTriangleStripArray) {
0789: pi.iGeom = (IndexedGeometryArray) geom;
0790: retFlag = intersectITSA((IndexedTriangleStripArray) geom,
0791: geomIndex, pnts, firstpick, pi);
0792: } else if (geom instanceof IndexedTriangleFanArray) {
0793: pi.iGeom = (IndexedGeometryArray) geom;
0794: retFlag = intersectITFA((IndexedTriangleFanArray) geom,
0795: geomIndex, pnts, firstpick, pi);
0796: } else if (geom instanceof QuadArray) {
0797: retFlag = intersectQA((QuadArray) geom, geomIndex, pnts,
0798: firstpick, pi);
0799: } else if (geom instanceof IndexedQuadArray) {
0800: pi.iGeom = (IndexedGeometryArray) geom;
0801: retFlag = intersectIQA((IndexedQuadArray) geom, geomIndex,
0802: pnts, firstpick, pi);
0803: } else {
0804: throw new RuntimeException("incorrect class type");
0805: }
0806: return retFlag;
0807:
0808: }
0809:
0810: /* ==================================================================== */
0811: /* INTERSECT METHODS BY PRIMITIVE TYPE */
0812: /* ==================================================================== */
0813:
0814: boolean intersectPoint(int[] vertidx, int[] coordidx,
0815: int geomIndex, Point3d[] pnts, PickIntersection pi) {
0816: // PickIntersection pi = new PickIntersection(this);
0817:
0818: Point3d[] point = new Point3d[1];
0819: point[0] = pnts[coordidx[0]];
0820:
0821: if (debug) {
0822: System.out.println("intersect point, point = " + point[0]);
0823: }
0824:
0825: boolean intersect = false;
0826: switch (pickShapeType) {
0827: case PICK_SHAPE_RAY:
0828: intersect = intersectPntAndRay(point[0], pickShapeStart,
0829: pickShapeDir, pi);
0830: break;
0831: case PICK_SHAPE_SEGMENT:
0832: if (intersectPntAndRay(point[0], pickShapeStart,
0833: pickShapeDir, pi)) {
0834: if (pi.getDistance() <= 1.0) { // TODO: why 1.0?
0835: intersect = true;
0836: }
0837: }
0838: break;
0839: /* case PICK_SHAPE_POINT:
0840: intersect = intersectPntAndPnt(point[0],
0841: ((PickPoint) pickShape).location );
0842: break;
0843: */
0844: case PICK_SHAPE_BOUNDING_BOX:
0845: intersect = ((BoundingBox) pickShapeBounds)
0846: .intersect(point[0]);
0847: pi.setPointCoordinatesVW(point[0]);
0848: break;
0849: case PICK_SHAPE_BOUNDING_SPHERE:
0850: intersect = ((BoundingSphere) pickShapeBounds)
0851: .intersect(point[0]);
0852: pi.setPointCoordinatesVW(point[0]);
0853: break;
0854: case PICK_SHAPE_BOUNDING_POLYTOPE:
0855: intersect = ((BoundingPolytope) pickShapeBounds)
0856: .intersect(point[0]);
0857: pi.setPointCoordinatesVW(point[0]);
0858: break;
0859: case PICK_SHAPE_CYLINDER:
0860: intersect = intersectCylinder(point[0],
0861: (PickCylinder) pickShape, pi);
0862: break;
0863: case PICK_SHAPE_CONE:
0864: intersect = intersectCone(point[0], (PickCone) pickShape,
0865: pi);
0866: break;
0867: }
0868: if (intersect) {
0869: PickIntersection newpi = new PickIntersection(this , pi.geom);
0870: newpi.iGeom = pi.iGeom;
0871: newpi.setDistance(pi.distance);
0872: newpi.setPointCoordinatesVW(pi.getPointCoordinatesVW());
0873:
0874: // Set PickIntersection parameters
0875: newpi.setGeomIndex(geomIndex);
0876: newpi.setVertexIndices(vertidx);
0877: newpi.setPrimitiveCoordinatesVW(point);
0878: intersections.add(newpi);
0879: return true;
0880: }
0881: return false;
0882: }
0883:
0884: boolean intersectLine(int[] vertidx, int[] coordidx, int geomIndex,
0885: Point3d[] pnts, PickIntersection pi) {
0886:
0887: Point3d[] linePts = new Point3d[2];
0888: linePts[0] = pnts[coordidx[0]];
0889: linePts[1] = pnts[coordidx[1]];
0890:
0891: boolean intersect = false;
0892: switch (pickShapeType) {
0893: case PICK_SHAPE_RAY:
0894: intersect = intersectLineAndRay(linePts[0], linePts[1],
0895: pickShapeStart, pickShapeDir, pi);
0896: break;
0897: case PICK_SHAPE_SEGMENT:
0898: if (intersectLineAndRay(linePts[0], linePts[1],
0899: pickShapeStart, pickShapeDir, pi)) {
0900: if (pi.getDistance() <= 1.0) {
0901: intersect = true;
0902: }
0903: }
0904: break;
0905: /* case PICK_SHAPE_POINT:
0906: dir.x = linePts[1].x - linePts[0].x;
0907: dir.y = linePts[1].y - linePts[0].y;
0908: dir.z = linePts[1].z - linePts[0].z;
0909: if (intersectPntAndRay(((PickPoint)pickShape).location,
0910: pnts[0], dir, dist)) {
0911: if(dist[0] <= 1.0) {
0912: intersect = true;
0913: }
0914: }
0915: break;
0916: */
0917: case PICK_SHAPE_BOUNDING_BOX:
0918: intersect = intersectBoundingBox(linePts,
0919: (BoundingBox) pickShapeBounds);
0920: pi.setPointCoordinatesVW(zeroPnt);
0921: break;
0922: case PICK_SHAPE_BOUNDING_SPHERE:
0923: intersect = intersectBoundingSphere(linePts,
0924: (BoundingSphere) pickShapeBounds);
0925: pi.setPointCoordinatesVW(zeroPnt);
0926: break;
0927: case PICK_SHAPE_BOUNDING_POLYTOPE:
0928: intersect = intersectBoundingPolytope(linePts,
0929: (BoundingPolytope) pickShapeBounds);
0930: pi.setPointCoordinatesVW(zeroPnt);
0931: break;
0932: case PICK_SHAPE_CYLINDER:
0933: intersect = intersectCylinder(linePts,
0934: (PickCylinder) pickShape, pi);
0935: break;
0936: case PICK_SHAPE_CONE:
0937: intersect = intersectCone(linePts, (PickCone) pickShape, pi);
0938: break;
0939: }
0940: if (intersect) {
0941: PickIntersection newpi = new PickIntersection(this , pi.geom);
0942: newpi.iGeom = pi.iGeom;
0943: newpi.setDistance(pi.distance);
0944: newpi.setPointCoordinatesVW(pi.getPointCoordinatesVW());
0945:
0946: // Set PickIntersection parameters
0947: newpi.setGeomIndex(geomIndex);
0948: newpi.setVertexIndices(vertidx);
0949: newpi.setPrimitiveCoordinatesVW(linePts);
0950: intersections.add(newpi);
0951: return true;
0952: }
0953: return false;
0954: }
0955:
0956: boolean intersectTri(int[] vertidx, int[] coordidx, int geomIndex,
0957: Point3d[] pnts, PickIntersection pi) {
0958:
0959: Point3d[] triPts = new Point3d[3];
0960:
0961: triPts[0] = pnts[coordidx[0]];
0962: triPts[1] = pnts[coordidx[1]];
0963: triPts[2] = pnts[coordidx[2]];
0964:
0965: boolean intersect = false;
0966: switch (pickShapeType) {
0967: case PICK_SHAPE_RAY:
0968: intersect = intersectRay(triPts, (PickRay) pickShape, pi);
0969: break;
0970: case PICK_SHAPE_SEGMENT:
0971: intersect = intersectSegment(triPts,
0972: (PickSegment) pickShape, pi);
0973: break;
0974: /* case PICK_SHAPE_POINT:
0975: if(inside(triPts, (PickPoint) pickShape, ccw)==false)
0976: return false;
0977: break;
0978: */
0979: case PICK_SHAPE_BOUNDING_BOX:
0980: intersect = intersectBoundingBox(triPts,
0981: (BoundingBox) pickShapeBounds);
0982: pi.setPointCoordinatesVW(zeroPnt);
0983: break;
0984: case PICK_SHAPE_BOUNDING_SPHERE:
0985: intersect = intersectBoundingSphere(triPts,
0986: (BoundingSphere) pickShapeBounds);
0987: pi.setPointCoordinatesVW(zeroPnt);
0988: break;
0989: case PICK_SHAPE_BOUNDING_POLYTOPE:
0990: intersect = intersectBoundingPolytope(triPts,
0991: (BoundingPolytope) pickShapeBounds);
0992: pi.setPointCoordinatesVW(zeroPnt);
0993: break;
0994: case PICK_SHAPE_CYLINDER:
0995: intersect = intersectCylinder(triPts,
0996: (PickCylinder) pickShape, pi);
0997: break;
0998: case PICK_SHAPE_CONE:
0999: intersect = intersectCone(triPts, (PickCone) pickShape, pi);
1000: break;
1001: }
1002: if (intersect) {
1003: PickIntersection newpi = new PickIntersection(this , pi.geom);
1004: newpi.iGeom = pi.iGeom;
1005: newpi.setDistance(pi.distance);
1006: newpi.setPointCoordinatesVW(pi.getPointCoordinatesVW());
1007:
1008: // Set PickIntersection parameters
1009: newpi.setGeomIndex(geomIndex);
1010: newpi.setVertexIndices(vertidx);
1011:
1012: newpi.setPrimitiveCoordinatesVW(triPts);
1013: intersections.add(newpi);
1014: return true;
1015: }
1016: return false;
1017: }
1018:
1019: boolean intersectQuad(int[] vertidx, int[] coordidx, int geomIndex,
1020: Point3d[] pnts, PickIntersection pi) {
1021:
1022: Point3d[] quadPts = new Point3d[4];
1023:
1024: quadPts[0] = pnts[coordidx[0]];
1025: quadPts[1] = pnts[coordidx[1]];
1026: quadPts[2] = pnts[coordidx[2]];
1027: quadPts[3] = pnts[coordidx[3]];
1028:
1029: // PickIntersection pi = new PickIntersection(this);
1030:
1031: boolean intersect = false;
1032: switch (pickShapeType) {
1033: case PICK_SHAPE_RAY:
1034: intersect = intersectRay(quadPts, (PickRay) pickShape, pi);
1035: break;
1036: case PICK_SHAPE_SEGMENT:
1037: intersect = intersectSegment(quadPts,
1038: (PickSegment) pickShape, pi);
1039: break;
1040: /* case PICK_SHAPE_POINT:
1041: if(inside(quadPts, (PickPoint) pickShape, ccw)==false)
1042: return false;
1043: break;
1044: */
1045: case PICK_SHAPE_BOUNDING_BOX:
1046: intersect = intersectBoundingBox(quadPts,
1047: (BoundingBox) pickShapeBounds);
1048: pi.setPointCoordinatesVW(zeroPnt);
1049: break;
1050: case PICK_SHAPE_BOUNDING_SPHERE:
1051: intersect = intersectBoundingSphere(quadPts,
1052: (BoundingSphere) pickShapeBounds);
1053: pi.setPointCoordinatesVW(zeroPnt);
1054: break;
1055: case PICK_SHAPE_BOUNDING_POLYTOPE:
1056: intersect = intersectBoundingPolytope(quadPts,
1057: (BoundingPolytope) pickShapeBounds);
1058: pi.setPointCoordinatesVW(zeroPnt);
1059: break;
1060: case PICK_SHAPE_CYLINDER:
1061: intersect = intersectCylinder(quadPts,
1062: (PickCylinder) pickShape, pi);
1063: break;
1064: case PICK_SHAPE_CONE:
1065: intersect = intersectCone(quadPts, (PickCone) pickShape, pi);
1066: break;
1067: }
1068: if (intersect) {
1069: PickIntersection newpi = new PickIntersection(this , pi.geom);
1070: newpi.iGeom = pi.iGeom;
1071: newpi.setDistance(pi.distance);
1072: newpi.setPointCoordinatesVW(pi.getPointCoordinatesVW());
1073:
1074: // Set PickIntersection parameters
1075: newpi.setGeomIndex(geomIndex);
1076: newpi.setVertexIndices(vertidx);
1077: newpi.setPrimitiveCoordinatesVW(quadPts);
1078: intersections.add(newpi);
1079: return true;
1080: }
1081: return false;
1082: }
1083:
1084: /* ==================================================================== */
1085: /* INTERSECT METHODS BY GEOMETRY TYPE */
1086: /* ==================================================================== */
1087:
1088: /**
1089: Intersect method for PointArray
1090: */
1091: boolean intersectPA(PointArray geom, int geomIndex, Point3d[] pnts,
1092: boolean firstpick, PickIntersection pi) {
1093:
1094: if (debug)
1095: System.out.println("intersect: PointArray");
1096:
1097: int[] pntVertIdx = new int[1];
1098: int numint = 0;
1099:
1100: for (int i = 0; i < pnts.length; i++) {
1101: pntVertIdx[0] = i;
1102: if (intersectPoint(pntVertIdx, pntVertIdx, geomIndex, pnts,
1103: pi)) {
1104: numint++;
1105: if (firstpick)
1106: return true;
1107: }
1108: }
1109: if (numint > 0)
1110: return true;
1111: return false;
1112: }
1113:
1114: /**
1115: Intersect method for IndexedPointArray
1116: */
1117: boolean intersectIPA(IndexedPointArray geom, int geomIndex,
1118: Point3d[] pnts, boolean firstpick, PickIntersection pi) {
1119:
1120: if (debug)
1121: System.out.println("intersect: IndexedPointArray");
1122:
1123: int[] pntVertIdx = new int[1];
1124: int[] pntCoordIdx = new int[1];
1125:
1126: int numint = 0;
1127: int indexCount = geom.getIndexCount();
1128:
1129: for (int i = 0; i < indexCount; i++) {
1130: pntVertIdx[0] = i;
1131: pntCoordIdx[0] = geom.getCoordinateIndex(i);
1132: if (intersectPoint(pntVertIdx, pntCoordIdx, geomIndex,
1133: pnts, pi)) {
1134: numint++;
1135: if (firstpick)
1136: return true;
1137: }
1138: }
1139: if (numint > 0)
1140: return true;
1141: return false;
1142: }
1143:
1144: /**
1145: Intersect method for LineArray
1146: */
1147: /**
1148: Intersect method for LineArray
1149: */
1150: boolean intersectLA(LineArray geom, int geomIndex, Point3d[] pnts,
1151: boolean firstpick, PickIntersection pi) {
1152:
1153: if (debug)
1154: System.out.println("intersect: LineArray");
1155:
1156: int[] lineVertIdx = new int[2];
1157:
1158: int numint = 0;
1159:
1160: for (int i = 0; i < pnts.length;) {
1161: /* set up the parameters for the current line */
1162: lineVertIdx[0] = i++;
1163: lineVertIdx[1] = i++;
1164: if (intersectLine(lineVertIdx, lineVertIdx, geomIndex,
1165: pnts, pi)) {
1166: numint++;
1167: if (firstpick)
1168: return true;
1169: }
1170: }
1171: if (numint > 0)
1172: return true;
1173: return false;
1174: }
1175:
1176: /**
1177: Intersect method for LineStripArray
1178: */
1179: boolean intersectLSA(LineStripArray geom, int geomIndex,
1180: Point3d[] pnts, boolean firstpick, PickIntersection pi) {
1181: int numint = 0;
1182:
1183: int[] stripVertexCounts = new int[geom.getNumStrips()];
1184: geom.getStripVertexCounts(stripVertexCounts);
1185: int stripStart = 0;
1186:
1187: if (debug)
1188: System.out.println("intersect: LineStripArray");
1189:
1190: int[] lineVertIdx = new int[2];
1191:
1192: for (int i = 0; i < stripVertexCounts.length; i++) {
1193: lineVertIdx[0] = stripStart;
1194: int end = stripStart + stripVertexCounts[i];
1195:
1196: for (int j = stripStart + 1; j < end; j++) {
1197: lineVertIdx[1] = j;
1198: if (intersectLine(lineVertIdx, lineVertIdx, geomIndex,
1199: pnts, pi)) {
1200: numint++;
1201: if (firstpick)
1202: return true;
1203: }
1204: lineVertIdx[0] = lineVertIdx[1];
1205: }
1206: stripStart += stripVertexCounts[i];
1207: }
1208: if (numint > 0)
1209: return true;
1210: return false;
1211: }
1212:
1213: /**
1214: Intersect method for IndexedLineArray
1215: */
1216: boolean intersectILA(IndexedLineArray geom, int geomIndex,
1217: Point3d[] pnts, boolean firstpick, PickIntersection pi) {
1218:
1219: int numint = 0;
1220: int indexCount = geom.getIndexCount();
1221: if (debug)
1222: System.out.println("intersect: IndexedLineArray");
1223:
1224: int[] lineVertIdx = new int[2];
1225: int[] lineCoordIdx = new int[2];
1226:
1227: for (int i = 0; i < indexCount;) {
1228: lineVertIdx[0] = i;
1229: lineCoordIdx[0] = geom.getCoordinateIndex(i++);
1230: lineVertIdx[1] = i;
1231: lineCoordIdx[1] = geom.getCoordinateIndex(i++);
1232: if (intersectLine(lineVertIdx, lineCoordIdx, geomIndex,
1233: pnts, pi)) {
1234: numint++;
1235: if (firstpick)
1236: return true;
1237: }
1238: }
1239: if (numint > 0)
1240: return true;
1241: return false;
1242: }
1243:
1244: /**
1245: Intersect method for IndexedLineStripArray
1246: */
1247: boolean intersectILSA(IndexedLineStripArray geom, int geomIndex,
1248: Point3d[] pnts, boolean firstpick, PickIntersection pi) {
1249: if (debug)
1250: System.out.println("intersect: IndexedLineStripArray");
1251:
1252: int[] lineVertIdx = new int[2];
1253: int[] lineCoordIdx = new int[2];
1254:
1255: int numint = 0;
1256: int[] stripVertexCounts = new int[geom.getNumStrips()];
1257: geom.getStripIndexCounts(stripVertexCounts);
1258: int stripStart = 0;
1259:
1260: for (int i = 0; i < stripVertexCounts.length; i++) {
1261:
1262: lineVertIdx[0] = stripStart;
1263: lineCoordIdx[0] = geom.getCoordinateIndex(stripStart);
1264: int end = stripStart + stripVertexCounts[i];
1265: for (int j = stripStart + 1; j < end; j++) {
1266: lineVertIdx[1] = j;
1267: lineCoordIdx[1] = geom.getCoordinateIndex(j);
1268: if (intersectLine(lineVertIdx, lineCoordIdx, geomIndex,
1269: pnts, pi)) {
1270: numint++;
1271: if (firstpick)
1272: return true;
1273: }
1274: lineVertIdx[0] = lineVertIdx[1];
1275: lineCoordIdx[0] = lineCoordIdx[1];
1276: }
1277: stripStart += stripVertexCounts[i];
1278: }
1279: if (numint > 0)
1280: return true;
1281: return false;
1282: }
1283:
1284: /**
1285: Intersect method for TriangleArray
1286: */
1287: boolean intersectTA(TriangleArray geom, int geomIndex,
1288: Point3d[] pnts, boolean firstpick, PickIntersection pi) {
1289:
1290: if (debug)
1291: System.out.println("intersect: TriangleArray");
1292:
1293: int[] triVertIdx = new int[3];
1294:
1295: int numint = 0;
1296: for (int i = 0; i < pnts.length;) {
1297: triVertIdx[0] = i++;
1298: triVertIdx[1] = i++;
1299: triVertIdx[2] = i++;
1300: if (intersectTri(triVertIdx, triVertIdx, geomIndex, pnts,
1301: pi)) {
1302: numint++;
1303: if (firstpick)
1304: return true;
1305: }
1306: }
1307:
1308: if (numint > 0)
1309: return true;
1310: return false;
1311: }
1312:
1313: /**
1314: Intersect method for IndexedTriangleArray
1315: */
1316: boolean intersectITA(IndexedTriangleArray geom, int geomIndex,
1317: Point3d[] pnts, boolean firstpick, PickIntersection pi) {
1318:
1319: if (debug)
1320: System.out.println("intersect: IndexedTriangleArray");
1321:
1322: int[] triVertIdx = new int[3];
1323: int[] triCoordIdx = new int[3];
1324:
1325: int numint = 0;
1326: int indexCount = geom.getIndexCount();
1327: for (int i = 0; i < indexCount;) {
1328: triVertIdx[0] = i;
1329: triCoordIdx[0] = geom.getCoordinateIndex(i++);
1330: triVertIdx[1] = i;
1331: triCoordIdx[1] = geom.getCoordinateIndex(i++);
1332: triVertIdx[2] = i;
1333: triCoordIdx[2] = geom.getCoordinateIndex(i++);
1334: if (intersectTri(triVertIdx, triCoordIdx, geomIndex, pnts,
1335: pi)) {
1336: numint++;
1337: if (firstpick)
1338: return true;
1339: }
1340: }
1341:
1342: if (numint > 0)
1343: return true;
1344: return false;
1345: }
1346:
1347: /**
1348: Intersect method for TriangleStripArray
1349: */
1350: boolean intersectTSA(TriangleStripArray geom, int geomIndex,
1351: Point3d[] pnts, boolean firstpick, PickIntersection pi) {
1352: if (debug)
1353: System.out.println("intersect: TriangleStripArray");
1354:
1355: boolean ccw;
1356: int numint = 0;
1357: int[] stripVertexCounts = new int[geom.getNumStrips()];
1358: geom.getStripVertexCounts(stripVertexCounts);
1359: int stripStart = 0;
1360: int start;
1361: int[] triVertIdx = new int[3];
1362:
1363: for (int i = 0; i < stripVertexCounts.length; i++) {
1364:
1365: start = stripStart;
1366: // start a new strip
1367: ccw = true;
1368: triVertIdx[0] = start++;
1369: triVertIdx[1] = start++;
1370:
1371: int end = start + stripVertexCounts[i] - 2;
1372: for (int j = start; j < end; j++) {
1373: /*
1374: if (ccw) {
1375: triVertIdx[2] = j;
1376: } else {
1377: triVertIdx[1] = j;
1378: }
1379: */
1380: triVertIdx[2] = j;
1381: if (intersectTri(triVertIdx, triVertIdx, geomIndex,
1382: pnts, pi)) {
1383: numint++;
1384: if (firstpick)
1385: return true;
1386: }
1387:
1388: // Advance to the next triangle, keeping the winding of the test
1389: // triangle correct.
1390: /*
1391: if (ccw) {
1392: triVertIdx[0] = triVertIdx[1];
1393: // triVertIdx[2] remains, triVertIdx[1] will be replaced
1394: ccw = false;
1395: } else {
1396: triVertIdx[0] = triVertIdx[2];
1397: // triVertIdx[1] remains, triVertIdx[2] will be replaced
1398: ccw = true;
1399: }
1400: */
1401: triVertIdx[0] = triVertIdx[1];
1402: triVertIdx[1] = triVertIdx[2];
1403:
1404: }
1405: stripStart += stripVertexCounts[i];
1406: }
1407:
1408: if (numint > 0)
1409: return true;
1410: return false;
1411: }
1412:
1413: /**
1414: Intersect method for IndexedTriangleStripArray
1415: */
1416: boolean intersectITSA(IndexedTriangleStripArray geom,
1417: int geomIndex, Point3d[] pnts, boolean firstpick,
1418: PickIntersection pi) {
1419:
1420: if (debug)
1421: System.out.println("intersect: IndexedTriangleStripArray");
1422: int numint = 0;
1423: boolean ccw;
1424:
1425: int[] stripVertexCounts = new int[geom.getNumStrips()];
1426: geom.getStripIndexCounts(stripVertexCounts);
1427: int stripStart = 0;
1428: int start;
1429: int[] triVertIdx = new int[3];
1430: int[] triCoordIdx = new int[3];
1431:
1432: for (int i = 0; i < stripVertexCounts.length; i++) {
1433:
1434: start = stripStart;
1435: // start a new strip
1436: ccw = true;
1437: triCoordIdx[0] = geom.getCoordinateIndex(start);
1438: triVertIdx[0] = start++;
1439: triCoordIdx[1] = geom.getCoordinateIndex(start);
1440: triVertIdx[1] = start++;
1441:
1442: int end = start + stripVertexCounts[i] - 2;
1443: for (int j = start; j < end; j++) {
1444: if (ccw) {
1445: triVertIdx[2] = j;
1446: triCoordIdx[2] = geom.getCoordinateIndex(j);
1447: } else {
1448: triVertIdx[1] = j;
1449: triCoordIdx[1] = geom.getCoordinateIndex(j);
1450: }
1451:
1452: if (intersectTri(triVertIdx, triCoordIdx, geomIndex,
1453: pnts, pi)) {
1454: numint++;
1455: if (firstpick)
1456: return true;
1457: }
1458:
1459: // Advance to the next triangle, keeping the winding of the test
1460: // triangle correct.
1461: if (ccw) {
1462: triVertIdx[0] = triVertIdx[1];
1463: // triVertIdx[2] remains, triVertIdx[1] will be replaced
1464: triCoordIdx[0] = triCoordIdx[1];
1465: ccw = false;
1466: } else {
1467: triVertIdx[0] = triVertIdx[2];
1468: // triVertIdx[1] remains, triVertIdx[2] will be replaced
1469: triCoordIdx[0] = triCoordIdx[2];
1470: ccw = true;
1471: }
1472: }
1473: stripStart += stripVertexCounts[i];
1474: }
1475:
1476: if (numint > 0)
1477: return true;
1478: return false;
1479:
1480: }
1481:
1482: /**
1483: Intersect method for TriangleFanArray
1484: */
1485: boolean intersectTFA(TriangleFanArray geom, int geomIndex,
1486: Point3d[] pnts, boolean firstpick, PickIntersection pi) {
1487:
1488: if (debug)
1489: System.out.println("intersect: TriangleFanArray");
1490:
1491: int numint = 0;
1492:
1493: int[] stripVertexCounts = new int[geom.getNumStrips()];
1494: geom.getStripVertexCounts(stripVertexCounts);
1495: int fanStart = 0;
1496: int start;
1497: int[] triVertIdx = new int[3];
1498:
1499: // System.out.println("stripVertexCounts.length " + stripVertexCounts.length);
1500: for (int i = 0; i < stripVertexCounts.length; i++) {
1501:
1502: start = fanStart;
1503: triVertIdx[0] = start++;
1504: triVertIdx[1] = start++;
1505:
1506: int end = start + stripVertexCounts[i] - 2;
1507: for (int j = start; j < end; j++) {
1508: triVertIdx[2] = j;
1509: if (intersectTri(triVertIdx, triVertIdx, geomIndex,
1510: pnts, pi)) {
1511: numint++;
1512: if (firstpick)
1513: return true;
1514: }
1515: triVertIdx[1] = triVertIdx[2];
1516: }
1517: fanStart += stripVertexCounts[i];
1518: }
1519: if (numint > 0)
1520: return true;
1521: return false;
1522: }
1523:
1524: /**
1525: Intersect method for IndexedTriangleFanArray
1526: */
1527: boolean intersectITFA(IndexedTriangleFanArray geom, int geomIndex,
1528: Point3d[] pnts, boolean firstpick, PickIntersection pi) {
1529:
1530: if (debug)
1531: System.out.println("intersect: IndexedTriangleFanArray");
1532:
1533: int numint = 0;
1534: int[] stripVertexCounts = new int[geom.getNumStrips()];
1535: geom.getStripIndexCounts(stripVertexCounts);
1536: int fanStart = 0;
1537: int start;
1538: int[] triVertIdx = new int[3];
1539: int[] triCoordIdx = new int[3];
1540:
1541: for (int i = 0; i < stripVertexCounts.length; i++) {
1542:
1543: start = fanStart;
1544: triCoordIdx[0] = geom.getCoordinateIndex(start);
1545: triVertIdx[0] = start++;
1546: triCoordIdx[1] = geom.getCoordinateIndex(start);
1547: triVertIdx[1] = start++;
1548:
1549: int end = start + stripVertexCounts[i] - 2;
1550: for (int j = start; j < end; j++) {
1551: triVertIdx[2] = j;
1552: triCoordIdx[2] = geom.getCoordinateIndex(j);
1553: if (intersectTri(triVertIdx, triCoordIdx, geomIndex,
1554: pnts, pi)) {
1555: numint++;
1556: if (firstpick)
1557: return true;
1558: }
1559: triVertIdx[1] = triVertIdx[2];
1560: triCoordIdx[1] = triCoordIdx[2];
1561: }
1562: fanStart += stripVertexCounts[i];
1563: }
1564: if (numint > 0)
1565: return true;
1566: return false;
1567: }
1568:
1569: /**
1570: Intersect method for QuadArray
1571: */
1572: boolean intersectQA(QuadArray geom, int geomIndex, Point3d[] pnts,
1573: boolean firstpick, PickIntersection pi) {
1574:
1575: if (debug)
1576: System.out.println("intersect: QuadArray");
1577:
1578: int[] quadVertIdx = new int[4];
1579:
1580: int numint = 0;
1581: for (int i = 0; i < pnts.length;) {
1582: quadVertIdx[0] = i++;
1583: quadVertIdx[1] = i++;
1584: quadVertIdx[2] = i++;
1585: quadVertIdx[3] = i++;
1586: if (intersectQuad(quadVertIdx, quadVertIdx, geomIndex,
1587: pnts, pi)) {
1588: numint++;
1589: if (firstpick)
1590: return true;
1591: }
1592: }
1593:
1594: if (numint > 0)
1595: return true;
1596: return false;
1597: }
1598:
1599: /**
1600: Intersect method for IndexedQuadArray
1601: */
1602: final boolean intersectIQA(IndexedQuadArray geom, int geomIndex,
1603: Point3d[] pnts, boolean firstpick, PickIntersection pi) {
1604:
1605: if (debug)
1606: System.out.println("intersect: IndexedQuadArray");
1607:
1608: int[] quadVertIdx = new int[4];
1609: int[] quadCoordIdx = new int[4];
1610:
1611: int numint = 0;
1612: int indexCount = geom.getIndexCount();
1613: // System.out.println ("intersect: IndexedQuadArray : indexCount " + indexCount);
1614: for (int i = 0; i < indexCount;) {
1615: quadVertIdx[0] = i;
1616: quadCoordIdx[0] = geom.getCoordinateIndex(i++);
1617: quadVertIdx[1] = i;
1618: quadCoordIdx[1] = geom.getCoordinateIndex(i++);
1619: quadVertIdx[2] = i;
1620: quadCoordIdx[2] = geom.getCoordinateIndex(i++);
1621: quadVertIdx[3] = i;
1622: quadCoordIdx[3] = geom.getCoordinateIndex(i++);
1623:
1624: if (intersectQuad(quadVertIdx, quadCoordIdx, geomIndex,
1625: pnts, pi)) {
1626: numint++;
1627: if (firstpick)
1628: return true;
1629: }
1630: }
1631:
1632: if (numint > 0)
1633: return true;
1634: return false;
1635:
1636: }
1637:
1638: /* ==================================================================== */
1639: /* GENERAL INTERSECT METHODS */
1640: /* ==================================================================== */
1641: static boolean intersectBoundingBox(Point3d coordinates[],
1642: BoundingBox box) {
1643: int i, j;
1644: int out[] = new int[6];
1645:
1646: Point3d lower = new Point3d();
1647: Point3d upper = new Point3d();
1648: box.getLower(lower);
1649: box.getUpper(upper);
1650:
1651: //Do trivial vertex test.
1652: for (i = 0; i < 6; i++)
1653: out[i] = 0;
1654: for (i = 0; i < coordinates.length; i++) {
1655: if ((coordinates[i].x >= lower.x)
1656: && (coordinates[i].x <= upper.x)
1657: && (coordinates[i].y >= lower.y)
1658: && (coordinates[i].y <= upper.y)
1659: && (coordinates[i].z >= lower.z)
1660: && (coordinates[i].z <= upper.z)) {
1661: // We're done! It's inside the boundingbox.
1662: return true;
1663: } else {
1664: if (coordinates[i].x < lower.x)
1665: out[0]++; // left
1666: if (coordinates[i].y < lower.y)
1667: out[1]++; // bottom
1668: if (coordinates[i].z < lower.z)
1669: out[2]++; // back
1670: if (coordinates[i].x > upper.x)
1671: out[3]++; // right
1672: if (coordinates[i].y > upper.y)
1673: out[4]++; // top
1674: if (coordinates[i].z > upper.z)
1675: out[5]++; // front
1676: }
1677: }
1678:
1679: if ((out[0] == coordinates.length)
1680: || (out[1] == coordinates.length)
1681: || (out[2] == coordinates.length)
1682: || (out[3] == coordinates.length)
1683: || (out[4] == coordinates.length)
1684: || (out[5] == coordinates.length)) {
1685: // we're done. primitive is outside of boundingbox.
1686: return false;
1687: }
1688: // Setup bounding planes.
1689: Point3d pCoor[] = new Point3d[4];
1690: for (i = 0; i < 4; i++)
1691: pCoor[i] = new Point3d();
1692:
1693: // left plane.
1694: pCoor[0].set(lower.x, lower.y, lower.z);
1695: pCoor[1].set(lower.x, lower.y, upper.z);
1696: pCoor[2].set(lower.x, upper.y, upper.z);
1697: pCoor[3].set(lower.x, upper.y, lower.z);
1698: if (intersectPolygon(pCoor, coordinates, false) == true)
1699: return true;
1700:
1701: // right plane.
1702: pCoor[0].set(upper.x, lower.y, lower.z);
1703: pCoor[1].set(upper.x, upper.y, lower.z);
1704: pCoor[2].set(upper.x, upper.y, upper.z);
1705: pCoor[3].set(upper.x, lower.y, upper.z);
1706: if (intersectPolygon(pCoor, coordinates, false) == true)
1707: return true;
1708:
1709: // bottom plane.
1710: pCoor[0].set(upper.x, lower.y, upper.z);
1711: pCoor[1].set(lower.x, lower.y, upper.z);
1712: pCoor[2].set(lower.x, lower.y, lower.z);
1713: pCoor[3].set(upper.x, lower.y, lower.z);
1714: if (intersectPolygon(pCoor, coordinates, false) == true)
1715: return true;
1716:
1717: // top plane.
1718: pCoor[0].set(upper.x, upper.y, upper.z);
1719: pCoor[1].set(upper.x, upper.y, lower.z);
1720: pCoor[2].set(lower.x, upper.y, lower.z);
1721: pCoor[3].set(lower.x, upper.y, upper.z);
1722: if (intersectPolygon(pCoor, coordinates, false) == true)
1723: return true;
1724:
1725: // front plane.
1726: pCoor[0].set(upper.x, upper.y, upper.z);
1727: pCoor[1].set(lower.x, upper.y, upper.z);
1728: pCoor[2].set(lower.x, lower.y, upper.z);
1729: pCoor[3].set(upper.x, lower.y, upper.z);
1730: if (intersectPolygon(pCoor, coordinates, false) == true)
1731: return true;
1732:
1733: // back plane.
1734: pCoor[0].set(upper.x, upper.y, lower.z);
1735: pCoor[1].set(upper.x, lower.y, lower.z);
1736: pCoor[2].set(lower.x, lower.y, lower.z);
1737: pCoor[3].set(lower.x, upper.y, lower.z);
1738: if (intersectPolygon(pCoor, coordinates, false) == true)
1739: return true;
1740:
1741: return false;
1742: }
1743:
1744: static boolean intersectBoundingSphere(Point3d coordinates[],
1745: BoundingSphere sphere) {
1746:
1747: int i, j;
1748: Vector3d tempV3D = new Vector3d();
1749: boolean esFlag;
1750: Point3d center = new Point3d();
1751: sphere.getCenter(center);
1752: double radius = sphere.getRadius();
1753: //Do trivial vertex test.
1754:
1755: for (i = 0; i < coordinates.length; i++) {
1756: tempV3D.x = coordinates[i].x - center.x;
1757: tempV3D.y = coordinates[i].y - center.y;
1758: tempV3D.z = coordinates[i].z - center.z;
1759:
1760: if (tempV3D.length() <= radius) {
1761: // We're done! It's inside the boundingSphere.
1762: return true;
1763: }
1764: }
1765:
1766: for (i = 0; i < coordinates.length; i++) {
1767: if (i < (coordinates.length - 1)) {
1768: esFlag = edgeIntersectSphere(sphere, coordinates[i],
1769: coordinates[i + 1]);
1770: } else {
1771: esFlag = edgeIntersectSphere(sphere, coordinates[i],
1772: coordinates[0]);
1773: }
1774: if (esFlag == true) {
1775: return true;
1776: }
1777: }
1778:
1779: if (coordinates.length < 3)
1780: return false; // We're done with line.
1781:
1782: // Find rho.
1783: // Compute plane normal.
1784: Vector3d vec0 = new Vector3d(); //Edge vector from point 0 to point 1;
1785: Vector3d vec1 = new Vector3d(); //Edge vector from point 0 to point 2 or 3;
1786: Vector3d pNrm = new Vector3d();
1787: Vector3d pa = new Vector3d();
1788: Point3d q = new Point3d();
1789: double nLenSq, pqLen, pNrmDotPa, tq;
1790:
1791: // compute plane normal for coordinates.
1792: for (i = 0; i < coordinates.length - 1;) {
1793: vec0.x = coordinates[i + 1].x - coordinates[i].x;
1794: vec0.y = coordinates[i + 1].y - coordinates[i].y;
1795: vec0.z = coordinates[i + 1].z - coordinates[i++].z;
1796: if (vec0.length() > 0.0)
1797: break;
1798: }
1799:
1800: for (j = i; j < coordinates.length - 1; j++) {
1801: vec1.x = coordinates[j + 1].x - coordinates[j].x;
1802: vec1.y = coordinates[j + 1].y - coordinates[j].y;
1803: vec1.z = coordinates[j + 1].z - coordinates[j].z;
1804: if (vec1.length() > 0.0)
1805: break;
1806: }
1807:
1808: if (j == (coordinates.length - 1)) {
1809: // System.out.println("(1) Degenerated polygon.");
1810: return false; // Degenerated polygon.
1811: }
1812:
1813: /*
1814: for (i=0; i<coordinates.length; i++)
1815: System.out.println("coordinates P" + i + " " + coordinates[i]);
1816: for (i=0; i<coord2.length; i++)
1817: System.out.println("coord2 P" + i + " " + coord2[i]);
1818: */
1819:
1820: pNrm.cross(vec0, vec1);
1821:
1822: nLenSq = pNrm.lengthSquared();
1823: if (nLenSq == 0.0) {
1824: // System.out.println("(2) Degenerated polygon.");
1825: return false; // Degenerated polygon.
1826: }
1827:
1828: pa.x = coordinates[0].x - center.x;
1829: pa.y = coordinates[0].y - center.y;
1830: pa.z = coordinates[0].z - center.z;
1831:
1832: pNrmDotPa = pNrm.dot(pa);
1833:
1834: pqLen = Math.sqrt(pNrmDotPa * pNrmDotPa / nLenSq);
1835:
1836: if (pqLen > radius)
1837: return false;
1838:
1839: tq = pNrmDotPa / nLenSq;
1840:
1841: q.x = center.x + tq * pNrm.x;
1842: q.y = center.y + tq * pNrm.y;
1843: q.z = center.z + tq * pNrm.z;
1844:
1845: // PolyPnt2D Test.
1846: return pointIntersectPolygon2D(pNrm, coordinates, q);
1847: }
1848:
1849: static boolean intersectBoundingPolytope(Point3d coordinates[],
1850: BoundingPolytope polytope) {
1851:
1852: boolean debug = false;
1853:
1854: // this is a multiplier to the halfplane distance coefficients
1855: double distanceSign = -1.0;
1856: // Variable needed for intersection.
1857: Point4d tP4d = new Point4d();
1858:
1859: Vector4d[] planes = new Vector4d[polytope.getNumPlanes()];
1860:
1861: for (int i = 0; i < planes.length; i++)
1862: planes[i] = new Vector4d();
1863:
1864: polytope.getPlanes(planes);
1865:
1866: if (coordinates.length == 2) {
1867: // we'll handle line separately.
1868: throw new java.lang.RuntimeException(
1869: "TODO: must make polytope.intersect(coordinates[0], coordinates[1], tP4d) public!");
1870: // TODO: must make this public !!!
1871: // return polytope.intersect(coordinates[0], coordinates[1], tP4d );
1872: }
1873:
1874: // It is a triangle or a quad.
1875:
1876: // first test to see if any of the coordinates are all inside of the
1877: // intersection polytope's half planes
1878: // essentially do a matrix multiply of the constraintMatrix K*3 with
1879: // the input coordinates 3*1 = K*1 vector
1880:
1881: if (debug) {
1882: System.out.println("The value of the input vertices are: ");
1883: for (int i = 0; i < coordinates.length; i++) {
1884: System.out.println("The " + i + " th vertex is: "
1885: + coordinates[i]);
1886: }
1887:
1888: System.out
1889: .println("The value of the input bounding Polytope's planes =");
1890: for (int i = 0; i < planes.length; i++) {
1891: System.out.println("The " + i + " th plane is: "
1892: + planes[i]);
1893: }
1894:
1895: }
1896:
1897: // the direction for the intersection cost function
1898: double centers[] = new double[4];
1899: centers[0] = 0.8;
1900: centers[1] = 0.9;
1901: centers[2] = 1.1;
1902: centers[3] = 1.2;
1903:
1904: boolean intersection = true;
1905: boolean PreTest = false;
1906:
1907: if (PreTest) {
1908: // for each coordinate, test it with each half plane
1909: for (int i = 0; i < coordinates.length; i++) {
1910: for (int j = 0; j < planes.length; j++) {
1911: if ((planes[j].x * coordinates[i].x + planes[j].y
1912: * coordinates[i].y + planes[j].z
1913: * coordinates[i].z) <= (distanceSign)
1914: * planes[j].w) {
1915: // the point satisfies this particular hyperplane
1916: intersection = true;
1917: } else {
1918: // the point fails this hyper plane try with a new hyper plane
1919: intersection = false;
1920: break;
1921: }
1922: }
1923: if (intersection) {
1924: // a point was found to be completely inside the bounding hull
1925: return true;
1926: }
1927: }
1928: } // end of pretest
1929:
1930: // at this point all points are outside of the bounding hull
1931: // build the problem tableau for the linear program
1932:
1933: int numberCols = planes.length + 2 + coordinates.length + 1;
1934: int numberRows = 1 + coordinates.length;
1935:
1936: double problemTableau[][] = new double[numberRows][numberCols];
1937:
1938: // compute -Mtrans = -A*P
1939: for (int i = 0; i < planes.length; i++) {
1940: for (int j = 0; j < coordinates.length; j++) {
1941: problemTableau[j][i] = (-1.0)
1942: * (planes[i].x * coordinates[j].x + planes[i].y
1943: * coordinates[j].y + planes[i].z
1944: * coordinates[j].z);
1945: }
1946: }
1947:
1948: // add the other rows
1949: for (int i = 0; i < coordinates.length; i++) {
1950: problemTableau[i][planes.length] = -1.0;
1951: problemTableau[i][planes.length + 1] = 1.0;
1952:
1953: for (int j = 0; j < coordinates.length; j++) {
1954: if (i == j) {
1955: problemTableau[i][j + planes.length + 2] = 1.0;
1956: } else {
1957: problemTableau[i][j + planes.length + 2] = 0.0;
1958: }
1959:
1960: // place the last column elements the Ci's
1961: problemTableau[i][numberCols - 1] = centers[i];
1962: }
1963: }
1964:
1965: // place the final rows value
1966: for (int j = 0; j < planes.length; j++) {
1967: problemTableau[numberRows - 1][j] = (distanceSign)
1968: * planes[j].w;
1969: }
1970: problemTableau[numberRows - 1][planes.length] = 1.0;
1971: problemTableau[numberRows - 1][planes.length + 1] = -1.0;
1972: for (int j = 0; j < coordinates.length; j++) {
1973: problemTableau[numberRows - 1][planes.length + 2 + j] = 0.0;
1974: }
1975:
1976: if (debug) {
1977: System.out.println("The value of the problem tableau is: ");
1978: for (int i = 0; i < problemTableau.length; i++) {
1979: for (int j = 0; j < problemTableau[0].length; j++) {
1980: System.out.print(problemTableau[i][j] + " ");
1981: }
1982: System.out.println();
1983: }
1984: }
1985:
1986: double distance = generalStandardSimplexSolver(problemTableau,
1987: Float.NEGATIVE_INFINITY);
1988: if (debug) {
1989: System.out
1990: .println("The value returned by the general standard simplex = "
1991: + distance);
1992: }
1993: if (distance == Float.POSITIVE_INFINITY) {
1994: return false;
1995: }
1996: return true;
1997: }
1998:
1999: // optimized version using arrays of doubles, but using the standard simplex
2000: // method to solve the LP tableau. This version has not been optimized to
2001: // work with a particular size input tableau and is much slower than some
2002: // of the other variants...supposedly
2003: static double generalStandardSimplexSolver(
2004: double problemTableau[][], double stopingValue) {
2005: boolean debug = false;
2006: int numRow = problemTableau.length;
2007: int numCol = problemTableau[0].length;
2008: boolean optimal = false;
2009: int i, pivotRowIndex, pivotColIndex;
2010: double maxElement, element, endElement, ratio, prevRatio;
2011: int count = 0;
2012: double multiplier;
2013:
2014: if (debug) {
2015: System.out.println("The number of rows is : " + numRow);
2016: System.out.println("The number of columns is : " + numCol);
2017: }
2018:
2019: // until the optimal solution is found continue to do
2020: // iterations of the simplex method
2021: while (!optimal) {
2022:
2023: if (debug) {
2024: System.out.println("input problem tableau is:");
2025: for (int k = 0; k < numRow; k++) {
2026: for (int j = 0; j < numCol; j++) {
2027: System.out.println("kth, jth value is:" + k
2028: + " " + j + " : "
2029: + problemTableau[k][j]);
2030: }
2031: }
2032: }
2033:
2034: // test to see if the current solution is optimal
2035: // check all bottom row elements except the right most one and
2036: // if all positive or zero its optimal
2037: for (i = 0, maxElement = 0, pivotColIndex = -1; i < numCol - 1; i++) {
2038: // a bottom row element
2039: element = problemTableau[numRow - 1][i];
2040: if (element < maxElement) {
2041: maxElement = element;
2042: pivotColIndex = i;
2043: }
2044: }
2045:
2046: // if there is no negative non-zero element then we
2047: // have found an optimal solution (the last row of the tableau)
2048: if (pivotColIndex == -1) {
2049: // found an optimal solution
2050: //System.out.println("Found an optimal solution");
2051: optimal = true;
2052: }
2053:
2054: //System.out.println("The value of maxElement is:" + maxElement);
2055:
2056: if (!optimal) {
2057: // Case when the solution is not optimal but not known to be
2058: // either unbounded or infeasable
2059:
2060: // from the above we have found the maximum negative element in
2061: // bottom row, we have also found the column for this value
2062: // the pivotColIndex represents this
2063:
2064: // initialize the values for the algorithm, -1 for pivotRowIndex
2065: // indicates no solution
2066:
2067: prevRatio = Float.POSITIVE_INFINITY;
2068: ratio = 0.0;
2069: pivotRowIndex = -1;
2070:
2071: // note if all of the elements in the pivot column are zero or
2072: // negative the problem is unbounded.
2073: for (i = 0; i < numRow - 1; i++) {
2074: element = problemTableau[i][pivotColIndex]; // r value
2075: endElement = problemTableau[i][numCol - 1]; // s value
2076:
2077: // pivot according to the rule that we want to choose the row
2078: // with smallest s/r ratio see third case
2079: // currently we ignore valuse of r==0 (case 1) and cases where the
2080: // ratio is negative, i.e. either r or s are negative (case 2)
2081: if (element == 0) {
2082: if (debug) {
2083: System.out
2084: .println("Division by zero has occurred");
2085: System.out
2086: .println("Within the linear program solver");
2087: System.out
2088: .println("Ignoring the zero as a potential pivot");
2089: }
2090: } else if ((element < 0.0) || (endElement < 0.0)) {
2091: if (debug) {
2092: System.out
2093: .println("Ignoring cases where element is negative");
2094: System.out
2095: .println("The value of element is: "
2096: + element);
2097: System.out
2098: .println("The value of end Element is: "
2099: + endElement);
2100: }
2101: } else {
2102: ratio = endElement / element; // should be s/r
2103: if (debug) {
2104: System.out
2105: .println("The value of element is: "
2106: + element);
2107: System.out
2108: .println("The value of endElement is: "
2109: + endElement);
2110: System.out
2111: .println("The value of ratio is: "
2112: + ratio);
2113: System.out
2114: .println("The value of prevRatio is: "
2115: + prevRatio);
2116: System.out
2117: .println("Value of ratio <= prevRatio is :"
2118: + (ratio <= prevRatio));
2119: }
2120: if (ratio <= prevRatio) {
2121: if (debug) {
2122: System.out
2123: .println("updating prevRatio with ratio");
2124: }
2125: prevRatio = ratio;
2126: pivotRowIndex = i;
2127: }
2128: }
2129: }
2130:
2131: // if the pivotRowIndex is still -1 then we know the pivotColumn
2132: // has no viable pivot points and the solution is unbounded or
2133: // infeasable (all pivot elements were either zero or negative or
2134: // the right most value was negative (the later shouldn't happen?)
2135: if (pivotRowIndex == -1) {
2136: if (debug) {
2137: System.out.println("UNABLE TO FIND SOLUTION");
2138: System.out
2139: .println("The system is infeasable or unbounded");
2140: }
2141: return (Float.POSITIVE_INFINITY);
2142: }
2143:
2144: // we now have the pivot row and col all that remains is
2145: // to divide through by this value and subtract the appropriate
2146: // multiple of the pivot row from all other rows to obtain
2147: // a tableau which has a column of all zeros and one 1 in the
2148: // intersection of pivot row and col
2149:
2150: // divide through by the pivot value
2151: double pivotValue = problemTableau[pivotRowIndex][pivotColIndex];
2152:
2153: if (debug) {
2154: System.out.println("The value of row index is: "
2155: + pivotRowIndex);
2156: System.out.println("The value of col index is: "
2157: + pivotColIndex);
2158: System.out.println("The value of pivotValue is: "
2159: + pivotValue);
2160: }
2161: // divide through by s on the pivot row to obtain a 1 in pivot col
2162: for (i = 0; i < numCol; i++) {
2163: problemTableau[pivotRowIndex][i] = problemTableau[pivotRowIndex][i]
2164: / pivotValue;
2165: }
2166:
2167: // subtract appropriate multiple of pivot row from all other rows
2168: // to zero out all rows except the final row and the pivot row
2169: for (i = 0; i < numRow; i++) {
2170: if (i != pivotRowIndex) {
2171: multiplier = problemTableau[i][pivotColIndex];
2172: for (int j = 0; j < numCol; j++) {
2173: problemTableau[i][j] = problemTableau[i][j]
2174: - multiplier
2175: * problemTableau[pivotRowIndex][j];
2176: }
2177: }
2178: }
2179: }
2180: // case when the element is optimal
2181: }
2182: return (problemTableau[numRow - 1][numCol - 1]);
2183: }
2184:
2185: static boolean edgeIntersectSphere(BoundingSphere sphere,
2186: Point3d start, Point3d end) {
2187:
2188: double abLenSq, acLenSq, apLenSq, abDotAp, radiusSq;
2189: Vector3d ab = new Vector3d();
2190: Vector3d ap = new Vector3d();
2191:
2192: Point3d center = new Point3d();
2193: sphere.getCenter(center);
2194: double radius = sphere.getRadius();
2195:
2196: ab.x = end.x - start.x;
2197: ab.y = end.y - start.y;
2198: ab.z = end.z - start.z;
2199:
2200: ap.x = center.x - start.x;
2201: ap.y = center.y - start.y;
2202: ap.z = center.z - start.z;
2203:
2204: abDotAp = ab.dot(ap);
2205:
2206: if (abDotAp < 0.0)
2207: return false; // line segment points away from sphere.
2208:
2209: abLenSq = ab.lengthSquared();
2210: acLenSq = abDotAp * abDotAp / abLenSq;
2211:
2212: if (acLenSq < abLenSq)
2213: return false; // C doesn't lies between end points of edge.
2214:
2215: radiusSq = radius * radius;
2216: apLenSq = ap.lengthSquared();
2217:
2218: if ((apLenSq - acLenSq) <= radiusSq)
2219: return true;
2220:
2221: return false;
2222: }
2223:
2224: static double det2D(Point2d a, Point2d b, Point2d p) {
2225: return (((p).x - (a).x) * ((a).y - (b).y) + ((a).y - (p).y)
2226: * ((a).x - (b).x));
2227: }
2228:
2229: // Assume coord is CCW.
2230: static boolean pointIntersectPolygon2D(Vector3d normal,
2231: Point3d[] coord, Point3d point) {
2232:
2233: double absNrmX, absNrmY, absNrmZ;
2234: Point2d coord2D[] = new Point2d[coord.length];
2235: Point2d pnt = new Point2d();
2236:
2237: int i, j, axis;
2238:
2239: // Project 3d points onto 2d plane.
2240: // Note : Area of polygon is not preserve in this projection, but
2241: // it doesn't matter here.
2242:
2243: // Find the axis of projection.
2244: absNrmX = Math.abs(normal.x);
2245: absNrmY = Math.abs(normal.y);
2246: absNrmZ = Math.abs(normal.z);
2247:
2248: if (absNrmX > absNrmY)
2249: axis = 0;
2250: else
2251: axis = 1;
2252:
2253: if (axis == 0) {
2254: if (absNrmX < absNrmZ)
2255: axis = 2;
2256: } else if (axis == 1) {
2257: if (absNrmY < absNrmZ)
2258: axis = 2;
2259: }
2260:
2261: // System.out.println("Normal " + normal + " axis " + axis );
2262:
2263: for (i = 0; i < coord.length; i++) {
2264: coord2D[i] = new Point2d();
2265:
2266: switch (axis) {
2267: case 0:
2268: coord2D[i].x = coord[i].y;
2269: coord2D[i].y = coord[i].z;
2270: break;
2271:
2272: case 1:
2273: coord2D[i].x = coord[i].x;
2274: coord2D[i].y = coord[i].z;
2275: break;
2276:
2277: case 2:
2278: coord2D[i].x = coord[i].x;
2279: coord2D[i].y = coord[i].y;
2280: break;
2281: }
2282: // System.out.println("i " + i + " u " + uCoor[i] + " v " + vCoor[i]);
2283: }
2284:
2285: switch (axis) {
2286: case 0:
2287: pnt.x = point.y;
2288: pnt.y = point.z;
2289: break;
2290:
2291: case 1:
2292: pnt.x = point.x;
2293: pnt.y = point.z;
2294: break;
2295:
2296: case 2:
2297: pnt.x = point.x;
2298: pnt.y = point.y;
2299: break;
2300: }
2301:
2302: // Do determinant test.
2303: for (j = 0; j < coord.length; j++) {
2304: if (j < (coord.length - 1))
2305: if (det2D(coord2D[j], coord2D[j + 1], pnt) > 0.0)
2306: ;
2307: else
2308: return false;
2309: else if (det2D(coord2D[j], coord2D[0], pnt) > 0.0)
2310: ;
2311: else
2312: return false;
2313: }
2314: return true;
2315: }
2316:
2317: static boolean edgeIntersectPlane(Vector3d normal, Point3d pnt,
2318: Point3d start, Point3d end, Point3d iPnt) {
2319:
2320: Vector3d tempV3d = new Vector3d();
2321: Vector3d direction = new Vector3d();
2322: double pD, pNrmDotrDir, tr;
2323:
2324: // Compute plane D.
2325: tempV3d.set((Tuple3d) pnt);
2326: pD = normal.dot(tempV3d);
2327:
2328: direction.x = end.x - start.x;
2329: direction.y = end.y - start.y;
2330: direction.z = end.z - start.z;
2331:
2332: pNrmDotrDir = normal.dot(direction);
2333:
2334: // edge is parallel to plane.
2335: if (pNrmDotrDir == 0.0) {
2336: // System.out.println("Edge is parallel to plane.");
2337: return false;
2338: }
2339:
2340: tempV3d.set((Tuple3d) start);
2341:
2342: tr = (pD - normal.dot(tempV3d)) / pNrmDotrDir;
2343:
2344: // Edge intersects the plane behind the edge's start.
2345: // or exceed the edge's length.
2346: if ((tr < 0.0) || (tr > 1.0)) {
2347: // System.out.println("Edge intersects the plane behind the start or exceed end.");
2348: return false;
2349: }
2350:
2351: iPnt.x = start.x + tr * direction.x;
2352: iPnt.y = start.y + tr * direction.y;
2353: iPnt.z = start.z + tr * direction.z;
2354:
2355: return true;
2356: }
2357:
2358: // Assume coord is CCW.
2359: static boolean edgeIntersectPolygon2D(Vector3d normal,
2360: Point3d[] coord, Point3d[] seg) {
2361:
2362: double absNrmX, absNrmY, absNrmZ;
2363: Point2d coord2D[] = new Point2d[coord.length];
2364: Point2d seg2D[] = new Point2d[2];
2365:
2366: int i, j, axis;
2367:
2368: // Project 3d points onto 2d plane.
2369: // Note : Area of polygon is not preserve in this projection, but
2370: // it doesn't matter here.
2371:
2372: // Find the axis of projection.
2373: absNrmX = Math.abs(normal.x);
2374: absNrmY = Math.abs(normal.y);
2375: absNrmZ = Math.abs(normal.z);
2376:
2377: if (absNrmX > absNrmY)
2378: axis = 0;
2379: else
2380: axis = 1;
2381:
2382: if (axis == 0) {
2383: if (absNrmX < absNrmZ)
2384: axis = 2;
2385: } else if (axis == 1) {
2386: if (absNrmY < absNrmZ)
2387: axis = 2;
2388: }
2389:
2390: // System.out.println("Normal " + normal + " axis " + axis );
2391:
2392: for (i = 0; i < coord.length; i++) {
2393: coord2D[i] = new Point2d();
2394:
2395: switch (axis) {
2396: case 0:
2397: coord2D[i].x = coord[i].y;
2398: coord2D[i].y = coord[i].z;
2399: break;
2400:
2401: case 1:
2402: coord2D[i].x = coord[i].x;
2403: coord2D[i].y = coord[i].z;
2404: break;
2405:
2406: case 2:
2407: coord2D[i].x = coord[i].x;
2408: coord2D[i].y = coord[i].y;
2409: break;
2410: }
2411: // System.out.println("i " + i + " u " + uCoor[i] + " v " + vCoor[i]);
2412: }
2413:
2414: for (i = 0; i < 2; i++) {
2415: seg2D[i] = new Point2d();
2416: switch (axis) {
2417: case 0:
2418: seg2D[i].x = seg[i].y;
2419: seg2D[i].y = seg[i].z;
2420: break;
2421:
2422: case 1:
2423: seg2D[i].x = seg[i].x;
2424: seg2D[i].y = seg[i].z;
2425: break;
2426:
2427: case 2:
2428: seg2D[i].x = seg[i].x;
2429: seg2D[i].y = seg[i].y;
2430: break;
2431: }
2432: // System.out.println("i " + i + " u " + uSeg[i] + " v " + vSeg[i]);
2433: }
2434:
2435: // Do determinant test.
2436: boolean pntTest[][] = new boolean[2][coord.length];
2437: boolean testFlag;
2438:
2439: for (j = 0; j < coord.length; j++) {
2440: for (i = 0; i < 2; i++) {
2441: if (j < (coord.length - 1))
2442: pntTest[i][j] = (det2D(coord2D[j], coord2D[j + 1],
2443: seg2D[i]) < 0.0);
2444: else
2445: pntTest[i][j] = (det2D(coord2D[j], coord2D[0],
2446: seg2D[i]) < 0.0);
2447: }
2448:
2449: if ((pntTest[0][j] == false) && (pntTest[1][j] == false))
2450: return false;
2451: }
2452:
2453: testFlag = true;
2454: for (i = 0; i < coord.length; i++) {
2455: if (pntTest[0][i] == false) {
2456: testFlag = false;
2457: break;
2458: }
2459: }
2460:
2461: if (testFlag == true)
2462: return true; // start point is inside polygon.
2463:
2464: testFlag = true;
2465: for (i = 0; i < coord.length; i++) {
2466: if (pntTest[1][i] == false) {
2467: testFlag = false;
2468: break;
2469: }
2470: }
2471:
2472: if (testFlag == true)
2473: return true; // end point is inside polygon.
2474:
2475: int cnt = 0;
2476: for (i = 0; i < coord.length; i++) {
2477: if (det2D(seg2D[0], seg2D[1], coord2D[i]) < 0.0)
2478: cnt++;
2479: }
2480:
2481: if ((cnt == 0) || (cnt == coord.length))
2482: return false;
2483:
2484: return true;
2485: }
2486:
2487: static boolean intersectPolygon(Point3d coord1[], Point3d coord2[],
2488: boolean doTrivialTest) {
2489: int i, j;
2490: Vector3d vec0 = new Vector3d(); //Edge vector from point 0 to point 1;
2491: Vector3d vec1 = new Vector3d(); //Edge vector from point 0 to point 2 or 3;
2492: Vector3d pNrm = new Vector3d();
2493: boolean epFlag;
2494:
2495: // compute plane normal for coord1.
2496: for (i = 0; i < coord1.length - 1;) {
2497: vec0.x = coord1[i + 1].x - coord1[i].x;
2498: vec0.y = coord1[i + 1].y - coord1[i].y;
2499: vec0.z = coord1[i + 1].z - coord1[i++].z;
2500: if (vec0.length() > 0.0)
2501: break;
2502: }
2503:
2504: for (j = i; j < coord1.length - 1; j++) {
2505: vec1.x = coord1[j + 1].x - coord1[j].x;
2506: vec1.y = coord1[j + 1].y - coord1[j].y;
2507: vec1.z = coord1[j + 1].z - coord1[j].z;
2508: if (vec1.length() > 0.0)
2509: break;
2510: }
2511:
2512: if (j == (coord1.length - 1)) {
2513: // System.out.println("(1) Degenerated polygon.");
2514: return false; // Degenerated polygon.
2515: }
2516:
2517: /*
2518: for (i=0; i<coord1.length; i++)
2519: System.out.println("coord1 P" + i + " " + coord1[i]);
2520: for (i=0; i<coord2.length; i++)
2521: System.out.println("coord2 P" + i + " " + coord2[i]);
2522: */
2523:
2524: pNrm.cross(vec0, vec1);
2525:
2526: if (pNrm.length() == 0.0) {
2527: // System.out.println("(2) Degenerated polygon.");
2528: return false; // Degenerated polygon.
2529: }
2530:
2531: // Do trivial test here.
2532: if (doTrivialTest == true) {
2533: // Not implemented yet.
2534: }
2535:
2536: j = 0;
2537: Point3d seg[] = new Point3d[2];
2538: seg[0] = new Point3d();
2539: seg[1] = new Point3d();
2540:
2541: for (i = 0; i < coord2.length; i++) {
2542: if (i < (coord2.length - 1))
2543: epFlag = edgeIntersectPlane(pNrm, coord1[0], coord2[i],
2544: coord2[i + 1], seg[j]);
2545: else
2546: epFlag = edgeIntersectPlane(pNrm, coord1[0], coord2[i],
2547: coord2[0], seg[j]);
2548: if (epFlag == true) {
2549: j++;
2550: if (j > 1)
2551: break;
2552: }
2553: }
2554:
2555: if (j == 0)
2556: return false;
2557:
2558: if (coord2.length < 3)
2559: return pointIntersectPolygon2D(pNrm, coord1, seg[0]);
2560:
2561: return edgeIntersectPolygon2D(pNrm, coord1, seg);
2562: }
2563:
2564: static final boolean isNonZero(double v) {
2565: return ((v > EPS) || (v < -EPS));
2566:
2567: }
2568:
2569: static boolean intersectRay(Point3d coordinates[], PickRay ray,
2570: PickIntersection pi) {
2571: Point3d origin = new Point3d();
2572: Vector3d direction = new Vector3d();
2573: boolean result;
2574: ray.get(origin, direction);
2575: result = intersectRayOrSegment(coordinates, direction, origin,
2576: pi, false);
2577: return result;
2578: }
2579:
2580: /**
2581: * Return true if triangle or quad intersects with ray and the distance is
2582: * stored in pr.
2583: * */
2584: static boolean intersectRayOrSegment(Point3d coordinates[],
2585: Vector3d direction, Point3d origin, PickIntersection pi,
2586: boolean isSegment) {
2587: Vector3d vec0, vec1, pNrm, tempV3d;
2588: Point3d iPnt;
2589:
2590: vec0 = new Vector3d();
2591: vec1 = new Vector3d();
2592: pNrm = new Vector3d();
2593:
2594: double absNrmX, absNrmY, absNrmZ, pD = 0.0;
2595: double pNrmDotrDir = 0.0;
2596:
2597: boolean isIntersect = false;
2598: int i, j, k = 0, l = 0;
2599:
2600: // Compute plane normal.
2601: for (i = 0; i < coordinates.length; i++) {
2602: if (i != coordinates.length - 1) {
2603: l = i + 1;
2604: } else {
2605: l = 0;
2606: }
2607: vec0.x = coordinates[l].x - coordinates[i].x;
2608: vec0.y = coordinates[l].y - coordinates[i].y;
2609: vec0.z = coordinates[l].z - coordinates[i].z;
2610: if (vec0.length() > 0.0) {
2611: break;
2612: }
2613: }
2614:
2615: for (j = l; j < coordinates.length; j++) {
2616: if (j != coordinates.length - 1) {
2617: k = j + 1;
2618: } else {
2619: k = 0;
2620: }
2621: vec1.x = coordinates[k].x - coordinates[j].x;
2622: vec1.y = coordinates[k].y - coordinates[j].y;
2623: vec1.z = coordinates[k].z - coordinates[j].z;
2624: if (vec1.length() > 0.0) {
2625: break;
2626: }
2627: }
2628:
2629: pNrm.cross(vec0, vec1);
2630:
2631: if ((vec1.length() == 0) || (pNrm.length() == 0)) {
2632: // degenerated to line if vec0.length() == 0
2633: // or vec0.length > 0 and vec0 parallel to vec1
2634: k = (l == 0 ? coordinates.length - 1 : l - 1);
2635: isIntersect = intersectLineAndRay(coordinates[l],
2636: coordinates[k], origin, direction, pi);
2637: return isIntersect;
2638: }
2639:
2640: // It is possible that Quad is degenerate to Triangle
2641: // at this point
2642:
2643: pNrmDotrDir = pNrm.dot(direction);
2644:
2645: // Ray is parallel to plane.
2646: if (pNrmDotrDir == 0.0) {
2647: // Ray is parallel to plane
2648: // Check line/triangle intersection on plane.
2649: for (i = 0; i < coordinates.length; i++) {
2650: if (i != coordinates.length - 1) {
2651: k = i + 1;
2652: } else {
2653: k = 0;
2654: }
2655: if (intersectLineAndRay(coordinates[i], coordinates[k],
2656: origin, direction, pi)) {
2657: isIntersect = true;
2658: break;
2659: }
2660: }
2661: return isIntersect;
2662: }
2663:
2664: // Plane equation: (p - p0)*pNrm = 0 or p*pNrm = pD;
2665: tempV3d = new Vector3d();
2666: tempV3d.set((Tuple3d) coordinates[0]);
2667: pD = pNrm.dot(tempV3d);
2668: tempV3d.set((Tuple3d) origin);
2669:
2670: // Substitute Ray equation:
2671: // p = origin + pi.distance*direction
2672: // into the above Plane equation
2673:
2674: double dist = (pD - pNrm.dot(tempV3d)) / pNrmDotrDir;
2675:
2676: // Ray intersects the plane behind the ray's origin.
2677: if ((dist < -EPS) || (isSegment && (dist > 1.0 + EPS))) {
2678: // Ray intersects the plane behind the ray's origin
2679: // or intersect point not fall in Segment
2680: return false;
2681: }
2682:
2683: // Now, one thing for sure the ray intersect the plane.
2684: // Find the intersection point.
2685: iPnt = new Point3d();
2686: iPnt.x = origin.x + direction.x * dist;
2687: iPnt.y = origin.y + direction.y * dist;
2688: iPnt.z = origin.z + direction.z * dist;
2689:
2690: // Project 3d points onto 2d plane.
2691: // Find the axis so that area of projection is maximize.
2692: absNrmX = Math.abs(pNrm.x);
2693: absNrmY = Math.abs(pNrm.y);
2694: absNrmZ = Math.abs(pNrm.z);
2695:
2696: // Check out
2697: // http://astronomy.swin.edu.au/~pbourke/geometry/insidepoly/
2698: // Solution 3:
2699: // All sign of (y - y0) (x1 - x0) - (x - x0) (y1 - y0)
2700: // must agree.
2701: double sign, t, lastSign = 0;
2702: Point3d p0 = coordinates[coordinates.length - 1];
2703: Point3d p1 = coordinates[0];
2704:
2705: isIntersect = true;
2706:
2707: if (absNrmX > absNrmY) {
2708: if (absNrmX < absNrmZ) {
2709: for (i = 0; i < coordinates.length; i++) {
2710: p0 = coordinates[i];
2711: p1 = (i != coordinates.length - 1 ? coordinates[i + 1]
2712: : coordinates[0]);
2713: sign = (iPnt.y - p0.y) * (p1.x - p0.x)
2714: - (iPnt.x - p0.x) * (p1.y - p0.y);
2715: if (isNonZero(sign)) {
2716: if (sign * lastSign < 0) {
2717: isIntersect = false;
2718: break;
2719: }
2720: lastSign = sign;
2721: } else { // point on line, check inside interval
2722: t = p1.y - p0.y;
2723: if (isNonZero(t)) {
2724: t = (iPnt.y - p0.y) / t;
2725: isIntersect = ((t > -EPS) && (t < 1 + EPS));
2726: break;
2727: } else {
2728: t = p1.x - p0.x;
2729: if (isNonZero(t)) {
2730: t = (iPnt.x - p0.x) / t;
2731: isIntersect = ((t > -EPS) && (t < 1 + EPS));
2732: break;
2733: } else {
2734: //degenerate line=>point
2735: }
2736: }
2737: }
2738: }
2739: } else {
2740: for (i = 0; i < coordinates.length; i++) {
2741: p0 = coordinates[i];
2742: p1 = (i != coordinates.length - 1 ? coordinates[i + 1]
2743: : coordinates[0]);
2744: sign = (iPnt.y - p0.y) * (p1.z - p0.z)
2745: - (iPnt.z - p0.z) * (p1.y - p0.y);
2746: if (isNonZero(sign)) {
2747: if (sign * lastSign < 0) {
2748: isIntersect = false;
2749: break;
2750: }
2751: lastSign = sign;
2752: } else { // point on line, check inside interval
2753: t = p1.y - p0.y;
2754:
2755: if (isNonZero(t)) {
2756: t = (iPnt.y - p0.y) / t;
2757: isIntersect = ((t > -EPS) && (t < 1 + EPS));
2758: break;
2759:
2760: } else {
2761: t = p1.z - p0.z;
2762: if (isNonZero(t)) {
2763: t = (iPnt.z - p0.z) / t;
2764: isIntersect = ((t > -EPS) && (t < 1 + EPS));
2765: break;
2766: } else {
2767: //degenerate line=>point
2768: }
2769: }
2770: }
2771: }
2772: }
2773: } else {
2774: if (absNrmY < absNrmZ) {
2775: for (i = 0; i < coordinates.length; i++) {
2776: p0 = coordinates[i];
2777: p1 = (i != coordinates.length - 1 ? coordinates[i + 1]
2778: : coordinates[0]);
2779: sign = (iPnt.y - p0.y) * (p1.x - p0.x)
2780: - (iPnt.x - p0.x) * (p1.y - p0.y);
2781: if (isNonZero(sign)) {
2782: if (sign * lastSign < 0) {
2783: isIntersect = false;
2784: break;
2785: }
2786: lastSign = sign;
2787: } else { // point on line, check inside interval
2788: t = p1.y - p0.y;
2789: if (isNonZero(t)) {
2790: t = (iPnt.y - p0.y) / t;
2791: isIntersect = ((t > -EPS) && (t < 1 + EPS));
2792: break;
2793: } else {
2794: t = p1.x - p0.x;
2795: if (isNonZero(t)) {
2796: t = (iPnt.x - p0.x) / t;
2797: isIntersect = ((t > -EPS) && (t < 1 + EPS));
2798: break;
2799: } else {
2800: //degenerate line=>point
2801: }
2802: }
2803: }
2804: }
2805: } else {
2806: for (i = 0; i < coordinates.length; i++) {
2807: p0 = coordinates[i];
2808: p1 = (i != coordinates.length - 1 ? coordinates[i + 1]
2809: : coordinates[0]);
2810: sign = (iPnt.x - p0.x) * (p1.z - p0.z)
2811: - (iPnt.z - p0.z) * (p1.x - p0.x);
2812: if (isNonZero(sign)) {
2813: if (sign * lastSign < 0) {
2814: isIntersect = false;
2815: break;
2816: }
2817: lastSign = sign;
2818: } else { // point on line, check inside interval
2819: t = p1.x - p0.x;
2820: if (isNonZero(t)) {
2821: t = (iPnt.x - p0.x) / t;
2822: isIntersect = ((t > -EPS) && (t < 1 + EPS));
2823: break;
2824: } else {
2825: t = p1.z - p0.z;
2826: if (isNonZero(t)) {
2827: t = (iPnt.z - p0.z) / t;
2828: isIntersect = ((t > -EPS) && (t < 1 + EPS));
2829: break;
2830: } else {
2831: //degenerate line=>point
2832: }
2833: }
2834: }
2835: }
2836: }
2837: }
2838:
2839: if (isIntersect) {
2840: pi.setDistance(dist * direction.length());
2841: pi.setPointCoordinatesVW(iPnt);
2842: }
2843: return isIntersect;
2844: }
2845:
2846: /**
2847: Return true if triangle or quad intersects with segment and the distance is
2848: stored in dist.
2849: */
2850: static boolean intersectSegment(Point3d coordinates[],
2851: PickSegment segment, PickIntersection pi) {
2852: Point3d start = new Point3d();
2853: Point3d end = new Point3d();
2854: Vector3d direction = new Vector3d();
2855: boolean result;
2856: segment.get(start, end);
2857: direction.x = end.x - start.x;
2858: direction.y = end.y - start.y;
2859: direction.z = end.z - start.z;
2860: result = intersectRayOrSegment(coordinates, direction, start,
2861: pi, true);
2862: return result;
2863: }
2864:
2865: /**
2866: Return true if point is on the inside of halfspace test. The halfspace is
2867: partition by the plane of triangle or quad.
2868: */
2869:
2870: static boolean inside(Point3d coordinates[], PickPoint point,
2871: int ccw) {
2872:
2873: Vector3d vec0 = new Vector3d(); //Edge vector from point 0 to point 1;
2874: Vector3d vec1 = new Vector3d(); //Edge vector from point 0 to point 2 or 3;
2875: Vector3d pNrm = new Vector3d();
2876: double absNrmX, absNrmY, absNrmZ, pD = 0.0;
2877: Vector3d tempV3d = new Vector3d();
2878: double pNrmDotrDir = 0.0;
2879:
2880: double tempD;
2881:
2882: int i, j;
2883:
2884: Point3d location = new Point3d();
2885: point.get(location);
2886:
2887: // Compute plane normal.
2888: for (i = 0; i < coordinates.length - 1;) {
2889: vec0.x = coordinates[i + 1].x - coordinates[i].x;
2890: vec0.y = coordinates[i + 1].y - coordinates[i].y;
2891: vec0.z = coordinates[i + 1].z - coordinates[i++].z;
2892: if (vec0.length() > 0.0)
2893: break;
2894: }
2895:
2896: for (j = i; j < coordinates.length - 1; j++) {
2897: vec1.x = coordinates[j + 1].x - coordinates[j].x;
2898: vec1.y = coordinates[j + 1].y - coordinates[j].y;
2899: vec1.z = coordinates[j + 1].z - coordinates[j].z;
2900: if (vec1.length() > 0.0)
2901: break;
2902: }
2903:
2904: if (j == (coordinates.length - 1)) {
2905: // System.out.println("(1) Degenerated polygon.");
2906: return false; // Degenerated polygon.
2907: }
2908:
2909: /*
2910: System.out.println("Ray orgin : " + origin + " dir " + direction);
2911: System.out.println("Triangle/Quad :");
2912: for (i=0; i<coordinates.length; i++)
2913: System.out.println("P" + i + " " + coordinates[i]);
2914: */
2915:
2916: if (ccw == 0x1)
2917: pNrm.cross(vec0, vec1);
2918: else
2919: pNrm.cross(vec1, vec0);
2920:
2921: if (pNrm.length() == 0.0) {
2922: // System.out.println("(2) Degenerated polygon.");
2923: return false; // Degenerated polygon.
2924: }
2925: // Compute plane D.
2926: tempV3d.set((Tuple3d) coordinates[0]);
2927: pD = pNrm.dot(tempV3d);
2928:
2929: tempV3d.set((Tuple3d) location);
2930:
2931: if ((pD - pNrm.dot(tempV3d)) > 0.0) {
2932: // System.out.println("point is on the outside of plane.");
2933: return false;
2934: } else
2935: return true;
2936: }
2937:
2938: static boolean intersectPntAndPnt(Point3d pnt1, Point3d pnt2,
2939: PickIntersection pi) {
2940:
2941: if ((pnt1.x == pnt2.x) && (pnt1.y == pnt2.y)
2942: && (pnt1.z == pnt2.z)) {
2943: pi.setPointCoordinatesVW(pnt1);
2944: pi.setDistance(0.0);
2945: return true;
2946: } else
2947: return false;
2948: }
2949:
2950: static boolean intersectPntAndRay(Point3d pnt, Point3d ori,
2951: Vector3d dir, PickIntersection pi) {
2952: int flag = 0;
2953: double temp;
2954: double dist;
2955:
2956: if (dir.x != 0.0) {
2957: flag = 0;
2958: dist = (pnt.x - ori.x) / dir.x;
2959: } else if (dir.y != 0.0) {
2960: if (pnt.x != ori.x)
2961: return false;
2962: flag = 1;
2963: dist = (pnt.y - ori.y) / dir.y;
2964: } else if (dir.z != 0.0) {
2965: if ((pnt.x != ori.x) || (pnt.y != ori.y))
2966: return false;
2967: flag = 2;
2968: dist = (pnt.z - ori.z) / dir.z;
2969:
2970: } else
2971: return false;
2972:
2973: if (dist < 0.0)
2974: return false;
2975:
2976: if (flag == 0) {
2977: temp = ori.y + dist * dir.y;
2978: if ((pnt.y < (temp - Double.MIN_VALUE))
2979: || (pnt.y > (temp + Double.MIN_VALUE)))
2980: return false;
2981: }
2982:
2983: if (flag < 2) {
2984: temp = ori.z + dist * dir.z;
2985: if ((pnt.z < (temp - Double.MIN_VALUE))
2986: || (pnt.z > (temp + Double.MIN_VALUE)))
2987: return false;
2988: }
2989:
2990: pi.setPointCoordinatesVW(pnt);
2991: pi.setDistance(dist);
2992:
2993: return true;
2994:
2995: }
2996:
2997: static boolean intersectLineAndRay(Point3d start, Point3d end,
2998: Point3d ori, Vector3d dir, PickIntersection pi) {
2999:
3000: double m00, m01, m10, m11;
3001: double mInv00, mInv01, mInv10, mInv11;
3002: double dmt, t, s, tmp1, tmp2;
3003: Vector3d lDir;
3004: double dist;
3005:
3006: // System.out.println("Intersect : intersectLineAndRay");
3007: // System.out.println("start " + start + " end " + end );
3008: // System.out.println("ori " + ori + " dir " + dir);
3009:
3010: lDir = new Vector3d(end.x - start.x, end.y - start.y, end.z
3011: - start.z);
3012:
3013: m00 = lDir.x;
3014: m01 = -dir.x;
3015: m10 = lDir.y;
3016: m11 = -dir.y;
3017:
3018: // Get the determinant.
3019: dmt = (m00 * m11) - (m10 * m01);
3020:
3021: if (dmt == 0.0) { // No solution, hence no intersect.
3022: // System.out.println("dmt is zero");
3023: boolean isIntersect = false;
3024: if ((lDir.x == 0) && (lDir.y == 0) && (lDir.z == 0)) {
3025: isIntersect = intersectPntAndRay(start, ori, dir, pi);
3026: if (isIntersect) {
3027: pi.setPointCoordinatesVW(start);
3028: pi.setDistance(0);
3029: }
3030: }
3031: return isIntersect;
3032: }
3033: // Find the inverse.
3034: tmp1 = 1 / dmt;
3035:
3036: mInv00 = tmp1 * m11;
3037: mInv01 = tmp1 * (-m01);
3038: mInv10 = tmp1 * (-m10);
3039: mInv11 = tmp1 * m00;
3040:
3041: tmp1 = ori.x - start.x;
3042: tmp2 = ori.y - start.y;
3043:
3044: t = mInv00 * tmp1 + mInv01 * tmp2;
3045: s = mInv10 * tmp1 + mInv11 * tmp2;
3046:
3047: if (s < 0.0) { // Before the origin of ray.
3048: // System.out.println("Before the origin of ray " + s);
3049: return false;
3050: }
3051: if ((t < 0) || (t > 1.0)) {// Before or after the end points of line.
3052: // System.out.println("Before or after the end points of line. " + t);
3053: return false;
3054: }
3055:
3056: tmp1 = ori.z + s * dir.z;
3057: tmp2 = start.z + t * lDir.z;
3058:
3059: if ((tmp1 < (tmp2 - Double.MIN_VALUE))
3060: || (tmp1 > (tmp2 + Double.MIN_VALUE))) {
3061: // System.out.println("No intersection : tmp1 " + tmp1 + " tmp2 " + tmp2);
3062: return false;
3063: }
3064: dist = s;
3065:
3066: pi.setDistance(dist);
3067: Point3d iPnt = new Point3d();
3068: iPnt.scaleAdd(s, dir, ori);
3069: pi.setPointCoordinatesVW(iPnt);
3070:
3071: // System.out.println("Intersected : tmp1 " + tmp1 + " tmp2 " + tmp2);
3072: return true;
3073: }
3074:
3075: /**
3076: Return true if triangle or quad intersects with cylinder and the
3077: distance is stored in pr.
3078: */
3079: static boolean intersectCylinder(Point3d coordinates[],
3080: PickCylinder cyl, PickIntersection pi) {
3081:
3082: Point3d origin = new Point3d();
3083: Point3d end = new Point3d();
3084: Vector3d direction = new Vector3d();
3085: Point3d iPnt1 = new Point3d();
3086: Point3d iPnt2 = new Point3d();
3087: Vector3d originToIpnt = new Vector3d();
3088:
3089: // Get cylinder information
3090: cyl.getOrigin(origin);
3091: cyl.getDirection(direction);
3092: double radius = cyl.getRadius();
3093:
3094: if (cyl instanceof PickCylinderSegment) {
3095: ((PickCylinderSegment) cyl).getEnd(end);
3096: }
3097:
3098: // If the ray intersects, we're good (do not do this if we only have
3099: // a segment
3100: if (coordinates.length > 2) {
3101: if (cyl instanceof PickCylinderRay) {
3102: if (intersectRay(coordinates, new PickRay(origin,
3103: direction), pi)) {
3104: return true;
3105: }
3106: } else {
3107: if (intersectSegment(coordinates, new PickSegment(
3108: origin, end), pi)) {
3109: return true;
3110: }
3111: }
3112: }
3113:
3114: // Ray doesn't intersect, check distance to edges
3115: double sqDistToEdge;
3116: for (int i = 0; i < coordinates.length - 1; i++) {
3117: if (cyl instanceof PickCylinderSegment) {
3118: sqDistToEdge = Distance.segmentToSegment(origin, end,
3119: coordinates[i], coordinates[i + 1], iPnt1,
3120: iPnt2, null);
3121: } else {
3122: sqDistToEdge = Distance.rayToSegment(origin, direction,
3123: coordinates[i], coordinates[i + 1], iPnt1,
3124: iPnt2, null);
3125: }
3126: if (sqDistToEdge <= radius * radius) {
3127: pi.setPointCoordinatesVW(iPnt2);
3128: originToIpnt.sub(iPnt1, origin);
3129: pi.setDistance(originToIpnt.length());
3130: return true;
3131: }
3132: }
3133: return false;
3134: }
3135:
3136: /**
3137: Return true if triangle or quad intersects with cone. The
3138: distance is stored in pr.
3139: */
3140: static boolean intersectCone(Point3d coordinates[], PickCone cone,
3141: PickIntersection pi) {
3142:
3143: Point3d origin = new Point3d();
3144: Point3d end = new Point3d();
3145: Vector3d direction = new Vector3d();
3146: Vector3d originToIpnt = new Vector3d();
3147: double distance;
3148:
3149: Point3d iPnt1 = new Point3d();
3150: Point3d iPnt2 = new Point3d();
3151: Vector3d vector = new Vector3d();
3152:
3153: // Get cone information
3154: cone.getOrigin(origin);
3155: cone.getDirection(direction);
3156: double radius;
3157:
3158: if (cone instanceof PickConeSegment) {
3159: ((PickConeSegment) cone).getEnd(end);
3160: }
3161:
3162: // If the ray intersects, we're good (do not do this if we only have
3163: // a segment
3164: if (coordinates.length > 2) {
3165: if (cone instanceof PickConeRay) {
3166: if (intersectRay(coordinates, new PickRay(origin,
3167: direction), pi)) {
3168: return true;
3169: }
3170: } else {
3171: if (intersectSegment(coordinates, new PickSegment(
3172: origin, end), pi)) {
3173: return true;
3174: }
3175: }
3176: }
3177:
3178: // Ray doesn't intersect, check distance to edges
3179: double sqDistToEdge;
3180: for (int i = 0; i < coordinates.length - 1; i++) {
3181: if (cone instanceof PickConeSegment) {
3182: sqDistToEdge = Distance.segmentToSegment(origin, end,
3183: coordinates[i], coordinates[i + 1], iPnt1,
3184: iPnt2, null);
3185: } else {
3186: sqDistToEdge = Distance.rayToSegment(origin, direction,
3187: coordinates[i], coordinates[i + 1], iPnt1,
3188: iPnt2, null);
3189: }
3190: originToIpnt.sub(iPnt1, origin);
3191: distance = originToIpnt.length();
3192: radius = Math.tan(cone.getSpreadAngle()) * distance;
3193: if (sqDistToEdge <= radius * radius) {
3194: // System.out.println ("intersectCone: edge "+i+" intersected");
3195: pi.setPointCoordinatesVW(iPnt2);
3196: pi.setDistance(distance);
3197: return true;
3198: }
3199: }
3200: return false;
3201: }
3202:
3203: /**
3204: Return true if point intersects with cylinder and the
3205: distance is stored in pi.
3206: */
3207: static boolean intersectCylinder(Point3d pt, PickCylinder cyl,
3208: PickIntersection pi) {
3209:
3210: Point3d origin = new Point3d();
3211: Point3d end = new Point3d();
3212: Vector3d direction = new Vector3d();
3213: Point3d iPnt = new Point3d();
3214: Vector3d originToIpnt = new Vector3d();
3215:
3216: // Get cylinder information
3217: cyl.getOrigin(origin);
3218: cyl.getDirection(direction);
3219: double radius = cyl.getRadius();
3220: double sqDist;
3221:
3222: if (cyl instanceof PickCylinderSegment) {
3223: ((PickCylinderSegment) cyl).getEnd(end);
3224: sqDist = Distance.pointToSegment(pt, origin, end, iPnt,
3225: null);
3226: } else {
3227: sqDist = Distance.pointToRay(pt, origin, direction, iPnt,
3228: null);
3229: }
3230: if (sqDist <= radius * radius) {
3231: pi.setPointCoordinatesVW(pt);
3232: originToIpnt.sub(iPnt, origin);
3233: pi.setDistance(originToIpnt.length());
3234: return true;
3235: }
3236: return false;
3237: }
3238:
3239: /**
3240: Return true if point intersects with cone and the
3241: distance is stored in pi.
3242: */
3243: static boolean intersectCone(Point3d pt, PickCone cone,
3244: PickIntersection pi) {
3245:
3246: // System.out.println ("Intersect.intersectCone point");
3247:
3248: Point3d origin = new Point3d();
3249: Point3d end = new Point3d();
3250: Vector3d direction = new Vector3d();
3251: Point3d iPnt = new Point3d();// the closest point on the cone vector
3252: Vector3d originToIpnt = new Vector3d();
3253:
3254: // Get cone information
3255: cone.getOrigin(origin);
3256: cone.getDirection(direction);
3257: double radius;
3258: double distance;
3259: double sqDist;
3260:
3261: if (cone instanceof PickConeSegment) {
3262: ((PickConeSegment) cone).getEnd(end);
3263: sqDist = Distance.pointToSegment(pt, origin, end, iPnt,
3264: null);
3265: } else {
3266: sqDist = Distance.pointToRay(pt, origin, direction, iPnt,
3267: null);
3268: }
3269: originToIpnt.sub(iPnt, origin);
3270: distance = originToIpnt.length();
3271: radius = Math.tan(cone.getSpreadAngle()) * distance;
3272: if (sqDist <= radius * radius) {
3273: pi.setPointCoordinatesVW(pt);
3274: pi.setDistance(distance);
3275: return true;
3276: }
3277: return false;
3278: }
3279:
3280: } // PickResult
|