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.collision.Collidable; 36 import com.jme3.collision.CollisionResult; 37 import com.jme3.collision.CollisionResults; 38 import com.jme3.export.*; 39 import com.jme3.math.Matrix4f; 40 import com.jme3.math.Ray; 41 import com.jme3.math.Triangle; 42 import com.jme3.math.Vector3f; 43 import com.jme3.util.TempVars; 44 import java.io.IOException; 45 import static java.lang.Math.max; 46 import static java.lang.Math.min; 47 import 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 */ 56 public 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 BIHNode(int l, int r)69 public BIHNode(int l, int r) { 70 leftIndex = l; 71 rightIndex = r; 72 axis = 3; // indicates leaf 73 } 74 BIHNode(int axis)75 public BIHNode(int axis) { 76 this.axis = axis; 77 } 78 BIHNode()79 public BIHNode() { 80 } 81 getLeftChild()82 public BIHNode getLeftChild() { 83 return left; 84 } 85 setLeftChild(BIHNode left)86 public void setLeftChild(BIHNode left) { 87 this.left = left; 88 } 89 getLeftPlane()90 public float getLeftPlane() { 91 return leftPlane; 92 } 93 setLeftPlane(float leftPlane)94 public void setLeftPlane(float leftPlane) { 95 this.leftPlane = leftPlane; 96 } 97 getRightChild()98 public BIHNode getRightChild() { 99 return right; 100 } 101 setRightChild(BIHNode right)102 public void setRightChild(BIHNode right) { 103 this.right = right; 104 } 105 getRightPlane()106 public float getRightPlane() { 107 return rightPlane; 108 } 109 setRightPlane(float rightPlane)110 public void setRightPlane(float rightPlane) { 111 this.rightPlane = rightPlane; 112 } 113 write(JmeExporter ex)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 read(JmeImporter im)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 BIHStackData(BIHNode node, float min, float max)141 BIHStackData(BIHNode node, float min, float max) { 142 this.node = node; 143 this.min = min; 144 this.max = max; 145 } 146 } 147 intersectWhere(Collidable col, BoundingBox box, Matrix4f worldMatrix, BIHTree tree, CollisionResults results)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 intersectBrute(Ray r, Matrix4f worldMatrix, BIHTree tree, float sceneMin, float sceneMax, CollisionResults results)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 intersectWhere(Ray r, Matrix4f worldMatrix, BIHTree tree, float sceneMin, float sceneMax, CollisionResults results)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