1/*
2 * Copyright (c) 2009-2010 jMonkeyEngine
3 * All rights reserved.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are
7 * met:
8 *
9 * * Redistributions of source code must retain the above copyright
10 *   notice, this list of conditions and the following disclaimer.
11 *
12 * * Redistributions in binary form must reproduce the above copyright
13 *   notice, this list of conditions and the following disclaimer in the
14 *   documentation and/or other materials provided with the distribution.
15 *
16 * * Neither the name of 'jMonkeyEngine' nor the names of its contributors
17 *   may be used to endorse or promote products derived from this software
18 *   without specific prior written permission.
19 *
20 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
22 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
23 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
24 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
25 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
26 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
27 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
28 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
29 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
30 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31 */
32package com.jme3.collision.bih;
33
34import com.jme3.bounding.BoundingBox;
35import com.jme3.collision.Collidable;
36import com.jme3.collision.CollisionResult;
37import com.jme3.collision.CollisionResults;
38import com.jme3.export.*;
39import com.jme3.math.Matrix4f;
40import com.jme3.math.Ray;
41import com.jme3.math.Triangle;
42import com.jme3.math.Vector3f;
43import com.jme3.util.TempVars;
44import java.io.IOException;
45import static java.lang.Math.max;
46import static java.lang.Math.min;
47import java.util.ArrayList;
48
49/**
50 * Bounding Interval Hierarchy.
51 * Based on:
52 *
53 * Instant Ray Tracing: The Bounding Interval Hierarchy
54 * By Carsten Wächter and Alexander Keller
55 */
56public final class BIHNode implements Savable {
57
58    private int leftIndex, rightIndex;
59    private BIHNode left;
60    private BIHNode right;
61    private float leftPlane;
62    private float rightPlane;
63    private int axis;
64
65    //Do not do this: It increases memory usage of each BIHNode by at least 56 bytes!
66    //
67    //private Triangle tmpTriangle = new Triangle();
68
69    public BIHNode(int l, int r) {
70        leftIndex = l;
71        rightIndex = r;
72        axis = 3; // indicates leaf
73    }
74
75    public BIHNode(int axis) {
76        this.axis = axis;
77    }
78
79    public BIHNode() {
80    }
81
82    public BIHNode getLeftChild() {
83        return left;
84    }
85
86    public void setLeftChild(BIHNode left) {
87        this.left = left;
88    }
89
90    public float getLeftPlane() {
91        return leftPlane;
92    }
93
94    public void setLeftPlane(float leftPlane) {
95        this.leftPlane = leftPlane;
96    }
97
98    public BIHNode getRightChild() {
99        return right;
100    }
101
102    public void setRightChild(BIHNode right) {
103        this.right = right;
104    }
105
106    public float getRightPlane() {
107        return rightPlane;
108    }
109
110    public void setRightPlane(float rightPlane) {
111        this.rightPlane = rightPlane;
112    }
113
114    public void write(JmeExporter ex) throws IOException {
115        OutputCapsule oc = ex.getCapsule(this);
116        oc.write(leftIndex, "left_index", 0);
117        oc.write(rightIndex, "right_index", 0);
118        oc.write(leftPlane, "left_plane", 0);
119        oc.write(rightPlane, "right_plane", 0);
120        oc.write(axis, "axis", 0);
121        oc.write(left, "left_node", null);
122        oc.write(right, "right_node", null);
123    }
124
125    public void read(JmeImporter im) throws IOException {
126        InputCapsule ic = im.getCapsule(this);
127        leftIndex = ic.readInt("left_index", 0);
128        rightIndex = ic.readInt("right_index", 0);
129        leftPlane = ic.readFloat("left_plane", 0);
130        rightPlane = ic.readFloat("right_plane", 0);
131        axis = ic.readInt("axis", 0);
132        left = (BIHNode) ic.readSavable("left_node", null);
133        right = (BIHNode) ic.readSavable("right_node", null);
134    }
135
136    public static final class BIHStackData {
137
138        private final BIHNode node;
139        private final float min, max;
140
141        BIHStackData(BIHNode node, float min, float max) {
142            this.node = node;
143            this.min = min;
144            this.max = max;
145        }
146    }
147
148    public final int intersectWhere(Collidable col,
149            BoundingBox box,
150            Matrix4f worldMatrix,
151            BIHTree tree,
152            CollisionResults results) {
153
154        TempVars vars = TempVars.get();
155        ArrayList<BIHStackData> stack = vars.bihStack;
156        stack.clear();
157
158        float[] minExts = {box.getCenter().x - box.getXExtent(),
159            box.getCenter().y - box.getYExtent(),
160            box.getCenter().z - box.getZExtent()};
161
162        float[] maxExts = {box.getCenter().x + box.getXExtent(),
163            box.getCenter().y + box.getYExtent(),
164            box.getCenter().z + box.getZExtent()};
165
166        stack.add(new BIHStackData(this, 0, 0));
167
168        Triangle t = new Triangle();
169        int cols = 0;
170
171        stackloop:
172        while (stack.size() > 0) {
173            BIHNode node = stack.remove(stack.size() - 1).node;
174
175            while (node.axis != 3) {
176                int a = node.axis;
177
178                float maxExt = maxExts[a];
179                float minExt = minExts[a];
180
181                if (node.leftPlane < node.rightPlane) {
182                    // means there's a gap in the middle
183                    // if the box is in that gap, we stop there
184                    if (minExt > node.leftPlane
185                            && maxExt < node.rightPlane) {
186                        continue stackloop;
187                    }
188                }
189
190                if (maxExt < node.rightPlane) {
191                    node = node.left;
192                } else if (minExt > node.leftPlane) {
193                    node = node.right;
194                } else {
195                    stack.add(new BIHStackData(node.right, 0, 0));
196                    node = node.left;
197                }
198//                if (maxExt < node.leftPlane
199//                 && maxExt < node.rightPlane){
200//                    node = node.left;
201//                }else if (minExt > node.leftPlane
202//                       && minExt > node.rightPlane){
203//                    node = node.right;
204//                }else{
205
206//                }
207            }
208
209            for (int i = node.leftIndex; i <= node.rightIndex; i++) {
210                tree.getTriangle(i, t.get1(), t.get2(), t.get3());
211                if (worldMatrix != null) {
212                    worldMatrix.mult(t.get1(), t.get1());
213                    worldMatrix.mult(t.get2(), t.get2());
214                    worldMatrix.mult(t.get3(), t.get3());
215                }
216
217                int added = col.collideWith(t, results);
218
219                if (added > 0) {
220                    int index = tree.getTriangleIndex(i);
221                    int start = results.size() - added;
222
223                    for (int j = start; j < results.size(); j++) {
224                        CollisionResult cr = results.getCollisionDirect(j);
225                        cr.setTriangleIndex(index);
226                    }
227
228                    cols += added;
229                }
230            }
231        }
232        vars.release();
233        return cols;
234    }
235
236    public final int intersectBrute(Ray r,
237            Matrix4f worldMatrix,
238            BIHTree tree,
239            float sceneMin,
240            float sceneMax,
241            CollisionResults results) {
242        float tHit = Float.POSITIVE_INFINITY;
243
244        TempVars vars = TempVars.get();
245
246        Vector3f v1 = vars.vect1,
247                v2 = vars.vect2,
248                v3 = vars.vect3;
249
250        int cols = 0;
251
252        ArrayList<BIHStackData> stack = vars.bihStack;
253        stack.clear();
254        stack.add(new BIHStackData(this, 0, 0));
255        stackloop:
256        while (stack.size() > 0) {
257
258            BIHStackData data = stack.remove(stack.size() - 1);
259            BIHNode node = data.node;
260
261            leafloop:
262            while (node.axis != 3) { // while node is not a leaf
263                BIHNode nearNode, farNode;
264                nearNode = node.left;
265                farNode = node.right;
266
267                stack.add(new BIHStackData(farNode, 0, 0));
268                node = nearNode;
269            }
270
271            // a leaf
272            for (int i = node.leftIndex; i <= node.rightIndex; i++) {
273                tree.getTriangle(i, v1, v2, v3);
274
275                if (worldMatrix != null) {
276                    worldMatrix.mult(v1, v1);
277                    worldMatrix.mult(v2, v2);
278                    worldMatrix.mult(v3, v3);
279                }
280
281                float t = r.intersects(v1, v2, v3);
282                if (t < tHit) {
283                    tHit = t;
284                    Vector3f contactPoint = new Vector3f(r.direction).multLocal(tHit).addLocal(r.origin);
285                    CollisionResult cr = new CollisionResult(contactPoint, tHit);
286                    cr.setTriangleIndex(tree.getTriangleIndex(i));
287                    results.addCollision(cr);
288                    cols++;
289                }
290            }
291        }
292        vars.release();
293        return cols;
294    }
295
296    public final int intersectWhere(Ray r,
297            Matrix4f worldMatrix,
298            BIHTree tree,
299            float sceneMin,
300            float sceneMax,
301            CollisionResults results) {
302
303        TempVars vars = TempVars.get();
304        ArrayList<BIHStackData> stack = vars.bihStack;
305        stack.clear();
306
307//        float tHit = Float.POSITIVE_INFINITY;
308
309        Vector3f o = vars.vect1.set(r.getOrigin());
310        Vector3f d =  vars.vect2.set(r.getDirection());
311
312        Matrix4f inv =vars.tempMat4.set(worldMatrix).invertLocal();
313
314        inv.mult(r.getOrigin(), r.getOrigin());
315
316        // Fixes rotation collision bug
317        inv.multNormal(r.getDirection(), r.getDirection());
318//        inv.multNormalAcross(r.getDirection(), r.getDirection());
319
320        float[] origins = {r.getOrigin().x,
321            r.getOrigin().y,
322            r.getOrigin().z};
323
324        float[] invDirections = {1f / r.getDirection().x,
325            1f / r.getDirection().y,
326            1f / r.getDirection().z};
327
328        r.getDirection().normalizeLocal();
329
330        Vector3f v1 = vars.vect3,
331                v2 = vars.vect4,
332                v3 = vars.vect5;
333        int cols = 0;
334
335        stack.add(new BIHStackData(this, sceneMin, sceneMax));
336        stackloop:
337        while (stack.size() > 0) {
338
339            BIHStackData data = stack.remove(stack.size() - 1);
340            BIHNode node = data.node;
341            float tMin = data.min,
342                    tMax = data.max;
343
344            if (tMax < tMin) {
345                continue;
346            }
347
348            leafloop:
349            while (node.axis != 3) { // while node is not a leaf
350                int a = node.axis;
351
352                // find the origin and direction value for the given axis
353                float origin = origins[a];
354                float invDirection = invDirections[a];
355
356                float tNearSplit, tFarSplit;
357                BIHNode nearNode, farNode;
358
359                tNearSplit = (node.leftPlane - origin) * invDirection;
360                tFarSplit = (node.rightPlane - origin) * invDirection;
361                nearNode = node.left;
362                farNode = node.right;
363
364                if (invDirection < 0) {
365                    float tmpSplit = tNearSplit;
366                    tNearSplit = tFarSplit;
367                    tFarSplit = tmpSplit;
368
369                    BIHNode tmpNode = nearNode;
370                    nearNode = farNode;
371                    farNode = tmpNode;
372                }
373
374                if (tMin > tNearSplit && tMax < tFarSplit) {
375                    continue stackloop;
376                }
377
378                if (tMin > tNearSplit) {
379                    tMin = max(tMin, tFarSplit);
380                    node = farNode;
381                } else if (tMax < tFarSplit) {
382                    tMax = min(tMax, tNearSplit);
383                    node = nearNode;
384                } else {
385                    stack.add(new BIHStackData(farNode, max(tMin, tFarSplit), tMax));
386                    tMax = min(tMax, tNearSplit);
387                    node = nearNode;
388                }
389            }
390
391//            if ( (node.rightIndex - node.leftIndex) > minTrisPerNode){
392//                // on demand subdivision
393//                node.subdivide();
394//                stack.add(new BIHStackData(node, tMin, tMax));
395//                continue stackloop;
396//            }
397
398            // a leaf
399            for (int i = node.leftIndex; i <= node.rightIndex; i++) {
400                tree.getTriangle(i, v1, v2, v3);
401
402                float t = r.intersects(v1, v2, v3);
403                if (!Float.isInfinite(t)) {
404                    if (worldMatrix != null) {
405                        worldMatrix.mult(v1, v1);
406                        worldMatrix.mult(v2, v2);
407                        worldMatrix.mult(v3, v3);
408                        float t_world = new Ray(o, d).intersects(v1, v2, v3);
409                        t = t_world;
410                    }
411
412                    Vector3f contactNormal = Triangle.computeTriangleNormal(v1, v2, v3, null);
413                    Vector3f contactPoint = new Vector3f(d).multLocal(t).addLocal(o);
414                    float worldSpaceDist = o.distance(contactPoint);
415
416                    CollisionResult cr = new CollisionResult(contactPoint, worldSpaceDist);
417                    cr.setContactNormal(contactNormal);
418                    cr.setTriangleIndex(tree.getTriangleIndex(i));
419                    results.addCollision(cr);
420                    cols++;
421                }
422            }
423        }
424        vars.release();
425        r.setOrigin(o);
426        r.setDirection(d);
427
428        return cols;
429    }
430}
431