/*
|
* 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.BulletGlobals;
|
import com.bulletphysics.collision.broadphase.BroadphaseNativeType;
|
import com.bulletphysics.linearmath.MatrixUtil;
|
import com.bulletphysics.linearmath.Transform;
|
import com.bulletphysics.linearmath.VectorUtil;
|
import cz.advel.stack.Stack;
|
import javax.vecmath.Matrix3f;
|
import javax.vecmath.Vector3f;
|
|
/**
|
* CapsuleShape represents a capsule around the Y axis, there is also the
|
* {@link CapsuleShapeX} aligned around the X axis and {@link CapsuleShapeZ} around
|
* the Z axis.<p>
|
*
|
* The total height is height+2*radius, so the height is just the height between
|
* the center of each "sphere" of the capsule caps.<p>
|
*
|
* CapsuleShape is a convex hull of two spheres. The {@link MultiSphereShape} is
|
* a more general collision shape that takes the convex hull of multiple sphere,
|
* so it can also represent a capsule when just using two spheres.
|
*
|
* @author jezek2
|
*/
|
public class CapsuleShape extends ConvexInternalShape
|
{
|
static final long serialVersionUID = -4332693269206844011L;
|
protected int upAxis;
|
protected boolean anchor;
|
|
// only used for CapsuleShapeZ and CapsuleShapeX subclasses.
|
CapsuleShape()
|
{
|
}
|
|
public CapsuleShape(float radius, float height)
|
{
|
this(radius,height,true);
|
}
|
|
public CapsuleShape(float radius, float height, boolean anchor)
|
{
|
upAxis = 1;
|
implicitShapeDimensions.set(radius, 0.5f * height, radius);
|
this.anchor = anchor;
|
}
|
|
@Override
|
public Vector3f localGetSupportingVertexWithoutMargin(Vector3f vec0, Vector3f out)
|
{
|
return localGetSupportingVertexWithNorm(vec0,out,null,null);
|
}
|
|
public Vector3f localGetSupportingVertexWithNorm(Vector3f vec0, Vector3f out, Vector3f normvec0, Vector3f normout)
|
{
|
Vector3f supVec = out;
|
supVec.set(0f, 0f, 0f);
|
|
// float maxDot = -1e30f;
|
|
Vector3f vec = (Vector3f) Stack.alloc(vec0);
|
Vector3f norm = (Vector3f) Stack.alloc(normvec0);
|
//Vector3f vec = (Vector3f) Stack.alloc(vec0);
|
double lenSqr = vec.lengthSquared();
|
// if (lenSqr < 0.0001f) {
|
// vec.set(1f, 0f, 0f);
|
// }
|
// else {
|
double rlen = 1 / Math.sqrt(lenSqr);
|
vec.scale((float)rlen);
|
// }
|
|
// Vector3f vtx = Stack.alloc(Vector3f.class);
|
// float newDot;
|
|
float radius = getRadius();
|
|
Vector3f tmp1 = Stack.alloc(Vector3f.class);
|
Vector3f tmp2 = Stack.alloc(Vector3f.class);
|
Vector3f pos = Stack.alloc(Vector3f.class);
|
|
pos.set(0f, 0f, 0f);
|
VectorUtil.setCoord(pos, getUpAxis(), getHalfHeight());
|
|
float factor = 1;
|
|
if (pos.x == 0)
|
factor = radius/2;
|
else
|
factor = pos.x;
|
|
vec.x *= factor;
|
if (norm != null)
|
norm.x /= factor;
|
|
if (pos.y == 0)
|
factor = radius/2;
|
else
|
factor = pos.y;
|
|
vec.y *= factor;
|
if (norm != null)
|
norm.y /= factor;
|
|
if (pos.z == 0)
|
factor = radius/2;
|
else
|
factor = pos.z;
|
|
vec.z *= factor;
|
if (norm != null)
|
norm.z /= factor;
|
|
tmp2.scale(getMargin(), vec);
|
|
out.set(vec);
|
out.sub(tmp2);
|
|
if (normout != null)
|
{
|
normout.set(norm);
|
normout.normalize();
|
}
|
// VectorUtil.mul(tmp1, vec, localScaling);
|
// tmp1.scale(radius);
|
// tmp2.scale(getMargin(), vec);
|
// vtx.add(pos, 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.scale(radius);
|
// tmp2.scale(getMargin(), vec);
|
// vtx.add(pos, tmp1);
|
// vtx.sub(tmp2);
|
// newDot = vec.dot(vtx);
|
// if (newDot > maxDot) {
|
// maxDot = newDot;
|
// supVec.set(vtx);
|
// }
|
|
return out;
|
}
|
|
@Override
|
public void batchedUnitVectorGetSupportingVertexWithoutMargin(Vector3f[] vectors, Vector3f[] supportVerticesOut, int numVectors) {
|
// TODO: implement
|
throw new UnsupportedOperationException("Not supported yet.");
|
}
|
|
@Override
|
public void calculateLocalInertia(float mass, Vector3f inertia) {
|
// as an approximation, take the inertia of the box that bounds the spheres
|
|
Transform ident = Stack.alloc(Transform.class);
|
ident.setIdentity();
|
|
float radius = getRadius();
|
|
Vector3f halfExtents = Stack.alloc(Vector3f.class);
|
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);
|
}
|
|
@Override
|
public BroadphaseNativeType getShapeType() {
|
return BroadphaseNativeType.CAPSULE_SHAPE_PROXYTYPE;
|
}
|
|
@Override
|
public void getAabb(Transform t, Vector3f aabbMin, Vector3f aabbMax) {
|
Vector3f tmp = Stack.alloc(Vector3f.class);
|
|
Vector3f halfExtents = Stack.alloc(Vector3f.class);
|
halfExtents.set(getRadius(), getRadius(), getRadius());
|
VectorUtil.setCoord(halfExtents, upAxis, getRadius() + getHalfHeight());
|
|
halfExtents.x += getMargin();
|
halfExtents.y += getMargin();
|
halfExtents.z += getMargin();
|
|
Matrix3f abs_b = Stack.alloc(Matrix3f.class);
|
abs_b.set(t.basis);
|
MatrixUtil.absolute(abs_b);
|
|
Vector3f center = t.origin;
|
Vector3f extent = Stack.alloc(Vector3f.class);
|
|
abs_b.getRow(0, tmp);
|
extent.x = tmp.dot(halfExtents);
|
abs_b.getRow(1, tmp);
|
extent.y = tmp.dot(halfExtents);
|
abs_b.getRow(2, tmp);
|
extent.z = tmp.dot(halfExtents);
|
|
aabbMin.sub(center, extent);
|
aabbMax.add(center, extent);
|
}
|
|
@Override
|
public String getName() {
|
return "CapsuleShape";
|
}
|
|
public int getUpAxis() {
|
return upAxis;
|
}
|
|
public float getRadius() {
|
int radiusAxis = (upAxis + 2) % 3;
|
return VectorUtil.getCoord(implicitShapeDimensions, radiusAxis);
|
}
|
|
public float getHalfHeight() {
|
return VectorUtil.getCoord(implicitShapeDimensions, upAxis);
|
}
|
|
}
|
