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 */ 32 package com.jme3.collision.bih; 33 34 import com.jme3.bounding.BoundingBox; 35 import com.jme3.bounding.BoundingSphere; 36 import com.jme3.bounding.BoundingVolume; 37 import com.jme3.collision.Collidable; 38 import com.jme3.collision.CollisionResults; 39 import com.jme3.collision.UnsupportedCollisionException; 40 import com.jme3.export.InputCapsule; 41 import com.jme3.export.JmeExporter; 42 import com.jme3.export.JmeImporter; 43 import com.jme3.export.OutputCapsule; 44 import com.jme3.math.FastMath; 45 import com.jme3.math.Matrix4f; 46 import com.jme3.math.Ray; 47 import com.jme3.math.Vector3f; 48 import com.jme3.scene.CollisionData; 49 import com.jme3.scene.Mesh; 50 import com.jme3.scene.Mesh.Mode; 51 import com.jme3.scene.VertexBuffer.Type; 52 import com.jme3.scene.mesh.IndexBuffer; 53 import com.jme3.scene.mesh.VirtualIndexBuffer; 54 import com.jme3.scene.mesh.WrappedIndexBuffer; 55 import com.jme3.util.TempVars; 56 import java.io.IOException; 57 import static java.lang.Math.max; 58 import java.nio.FloatBuffer; 59 60 public 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
