public override void getAabb( ref btTransform t, out btVector3 aabbMin, out btVector3 aabbMax )
{
btVector3 halfExtents = new btVector3( getRadius(), getRadius(), getRadius() );
btVector3 tmp = halfExtents;
halfExtents[m_upAxis] = getRadius() + getHalfHeight();
halfExtents.Add( ref tmp, out halfExtents );
btMatrix3x3 abs_b; t.m_basis.absolute( out abs_b );
btVector3 extent; halfExtents.dot3( ref abs_b.m_el0, ref abs_b.m_el1, ref abs_b.m_el2, out extent );
t.m_origin.Sub( ref extent, out aabbMin );
t.m_origin.Add( ref extent, out aabbMax );
}
public void getAabbNonVirtual( ref btTransform t, out btVector3 aabbMin, out btVector3 aabbMax )
{
switch( m_shapeType )
{
case BroadphaseNativeTypes.SPHERE_SHAPE_PROXYTYPE:
{
btSphereShape sphereShape = (btSphereShape)this;
double radius = sphereShape.getImplicitShapeDimensions().x;// * convexShape.getLocalScaling().x;
double margin = radius + sphereShape.getMarginNonVirtual();
btVector3 extent = new btVector3( margin, margin, margin );
t.m_origin.Sub( ref extent, out aabbMin );
t.m_origin.Add( ref extent, out aabbMax );
}
break;
case BroadphaseNativeTypes.CYLINDER_SHAPE_PROXYTYPE:
/* fall through */
case BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE:
{
btBoxShape convexShape = (btBoxShape)this;
double margin = convexShape.getMarginNonVirtual();
btVector3 halfExtents = convexShape.getImplicitShapeDimensions();
btVector3 tmp = new btVector3( margin, margin, margin );
halfExtents.Add( ref tmp, out halfExtents );
btMatrix3x3 abs_b; t.m_basis.absolute( out abs_b );
btVector3 extent; halfExtents.dot3( ref abs_b.m_el0, ref abs_b.m_el1, ref abs_b.m_el2, out extent );
t.m_origin.Sub( ref extent, out aabbMin );
t.m_origin.Add( ref extent, out aabbMax );
break;
}
case BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE:
{
btTriangleShape triangleShape = (btTriangleShape)this;
double margin = triangleShape.getMarginNonVirtual();
aabbMax = aabbMin = btVector3.Zero;
for( int i = 0; i < 3; i++ )
{
btVector3 vec = btVector3.Zero;
vec[i] = (double)( 1.0 );
btVector3 tmp;
t.m_basis.ApplyInverse( ref vec, out tmp );
btVector3 sv; localGetSupportVertexWithoutMarginNonVirtual( ref tmp, out sv );
t.Apply( ref sv, out tmp );
aabbMax[i] = tmp[i] + margin;
vec[i] = (double)( -1.0);
t.m_basis.ApplyInverse( ref vec, out tmp );
localGetSupportVertexWithoutMarginNonVirtual( ref tmp, out sv );
t.Apply( ref sv, out tmp );
aabbMin[i] = tmp[i] - margin;
}
}
break;
case BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE:
{
btCapsuleShape capsuleShape = (btCapsuleShape)this;
btVector3 halfExtents = new btVector3( capsuleShape.getRadius(), capsuleShape.getRadius(), capsuleShape.getRadius());
int m_upAxis = capsuleShape.getUpAxis();
halfExtents[m_upAxis] = capsuleShape.getRadius() + capsuleShape.getHalfHeight();
btVector3 tmp = new btVector3( capsuleShape.getMarginNonVirtual(), capsuleShape.getMarginNonVirtual(), capsuleShape.getMarginNonVirtual() );
halfExtents.Add( ref tmp, out halfExtents );
btMatrix3x3 abs_b; t.m_basis.absolute( out abs_b );
btVector3 extent; halfExtents.dot3( ref abs_b.m_el0, ref abs_b.m_el1, ref abs_b.m_el2, out extent );
t.m_origin.Sub( ref extent, out aabbMin );
t.m_origin.Add( ref extent, out aabbMax );
}
break;
case BroadphaseNativeTypes.CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
case BroadphaseNativeTypes.CONVEX_HULL_SHAPE_PROXYTYPE:
{
btPolyhedralConvexAabbCachingShape convexHullShape = (btPolyhedralConvexAabbCachingShape)this;
double margin = convexHullShape.getMarginNonVirtual();
convexHullShape.getNonvirtualAabb( ref t, out aabbMin, out aabbMax, margin );
}
break;
default:
getAabb( ref t, out aabbMin, out aabbMax );
break;
}
// should never reach here
Debug.Assert( false );
aabbMin = aabbMax = btVector3.Zero;
}
public override void localGetSupportingVertexWithoutMargin( ref btVector3 dir, out btVector3 result )
{
btVector3 dots; dir.dot3( ref m_vertices1, ref m_vertices2, ref m_vertices3, out dots );
getVertex( dots.maxAxis(), out result );
}
/* void multInverseLeft(btTransform t1, btTransform t2) {
btVector3 v = t2.m_origin - t1.m_origin;
m_basis = btMultTransposeLeft(t1.m_basis, t2.m_basis);
m_origin = v t1.m_basis;
}
*/
/*@brief Return the transform of the vector */
public void Apply( ref btVector3 x, out btVector3 result )
{
btVector3 tmp;
x.dot3( ref m_basis, out tmp );
tmp.Add( ref m_origin, out result );
}
