/*
* Java port of Bullet (c) 2008 Martin Dvorak <jezek2@advel.cz>
*
* Bullet Continuous Collision Detection and Physics Library
* Copyright (c) 2003-2008 Erwin Coumans http://www.bulletphysics.com/
*
* This software is provided 'as-is', without any express or implied warranty.
* In no event will the authors be held liable for any damages arising from
* the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/
// Dbvt implementation by Nathanael Presson
package com.bulletphysics.collision.broadphase;
import com.bulletphysics.linearmath.MatrixUtil;
import com.bulletphysics.linearmath.Transform;
import com.bulletphysics.linearmath.VectorUtil;
import cz.advel.stack.Stack;
import javax.vecmath.Vector3f;
/**
*
* @author jezek2
*/
public class DbvtAabbMm {
private final Vector3f mi = new Vector3f();
private final Vector3f mx = new Vector3f();
public DbvtAabbMm() {
}
public DbvtAabbMm(DbvtAabbMm o) {
set(o);
}
public void set(DbvtAabbMm o) {
mi.set(o.mi);
mx.set(o.mx);
}
public static void swap(DbvtAabbMm p1, DbvtAabbMm p2) {
Vector3f tmp = Stack.alloc(Vector3f.class);
tmp.set(p1.mi);
p1.mi.set(p2.mi);
p2.mi.set(tmp);
tmp.set(p1.mx);
p1.mx.set(p2.mx);
p2.mx.set(tmp);
}
public Vector3f Center(Vector3f out) {
out.add(mi, mx);
out.scale(0.5f);
return out;
}
public Vector3f Lengths(Vector3f out) {
out.sub(mx, mi);
return out;
}
public Vector3f Extents(Vector3f out) {
out.sub(mx, mi);
out.scale(0.5f);
return out;
}
public Vector3f Mins() {
return mi;
}
public Vector3f Maxs() {
return mx;
}
public static DbvtAabbMm FromCE(Vector3f c, Vector3f e, DbvtAabbMm out) {
DbvtAabbMm box = out;
box.mi.sub(c, e);
box.mx.add(c, e);
return box;
}
public static DbvtAabbMm FromCR(Vector3f c, float r, DbvtAabbMm out) {
Vector3f tmp = Stack.alloc(Vector3f.class);
tmp.set(r, r, r);
return FromCE(c, tmp, out);
}
public static DbvtAabbMm FromMM(Vector3f mi, Vector3f mx, DbvtAabbMm out) {
DbvtAabbMm box = out;
box.mi.set(mi);
box.mx.set(mx);
return box;
}
//public static DbvtAabbMm FromPoints( btVector3* pts,int n);
//public static DbvtAabbMm FromPoints( btVector3** ppts,int n);
public void Expand(Vector3f e) {
mi.sub(e);
mx.add(e);
}
public void SignedExpand(Vector3f e) {
if (e.x > 0) {
mx.x += e.x;
}
else {
mi.x += e.x;
}
if (e.y > 0) {
mx.y += e.y;
}
else {
mi.y += e.y;
}
if (e.z > 0) {
mx.z += e.z;
}
else {
mi.z += e.z;
}
}
public boolean Contain(DbvtAabbMm a) {
return ((mi.x <= a.mi.x) &&
(mi.y <= a.mi.y) &&
(mi.z <= a.mi.z) &&
(mx.x >= a.mx.x) &&
(mx.y >= a.mx.y) &&
(mx.z >= a.mx.z));
}
public int Classify(Vector3f n, float o, int s) {
Vector3f pi = Stack.alloc(Vector3f.class);
Vector3f px = Stack.alloc(Vector3f.class);
switch (s) {
case (0 + 0 + 0):
px.set(mi.x, mi.y, mi.z);
pi.set(mx.x, mx.y, mx.z);
break;
case (1 + 0 + 0):
px.set(mx.x, mi.y, mi.z);
pi.set(mi.x, mx.y, mx.z);
break;
case (0 + 2 + 0):
px.set(mi.x, mx.y, mi.z);
pi.set(mx.x, mi.y, mx.z);
break;
case (1 + 2 + 0):
px.set(mx.x, mx.y, mi.z);
pi.set(mi.x, mi.y, mx.z);
break;
case (0 + 0 + 4):
px.set(mi.x, mi.y, mx.z);
pi.set(mx.x, mx.y, mi.z);
break;
case (1 + 0 + 4):
px.set(mx.x, mi.y, mx.z);
pi.set(mi.x, mx.y, mi.z);
break;
case (0 + 2 + 4):
px.set(mi.x, mx.y, mx.z);
pi.set(mx.x, mi.y, mi.z);
break;
case (1 + 2 + 4):
px.set(mx.x, mx.y, mx.z);
pi.set(mi.x, mi.y, mi.z);
break;
}
if ((n.dot(px) + o) < 0) {
return -1;
}
if ((n.dot(pi) + o) >= 0) {
return +1;
}
return 0;
}
public float ProjectMinimum(Vector3f v, int signs) {
Vector3f[] b = new Vector3f[] { mx, mi };
Vector3f p = Stack.alloc(Vector3f.class);
p.set(b[(signs >> 0) & 1].x,
b[(signs >> 1) & 1].y,
b[(signs >> 2) & 1].z);
return p.dot(v);
}
public static boolean Intersect(DbvtAabbMm a, DbvtAabbMm b) {
return ((a.mi.x <= b.mx.x) &&
(a.mx.x >= b.mi.x) &&
(a.mi.y <= b.mx.y) &&
(a.mx.y >= b.mi.y) &&
(a.mi.z <= b.mx.z) &&
(a.mx.z >= b.mi.z));
}
public static boolean Intersect(DbvtAabbMm a, DbvtAabbMm b, Transform xform) {
Vector3f d0 = Stack.alloc(Vector3f.class);
Vector3f d1 = Stack.alloc(Vector3f.class);
Vector3f tmp = Stack.alloc(Vector3f.class);
// JAVA NOTE: check
b.Center(d0);
xform.transform(d0);
d0.sub(a.Center(tmp));
MatrixUtil.transposeTransform(d1, d0, xform.basis);
float[] s0 = new float[] { 0, 0 };
float[] s1 = new float[2];
s1[0] = xform.origin.dot(d0);
s1[1] = s1[0];
a.AddSpan(d0, s0, 0, s0, 1);
b.AddSpan(d1, s1, 0, s1, 1);
if (s0[0] > (s1[1])) {
return false;
}
if (s0[1] < (s1[0])) {
return false;
}
return true;
}
public static boolean Intersect(DbvtAabbMm a, Vector3f b) {
return ((b.x >= a.mi.x) &&
(b.y >= a.mi.y) &&
(b.z >= a.mi.z) &&
(b.x <= a.mx.x) &&
(b.y <= a.mx.y) &&
(b.z <= a.mx.z));
}
public static boolean Intersect(DbvtAabbMm a, Vector3f org, Vector3f invdir, int[] signs) {
Vector3f[] bounds = new Vector3f[]{a.mi, a.mx};
float txmin = (bounds[signs[0]].x - org.x) * invdir.x;
float txmax = (bounds[1 - signs[0]].x - org.x) * invdir.x;
float tymin = (bounds[signs[1]].y - org.y) * invdir.y;
float tymax = (bounds[1 - signs[1]].y - org.y) * invdir.y;
if ((txmin > tymax) || (tymin > txmax)) {
return false;
}
if (tymin > txmin) {
txmin = tymin;
}
if (tymax < txmax) {
txmax = tymax;
}
float tzmin = (bounds[signs[2]].z - org.z) * invdir.z;
float tzmax = (bounds[1 - signs[2]].z - org.z) * invdir.z;
if ((txmin > tzmax) || (tzmin > txmax)) {
return false;
}
if (tzmin > txmin) {
txmin = tzmin;
}
if (tzmax < txmax) {
txmax = tzmax;
}
return (txmax > 0);
}
public static float Proximity(DbvtAabbMm a, DbvtAabbMm b) {
Vector3f d = Stack.alloc(Vector3f.class);
Vector3f tmp = Stack.alloc(Vector3f.class);
d.add(a.mi, a.mx);
tmp.add(b.mi, b.mx);
d.sub(tmp);
return Math.abs(d.x) + Math.abs(d.y) + Math.abs(d.z);
}
public static void Merge(DbvtAabbMm a, DbvtAabbMm b, DbvtAabbMm r) {
for (int i=0; i<3; i++) {
if (VectorUtil.getCoord(a.mi, i) < VectorUtil.getCoord(b.mi, i)) {
VectorUtil.setCoord(r.mi, i, VectorUtil.getCoord(a.mi, i));
}
else {
VectorUtil.setCoord(r.mi, i, VectorUtil.getCoord(b.mi, i));
}
if (VectorUtil.getCoord(a.mx, i) > VectorUtil.getCoord(b.mx, i)) {
VectorUtil.setCoord(r.mx, i, VectorUtil.getCoord(a.mx, i));
}
else {
VectorUtil.setCoord(r.mx, i, VectorUtil.getCoord(b.mx, i));
}
}
}
public static boolean NotEqual(DbvtAabbMm a, DbvtAabbMm b) {
return ((a.mi.x != b.mi.x) ||
(a.mi.y != b.mi.y) ||
(a.mi.z != b.mi.z) ||
(a.mx.x != b.mx.x) ||
(a.mx.y != b.mx.y) ||
(a.mx.z != b.mx.z));
}
private void AddSpan(Vector3f d, float[] smi, int smi_idx, float[] smx, int smx_idx) {
for (int i=0; i<3; i++) {
if (VectorUtil.getCoord(d, i) < 0) {
smi[smi_idx] += VectorUtil.getCoord(mx, i) * VectorUtil.getCoord(d, i);
smx[smx_idx] += VectorUtil.getCoord(mi, i) * VectorUtil.getCoord(d, i);
}
else {
smi[smi_idx] += VectorUtil.getCoord(mi, i) * VectorUtil.getCoord(d, i);
smx[smx_idx] += VectorUtil.getCoord(mx, i) * VectorUtil.getCoord(d, i);
}
}
}
}