Add AABBox.getRayIntersection(..), VectorUtil.line2PlaneIntersection(..) incl. getNormal*(..) and getPlane*(..)

AABBox.getRayIntersection(..) provides the intersecting coordinates,
where the fast alternative AABBox.intersectsRay(..) does not.

4 files changed, 319 insertions, 2 deletions

diff --git a/src/jogl/classes/com/jogamp/opengl/math/Ray.java b/src/jogl/classes/com/jogamp/opengl/math/Ray.java
index 3fe7dd3b1..0daca2504 100644
--- a/src/jogl/classes/com/jogamp/opengl/math/Ray.java
+++ b/src/jogl/classes/com/jogamp/opengl/math/Ray.java
@@ -41,7 +41,7 @@ import com.jogamp.opengl.math.geom.AABBox;
  * </p>
  * <p>
  * A {@link Ray} maybe used for <i>picking</i>
- * using a {@link AABBox#intersectsRay(Ray, float[]) bounding box}.
+ * using a {@link AABBox#getRayIntersection(Ray, float[]) bounding box}.
  * </p>
  */
 public class Ray {
diff --git a/src/jogl/classes/com/jogamp/opengl/math/VectorUtil.java b/src/jogl/classes/com/jogamp/opengl/math/VectorUtil.java
index 557884c66..1d78ff30d 100644
--- a/src/jogl/classes/com/jogamp/opengl/math/VectorUtil.java
+++ b/src/jogl/classes/com/jogamp/opengl/math/VectorUtil.java
@@ -241,9 +241,24 @@ public class VectorUtil {
     /**
      * Compute the squared length of a vector, a.k.a the squared <i>norm</i> or squared <i>magnitude</i>
      */
+    public static float vec2NormSquare(final float[] vec) {
+        return vec[0]*vec[0] + vec[1]*vec[1];
+    }
+
+    /**
+     * Compute the squared length of a vector, a.k.a the squared <i>norm</i> or squared <i>magnitude</i>
+     */
     public static float vec3NormSquare(final float[] vec) {
         return vec[0]*vec[0] + vec[1]*vec[1] + vec[2]*vec[2];
     }
+
+    /**
+     * Compute the length of a vector, a.k.a the <i>norm</i> or <i>magnitude</i>
+     */
+    public static float vec2Norm(final float[] vec) {
+        return FloatUtil.sqrt(vec2NormSquare(vec));
+    }
+
     /**
      * Compute the length of a vector, a.k.a the <i>norm</i> or <i>magnitude</i>
      */
@@ -258,6 +273,44 @@ public class VectorUtil {
      * @return normalized output vector
      * @return result vector for chaining
      */
+    public static float[] normalizeVec2(final float[] result, final float[] vector) {
+        final float lengthSq = vec2NormSquare(vector);
+        if ( FloatUtil.isZero(lengthSq, FloatUtil.EPSILON) ) {
+            result[0] = 0f;
+            result[1] = 0f;
+        } else {
+            final float invSqr = 1f / FloatUtil.sqrt(lengthSq);
+            result[0] = vector[0] * invSqr;
+            result[1] = vector[1] * invSqr;
+        }
+        return result;
+    }
+
+    /**
+     * Normalize a vector in place
+     * @param vector input vector
+     * @return normalized output vector
+     */
+    public static float[] normalizeVec2(final float[] vector) {
+        final float lengthSq = vec2NormSquare(vector);
+        if ( FloatUtil.isZero(lengthSq, FloatUtil.EPSILON) ) {
+            vector[0] = 0f;
+            vector[1] = 0f;
+        } else {
+            final float invSqr = 1f / FloatUtil.sqrt(lengthSq);
+            vector[0] *= invSqr;
+            vector[1] *= invSqr;
+        }
+        return vector;
+    }
+
+    /**
+     * Normalize a vector
+     * @param result output vector, may be vector (in-place)
+     * @param vector input vector
+     * @return normalized output vector
+     * @return result vector for chaining
+     */
     public static float[] normalizeVec3(final float[] result, final float[] vector) {
         final float lengthSq = vec3NormSquare(vector);
         if ( FloatUtil.isZero(lengthSq, FloatUtil.EPSILON) ) {
@@ -300,6 +353,19 @@ public class VectorUtil {
      * @param scale single scale constant for all vector components
      * @return result vector for chaining
      */
+    public static float[] scaleVec2(final float[] result, final float[] vector, final float scale) {
+        result[0] = vector[0] * scale;
+        result[1] = vector[1] * scale;
+        return result;
+    }
+
+    /**
+     * Scales a vector by param using given result float[]
+     * @param result vector for the result, may be vector (in-place)
+     * @param vector input vector
+     * @param scale single scale constant for all vector components
+     * @return result vector for chaining
+     */
     public static float[] scaleVec3(final float[] result, final float[] vector, final float scale) {
         result[0] = vector[0] * scale;
         result[1] = vector[1] * scale;
@@ -323,6 +389,33 @@ public class VectorUtil {
     }
 
     /**
+     * Scales a vector by param using given result float[]
+     * @param result vector for the result, may be vector (in-place)
+     * @param vector input vector
+     * @param scale 2 component scale constant for each vector component
+     * @return result vector for chaining
+     */
+    public static float[] scaleVec2(final float[] result, final float[] vector, final float[] scale)
+    {
+        result[0] = vector[0] * scale[0];
+        result[1] = vector[1] * scale[1];
+        return result;
+    }
+
+    /**
+     * Adds two vectors, result = v1 + v2
+     * @param result float[2] result vector, may be either v1 or v2 (in-place)
+     * @param v1 vector 1
+     * @param v2 vector 2
+     * @return result vector for chaining
+     */
+    public static float[] addVec2(final float[] result, final float[] v1, final float[] v2) {
+        result[0] = v1[0] + v2[0];
+        result[1] = v1[1] + v2[1];
+        return result;
+    }
+
+    /**
      * Adds two vectors, result = v1 + v2
      * @param result float[3] result vector, may be either v1 or v2 (in-place)
      * @param v1 vector 1
@@ -721,6 +814,101 @@ public class VectorUtil {
         return area(vertices) >= 0 ? Winding.CCW : Winding.CW ;
     }
 
+    /**
+     * @param result vec2 result for normal
+     * @param v1 vec2
+     * @param v2 vec2
+     * @return result for chaining
+     */
+    public static float[] getNormalVec2(final float[] result, final float[] v1, final float[] v2 ) {
+        subVec2(result, v2, v1);
+        final float tmp = result [ 0 ] ; result [ 0 ] = -result [ 1 ] ; result [ 1 ] = tmp ;
+        return normalizeVec2 ( result ) ;
+    }
+
+    /**
+     * Returns the 3d surface normal of a triangle given three vertices.
+     *
+     * @param result vec3 result for normal
+     * @param v1 vec3
+     * @param v2 vec3
+     * @param v3 vec3
+     * @param tmp1Vec3 temp vec3
+     * @param tmp2Vec3 temp vec3
+     * @return result for chaining
+     */
+    public static float[] getNormalVec3(final float[] result, final float[] v1, final float[] v2, final float[] v3,
+                                        final float[] tmp1Vec3, final float[] tmp2Vec3) {
+        subVec3 ( tmp1Vec3, v2, v1 );
+        subVec3 ( tmp2Vec3, v3, v1 ) ;
+        return normalizeVec3 ( crossVec3(result, tmp1Vec3, tmp2Vec3) ) ;
+    }
+
+    /**
+     * Finds the plane equation of a plane given its normal and a point on the plane.
+     *
+     * @param resultV4 vec4 plane equation
+     * @param normalVec3
+     * @param pVec3
+     * @return result for chaining
+     */
+    public static float[] getPlaneVec3(final float[/*4*/] resultV4, final float[] normalVec3, final float[] pVec3) {
+        /**
+            Ax + By + Cz + D == 0 ;
+            D = - ( Ax + By + Cz )
+              = - ( A*a[0] + B*a[1] + C*a[2] )
+              = - vec3Dot ( normal, a ) ;
+         */
+        System.arraycopy(normalVec3, 0, resultV4, 0, 3);
+        resultV4 [ 3 ] = -vec3Dot(normalVec3, pVec3) ;
+        return resultV4;
+    }
+
+    /**
+     * This finds the plane equation of a triangle given three vertices.
+     *
+     * @param resultVec4 vec4 plane equation
+     * @param v1 vec3
+     * @param v2 vec3
+     * @param v3 vec3
+     * @param temp1V3
+     * @param temp2V3
+     * @return result for chaining
+     */
+    public static float[] getPlaneVec3(final float[/*4*/] resultVec4, final float[] v1, final float[] v2, final float[] v3,
+                                       final float[] temp1V3, final float[] temp2V3) {
+        /**
+            Ax + By + Cz + D == 0 ;
+            D = - ( Ax + By + Cz )
+              = - ( A*a[0] + B*a[1] + C*a[2] )
+              = - vec3Dot ( normal, a ) ;
+         */
+      getNormalVec3( resultVec4, v1, v2, v3, temp1V3, temp2V3 ) ;
+      resultVec4 [ 3 ] = -vec3Dot (resultVec4, v1) ;
+      return resultVec4;
+    }
+
+    /**
+     * Return intersection of an infinite line with a plane if exists, otherwise null.
+     * <p>
+     * Thanks to <i>Norman Vine -- [email protected]  (with hacks by Steve)</i>
+     * </p>
+     *
+     * @param result vec3 result buffer for intersecting coords
+     * @param ray here representing an infinite line, origin and direction.
+     * @param plane vec4 plane equation
+     * @param epsilon
+     * @return resulting intersecting if exists, otherwise null
+     */
+    public static float[] line2PlaneIntersection(final float[] result, final Ray ray, float[/*4*/] plane, final float epsilon) {
+        final float tmp = vec3Dot(ray.dir, plane) ;
+
+        if ( Math.abs(tmp) < epsilon ) {
+            return null; // ray is parallel to plane
+        }
+        scaleVec3 ( result, ray.dir, -( vec3Dot(ray.orig, plane) + plane[3] ) / tmp ) ;
+        return addVec3(result, result, ray.orig);
+    }
 
     /** Compute intersection between two segments
      * @param a vertex 1 of first segment
diff --git a/src/jogl/classes/com/jogamp/opengl/math/geom/AABBox.java b/src/jogl/classes/com/jogamp/opengl/math/geom/AABBox.java
index 8a0bf37a0..6f1384c28 100644
--- a/src/jogl/classes/com/jogamp/opengl/math/geom/AABBox.java
+++ b/src/jogl/classes/com/jogamp/opengl/math/geom/AABBox.java
@@ -432,6 +432,135 @@ public class AABBox {
         return true;
     }
 
+    /**
+     * Return intersection of a {@link Ray} with this bounding box,
+     * or null if none exist.
+     * <p>
+     * <ul>
+     *  <li>Original code by Andrew Woo, from "Graphics Gems", Academic Press, 1990 [2]</li>
+     *  <li>Optimized code by Pierre Terdiman, 2000 (~20-30% faster on my Celeron 500)</li>
+     *  <li>Epsilon value added by Klaus Hartmann.</li>
+     * </ul>
+     * Method is based on the requirements:
+     * <ul>
+     *  <li>the integer representation of 0.0f is 0x00000000</li>
+     *  <li>the sign bit of the float is the most significant one</li>
+     * </ul>
+     * Report bugs: [email protected]
+     * </p>
+     * <pre>
+     * [1] http://www.codercorner.com/RayAABB.cpp
+     * [2] http://tog.acm.org/resources/GraphicsGems/gems/RayBox.c
+     * </pre>
+     * @param result vec3
+     * @param ray
+     * @param epsilon
+     * @param assumeIntersection if true, method assumes an intersection, i.e. by pre-checking via {@link #intersectsRay(Ray)}.
+     *                           In this case method will not validate a possible non-intersection and just computes
+     *                           coordinates.
+     * @param tmp1V3 temp vec3
+     * @param tmp2V3 temp vec3
+     * @param tmp3V3 temp vec3
+     * @return float[3] result of intersection coordinates, or null if none exists
+     */
+    public final float[] getRayIntersection(final float[] result, final Ray ray, final float epsilon,
+                                            final boolean assumeIntersection,
+                                            final float[] tmp1V3, final float[] tmp2V3, final float[] tmp3V3) {
+        final float[] maxT = { -1f, -1f, -1f };
+
+        final float[] origin = ray.orig;
+        final float[] dir = ray.dir;
+
+        boolean inside = true;
+
+        // Find candidate planes.
+        for(int i=0; i<3; i++) {
+            if(origin[i] < low[i]) {
+                result[i] = low[i];
+                inside    = false;
+
+                // Calculate T distances to candidate planes
+                if( 0 != Float.floatToIntBits(dir[i]) ) {
+                    maxT[i] = (low[i] - origin[i]) / dir[i];
+                }
+            } else if(origin[i] > high[i]) {
+                result[i] = high[i];
+                inside    = false;
+
+                // Calculate T distances to candidate planes
+                if( 0 != Float.floatToIntBits(dir[i]) ) {
+                    maxT[i] = (high[i] - origin[i]) / dir[i];
+                }
+            }
+        }
+
+        // Ray origin inside bounding box
+        if(inside) {
+            System.arraycopy(origin, 0, result, 0, 3);
+            return result;
+        }
+
+        // Get largest of the maxT's for final choice of intersection
+        int whichPlane = 0;
+        if(maxT[1] > maxT[whichPlane]) { whichPlane = 1; }
+        if(maxT[2] > maxT[whichPlane]) { whichPlane = 2; }
+
+        if( !assumeIntersection ) {
+            // Check final candidate actually inside box
+            if( 0 != ( Float.floatToIntBits(maxT[whichPlane]) & 0x80000000 ) ) {
+                return null;
+            }
+
+            /** Use unrolled version below ..
+            for(int i=0; i<3; i++) {
+                if( i!=whichPlane ) {
+                    result[i] = origin[i] + maxT[whichPlane] * dir[i];
+                    if(result[i] < minB[i] - epsilon || result[i] > maxB[i] + epsilon) { return null; }
+                    // if(result[i] < minB[i] || result[i] > maxB[i] ) { return null; }
+                }
+            } */
+            switch( whichPlane ) {
+                case 0:
+                    result[1] = origin[1] + maxT[whichPlane] * dir[1];
+                    if(result[1] < low[1] - epsilon || result[1] > high[1] + epsilon) { return null; }
+                    result[2] = origin[2] + maxT[whichPlane] * dir[2];
+                    if(result[2] < low[2] - epsilon || result[2] > high[2] + epsilon) { return null; }
+                    break;
+                case 1:
+                    result[0] = origin[0] + maxT[whichPlane] * dir[0];
+                    if(result[0] < low[0] - epsilon || result[0] > high[0] + epsilon) { return null; }
+                    result[2] = origin[2] + maxT[whichPlane] * dir[2];
+                    if(result[2] < low[2] - epsilon || result[2] > high[2] + epsilon) { return null; }
+                    break;
+                case 2:
+                    result[0] = origin[0] + maxT[whichPlane] * dir[0];
+                    if(result[0] < low[0] - epsilon || result[0] > high[0] + epsilon) { return null; }
+                    result[1] = origin[1] + maxT[whichPlane] * dir[1];
+                    if(result[1] < low[1] - epsilon || result[1] > high[1] + epsilon) { return null; }
+                    break;
+                default:
+                    throw new InternalError("XXX");
+            }
+        } else {
+            switch( whichPlane ) {
+                case 0:
+                    result[1] = origin[1] + maxT[whichPlane] * dir[1];
+                    result[2] = origin[2] + maxT[whichPlane] * dir[2];
+                    break;
+                case 1:
+                    result[0] = origin[0] + maxT[whichPlane] * dir[0];
+                    result[2] = origin[2] + maxT[whichPlane] * dir[2];
+                    break;
+                case 2:
+                    result[0] = origin[0] + maxT[whichPlane] * dir[0];
+                    result[1] = origin[1] + maxT[whichPlane] * dir[1];
+                    break;
+                default:
+                    throw new InternalError("XXX");
+            }
+        }
+        return result; // ray hits box
+    }
 
     /**
      * Get the size of this AABBox where the size is represented by the
diff --git a/src/jogl/classes/com/jogamp/opengl/util/PMVMatrix.java b/src/jogl/classes/com/jogamp/opengl/util/PMVMatrix.java
index 8f5beeebe..c6f1f529d 100644
--- a/src/jogl/classes/com/jogamp/opengl/util/PMVMatrix.java
+++ b/src/jogl/classes/com/jogamp/opengl/util/PMVMatrix.java
@@ -784,7 +784,7 @@ public class PMVMatrix implements GLMatrixFunc {
     /**
      * Map two window coordinates w/ shared X/Y and distinctive Z
      * to a {@link Ray}. The resulting {@link Ray} maybe used for <i>picking</i>
-     * using a {@link AABBox#intersectsRay(Ray, float[]) bounding box}.
+     * using a {@link AABBox#getRayIntersection(Ray, float[]) bounding box}.
      * <p>
      * Notes for picking <i>winz0</i> and <i>winz1</i>:
      * <ul>