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 Wchter 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 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
