public static void AabbExpand( ref btVector3 aabbMin,
ref btVector3 aabbMax,
ref btVector3 expansionMin,
ref btVector3 expansionMax )
{
aabbMin.Add( ref expansionMin, out aabbMin );
aabbMin.Add( ref expansionMax, out aabbMin );
}
public override void drawContactPoint( ref btVector3 PointOnB, ref btVector3 normalOnB, double distance, int lifeTime, ref btVector3 color )
{
btVector3 tmpD;
PointOnB.Add( ref normalOnB, out tmpD );
drawLine( ref PointOnB, ref tmpD, ref color, ref color );
}
public override void setMargin( double collisionMargin )
{
//correct the m_implicitShapeDimensions for the margin
btVector3 oldMargin = new btVector3( getMargin(), getMargin(), getMargin() );
btVector3 implicitShapeDimensionsWithMargin; oldMargin.Add( ref m_implicitShapeDimensions, out implicitShapeDimensionsWithMargin );
base.setMargin( collisionMargin );
btVector3 newMargin = new btVector3( getMargin(), getMargin(), getMargin() );
implicitShapeDimensionsWithMargin.Sub( ref newMargin, out m_implicitShapeDimensions );
}
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 override void localGetSupportingVertex( ref btVector3 vec, out btVector3 result )
{
coneLocalSupport( ref vec, out result );
if( getMargin() != btScalar.BT_ZERO )
{
btVector3 vecnorm = vec;
if( vecnorm.length2() < ( btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) )
{
vecnorm = btVector3.NegOne;
}
vecnorm.normalize();
vecnorm.Mult( getMargin(), out vecnorm );
result.Add( ref vecnorm, out result );
}
}
internal eStatus._ Evaluate( tShape shapearg, ref btVector3 guess )
{
uint iterations = 0;
double sqdist = 0;
double alpha = 0;
btVector3[] lastw = new btVector3[4];
uint clastw = 0;
/* Initialize solver */
m_free[0] = new sSV();
m_free[1] = new sSV();
m_free[2] = new sSV();
m_free[3] = new sSV();
m_nfree = 4;
m_current = 0;
m_status = eStatus._.Valid;
m_shape = shapearg;
m_distance = 0;
/* Initialize simplex */
m_simplices0.rank = 0;
m_ray = guess;
double sqrl = m_ray.length2();
btVector3 tmp;
if( sqrl > 0 )
m_ray.Invert( out tmp );
else
tmp = btVector3.xAxis;
appendvertice( m_simplices0, ref tmp );
m_simplices0.p[0] = 1;
m_ray = m_simplices0.c[0].w;
sqdist = sqrl;
lastw[0] =
lastw[1] =
lastw[2] =
lastw[3] = m_ray;
/* Loop */
do
{
uint next = 1 - m_current;
sSimplex cs = m_current==0?m_simplices0:m_simplices1;
sSimplex ns = next==0?m_simplices0:m_simplices1;
/* Check zero */
double rl = m_ray.length();
if( rl < GJK_MIN_DISTANCE )
{/* Touching or inside */
m_status = eStatus._.Inside;
break;
}
/* Append new vertice in -'v' direction */
m_ray.Invert( out tmp );
appendvertice( cs, ref tmp );
btVector3 w = cs.c[cs.rank - 1].w;
bool found = false;
for( uint i = 0; i < 4; ++i )
{
w.Sub( ref lastw[i], out tmp );
if( tmp.length2() < GJK_DUPLICATED_EPS )
{ found = true; break; }
}
if( found )
{/* Return old simplex */
removevertice( cs );
break;
}
else
{/* Update lastw */
lastw[clastw = ( clastw + 1 ) & 3] = w;
}
/* Check for termination */
double omega = btVector3.btDot( ref m_ray, ref w ) / rl;
alpha = btScalar.btMax( omega, alpha );
if( ( ( rl - alpha ) - ( GJK_ACCURARY * rl ) ) <= 0 )
{/* Return old simplex */
removevertice( cs );
break;
}
/* Reduce simplex */
double[] weights = new double[4];
uint mask = 0;
switch( cs.rank )
{
case 2:
sqdist = projectorigin( ref cs.c[0].w,
ref cs.c[1].w,
weights, out mask ); break;
case 3:
sqdist = projectorigin( ref cs.c[0].w,
ref cs.c[1].w,
ref cs.c[2].w,
weights, out mask ); break;
case 4:
sqdist = projectorigin( ref cs.c[0].w,
ref cs.c[1].w,
ref cs.c[2].w,
ref cs.c[3].w,
weights, out mask ); break;
}
if( sqdist >= 0 )
{/* Valid */
ns.rank = 0;
m_ray = btVector3.Zero;
m_current = next;
for( int i = 0, ni = (int)cs.rank; i < ni; ++i )
{
if( ( mask & ( (uint)1 << i ) ) != 0 )
{
ns.c[ns.rank] = cs.c[i];
ns.p[ns.rank++] = weights[i];
btVector3 tmp2;
cs.c[i].w.Mult( weights[i], out tmp2 );
m_ray.Add( ref tmp2, out m_ray );
}
else
{
m_free[m_nfree++] = cs.c[i];
}
}
if( mask == 15 ) m_status = eStatus._.Inside;
}
else
{/* Return old simplex */
removevertice( cs );
break;
}
m_status = ( ( ++iterations ) < GJK_MAX_ITERATIONS ) ? m_status : eStatus._.Failed;
} while( m_status == eStatus._.Valid );
m_simplex = m_current==0?m_simplices0:m_simplices1;
switch( m_status )
{
case eStatus._.Valid: m_distance = m_ray.length(); break;
case eStatus._.Inside: m_distance = 0; break;
default:
break;
}
return ( m_status );
}
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 void localGetSupportVertexNonVirtual( ref btVector3 localDir, out btVector3 result )
{
btVector3 localDirNorm = localDir;
if( localDirNorm.length2() < ( btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) )
{
localDirNorm = btVector3.NegOne;
}
localDirNorm.normalize();
localDirNorm.Mult( getMarginNonVirtual(), out localDirNorm );
localGetSupportVertexWithoutMarginNonVirtual( ref localDirNorm, out result );
result.Add( ref localDirNorm, out result );
}
public void getHalfExtentsWithMargin( out btVector3 halfExtents )
{
getHalfExtentsWithoutMargin( out halfExtents );
btVector3 margin = new btVector3( getMargin(), getMargin(), getMargin() );
halfExtents.Add( ref margin, out halfExtents );
}
public void PlaneLineIntersection( ref btVector3 p0, ref btVector3 p1, out btVector3 result )
{
// returns the point where the line p0-p1 intersects the plane n&d
btVector3 dif;
p1.Sub( ref p0, out dif );
double dn = normal.dot( ref dif );
double t = -( dist + normal.dot( ref p0 ) ) / dn;
dif.Mult( t, out dif );
p0.Add( ref dif, out result );
}
public static double DistanceBetweenLines( ref btVector3 ustart, ref btVector3 udir
, ref btVector3 vstart, ref btVector3 vdir
, out btVector3 upoint, out btVector3 vpoint )
{
btVector3 cp;
udir.cross( ref vdir, out cp );
cp.normalize();
double distu = -cp.dot( ref ustart );
double distv = -cp.dot( ref vstart );
double dist = btScalar.btFabs( distu - distv );
btPlane plane;
btVector3 tmp;
//if( upoint )
{
vdir.cross( ref cp, out plane.normal );
plane.normal.normalize();
plane.dist = -plane.normal.dot( ref vstart );
ustart.Add( ref udir, out tmp );
plane.PlaneLineIntersection( ref ustart, ref tmp, out upoint );
}
//if( vpoint )
{
udir.cross( ref cp, out plane.normal );
plane.normal.normalize();
plane.dist = -plane.normal.dot( ref ustart );
ustart.Add( ref vdir, out tmp );
plane.PlaneLineIntersection( ref vstart, ref tmp, out vpoint );
}
return dist;
}
public bool closestPtPointTriangle( ref btVector3 p, ref btVector3 a, ref btVector3 b, ref btVector3 c, ref btSubSimplexClosestResult result )
{
result.m_usedVertices = 0;//.reset();
// Check if P in vertex region outside A
btVector3 ab; b.Sub( ref a, out ab );
btVector3 ac; c.Sub( ref a, out ac );
btVector3 ap; p.Sub( ref a, out ap );
double d1 = ab.dot( ref ap );
double d2 = ac.dot( ref ap );
if( d1 <= (double)( 0.0 ) && d2 <= (double)( 0.0 ) )
{
result.m_closestPointOnSimplex = a;
result.m_usedVertices |= btUsageBitfield.usedVertexA;
result.setBarycentricCoordinates( 1, 0, 0 );
return true;// a; // barycentric coordinates (1,0,0)
}
// Check if P in vertex region outside B
btVector3 bp; p.Sub( ref b, out bp );
double d3 = ab.dot( ref bp );
double d4 = ac.dot( ref bp );
if( d3 >= (double)( 0.0 ) && d4 <= d3 )
{
result.m_closestPointOnSimplex = b;
result.m_usedVertices |= btUsageBitfield.usedVertexB;
result.setBarycentricCoordinates( 0, 1, 0 );
return true; // b; // barycentric coordinates (0,1,0)
}
// Check if P in edge region of AB, if so return projection of P onto AB
double vc = d1 * d4 - d3 * d2;
btVector3 tmp;
if( vc <= (double)( 0.0 ) && d1 >= (double)( 0.0 ) && d3 <= (double)( 0.0 ) )
{
double v = d1 / ( d1 - d3 );
ab.Mult( v, out tmp );
a.Add( ref tmp, out result.m_closestPointOnSimplex );
result.m_usedVertices |= btUsageBitfield.usedVertexA;
result.m_usedVertices |= btUsageBitfield.usedVertexB;
result.setBarycentricCoordinates( 1 - v, v, 0 );
return true;
//return a + v * ab; // barycentric coordinates (1-v,v,0)
}
// Check if P in vertex region outside C
btVector3 cp; p.Sub( ref c, out cp );
double d5 = ab.dot( ref cp );
double d6 = ac.dot( ref cp );
if( d6 >= (double)( 0.0 ) && d5 <= d6 )
{
result.m_closestPointOnSimplex = c;
result.m_usedVertices |= btUsageBitfield.usedVertexC;
result.setBarycentricCoordinates( 0, 0, 1 );
return true;//c; // barycentric coordinates (0,0,1)
}
// Check if P in edge region of AC, if so return projection of P onto AC
double vb = d5 * d2 - d1 * d6;
if( vb <= (double)( 0.0 ) && d2 >= (double)( 0.0 ) && d6 <= (double)( 0.0 ) )
{
double w = d2 / ( d2 - d6 );
ac.Mult( w, out tmp );
a.Add( ref tmp, out result.m_closestPointOnSimplex );
result.m_usedVertices |= btUsageBitfield.usedVertexA;
result.m_usedVertices |= btUsageBitfield.usedVertexC;
result.setBarycentricCoordinates( 1 - w, 0, w );
return true;
//return a + w * ac; // barycentric coordinates (1-w,0,w)
}
// Check if P in edge region of BC, if so return projection of P onto BC
double va = d3 * d6 - d5 * d4;
if( va <= (double)( 0.0 ) && ( d4 - d3 ) >= (double)( 0.0 ) && ( d5 - d6 ) >= (double)( 0.0 ) )
{
double w = ( d4 - d3 ) / ( ( d4 - d3 ) + ( d5 - d6 ) );
btVector3 tmp2;
c.Sub( ref b, out tmp2 );
tmp2.Mult( w, out tmp );
b.Add( ref tmp, out result.m_closestPointOnSimplex );
//result.m_closestPointOnSimplex = b + w * ( c - b );
result.m_usedVertices |= btUsageBitfield.usedVertexB;
result.m_usedVertices |= btUsageBitfield.usedVertexC;
result.setBarycentricCoordinates( 0, 1 - w, w );
return true;
// return b + w * (c - b); // barycentric coordinates (0,1-w,w)
}
// P inside face region. Compute Q through its barycentric coordinates (u,v,w)
{
double denom = (double)( 1.0 ) / ( va + vb + vc );
double v = vb * denom;
double w = vc * denom;
ab.Mult( v, out ab );
ac.Mult( w, out ac );
a.Add( ref ab, out tmp );
tmp.Add( ref ac, out result.m_closestPointOnSimplex );
//result.m_closestPointOnSimplex = a + ab * v + ac * w;
result.m_usedVertices |= btUsageBitfield.usedVertexA;
result.m_usedVertices |= btUsageBitfield.usedVertexB;
result.m_usedVertices |= btUsageBitfield.usedVertexC;
result.setBarycentricCoordinates( 1 - v - w, v, w );
}
return true;
// return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = (double)(1.0) - v - w
}
///calculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep)
///result is conservative
public void calculateTemporalAabb( ref btTransform curTrans, ref btVector3 linvel, ref btVector3 angvel, double timeStep
, out btVector3 temporalAabbMin, out btVector3 temporalAabbMax )
{
//start with static aabb
getAabb( ref curTrans, out temporalAabbMin, out temporalAabbMax );
double temporalAabbMaxx = temporalAabbMax.x;
double temporalAabbMaxy = temporalAabbMax.y;
double temporalAabbMaxz = temporalAabbMax.z;
double temporalAabbMinx = temporalAabbMin.x;
double temporalAabbMiny = temporalAabbMin.y;
double temporalAabbMinz = temporalAabbMin.z;
// add linear motion
btVector3 linMotion;
linvel.Mult( timeStep, out linMotion );
///@todo: simd would have a vector max/min operation, instead of per-element access
if( linMotion.x > btScalar.BT_ZERO )
temporalAabbMaxx += linMotion.x;
else
temporalAabbMinx += linMotion.x;
if( linMotion.y > btScalar.BT_ZERO )
temporalAabbMaxy += linMotion.y;
else
temporalAabbMiny += linMotion.y;
if( linMotion.z > btScalar.BT_ZERO )
temporalAabbMaxz += linMotion.z;
else
temporalAabbMinz += linMotion.z;
//add conservative angular motion
double angularMotion = angvel.length() * getAngularMotionDisc() * timeStep;
btVector3 angularMotion3d = new btVector3( angularMotion, angularMotion, angularMotion );
temporalAabbMin = new btVector3( temporalAabbMinx, temporalAabbMiny, temporalAabbMinz );
temporalAabbMax = new btVector3( temporalAabbMaxx, temporalAabbMaxy, temporalAabbMaxz );
temporalAabbMin.Sub( ref angularMotion3d, out temporalAabbMin );
temporalAabbMax.Add( ref angularMotion3d, out temporalAabbMax );
}
