/*
|
* 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 implements java.io.Serializable {
|
|
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);
|
}
|
}
|
}
|
|
}
|
