@Override
public void getAabb(Transform t, Vector3 aabbMin, Vector3 aabbMax) {
Stack stack = Stack.enter();
Vector3 tmp = stack.allocVector3();
Vector3 halfExtents = stack.allocVector3();
halfExtents.set(getRadius(), getRadius(), getRadius());
VectorUtil.setCoord(halfExtents, upAxis, getRadius() + getHalfHeight());
halfExtents.x += getMargin();
halfExtents.y += getMargin();
halfExtents.z += getMargin();
Matrix3 abs_b = stack.allocMatrix3();
abs_b.set(t.basis);
MatrixUtil.absolute(abs_b);
Vector3 center = t.origin;
Vector3 extent = stack.allocVector3();
MatrixUtil.getRow(abs_b, 0, tmp);
extent.x = tmp.dot(halfExtents);
MatrixUtil.getRow(abs_b, 1, tmp);
extent.y = tmp.dot(halfExtents);
MatrixUtil.getRow(abs_b, 2, tmp);
extent.z = tmp.dot(halfExtents);
aabbMin.set(center).sub(extent);
aabbMax.set(center).add(extent);
stack.leave();
}
@Override
public Vector3 localGetSupportingVertexWithoutMargin(Vector3 vec0, Vector3 out) {
Stack stack = Stack.enter();
Vector3 supVec = out;
supVec.set(0f, 0f, 0f);
float maxDot = -1e30f;
Vector3 vec = stack.alloc(vec0);
float lenSqr = vec.len2();
if (lenSqr < 0.0001f) {
vec.set(1f, 0f, 0f);
} else {
float rlen = 1f / (float) Math.sqrt(lenSqr);
vec.scl(rlen);
}
Vector3 vtx = stack.allocVector3();
float newDot;
float radius = getRadius();
Vector3 tmp1 = stack.allocVector3();
Vector3 tmp2 = stack.allocVector3();
Vector3 pos = stack.allocVector3();
{
pos.set(0f, 0f, 0f);
VectorUtil.setCoord(pos, getUpAxis(), getHalfHeight());
VectorUtil.mul(tmp1, vec, localScaling);
tmp1.scl(radius);
tmp2.set(vec).scl(getMargin());
vtx.set(pos).add(tmp1);
vtx.sub(tmp2);
newDot = vec.dot(vtx);
if (newDot > maxDot) {
maxDot = newDot;
supVec.set(vtx);
}
}
{
pos.set(0f, 0f, 0f);
VectorUtil.setCoord(pos, getUpAxis(), -getHalfHeight());
VectorUtil.mul(tmp1, vec, localScaling);
tmp1.scl(radius);
tmp2.set(vec).scl(getMargin());
vtx.set(pos).add(tmp1);
vtx.sub(tmp2);
newDot = vec.dot(vtx);
if (newDot > maxDot) {
maxDot = newDot;
supVec.set(vtx);
}
}
stack.leave();
return out;
}
/**
* Returns a plane with an unscaled normal.
*
* @param normal
* @param point
* @return
*/
public static Plane safePlane(Vector3 normal, Vector3 point) {
Plane p = new Plane(normal, point);
p.set(normal.x, normal.y, normal.z, -normal.dot(point));
return p;
}