///applyDamping damps the velocity, using the given m_linearDamping and m_angularDamping
public void applyDamping( double timeStep )
{
//On new damping: see discussion/issue report here: http://code.google.com/p/bullet/issues/detail?id=74
//todo: do some performance comparisons (but other parts of the engine are probably bottleneck anyway
//#define USE_OLD_DAMPING_METHOD 1
#if USE_OLD_DAMPING_METHOD
m_linearVelocity *= GEN_clamped( ( (double)( 1.0) - timeStep * m_linearDamping ), (double)btScalar.BT_ZERO, (double)(double)( 1.0 ) );
m_angularVelocity *= GEN_clamped( ( (double)( 1.0) - timeStep * m_angularDamping ), (double)btScalar.BT_ZERO, (double)(double)( 1.0 ) );
#else
m_linearVelocity.Mult( btScalar.btPow( (double)( 1 ) - m_linearDamping, timeStep ), out m_linearVelocity );
m_angularVelocity.Mult( btScalar.btPow( (double)( 1 ) - m_angularDamping, timeStep ), out m_angularVelocity );
//m_linearVelocity *= btScalar.btPow( (double)( 1 ) - m_linearDamping, timeStep );
//m_angularVelocity *= btScalar.btPow( (double)( 1 ) - m_angularDamping, timeStep );
#endif
if( m_additionalDamping )
{
//Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc.
//Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete
if( ( m_angularVelocity.length2() < m_additionalAngularDampingThresholdSqr ) &
( m_linearVelocity.length2() < m_additionalLinearDampingThresholdSqr ) )
{
m_linearVelocity.Mult( m_additionalDampingFactor, out m_linearVelocity );
m_angularVelocity.Mult( m_additionalDampingFactor, out m_angularVelocity );
//m_angularVelocity *= m_additionalDampingFactor;
//m_linearVelocity *= m_additionalDampingFactor;
}
double speed = m_linearVelocity.length();
if( speed < m_linearDamping )
{
double dampVel = (double)( 0.005 );
if( speed > dampVel )
{
btVector3 dir; m_linearVelocity.normalized( out dir );
dir.Mult( dampVel, out dir );
m_linearVelocity.Sub( ref dir, out m_linearVelocity );
//m_linearVelocity -= dir * dampVel;
}
else
{
m_linearVelocity = btVector3.Zero;
}
}
double angSpeed = m_angularVelocity.length();
if( angSpeed < m_angularDamping )
{
double angDampVel = (double)( 0.005 );
if( angSpeed > angDampVel )
{
btVector3 dir; m_angularVelocity.normalized( out dir );
dir.Mult( angDampVel, out dir );
m_angularVelocity.Sub( ref dir, out m_angularVelocity );
//m_angularVelocity -= dir * angDampVel;
}
else
{
m_angularVelocity = btVector3.Zero;
}
}
}
}
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 );
}
static double projectorigin( ref btVector3 a,
ref btVector3 b,
double[] w, out uint m )
{
btVector3 d; b.Sub( ref a, out d );
double l = d.length2();
if( l > GJK_SIMPLEX2_EPS )
{
double t = ( l > 0 ? -btVector3.btDot( ref a, ref d ) / l : 0 );
if( t >= 1 ) { w[0] = 0; w[1] = 1; m = 2; return ( b.length2() ); }
else if( t <= 0 ) { w[0] = 1; w[1] = 0; m = 1; return ( a.length2() ); }
else
{
w[0] = 1 - ( w[1] = t ); m = 3;
btVector3 result;
a.AddScale( ref d, t, out result );
return ( result.length2() );
}
}
m = 10;
return ( -1 );
}
internal static void calculateDiffAxisAngleQuaternion( ref btQuaternion orn0, ref btQuaternion orn1a, out btVector3 axis, out double angle )
{
btQuaternion orn1;
orn0.nearest( ref orn1a, out orn1 );
btQuaternion dorn;
btQuaternion tmp;
orn0.inverse( out tmp );
orn1.Mult( ref tmp, out dorn );
angle = dorn.getAngle();
axis.x = dorn.x; axis.y = dorn.y; axis.z = dorn.z; axis.w = 0;
//check for axis length
double len = axis.length2();
if( len < btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON )
axis = btVector3.xAxis;
else
axis.Div( btScalar.btSqrt( len ), out axis );
}
internal static void calculateDiffAxisAngle( ref btTransform transform0, ref btTransform transform1, out btVector3 axis, out double angle )
{
btMatrix3x3 tmp_m;
transform0.m_basis.inverse( out tmp_m );
btMatrix3x3 dmat;
transform1.m_basis.Mult( ref tmp_m, out dmat );
btQuaternion dorn;
dmat.getRotation( out dorn );
///floating point inaccuracy can lead to w component > 1..., which breaks
dorn.normalize();
angle = dorn.getAngle();
axis.x = dorn.x; axis.y = dorn.y; axis.z = dorn.z; axis.w = 0;
//check for axis length
double len = axis.length2();
if( len < btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON )
axis = btVector3.xAxis;
else
axis.Div( btScalar.btSqrt( len ), out axis );
}
internal void getClosestPointsNonVirtual( btDiscreteCollisionDetectorInterface.ClosestPointInput input
, btDiscreteCollisionDetectorInterface.Result output, btIDebugDraw debugDraw )
#endif
{
m_cachedSeparatingDistance = 0;
double distance = btScalar.BT_ZERO;
btVector3 normalInB = btVector3.Zero;
btVector3 pointOnA, pointOnB = btVector3.Zero;
btTransform localTransA; input.m_transformA.Get( out localTransA );
btTransform localTransB; input.m_transformB.Get( out localTransB );
btVector3 positionOffset; localTransA.m_origin.Add( ref localTransB.m_origin, out positionOffset );
positionOffset.Mult( (double)( 0.5 ), out positionOffset );
localTransA.m_origin.Sub( ref positionOffset, out localTransA.m_origin );
localTransB.m_origin.Sub( ref positionOffset, out localTransB.m_origin );
bool check2d = m_minkowskiA.isConvex2d() && m_minkowskiB.isConvex2d();
double marginA = m_marginA;
double marginB = m_marginB;
gNumGjkChecks++;
//for CCD we don't use margins
if( m_ignoreMargin )
{
marginA = btScalar.BT_ZERO;
marginB = btScalar.BT_ZERO;
}
m_curIter = 0;
int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
m_cachedSeparatingAxis.setValue( 0, 1, 0 );
bool isValid = false;
bool checkSimplex = false;
bool checkPenetration = true;
m_degenerateSimplex = 0;
m_lastUsedMethod = -1;
{
double squaredDistance = btScalar.BT_LARGE_FLOAT;
double delta = btScalar.BT_ZERO;
double margin = marginA + marginB;
m_simplexSolver.reset();
for( ;;)
//while (true)
{
btVector3 tmp;
m_cachedSeparatingAxis.Invert( out tmp );
btVector3 seperatingAxisInA; localTransA.m_basis.ApplyInverse( ref tmp, out seperatingAxisInA );
btVector3 seperatingAxisInB; localTransA.m_basis.ApplyInverse( ref m_cachedSeparatingAxis, out seperatingAxisInB );
btVector3 pInA; m_minkowskiA.localGetSupportVertexWithoutMarginNonVirtual( ref seperatingAxisInA, out pInA );
btVector3 qInB; m_minkowskiB.localGetSupportVertexWithoutMarginNonVirtual( ref seperatingAxisInB, out qInB );
btVector3 pWorld; localTransA.Apply( ref pInA, out pWorld );
btVector3 qWorld; localTransB.Apply( ref qInB, out qWorld );
btScalar.Dbg( "pWorld is " + pWorld + " qWorld is " + qWorld );
if( check2d )
{
pWorld[2] = 0;
qWorld[2] = 0;
}
btVector3 w; pWorld.Sub( ref qWorld, out w );
delta = m_cachedSeparatingAxis.dot( ref w );
// potential exit, they don't overlap
if( ( delta > (double)( 0.0 ) ) && ( delta * delta > squaredDistance * input.m_maximumDistanceSquared ) )
{
m_degenerateSimplex = 10;
checkSimplex = true;
//checkPenetration = false;
break;
}
//exit 0: the new point is already in the simplex, or we didn't come any closer
if( m_simplexSolver.inSimplex( ref w ) )
{
m_degenerateSimplex = 1;
checkSimplex = true;
break;
}
// are we getting any closer ?
double f0 = squaredDistance - delta;
double f1 = squaredDistance * REL_ERROR2;
btScalar.Dbg( "f0 is " + f0.ToString( "g17" ) + " f1 is " + f1.ToString( "g17" ) );
if( f0 <= f1 )
{
if( f0 <= btScalar.BT_ZERO )
{
m_degenerateSimplex = 2;
}
else
{
m_degenerateSimplex = 11;
}
checkSimplex = true;
break;
}
//add current vertex to simplex
m_simplexSolver.addVertex( ref w, ref pWorld, ref qWorld );
btVector3 newCachedSeparatingAxis;
//calculate the closest point to the origin (update vector v)
if( !m_simplexSolver.closest( out newCachedSeparatingAxis ) )
{
m_degenerateSimplex = 3;
checkSimplex = true;
break;
}
if( newCachedSeparatingAxis.length2() < REL_ERROR2 )
{
m_cachedSeparatingAxis = newCachedSeparatingAxis;
m_degenerateSimplex = 6;
checkSimplex = true;
break;
}
double previousSquaredDistance = squaredDistance;
squaredDistance = newCachedSeparatingAxis.length2();
#if asdfasdf
///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
if (squaredDistance>previousSquaredDistance)
{
m_degenerateSimplex = 7;
squaredDistance = previousSquaredDistance;
checkSimplex = false;
break;
}
#endif //
//redundant m_simplexSolver.compute_points(pointOnA, pointOnB);
//are we getting any closer ?
if( previousSquaredDistance - squaredDistance <= btScalar.SIMD_EPSILON * previousSquaredDistance )
{
// m_simplexSolver.backup_closest(m_cachedSeparatingAxis);
checkSimplex = true;
m_degenerateSimplex = 12;
break;
}
m_cachedSeparatingAxis = newCachedSeparatingAxis;
//degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
if( m_curIter++ > gGjkMaxIter )
{
#if DEBUG
Console.WriteLine( "btGjkPairDetector maxIter exceeded:{0}", m_curIter );
Console.WriteLine( "sepAxis=({0},{1},{2}), squaredDistance = {3}, shapeTypeA={4},shapeTypeB={5}\n",
m_cachedSeparatingAxis.x,
m_cachedSeparatingAxis.y,
m_cachedSeparatingAxis.z,
squaredDistance,
m_minkowskiA.getShapeType(),
m_minkowskiB.getShapeType() );
#endif
break;
}
bool check = ( !m_simplexSolver.fullSimplex() );
//bool check = (!m_simplexSolver.fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver.maxVertex());
if( !check )
{
//do we need this backup_closest here ?
// m_simplexSolver.backup_closest(m_cachedSeparatingAxis);
m_degenerateSimplex = 13;
break;
}
}
if( checkSimplex )
{
m_simplexSolver.compute_points( out pointOnA, out pointOnB );
btScalar.Dbg( "new simplex points " + pointOnA.ToString() + " and " + pointOnB );
normalInB = m_cachedSeparatingAxis;
double lenSqr = m_cachedSeparatingAxis.length2();
//valid normal
if( lenSqr < 0.0001 )
{
m_degenerateSimplex = 5;
}
if( lenSqr > btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON )
{
double rlen = btScalar.BT_ONE / btScalar.btSqrt( lenSqr );
normalInB.Mult( rlen, out normalInB );
//normalInB *= rlen; //normalize
double s = btScalar.btSqrt( squaredDistance );
Debug.Assert( s > (double)( 0.0 ) );
pointOnA.SubScale( ref m_cachedSeparatingAxis, ( marginA / s ), out pointOnA );
pointOnB.AddScale( ref m_cachedSeparatingAxis, ( marginB / s ), out pointOnB );
//pointOnA -= m_cachedSeparatingAxis * ( marginA / s );
//pointOnB += m_cachedSeparatingAxis * ( marginB / s );
distance = ( ( btScalar.BT_ONE / rlen ) - margin );
isValid = true;
m_lastUsedMethod = 1;
}
else
{
m_lastUsedMethod = 2;
}
}
bool catchDegeneratePenetrationCase =
( m_catchDegeneracies != 0
&& m_penetrationDepthSolver != null
&& m_degenerateSimplex != 0
&& ( ( distance + margin ) < 0.01 ) );
//if (checkPenetration && !isValid)
if( checkPenetration && ( !isValid || catchDegeneratePenetrationCase ) )
{
//penetration case
//if there is no way to handle penetrations, bail out
if( m_penetrationDepthSolver != null )
{
// Penetration depth case.
btVector3 tmpPointOnA, tmpPointOnB;
gNumDeepPenetrationChecks++;
m_cachedSeparatingAxis.setZero();
bool isValid2 = m_penetrationDepthSolver.calcPenDepth(
m_simplexSolver,
m_minkowskiA, m_minkowskiB,
ref localTransA, ref localTransB,
ref m_cachedSeparatingAxis, out tmpPointOnA, out tmpPointOnB,
debugDraw
);
btScalar.Dbg( "points are " + tmpPointOnA.ToString() + " and " + tmpPointOnB.ToString() );
if( isValid2 )
{
btVector3 tmpNormalInB; tmpPointOnB.Sub( ref tmpPointOnA, out tmpNormalInB );
double lenSqr = tmpNormalInB.length2();
if( lenSqr <= ( btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) )
{
tmpNormalInB = m_cachedSeparatingAxis;
lenSqr = m_cachedSeparatingAxis.length2();
}
if( lenSqr > ( btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) )
{
tmpNormalInB.Mult( btScalar.btSqrt( lenSqr ), out tmpNormalInB );
btVector3 tmp;
tmpPointOnA.Sub( ref tmpPointOnB, out tmp );
double distance2 = -tmp.length();
m_lastUsedMethod = 3;
//only replace valid penetrations when the result is deeper (check)
if( !isValid || ( distance2 < distance ) )
{
distance = distance2;
pointOnA = tmpPointOnA;
pointOnB = tmpPointOnB;
normalInB = tmpNormalInB;
///todo: need to track down this EPA penetration solver degeneracy
///the penetration solver reports penetration but the contact normal
///connecting the contact points is pointing in the opposite direction
///until then, detect the issue and revert the normal
{
btScalar d1 = 0;
{
normalInB.Invert( out tmp );
btVector3 seperatingAxisInA; input.m_transformA.m_basis.ApplyInverse( ref normalInB, out seperatingAxisInA );
btVector3 seperatingAxisInB; input.m_transformB.m_basis.ApplyInverse( ref tmp, out seperatingAxisInB );
btVector3 pInA; m_minkowskiA.localGetSupportVertexWithoutMarginNonVirtual( ref seperatingAxisInA, out pInA );
btVector3 qInB; m_minkowskiB.localGetSupportVertexWithoutMarginNonVirtual( ref seperatingAxisInB, out qInB );
btVector3 pWorld; localTransA.Apply( ref pInA, out pWorld );
btVector3 qWorld; localTransB.Apply( ref qInB, out qWorld );
btVector3 w; pWorld.Sub( ref qWorld, out w );
d1 = ( tmp ).dot( w );
}
btScalar d0 = btScalar.BT_ZERO;
{
normalInB.Invert( out tmp );
btVector3 seperatingAxisInA; input.m_transformA.m_basis.ApplyInverse( ref tmp, out seperatingAxisInA );
btVector3 seperatingAxisInB; input.m_transformB.m_basis.ApplyInverse( ref normalInB, out seperatingAxisInB ) ;
btVector3 pInA; m_minkowskiA.localGetSupportVertexWithoutMarginNonVirtual( ref seperatingAxisInA, out pInA );
btVector3 qInB; m_minkowskiB.localGetSupportVertexWithoutMarginNonVirtual( ref seperatingAxisInB, out qInB );
btVector3 pWorld; localTransA.Apply( ref pInA, out pWorld );
btVector3 qWorld; localTransB.Apply( ref qInB, out qWorld );
btVector3 w; pWorld.Sub( ref qWorld, out w );
d0 = normalInB.dot( w );
}
if( d1 > d0 )
{
m_lastUsedMethod = 10;
normalInB *= -1;
}
}
isValid = true;
}
else
{
m_lastUsedMethod = 8;
}
}
else
{
m_lastUsedMethod = 9;
}
}
else
{
///this is another degenerate case, where the initial GJK calculation reports a degenerate case
///EPA reports no penetration, and the second GJK (using the supporting vector without margin)
///reports a valid positive distance. Use the results of the second GJK instead of failing.
///thanks to Jacob.Langford for the reproduction case
///http://code.google.com/p/bullet/issues/detail?id=250
if( m_cachedSeparatingAxis.length2() > btScalar.BT_ZERO )
{
btVector3 tmp;
tmpPointOnA.Sub( ref tmpPointOnB, out tmp );
double distance2 = tmp.length() - margin;
//only replace valid distances when the distance is less
btScalar.Dbg( "old distance " + distance2.ToString( "g17" ) + " new distance " + distance.ToString( "g17" ) );
if( !isValid || ( distance2 < distance ) )
{
distance = distance2;
pointOnA = tmpPointOnA;
pointOnB = tmpPointOnB;
pointOnA.SubScale( ref m_cachedSeparatingAxis, marginA, out pointOnA );
pointOnB.AddScale( ref m_cachedSeparatingAxis, marginB, out pointOnA );
//pointOnA -= m_cachedSeparatingAxis * marginA;
//pointOnB += m_cachedSeparatingAxis * marginB;
normalInB = m_cachedSeparatingAxis;
normalInB.normalize();
isValid = true;
m_lastUsedMethod = 6;
}
else
{
m_lastUsedMethod = 5;
}
}
}
}
}
}
btScalar.Dbg( "Pair detector : valid=" + (isValid?"1":"0" )+ " distance=" + distance.ToString( "g17" ) + " maxDistance=" + input.m_maximumDistanceSquared.ToString( "g17" ) );
if( isValid && ( ( distance < 0 ) || ( distance * distance < input.m_maximumDistanceSquared ) ) )
{
m_cachedSeparatingAxis = normalInB;
m_cachedSeparatingDistance = distance;
btVector3 tmp;
pointOnB.Add( ref positionOffset, out tmp );
output.addContactPoint(
ref normalInB,
ref tmp,
distance );
}
}
/*@brief Return the angle between this and another vector
@param v The other vector */
public double angle( ref btVector3 v )
{
double s = btScalar.btSqrt( ( length2() * v.length2() ) );
#if PARANOID_ASSERTS
Debug.Assert( s != (double)(0.0));
#endif
return btScalar.btAcos( ( dot( ref v ) / s ) );
}
/*
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 );
}
}
}