static void PrintTransform( ref btTransform t )
{
int n;
for( n = 0; n < 3; n++ )
Console.WriteLine( " < {0} {1} {2} {3} >", t.m_basis[n][0], t.m_basis[n][1], t.m_basis[n][2], t.m_basis[n][3] );
Console.WriteLine( "O = <{0},{1},{2}>", t.m_basis[0], t.m_basis[1], t.m_basis[1] );
}
/*
internal void Initialize( btCollisionAlgorithmConstructionInfo ci)
{
base.Initialize( ci );
}
*/
internal override void processCollision( btCollisionObjectWrapper a
, ref btTransform body0Transform
, btCollisionObjectWrapper body1Wrap
, ref btTransform body1Transform
, btDispatcherInfo c, btManifoldResult d)
{
}
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;
}
}
internal override void processCollision( btCollisionObjectWrapper body0Wrap
, ref btTransform body0Transform
, btCollisionObjectWrapper body1Wrap
, ref btTransform body1Transform
, btDispatcherInfo dispatchInfo, btManifoldResult resultOut )
{
//(void)dispatchInfo;
if( m_manifoldPtr == null )
return;
resultOut.setPersistentManifold( m_manifoldPtr );
btSphereShape sphere0 = (btSphereShape)body0Wrap.getCollisionShape();
btSphereShape sphere1 = (btSphereShape)body1Wrap.getCollisionShape();
btVector3 diff; body0Wrap.m_collisionObject.m_worldTransform.m_origin.Sub( ref body1Wrap.m_collisionObject.m_worldTransform.m_origin, out diff );
double len = diff.length();
double radius0 = sphere0.getRadius();
double radius1 = sphere1.getRadius();
#if CLEAR_MANIFOLD
m_manifoldPtr.clearManifold(); //don't do this, it disables warmstarting
#endif
///iff distance positive, don't generate a new contact
if( len > ( radius0 + radius1 ) )
{
#if !CLEAR_MANIFOLD
resultOut.refreshContactPoints();
#endif //CLEAR_MANIFOLD
return;
}
///distance (negative means penetration)
double dist = len - ( radius0 + radius1 );
btVector3 normalOnSurfaceB = btVector3.xAxis;
if( len > btScalar.SIMD_EPSILON )
{
normalOnSurfaceB = diff / len;
}
///point on A (worldspace)
///btVector3 pos0 = col0.getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB;
///point on B (worldspace)
btVector3 pos1; body1Wrap.m_collisionObject.m_worldTransform.m_origin.AddScale( ref normalOnSurfaceB, radius1, out pos1 );
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut.addContactPoint( ref normalOnSurfaceB, ref pos1, dist );
#if !CLEAR_MANIFOLD
resultOut.refreshContactPoints();
#endif //CLEAR_MANIFOLD
}
/// 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 override void getAabb( ref btTransform t, out btVector3 aabbMin, out btVector3 aabbMax )
{
btVector3 halfExtents = new btVector3( getRadius(), getRadius(), getRadius() );
btVector3 tmp = halfExtents;
halfExtents[m_upAxis] = getRadius() + getHalfHeight();
halfExtents.Add( ref tmp, out halfExtents );
btMatrix3x3 abs_b; t.m_basis.absolute( out abs_b );
btVector3 extent; halfExtents.dot3( ref abs_b.m_el0, ref abs_b.m_el1, ref abs_b.m_el2, out extent );
t.m_origin.Sub( ref extent, out aabbMin );
t.m_origin.Add( ref extent, out aabbMax );
}
public static void Run()
{
{
btTransform planeObjWorld = new btTransform( new btQuaternion( 0.2, 0.1, 0.3, 1 ), new btVector3( 3, 4, 5 ) );
btTransform convexWorldTransform = new btTransform( new btQuaternion( 0.2, 0.3, 0, 1 ), new btVector3( 3, 4, 5 ) );
btTransform convexInPlaneTrans;
btTransform tmp;
planeObjWorld.inverse( out tmp );
tmp.Apply( ref convexWorldTransform, out convexInPlaneTrans );
PrintTransform( ref convexInPlaneTrans );
planeObjWorld.inverseTimes( ref convexWorldTransform, out convexInPlaneTrans );
PrintTransform( ref convexInPlaneTrans );
//convexInPlaneTrans = planeObjWorld( convexWorldTransform;
//PrintTransform( &convexInPlaneTrans );
}
{
btTransform planeObjWorld = new btTransform( new btQuaternion( 0.2, 0.1, 0.3, 1 ), new btVector3( 5, 2, 1 ) );
btTransform convexWorldTransform = new btTransform( new btQuaternion( 0.2, 0.3, 0, 1 ), new btVector3( 3, 4, 5 ) );
btTransform convexInPlaneTrans;
btTransform tmp;
planeObjWorld.inverse( out tmp );
tmp.Apply( ref convexWorldTransform, out convexInPlaneTrans );
PrintTransform( ref convexInPlaneTrans );
planeObjWorld.inverseTimes( ref convexWorldTransform, out convexInPlaneTrans );
PrintTransform( ref convexInPlaneTrans );
//convexInPlaneTrans = planeObjWorld( convexWorldTransform;
//PrintTransform( &convexInPlaneTrans );
btQuaternion perturbeRot = new btQuaternion( 0.1, 0.5, 0.25, 0.8 );
perturbeRot.normalize();
btTransform planeObjWrapTrans = new btTransform( new btQuaternion( 0.2, 0.1, 0.3, 1 ), new btVector3( 4, 7, 2 ) );
planeObjWrapTrans.inverseTimes( ref convexWorldTransform, out convexInPlaneTrans );
//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;
btTransform planeInConvex;
convexWorldTransform.inverseTimes( ref planeObjWrapTrans, out planeInConvex );
PrintTransform( ref planeInConvex );
}
Console.Read();
}
public override void getAabb( ref btTransform t, out btVector3 aabbMin, out btVector3 aabbMax )
{
//(void)t;
/*
btVector3 infvec ((double)(BT_LARGE_FLOAT),(double)(BT_LARGE_FLOAT),(double)(BT_LARGE_FLOAT));
btVector3 center = m_planeNormal*m_planeConstant;
aabbMin = center + infvec*m_planeNormal;
aabbMax = aabbMin;
aabbMin.setMin(center - infvec*m_planeNormal);
aabbMax.setMax(center - infvec*m_planeNormal);
*/
aabbMin = btVector3.Min;
aabbMax = btVector3.Max;
}
internal override void processCollision( btCollisionObjectWrapper col0Wrap
, ref btTransform body0Transform
, btCollisionObjectWrapper col1Wrap
, ref btTransform body1Transform
, btDispatcherInfo dispatchInfo, btManifoldResult resultOut )
{
if( m_manifoldPtr == null )
return;
btCollisionObjectWrapper sphereObjWrap = m_swapped ? col1Wrap : col0Wrap;
btCollisionObjectWrapper triObjWrap = m_swapped ? col0Wrap : col1Wrap;
btSphereShape sphere = (btSphereShape)sphereObjWrap.getCollisionShape();
btTriangleShape triangle = (btTriangleShape)triObjWrap.getCollisionShape();
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut.setPersistentManifold( m_manifoldPtr );
SphereTriangleDetector detector = BulletGlobals.SphereTriangleDetectorPool.Get();
detector.Initialize( sphere, triangle, m_manifoldPtr.getContactBreakingThreshold());
btDiscreteCollisionDetectorInterface.ClosestPointInput input = BulletGlobals.ClosestPointInputPool.Get();
input.m_maximumDistanceSquared = btScalar.BT_LARGE_FLOAT;///@todo: tighter bounds
if( m_swapped )
{
input.m_transformA = body1Transform;
input.m_transformB = body0Transform;
}
else
{
input.m_transformA = body0Transform;
input.m_transformB = body1Transform;
}
bool swapResults = m_swapped;
detector.getClosestPoints( input, resultOut, dispatchInfo.m_debugDraw, swapResults );
BulletGlobals.ClosestPointInputPool.Free( input );
BulletGlobals.SphereTriangleDetectorPool.Free( detector );
if( m_ownManifold )
resultOut.refreshContactPoints();
}
// this is a
internal override bool calcPenDepth( btSimplexSolverInterface simplexSolver,
btConvexShape pConvexA, btConvexShape pConvexB,
ref btTransform transformA, ref btTransform transformB,
ref btVector3 v, out btVector3 wWitnessOnA, out btVector3 wWitnessOnB,
btIDebugDraw debugDraw )
{
//(void)debugDraw;
//(void)v;
//(void)simplexSolver;
// double radialmargin(btScalar.BT_ZERO);
btVector3 guessVector; transformB.m_origin.Sub( ref transformA.m_origin, out guessVector );
btGjkEpaSolver2.sResults results;
if( btGjkEpaSolver2.Penetration( pConvexA, ref transformA,
pConvexB, ref transformB,
ref guessVector, out results ) )
{
// debugDraw.drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
//resultOut.addContactPoint(results.normal,results.witnesses[1],-results.depth);
wWitnessOnA = results.witness0;
wWitnessOnB = results.witness1;
v = results.normal;
return true;
}
else
{
if( btGjkEpaSolver2.Distance( pConvexA, ref transformA, pConvexB, ref transformB, ref guessVector, out results ) )
{
wWitnessOnA = results.witness0;
wWitnessOnB = results.witness1;
v = results.normal;
return false;
}
}
wWitnessOnA = results.witness0;
wWitnessOnB = results.witness1;
return false;
}
void Setup()
{
world = new btDiscreteDynamicsWorld();
world.setDebugDrawer( Program.Drawer );
btVector3 tmp; btVector3.yAxis.Add( ref btVector3.xAxis, out tmp );
tmp.normalized( out tmp );
btCollisionShape groundShape;
if( sloped )
groundShape = new btStaticPlaneShape( ref btVector3.Zero, ref tmp );
else
groundShape = new btStaticPlaneShape( ref btVector3.Zero, ref btVector3.yAxis );
btDefaultMotionState groundMotionState = new btDefaultMotionState();
btRigidBody.btRigidBodyConstructionInfo
groundRigidBodyCI = new btRigidBody.btRigidBodyConstructionInfo( 0, groundMotionState
, groundShape, ref btVector3.Zero );
btRigidBody groundRigidBody = new btRigidBody( groundRigidBodyCI );
world.addRigidBody( groundRigidBody );
btCollisionShape fallShape = new btSphereShape( btScalar.BT_ONE );
btVector3 origin = new btVector3( 0, 50, 0 );
btTransform init = new btTransform( ref btQuaternion.Identity, ref origin );
btDefaultMotionState fallMotionState = new btDefaultMotionState( ref init );
btScalar mass = 1;
btVector3 fallInertia;
fallShape.calculateLocalInertia( mass, out fallInertia );
btRigidBody.btRigidBodyConstructionInfo
fallingRigidBodyCI = new btRigidBody.btRigidBodyConstructionInfo( mass, fallMotionState
, fallShape, ref fallInertia );
fallingRigidBody = new btRigidBody( fallingRigidBodyCI );
world.addRigidBody( fallingRigidBody );
}
internal override void processCollision( btCollisionObjectWrapper body0Wrap, ref btTransform body0Transform
, btCollisionObjectWrapper body1Wrap, ref btTransform body1Transform, btDispatcherInfo dispatchInfo, btManifoldResult resultOut )
{
if( m_manifoldPtr == null )
return;
btBoxShape box0 = (btBoxShape)body0Wrap.m_shape;
btBoxShape box1 = (btBoxShape)body1Wrap.m_shape;
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut.setPersistentManifold( m_manifoldPtr );
#if !USE_PERSISTENT_CONTACTS
m_manifoldPtr.clearManifold();
#endif //USE_PERSISTENT_CONTACTS
btDiscreteCollisionDetectorInterface.ClosestPointInput input = new btDiscreteCollisionDetectorInterface.ClosestPointInput();
input.m_maximumDistanceSquared = btScalar.BT_LARGE_FLOAT;
input.m_transformA = body0Transform;
input.m_transformB = body1Transform;
//btBoxBoxDetector detector = BulletGlobals.
// new btBoxBoxDetectors( box0, box1 );
btBoxBoxDetector.getClosestPoints( box0, box1, input, resultOut, dispatchInfo.m_debugDraw );
#if USE_PERSISTENT_CONTACTS
// refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added
if( m_ownManifold )
{
resultOut.refreshContactPoints();
}
#endif //USE_PERSISTENT_CONTACTS
}
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 );
}
}
int setAngularLimits( btConstraintInfo2 info, int row_offset, ref btTransform transA, ref btTransform transB, ref btVector3 linVelA, ref btVector3 linVelB, ref btVector3 angVelA, ref btVector3 angVelB )
{
int row = row_offset;
//order of rotational constraint rows
int[] cIdx = { 0, 1, 2 };
switch( m_rotateOrder )
{
case RotateOrder.RO_XYZ: cIdx[0] = 0; cIdx[1] = 1; cIdx[2] = 2; break;
case RotateOrder.RO_XZY: cIdx[0] = 0; cIdx[1] = 2; cIdx[2] = 1; break;
case RotateOrder.RO_YXZ: cIdx[0] = 1; cIdx[1] = 0; cIdx[2] = 2; break;
case RotateOrder.RO_YZX: cIdx[0] = 1; cIdx[1] = 2; cIdx[2] = 0; break;
case RotateOrder.RO_ZXY: cIdx[0] = 2; cIdx[1] = 0; cIdx[2] = 1; break;
case RotateOrder.RO_ZYX: cIdx[0] = 2; cIdx[1] = 1; cIdx[2] = 0; break;
default: Debug.Assert( false ); break;
}
for( int ii = 0; ii < 3; ii++ )
{
int i = cIdx[ii];
if( m_angularLimits[i].m_currentLimit != 0 || m_angularLimits[i].m_enableMotor || m_angularLimits[i].m_enableSpring )
{
btVector3 axis = getAxis( i );
//int flags = m_flags >> ( ( i + 3 ) * BT_6DOF_FLAGS_AXIS_SHIFT2 );
if( 0 == ( m_flags & bt6DofFlagsIndexed.BT_6DOF_FLAGS_ERP_STOP2[i] ) )
{
m_angularLimits[i].m_stopCFM = info.m_solverConstraints[0].m_cfm;
}
if( 0 == ( m_flags & bt6DofFlagsIndexed.BT_6DOF_FLAGS_ERP_STOP2[i] ) )
{
m_angularLimits[i].m_stopERP = info.erp;
}
if( 0 == ( m_flags & bt6DofFlagsIndexed.BT_6DOF_FLAGS_CFM_MOTO2[i] ) )
{
m_angularLimits[i].m_motorCFM = info.m_solverConstraints[0].m_cfm;
}
if( 0 == ( m_flags & bt6DofFlagsIndexed.BT_6DOF_FLAGS_ERP_MOTO2[i] ) )
{
m_angularLimits[i].m_motorERP = info.erp;
}
row += get_limit_motor_info2( m_angularLimits[i], ref transA, ref transB
, ref linVelA, ref linVelB, ref angVelA, ref angVelB
, info, row, ref axis, true );
}
}
return row;
}
void setFrames( ref btTransform frameA, ref btTransform frameB )
{
m_frameInA = frameA;
m_frameInB = frameB;
buildJacobian();
calculateTransforms();
}
internal override void processCollision( btCollisionObjectWrapper body0Wrap
, ref btTransform body0Transform
, btCollisionObjectWrapper body1Wrap
, ref btTransform body1Transform
, btDispatcherInfo dispatchInfo, btManifoldResult resultOut )
{
btCollisionObjectWrapper colObjWrap = m_isSwapped ? body1Wrap : body0Wrap;
btCollisionObjectWrapper otherObjWrap = m_isSwapped ? body0Wrap : body1Wrap;
Debug.Assert( colObjWrap.getCollisionShape().isCompound() );
btCompoundShape compoundShape = (btCompoundShape)( colObjWrap.getCollisionShape() );
///btCompoundShape might have changed:
////make sure the internal child collision algorithm caches are still valid
if( compoundShape.getUpdateRevision() != m_compoundShapeRevision )
{
///clear and update all
removeChildAlgorithms();
preallocateChildAlgorithms( body0Wrap, body1Wrap );
m_compoundShapeRevision = compoundShape.getUpdateRevision();
}
if( m_childCollisionAlgorithms.Count == 0 )
return;
btDbvt tree = compoundShape.getDynamicAabbTree();
//use a dynamic aabb tree to cull potential child-overlaps
btCompoundLeafCallback callback = BulletGlobals.CompoundLeafCallbackPool.Get();
callback.Initialize( colObjWrap, otherObjWrap, m_dispatcher, dispatchInfo, resultOut, m_childCollisionAlgorithms.InternalArray, m_sharedManifold);
///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();
for( i = 0; i < m_childCollisionAlgorithms.Count; i++ )
{
if( m_childCollisionAlgorithms[i] != null )
{
m_childCollisionAlgorithms[i].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 );//??necessary?
}
}
manifoldArray.Count = ( 0 );
}
}
}
if( tree != null )
{
btVector3 localAabbMin, localAabbMax;
btTransform otherInCompoundSpace;
if( m_isSwapped )
body1Transform.inverseTimes(ref body0Transform, out otherInCompoundSpace);
else
body0Transform.inverseTimes( ref body1Transform, out otherInCompoundSpace );
otherObjWrap.getCollisionShape().getAabb( ref otherInCompoundSpace, out localAabbMin, out localAabbMax );
btDbvt.btDbvtVolume bounds = btDbvt.btDbvtVolume.FromMM( ref localAabbMin, ref localAabbMax );
//process all children, that overlap with the given AABB bounds
btDbvt.CollideTV( tree.m_root, ref bounds, callback );
}
else
{
//iterate over all children, perform an AABB check inside ProcessChildShape
int numChildren = m_childCollisionAlgorithms.Count;
int i;
for( i = 0; i < numChildren; i++ )
{
callback.ProcessChildShape( compoundShape.getChildShape( i ), i );
}
}
{
//iterate over all children, perform an AABB check inside ProcessChildShape
int numChildren = m_childCollisionAlgorithms.Count;
int i;
//btManifoldArray manifoldArray;
btCollisionShape childShape = null;
//btITransform orgTrans;
btTransform newChildWorldTrans;
btVector3 aabbMin0, aabbMax0, aabbMin1, aabbMax1;
for( i = 0; i < numChildren; i++ )
{
if( m_childCollisionAlgorithms[i] != null )
{
childShape = compoundShape.getChildShape( i );
//if not longer overlapping, remove the algorithm
//orgTrans = colObjWrap.m_worldTransform;
//btTransform childTrans = compoundShape.getChildTransform( i );
( ( m_isSwapped ) ? body1Transform : body0Transform ).Apply( ref compoundShape.m_children.InternalArray[i].m_transform, out newChildWorldTrans );
//perform an AABB check first
childShape.getAabb( ref newChildWorldTrans, out aabbMin0, out aabbMax0 );
if( m_isSwapped )
otherObjWrap.m_shape.getAabb( ref body0Transform, out aabbMin1, out aabbMax1 );
else
otherObjWrap.m_shape.getAabb( ref body1Transform, out aabbMin1, out aabbMax1 );
if( !btAabbUtil.TestAabbAgainstAabb2( ref aabbMin0, ref aabbMax0, ref aabbMin1, ref aabbMax1 ) )
{
//m_childCollisionAlgorithms[i].~btCollisionAlgorithm();
m_dispatcher.freeCollisionAlgorithm( m_childCollisionAlgorithms[i] );
m_childCollisionAlgorithms[i] = null;
}
}
}
}
}
int setLinearLimits( btConstraintInfo2 info, int row, ref btTransform transA, ref btTransform transB, ref btVector3 linVelA, ref btVector3 linVelB, ref btVector3 angVelA, ref btVector3 angVelB )
{
//solve linear limits
btRotationalLimitMotor2 limot = new btRotationalLimitMotor2();
for( int i = 0; i < 3; i++ )
{
if( m_linearLimits.m_currentLimit[i] != 0 || m_linearLimits.m_enableMotor[i] || m_linearLimits.m_enableSpring[i] )
{ // re-use rotational motor code
limot.m_bounce = m_linearLimits.m_bounce[i];
limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i];
limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];
limot.m_currentLimitErrorHi = m_linearLimits.m_currentLimitErrorHi[i];
limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
limot.m_servoMotor = m_linearLimits.m_servoMotor[i];
limot.m_servoTarget = m_linearLimits.m_servoTarget[i];
limot.m_enableSpring = m_linearLimits.m_enableSpring[i];
limot.m_springStiffness = m_linearLimits.m_springStiffness[i];
limot.m_springStiffnessLimited = m_linearLimits.m_springStiffnessLimited[i];
limot.m_springDamping = m_linearLimits.m_springDamping[i];
limot.m_springDampingLimited = m_linearLimits.m_springDampingLimited[i];
limot.m_equilibriumPoint = m_linearLimits.m_equilibriumPoint[i];
limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
btVector3 axis = m_calculatedTransformA.m_basis.getColumn( i );
//int flags = m_flags >> ( i * BT_6DOF_FLAGS_AXIS_SHIFT2 );
limot.m_stopCFM = ( m_flags & bt6DofFlagsIndexed.BT_6DOF_FLAGS_CFM_STOP2[i] ) != 0 ? m_linearLimits.m_stopCFM[i] : info.m_solverConstraints[0].m_cfm;
limot.m_stopERP = ( m_flags & bt6DofFlagsIndexed.BT_6DOF_FLAGS_ERP_STOP2[i] ) != 0 ? m_linearLimits.m_stopERP[i] : info.erp;
limot.m_motorCFM = ( m_flags & bt6DofFlagsIndexed.BT_6DOF_FLAGS_CFM_MOTO2[i] ) != 0 ? m_linearLimits.m_motorCFM[i] : info.m_solverConstraints[0].m_cfm;
limot.m_motorERP = ( m_flags & bt6DofFlagsIndexed.BT_6DOF_FLAGS_ERP_MOTO2[i] ) != 0 ? m_linearLimits.m_motorERP[i] : info.erp;
//rotAllowed is a bit of a magic from the original 6dof. The calculation of it here is something that imitates the original behavior as much as possible.
int indx1 = ( i + 1 ) % 3;
int indx2 = ( i + 2 ) % 3;
bool rotAllowed = true; // rotations around orthos to current axis (it is used only when one of the body is static)
bool indx1Violated = m_angularLimits[indx1].m_currentLimit == 1 ||
m_angularLimits[indx1].m_currentLimit == 2 ||
( m_angularLimits[indx1].m_currentLimit == 3 && ( m_angularLimits[indx1].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx1].m_currentLimitError > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) ) ||
( m_angularLimits[indx1].m_currentLimit == 4 && ( m_angularLimits[indx1].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx1].m_currentLimitErrorHi > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) );
bool indx2Violated = m_angularLimits[indx2].m_currentLimit == 1 ||
m_angularLimits[indx2].m_currentLimit == 2 ||
( m_angularLimits[indx2].m_currentLimit == 3 && ( m_angularLimits[indx2].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx2].m_currentLimitError > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) ) ||
( m_angularLimits[indx2].m_currentLimit == 4 && ( m_angularLimits[indx2].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx2].m_currentLimitErrorHi > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) );
if( indx1Violated && indx2Violated )
{
rotAllowed = false;
}
row += get_limit_motor_info2( limot, ref transA, ref transB
, ref linVelA, ref linVelB
, ref angVelA, ref angVelB
, info, row, ref axis, false, rotAllowed );
}
}
return row;
}
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( btConstraintInfo2 info, ref btTransform body0_trans, ref btTransform body1_trans )
{
Debug.Assert( !m_useSolveConstraintObsolete );
//retrieve matrices
// anchor points in global coordinates with respect to body PORs.
// 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;
btVector3 a1; body0_trans.m_basis.Apply( ref m_pivotInA, 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_relpos1CrossNormal
, out info.m_solverConstraints[1].m_relpos1CrossNormal
, out info.m_solverConstraints[2].m_relpos1CrossNormal );
}
btVector3.xAxis.Invert( out info.m_solverConstraints[0].m_contactNormal2 );
btVector3.yAxis.Invert( out info.m_solverConstraints[1].m_contactNormal2 );
btVector3.zAxis.Invert( out info.m_solverConstraints[2].m_contactNormal2 );
//info.m_J2linearAxis[0] = -1;
//info.m_J2linearAxis[info.rowskip + 1] = -1;
//info.m_J2linearAxis[2 * info.rowskip + 2] = -1;
btVector3 a2; body1_trans.m_basis.Apply( ref m_pivotInB, out a2 );
//getBasis() * getPivotInB();
{
// btVector3 a2n = -a2;
//btVector3* angular0 = (btVector3*)( info.m_J2angularAxis );
//btVector3* angular1 = (btVector3*)( info.m_J2angularAxis + info.rowskip );
//btVector3* angular2 = (btVector3*)( info.m_J2angularAxis + 2 * info.rowskip );
a2.getSkewSymmetricMatrix( out info.m_solverConstraints[0].m_relpos2CrossNormal
, out info.m_solverConstraints[1].m_relpos2CrossNormal
, out info.m_solverConstraints[2].m_relpos2CrossNormal );
}
// set right hand side
double currERP = ( m_flags & btPoint2PointFlags.BT_P2P_FLAGS_ERP )!= 0 ? m_erp : info.erp;
double k = info.fps * currERP;
int j;
for( j = 0; j < 3; j++ )
{
info.m_solverConstraints[j].m_rhs = k * ( a2[j] + body1_trans.m_origin[j] - a1[j] - body0_trans.m_origin[j] );
//Console.WriteLine("info.m_constraintError[%d]=%f\n",j,info.m_constraintError[j]);
}
if( ( m_flags & btPoint2PointFlags.BT_P2P_FLAGS_CFM ) != 0 )
{
for( j = 0; j < 3; j++ )
{
info.m_solverConstraints[j].m_cfm = m_cfm;
}
}
double impulseClamp = m_setting.m_impulseClamp;//
for( j = 0; j < 3; j++ )
{
if( m_setting.m_impulseClamp > 0 )
{
info.m_solverConstraints[j]. m_lowerLimit = -impulseClamp;
info.m_solverConstraints[j].m_upperLimit = impulseClamp;
}
}
info.m_damping = m_setting.m_damping;
}
void setCenterOfMassTransform( ref btTransform xform )
{
if( isKinematicObject() )
{
m_interpolationWorldTransform = m_worldTransform;
}
else
{
m_interpolationWorldTransform = xform;
}
m_updateRevision++;
m_interpolationLinearVelocity = m_linearVelocity;
m_interpolationAngularVelocity = m_angularVelocity;
m_worldTransform = xform;
updateInertiaTensor();
}
public static void btTransformAabb( ref btVector3 halfExtents, double margin, ref btTransform t
, out btVector3 aabbMinOut, out btVector3 aabbMaxOut )
{
btVector3 halfExtentsWithMargin; halfExtents.AddScale( ref btVector3.One, margin, out halfExtentsWithMargin );
btMatrix3x3 abs_b; t.m_basis.absolute( out abs_b);
btVector3 extent; halfExtentsWithMargin.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 aabbMinOut );
t.m_origin.Add( ref extent, out aabbMaxOut );
}
/// cast a convex against another convex object internal abstract bool calcTimeOfImpact( ref btTransform fromA, ref btTransform toA, ref btTransform fromB, ref btTransform toB, CastResult result );
// interia default = btVector3.Zero
public btRigidBodyConstructionInfo( double mass, btMotionState motionState
, btCollisionShape collisionShape
, ref btVector3 localInertia )
{
m_mass = ( mass );
m_motionState = ( motionState );
m_collisionShape = ( collisionShape );
m_localInertia = ( localInertia );
m_linearDamping = ( btScalar.BT_ZERO );
m_angularDamping = ( btScalar.BT_ZERO );
m_friction = ( (double)( 0.5 ) );
m_rollingFriction = ( (double)( 0 ) );
m_restitution = ( btScalar.BT_ZERO );
m_linearSleepingThreshold = ( (double)( 0.8 ) );
m_angularSleepingThreshold = ( (double)( 1 ) );
m_additionalDamping = ( false );
m_additionalDampingFactor = (double)( 0.005 );
m_additionalLinearDampingThresholdSqr = ( (double)( 0.01 ) );
m_additionalAngularDampingThresholdSqr = ( (double)( 0.01 ) );
m_additionalAngularDampingFactor = ( (double)( 0.01 ) );
m_startWorldTransform = btTransform.Identity;
}
internal btGeneric6DofSpring2Constraint( btRigidBody rbB, ref btTransform frameInB, RotateOrder rotOrder )
: base( btObjectTypes.D6_SPRING_2_CONSTRAINT_TYPE, getFixedBody(), rbB )
{
m_frameInB = ( frameInB );
m_rotateOrder = ( rotOrder );
m_flags = ( 0 );
///not providing rigidbody A means implicitly using worldspace for body A
rbB.m_worldTransform.Apply( ref m_frameInB, out m_frameInA );
calculateTransforms();
}
int get_limit_motor_info2(
btRotationalLimitMotor2 limot,
ref btTransform transA, ref btTransform transB, ref btVector3 linVelA, ref btVector3 linVelB, ref btVector3 angVelA, ref btVector3 angVelB,
btConstraintInfo2 info, int row, ref btVector3 ax1, bool rotational, bool rotAllowed = false )
{
int count = 0;
//int srow = row * info.rowskip;
if( limot.m_currentLimit == 4 )
{
double vel = rotational ? angVelA.dot( ax1 ) - angVelB.dot( ax1 ) : linVelA.dot( ax1 ) - linVelB.dot( ax1 );
calculateJacobi( limot, ref transA, ref transB, info, row, ref ax1, rotational, rotAllowed );
info.m_solverConstraints[row].m_rhs = info.fps * limot.m_stopERP * limot.m_currentLimitError * ( rotational ? -1 : 1 );
if( rotational )
{
if( info.m_solverConstraints[row].m_rhs - vel * limot.m_stopERP > 0 )
{
double bounceerror = -limot.m_bounce * vel;
if( bounceerror > info.m_solverConstraints[row].m_rhs ) info.m_solverConstraints[row].m_rhs = bounceerror;
}
}
else
{
if( info.m_solverConstraints[row].m_rhs - vel * limot.m_stopERP < 0 )
{
double bounceerror = -limot.m_bounce * vel;
if( bounceerror < info.m_solverConstraints[row].m_rhs ) info.m_solverConstraints[row].m_rhs = bounceerror;
}
}
info.m_solverConstraints[row].m_lowerLimit = rotational ? 0 : btScalar.BT_MIN_FLOAT;
info.m_solverConstraints[row].m_upperLimit = rotational ? btScalar.BT_MAX_FLOAT : 0;
info.m_solverConstraints[row].m_cfm = limot.m_stopCFM;
row ++;
++count;
calculateJacobi( limot, ref transA, ref transB, info, row, ref ax1, rotational, rotAllowed );
info.m_solverConstraints[row].m_rhs = info.fps * limot.m_stopERP * limot.m_currentLimitErrorHi * ( rotational ? -1 : 1 );
if( rotational )
{
if( info.m_solverConstraints[row].m_rhs - vel * limot.m_stopERP < 0 )
{
double bounceerror = -limot.m_bounce * vel;
if( bounceerror < info.m_solverConstraints[row].m_rhs ) info.m_solverConstraints[row].m_rhs = bounceerror;
}
}
else
{
if( info.m_solverConstraints[row].m_rhs - vel * limot.m_stopERP > 0 )
{
double bounceerror = -limot.m_bounce * vel;
if( bounceerror > info.m_solverConstraints[row].m_rhs ) info.m_solverConstraints[row].m_rhs = bounceerror;
}
}
info.m_solverConstraints[row].m_lowerLimit = rotational ? btScalar.BT_MIN_FLOAT : 0;
info.m_solverConstraints[row].m_upperLimit = rotational ? 0 : btScalar.BT_MAX_FLOAT;
info.m_solverConstraints[row].m_cfm = limot.m_stopCFM;
row ++;
++count;
}
else
if( limot.m_currentLimit == 3 )
{
calculateJacobi( limot, ref transA, ref transB, info, row, ref ax1, rotational, rotAllowed );
info.m_solverConstraints[row].m_rhs = info.fps * limot.m_stopERP * limot.m_currentLimitError * ( rotational ? -1 : 1 );
info.m_solverConstraints[row].m_lowerLimit = btScalar.BT_MIN_FLOAT;
info.m_solverConstraints[row].m_upperLimit = btScalar.BT_MAX_FLOAT;
info.m_solverConstraints[row].m_cfm = limot.m_stopCFM;
row++;
++count;
}
if( limot.m_enableMotor && !limot.m_servoMotor )
{
calculateJacobi( limot, ref transA, ref transB, info, row, ref ax1, rotational, rotAllowed );
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_motorERP );
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;
info.m_solverConstraints[row].m_cfm = limot.m_motorCFM;
row++;
++count;
}
if( limot.m_enableMotor && limot.m_servoMotor )
{
double error = limot.m_currentPosition - limot.m_servoTarget;
calculateJacobi( limot, ref transA, ref transB, info, row, ref ax1, rotational, rotAllowed );
double targetvelocity = error < 0 ? -limot.m_targetVelocity : limot.m_targetVelocity;
double tag_vel = -targetvelocity;
double mot_fact;
if( error != 0 )
{
double lowLimit;
double hiLimit;
if( limot.m_loLimit > limot.m_hiLimit )
{
lowLimit = error > 0 ? limot.m_servoTarget : btScalar.BT_MIN_FLOAT;
hiLimit = error < 0 ? limot.m_servoTarget : btScalar.BT_MAX_FLOAT;
}
else
{
lowLimit = error > 0 && limot.m_servoTarget > limot.m_loLimit ? limot.m_servoTarget : limot.m_loLimit;
hiLimit = error < 0 && limot.m_servoTarget < limot.m_hiLimit ? limot.m_servoTarget : limot.m_hiLimit;
}
mot_fact = getMotorFactor( limot.m_currentPosition, lowLimit, hiLimit, tag_vel, info.fps * limot.m_motorERP );
}
else
{
mot_fact = 0;
}
info.m_solverConstraints[row].m_rhs = mot_fact * targetvelocity * ( rotational ? -1 : 1 );
info.m_solverConstraints[row].m_lowerLimit = -limot.m_maxMotorForce;
info.m_solverConstraints[row].m_upperLimit = limot.m_maxMotorForce;
info.m_solverConstraints[row].m_cfm = limot.m_motorCFM;
row++;
//srow += info.rowskip;
++count;
}
if( limot.m_enableSpring )
{
double error = limot.m_currentPosition - limot.m_equilibriumPoint;
calculateJacobi( limot, ref transA, ref transB, info, row, ref ax1, rotational, rotAllowed );
//double cfm = 1.0 / ((1.0/info.fps)*limot.m_springStiffness+ limot.m_springDamping);
//if(cfm > 0.99999)
// cfm = 0.99999;
//double erp = (1.0/info.fps)*limot.m_springStiffness / ((1.0/info.fps)*limot.m_springStiffness + limot.m_springDamping);
//info.m_constraintError[srow] = info.fps * erp * error * (rotational ? -1.0 : 1.0);
//info.m_lowerLimit[srow] = -SIMD_INFINITY;
//info.m_upperLimit[srow] = SIMD_INFINITY;
double dt = btScalar.BT_ONE / info.fps;
double kd = limot.m_springDamping;
double ks = limot.m_springStiffness;
double vel = rotational ? angVelA.dot( ax1 ) - angVelB.dot( ax1 ) : linVelA.dot( ax1 ) - linVelB.dot( ax1 );
// double erp = 0.1;
double cfm = btScalar.BT_ZERO;
double mA = btScalar.BT_ONE / m_rbA.getInvMass();
double mB = btScalar.BT_ONE / m_rbB.getInvMass();
double m = mA > mB ? mB : mA;
double angularfreq = btScalar.btSqrt( ks / m );
//limit stiffness (the spring should not be sampled faster that the quarter of its angular frequency)
if( limot.m_springStiffnessLimited && 0.25 < angularfreq * dt )
{
ks = btScalar.BT_ONE / dt / dt / (double)( 16.0 ) * m;
}
//avoid damping that would blow up the spring
if( limot.m_springDampingLimited && kd * dt > m )
{
kd = m / dt;
}
double fs = ks * error * dt;
double fd = -kd * ( vel ) * ( rotational ? -1 : 1 ) * dt;
double f = ( fs + fd );
info.m_solverConstraints[row].m_rhs = ( vel + f * ( rotational ? -1 : 1 ) );
double minf = f < fd ? f : fd;
double maxf = f < fd ? fd : f;
if( !rotational )
{
info.m_solverConstraints[row].m_lowerLimit = minf > 0 ? 0 : minf;
info.m_solverConstraints[row].m_upperLimit = maxf < 0 ? 0 : maxf;
}
else
{
info.m_solverConstraints[row].m_lowerLimit = -maxf > 0 ? 0 : -maxf;
info.m_solverConstraints[row].m_upperLimit = -minf < 0 ? 0 : -minf;
}
info.m_solverConstraints[row].m_cfm = cfm;
row++;
++count;
}
return count;
}
/// cast a convex against another convex object
internal override bool calcTimeOfImpact(
ref btTransform fromA,
ref btTransform toA,
ref btTransform fromB,
ref btTransform toB,
CastResult result)
{
m_simplexSolver.reset();
/// compute linear velocity for this interval, to interpolate
//assume no rotation/angular velocity, assert here?
btVector3 linVelA, linVelB;
toA.m_origin.Sub( ref fromA.m_origin, out linVelA );
toB.m_origin.Sub( ref fromB.m_origin, out linVelB );
double radius = (double)( 0.001 );
double lambda = btScalar.BT_ZERO;
btVector3 v = btVector3.xAxis;
int maxIter = MAX_ITERATIONS;
btVector3 n = btVector3.Zero;
bool hasResult = false;
btVector3 c;
btVector3 r; linVelA.Sub( ref linVelB, out r );
double lastLambda = lambda;
//double epsilon = (double)(0.001);
int numIter = 0;
//first solution, using GJK
// result.drawCoordSystem(sphereTr);
btPointCollector pointCollector = new btPointCollector();
btGjkPairDetector gjk = BulletGlobals.GjkPairDetectorPool.Get();
gjk.Initialize( m_convexA, m_convexB, m_simplexSolver, null);//m_penetrationDepthSolver);
btGjkPairDetector.ClosestPointInput input = new btDiscreteCollisionDetectorInterface.ClosestPointInput();
//we don't use margins during CCD
// gjk.setIgnoreMargin(true);
input.m_transformA = fromA.T;
input.m_transformB = fromB.T;
gjk.getClosestPoints( input, pointCollector, null );
hasResult = pointCollector.m_hasResult;
c = pointCollector.m_pointInWorld;
if( hasResult )
{
double dist;
dist = pointCollector.m_distance;
n = pointCollector.m_normalOnBInWorld;
//not close enough
while( dist > radius )
{
numIter++;
if( numIter > maxIter )
{
BulletGlobals.GjkPairDetectorPool.Free( gjk );
return false; //todo: report a failure
}
double dLambda = btScalar.BT_ZERO;
double projectedLinearVelocity = r.dot( ref n );
dLambda = dist / ( projectedLinearVelocity );
lambda = lambda - dLambda;
if( lambda > btScalar.BT_ONE )
{
BulletGlobals.GjkPairDetectorPool.Free( gjk );
return false;
}
if( lambda < btScalar.BT_ZERO )
{
BulletGlobals.GjkPairDetectorPool.Free( gjk );
return false;
}
//todo: next check with relative epsilon
if( lambda <= lastLambda )
{
BulletGlobals.GjkPairDetectorPool.Free( gjk );
return false;
//n.setValue(0,0,0);
//break;
}
lastLambda = lambda;
//interpolate to next lambda
result.DebugDraw( lambda );
btVector3 tmp;
btVector3.setInterpolate3( out tmp, ref fromA.m_origin, ref toA.m_origin, lambda );
input.m_transformA.setOrigin( ref tmp );
btVector3.setInterpolate3( out tmp, ref fromB.m_origin, ref toB.m_origin, lambda );
input.m_transformB.setOrigin( ref tmp );
gjk.getClosestPoints( input, pointCollector, null );
if( pointCollector.m_hasResult )
{
if( pointCollector.m_distance < btScalar.BT_ZERO )
{
result.m_fraction = lastLambda;
n = pointCollector.m_normalOnBInWorld;
result.m_normal = n;
result.m_hitPoint = pointCollector.m_pointInWorld;
BulletGlobals.GjkPairDetectorPool.Free( gjk );
return true;
}
c = pointCollector.m_pointInWorld;
n = pointCollector.m_normalOnBInWorld;
dist = pointCollector.m_distance;
}
else
{
//??
BulletGlobals.GjkPairDetectorPool.Free( gjk );
return false;
}
}
//is n normalized?
//don't report time of impact for motion away from the contact normal (or causes minor penetration)
if( n.dot( ref r ) >= -result.m_allowedPenetration )
{
BulletGlobals.GjkPairDetectorPool.Free( gjk );
return false;
}
result.m_fraction = lambda;
result.m_normal = n;
result.m_hitPoint = c;
BulletGlobals.GjkPairDetectorPool.Free( gjk );
return true;
}
BulletGlobals.GjkPairDetectorPool.Free( gjk );
return false;
}
/*
void setAxis( ref btVector3 axis1, ref btVector3 axis2 );
void setBounce( int index, double bounce );
void enableMotor( int index, bool onOff );
void setServo( int index, bool onOff ); // set the type of the motor (servo or not) (the motor has to be turned on for servo also)
void setTargetVelocity( int index, double velocity );
void setServoTarget( int index, double target );
void setMaxMotorForce( int index, double force );
void enableSpring( int index, bool onOff );
void setStiffness( int index, double stiffness, bool limitIfNeeded = true ); // if limitIfNeeded is true the system will automatically limit the stiffness in necessary situations where otherwise the spring would move unrealistically too widely
void setDamping( int index, double damping, bool limitIfNeeded = true ); // if limitIfNeeded is true the system will automatically limit the damping in necessary situations where otherwise the spring would blow up
void setEquilibriumPoint(); // set the current constraint position/orientation as an equilibrium point for all DOF
void setEquilibriumPoint( int index ); // set the current constraint position/orientation as an equilibrium point for given DOF
void setEquilibriumPoint( int index, double val );
*/
//override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
//If no axis is provided, it uses the default axis for this constraint.
internal btGeneric6DofSpring2Constraint( btRigidBody rbA, btRigidBody rbB, ref btTransform frameInA, ref btTransform frameInB, RotateOrder rotOrder )
: base( btObjectTypes.D6_SPRING_2_CONSTRAINT_TYPE, rbA, rbB )
{
m_frameInA = ( frameInA );
m_frameInB = ( frameInB );
m_rotateOrder = ( rotOrder );
m_flags = ( 0 );
calculateTransforms();
}
//broken due to scaling
public override void getAabb( ref btTransform t, out btVector3 aabbMin, out btVector3 aabbMax )
{
btVector3 extent = new btVector3( getMargin() );
t.m_origin.Sub( ref extent, out aabbMin );
t.m_origin.Add( ref extent, out aabbMax );
}
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 virtual void drawCoordSystem( ref btTransform trans ) { }