public virtual void project( ref btTransform trans, ref btVector3 dir
, ref double min, ref double max
, out btVector3 witnesPtMin, out btVector3 witnesPtMax )
{
btVector3 localAxis; trans.m_basis.ApplyInverse( ref dir, out localAxis );
btVector3 tmpv;
localGetSupportingVertex( ref localAxis, out tmpv );
btVector3 vtx1; trans.Apply( ref tmpv, out vtx1 );
localAxis.Invert( out localAxis );
localGetSupportingVertex( ref localAxis, out tmpv );
btVector3 vtx2; trans.Apply( ref tmpv, out vtx2 );
min = vtx1.dot( ref dir );
max = vtx2.dot( ref dir );
witnesPtMax = vtx2;
witnesPtMin = vtx1;
if( min > max )
{
double tmp = min;
min = max;
max = tmp;
witnesPtMax = vtx1;
witnesPtMin = vtx2;
}
}
/// This maximum should not be necessary. It allows for untested/degenerate cases in production code.
/// You don't want your game ever to lock-up.
void computeClosestPoints( ref btTransform transA, ref btTransform transB, btPointCollector pointCollector)
{
if( m_convexB1 != null)
{
m_simplexSolver.reset();
btGjkPairDetector gjk = BulletGlobals.GjkPairDetectorPool.Get();
gjk.Initialize( m_convexA, m_convexB1, m_convexA.getShapeType(), m_convexB1.getShapeType(), m_convexA.getMargin(), m_convexB1.getMargin(), m_simplexSolver, m_penetrationDepthSolver);
btGjkPairDetector.ClosestPointInput input = new btDiscreteCollisionDetectorInterface.ClosestPointInput();
input.m_transformA = transA;
input.m_transformB = transB;
gjk.getClosestPoints( input, pointCollector, null );
BulletGlobals.GjkPairDetectorPool.Free( gjk );
}
else
{
//convex versus plane
btConvexShape convexShape = m_convexA;
btStaticPlaneShape planeShape = m_planeShape;
btVector3 planeNormal; planeShape.getPlaneNormal( out planeNormal );
double planeConstant = planeShape.getPlaneConstant();
//btTransform convexWorldTransform = transA;
btTransform convexInPlaneTrans;
btTransform tmpInv;
transB.inverse( out tmpInv );
tmpInv.Apply( ref transA, out convexInPlaneTrans );
btTransform planeInConvex;
convexInPlaneTrans.inverse( out tmpInv );
tmpInv.Apply( ref transB, out planeInConvex );
//planeInConvex = convexWorldTransform.inverse() * transB;
btVector3 tmp;
planeInConvex.m_basis.Mult( ref planeNormal, out tmp );
tmp.Invert( out tmp );
btVector3 vtx; convexShape.localGetSupportingVertex( ref tmp, out vtx );
btVector3 vtxInPlane; convexInPlaneTrans.Apply( ref vtx, out vtxInPlane );
double distance = ( planeNormal.dot( vtxInPlane ) - planeConstant );
btVector3 vtxInPlaneProjected;// = vtxInPlane - distance * planeNormal;
vtxInPlane.SubScale( ref planeNormal, distance, out vtxInPlaneProjected );
btVector3 vtxInPlaneWorld; transB.Apply(ref vtxInPlaneProjected, out vtxInPlaneWorld );
btVector3 normalOnSurfaceB; transB.m_basis.Apply( ref planeNormal, out normalOnSurfaceB );
pointCollector.addContactPoint(
ref normalOnSurfaceB,
ref vtxInPlaneWorld,
distance );
}
}
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 static void btTransformAabb( ref btVector3 localAabbMin, ref btVector3 localAabbMax, double margin, ref btTransform trans, out btVector3 aabbMinOut, out btVector3 aabbMaxOut )
{
Debug.Assert( localAabbMin.x <= localAabbMax.x );
Debug.Assert( localAabbMin.y <= localAabbMax.y );
Debug.Assert( localAabbMin.z <= localAabbMax.z );
btVector3 tmp;
localAabbMax.Sub( ref localAabbMin, out tmp );
btVector3 localHalfExtents; tmp.Mult( (double)( 0.5 ), out localHalfExtents );
btVector3 localCenter = localHalfExtents;
btVector3.Zero.Mult( margin, out tmp );
localHalfExtents.Add( ref tmp, out localHalfExtents );
//localHalfExtents += btVector3( margin, margin, margin );
btMatrix3x3 abs_b; trans.m_basis.absolute( out abs_b );
btVector3 center; trans.Apply( ref localCenter, out center );
btVector3 extent; localHalfExtents.dot3( ref abs_b.m_el0, ref abs_b.m_el1, ref abs_b.m_el2, out extent );
center.Sub( ref extent, out aabbMinOut );
center.Add(ref extent, out aabbMaxOut );
}
void getInfo2NonVirtual( ref btConstraintInfo2 info, ref btTransform transA, ref btTransform transB, ref btMatrix3x3 invInertiaWorldA, ref btMatrix3x3 invInertiaWorldB )
{
calcAngleInfo2( ref transA, ref transB, ref invInertiaWorldA, ref invInertiaWorldB );
Debug.Assert( !m_useSolveConstraintObsolete );
// set jacobian
info.m_solverConstraints[0].m_contactNormal1 = btVector3.xAxis;
info.m_solverConstraints[1].m_contactNormal1 = btVector3.yAxis;
info.m_solverConstraints[2].m_contactNormal1 = btVector3.zAxis;
//info.m_J1linearAxis = 1;
//info.m_J1linearAxis[info.rowskip + 1] = 1;
//info.m_J1linearAxis[2 * info.rowskip + 2] = 1;
btVector3 a1; transA.m_basis.Apply( ref m_rbAFrame.m_origin, out a1 );
{
//btVector3* angular0 = (btVector3*)( info.m_J1angularAxis );
//btVector3* angular1 = (btVector3*)( info.m_J1angularAxis + info.rowskip );
//btVector3* angular2 = (btVector3*)( info.m_J1angularAxis + 2 * info.rowskip );
btVector3 a1neg; a1.Invert( out a1neg );
a1neg.getSkewSymmetricMatrix( out info.m_solverConstraints[0].m_contactNormal1
, out info.m_solverConstraints[1].m_contactNormal1
, out info.m_solverConstraints[2].m_contactNormal1 );
}
info.m_solverConstraints[0].m_contactNormal2 = -btVector3.xAxis;
info.m_solverConstraints[1].m_contactNormal2 = -btVector3.yAxis;
info.m_solverConstraints[2].m_contactNormal2 = -btVector3.zAxis;
//info.m_J2linearAxis[0] = -1;
//info.m_J2linearAxis[info.rowskip + 1] = -1;
//info.m_J2linearAxis[2 * info.rowskip + 2] = -1;
btVector3 a2; transB.m_basis.Apply( ref m_rbBFrame.m_origin, out a2 );
{
a2.getSkewSymmetricMatrix( out info.m_solverConstraints[0].m_contactNormal1
, out info.m_solverConstraints[1].m_contactNormal1
, out info.m_solverConstraints[2].m_contactNormal1 );
}
// set right hand side
double linERP = ( m_flags & btConeTwistFlags.BT_CONETWIST_FLAGS_LIN_ERP ) != 0 ? m_linERP : info.erp;
double k = info.fps * linERP;
int j;
for( j = 0; j < 3; j++ )
{
info.m_solverConstraints[j].m_rhs = k * ( a2[j] + transB.m_origin[j] - a1[j] - transA.m_origin[j] );
info.m_solverConstraints[j].m_lowerLimit = btScalar.BT_MIN_FLOAT;
info.m_solverConstraints[j].m_upperLimit = btScalar.BT_MAX_FLOAT;
if( ( m_flags & btConeTwistFlags.BT_CONETWIST_FLAGS_LIN_CFM ) != 0 )
{
info.m_solverConstraints[j].m_cfm = m_linCFM;
}
}
int row = 3;
//int srow = row * info.rowskip;
btVector3 ax1;
// angular limits
if( m_solveSwingLimit )
{
//double* J1 = info.m_J1angularAxis;
//double* J2 = info.m_J2angularAxis;
if( ( m_swingSpan1 < m_fixThresh ) && ( m_swingSpan2 < m_fixThresh ) )
{
btTransform trA; transA.Apply( ref m_rbAFrame, out trA );
btVector3 p; trA.m_basis.getColumn( 1, out p );
btVector3 q; trA.m_basis.getColumn( 2, out q );
int row1 = row + 1;
//int srow1 = srow + info.rowskip;
info.m_solverConstraints[row].m_relpos1CrossNormal = p;
info.m_solverConstraints[row1].m_relpos1CrossNormal = q;
p.Invert( out info.m_solverConstraints[row].m_relpos2CrossNormal );
q.Invert( out info.m_solverConstraints[row1].m_relpos2CrossNormal );
double fact = info.fps * m_relaxationFactor;
info.m_solverConstraints[row].m_rhs = fact * m_swingAxis.dot( p );
info.m_solverConstraints[row1].m_rhs = fact * m_swingAxis.dot( q );
info.m_solverConstraints[row].m_lowerLimit = btScalar.BT_MIN_FLOAT;
info.m_solverConstraints[row1].m_upperLimit = btScalar.BT_MAX_FLOAT;
info.m_solverConstraints[row].m_lowerLimit = btScalar.BT_MIN_FLOAT;
info.m_solverConstraints[row1].m_upperLimit = btScalar.BT_MAX_FLOAT;
row = row1 + 1;
//srow = srow1 + info.rowskip;
}
else
{
ax1 = m_swingAxis * m_relaxationFactor * m_relaxationFactor;
info.m_solverConstraints[row].m_relpos1CrossNormal = ax1;
ax1.Invert( out info.m_solverConstraints[row].m_relpos2CrossNormal );
k = info.fps * m_biasFactor;
info.m_solverConstraints[row].m_rhs = k * m_swingCorrection;
if( ( m_flags & btConeTwistFlags.BT_CONETWIST_FLAGS_ANG_CFM ) != 0 )
{
info.m_solverConstraints[row].m_cfm = m_angCFM;
}
// m_swingCorrection is always positive or 0
info.m_solverConstraints[row].m_lowerLimit = 0;
info.m_solverConstraints[row].m_upperLimit = ( m_bMotorEnabled && m_maxMotorImpulse >= 0.0f ) ? m_maxMotorImpulse : btScalar.BT_MAX_FLOAT;
//srow += info.rowskip;
row++;
}
}
if( m_solveTwistLimit )
{
ax1 = m_twistAxis * m_relaxationFactor * m_relaxationFactor;
info.m_solverConstraints[row].m_relpos1CrossNormal = ax1;
ax1.Invert( out info.m_solverConstraints[row].m_relpos2CrossNormal );
k = info.fps * m_biasFactor;
info.m_solverConstraints[row].m_rhs = k * m_twistCorrection;
if( ( m_flags & btConeTwistFlags.BT_CONETWIST_FLAGS_ANG_CFM ) != 0 )
{
info.m_solverConstraints[row].m_cfm = m_angCFM;
}
if( m_twistSpan > 0.0f )
{
if( m_twistCorrection > 0.0f )
{
info.m_solverConstraints[row].m_lowerLimit = 0;
info.m_solverConstraints[row].m_upperLimit = btScalar.BT_MAX_FLOAT;
}
else
{
info.m_solverConstraints[row].m_lowerLimit = btScalar.BT_MIN_FLOAT;
info.m_solverConstraints[row].m_upperLimit = 0;
}
}
else
{
info.m_solverConstraints[row].m_lowerLimit = btScalar.BT_MIN_FLOAT;
info.m_solverConstraints[row].m_upperLimit = btScalar.BT_MAX_FLOAT;
}
//srow += info.rowskip;
row++;
}
}
//
// Api
//
//
//
public static bool Distance( btConvexShape shape0,
ref btTransform wtrs0,
btConvexShape shape1,
ref btTransform wtrs1,
ref btVector3 guess,
out sResults results )
{
tShape shape = new tShape();
results.witness0 =
results.witness1 = btVector3.Zero;
results.status = btGjkEpaSolver2.sResults.eStatus.Separated;
Initialize( shape0, ref wtrs0, shape1, ref wtrs1, out results, shape, false );
GJK gjk = new GJK();
GJK.eStatus._ gjk_status = gjk.Evaluate( shape, ref guess );
if( gjk_status == GJK.eStatus._.Valid )
{
btVector3 w0 = btVector3.Zero;
btVector3 w1 = btVector3.Zero;
for( uint i = 0; i < gjk.m_simplex.rank; ++i )
{
double p = gjk.m_simplex.p[i];
btVector3 tmp;
shape.Support( ref gjk.m_simplex.c[i].d, 0, out tmp );
w0.AddScale( ref tmp, p, out w0 );
gjk.m_simplex.c[i].d.Invert( out tmp );
shape.Support( ref tmp, 1, out tmp );
w1.AddScale( ref tmp, p, out w1 );
}
wtrs0.Apply( ref w0, out results.witness0 );
wtrs0.Apply( ref w1, out results.witness1 );
w0.Sub( ref w1, out results.normal );
results.distance = results.normal.length();
results.normal.Div( ( results.distance > GJK_MIN_DISTANCE ) ? results.distance : 1, out results.normal );
return ( true );
}
else
{
results.status = gjk_status == GJK.eStatus._.Inside
? sResults.eStatus.Penetrating
: sResults.eStatus.GJK_Failed;
return ( false );
}
}
void getInfo2InternalUsingFrameOffset( ref btConstraintInfo2 info, ref btTransform transA, ref btTransform transB, ref btVector3 angVelA, ref btVector3 angVelB )
{
//Debug.Assert( !m_useSolveConstraintObsolete );
//int i;
// transforms in world space
btTransform trA; transA.Apply( ref m_rbAFrame, out trA );
btTransform trB; transB.Apply( ref m_rbBFrame, out trB );
// pivot point
// btVector3 pivotAInW = trA.getOrigin();
// btVector3 pivotBInW = trB.getOrigin();
#if true
// difference between frames in WCS
btVector3 ofs; trB.m_origin.Sub( ref trA.m_origin, out ofs );// getOrigin() - trA.getOrigin();
// now get weight factors depending on masses
double miA = getRigidBodyA().getInvMass();
double miB = getRigidBodyB().getInvMass();
bool hasStaticBody = ( miA < btScalar.SIMD_EPSILON ) || ( miB < btScalar.SIMD_EPSILON );
double miS = miA + miB;
double factA, factB;
if( miS > (double)( 0 ) )
{
factA = miB / miS;
}
else
{
factA = (double)( 0.5f );
}
factB = (double)( 1.0f ) - factA;
// get the desired direction of hinge axis
// as weighted sum of Z-orthos of frameA and frameB in WCS
btVector3 ax1A; trA.m_basis.getColumn( 2, out ax1A );
btVector3 ax1B; trB.m_basis.getColumn( 2, out ax1B );
btVector3 tmp;
ax1A.Mult( factA, out tmp );
btVector3 ax1; tmp.AddScale( ref ax1B, factB, out ax1 );
ax1.normalize();
// fill first 3 rows
// we want: velA + wA x relA == velB + wB x relB
btTransform bodyA_trans = transA;
btTransform bodyB_trans = transB;
//int nrow = 2; // last filled row
btVector3 tmpA, tmpB, relA, relB, p, q;
// get vector from bodyB to frameB in WCS
trB.m_origin.Sub( ref bodyB_trans.m_origin, out relB );
// get its projection to hinge axis
btVector3 projB; ax1.Mult( relB.dot( ref ax1 ), out projB );
// get vector directed from bodyB to hinge axis (and orthogonal to it)
btVector3 orthoB; relB.Sub( ref projB, out orthoB );
// same for bodyA
trA.m_origin.Sub( ref bodyA_trans.m_origin, out relA );
btVector3 projA; ax1.Mult( relA.dot( ref ax1 ), out projA );
btVector3 orthoA; relA.Sub( ref projA, out orthoA );
btVector3 totalDist; projA.Sub( ref projB, out totalDist );
// get offset vectors relA and relB
orthoA.AddScale( ref totalDist, factA, out relA );
orthoB.AddScale( ref totalDist, -factB, out relB );
// now choose average ortho to hinge axis
orthoB.Mult( factA, out tmp );
tmp.AddScale( ref orthoA, factB, out p );
double len2 = p.length2();
if( len2 > btScalar.SIMD_EPSILON )
{
p.normalize();
}
else
{
trA.m_basis.getColumn( 1, out p );
}
// make one more ortho
ax1.cross( ref p, out q );
// fill three rows
relA.cross( ref p, out tmpA );
relB.cross( ref p, out tmpB );
info.m_solverConstraints[0].m_relpos1CrossNormal = tmpA;
tmpB.Invert( out info.m_solverConstraints[0].m_relpos2CrossNormal ); // = -tmpB;
relA.cross( ref q, out tmpA );
relB.cross( ref q, out tmpB );
if( hasStaticBody && getSolveLimit() )
{ // to make constraint between static and dynamic objects more rigid
// remove wA (or wB) from equation if angular limit is hit
tmpB.Mult( factB, out tmpB );
tmpA.Mult( factA, out tmpA );
}
info.m_solverConstraints[1].m_relpos1CrossNormal = tmpA;
tmpB.Invert( out info.m_solverConstraints[1].m_relpos2CrossNormal );
relA.cross( ref ax1, out tmpA );
relB.cross( ref ax1, out tmpB );
if( hasStaticBody )
{ // to make constraint between static and dynamic objects more rigid
// remove wA (or wB) from equation
tmpB.Mult( factB, out tmpB );
tmpA.Mult( factA, out tmpA );
}
info.m_solverConstraints[2].m_relpos1CrossNormal = tmpA;
tmpB.Invert( out info.m_solverConstraints[2].m_relpos2CrossNormal );
double normalErp = ( ( m_flags & btHingeFlags.BT_HINGE_FLAGS_ERP_NORM ) != 0 ) ? m_normalERP : info.erp;
double k = info.fps * normalErp;
if( !m_angularOnly )
{
info.m_solverConstraints[0].m_contactNormal1 = p;
info.m_solverConstraints[1].m_contactNormal1 = q;
info.m_solverConstraints[2].m_contactNormal1 = ax1;
p.Invert( out info.m_solverConstraints[0].m_contactNormal2 );
q.Invert( out info.m_solverConstraints[1].m_contactNormal2 );
ax1.Invert( out info.m_solverConstraints[2].m_contactNormal2 );
// compute three elements of right hand side
double rhs = k * p.dot( ofs );
info.m_solverConstraints[0].m_rhs = rhs;
rhs = k * q.dot( ofs );
info.m_solverConstraints[1].m_rhs = rhs;
rhs = k * ax1.dot( ofs );
info.m_solverConstraints[2].m_rhs = rhs;
}
// the hinge axis should be the only unconstrained
// rotational axis, the angular velocity of the two bodies perpendicular to
// the hinge axis should be equal. thus the constraint equations are
// p*w1 - p*w2 = 0
// q*w1 - q*w2 = 0
// where p and q are unit vectors normal to the hinge axis, and w1 and w2
// are the angular velocity vectors of the two bodies.
//int s3 = 3 * s;
//int s4 = 4 * s;
info.m_solverConstraints[3].m_relpos1CrossNormal = p;
info.m_solverConstraints[4].m_relpos1CrossNormal = q;
p.Invert( out info.m_solverConstraints[3].m_relpos2CrossNormal );
q.Invert( out info.m_solverConstraints[4].m_relpos2CrossNormal );
// compute the right hand side of the constraint equation. set relative
// body velocities along p and q to bring the hinge back into alignment.
// if ax1A,ax1B are the unit length hinge axes as computed from bodyA and
// bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2).
// if "theta" is the angle between ax1 and ax2, we need an angular velocity
// along u to cover angle erp*theta in one step :
// |angular_velocity| = angle/time = erp*theta / stepsize
// = (erp*fps) * theta
// angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
// = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
// ...as ax1 and ax2 are unit length. if theta is smallish,
// theta ~= sin(theta), so
// angular_velocity = (erp*fps) * (ax1 x ax2)
// ax1 x ax2 is in the plane space of ax1, so we project the angular
// velocity to p and q to find the right hand side.
k = info.fps * normalErp;//??
btVector3 u; ax1A.cross( ref ax1B, out u );
info.m_solverConstraints[3].m_rhs = k * u.dot( p );
info.m_solverConstraints[4].m_rhs = k * u.dot( q );
#endif
// check angular limits
//int nrow = 4; // last filled row
//int srow;
double limit_err = (double)( 0.0 );
int limit = 0;
if( getSolveLimit() )
{
#if _BT_USE_CENTER_LIMIT_
limit_err = m_limit.getCorrection() * m_referenceSign;
#else
limit_err = m_correction * m_referenceSign;
#endif
limit = ( limit_err > (double)( 0.0 ) ) ? 1 : 2;
}
// if the hinge has joint limits or motor, add in the extra row
bool powered = false;
if( getEnableAngularMotor() )
{
powered = true;
}
if( limit != 0 || powered )
{
info.m_solverConstraints[5].m_relpos1CrossNormal = ax1;
ax1.Invert( out info.m_solverConstraints[5].m_relpos2CrossNormal );
double lostop = getLowerLimit();
double histop = getUpperLimit();
if( limit != 0 && ( lostop == histop ) )
{ // the joint motor is ineffective
powered = false;
}
info.m_solverConstraints[5].m_rhs = (double)( 0.0f );
double currERP = ( ( m_flags & btHingeFlags.BT_HINGE_FLAGS_ERP_STOP ) != 0 ) ? m_stopERP : normalErp;
if( powered )
{
if( ( m_flags & btHingeFlags.BT_HINGE_FLAGS_CFM_NORM ) != 0 )
{
info.m_solverConstraints[5].m_cfm = m_normalCFM;
}
double mot_fact = getMotorFactor( m_hingeAngle, lostop, histop, m_motorTargetVelocity, info.fps * currERP );
info.m_solverConstraints[5].m_rhs += mot_fact * m_motorTargetVelocity * m_referenceSign;
info.m_solverConstraints[5].m_lowerLimit = -m_maxMotorImpulse;
info.m_solverConstraints[5].m_upperLimit = m_maxMotorImpulse;
}
if( limit != 0 )
{
k = info.fps * currERP;
info.m_solverConstraints[5].m_rhs += k * limit_err;
if( ( m_flags & btHingeFlags.BT_HINGE_FLAGS_CFM_STOP ) != 0 )
{
info.m_solverConstraints[5].m_cfm = m_stopCFM;
}
if( lostop == histop )
{
// limited low and high simultaneously
info.m_solverConstraints[5].m_lowerLimit = btScalar.BT_MIN_FLOAT;
info.m_solverConstraints[5].m_upperLimit = btScalar.BT_MAX_FLOAT;
}
else if( limit == 1 )
{ // low limit
info.m_solverConstraints[5].m_lowerLimit = 0;
info.m_solverConstraints[5].m_upperLimit = btScalar.BT_MAX_FLOAT;
}
else
{ // high limit
info.m_solverConstraints[5].m_lowerLimit = btScalar.BT_MIN_FLOAT;
info.m_solverConstraints[5].m_upperLimit = 0;
}
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
#if _BT_USE_CENTER_LIMIT_
double bounce = m_limit.getRelaxationFactor();
#else
double bounce = m_relaxationFactor;
#endif
if( bounce > (double)( 0.0 ) )
{
double vel = angVelA.dot( ax1 );
vel -= angVelB.dot( ax1 );
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if( limit == 1 )
{ // low limit
if( vel < 0 )
{
double newc = -bounce * vel;
if( newc > info.m_solverConstraints[5].m_rhs )
{
info.m_solverConstraints[5].m_rhs = newc;
}
}
}
else
{ // high limit - all those computations are reversed
if( vel > 0 )
{
double newc = -bounce * vel;
if( newc < info.m_solverConstraints[5].m_rhs )
{
info.m_solverConstraints[5].m_rhs = newc;
}
}
}
}
#if _BT_USE_CENTER_LIMIT_
info.m_solverConstraints[5].m_rhs *= m_limit.getBiasFactor();
#else
info.m_solverConstraints[5].m_rhs *= m_biasFactor;
#endif
} // if(limit)
} // if angular limit or powered
}
public virtual void debugDrawObject( ref btTransform worldTransform, btCollisionShape shape, btVector3 color )
{
// Draw a small simplex at the center of the object
if( m_debugDrawer != null && ( ( m_debugDrawer.getDebugMode() & btIDebugDraw.DebugDrawModes.DBG_DrawFrames ) != 0 ) )
{
m_debugDrawer.drawTransform( ref worldTransform, 1 );
}
if( shape.getShapeType() == BroadphaseNativeTypes.COMPOUND_SHAPE_PROXYTYPE )
{
btCompoundShape compoundShape = (btCompoundShape)( shape );
for( int i = compoundShape.getNumChildShapes() - 1; i >= 0; i-- )
{
//btITransform childTrans = compoundShape.getChildTransform( i );
btCollisionShape colShape = compoundShape.getChildShape( i );
btTransform tmp; worldTransform.Apply( ref compoundShape.m_children.InternalArray[i].m_transform, out tmp );
debugDrawObject( ref tmp, colShape, color );
}
}
else
{
switch( shape.getShapeType() )
{
case BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE:
{
btBoxShape boxShape = (btBoxShape)( shape );
btVector3 halfExtents; boxShape.getHalfExtentsWithMargin( out halfExtents );
btVector3 tmp; halfExtents.Invert( out tmp );
m_debugDrawer.drawBox( ref tmp, ref halfExtents, ref worldTransform, ref color );
break;
}
case BroadphaseNativeTypes.SPHERE_SHAPE_PROXYTYPE:
{
btSphereShape sphereShape = (btSphereShape)( shape );
double radius = sphereShape.getMargin();//radius doesn't include the margin, so draw with margin
m_debugDrawer.drawSphere( radius, ref worldTransform, ref color );
break;
}
case BroadphaseNativeTypes.MULTI_SPHERE_SHAPE_PROXYTYPE:
{
btMultiSphereShape multiSphereShape = (btMultiSphereShape)( shape );
btTransform childTransform = btTransform.Identity;
for( int i = multiSphereShape.getSphereCount() - 1; i >= 0; i-- )
{
multiSphereShape.getSpherePosition( i, out childTransform.m_origin );
btTransform tmp;
worldTransform.Apply( ref childTransform, out tmp );
m_debugDrawer.drawSphere( multiSphereShape.getSphereRadius( i ), ref tmp, ref color );
}
break;
}
case BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE:
{
btCapsuleShape capsuleShape = (btCapsuleShape)( shape );
double radius = capsuleShape.getRadius();
double halfHeight = capsuleShape.getHalfHeight();
int upAxis = capsuleShape.getUpAxis();
m_debugDrawer.drawCapsule( radius, halfHeight, upAxis, ref worldTransform, ref color );
break;
}
case BroadphaseNativeTypes.CONE_SHAPE_PROXYTYPE:
{
btConeShape coneShape = (btConeShape)( shape );
double radius = coneShape.getRadius();//+coneShape.getMargin();
double height = coneShape.getHeight();//+coneShape.getMargin();
int upAxis = coneShape.getConeUpIndex();
m_debugDrawer.drawCone( radius, height, upAxis, ref worldTransform, ref color );
break;
}
case BroadphaseNativeTypes.CYLINDER_SHAPE_PROXYTYPE:
{
btCylinderShape cylinder = (btCylinderShape)( shape );
int upAxis = cylinder.getUpAxis();
double radius = cylinder.getRadius();
btVector3 tmp;
cylinder.getHalfExtentsWithMargin( out tmp );
double halfHeight = tmp[upAxis];
m_debugDrawer.drawCylinder( radius, halfHeight, upAxis, ref worldTransform, ref color );
break;
}
case BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE:
{
btStaticPlaneShape staticPlaneShape = (btStaticPlaneShape)( shape );
double planeConst = staticPlaneShape.getPlaneConstant();
btVector3 planeNormal; staticPlaneShape.getPlaneNormal( out planeNormal );
m_debugDrawer.drawPlane( ref planeNormal, planeConst, ref worldTransform, ref color );
break;
}
default:
{
/// for polyhedral shapes
if( shape.isPolyhedral() )
{
btPolyhedralConvexShape polyshape = (btPolyhedralConvexShape)shape;
int i;
if( polyshape.getConvexPolyhedron() != null )
{
btConvexPolyhedron poly = polyshape.getConvexPolyhedron();
for( i = 0; i < poly.m_faces.Count; i++ )
{
btVector3 centroid = btVector3.Zero;
int numVerts = poly.m_faces[i].m_indices.Count;
if( numVerts > 0 )
{
int lastV = poly.m_faces[i].m_indices[numVerts - 1];
for( int v = 0; v < poly.m_faces[i].m_indices.Count; v++ )
{
int curVert = poly.m_faces[i].m_indices[v];
centroid += poly.m_vertices[curVert];
btVector3 tmp1, tmp2;
worldTransform.Apply( ref poly.m_vertices.InternalArray[lastV], out tmp1 );
worldTransform.Apply( ref poly.m_vertices.InternalArray[curVert], out tmp2 );
m_debugDrawer.drawLine( ref tmp1, ref tmp2, ref color );
lastV = curVert;
}
}
centroid *= (double)( 1 ) / (double)( numVerts );
if( ( m_debugDrawer.getDebugMode() & btIDebugDraw.DebugDrawModes.DBG_DrawNormals ) != 0 )
{
btVector3 normalColor = new btVector3( 1, 1, 0, 1 );
btVector3 faceNormal = new btVector3( poly.m_faces[i].m_plane[0], poly.m_faces[i].m_plane[1], poly.m_faces[i].m_plane[2] );
btVector3 tmp, tmp2;
centroid.Add( ref faceNormal, out tmp );
worldTransform.Apply( ref tmp, out tmp2 );
worldTransform.Apply( ref centroid, out tmp );
m_debugDrawer.drawLine( ref tmp, ref tmp2, ref normalColor );
}
}
}
else
{
for( i = 0; i < polyshape.getNumEdges(); i++ )
{
btVector3 a, b;
polyshape.getEdge( i, out a, out b );
btVector3 wa; worldTransform.Apply( ref a, out wa );
btVector3 wb; worldTransform.Apply( ref b, out wb );
m_debugDrawer.drawLine( ref wa, ref wb, ref color );
}
}
}
if( shape.isConcave() )
{
btConcaveShape concaveMesh = (btConcaveShape)shape;
///@todo pass camera, for some culling? no . we are not a graphics lib
btVector3 aabbMax = btVector3.Max;
btVector3 aabbMin = btVector3.Min;
DebugDrawcallback drawCallback = new DebugDrawcallback( m_debugDrawer, ref worldTransform, ref color );
concaveMesh.processAllTriangles( drawCallback, ref aabbMin, ref aabbMax );
}
if( shape.getShapeType() == BroadphaseNativeTypes.CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE )
{
Debug.Assert( false, "This needs some work... don't know the mesher stride interface types; replace with interfaces..." );
#if asdfasdf
btConvexTriangleMeshShape convexMesh = (btConvexTriangleMeshShape)shape;
//todo: pass camera for some culling
btVector3 aabbMax = btVector3.Max;// ( (double)( BT_LARGE_FLOAT ), (double)( BT_LARGE_FLOAT ), (double)( BT_LARGE_FLOAT ));
btVector3 aabbMin = btVector3.Min;// ( (double)( -BT_LARGE_FLOAT ), (double)( -BT_LARGE_FLOAT ), (double)( -BT_LARGE_FLOAT ));
//DebugDrawcallback drawCallback;
DebugDrawcallback drawCallback = new DebugDrawcallback( m_debugDrawer, ref worldTransform, color );
convexMesh.getMeshInterface().InternalProcessAllTriangles( drawCallback, aabbMin, aabbMax );
#endif
}
break;
}
}
}
}
public static void objectQuerySingleInternal( btConvexShape castShape, ref btTransform convexFromTrans, ref btTransform convexToTrans
, btCollisionShape collisionShape//btCollisionObjectWrapper colObjWrap
, btCollisionObject collisionObject
, ref btTransform colObjTransform
, ConvexResultCallback resultCallback, double allowedPenetration )
{
//btCollisionShape collisionShape = colObjWrap.getCollisionShape();
//btTransform colObjWorldTransform = colObjWrap.m_worldTransform;
if( collisionShape.isConvex() )
{
//CProfileSample sample = new CProfileSample("convexSweepConvex");
btConvexCast.CastResult castResult = new btConvexCast.CastResult();
castResult.m_allowedPenetration = allowedPenetration;
castResult.m_fraction = resultCallback.m_closestHitFraction;//btScalar.BT_ONE;//??
btConvexShape convexShape = (btConvexShape)collisionShape;
btVoronoiSimplexSolver simplexSolver = BulletGlobals.VoronoiSimplexSolverPool.Get();
btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver = BulletGlobals.GjkEpaPenetrationDepthSolverPool.Get();
// new btGjkEpaPenetrationDepthSolver();
btContinuousConvexCollision convexCaster1 = BulletGlobals.ContinuousConvexCollisionPool.Get();
convexCaster1.Initialize( castShape, convexShape, simplexSolver, gjkEpaPenetrationSolver );
//btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver);
//btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver);
btConvexCast castPtr = convexCaster1;
if( castPtr.calcTimeOfImpact( ref convexFromTrans, ref convexToTrans, ref colObjTransform, ref colObjTransform, castResult ) )
{
//add hit
if( castResult.m_normal.length2() > (double)( 0.0001 ) )
{
if( castResult.m_fraction < resultCallback.m_closestHitFraction )
{
castResult.m_normal.normalize();
LocalConvexResult localConvexResult =
new LocalConvexResult
(
collisionObject,
null,
ref castResult.m_normal,
ref castResult.m_hitPoint,
castResult.m_fraction
);
bool normalInWorldSpace = true;
resultCallback.addSingleResult( ref localConvexResult, normalInWorldSpace );
}
}
}
BulletGlobals.GjkEpaPenetrationDepthSolverPool.Free( gjkEpaPenetrationSolver );
BulletGlobals.VoronoiSimplexSolverPool.Free( simplexSolver );
BulletGlobals.ContinuousConvexCollisionPool.Free( convexCaster1 );
}
else
{
if( collisionShape.isConcave() )
{
if( collisionShape.getShapeType() == BroadphaseNativeTypes.TRIANGLE_MESH_SHAPE_PROXYTYPE )
{
//CProfileSample sample = new CProfileSample("convexSweepbtBvhTriangleMesh");
btConcaveShape triangleMesh = (btConcaveShape)collisionShape;
btTransform worldTocollisionObject; colObjTransform.inverse( out worldTocollisionObject );
btVector3 convexFromLocal; worldTocollisionObject.Apply( ref convexFromTrans.m_origin, out convexFromLocal );
btVector3 convexToLocal; worldTocollisionObject.Apply( ref convexToTrans.m_origin, out convexToLocal );
// rotation of box in local mesh space = MeshRotation^-1 ConvexToRotation
btTransform rotationXform; worldTocollisionObject.m_basis.Apply( ref convexToTrans.m_basis, out rotationXform.m_basis );
rotationXform.m_origin = btVector3.Zero;
//ConvexCast::CastResult
BridgeTriangleConvexcastCallback tccb = BulletGlobals.BridgeTriangleConvexcastCallbackPool.Get();
tccb.Initialize( castShape, ref convexFromTrans, ref convexToTrans
, resultCallback, collisionObject
, triangleMesh, ref colObjTransform );
tccb.m_hitFraction = resultCallback.m_closestHitFraction;
tccb.m_allowedPenetration = allowedPenetration;
btVector3 boxMinLocal, boxMaxLocal;
castShape.getAabb( ref rotationXform, out boxMinLocal, out boxMaxLocal );
triangleMesh.performConvexcast( tccb, ref convexFromLocal, ref convexToLocal, ref boxMinLocal, ref boxMaxLocal );
BulletGlobals.BridgeTriangleConvexcastCallbackPool.Free( tccb );
}
else
{
if( collisionShape.getShapeType() == BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE )
{
btConvexCast.CastResult castResult = BulletGlobals.CastResultPool.Get();
castResult.m_allowedPenetration = allowedPenetration;
castResult.m_fraction = resultCallback.m_closestHitFraction;
btStaticPlaneShape planeShape = (btStaticPlaneShape)collisionShape;
btContinuousConvexCollision convexCaster1 = BulletGlobals.ContinuousConvexCollisionPool.Get();
convexCaster1.Initialize( castShape, planeShape );
btConvexCast castPtr = convexCaster1;
if( castPtr.calcTimeOfImpact(ref convexFromTrans, ref convexToTrans, ref colObjTransform, ref colObjTransform, castResult ) )
{
//add hit
if( castResult.m_normal.length2() > (double)( 0.0001 ) )
{
if( castResult.m_fraction < resultCallback.m_closestHitFraction )
{
castResult.m_normal.normalize();
LocalConvexResult localConvexResult = new LocalConvexResult
(
collisionObject,
null,
ref castResult.m_normal,
ref castResult.m_hitPoint,
castResult.m_fraction
);
bool normalInWorldSpace = true;
resultCallback.addSingleResult( ref localConvexResult, normalInWorldSpace );
}
}
}
}
else
{
//CProfileSample sample = new CProfileSample("convexSweepConcave");
btConcaveShape concaveShape = (btConcaveShape)collisionShape;
btTransform worldTocollisionObject; colObjTransform.inverse( out worldTocollisionObject );
btVector3 convexFromLocal; worldTocollisionObject.Apply( ref convexFromTrans.m_origin, out convexFromLocal );
btVector3 convexToLocal; worldTocollisionObject.Apply( ref convexToTrans.m_origin, out convexToLocal );
// rotation of box in local mesh space = MeshRotation^-1 ConvexToRotation
btTransform rotationXform; worldTocollisionObject.m_basis.Apply( ref convexToTrans.m_basis, out rotationXform.m_basis );
rotationXform.m_origin = btVector3.Zero;
BridgeTriangleConvexcastCallback tccb = BulletGlobals.BridgeTriangleConvexcastCallbackPool.Get();
tccb.Initialize( castShape, ref convexFromTrans, ref convexToTrans, resultCallback, collisionObject
, concaveShape, ref colObjTransform );
tccb.m_hitFraction = resultCallback.m_closestHitFraction;
tccb.m_allowedPenetration = allowedPenetration;
btVector3 boxMinLocal, boxMaxLocal;
castShape.getAabb( ref rotationXform, out boxMinLocal, out boxMaxLocal );
btVector3 rayAabbMinLocal = convexFromLocal;
rayAabbMinLocal.setMin( ref convexToLocal );
btVector3 rayAabbMaxLocal = convexFromLocal;
rayAabbMaxLocal.setMax( ref convexToLocal );
rayAabbMinLocal += boxMinLocal;
rayAabbMaxLocal += boxMaxLocal;
concaveShape.processAllTriangles( tccb, ref rayAabbMinLocal, ref rayAabbMaxLocal );
BulletGlobals.BridgeTriangleConvexcastCallbackPool.Free( tccb );
}
}
}
else
{
///@todo : use AABB tree or other BVH acceleration structure!
if( collisionShape.isCompound() )
{
CProfileSample sample = new CProfileSample( "convexSweepCompound" );
btCompoundShape compoundShape = (btCompoundShape)( collisionShape );
int i = 0;
for( i = 0; i < compoundShape.getNumChildShapes(); i++ )
{
//btTransform childTrans = compoundShape.getChildTransform( i );
btCollisionShape childCollisionShape = compoundShape.getChildShape( i );
btTransform childWorldTrans; colObjTransform.Apply( ref compoundShape.m_children.InternalArray[i].m_transform
, out childWorldTrans );
LocalInfoAdder my_cb = new LocalInfoAdder( i, resultCallback );
//btCollisionObjectWrapper tmpObj = BulletGlobals.CollisionObjectWrapperPool.Get();
//tmpObj.Initialize( colObjWrap, childCollisionShape, colObjWrap.m_collisionObject, ref childWorldTrans, -1, i );
objectQuerySingleInternal( castShape, ref convexFromTrans, ref convexToTrans
, childCollisionShape
, collisionObject
, ref childWorldTrans
, my_cb, allowedPenetration );
//BulletGlobals.CollisionObjectWrapperPool.Free( tmpObj );
}
}
}
}
}
///SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects.
///Precondition is that objects should not penetration/overlap at the start from the interval. Overlap can be tested using btGjkPairDetector.
///
///Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases.
///See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565
internal override bool calcTimeOfImpact(
ref btTransform fromA,
ref btTransform toA,
ref btTransform fromB,
ref btTransform toB,
CastResult result )
{
m_simplexSolver.reset();
btVector3 linVelA, linVelB;
toA.m_origin.Sub( ref fromA.m_origin, out linVelA );
toB.m_origin.Sub( ref fromB.m_origin, out linVelB );
double lambda = btScalar.BT_ZERO;
btTransform interpolatedTransA; fromA.Get( out interpolatedTransA );
btTransform interpolatedTransB; fromB.Get( out interpolatedTransB );
///take relative motion
btVector3 r; linVelA.Sub( ref linVelB, out r );
btVector3 v;
btVector3 tmp;
btVector3 tmp2;
r.Invert( out v );
fromA.m_basis.ApplyInverse( ref v, out tmp );
m_convexA.localGetSupportingVertex( ref tmp, out tmp2 );
btVector3 supVertexA; fromA.Apply( ref tmp2, out supVertexA );
//btVector3 supVertexA = fromA( m_convexA.localGetSupportingVertex( -r * fromA.getBasis() ) );
fromB.m_basis.ApplyInverse( ref r, out tmp );
m_convexB.localGetSupportingVertex( ref tmp, out tmp2 );
btVector3 supVertexB; fromB.Apply( ref tmp2, out supVertexB );
//btVector3 supVertexB = fromB( m_convexB.localGetSupportingVertex( r * fromB.getBasis() ) );
supVertexA.Sub( ref supVertexB, out v );
//v = supVertexA - supVertexB;
int maxIter = MAX_ITERATIONS;
btVector3 n = btVector3.Zero;
//btVector3 c;
double dist2 = v.length2();
btVector3 w;
double VdotR;
while( ( dist2 > btScalar.SIMD_LARGE_EPSILON ) && ( maxIter-- != 0 ) )
{
//btVector3 tmp, tmp2;
v.Invert( out tmp );
interpolatedTransA.m_basis.ApplyInverse( ref tmp, out tmp2 );
m_convexA.localGetSupportingVertex( ref tmp2, out tmp );
interpolatedTransA.Apply( ref tmp, out supVertexA );
//supVertexA = interpolatedTransA( m_convexA.localGetSupportingVertex( -v * interpolatedTransA.getBasis() ) );
interpolatedTransB.m_basis.ApplyInverse( ref v, out tmp2 );
m_convexB.localGetSupportingVertex( ref tmp2, out tmp );
interpolatedTransB.Apply( ref tmp, out supVertexB );
//supVertexB = interpolatedTransB( m_convexB.localGetSupportingVertex( v * interpolatedTransB.getBasis() ) );
supVertexA.Sub( ref supVertexB, out w );
//w = supVertexA - supVertexB;
double VdotW = v.dot( ref w );
if( lambda > (double)( 1.0 ) )
{
return false;
}
if( VdotW > btScalar.BT_ZERO )
{
VdotR = v.dot( ref r );
if( VdotR >= -( btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) )
return false;
else
{
lambda = lambda - VdotW / VdotR;
//interpolate to next lambda
// x = s + lambda * r;
interpolatedTransA.m_origin.setInterpolate3( ref fromA.m_origin, ref toA.m_origin, lambda );
interpolatedTransB.m_origin.setInterpolate3( ref fromB.m_origin, ref toB.m_origin, lambda );
//m_simplexSolver.reset();
//check next line
supVertexA.Sub( ref supVertexB, out w );
//w = supVertexA - supVertexB;
n = v;
}
}
///Just like regular GJK only add the vertex if it isn't already (close) to current vertex, it would lead to divisions by zero and NaN etc.
if( !m_simplexSolver.inSimplex( ref w ) )
m_simplexSolver.addVertex( ref w, ref supVertexA, ref supVertexB );
if( m_simplexSolver.closest( out v ) )
{
dist2 = v.length2();
//todo: check this normal for validity
//n=v;
//Console.WriteLine("V=%f , %f, %f\n",v,v[1],v[2]);
//Console.WriteLine("DIST2=%f\n",dist2);
//Console.WriteLine("numverts = %i\n",m_simplexSolver.numVertices());
}
else
{
dist2 = btScalar.BT_ZERO;
}
}
//int numiter = MAX_ITERATIONS - maxIter;
// Console.WriteLine("number of iterations: %d", numiter);
//don't report a time of impact when moving 'away' from the hitnormal
result.m_fraction = lambda;
if( n.length2() >= ( btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) )
n.normalized( out result.m_normal );
else
result.m_normal = btVector3.Zero;
//don't report time of impact for motion away from the contact normal (or causes minor penetration)
if( result.m_normal.dot( ref r ) >= -result.m_allowedPenetration )
return false;
btVector3 hitA, hitB;
m_simplexSolver.compute_points( out hitA, out hitB );
result.m_hitPoint = hitB;
return true;
}
void collideSingleContact( bool usePertube, ref btQuaternion perturbeRot
, btCollisionObjectWrapper convexObjWrap
, ref btTransform convexTransform
, btCollisionObjectWrapper planeObjWrap
, ref btTransform planeTransform
, btDispatcherInfo dispatchInfo, btManifoldResult resultOut
, ref btVector3 planeNormal, double planeConstant
)
{
//btCollisionObjectWrapper convexObjWrap = m_swapped ? body1Wrap : body0Wrap;
//btCollisionObjectWrapper planeObjWrap = m_swapped ? body0Wrap : body1Wrap;
btConvexShape convexShape = (btConvexShape)convexObjWrap.getCollisionShape();
btStaticPlaneShape planeShape = (btStaticPlaneShape)planeObjWrap.getCollisionShape();
bool hasCollision = false;
//planeNormal = planeShape.getPlaneNormal().Copy( out planeNormal );
//double planeConstant = planeShape.getPlaneConstant();
btTransform convexWorldTransform = convexTransform;
//btTransform planeWorldTransform = planeObjWrap.m_worldTransform;
btTransform convexInPlaneTrans;
planeTransform.inverseTimes( ref convexWorldTransform, out convexInPlaneTrans );
if( usePertube )
{
//now perturbe the convex-world transform
btMatrix3x3 perturbeMat = new btMatrix3x3( ref perturbeRot );
btMatrix3x3 tmpPerturbe; convexWorldTransform.m_basis.Apply( ref perturbeMat, out tmpPerturbe );
convexWorldTransform.m_basis = tmpPerturbe;
//convexWorldTransform.getBasis() *= btMatrix3x3( perturbeRot );
}
btTransform planeInConvex;
convexTransform.inverseTimes( ref planeObjWrap.m_collisionObject.m_worldTransform, out planeInConvex );
btVector3 tmp, tmp2;
planeNormal.Invert( out tmp );
planeInConvex.m_basis.Apply( ref tmp, out tmp2 );
btVector3 vtx; convexShape.localGetSupportingVertex( ref tmp2, out vtx );
btVector3 vtxInPlane; convexInPlaneTrans.Apply( ref vtx, out vtxInPlane );
double distance = ( planeNormal.dot( ref vtxInPlane ) - planeConstant );
btVector3 vtxInPlaneProjected; vtxInPlane.AddScale( ref planeNormal, -distance, out vtxInPlaneProjected );
btVector3 vtxInPlaneWorld; planeTransform.Apply( ref vtxInPlaneProjected, out vtxInPlaneWorld );
hasCollision = distance < m_manifoldPtr.getContactBreakingThreshold();
resultOut.setPersistentManifold( m_manifoldPtr );
if( hasCollision )
{
/// report a contact. internally this will be kept persistent, and contact reduction is done
btVector3 normalOnSurfaceB; planeTransform.m_basis.Apply( ref planeNormal, out normalOnSurfaceB );
btScalar.Dbg( "Convex plane adds point " + normalOnSurfaceB + " " + vtxInPlaneWorld + " " + distance.ToString( "g17" ) );
resultOut.addContactPoint( ref normalOnSurfaceB, ref vtxInPlaneWorld, distance );
}
}
/// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin
public void refreshContactPoints( ref btTransform trA, ref btTransform trB )
{
int i;
#if DEBUG_PERSISTENCY
Console.WriteLine( "refreshContactPoints posA = ("
+ trA.m_origin.x
+ ","
+ trA.m_origin.y
+ ","
+ trA.m_origin.z
+ ") posB = ("
+ trB.m_origin.x
+ ","
+ trB.m_origin.y
+ ","
+ trB.m_origin.z
+ ")\n" );
#endif //DEBUG_PERSISTENCY
/// first refresh worldspace positions and distance
for( i = m_cachedPoints - 1; i >= 0; i-- )
{
btManifoldPoint manifoldPoint = m_pointCache[i];
trA.Apply( ref manifoldPoint.m_localPointA, out manifoldPoint.m_positionWorldOnA );
trB.Apply( ref manifoldPoint.m_localPointB, out manifoldPoint.m_positionWorldOnB );
btVector3 tmp;
manifoldPoint.m_positionWorldOnA.Sub( ref manifoldPoint.m_positionWorldOnB, out tmp );
manifoldPoint.m_distance1 = tmp.dot( ref manifoldPoint.m_normalWorldOnB );
manifoldPoint.m_lifeTime++;
}
/// then
double distance2d;
btVector3 projectedDifference, projectedPoint;
for( i = m_cachedPoints - 1; i >= 0; i-- )
{
btManifoldPoint manifoldPoint = m_pointCache[i];
//contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
if( !validContactDistance( manifoldPoint ) )
{
removeContactPoint( i );
}
else
{
//contact also becomes invalid when relative movement orthogonal to normal exceeds margin
manifoldPoint.m_positionWorldOnA.SubScale( ref manifoldPoint.m_normalWorldOnB, manifoldPoint.m_distance1, out projectedPoint );
manifoldPoint.m_positionWorldOnB.Sub( ref projectedPoint, out projectedDifference );
distance2d = projectedDifference.dot( ref projectedDifference );
if( distance2d > m_contactBreakingThresholdSquared )
{
removeContactPoint( i );
}
else
{
//contact point processed callback
if( gContactProcessedCallback != null )
gContactProcessedCallback( manifoldPoint, m_body0, m_body1 );
}
}
}
}
public void project( ref btTransform trans, ref btVector3 dir, out double minProj, out double maxProj, out btVector3 witnesPtMin, out btVector3 witnesPtMax )
{
btVector3[] arr_vertices = m_vertices.InternalArray;
int numVerts;
if( ( numVerts = m_vertices.Count ) > 0 )
{
btVector3 pt; trans.Apply( ref arr_vertices[0], out pt );
double dp = pt.dot( ref dir );
minProj = dp;
witnesPtMin = pt;
maxProj = dp;
witnesPtMax = pt;
for( int i = 1; i < numVerts; i++ )
{
trans.Apply( ref arr_vertices[i], out pt );
dp = pt.dot( ref dir );
if( dp < minProj )
{
minProj = dp;
witnesPtMin = pt;
}
if( dp > maxProj )
{
maxProj = dp;
witnesPtMax = pt;
}
}
}
else
{
Debug.Assert( false );
minProj = double.MaxValue;
maxProj = double.MinValue;
witnesPtMax = witnesPtMin = btVector3.Zero;
}
#if asdfasdf
if( minProj > maxProj )
{
btScalar.btSwap( ref minProj, ref maxProj );
btScalar.btSwap( ref witnesPtMin, ref witnesPtMax );
}
#endif
}
/*
void solveConstraintObsolete( btSolverBody bodyA, btSolverBody bodyB, double timeStep )
{
if( m_useSolveConstraintObsolete )
{
btVector3 pivotAInW = m_rbA.m_worldTransform * m_rbAFrame.m_origin;
btVector3 pivotBInW = m_rbB.m_worldTransform * m_rbBFrame.m_origin;
double tau = (double)( 0.3 );
//linear part
if( !m_angularOnly )
{
btVector3 rel_pos1 = pivotAInW - m_rbA.m_worldTransform.m_origin;
btVector3 rel_pos2 = pivotBInW - m_rbB.m_worldTransform.m_origin;
btVector3 vel1;
bodyA.internalGetVelocityInLocalPointObsolete( rel_pos1, vel1 );
btVector3 vel2;
bodyB.internalGetVelocityInLocalPointObsolete( rel_pos2, vel2 );
btVector3 vel = vel1 - vel2;
for( int i = 0; i < 3; i++ )
{
btIVector3 normal = m_jac[i].m_linearJointAxis;
double jacDiagABInv = btScalar.BT_ONE / m_jac[i].getDiagonal();
double rel_vel;
rel_vel = normal.dot( vel );
//positional error (zeroth order error)
double depth = -( pivotAInW - pivotBInW ).dot( normal ); //this is the error projected on the normal
double impulse = depth * tau / timeStep * jacDiagABInv - rel_vel * jacDiagABInv;
m_appliedImpulse += impulse;
btVector3 ftorqueAxis1 = rel_pos1.cross( normal );
btVector3 ftorqueAxis2 = rel_pos2.cross( normal );
bodyA.internalApplyImpulse( normal * m_rbA.getInvMass(), m_rbA.m_invInertiaTensorWorld * ftorqueAxis1, impulse );
bodyB.internalApplyImpulse( normal * m_rbB.getInvMass(), m_rbB.m_invInertiaTensorWorld * ftorqueAxis2, -impulse );
}
}
// apply motor
if( m_bMotorEnabled )
{
// compute current and predicted transforms
btTransform trACur = m_rbA.m_worldTransform;
btTransform trBCur = m_rbB.m_worldTransform;
btVector3 omegaA; bodyA.internalGetAngularVelocity( omegaA );
btVector3 omegaB; bodyB.internalGetAngularVelocity( omegaB );
btTransform trAPred; trAPred.setIdentity();
btVector3 zerovec( 0, 0, 0);
btTransformUtil::integrateTransform(
trACur, zerovec, omegaA, timeStep, trAPred );
btTransform trBPred; trBPred.setIdentity();
btTransformUtil::integrateTransform(
trBCur, zerovec, omegaB, timeStep, trBPred );
// compute desired transforms in world
btTransform trPose( m_qTarget );
btTransform trABDes = m_rbBFrame * trPose * m_rbAFrame.inverse();
btTransform trADes = trBPred * trABDes;
btTransform trBDes = trAPred * trABDes.inverse();
// compute desired omegas in world
btVector3 omegaADes, omegaBDes;
btTransformUtil::calculateVelocity( trACur, trADes, timeStep, zerovec, omegaADes );
btTransformUtil::calculateVelocity( trBCur, trBDes, timeStep, zerovec, omegaBDes );
// compute delta omegas
btVector3 dOmegaA = omegaADes - omegaA;
btVector3 dOmegaB = omegaBDes - omegaB;
// compute weighted avg axis of dOmega (weighting based on inertias)
btVector3 axisA, axisB;
double kAxisAInv = 0, kAxisBInv = 0;
if( dOmegaA.length2() > btScalar.SIMD_EPSILON )
{
axisA = dOmegaA.normalized();
kAxisAInv = m_rbA.computeAngularImpulseDenominator( axisA );
}
if( dOmegaB.length2() > btScalar.SIMD_EPSILON )
{
axisB = dOmegaB.normalized();
kAxisBInv = m_rbB.computeAngularImpulseDenominator( axisB );
}
btVector3 avgAxis = kAxisAInv * axisA + kAxisBInv * axisB;
static bool bDoTorque = true;
if( bDoTorque & avgAxis.length2() > btScalar.SIMD_EPSILON )
{
avgAxis.normalize();
kAxisAInv = m_rbA.computeAngularImpulseDenominator( avgAxis );
kAxisBInv = m_rbB.computeAngularImpulseDenominator( avgAxis );
double kInvCombined = kAxisAInv + kAxisBInv;
btVector3 impulse = ( kAxisAInv * dOmegaA - kAxisBInv * dOmegaB ) /
( kInvCombined * kInvCombined );
if( m_maxMotorImpulse >= 0 )
{
double fMaxImpulse = m_maxMotorImpulse;
if( m_bNormalizedMotorStrength )
fMaxImpulse = fMaxImpulse / kAxisAInv;
btVector3 newUnclampedAccImpulse = m_accMotorImpulse + impulse;
double newUnclampedMag = newUnclampedAccImpulse.length();
if( newUnclampedMag > fMaxImpulse )
{
newUnclampedAccImpulse.normalize();
newUnclampedAccImpulse *= fMaxImpulse;
impulse = newUnclampedAccImpulse - m_accMotorImpulse;
}
m_accMotorImpulse += impulse;
}
double impulseMag = impulse.length();
btVector3 impulseAxis = impulse / impulseMag;
bodyA.internalApplyImpulse( btVector3( 0, 0, 0 ), m_rbA.m_invInertiaTensorWorld * impulseAxis, impulseMag );
bodyB.internalApplyImpulse( btVector3( 0, 0, 0 ), m_rbB.m_invInertiaTensorWorld * impulseAxis, -impulseMag );
}
}
else if( m_damping > btScalar.SIMD_EPSILON ) // no motor: do a little damping
{
btVector3 angVelA; bodyA.internalGetAngularVelocity( angVelA );
btVector3 angVelB; bodyB.internalGetAngularVelocity( angVelB );
btVector3 relVel = angVelB - angVelA;
if( relVel.length2() > btScalar.SIMD_EPSILON )
{
btVector3 relVelAxis = relVel.normalized();
double m_kDamping = btScalar.BT_ONE /
( m_rbA.computeAngularImpulseDenominator( relVelAxis ) +
m_rbB.computeAngularImpulseDenominator( relVelAxis ) );
btVector3 impulse = m_damping * m_kDamping * relVel;
double impulseMag = impulse.length();
btVector3 impulseAxis = impulse / impulseMag;
bodyA.internalApplyImpulse( btVector3( 0, 0, 0 ), m_rbA.m_invInertiaTensorWorld * impulseAxis, impulseMag );
bodyB.internalApplyImpulse( btVector3( 0, 0, 0 ), m_rbB.m_invInertiaTensorWorld * impulseAxis, -impulseMag );
}
}
// joint limits
{
///solve angular part
btVector3 angVelA;
bodyA.internalGetAngularVelocity( angVelA );
btVector3 angVelB;
bodyB.internalGetAngularVelocity( angVelB );
// solve swing limit
if( m_solveSwingLimit )
{
double amplitude = m_swingLimitRatio * m_swingCorrection * m_biasFactor / timeStep;
double relSwingVel = ( angVelB - angVelA ).dot( m_swingAxis );
if( relSwingVel > 0 )
amplitude += m_swingLimitRatio * relSwingVel * m_relaxationFactor;
double impulseMag = amplitude * m_kSwing;
// Clamp the accumulated impulse
double temp = m_accSwingLimitImpulse;
m_accSwingLimitImpulse = btMax( m_accSwingLimitImpulse + impulseMag, (double)( 0.0 ) );
impulseMag = m_accSwingLimitImpulse - temp;
btVector3 impulse = m_swingAxis * impulseMag;
// don't let cone response affect twist
// (this can happen since body A's twist doesn't match body B's AND we use an elliptical cone limit)
{
btVector3 impulseTwistCouple = impulse.dot( m_twistAxisA ) * m_twistAxisA;
btVector3 impulseNoTwistCouple = impulse - impulseTwistCouple;
impulse = impulseNoTwistCouple;
}
impulseMag = impulse.length();
btVector3 noTwistSwingAxis = impulse / impulseMag;
bodyA.internalApplyImpulse( btVector3( 0, 0, 0 ), m_rbA.m_invInertiaTensorWorld * noTwistSwingAxis, impulseMag );
bodyB.internalApplyImpulse( btVector3( 0, 0, 0 ), m_rbB.m_invInertiaTensorWorld * noTwistSwingAxis, -impulseMag );
}
// solve twist limit
if( m_solveTwistLimit )
{
double amplitude = m_twistLimitRatio * m_twistCorrection * m_biasFactor / timeStep;
double relTwistVel = ( angVelB - angVelA ).dot( m_twistAxis );
if( relTwistVel > 0 ) // only damp when moving towards limit (m_twistAxis flipping is important)
amplitude += m_twistLimitRatio * relTwistVel * m_relaxationFactor;
double impulseMag = amplitude * m_kTwist;
// Clamp the accumulated impulse
double temp = m_accTwistLimitImpulse;
m_accTwistLimitImpulse = btMax( m_accTwistLimitImpulse + impulseMag, (double)( 0.0 ) );
impulseMag = m_accTwistLimitImpulse - temp;
// btVector3 impulse = m_twistAxis * impulseMag;
bodyA.internalApplyImpulse( btVector3( 0, 0, 0 ), m_rbA.m_invInertiaTensorWorld * m_twistAxis, impulseMag );
bodyB.internalApplyImpulse( btVector3( 0, 0, 0 ), m_rbB.m_invInertiaTensorWorld * m_twistAxis, -impulseMag );
}
}
}
}
*/
/*
void calcAngleInfo()
{
m_swingCorrection = btScalar.BT_ZERO;
m_twistLimitSign = btScalar.BT_ZERO;
m_solveTwistLimit = false;
m_solveSwingLimit = false;
btVector3 b1Axis1 = btVector3.Zero,b1Axis2 = btVector3.Zero, b1Axis3 = btVector3.Zero;
btVector3 b2Axis1 = btVector3.Zero, b2Axis2 = btVector3.Zero;
b1Axis1 = m_rbA.m_worldTransform.m_basis * this.m_rbAFrame.m_basis.getColumn( 0 );
b2Axis1 = m_rbB.m_worldTransform.m_basis * this.m_rbBFrame.m_basis.getColumn( 0 );
double swing1 = btScalar.BT_ZERO, swing2 = btScalar.BT_ZERO;
double swx = btScalar.BT_ZERO, swy = btScalar.BT_ZERO;
double thresh = 10;
double fact;
// Get Frame into world space
if( m_swingSpan1 >= (double)( 0.05f ) )
{
b1Axis2 = m_rbA.m_worldTransform.m_basis * this.m_rbAFrame.m_basis.getColumn( 1 );
swx = b2Axis1.dot( b1Axis1 );
swy = b2Axis1.dot( b1Axis2 );
swing1 = btScalar.btAtan2Fast( swy, swx );
fact = ( swy * swy + swx * swx ) * thresh * thresh;
fact = fact / ( fact + (double)( 1.0 ) );
swing1 *= fact;
}
if( m_swingSpan2 >= (double)( 0.05f ) )
{
b1Axis3 = m_rbA.m_worldTransform.m_basis * this.m_rbAFrame.m_basis.getColumn( 2 );
swx = b2Axis1.dot( b1Axis1 );
swy = b2Axis1.dot( b1Axis3 );
swing2 = btScalar.btAtan2Fast( swy, swx );
fact = ( swy * swy + swx * swx ) * thresh * thresh;
fact = fact / ( fact + (double)( 1.0 ) );
swing2 *= fact;
}
double RMaxAngle1Sq = 1.0f / ( m_swingSpan1 * m_swingSpan1 );
double RMaxAngle2Sq = 1.0f / ( m_swingSpan2 * m_swingSpan2 );
double EllipseAngle = btScalar.btFabs( swing1 * swing1 ) * RMaxAngle1Sq + btScalar.btFabs( swing2 * swing2 ) * RMaxAngle2Sq;
if( EllipseAngle > 1.0f )
{
m_swingCorrection = EllipseAngle - 1.0f;
m_solveSwingLimit = true;
// Calculate necessary axis & factors
m_swingAxis = b2Axis1.cross( b1Axis2 * b2Axis1.dot( b1Axis2 ) + b1Axis3 * b2Axis1.dot( b1Axis3 ) );
m_swingAxis.normalize();
double swingAxisSign = ( b2Axis1.dot( b1Axis1 ) >= 0.0f ) ? 1.0f : -1.0f;
m_swingAxis *= swingAxisSign;
}
// Twist limits
if( m_twistSpan >= btScalar.BT_ZERO )
{
btVector3 b2Axis2 = m_rbB.m_worldTransform.m_basis * this.m_rbBFrame.m_basis.getColumn( 1 );
btQuaternion rotationArc = btQuaternion.shortestArcQuat( b2Axis1, b1Axis1 );
btVector3 TwistRef = btQuaternion.quatRotate( rotationArc, b2Axis2 );
double twist = btScalar.btAtan2Fast( TwistRef.dot( b1Axis3 ), TwistRef.dot( b1Axis2 ) );
m_twistAngle = twist;
// double lockedFreeFactor = (m_twistSpan > (double)(0.05f)) ? m_limitSoftness : btScalar.BT_ZERO;
double lockedFreeFactor = ( m_twistSpan > (double)( 0.05f ) ) ? (double)( 1.0f ) : btScalar.BT_ZERO;
if( twist <= -m_twistSpan * lockedFreeFactor )
{
m_twistCorrection = -( twist + m_twistSpan );
m_solveTwistLimit = true;
m_twistAxis = ( b2Axis1 + b1Axis1 ) * 0.5f;
m_twistAxis.normalize();
m_twistAxis *= -1.0f;
}
else if( twist > m_twistSpan * lockedFreeFactor )
{
m_twistCorrection = ( twist - m_twistSpan );
m_solveTwistLimit = true;
m_twistAxis = ( b2Axis1 + b1Axis1 ) * 0.5f;
m_twistAxis.normalize();
}
}
}
*/
void calcAngleInfo2( ref btTransform transA, ref btTransform transB, ref btMatrix3x3 invInertiaWorldA, ref btMatrix3x3 invInertiaWorldB )
{
m_swingCorrection = btScalar.BT_ZERO;
m_twistLimitSign = btScalar.BT_ZERO;
m_solveTwistLimit = false;
m_solveSwingLimit = false;
// compute rotation of A wrt B (in constraint space)
if( m_bMotorEnabled && ( !m_useSolveConstraintObsolete ) )
{ // it is assumed that setMotorTarget() was alredy called
// and motor target m_qTarget is within constraint limits
// TODO : split rotation to pure swing and pure twist
// compute desired transforms in world
btTransform trPose = new btTransform( ref m_qTarget );
btTransform trA; transA.Apply( ref m_rbAFrame, out trA );
btTransform trB; transB.Apply( ref m_rbBFrame, out trB );
btTransform tmp;
btTransform trAInv;
trA.inverse( out trAInv );
trB.Apply( ref trPose, out tmp );
btTransform trDeltaAB;// = trB * trPose * trA.inverse();
tmp.Apply( ref trAInv, out trDeltaAB );
btQuaternion qDeltaAB; trDeltaAB.getRotation( out qDeltaAB );
btVector3 swingAxis = new btVector3( qDeltaAB.x, qDeltaAB.y, qDeltaAB.z );
double swingAxisLen2 = swingAxis.length2();
if( btScalar.btFuzzyZero( swingAxisLen2 ) )
{
return;
}
m_swingAxis = swingAxis;
m_swingAxis.normalize();
m_swingCorrection = qDeltaAB.getAngle();
if( !btScalar.btFuzzyZero( m_swingCorrection ) )
{
m_solveSwingLimit = true;
}
return;
}
{
// compute rotation of A wrt B (in constraint space)
btQuaternion tmpA;
transA.getRotation( out tmpA );
btQuaternion tmpAFrame;
m_rbAFrame.getRotation( out tmpAFrame );
btQuaternion qA; tmpA.Mult( ref tmpAFrame, out qA );
transB.getRotation( out tmpA );
m_rbBFrame.getRotation( out tmpAFrame );
btQuaternion qB; tmpA.Mult( ref tmpAFrame, out qB );// transB.getRotation() * m_rbBFrame.getRotation();
btQuaternion qBInv;
qB.inverse( out qBInv );
btQuaternion qAB; qBInv.Mult( ref qA, out qAB );
// split rotation into cone and twist
// (all this is done from B's perspective. Maybe I should be averaging axes...)
btVector3 vConeNoTwist; btQuaternion.quatRotate( ref qAB, ref vTwist, out vConeNoTwist ); vConeNoTwist.normalize();
btQuaternion qABCone; btQuaternion.shortestArcQuat( ref vTwist, ref vConeNoTwist, out qABCone ); qABCone.normalize();
btQuaternion qABInv;
qABCone.inverse( out qABInv );
btQuaternion qABTwist; qABInv.Mult( ref qAB, out qABTwist ); qABTwist.normalize();
if( m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh )
{
double swingAngle, swingLimit = 0;
btVector3 swingAxis;
computeConeLimitInfo( ref qABCone, out swingAngle, out swingAxis, out swingLimit );
if( swingAngle > swingLimit * m_limitSoftness )
{
m_solveSwingLimit = true;
// compute limit ratio: 0.1, where
// 0 == beginning of soft limit
// 1 == hard/real limit
m_swingLimitRatio = 1;
if( swingAngle < swingLimit && m_limitSoftness < 1 - btScalar.SIMD_EPSILON )
{
m_swingLimitRatio = ( swingAngle - swingLimit * m_limitSoftness ) /
( swingLimit - swingLimit * m_limitSoftness );
}
// swing correction tries to get back to soft limit
m_swingCorrection = swingAngle - ( swingLimit * m_limitSoftness );
// adjustment of swing axis (based on ellipse normal)
adjustSwingAxisToUseEllipseNormal( ref swingAxis );
// Calculate necessary axis & factors
btVector3 swingAxisInv;
swingAxis.Invert( out swingAxisInv );
btQuaternion.quatRotate( ref qB, ref swingAxisInv, out m_swingAxis );
m_twistAxisA.setValue( 0, 0, 0 );
m_kSwing = btScalar.BT_ONE /
( computeAngularImpulseDenominator( ref m_swingAxis, invInertiaWorldA ) +
computeAngularImpulseDenominator( ref m_swingAxis, invInertiaWorldB ) );
}
}
else
{
// you haven't set any limits;
// or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?)
// anyway, we have either hinge or fixed joint
btVector3 ivA = transA.m_basis * m_rbAFrame.m_basis.getColumn( 0 );
btVector3 jvA = transA.m_basis * m_rbAFrame.m_basis.getColumn( 1 );
btVector3 kvA = transA.m_basis * m_rbAFrame.m_basis.getColumn( 2 );
btVector3 ivB = transB.m_basis * m_rbBFrame.m_basis.getColumn( 0 );
btVector3 target;
double x = ivB.dot( ivA );
double y = ivB.dot( jvA );
double z = ivB.dot( kvA );
if( ( m_swingSpan1 < m_fixThresh ) && ( m_swingSpan2 < m_fixThresh ) )
{ // fixed. We'll need to add one more row to constraint
if( ( !btScalar.btFuzzyZero( y ) ) || ( !( btScalar.btFuzzyZero( z ) ) ) )
{
m_solveSwingLimit = true;
m_swingAxis = -ivB.cross( ivA );
}
}
else
{
if( m_swingSpan1 < m_fixThresh )
{ // hinge around Y axis
// if(!(btFuzzyZero(y)))
if( ( !( btScalar.btFuzzyZero( x ) ) ) || ( !( btScalar.btFuzzyZero( z ) ) ) )
{
m_solveSwingLimit = true;
if( m_swingSpan2 >= m_fixThresh )
{
y = (double)( 0 );
double span2 = btScalar.btAtan2( z, x );
if( span2 > m_swingSpan2 )
{
x = btScalar.btCos( m_swingSpan2 );
z = btScalar.btSin( m_swingSpan2 );
}
else if( span2 < -m_swingSpan2 )
{
x = btScalar.btCos( m_swingSpan2 );
z = -btScalar.btSin( m_swingSpan2 );
}
}
}
}
else
{ // hinge around Z axis
// if(!btFuzzyZero(z))
if( ( !( btScalar.btFuzzyZero( x ) ) ) || ( !( btScalar.btFuzzyZero( y ) ) ) )
{
m_solveSwingLimit = true;
if( m_swingSpan1 >= m_fixThresh )
{
z = (double)( 0 );
double span1 = btScalar.btAtan2( y, x );
if( span1 > m_swingSpan1 )
{
x = btScalar.btCos( m_swingSpan1 );
y = btScalar.btSin( m_swingSpan1 );
}
else if( span1 < -m_swingSpan1 )
{
x = btScalar.btCos( m_swingSpan1 );
y = -btScalar.btSin( m_swingSpan1 );
}
}
}
}
target.x = x * ivA[0] + y * jvA[0] + z * kvA[0];
target.y = x * ivA[1] + y * jvA[1] + z * kvA[1];
target.z = x * ivA[2] + y * jvA[2] + z * kvA[2];
target.w = 0;
target.normalize();
m_swingAxis = -ivB.cross( target );
m_swingCorrection = m_swingAxis.length();
if( !btScalar.btFuzzyZero( m_swingCorrection ) )
m_swingAxis.normalize();
}
}
if( m_twistSpan >= (double)( 0 ) )
{
btVector3 twistAxis;
computeTwistLimitInfo( ref qABTwist, out m_twistAngle, out twistAxis );
twistAxis.Invert( out twistAxis );
if( m_twistAngle > m_twistSpan * m_limitSoftness )
{
m_solveTwistLimit = true;
m_twistLimitRatio = 1;
if( m_twistAngle < m_twistSpan && m_limitSoftness < 1 - btScalar.SIMD_EPSILON )
{
m_twistLimitRatio = ( m_twistAngle - m_twistSpan * m_limitSoftness ) /
( m_twistSpan - m_twistSpan * m_limitSoftness );
}
// twist correction tries to get back to soft limit
m_twistCorrection = m_twistAngle - ( m_twistSpan * m_limitSoftness );
btQuaternion.quatRotate( ref qB, ref twistAxis, out m_twistAxis );
m_kTwist = btScalar.BT_ONE /
( computeAngularImpulseDenominator( ref m_twistAxis, invInertiaWorldA ) +
computeAngularImpulseDenominator( ref m_twistAxis, invInertiaWorldB ) );
}
if( m_solveSwingLimit )
{
btQuaternion.quatRotate( ref qA, ref twistAxis, out m_twistAxisA );
}
}
else
{
m_twistAngle = (double)( 0 );
}
}
}
public override void getAabbSlow( ref btTransform trans, out btVector3 minAabb, out btVector3 maxAabb )
{
//use localGetSupportingVertexWithoutMargin?
double margin = getMargin();
double[] max = new double[3];
double[] min = new double[3];
for( int i = 0; i < 3; i++ )
{
btVector3 vec = btVector3.Zero;
vec[i] = (double)( 1.0 );
btVector3 tmp1;
trans.m_basis.ApplyInverse( ref vec, out tmp1 );
btVector3 sv; localGetSupportingVertex( ref tmp1, out sv );
btVector3 tmp; trans.Apply( ref sv, out tmp );
max[i] = tmp[i] + margin;
vec[i] = (double)( -1.0 );
trans.m_basis.ApplyInverse( ref vec, out tmp1 );
localGetSupportingVertex( ref tmp1, out tmp );
trans.Apply( ref tmp, out tmp1 );
min[i] = tmp1[i] - margin;
}
maxAabb = new btVector3( max[0], max[1], max[2] );
minAabb = new btVector3( min[0], min[1], min[2] );
}
internal override void processCollision( btCollisionObjectWrapper body0Wrap
, ref btTransform body0Transform
, btCollisionObjectWrapper body1Wrap
, ref btTransform body1Transform
, btDispatcherInfo dispatchInfo, btManifoldResult resultOut )
{
btCollisionObjectWrapper col0ObjWrap = body0Wrap;
btCollisionObjectWrapper col1ObjWrap = body1Wrap;
Debug.Assert( col0ObjWrap.getCollisionShape().isCompound() );
Debug.Assert( col1ObjWrap.getCollisionShape().isCompound() );
btCompoundShape compoundShape0 = (btCompoundShape)( col0ObjWrap.getCollisionShape() );
btCompoundShape compoundShape1 = (btCompoundShape)( col1ObjWrap.getCollisionShape() );
btDbvt tree0 = compoundShape0.getDynamicAabbTree();
btDbvt tree1 = compoundShape1.getDynamicAabbTree();
if( tree0 == null || tree1 == null )
{
base.processCollision( body0Wrap, ref body0Transform, body1Wrap, ref body1Transform, dispatchInfo, resultOut );
return;
}
///btCompoundShape might have changed:
////make sure the internal child collision algorithm caches are still valid
if( ( compoundShape0.getUpdateRevision() != m_compoundShapeRevision0 ) || ( compoundShape1.getUpdateRevision() != m_compoundShapeRevision1 ) )
{
///clear all
removeChildAlgorithms();
m_compoundShapeRevision0 = compoundShape0.getUpdateRevision();
m_compoundShapeRevision1 = compoundShape1.getUpdateRevision();
}
///we need to refresh all contact manifolds
///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep
///so we should add a 'refreshManifolds' in the btCollisionAlgorithm
{
int i;
btManifoldArray manifoldArray = new btManifoldArray();
btSimplePairArray pairs = m_childCollisionAlgorithmCache.getOverlappingPairArray();
for( i = 0; i < pairs.Count; i++ )
{
if( pairs[i].m_userPointer != null )
{
btCollisionAlgorithm algo = (btCollisionAlgorithm)pairs[i].m_userPointer;
algo.getAllContactManifolds( manifoldArray );
for( int m = 0; m < manifoldArray.Count; m++ )
{
if( manifoldArray[m].m_cachedPoints != 0 )
{
resultOut.setPersistentManifold( manifoldArray[m] );
resultOut.refreshContactPoints();
resultOut.setPersistentManifold( null );
}
}
manifoldArray.Count =( 0 );
}
}
}
btCompoundCompoundLeafCallback callback = new btCompoundCompoundLeafCallback
( col0ObjWrap, col1ObjWrap,this.m_dispatcher, dispatchInfo, resultOut, this.m_childCollisionAlgorithmCache, m_sharedManifold);
btTransform xform; body0Transform.inverseTimes( ref body1Transform, out xform );
MycollideTT( tree0.m_root, tree1.m_root, ref xform, callback );
//Console.WriteLine("#compound-compound child/leaf overlap =%d \r",callback.m_numOverlapPairs);
//remove non-overlapping child pairs
{
Debug.Assert( m_removePairs.Count == 0 );
//iterate over all children, perform an AABB check inside ProcessChildShape
btSimplePairArray pairs = m_childCollisionAlgorithmCache.getOverlappingPairArray();
int i;
//btManifoldArray manifoldArray;
btVector3 aabbMin0, aabbMax0, aabbMin1, aabbMax1;
for( i = 0; i < pairs.Count; i++ )
{
if( pairs[i].m_userPointer != null )
{
btCollisionAlgorithm algo = (btCollisionAlgorithm)pairs[i].m_userPointer;
{
btCollisionShape childShape0 = null;
btTransform newChildWorldTrans0;
//btTransform orgInterpolationTrans0;
childShape0 = compoundShape0.getChildShape( pairs[i].m_indexA );
//orgInterpolationTrans0 = col0ObjWrap.m_worldTransform;
btTransform childTrans0 = compoundShape0.getChildTransform( pairs[i].m_indexA );
body0Transform.Apply( ref childTrans0, out newChildWorldTrans0 );
childShape0.getAabb( ref newChildWorldTrans0, out aabbMin0, out aabbMax0 );
}
{
btCollisionShape childShape1 = null;
btTransform newChildWorldTrans1;
childShape1 = compoundShape1.getChildShape( pairs[i].m_indexB );
btTransform childTrans1 = compoundShape1.getChildTransform( pairs[i].m_indexB );
body1Transform.Apply( ref childTrans1, out newChildWorldTrans1 );
childShape1.getAabb( ref newChildWorldTrans1, out aabbMin1, out aabbMax1 );
}
if( !btAabbUtil.TestAabbAgainstAabb2( ref aabbMin0, ref aabbMax0, ref aabbMin1, ref aabbMax1 ) )
{
//algo.~btCollisionAlgorithm();
m_dispatcher.freeCollisionAlgorithm( algo );
m_removePairs.Add( new btSimplePair( pairs[i].m_indexA, pairs[i].m_indexB ) );
}
}
}
for( i = 0; i < m_removePairs.Count; i++ )
{
m_childCollisionAlgorithmCache.removeOverlappingPair( m_removePairs[i].m_indexA, m_removePairs[i].m_indexB );
}
m_removePairs.Clear();
}
}
//
public static double SignedDistance( ref btVector3 position,
double margin,
btConvexShape shape0,
ref btTransform wtrs0,
sResults results )
{
tShape shape = new tShape();
using( btSphereShape shape1 = BulletGlobals.SphereShapePool.Get() )
{
shape1.Initialize( margin );
btTransform wtrs1 = new btTransform( ref btQuaternion.Zero, ref position );
Initialize( shape0, ref wtrs0, shape1, ref wtrs1, out results, shape, false );
GJK gjk = new GJK();
GJK.eStatus._ gjk_status = gjk.Evaluate( shape, ref btVector3.One );
if( gjk_status == GJK.eStatus._.Valid )
{
btVector3 w0 = btVector3.Zero;
btVector3 w1 = btVector3.Zero;
for( uint i = 0; i < gjk.m_simplex.rank; ++i )
{
double p = gjk.m_simplex.p[i];
btVector3 tmp;
shape.Support( ref gjk.m_simplex.c[i].d, 0, out tmp );
w0.AddScale( ref tmp, p, out w0 );
btVector3 tmp2;
gjk.m_simplex.c[i].d.Invert( out tmp2 );
shape.Support( ref tmp2, 1, out tmp );
w1.AddScale( ref tmp, p, out w1 );
}
wtrs0.Apply( ref w0, out results.witness0 );
wtrs0.Apply( ref w1, out results.witness1 );
btVector3 delta; results.witness1.Sub( ref results.witness0, out delta );
margin = shape0.getMarginNonVirtual() +
shape1.getMarginNonVirtual();
double length = delta.length();
delta.Div( length, out results.normal );
results.witness0.AddScale( ref results.normal, margin, out results.witness0 );
return ( length - margin );
}
else
{
if( gjk_status == GJK.eStatus._.Inside )
{
if( Penetration( shape0, ref wtrs0, shape1, ref wtrs1, ref gjk.m_ray, out results ) )
{
btVector3 delta; results.witness0.Sub(
ref results.witness1, out delta );
double length = delta.length();
if( length >= btScalar.SIMD_EPSILON )
delta.Div( length, out results.normal );
return ( -length );
}
}
}
}
return ( btScalar.SIMD_INFINITY );
}
internal void calculateTransforms( ref btTransform transA, ref btTransform transB )
{
if( m_useLinearReferenceFrameA /*|| ( !m_useSolveConstraintObsolete )*/ )
{
transA.Apply( ref m_frameInA, out m_calculatedTransformA );
transB.Apply( ref m_frameInB, out m_calculatedTransformB );
}
else
{
transA.Apply( ref m_frameInA, out m_calculatedTransformB );
transB.Apply( ref m_frameInB, out m_calculatedTransformA );
}
m_realPivotAInW = m_calculatedTransformA.m_origin;
m_realPivotBInW = m_calculatedTransformB.m_origin;
m_sliderAxis = m_calculatedTransformA.m_basis.getColumn( 0 ); // along X
/*
if( m_useLinearReferenceFrameA || m_useSolveConstraintObsolete )
{
m_delta = m_realPivotBInW - m_realPivotAInW;
}
else
*/
{
m_delta = m_realPivotAInW - m_realPivotBInW;
}
m_realPivotAInW.AddScale( ref m_sliderAxis, m_sliderAxis.dot( ref m_delta ), out m_projPivotInW );
//m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot( ref m_delta ) * m_sliderAxis;
btVector3 normalWorld;
int i;
//linear part
for( i = 0; i < 3; i++ )
{
normalWorld = m_calculatedTransformA.m_basis.getColumn( i );
m_depth[i] = m_delta.dot( normalWorld );
}
}
//
public static bool Penetration( btConvexShape shape0,
ref btTransform wtrs0,
btConvexShape shape1,
ref btTransform wtrs1,
ref btVector3 guess,
out sResults results,
bool usemargins = false )
{
tShape shape = new tShape();
Initialize( shape0, ref wtrs0, shape1, ref wtrs1, out results, shape, usemargins );
GJK gjk = new GJK();
btVector3 tmp;
guess.Invert( out tmp );
GJK.eStatus._ gjk_status = gjk.Evaluate( shape, ref tmp );
switch( gjk_status )
{
case GJK.eStatus._.Inside:
{
EPA epa = new EPA();
EPA.eStatus._ epa_status = epa.Evaluate( gjk, ref tmp );
if( epa_status != EPA.eStatus._.Failed )
{
btVector3 w0 = btVector3.Zero;
for( uint i = 0; i < epa.m_result.rank; ++i )
{
shape.Support( ref epa.m_result.c[i].d, 0, out tmp );
w0.AddScale( ref tmp, epa.m_result.p[i], out w0 );
}
results.status = sResults.eStatus.Penetrating;
wtrs0.Apply( ref w0, out results.witness0 );
w0.SubScale( ref epa.m_normal, epa.m_depth, out tmp );
wtrs0.Apply( ref tmp, out results.witness1 );
epa.m_normal.Invert( out results.normal );
results.distance = -epa.m_depth;
return ( true );
}
else results.status = sResults.eStatus.EPA_Failed;
}
break;
case GJK.eStatus._.Failed:
results.status = sResults.eStatus.GJK_Failed;
break;
default:
break;
}
return ( false );
}
///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
public override void getAabb( ref btTransform trans, out btVector3 aabbMin, out btVector3 aabbMax )
{
btVector3 localHalfExtents; btVector3.getHalfExtent( ref m_localAabbMin, ref m_localAabbMax, out localHalfExtents );
btVector3 localCenter; btVector3.getCenter( ref m_localAabbMin, ref m_localAabbMax, out localCenter );
//avoid an illegal AABB when there are no children
if( m_children.Count == 0 )
{
localHalfExtents.setValue( 0, 0, 0 );
localCenter.setValue( 0, 0, 0 );
}
btVector3 margin = new btVector3( getMargin() );
localHalfExtents.Add( ref margin, out localHalfExtents );
btMatrix3x3 abs_b; trans.m_basis.absolute( out abs_b );
btVector3 center; trans.Apply( ref localCenter, out center );
btVector3 extent; localHalfExtents.dot3( ref abs_b, out extent );
aabbMin = center - extent;
aabbMax = center + extent;
}
//! Calcs global transform of the offsets
/*!
Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies.
\sa btGeneric6DofConstraint.getCalculatedTransformA , btGeneric6DofConstraint.getCalculatedTransformB, btGeneric6DofConstraint.calculateAngleInfo
*/
internal void calculateTransforms( ref btTransform transA, ref btTransform transB )
{
transA.Apply( ref m_frameInA, out m_calculatedTransformA );
transB.Apply( ref m_frameInB, out m_calculatedTransformB );
calculateLinearInfo();
calculateAngleInfo();
if( m_useOffsetForConstraintFrame )
{ // get weight factors depending on masses
double miA = getRigidBodyA().getInvMass();
double miB = getRigidBodyB().getInvMass();
m_hasStaticBody = ( miA < btScalar.SIMD_EPSILON ) || ( miB < btScalar.SIMD_EPSILON );
double miS = miA + miB;
if( miS > (double)( 0 ) )
{
m_factA = miB / miS;
}
else
{
m_factA = (double)( 0.5f );
}
m_factB = (double)( 1.0f ) - m_factA;
}
}
//
// Convex-Convex collision algorithm
//
internal override void processCollision( btCollisionObjectWrapper body0Wrap
, ref btTransform body0Transform
, btCollisionObjectWrapper body1Wrap
, ref btTransform body1Transform
, btDispatcherInfo dispatchInfo, btManifoldResult resultOut )
{
if( m_manifoldPtr == null )
{
//swapped?
m_manifoldPtr = m_dispatcher.getNewManifold( body0Wrap.m_collisionObject, body1Wrap.m_collisionObject );
m_ownManifold = true;
}
resultOut.setPersistentManifold( m_manifoldPtr );
//comment-out next line to test multi-contact generation
//resultOut.getPersistentManifold().clearManifold();
btConvexShape min0 = (btConvexShape)body0Wrap.getCollisionShape();
btConvexShape min1 = (btConvexShape)body1Wrap.getCollisionShape();
btVector3 normalOnB;
btVector3 pointOnBWorld;
#if !BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
if( ( min0.getShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE )
&& ( min1.getShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE ) )
{
btCapsuleShape capsuleA = (btCapsuleShape)min0;
btCapsuleShape capsuleB = (btCapsuleShape)min1;
// btVector3 localScalingA = capsuleA.getLocalScaling();
// btVector3 localScalingB = capsuleB.getLocalScaling();
double threshold = m_manifoldPtr.getContactBreakingThreshold();
double dist = capsuleCapsuleDistance( out normalOnB, out pointOnBWorld
, capsuleA.getHalfHeight(), capsuleA.getRadius()
, capsuleB.getHalfHeight(), capsuleB.getRadius()
, capsuleA.getUpAxis(), capsuleB.getUpAxis()
, ref body0Wrap.m_collisionObject.m_worldTransform, ref body1Wrap.m_collisionObject.m_worldTransform, threshold );
if( dist < threshold )
{
Debug.Assert( normalOnB.length2() >= ( btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) );
resultOut.addContactPoint( ref normalOnB, ref pointOnBWorld, dist );
}
resultOut.refreshContactPoints();
return;
}
#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
m_sepDistance.updateSeparatingDistance(body0.getWorldTransform(),body1.getWorldTransform());
}
if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0)
#endif //USE_SEPDISTANCE_UTIL2
{
btGjkPairDetector.ClosestPointInput input = BulletGlobals.ClosestPointInputPool.Get();
input.Initialize();
btGjkPairDetector gjkPairDetector = BulletGlobals.GjkPairDetectorPool.Get();
gjkPairDetector.Initialize( min0, min1, m_simplexSolver, m_pdSolver );
//TODO: if (dispatchInfo.m_useContinuous)
gjkPairDetector.setMinkowskiA( min0 );
gjkPairDetector.setMinkowskiB( min1 );
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
input.m_maximumDistanceSquared = BT_LARGE_FLOAT;
} else
#endif //USE_SEPDISTANCE_UTIL2
{
//if (dispatchInfo.m_convexMaxDistanceUseCPT)
//{
// input.m_maximumDistanceSquared = min0.getMargin() + min1.getMargin() + m_manifoldPtr.getContactProcessingThreshold();
//} else
//{
input.m_maximumDistanceSquared = min0.getMargin() + min1.getMargin() + m_manifoldPtr.getContactBreakingThreshold();
// }
input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared;
}
input.m_transformA = body0Transform;
input.m_transformB = body1Transform;
#if USE_SEPDISTANCE_UTIL2
double sepDist = 0;
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
sepDist = gjkPairDetector.getCachedSeparatingDistance();
if (sepDist>SIMD_EPSILON)
{
sepDist += dispatchInfo.m_convexConservativeDistanceThreshold;
//now perturbe directions to get multiple contact points
}
}
#endif //USE_SEPDISTANCE_UTIL2
if( min0.isPolyhedral() && min1.isPolyhedral() )
{
btDummyResult dummy = new btDummyResult();
///btBoxShape is an exception: its vertices are created WITH margin so don't subtract it
double min0Margin = min0.getShapeType() == BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE ? 0 : min0.getMargin();
double min1Margin = min1.getShapeType() == BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE ? 0 : min1.getMargin();
btWithoutMarginResult withoutMargin = new btWithoutMarginResult( resultOut, min0Margin, min1Margin );
btPolyhedralConvexShape polyhedronA = (btPolyhedralConvexShape)min0;
btPolyhedralConvexShape polyhedronB = (btPolyhedralConvexShape)min1;
if( polyhedronA.getConvexPolyhedron() != null && polyhedronB.getConvexPolyhedron() != null )
{
double threshold = m_manifoldPtr.getContactBreakingThreshold();
double minDist = -1e30f;
btVector3 sepNormalWorldSpace;
bool foundSepAxis = true;
if( dispatchInfo.m_enableSatConvex )
{
foundSepAxis = btPolyhedralContactClipping.findSeparatingAxis(
polyhedronA.getConvexPolyhedron(), polyhedronB.getConvexPolyhedron(),
ref body0Wrap.m_collisionObject.m_worldTransform,
ref body1Wrap.m_collisionObject.m_worldTransform,
out sepNormalWorldSpace, resultOut );
}
else
{
#if ZERO_MARGIN
gjkPairDetector.setIgnoreMargin(true);
gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
#else
gjkPairDetector.getClosestPoints( input, withoutMargin, dispatchInfo.m_debugDraw );
//gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
#endif //ZERO_MARGIN
//double l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
//if (l2>SIMD_EPSILON)
{
sepNormalWorldSpace = withoutMargin.m_reportedNormalOnWorld;//gjkPairDetector.getCachedSeparatingAxis()*(1/l2);
//minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance();
minDist = withoutMargin.m_reportedDistance;//gjkPairDetector.getCachedSeparatingDistance()+min0.getMargin()+min1.getMargin();
#if ZERO_MARGIN
foundSepAxis = true;//gjkPairDetector.getCachedSeparatingDistance()<0;
#else
foundSepAxis = withoutMargin.m_foundResult && minDist < 0;//-(min0.getMargin()+min1.getMargin());
#endif
}
}
if( foundSepAxis )
{
// Console.WriteLine("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.x,sepNormalWorldSpace.y,sepNormalWorldSpace.z);
btPolyhedralContactClipping.clipHullAgainstHull( ref sepNormalWorldSpace, polyhedronA.getConvexPolyhedron(), polyhedronB.getConvexPolyhedron(),
ref body0Wrap.m_collisionObject.m_worldTransform,
ref body1Wrap.m_collisionObject.m_worldTransform
, minDist - threshold, threshold, resultOut );
}
if( m_ownManifold )
{
resultOut.refreshContactPoints();
}
BulletGlobals.ClosestPointInputPool.Free( input );
BulletGlobals.GjkPairDetectorPool.Free( gjkPairDetector );
return;
}
else
{
//we can also deal with convex versus triangle (without connectivity data)
if( polyhedronA.getConvexPolyhedron() != null && polyhedronB.getShapeType() == BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE )
{
btVertexArray vertices = new btVertexArray();
btTriangleShape tri = (btTriangleShape)polyhedronB;
btVector3 tmp;
body1Transform.Apply( ref tri.m_vertices1, out tmp );
vertices.Add( ref tmp );
body1Transform.Apply( ref tri.m_vertices2, out tmp );
vertices.Add( ref tmp );
body1Transform.Apply( ref tri.m_vertices3, out tmp );
vertices.Add( ref tmp );
//tri.initializePolyhedralFeatures();
double threshold = m_manifoldPtr.getContactBreakingThreshold();
btVector3 sepNormalWorldSpace;
double minDist = -btScalar.BT_LARGE_FLOAT;
double maxDist = threshold;
bool foundSepAxis = false;
if( false )
{
polyhedronB.initializePolyhedralFeatures();
foundSepAxis = btPolyhedralContactClipping.findSeparatingAxis(
polyhedronA.getConvexPolyhedron(), polyhedronB.getConvexPolyhedron(),
ref body0Wrap.m_collisionObject.m_worldTransform,
ref body1Wrap.m_collisionObject.m_worldTransform,
out sepNormalWorldSpace, resultOut );
// Console.WriteLine("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.x,sepNormalWorldSpace.y,sepNormalWorldSpace.z);
btPolyhedralContactClipping.clipFaceAgainstHull( ref sepNormalWorldSpace
, polyhedronA.getConvexPolyhedron(),
ref body0Wrap.m_collisionObject.m_worldTransform, vertices, minDist - threshold, maxDist, resultOut );
}
else
{
#if ZERO_MARGIN
gjkPairDetector.setIgnoreMargin(true);
gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
#else
gjkPairDetector.getClosestPoints( input, dummy, dispatchInfo.m_debugDraw );
#endif//ZERO_MARGIN
double l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
if( l2 > btScalar.SIMD_EPSILON )
{
gjkPairDetector.getCachedSeparatingAxis().Mult( ( 1 / l2 ), out sepNormalWorldSpace );
//minDist = gjkPairDetector.getCachedSeparatingDistance();
//maxDist = threshold;
minDist = gjkPairDetector.getCachedSeparatingDistance() - min0.getMargin() - min1.getMargin();
//foundSepAxis = true;
btPolyhedralContactClipping.clipFaceAgainstHull( ref sepNormalWorldSpace
, polyhedronA.getConvexPolyhedron(),
ref body0Wrap.m_collisionObject.m_worldTransform, vertices, minDist - threshold, maxDist, resultOut );
}
else
{
//sepNormalWorldSpace = btVector3.Zero;
}
}
if( m_ownManifold )
{
resultOut.refreshContactPoints();
}
BulletGlobals.ClosestPointInputPool.Free( input );
BulletGlobals.GjkPairDetectorPool.Free( gjkPairDetector );
return;
}
}
}
gjkPairDetector.getClosestPoints( input, resultOut, dispatchInfo.m_debugDraw );
//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
//perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
if( m_numPerturbationIterations != 0
&& resultOut.m_manifoldPtr.m_cachedPoints < m_minimumPointsPerturbationThreshold )
{
int i;
btVector3 v0, v1;
btVector3 sepNormalWorldSpace;
double l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
if( l2 > btScalar.SIMD_EPSILON )
{
gjkPairDetector.getCachedSeparatingAxis().Mult( ( 1 / l2 ), out sepNormalWorldSpace );
btVector3.btPlaneSpace1( ref sepNormalWorldSpace, out v0, out v1 );
bool perturbeA = true;
double angleLimit = 0.125f * btScalar.SIMD_PI;
double perturbeAngle;
double radiusA = min0.getAngularMotionDisc();
double radiusB = min1.getAngularMotionDisc();
if( radiusA < radiusB )
{
perturbeAngle = btPersistentManifold.gContactBreakingThreshold / radiusA;
perturbeA = true;
}
else
{
perturbeAngle = btPersistentManifold.gContactBreakingThreshold / radiusB;
perturbeA = false;
}
if( perturbeAngle > angleLimit )
perturbeAngle = angleLimit;
btTransform unPerturbedTransform;
if( perturbeA )
{
unPerturbedTransform = input.m_transformA;
}
else
{
unPerturbedTransform = input.m_transformB;
}
for( i = 0; i < m_numPerturbationIterations; i++ )
{
if( v0.length2() > btScalar.SIMD_EPSILON )
{
btQuaternion perturbeRot = new btQuaternion( ref v0, perturbeAngle );
double iterationAngle = i * ( btScalar.SIMD_2_PI / (double)( m_numPerturbationIterations ) );
btQuaternion rotq = new btQuaternion( ref sepNormalWorldSpace, iterationAngle );
if( perturbeA )
{
btQuaternion tmpq;
btQuaternion tmpq2;
rotq.inverse( out tmpq );
btQuaternion.Mult( ref tmpq, ref perturbeRot, out tmpq2 );
btQuaternion.Mult( ref tmpq2, ref rotq, out tmpq );
btMatrix3x3 m = new btMatrix3x3( ref tmpq );
btMatrix3x3 m2; body0Transform.getBasis( out m2 );
btMatrix3x3 m3;
btMatrix3x3.Mult( ref m, ref m2, out m3 );
input.m_transformA.setBasis( ref m3 );
input.m_transformB = body1Transform;
#if DEBUG_CONTACTS
dispatchInfo.m_debugDraw.drawTransform(input.m_transformA,10.0);
#endif //DEBUG_CONTACTS
}
else
{
btQuaternion tmpq;
btQuaternion tmpq2;
rotq.inverse( out tmpq );
btQuaternion.Mult( ref tmpq, ref perturbeRot, out tmpq2 );
btQuaternion.Mult( ref tmpq2, ref rotq, out tmpq );
btMatrix3x3 m = new btMatrix3x3( ref tmpq );
btMatrix3x3 m2; body1Transform.getBasis( out m2 );
btMatrix3x3 m3;
btMatrix3x3.Mult( ref m, ref m2, out m3 );
input.m_transformA = body0Transform;
input.m_transformB.setBasis( ref m3 );
#if DEBUG_CONTACTS
dispatchInfo.m_debugDraw.drawTransform(input.m_transformB,10.0);
#endif
}
btPerturbedContactResult perturbedResultOut = BulletGlobals.PerturbedContactResultPool.Get();
if( perturbeA )
perturbedResultOut.Initialize( resultOut
, ref input.m_transformA, ref input.m_transformB
, ref input.m_transformA
, perturbeA
, dispatchInfo.m_debugDraw );
else
perturbedResultOut.Initialize( resultOut
, ref input.m_transformA, ref input.m_transformB
, ref input.m_transformB
, perturbeA
, dispatchInfo.m_debugDraw );
gjkPairDetector.getClosestPoints( input, perturbedResultOut, dispatchInfo.m_debugDraw );
BulletGlobals.PerturbedContactResultPool.Free( perturbedResultOut );
}
}
}
}
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON))
{
m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0.getWorldTransform(),body1.getWorldTransform());
}
#endif //USE_SEPDISTANCE_UTIL2
BulletGlobals.ClosestPointInputPool.Free( input );
BulletGlobals.GjkPairDetectorPool.Free( gjkPairDetector );
}
if( m_ownManifold )
{
resultOut.refreshContactPoints();
}
}
void calculateTransforms( ref btTransform transA, ref btTransform transB )
{
transA.Apply( ref m_frameInA, out m_calculatedTransformA );
transB.Apply( ref m_frameInB, out m_calculatedTransformB );
calculateLinearInfo();
calculateAngleInfo();
double miA = getRigidBodyA().getInvMass();
double miB = getRigidBodyB().getInvMass();
m_hasStaticBody = ( miA < btScalar.SIMD_EPSILON ) || ( miB < btScalar.SIMD_EPSILON );
double miS = miA + miB;
if( miS > (double)( 0 ) )
{
m_factA = miB / miS;
}
else
{
m_factA = (double)( 0.5f );
}
m_factB = (double)( 1.0f ) - m_factA;
}
public override void project( ref btTransform trans, ref btVector3 dir, ref double minProj, ref double maxProj, out btVector3 witnesPtMin, out btVector3 witnesPtMax )
{
minProj = btScalar.BT_MAX_FLOAT;
maxProj = btScalar.BT_MIN_FLOAT;
witnesPtMax = witnesPtMin = btVector3.Zero;
int numVerts = m_unscaledPoints.Count;
for( int i = 0; i < numVerts; i++ )
{
btVector3 vtx; m_unscaledPoints[i].Mult(ref m_localScaling, out vtx );
btVector3 pt; trans.Apply( ref vtx, out pt );
double dp = pt.dot( ref dir );
if( dp < minProj )
{
minProj = dp;
witnesPtMin = pt;
}
if( dp > maxProj )
{
maxProj = dp;
witnesPtMax = pt;
}
}
if( minProj > maxProj )
{
btScalar.btSwap( ref minProj, ref maxProj );
btScalar.btSwap( ref witnesPtMin, ref witnesPtMax );
}
}
