/*
* Copyright (c) 2009-2012 jMonkeyEngine
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are
* met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* * Neither the name of 'jMonkeyEngine' nor the names of its contributors
* may be used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
* TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
* PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
* LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
* NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package com.jme3.collision.bih;
import com.jme3.bounding.BoundingBox;
import com.jme3.collision.Collidable;
import com.jme3.collision.CollisionResult;
import com.jme3.collision.CollisionResults;
import com.jme3.export.*;
import com.jme3.math.Matrix4f;
import com.jme3.math.Ray;
import com.jme3.math.Triangle;
import com.jme3.math.Vector3f;
import com.jme3.util.TempVars;
import java.io.IOException;
import static java.lang.Math.max;
import static java.lang.Math.min;
import java.util.ArrayList;
/**
* Bounding Interval Hierarchy.
* Based on:
*
* Instant Ray Tracing: The Bounding Interval Hierarchy
* By Carsten Wächter and Alexander Keller
*/
public final class BIHNode implements Savable {
private int leftIndex, rightIndex;
private BIHNode left;
private BIHNode right;
private float leftPlane;
private float rightPlane;
private int axis;
//Do not do this: It increases memory usage of each BIHNode by at least 56 bytes!
//
//private Triangle tmpTriangle = new Triangle();
public BIHNode(int l, int r) {
leftIndex = l;
rightIndex = r;
axis = 3; // indicates leaf
}
public BIHNode(int axis) {
this.axis = axis;
}
public BIHNode() {
}
public BIHNode getLeftChild() {
return left;
}
public void setLeftChild(BIHNode left) {
this.left = left;
}
public float getLeftPlane() {
return leftPlane;
}
public void setLeftPlane(float leftPlane) {
this.leftPlane = leftPlane;
}
public BIHNode getRightChild() {
return right;
}
public void setRightChild(BIHNode right) {
this.right = right;
}
public float getRightPlane() {
return rightPlane;
}
public void setRightPlane(float rightPlane) {
this.rightPlane = rightPlane;
}
public void write(JmeExporter ex) throws IOException {
OutputCapsule oc = ex.getCapsule(this);
oc.write(leftIndex, "left_index", 0);
oc.write(rightIndex, "right_index", 0);
oc.write(leftPlane, "left_plane", 0);
oc.write(rightPlane, "right_plane", 0);
oc.write(axis, "axis", 0);
oc.write(left, "left_node", null);
oc.write(right, "right_node", null);
}
public void read(JmeImporter im) throws IOException {
InputCapsule ic = im.getCapsule(this);
leftIndex = ic.readInt("left_index", 0);
rightIndex = ic.readInt("right_index", 0);
leftPlane = ic.readFloat("left_plane", 0);
rightPlane = ic.readFloat("right_plane", 0);
axis = ic.readInt("axis", 0);
left = (BIHNode) ic.readSavable("left_node", null);
right = (BIHNode) ic.readSavable("right_node", null);
}
public static final class BIHStackData {
private final BIHNode node;
private final float min, max;
BIHStackData(BIHNode node, float min, float max) {
this.node = node;
this.min = min;
this.max = max;
}
}
public final int intersectWhere(Collidable col,
BoundingBox box,
Matrix4f worldMatrix,
BIHTree tree,
CollisionResults results) {
TempVars vars = TempVars.get();
ArrayList<BIHStackData> stack = vars.bihStack;
stack.clear();
float[] minExts = {box.getCenter().x - box.getXExtent(),
box.getCenter().y - box.getYExtent(),
box.getCenter().z - box.getZExtent()};
float[] maxExts = {box.getCenter().x + box.getXExtent(),
box.getCenter().y + box.getYExtent(),
box.getCenter().z + box.getZExtent()};
stack.add(new BIHStackData(this, 0, 0));
Triangle t = new Triangle();
int cols = 0;
stackloop:
while (stack.size() > 0) {
BIHNode node = stack.remove(stack.size() - 1).node;
while (node.axis != 3) {
int a = node.axis;
float maxExt = maxExts[a];
float minExt = minExts[a];
if (node.leftPlane < node.rightPlane) {
// means there's a gap in the middle
// if the box is in that gap, we stop there
if (minExt > node.leftPlane
&& maxExt < node.rightPlane) {
continue stackloop;
}
}
if (maxExt < node.rightPlane) {
node = node.left;
} else if (minExt > node.leftPlane) {
node = node.right;
} else {
stack.add(new BIHStackData(node.right, 0, 0));
node = node.left;
}
// if (maxExt < node.leftPlane
// && maxExt < node.rightPlane){
// node = node.left;
// }else if (minExt > node.leftPlane
// && minExt > node.rightPlane){
// node = node.right;
// }else{
// }
}
for (int i = node.leftIndex; i <= node.rightIndex; i++) {
tree.getTriangle(i, t.get1(), t.get2(), t.get3());
if (worldMatrix != null) {
worldMatrix.mult(t.get1(), t.get1());
worldMatrix.mult(t.get2(), t.get2());
worldMatrix.mult(t.get3(), t.get3());
}
int added = col.collideWith(t, results);
if (added > 0) {
int index = tree.getTriangleIndex(i);
int start = results.size() - added;
for (int j = start; j < results.size(); j++) {
CollisionResult cr = results.getCollisionDirect(j);
cr.setTriangleIndex(index);
}
cols += added;
}
}
}
vars.release();
return cols;
}
public final int intersectBrute(Ray r,
Matrix4f worldMatrix,
BIHTree tree,
float sceneMin,
float sceneMax,
CollisionResults results) {
float tHit = Float.POSITIVE_INFINITY;
TempVars vars = TempVars.get();
Vector3f v1 = vars.vect1,
v2 = vars.vect2,
v3 = vars.vect3;
int cols = 0;
ArrayList<BIHStackData> stack = vars.bihStack;
stack.clear();
stack.add(new BIHStackData(this, 0, 0));
stackloop:
while (stack.size() > 0) {
BIHStackData data = stack.remove(stack.size() - 1);
BIHNode node = data.node;
leafloop:
while (node.axis != 3) { // while node is not a leaf
BIHNode nearNode, farNode;
nearNode = node.left;
farNode = node.right;
stack.add(new BIHStackData(farNode, 0, 0));
node = nearNode;
}
// a leaf
for (int i = node.leftIndex; i <= node.rightIndex; i++) {
tree.getTriangle(i, v1, v2, v3);
if (worldMatrix != null) {
worldMatrix.mult(v1, v1);
worldMatrix.mult(v2, v2);
worldMatrix.mult(v3, v3);
}
float t = r.intersects(v1, v2, v3);
if (t < tHit) {
tHit = t;
Vector3f contactPoint = new Vector3f(r.direction).multLocal(tHit).addLocal(r.origin);
CollisionResult cr = new CollisionResult(contactPoint, tHit);
cr.setTriangleIndex(tree.getTriangleIndex(i));
results.addCollision(cr);
cols++;
}
}
}
vars.release();
return cols;
}
public final int intersectWhere(Ray r,
Matrix4f worldMatrix,
BIHTree tree,
float sceneMin,
float sceneMax,
CollisionResults results) {
TempVars vars = TempVars.get();
ArrayList<BIHStackData> stack = vars.bihStack;
stack.clear();
// float tHit = Float.POSITIVE_INFINITY;
Vector3f o = vars.vect1.set(r.getOrigin());
Vector3f d = vars.vect2.set(r.getDirection());
Matrix4f inv =vars.tempMat4.set(worldMatrix).invertLocal();
inv.mult(r.getOrigin(), r.getOrigin());
// Fixes rotation collision bug
inv.multNormal(r.getDirection(), r.getDirection());
// inv.multNormalAcross(r.getDirection(), r.getDirection());
float[] origins = {r.getOrigin().x,
r.getOrigin().y,
r.getOrigin().z};
float[] invDirections = {1f / r.getDirection().x,
1f / r.getDirection().y,
1f / r.getDirection().z};
r.getDirection().normalizeLocal();
Vector3f v1 = vars.vect3,
v2 = vars.vect4,
v3 = vars.vect5;
int cols = 0;
stack.add(new BIHStackData(this, sceneMin, sceneMax));
stackloop:
while (stack.size() > 0) {
BIHStackData data = stack.remove(stack.size() - 1);
BIHNode node = data.node;
float tMin = data.min,
tMax = data.max;
if (tMax < tMin) {
continue;
}
leafloop:
while (node.axis != 3) { // while node is not a leaf
int a = node.axis;
// find the origin and direction value for the given axis
float origin = origins[a];
float invDirection = invDirections[a];
float tNearSplit, tFarSplit;
BIHNode nearNode, farNode;
tNearSplit = (node.leftPlane - origin) * invDirection;
tFarSplit = (node.rightPlane - origin) * invDirection;
nearNode = node.left;
farNode = node.right;
if (invDirection < 0) {
float tmpSplit = tNearSplit;
tNearSplit = tFarSplit;
tFarSplit = tmpSplit;
BIHNode tmpNode = nearNode;
nearNode = farNode;
farNode = tmpNode;
}
if (tMin > tNearSplit && tMax < tFarSplit) {
continue stackloop;
}
if (tMin > tNearSplit) {
tMin = max(tMin, tFarSplit);
node = farNode;
} else if (tMax < tFarSplit) {
tMax = min(tMax, tNearSplit);
node = nearNode;
} else {
stack.add(new BIHStackData(farNode, max(tMin, tFarSplit), tMax));
tMax = min(tMax, tNearSplit);
node = nearNode;
}
}
// if ( (node.rightIndex - node.leftIndex) > minTrisPerNode){
// // on demand subdivision
// node.subdivide();
// stack.add(new BIHStackData(node, tMin, tMax));
// continue stackloop;
// }
// a leaf
for (int i = node.leftIndex; i <= node.rightIndex; i++) {
tree.getTriangle(i, v1, v2, v3);
float t = r.intersects(v1, v2, v3);
if (!Float.isInfinite(t)) {
if (worldMatrix != null) {
worldMatrix.mult(v1, v1);
worldMatrix.mult(v2, v2);
worldMatrix.mult(v3, v3);
float t_world = new Ray(o, d).intersects(v1, v2, v3);
t = t_world;
}
Vector3f contactNormal = Triangle.computeTriangleNormal(v1, v2, v3, null);
Vector3f contactPoint = new Vector3f(d).multLocal(t).addLocal(o);
float worldSpaceDist = o.distance(contactPoint);
CollisionResult cr = new CollisionResult(contactPoint, worldSpaceDist);
cr.setContactNormal(contactNormal);
cr.setTriangleIndex(tree.getTriangleIndex(i));
results.addCollision(cr);
cols++;
}
}
}
vars.release();
r.setOrigin(o);
r.setDirection(d);
return cols;
}
}