@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; }
@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(); }