/*
|
* 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.
|
*/
|
|
package com.bulletphysics.collision.shapes;
|
|
import com.bulletphysics.collision.broadphase.BroadphaseNativeType;
|
import com.bulletphysics.linearmath.MatrixUtil;
|
import com.bulletphysics.linearmath.Transform;
|
import com.bulletphysics.linearmath.VectorUtil;
|
import com.bulletphysics.util.ObjectArrayList;
|
import cz.advel.stack.Stack;
|
import javax.vecmath.Matrix3f;
|
import javax.vecmath.Vector3f;
|
|
// JAVA NOTE: CompoundShape from 2.71
|
|
/**
|
* CompoundShape allows to store multiple other {@link CollisionShape}s. This allows
|
* for moving concave collision objects. This is more general than the {@link BvhTriangleMeshShape}.
|
*
|
* @author jezek2
|
*/
|
public class CompoundShape extends CollisionShape {
|
|
private final ObjectArrayList<CompoundShapeChild> children = new ObjectArrayList<CompoundShapeChild>();
|
private final Vector3f localAabbMin = new Vector3f(1e30f, 1e30f, 1e30f);
|
private final Vector3f localAabbMax = new Vector3f(-1e30f, -1e30f, -1e30f);
|
|
private OptimizedBvh aabbTree = null;
|
|
private float collisionMargin = 0f;
|
protected final Vector3f localScaling = new Vector3f(1f, 1f, 1f);
|
|
public void addChildShape(Transform localTransform, CollisionShape shape) {
|
//m_childTransforms.push_back(localTransform);
|
//m_childShapes.push_back(shape);
|
CompoundShapeChild child = new CompoundShapeChild();
|
child.transform.set(localTransform);
|
child.childShape = shape;
|
child.childShapeType = shape.getShapeType();
|
child.childMargin = shape.getMargin();
|
|
children.add(child);
|
|
// extend the local aabbMin/aabbMax
|
Vector3f _localAabbMin = Stack.alloc(Vector3f.class), _localAabbMax = Stack.alloc(Vector3f.class);
|
shape.getAabb(localTransform, _localAabbMin, _localAabbMax);
|
|
// JAVA NOTE: rewritten
|
// for (int i=0;i<3;i++)
|
// {
|
// if (this.localAabbMin[i] > _localAabbMin[i])
|
// {
|
// this.localAabbMin[i] = _localAabbMin[i];
|
// }
|
// if (this.localAabbMax[i] < _localAabbMax[i])
|
// {
|
// this.localAabbMax[i] = _localAabbMax[i];
|
// }
|
// }
|
VectorUtil.setMin(this.localAabbMin, _localAabbMin);
|
VectorUtil.setMax(this.localAabbMax, _localAabbMax);
|
}
|
|
/**
|
* Remove all children shapes that contain the specified shape.
|
*/
|
public void removeChildShape(CollisionShape shape) {
|
boolean done_removing;
|
|
// Find the children containing the shape specified, and remove those children.
|
do {
|
done_removing = true;
|
|
for (int i = 0; i < children.size(); i++) {
|
if (children.getQuick(i).childShape == shape) {
|
children.removeQuick(i);
|
done_removing = false; // Do another iteration pass after removing from the vector
|
break;
|
}
|
}
|
}
|
while (!done_removing);
|
|
recalculateLocalAabb();
|
}
|
|
public int getNumChildShapes() {
|
return children.size();
|
}
|
|
public CollisionShape getChildShape(int index) {
|
return children.getQuick(index).childShape;
|
}
|
|
public Transform getChildTransform(int index, Transform out) {
|
out.set(children.getQuick(index).transform);
|
return out;
|
}
|
|
public ObjectArrayList<CompoundShapeChild> getChildList() {
|
return children;
|
}
|
|
/**
|
* getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version.
|
*/
|
@Override
|
public void getAabb(Transform trans, Vector3f aabbMin, Vector3f aabbMax) {
|
Vector3f localHalfExtents = Stack.alloc(Vector3f.class);
|
localHalfExtents.sub(localAabbMax, localAabbMin);
|
localHalfExtents.scale(0.5f);
|
localHalfExtents.x += getMargin();
|
localHalfExtents.y += getMargin();
|
localHalfExtents.z += getMargin();
|
|
Vector3f localCenter = Stack.alloc(Vector3f.class);
|
localCenter.add(localAabbMax, localAabbMin);
|
localCenter.scale(0.5f);
|
|
Matrix3f abs_b = (Matrix3f) Stack.alloc(trans.basis);
|
MatrixUtil.absolute(abs_b);
|
|
Vector3f center = (Vector3f) Stack.alloc(localCenter);
|
trans.transform(center);
|
|
Vector3f tmp = Stack.alloc(Vector3f.class);
|
|
Vector3f extent = Stack.alloc(Vector3f.class);
|
abs_b.getRow(0, tmp);
|
extent.x = tmp.dot(localHalfExtents);
|
abs_b.getRow(1, tmp);
|
extent.y = tmp.dot(localHalfExtents);
|
abs_b.getRow(2, tmp);
|
extent.z = tmp.dot(localHalfExtents);
|
|
aabbMin.sub(center, extent);
|
aabbMax.add(center, extent);
|
}
|
|
/**
|
* Re-calculate the local Aabb. Is called at the end of removeChildShapes.
|
* Use this yourself if you modify the children or their transforms.
|
*/
|
public void recalculateLocalAabb() {
|
// Recalculate the local aabb
|
// Brute force, it iterates over all the shapes left.
|
localAabbMin.set(1e30f, 1e30f, 1e30f);
|
localAabbMax.set(-1e30f, -1e30f, -1e30f);
|
|
Vector3f tmpLocalAabbMin = Stack.alloc(Vector3f.class);
|
Vector3f tmpLocalAabbMax = Stack.alloc(Vector3f.class);
|
|
// extend the local aabbMin/aabbMax
|
for (int j = 0; j < children.size(); j++) {
|
children.getQuick(j).childShape.getAabb(children.getQuick(j).transform, tmpLocalAabbMin, tmpLocalAabbMax);
|
|
for (int i = 0; i < 3; i++) {
|
if (VectorUtil.getCoord(localAabbMin, i) > VectorUtil.getCoord(tmpLocalAabbMin, i)) {
|
VectorUtil.setCoord(localAabbMin, i, VectorUtil.getCoord(tmpLocalAabbMin, i));
|
}
|
if (VectorUtil.getCoord(localAabbMax, i) < VectorUtil.getCoord(tmpLocalAabbMax, i)) {
|
VectorUtil.setCoord(localAabbMax, i, VectorUtil.getCoord(tmpLocalAabbMax, i));
|
}
|
}
|
}
|
}
|
|
@Override
|
public void setLocalScaling(Vector3f scaling) {
|
localScaling.set(scaling);
|
}
|
|
@Override
|
public Vector3f getLocalScaling(Vector3f out) {
|
out.set(localScaling);
|
return out;
|
}
|
|
@Override
|
public void calculateLocalInertia(float mass, Vector3f inertia) {
|
// approximation: take the inertia from the aabb for now
|
Transform ident = Stack.alloc(Transform.class);
|
ident.setIdentity();
|
Vector3f aabbMin = Stack.alloc(Vector3f.class), aabbMax = Stack.alloc(Vector3f.class);
|
getAabb(ident, aabbMin, aabbMax);
|
|
Vector3f halfExtents = Stack.alloc(Vector3f.class);
|
halfExtents.sub(aabbMax, aabbMin);
|
halfExtents.scale(0.5f);
|
|
float lx = 2f * halfExtents.x;
|
float ly = 2f * halfExtents.y;
|
float lz = 2f * halfExtents.z;
|
|
inertia.x = (mass / 12f) * (ly * ly + lz * lz);
|
inertia.y = (mass / 12f) * (lx * lx + lz * lz);
|
inertia.z = (mass / 12f) * (lx * lx + ly * ly);
|
}
|
|
@Override
|
public BroadphaseNativeType getShapeType() {
|
return BroadphaseNativeType.COMPOUND_SHAPE_PROXYTYPE;
|
}
|
|
@Override
|
public void setMargin(float margin) {
|
collisionMargin = margin;
|
}
|
|
@Override
|
public float getMargin() {
|
return collisionMargin;
|
}
|
|
@Override
|
public String getName() {
|
return "Compound";
|
}
|
|
// this is optional, but should make collision queries faster, by culling non-overlapping nodes
|
// void createAabbTreeFromChildren();
|
|
public OptimizedBvh getAabbTree() {
|
return aabbTree;
|
}
|
|
/**
|
* Computes the exact moment of inertia and the transform from the coordinate
|
* system defined by the principal axes of the moment of inertia and the center
|
* of mass to the current coordinate system. "masses" points to an array
|
* of masses of the children. The resulting transform "principal" has to be
|
* applied inversely to all children transforms in order for the local coordinate
|
* system of the compound shape to be centered at the center of mass and to coincide
|
* with the principal axes. This also necessitates a correction of the world transform
|
* of the collision object by the principal transform.
|
*/
|
public void calculatePrincipalAxisTransform(float[] masses, Transform principal, Vector3f inertia) {
|
int n = children.size();
|
|
float totalMass = 0;
|
Vector3f center = Stack.alloc(Vector3f.class);
|
center.set(0, 0, 0);
|
for (int k = 0; k < n; k++) {
|
center.scaleAdd(masses[k], children.getQuick(k).transform.origin, center);
|
totalMass += masses[k];
|
}
|
center.scale(1f / totalMass);
|
principal.origin.set(center);
|
|
Matrix3f tensor = Stack.alloc(Matrix3f.class);
|
tensor.setZero();
|
|
for (int k = 0; k < n; k++) {
|
Vector3f i = Stack.alloc(Vector3f.class);
|
children.getQuick(k).childShape.calculateLocalInertia(masses[k], i);
|
|
Transform t = children.getQuick(k).transform;
|
Vector3f o = Stack.alloc(Vector3f.class);
|
o.sub(t.origin, center);
|
|
// compute inertia tensor in coordinate system of compound shape
|
Matrix3f j = Stack.alloc(Matrix3f.class);
|
j.transpose(t.basis);
|
|
j.m00 *= i.x;
|
j.m01 *= i.x;
|
j.m02 *= i.x;
|
j.m10 *= i.y;
|
j.m11 *= i.y;
|
j.m12 *= i.y;
|
j.m20 *= i.z;
|
j.m21 *= i.z;
|
j.m22 *= i.z;
|
|
j.mul(t.basis, j);
|
|
// add inertia tensor
|
tensor.add(j);
|
|
// compute inertia tensor of pointmass at o
|
float o2 = o.lengthSquared();
|
j.setRow(0, o2, 0, 0);
|
j.setRow(1, 0, o2, 0);
|
j.setRow(2, 0, 0, o2);
|
j.m00 += o.x * -o.x;
|
j.m01 += o.y * -o.x;
|
j.m02 += o.z * -o.x;
|
j.m10 += o.x * -o.y;
|
j.m11 += o.y * -o.y;
|
j.m12 += o.z * -o.y;
|
j.m20 += o.x * -o.z;
|
j.m21 += o.y * -o.z;
|
j.m22 += o.z * -o.z;
|
|
// add inertia tensor of pointmass
|
tensor.m00 += masses[k] * j.m00;
|
tensor.m01 += masses[k] * j.m01;
|
tensor.m02 += masses[k] * j.m02;
|
tensor.m10 += masses[k] * j.m10;
|
tensor.m11 += masses[k] * j.m11;
|
tensor.m12 += masses[k] * j.m12;
|
tensor.m20 += masses[k] * j.m20;
|
tensor.m21 += masses[k] * j.m21;
|
tensor.m22 += masses[k] * j.m22;
|
}
|
|
MatrixUtil.diagonalize(tensor, principal.basis, 0.00001f, 20);
|
|
inertia.set(tensor.m00, tensor.m11, tensor.m22);
|
}
|
|
}
|
