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());
// this is a no-no: allocating float arrays for immediate use? blarny!
/*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));
vars.addStackData(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, invDirection;
switch (a) {
default:
case 0: // x
origin = r.getOrigin().x;
invDirection = 1f / r.getDirection().x;
break;
case 1: // y
origin = r.getOrigin().y;
invDirection = 1f / r.getDirection().y;
break;
case 2: // z
origin = r.getOrigin().z;
invDirection = 1f / r.getDirection().z;
break;
}
// 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));
vars.addStackData(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);
vars.ray.setOrigin(o);
vars.ray.setDirection(d);
float t_world = vars.ray.intersects(v1, v2, v3);
t = t_world;
}
Vector3f contactPoint = vars.vect4.set(d).multLocal(t).addLocal(o);
float worldSpaceDist = o.distance(contactPoint);
// don't add the collision if it is longer than the ray length
if (worldSpaceDist <= r.limit) {
CollisionResult cr =
results.addReusedCollision(
contactPoint.x, contactPoint.y, contactPoint.z, worldSpaceDist);
if (cr.getContactNormal() == null) {
cr.setContactNormal(Triangle.computeTriangleNormal(v1, v2, v3, null));
} else {
Triangle.computeTriangleNormal(v1, v2, v3, cr.getContactNormal());
}
cr.setTriangleIndex(tree.getTriangleIndex(i));
cols++;
}
}
}
}
vars.release();
r.setOrigin(o);
r.setDirection(d);
return cols;
}
