BIHTree.java revision 59b2e6871c65f58fdad78cd7229c292f6a177578 
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.bounding.BoundingSphere;
36import com.jme3.bounding.BoundingVolume;
37import com.jme3.collision.Collidable;
38import com.jme3.collision.CollisionResults;
39import com.jme3.collision.UnsupportedCollisionException;
40import com.jme3.export.InputCapsule;
41import com.jme3.export.JmeExporter;
42import com.jme3.export.JmeImporter;
43import com.jme3.export.OutputCapsule;
44import com.jme3.math.FastMath;
45import com.jme3.math.Matrix4f;
46import com.jme3.math.Ray;
47import com.jme3.math.Vector3f;
48import com.jme3.scene.CollisionData;
49import com.jme3.scene.Mesh;
50import com.jme3.scene.Mesh.Mode;
51import com.jme3.scene.VertexBuffer.Type;
52import com.jme3.scene.mesh.IndexBuffer;
53import com.jme3.scene.mesh.VirtualIndexBuffer;
54import com.jme3.scene.mesh.WrappedIndexBuffer;
55import com.jme3.util.TempVars;
56import java.io.IOException;
57import static java.lang.Math.max;
58import java.nio.FloatBuffer;
59
60public class BIHTree implements CollisionData {
61
62    public static final int MAX_TREE_DEPTH = 100;
63    public static final int MAX_TRIS_PER_NODE = 21;
64    private Mesh mesh;
65    private BIHNode root;
66    private int maxTrisPerNode;
67    private int numTris;
68    private float[] pointData;
69    private int[] triIndices;
70
71    private transient CollisionResults boundResults = new CollisionResults();
72    private transient float[] bihSwapTmp;
73
74    private static final TriangleAxisComparator[] comparators = new TriangleAxisComparator[]
75    {
76        new TriangleAxisComparator(0),
77        new TriangleAxisComparator(1),
78        new TriangleAxisComparator(2)
79    };
80
81    private void initTriList(FloatBuffer vb, IndexBuffer ib) {
82        pointData = new float[numTris * 3 * 3];
83        int p = 0;
84        for (int i = 0; i < numTris * 3; i += 3) {
85            int vert = ib.get(i) * 3;
86            pointData[p++] = vb.get(vert++);
87            pointData[p++] = vb.get(vert++);
88            pointData[p++] = vb.get(vert);
89
90            vert = ib.get(i + 1) * 3;
91            pointData[p++] = vb.get(vert++);
92            pointData[p++] = vb.get(vert++);
93            pointData[p++] = vb.get(vert);
94
95            vert = ib.get(i + 2) * 3;
96            pointData[p++] = vb.get(vert++);
97            pointData[p++] = vb.get(vert++);
98            pointData[p++] = vb.get(vert);
99        }
100
101        triIndices = new int[numTris];
102        for (int i = 0; i < numTris; i++) {
103            triIndices[i] = i;
104        }
105    }
106
107    public BIHTree(Mesh mesh, int maxTrisPerNode) {
108        this.mesh = mesh;
109        this.maxTrisPerNode = maxTrisPerNode;
110
111        if (maxTrisPerNode < 1 || mesh == null) {
112            throw new IllegalArgumentException();
113        }
114
115        bihSwapTmp = new float[9];
116
117        FloatBuffer vb = (FloatBuffer) mesh.getBuffer(Type.Position).getData();
118        IndexBuffer ib = mesh.getIndexBuffer();
119        if (ib == null) {
120            ib = new VirtualIndexBuffer(mesh.getVertexCount(), mesh.getMode());
121        } else if (mesh.getMode() != Mode.Triangles) {
122            ib = new WrappedIndexBuffer(mesh);
123        }
124
125        numTris = ib.size() / 3;
126        initTriList(vb, ib);
127    }
128
129    public BIHTree(Mesh mesh) {
130        this(mesh, MAX_TRIS_PER_NODE);
131    }
132
133    public BIHTree() {
134    }
135
136    public void construct() {
137        BoundingBox sceneBbox = createBox(0, numTris - 1);
138        root = createNode(0, numTris - 1, sceneBbox, 0);
139    }
140
141    private BoundingBox createBox(int l, int r) {
142        TempVars vars = TempVars.get();
143
144        Vector3f min = vars.vect1.set(new Vector3f(Float.POSITIVE_INFINITY, Float.POSITIVE_INFINITY, Float.POSITIVE_INFINITY));
145        Vector3f max = vars.vect2.set(new Vector3f(Float.NEGATIVE_INFINITY, Float.NEGATIVE_INFINITY, Float.NEGATIVE_INFINITY));
146
147        Vector3f v1 = vars.vect3,
148                v2 = vars.vect4,
149                v3 = vars.vect5;
150
151        for (int i = l; i <= r; i++) {
152            getTriangle(i, v1, v2, v3);
153            BoundingBox.checkMinMax(min, max, v1);
154            BoundingBox.checkMinMax(min, max, v2);
155            BoundingBox.checkMinMax(min, max, v3);
156        }
157
158        BoundingBox bbox = new BoundingBox(min, max);
159        vars.release();
160        return bbox;
161    }
162
163    int getTriangleIndex(int triIndex) {
164        return triIndices[triIndex];
165    }
166
167    private int sortTriangles(int l, int r, float split, int axis) {
168        int pivot = l;
169        int j = r;
170
171        TempVars vars = TempVars.get();
172
173        Vector3f v1 = vars.vect1,
174                v2 = vars.vect2,
175                v3 = vars.vect3;
176
177        while (pivot <= j) {
178            getTriangle(pivot, v1, v2, v3);
179            v1.addLocal(v2).addLocal(v3).multLocal(FastMath.ONE_THIRD);
180            if (v1.get(axis) > split) {
181                swapTriangles(pivot, j);
182                --j;
183            } else {
184                ++pivot;
185            }
186        }
187
188        vars.release();
189        pivot = (pivot == l && j < pivot) ? j : pivot;
190        return pivot;
191    }
192
193    private void setMinMax(BoundingBox bbox, boolean doMin, int axis, float value) {
194        Vector3f min = bbox.getMin(null);
195        Vector3f max = bbox.getMax(null);
196
197        if (doMin) {
198            min.set(axis, value);
199        } else {
200            max.set(axis, value);
201        }
202
203        bbox.setMinMax(min, max);
204    }
205
206    private float getMinMax(BoundingBox bbox, boolean doMin, int axis) {
207        if (doMin) {
208            return bbox.getMin(null).get(axis);
209        } else {
210            return bbox.getMax(null).get(axis);
211        }
212    }
213
214//    private BIHNode createNode2(int l, int r, BoundingBox nodeBbox, int depth){
215//        if ((r - l) < maxTrisPerNode || depth > 100)
216//            return createLeaf(l, r);
217//
218//        BoundingBox currentBox = createBox(l, r);
219//        int axis = depth % 3;
220//        float split = currentBox.getCenter().get(axis);
221//
222//        TriangleAxisComparator comparator = comparators[axis];
223//        Arrays.sort(tris, l, r, comparator);
224//        int splitIndex = -1;
225//
226//        float leftPlane, rightPlane = Float.POSITIVE_INFINITY;
227//        leftPlane = tris[l].getExtreme(axis, false);
228//        for (int i = l; i <= r; i++){
229//            BIHTriangle tri = tris[i];
230//            if (splitIndex == -1){
231//                float v = tri.getCenter().get(axis);
232//                if (v > split){
233//                    if (i == 0){
234//                        // no left plane
235//                        splitIndex = -2;
236//                    }else{
237//                        splitIndex = i;
238//                        // first triangle assigned to right
239//                        rightPlane = tri.getExtreme(axis, true);
240//                    }
241//                }else{
242//                    // triangle assigned to left
243//                    float ex = tri.getExtreme(axis, false);
244//                    if (ex > leftPlane)
245//                        leftPlane = ex;
246//                }
247//            }else{
248//                float ex = tri.getExtreme(axis, true);
249//                if (ex < rightPlane)
250//                    rightPlane = ex;
251//            }
252//        }
253//
254//        if (splitIndex < 0){
255//            splitIndex = (r - l) / 2;
256//
257//            leftPlane = Float.NEGATIVE_INFINITY;
258//            rightPlane = Float.POSITIVE_INFINITY;
259//
260//            for (int i = l; i < splitIndex; i++){
261//                float ex = tris[i].getExtreme(axis, false);
262//                if (ex > leftPlane){
263//                    leftPlane = ex;
264//                }
265//            }
266//            for (int i = splitIndex; i <= r; i++){
267//                float ex = tris[i].getExtreme(axis, true);
268//                if (ex < rightPlane){
269//                    rightPlane = ex;
270//                }
271//            }
272//        }
273//
274//        BIHNode node = new BIHNode(axis);
275//        node.leftPlane = leftPlane;
276//        node.rightPlane = rightPlane;
277//
278//        node.leftIndex = l;
279//        node.rightIndex = r;
280//
281//        BoundingBox leftBbox = new BoundingBox(currentBox);
282//        setMinMax(leftBbox, false, axis, split);
283//        node.left = createNode2(l, splitIndex-1, leftBbox, depth+1);
284//
285//        BoundingBox rightBbox = new BoundingBox(currentBox);
286//        setMinMax(rightBbox, true, axis, split);
287//        node.right = createNode2(splitIndex, r, rightBbox, depth+1);
288//
289//        return node;
290//    }
291    private BIHNode createNode(int l, int r, BoundingBox nodeBbox, int depth) {
292        if ((r - l) < maxTrisPerNode || depth > MAX_TREE_DEPTH) {
293            return new BIHNode(l, r);
294        }
295
296        BoundingBox currentBox = createBox(l, r);
297
298        Vector3f exteriorExt = nodeBbox.getExtent(null);
299        Vector3f interiorExt = currentBox.getExtent(null);
300        exteriorExt.subtractLocal(interiorExt);
301
302        int axis = 0;
303        if (exteriorExt.x > exteriorExt.y) {
304            if (exteriorExt.x > exteriorExt.z) {
305                axis = 0;
306            } else {
307                axis = 2;
308            }
309        } else {
310            if (exteriorExt.y > exteriorExt.z) {
311                axis = 1;
312            } else {
313                axis = 2;
314            }
315        }
316        if (exteriorExt.equals(Vector3f.ZERO)) {
317            axis = 0;
318        }
319
320//        Arrays.sort(tris, l, r, comparators[axis]);
321        float split = currentBox.getCenter().get(axis);
322        int pivot = sortTriangles(l, r, split, axis);
323        if (pivot == l || pivot == r) {
324            pivot = (r + l) / 2;
325        }
326
327        //If one of the partitions is empty, continue with recursion: same level but different bbox
328        if (pivot < l) {
329            //Only right
330            BoundingBox rbbox = new BoundingBox(currentBox);
331            setMinMax(rbbox, true, axis, split);
332            return createNode(l, r, rbbox, depth + 1);
333        } else if (pivot > r) {
334            //Only left
335            BoundingBox lbbox = new BoundingBox(currentBox);
336            setMinMax(lbbox, false, axis, split);
337            return createNode(l, r, lbbox, depth + 1);
338        } else {
339            //Build the node
340            BIHNode node = new BIHNode(axis);
341
342            //Left child
343            BoundingBox lbbox = new BoundingBox(currentBox);
344            setMinMax(lbbox, false, axis, split);
345
346            //The left node right border is the plane most right
347            node.setLeftPlane(getMinMax(createBox(l, max(l, pivot - 1)), false, axis));
348            node.setLeftChild(createNode(l, max(l, pivot - 1), lbbox, depth + 1)); //Recursive call
349
350            //Right Child
351            BoundingBox rbbox = new BoundingBox(currentBox);
352            setMinMax(rbbox, true, axis, split);
353            //The right node left border is the plane most left
354            node.setRightPlane(getMinMax(createBox(pivot, r), true, axis));
355            node.setRightChild(createNode(pivot, r, rbbox, depth + 1)); //Recursive call
356
357            return node;
358        }
359    }
360
361    public void getTriangle(int index, Vector3f v1, Vector3f v2, Vector3f v3) {
362        int pointIndex = index * 9;
363
364        v1.x = pointData[pointIndex++];
365        v1.y = pointData[pointIndex++];
366        v1.z = pointData[pointIndex++];
367
368        v2.x = pointData[pointIndex++];
369        v2.y = pointData[pointIndex++];
370        v2.z = pointData[pointIndex++];
371
372        v3.x = pointData[pointIndex++];
373        v3.y = pointData[pointIndex++];
374        v3.z = pointData[pointIndex++];
375    }
376
377    public void swapTriangles(int index1, int index2) {
378        int p1 = index1 * 9;
379        int p2 = index2 * 9;
380
381        // store p1 in tmp
382        System.arraycopy(pointData, p1, bihSwapTmp, 0, 9);
383
384        // copy p2 to p1
385        System.arraycopy(pointData, p2, pointData, p1, 9);
386
387        // copy tmp to p2
388        System.arraycopy(bihSwapTmp, 0, pointData, p2, 9);
389
390        // swap indices
391        int tmp2 = triIndices[index1];
392        triIndices[index1] = triIndices[index2];
393        triIndices[index2] = tmp2;
394    }
395
396    private int collideWithRay(Ray r,
397            Matrix4f worldMatrix,
398            BoundingVolume worldBound,
399            CollisionResults results) {
400
401        boundResults.clear();
402        worldBound.collideWith(r, boundResults);
403        if (boundResults.size() > 0) {
404            float tMin = boundResults.getClosestCollision().getDistance();
405            float tMax = boundResults.getFarthestCollision().getDistance();
406
407            if (tMax <= 0) {
408                tMax = Float.POSITIVE_INFINITY;
409            } else if (tMin == tMax) {
410                tMin = 0;
411            }
412
413            if (tMin <= 0) {
414                tMin = 0;
415            }
416
417            if (r.getLimit() < Float.POSITIVE_INFINITY) {
418                tMax = Math.min(tMax, r.getLimit());
419                if (tMin > tMax){
420                    return 0;
421                }
422            }
423
424//            return root.intersectBrute(r, worldMatrix, this, tMin, tMax, results);
425            return root.intersectWhere(r, worldMatrix, this, tMin, tMax, results);
426        }
427        return 0;
428    }
429
430    private int collideWithBoundingVolume(BoundingVolume bv,
431            Matrix4f worldMatrix,
432            CollisionResults results) {
433        BoundingBox bbox;
434        if (bv instanceof BoundingSphere) {
435            BoundingSphere sphere = (BoundingSphere) bv;
436            bbox = new BoundingBox(bv.getCenter().clone(), sphere.getRadius(),
437                    sphere.getRadius(),
438                    sphere.getRadius());
439        } else if (bv instanceof BoundingBox) {
440            bbox = new BoundingBox((BoundingBox) bv);
441        } else {
442            throw new UnsupportedCollisionException();
443        }
444
445        bbox.transform(worldMatrix.invert(), bbox);
446        return root.intersectWhere(bv, bbox, worldMatrix, this, results);
447    }
448
449    public int collideWith(Collidable other,
450            Matrix4f worldMatrix,
451            BoundingVolume worldBound,
452            CollisionResults results) {
453
454        if (other instanceof Ray) {
455            Ray ray = (Ray) other;
456            return collideWithRay(ray, worldMatrix, worldBound, results);
457        } else if (other instanceof BoundingVolume) {
458            BoundingVolume bv = (BoundingVolume) other;
459            return collideWithBoundingVolume(bv, worldMatrix, results);
460        } else {
461            throw new UnsupportedCollisionException();
462        }
463    }
464
465    public void write(JmeExporter ex) throws IOException {
466        OutputCapsule oc = ex.getCapsule(this);
467        oc.write(mesh, "mesh", null);
468        oc.write(root, "root", null);
469        oc.write(maxTrisPerNode, "tris_per_node", 0);
470        oc.write(pointData, "points", null);
471        oc.write(triIndices, "indices", null);
472    }
473
474    public void read(JmeImporter im) throws IOException {
475        InputCapsule ic = im.getCapsule(this);
476        mesh = (Mesh) ic.readSavable("mesh", null);
477        root = (BIHNode) ic.readSavable("root", null);
478        maxTrisPerNode = ic.readInt("tris_per_node", 0);
479        pointData = ic.readFloatArray("points", null);
480        triIndices = ic.readIntArray("indices", null);
481    }
482}
483