@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;
}
@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 void calculateLocalInertia(float mass, Vector3 inertia) {
// as an approximation, take the inertia of the box that bounds the spheres
Stack stack = Stack.enter();
Transform ident = stack.allocTransform();
ident.setIdentity();
float radius = getRadius();
Vector3 halfExtents = stack.allocVector3();
halfExtents.set(radius, radius, radius);
VectorUtil.setCoord(halfExtents, getUpAxis(), radius + getHalfHeight());
float margin = BulletGlobals.CONVEX_DISTANCE_MARGIN;
float lx = 2f * (halfExtents.x + margin);
float ly = 2f * (halfExtents.y + margin);
float lz = 2f * (halfExtents.z + margin);
float x2 = lx * lx;
float y2 = ly * ly;
float z2 = lz * lz;
float scaledmass = mass * 0.08333333f;
inertia.x = scaledmass * (y2 + z2);
inertia.y = scaledmass * (x2 + z2);
inertia.z = scaledmass * (x2 + y2);
stack.leave();
}