internal int get_limit_motor_info2(
btRotationalLimitMotor limot,
ref btTransform transA, ref btTransform transB, ref btVector3 linVelA, ref btVector3 linVelB, ref btVector3 angVelA, ref btVector3 angVelB,
ref btConstraintInfo2 info, int row, ref btVector3 ax1, bool rotational, bool rotAllowed = false )
{
//int srow = row * info.rowskip;
bool powered = limot.m_enableMotor;
int limit = limot.m_currentLimit;
if( powered || limit != 0 )
{ // if the joint is powered, or has joint limits, add in the extra row
if( rotational )
{
info.m_solverConstraints[row].m_relpos1CrossNormal = ax1;
ax1.Invert( out info.m_solverConstraints[row].m_relpos2CrossNormal );
}
else
{
info.m_solverConstraints[row].m_contactNormal1 = ax1;
ax1.Invert( out info.m_solverConstraints[row].m_contactNormal2 );
}
/*
double* J1 = rotational ? info.m_J1angularAxis : info.m_J1linearAxis;
double* J2 = rotational ? info.m_J2angularAxis : info.m_J2linearAxis;
J1[srow + 0] = ax1[0];
J1[srow + 1] = ax1[1];
J1[srow + 2] = ax1[2];
J2[srow + 0] = -ax1[0];
J2[srow + 1] = -ax1[1];
J2[srow + 2] = -ax1[2];
*/
if( ( !rotational ) )
{
if( m_useOffsetForConstraintFrame )
{
btVector3 tmpA, tmpB, relA, relB;
// get vector from bodyB to frameB in WCS
m_calculatedTransformB.m_origin.Sub( ref transB.m_origin, out relB );
// get its projection to constraint axis
btVector3 projB = ax1 * relB.dot( ax1 );
// get vector directed from bodyB to constraint axis (and orthogonal to it)
btVector3 orthoB = relB - projB;
// same for bodyA
m_calculatedTransformA.m_origin.Sub( ref transA.m_origin, out relA );
btVector3 projA = ax1 * relA.dot( ax1 );
btVector3 orthoA = relA - projA;
// get desired offset between frames A and B along constraint axis
double desiredOffs = limot.m_currentPosition - limot.m_currentLimitError;
// desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis
btVector3 totalDist = projA + ax1 * desiredOffs - projB;
// get offset vectors relA and relB
relA = orthoA + totalDist * m_factA;
relB = orthoB - totalDist * m_factB;
tmpA = relA.cross( ax1 );
tmpB = relB.cross( ax1 );
if( m_hasStaticBody && ( !rotAllowed ) )
{
tmpA *= m_factA;
tmpB *= m_factB;
}
//int i;
info.m_solverConstraints[row].m_relpos1CrossNormal = tmpA;
tmpB.Invert( out info.m_solverConstraints[row].m_relpos2CrossNormal );
//for( i = 0; i < 3; i++ ) info.m_J1angularAxis[srow + i] = tmpA[i];
//for( i = 0; i < 3; i++ ) info.m_J2angularAxis[srow + i] = -tmpB[i];
}
else
{
btVector3 ltd; // Linear Torque Decoupling vector
btVector3 c = m_calculatedTransformB.m_origin - transA.m_origin;
ltd = c.cross( ax1 );
info.m_solverConstraints[row].m_relpos1CrossNormal = ltd;
//info.m_J1angularAxis[srow + 0] = ltd[0];
//info.m_J1angularAxis[srow + 1] = ltd[1];
//info.m_J1angularAxis[srow + 2] = ltd[2];
c = m_calculatedTransformB.m_origin - transB.m_origin;
ltd = -c.cross( ax1 );
info.m_solverConstraints[row].m_relpos2CrossNormal = ltd;
//info.m_J2angularAxis[srow + 0] = ltd[0];
//info.m_J2angularAxis[srow + 1] = ltd[1];
//info.m_J2angularAxis[srow + 2] = ltd[2];
}
}
// if we're limited low and high simultaneously, the joint motor is
// ineffective
if( limit != 0 && ( limot.m_loLimit == limot.m_hiLimit ) ) powered = false;
info.m_solverConstraints[row].m_rhs = (double)( 0 );
if( powered )
{
info.m_solverConstraints[row].m_cfm = limot.m_normalCFM;
if( limit == 0 )
{
double tag_vel = rotational ? limot.m_targetVelocity : -limot.m_targetVelocity;
double mot_fact = getMotorFactor( limot.m_currentPosition,
limot.m_loLimit,
limot.m_hiLimit,
tag_vel,
info.fps * limot.m_stopERP );
info.m_solverConstraints[row].m_rhs += mot_fact * limot.m_targetVelocity;
info.m_solverConstraints[row].m_lowerLimit = -limot.m_maxMotorForce;
info.m_solverConstraints[row].m_upperLimit = limot.m_maxMotorForce;
}
}
if( limit != 0 )
{
double k = info.fps * limot.m_stopERP;
if( !rotational )
{
info.m_solverConstraints[row].m_rhs += k * limot.m_currentLimitError;
}
else
{
info.m_solverConstraints[row].m_rhs += -k * limot.m_currentLimitError;
}
info.m_solverConstraints[row].m_cfm = limot.m_stopCFM;
if( limot.m_loLimit == limot.m_hiLimit )
{ // limited low and high simultaneously
info.m_solverConstraints[row].m_lowerLimit = btScalar.BT_MIN_FLOAT;
info.m_solverConstraints[row].m_upperLimit = btScalar.BT_MAX_FLOAT;
}
else
{
if( limit == 1 )
{
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;
}
// deal with bounce
if( limot.m_bounce > 0 )
{
// calculate joint velocity
double vel;
if( rotational )
{
vel = angVelA.dot( ax1 );
//make sure that if no body . angVelB == zero vec
// if (body1)
vel -= angVelB.dot( ax1 );
}
else
{
vel = linVelA.dot( ax1 );
//make sure that if no body . angVelB == zero vec
// if (body1)
vel -= linVelB.dot( ax1 );
}
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if( limit == 1 )
{
if( vel < 0 )
{
double newc = -limot.m_bounce * vel;
if( newc > info.m_solverConstraints[row].m_rhs )
info.m_solverConstraints[row].m_rhs = newc;
}
}
else
{
if( vel > 0 )
{
double newc = -limot.m_bounce * vel;
if( newc < info.m_solverConstraints[row].m_rhs )
info.m_solverConstraints[row].m_rhs = newc;
}
}
}
}
}
return 1;
}
else return 0;
}
void calculateJacobi( btRotationalLimitMotor2 limot, ref btTransform transA, ref btTransform transB, btConstraintInfo2 info, int row, ref btVector3 ax1, bool rotational, bool rotAllowed )
{
if( rotational )
{
info.m_solverConstraints[row].m_relpos1CrossNormal = ax1;
ax1.Invert( out info.m_solverConstraints[row].m_relpos2CrossNormal );
}
else
{
info.m_solverConstraints[row].m_contactNormal1 = ax1;
ax1.Invert( out info.m_solverConstraints[row].m_contactNormal2 );
}
if( !rotational )
{
btVector3 tmpA, tmpB, relA, relB;
// get vector from bodyB to frameB in WCS
m_calculatedTransformB.m_origin.Sub( ref transB.m_origin, out relB );
// same for bodyA
m_calculatedTransformA.m_origin.Sub( ref transA.m_origin, out relA ) ;
relA.cross( ref ax1, out tmpA );
relB.cross( ref ax1, out tmpB );
if( m_hasStaticBody && ( !rotAllowed ) )
{
tmpA *= m_factA;
tmpB *= m_factB;
}
//int i;
info.m_solverConstraints[row].m_relpos1CrossNormal = tmpA;
tmpB.Invert( out info.m_solverConstraints[row].m_relpos2CrossNormal );
}
}
internal void setupRollingFrictionConstraint( btSolverConstraint solverConstraint, ref btVector3 normalAxis1
, btSolverBody solverBodyA, btSolverBody solverBodyB,
btManifoldPoint cp, ref btVector3 rel_pos1, ref btVector3 rel_pos2,
btCollisionObject colObj0, btCollisionObject colObj1, double relaxation,
double desiredVelocity = 0, double cfmSlip = 0.0 )
{
btVector3 normalAxis = btVector3.Zero;
solverConstraint.m_contactNormal1 = normalAxis;
normalAxis.Invert( out solverConstraint.m_contactNormal2 );
//btSolverBody solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
//btSolverBody solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
btRigidBody body0 = solverBodyA.m_originalBody;
btRigidBody body1 = solverBodyB.m_originalBody;
solverConstraint.m_solverBodyA = solverBodyA;
solverConstraint.m_solverBodyB = solverBodyB;
solverConstraint.m_friction = cp.m_combinedRollingFriction;
solverConstraint.m_originalContactPoint = null;
solverConstraint.m_appliedImpulse = 0;
solverConstraint.m_appliedPushImpulse = 0;
btVector3 iMJaA;
btVector3 iMJaB;
{
btVector3 ftorqueAxis1; normalAxis1.Invert( out ftorqueAxis1 );
solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
if( body0 != null )
{
body0.m_invInertiaTensorWorld.Apply( ref ftorqueAxis1, out iMJaA );
iMJaA.Mult( ref body0.m_angularFactor, out solverConstraint.m_angularComponentA );
}
else
iMJaA = btVector3.Zero;
//solverConstraint.m_angularComponentA = body0 != null ? body0.getInvInertiaTensorWorld() * ftorqueAxis1 * body0.getAngularFactor() : btVector3.Zero;
}
{
btVector3 ftorqueAxis1 = normalAxis1;
solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
if( body1 != null )
{
body1.m_invInertiaTensorWorld.Apply( ref ftorqueAxis1, out iMJaB );
iMJaB.Mult( ref body1.m_angularFactor, out solverConstraint.m_angularComponentB );
}
else
iMJaB = btVector3.Zero;
//solverConstraint.m_angularComponentB = body1 != null ? body1.getInvInertiaTensorWorld() * ftorqueAxis1 * body1.getAngularFactor() : btVector3.Zero;
}
{
//btVector3 iMJaA = body0 != null ? body0.getInvInertiaTensorWorld() * solverConstraint.m_relpos1CrossNormal : btVector3.Zero;
//btVector3 iMJaB = body1 != null ? body1.getInvInertiaTensorWorld() * solverConstraint.m_relpos2CrossNormal : btVector3.Zero;
double sum = 0;
sum += iMJaA.dot( ref solverConstraint.m_relpos1CrossNormal );
sum += iMJaB.dot( ref solverConstraint.m_relpos2CrossNormal );
btScalar.Dbg( "m_jacDiagABInv 2 set to " + ( btScalar.BT_ONE / sum ).ToString( "g17" ) );
solverConstraint.m_jacDiagABInv = btScalar.BT_ONE / sum;
}
{
double rel_vel;
double vel1Dotn;
double vel2Dotn;
//double vel1Dotn = solverConstraint.m_contactNormal1.dot( body0 != null ? solverBodyA.m_linearVelocity + solverBodyA.m_externalForceImpulse : btVector3.Zero )
// + solverConstraint.m_relpos1CrossNormal.dot( body0 != null ? solverBodyA.m_angularVelocity : btVector3.Zero );
//double vel2Dotn = solverConstraint.m_contactNormal2.dot( body1 != null ? solverBodyB.m_linearVelocity + solverBodyB.m_externalForceImpulse : btVector3.Zero )
// + solverConstraint.m_relpos2CrossNormal.dot( body1 != null ? solverBodyB.m_angularVelocity : btVector3.Zero );
if( body0 != null )
vel1Dotn = solverConstraint.m_contactNormal1.dotAdded( ref solverBodyA.m_linearVelocity, ref solverBodyA.m_externalForceImpulse )
+ solverConstraint.m_relpos1CrossNormal.dot( ref solverBodyA.m_angularVelocity );
else
vel1Dotn = 0;
if( body1 != null )
vel2Dotn = solverConstraint.m_contactNormal1.dotAdded( ref solverBodyB.m_linearVelocity, ref solverBodyB.m_externalForceImpulse )
+ solverConstraint.m_relpos1CrossNormal.dot( ref solverBodyB.m_angularVelocity );
else
vel2Dotn = 0;
rel_vel = vel1Dotn + vel2Dotn;
// double positionalError = 0;
double velocityError = desiredVelocity - rel_vel;
double velocityImpulse = velocityError * ( solverConstraint.m_jacDiagABInv );
solverConstraint.m_rhs = velocityImpulse;
btScalar.Dbg( "Constraint 2 m_rhs " + solverConstraint.m_rhs.ToString( "g17" ) );
solverConstraint.m_cfm = cfmSlip;
solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
solverConstraint.m_upperLimit = solverConstraint.m_friction;
}
}
public override void getPreferredPenetrationDirection( int index, out btVector3 penetrationVector )
{
calcNormal( out penetrationVector );
if( index > 0 )
penetrationVector.Invert( out penetrationVector );
}
internal void setupFrictionConstraint( btSolverConstraint solverConstraint, ref btVector3 normalAxis
//, int solverBodyIdA, int solverBodyIdB
, btSolverBody solverBodyA, btSolverBody solverBodyB
, btManifoldPoint cp, ref btVector3 rel_pos1, ref btVector3 rel_pos2, btCollisionObject colObj0, btCollisionObject colObj1, double relaxation, double desiredVelocity = 0, double cfmSlip = 0.0 )
{
//btSolverBody solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
//btSolverBody solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
btRigidBody body0 = solverBodyA.m_originalBody;
btRigidBody body1 = solverBodyB.m_originalBody;
solverConstraint.m_solverBodyA = solverBodyA;
solverConstraint.m_solverBodyB = solverBodyB;
solverConstraint.m_friction = cp.m_combinedFriction;
solverConstraint.m_originalContactPoint = null;
solverConstraint.m_appliedImpulse = 0;
solverConstraint.m_appliedPushImpulse = 0;
if( body0 != null )
{
solverConstraint.m_contactNormal1 = normalAxis;
rel_pos1.cross( ref solverConstraint.m_contactNormal1, out solverConstraint.m_relpos1CrossNormal );
btVector3 tmp;
body0.m_invInertiaTensorWorld.Apply( ref solverConstraint.m_relpos1CrossNormal, out tmp );
tmp.Mult( ref body0.m_angularFactor, out solverConstraint.m_angularComponentA );
}
else
{
solverConstraint.m_contactNormal1.setZero();
solverConstraint.m_relpos1CrossNormal.setZero();
solverConstraint.m_angularComponentA.setZero();
}
if( body1 != null )
{
normalAxis.Invert( out solverConstraint.m_contactNormal2 );
rel_pos2.cross( ref solverConstraint.m_contactNormal2, out solverConstraint.m_relpos2CrossNormal );
btVector3 tmp;
body1.m_invInertiaTensorWorld.Apply( ref solverConstraint.m_relpos2CrossNormal, out tmp );
tmp.Mult( ref body1.m_angularFactor, out solverConstraint.m_angularComponentB );
}
else
{
solverConstraint.m_contactNormal2 = btVector3.Zero;
solverConstraint.m_relpos2CrossNormal = btVector3.Zero;
solverConstraint.m_angularComponentB = btVector3.Zero;
}
{
btVector3 vec;
double denom0 = 0;
double denom1 = 0;
if( body0 != null )
{
solverConstraint.m_angularComponentA.cross( ref rel_pos1, out vec );
denom0 = body0.getInvMass() + normalAxis.dot( ref vec );
}
if( body1 != null )
{
btVector3 tmp;
solverConstraint.m_angularComponentB.Invert( out tmp );
tmp.cross( ref rel_pos2, out vec );
denom1 = body1.getInvMass() + normalAxis.dot( ref vec );
}
double denom = relaxation / ( denom0 + denom1 );
btScalar.Dbg( "m_jacDiagABInv 1 set to " + denom.ToString( "g17" ) );
solverConstraint.m_jacDiagABInv = denom;
}
{
double rel_vel;
double vel1Dotn;
double vel2Dotn;
//double vel1Dotn = solverConstraint.m_contactNormal1.dot( body0 != null ? solverBodyA.m_linearVelocity + solverBodyA.m_externalForceImpulse : btVector3.Zero )
// + solverConstraint.m_relpos1CrossNormal.dot( body0 != null ? solverBodyA.m_angularVelocity : btVector3.Zero );
if( body0 != null )
vel1Dotn = solverConstraint.m_contactNormal1.dotAdded( ref solverBodyA.m_linearVelocity, ref solverBodyA.m_externalForceImpulse )
+ solverConstraint.m_relpos1CrossNormal.dot( ref solverBodyA.m_angularVelocity );
else
vel1Dotn = 0;
//double vel2Dotn = solverConstraint.m_contactNormal2.dot( body1 != null ? solverBodyB.m_linearVelocity + solverBodyB.m_externalForceImpulse : btVector3.Zero )
// + solverConstraint.m_relpos2CrossNormal.dot( body1 != null ? solverBodyB.m_angularVelocity : btVector3.Zero );
if( body1 != null )
vel2Dotn = solverConstraint.m_contactNormal2.dotAdded( ref solverBodyB.m_linearVelocity, ref solverBodyB.m_externalForceImpulse )
+ solverConstraint.m_relpos2CrossNormal.dot( ref solverBodyB.m_angularVelocity );
else
vel2Dotn = 0;
rel_vel = vel1Dotn + vel2Dotn;
// double positionalError = 0;
double velocityError = desiredVelocity - rel_vel;
double velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
solverConstraint.m_rhs = velocityImpulse;
btScalar.Dbg( "Constraint 1 m_rhs " + solverConstraint.m_rhs.ToString( "g17" ) );
solverConstraint.m_rhsPenetration = 0;
solverConstraint.m_cfm = cfmSlip;
solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
solverConstraint.m_upperLimit = solverConstraint.m_friction;
}
}
//
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 );
}
internal eStatus._ Evaluate( GJK gjk, ref btVector3 guess )
{
GJK.sSimplex simplex = gjk.m_simplex;
if( ( simplex.rank > 1 ) && gjk.EncloseOrigin() )
{
/* Clean up */
while( m_hull.root != null )
{
sFace f = m_hull.root;
remove( m_hull, f );
append( m_stock, f );
}
m_status = eStatus._.Valid;
m_nextsv = 0;
/* Orient simplex */
if( btVector3.det( ref simplex.c[0].w, ref simplex.c[3].w,
ref simplex.c[1].w, ref simplex.c[3].w,
ref simplex.c[2].w, ref simplex.c[3].w ) < 0 )
{
btScalar.btSwap( ref simplex.c[0], ref simplex.c[1] );
btScalar.btSwap( ref simplex.p[0], ref simplex.p[1] );
}
/* Build initial hull */
sFace[] tetra = {newface(simplex.c[0],simplex.c[1],simplex.c[2],true),
newface(simplex.c[1],simplex.c[0],simplex.c[3],true),
newface(simplex.c[2],simplex.c[1],simplex.c[3],true),
newface(simplex.c[0],simplex.c[2],simplex.c[3],true)};
if( m_hull.count == 4 )
{
sFace best = findbest();
sFace outer = new sFace( best );
uint pass = 0;
uint iterations = 0;
bind( tetra[0], 0, tetra[1], 0 );
bind( tetra[0], 1, tetra[2], 0 );
bind( tetra[0], 2, tetra[3], 0 );
bind( tetra[1], 1, tetra[3], 2 );
bind( tetra[1], 2, tetra[2], 1 );
bind( tetra[2], 2, tetra[3], 1 );
m_status = eStatus._.Valid;
for( ; iterations < EPA_MAX_ITERATIONS; ++iterations )
{
if( m_nextsv < EPA_MAX_VERTICES )
{
sHorizon horizon = new sHorizon();
sSV w = m_sv_store[m_nextsv++];
bool valid = true;
best.pass = (byte)( ++pass );
gjk.getsupport( ref best.n, w );
double wdist = best.n.dot( ref w.w ) - best.d;
if( wdist > EPA_ACCURACY )
{
for( uint j = 0; ( j < 3 ) && valid; ++j )
{
valid &= expand( pass, w,
best.f[j], best.e[j],
horizon );
}
if( valid & ( horizon.nf >= 3 ) )
{
bind( horizon.cf, 1, horizon.ff, 2 );
remove( m_hull, best );
append( m_stock, best );
best = findbest();
outer = new sFace( best );
}
else { m_status = eStatus._.InvalidHull; break; }
}
else { m_status = eStatus._.AccuraryReached; break; }
}
else { m_status = eStatus._.OutOfVertices; break; }
}
btVector3 projection; outer.n.Mult( outer.d, out projection );
m_normal = outer.n;
m_depth = outer.d;
m_result.rank = 3;
m_result.c[0] = outer.c[0];
m_result.c[1] = outer.c[1];
m_result.c[2] = outer.c[2];
btVector3 tmp;
btVector3.btCross2Del( ref outer.c[1].w, ref projection,
ref outer.c[2].w, ref projection, out tmp );
m_result.p[0] = tmp.length();
btVector3.btCross2Del( ref outer.c[2].w, ref projection,
ref outer.c[0].w, ref projection, out tmp );
m_result.p[1] = tmp.length();
btVector3.btCross2Del( ref outer.c[0].w, ref projection,
ref outer.c[1].w, ref projection, out tmp );
m_result.p[2] = tmp.length();
double sum = m_result.p[0] + m_result.p[1] + m_result.p[2];
m_result.p[0] /= sum;
m_result.p[1] /= sum;
m_result.p[2] /= sum;
return ( m_status );
}
}
/* Fallback */
m_status = eStatus._.FallBack;
guess.Invert( out m_normal );
//m_normal = -guess;
double nl = m_normal.length();
if( nl > 0 )
m_normal.Div( nl, out m_normal );
else
m_normal = btVector3.xAxis;
m_depth = 0;
m_result.rank = 1;
m_result.c[0] = simplex.c[0];
m_result.p[0] = 1;
return ( m_status );
}
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 );
}
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 );
}
}
