public override void localGetSupportingVertex( ref btVector3 vec, out btVector3 supVertex )
{
localGetSupportingVertexWithoutMargin( ref vec, out supVertex );
if( getMargin() != btScalar.BT_ZERO )
{
btVector3 vecnorm = vec;
if( vecnorm.length2() < ( btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) )
{
vecnorm.setValue( (double)( -1.0 ), (double)( -1.0 ), (double)( -1.0 ) );
}
vecnorm.normalize();
supVertex.AddScale( ref vecnorm, getMargin(), out supVertex );
}
}
public static void btTransformAabb( ref btVector3 halfExtents, double margin, btITransform t
, out btVector3 aabbMinOut, out btVector3 aabbMaxOut )
{
btVector3 halfExtentsWithMargin; halfExtents.AddScale( ref btVector3.One, margin, out halfExtentsWithMargin );
btMatrix3x3 abs_b; t.getBasis().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 );
}
public static bool btRayAabb( ref btVector3 rayFrom,
ref btVector3 rayTo,
ref btVector3 aabbMin,
ref btVector3 aabbMax,
double param, ref btVector3 normal )
{
btVector3 aabbHalfExtent; aabbMax.SubScale( ref aabbMin , (double)( 0.5 ), out aabbHalfExtent );
btVector3 aabbCenter; aabbMax.AddScale( ref aabbMin, (double)( 0.5 ), out aabbCenter );
btVector3 source; rayFrom.Sub( ref aabbCenter, out source );
btVector3 target; rayTo.Sub( ref aabbCenter, out target );
int sourceOutcode = btOutcode( ref source, ref aabbHalfExtent );
int targetOutcode = btOutcode( ref target, ref aabbHalfExtent );
if( ( sourceOutcode & targetOutcode ) == 0x0 )
{
double lambda_enter = btScalar.BT_ZERO;
double lambda_exit = param;
btVector3 r; target.Sub( ref source, out r );
int i;
double normSign = 1;
btVector3 hitNormal = btVector3.Zero;
int bit = 1;
for( int j = 0; j < 2; j++ )
{
for( i = 0; i != 3; ++i )
{
if( ( sourceOutcode & bit ) != 0 )
{
double lambda = ( -source[i] - aabbHalfExtent[i] * normSign ) / r[i];
if( lambda_enter <= lambda )
{
lambda_enter = lambda;
hitNormal.setValue( 0, 0, 0 );
hitNormal[i] = normSign;
}
}
else if( ( targetOutcode & bit ) != 0 )
{
double lambda = ( -source[i] - aabbHalfExtent[i] * normSign ) / r[i];
btScalar.btSetMin( ref lambda_exit, lambda );
}
bit <<= 1;
}
normSign = (double)( -1.0);
}
if( lambda_enter <= lambda_exit )
{
param = lambda_enter;
normal = hitNormal;
return true;
}
}
return false;
}
public virtual void drawSpherePatch( ref btVector3 center, ref btVector3 up, ref btVector3 axis, double radius,
double minTh, double maxTh, double minPs, double maxPs, ref btVector3 color
, double stepDegrees = btScalar.BT_TEN
, bool drawCenter = true )
{
btVector3[] vA = new btVector3[74];
btVector3[] vB = new btVector3[74];
btVector3[] vT;
//btVector3* pvA = vA, *pvB = vB, *pT;
btVector3 npole; center.AddScale( ref up, radius, out npole );
btVector3 spole; center.AddScale( ref up, -radius, out spole );
btVector3 arcStart = btVector3.Zero;
double step = stepDegrees * btScalar.SIMD_RADS_PER_DEG;
btVector3 kv = up;
btVector3 iv = axis;
btVector3 jv; kv.cross( ref iv, out jv );
bool drawN = false;
bool drawS = false;
if( minTh <= -btScalar.SIMD_HALF_PI )
{
minTh = -btScalar.SIMD_HALF_PI + step;
drawN = true;
}
if( maxTh >= btScalar.SIMD_HALF_PI )
{
maxTh = btScalar.SIMD_HALF_PI - step;
drawS = true;
}
if( minTh > maxTh )
{
minTh = -btScalar.SIMD_HALF_PI + step;
maxTh = btScalar.SIMD_HALF_PI - step;
drawN = drawS = true;
}
int n_hor = (int)( ( maxTh - minTh ) / step ) + 1;
if( n_hor < 2 ) n_hor = 2;
double step_h = ( maxTh - minTh ) / (double)( n_hor - 1 );
bool isClosed = false;
if( minPs > maxPs )
{
minPs = -btScalar.SIMD_PI + step;
maxPs = btScalar.SIMD_PI;
isClosed = true;
}
else if( ( maxPs - minPs ) >= btScalar.SIMD_2_PI )
{
isClosed = true;
}
else
{
isClosed = false;
}
int n_vert = (int)( ( maxPs - minPs ) / step ) + 1;
if( n_vert < 2 ) n_vert = 2;
double step_v = ( maxPs - minPs ) / (double)( n_vert - 1 );
for( int i = 0; i < n_hor; i++ )
{
double th = minTh + (double)( i ) * step_h;
double sth = radius * btScalar.btSin( th );
double cth = radius * btScalar.btCos( th );
for( int j = 0; j < n_vert; j++ )
{
double psi = minPs + (double)( j ) * step_v;
double sps = btScalar.btSin( psi );
double cps = btScalar.btCos( psi );
center.AddScale( ref iv, cth * cps, out vB[j] );
vB[j].AddScale( ref jv, cth * sps, out vB[j] );
vB[j].AddScale( ref kv, sth, out vB[j] );
//pvB[j] = center + cth * cps * iv + cth * sps * jv + sth * kv;
if( i != 0 )
{
drawLine( ref vA[j], ref vB[j], ref color );
}
else if( drawS )
{
drawLine( ref spole, ref vB[j], ref color );
}
if( j != 0 )
{
drawLine( ref vB[j - 1], ref vB[j], ref color );
}
else
{
arcStart = vB[j];
}
if( ( i == ( n_hor - 1 ) ) && drawN )
{
drawLine( ref npole, ref vB[j], ref color );
}
if( drawCenter )
{
if( isClosed )
{
if( j == ( n_vert - 1 ) )
{
drawLine( ref arcStart, ref vB[j], ref color );
}
}
else
{
if( ( ( i== 0 ) || ( i == ( n_hor - 1 ) ) ) && ( ( j == 0 ) || ( j == ( n_vert - 1 ) ) ) )
{
drawLine( ref center, ref vB[j], ref color );
}
}
}
}
vT = vA; vA = vB; vB = vT;
//pT = pvA; pvA = pvB; pvB = pT;
}
}
public virtual void drawArc( ref btVector3 center, ref btVector3 normal, ref btVector3 axis
, double radiusA, double radiusB, double minAngle, double maxAngle,
ref btVector3 color, bool drawSect, double stepDegrees = btScalar.BT_TEN )
{
btVector3 vx = axis;
btVector3 vy; normal.cross( ref axis, out vy );
double step = stepDegrees * btScalar.SIMD_RADS_PER_DEG;
int nSteps = (int)btScalar.btFabs( ( maxAngle - minAngle ) / step );
if( nSteps == 0 ) nSteps = 1;
btVector3 prev;
center.AddScale( ref vx, radiusA * btScalar.btCos( minAngle ), out prev );
prev.AddScale( ref vy, radiusB * btScalar.btSin( minAngle ), out prev );
if( drawSect )
{
drawLine( ref center, ref prev, ref color );
}
for( int i = 1; i <= nSteps; i++ )
{
double angle = minAngle + ( maxAngle - minAngle ) * i / (double)nSteps;
btVector3 next;
center.AddScale( ref vx, radiusA * btScalar.btCos( angle ), out next );
next.AddScale( ref vy, radiusB * btScalar.btSin( angle ), out next );
drawLine( ref prev, ref next, ref color );
prev = next;
}
if( drawSect )
{
drawLine( ref center, ref prev, ref color );
}
}
public override void localGetSupportingVertex( ref btVector3 vec, out btVector3 supVertex )
{
localGetSupportingVertexWithoutMargin( ref vec, out supVertex );
btVector3 vecnorm = vec;
if( vecnorm.length2() < ( btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) )
{
vecnorm = btVector3.NegOne;
}
vecnorm.normalize();
supVertex.AddScale( ref vecnorm, getMargin(), out supVertex );
//supVertex += getMargin() * vecnorm;
}
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 );
}
public override void addContactPoint( ref btVector3 normalOnBInWorld, ref btVector3 pointInWorld, double orgDepth )
{
btVector3 endPt, startPt;
double newDepth;
//btVector3 newNormal;
if( m_perturbA )
{
btVector3 endPtOrg; pointInWorld.AddScale( ref normalOnBInWorld, orgDepth, out endPtOrg );
btTransform inv; m_transformA.inverse( out inv );
btTransform tmp;
m_unPerturbedTransform.Apply( ref inv, out tmp );
/*endPt =*/
tmp.Apply( ref endPtOrg, out endPt );
btVector3 tmp2;
endPt.Sub( ref pointInWorld, out tmp2 );
newDepth = tmp2.dot( ref normalOnBInWorld );
endPt.AddScale( ref normalOnBInWorld, newDepth, out startPt );
}
else
{
pointInWorld.AddScale( ref normalOnBInWorld, orgDepth, out endPt );
btTransform inv; m_transformB.inverse( out inv );
btTransform tmp;
m_unPerturbedTransform.Apply( ref inv, out tmp );
tmp.Apply( ref pointInWorld, out startPt );
//startPt = ( m_unPerturbedTransform * m_transformB.inverse() )( pointInWorld );
btVector3 tmp2;
endPt.Sub( ref startPt, out tmp2 );
newDepth = tmp2.dot( ref normalOnBInWorld );
}
//#define DEBUG_CONTACTS 1
#if DEBUG_CONTACTS
m_debugDrawer.drawLine(ref startPt,ref endPt,btVector3(1,0,0));
m_debugDrawer.drawSphere(ref startPt,0.05,btVector3(0,1,0));
m_debugDrawer.drawSphere(ref endPt,0.05,btVector3(0,0,1));
#endif //DEBUG_CONTACTS
m_originalManifoldResult.addContactPoint( ref normalOnBInWorld, ref startPt, newDepth );
}
internal void calculateTransforms( ref btTransform transA, ref btTransform transB )
{
if( m_useLinearReferenceFrameA /*|| ( !m_useSolveConstraintObsolete )*/ )
{
transA.Apply( ref m_frameInA, out m_calculatedTransformA );
transB.Apply( ref m_frameInB, out m_calculatedTransformB );
}
else
{
transA.Apply( ref m_frameInA, out m_calculatedTransformB );
transB.Apply( ref m_frameInB, out m_calculatedTransformA );
}
m_realPivotAInW = m_calculatedTransformA.m_origin;
m_realPivotBInW = m_calculatedTransformB.m_origin;
m_sliderAxis = m_calculatedTransformA.m_basis.getColumn( 0 ); // along X
/*
if( m_useLinearReferenceFrameA || m_useSolveConstraintObsolete )
{
m_delta = m_realPivotBInW - m_realPivotAInW;
}
else
*/
{
m_delta = m_realPivotAInW - m_realPivotBInW;
}
m_realPivotAInW.AddScale( ref m_sliderAxis, m_sliderAxis.dot( ref m_delta ), out m_projPivotInW );
//m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot( ref m_delta ) * m_sliderAxis;
btVector3 normalWorld;
int i;
//linear part
for( i = 0; i < 3; i++ )
{
normalWorld = m_calculatedTransformA.m_basis.getColumn( i );
m_depth[i] = m_delta.dot( normalWorld );
}
}
public override void addContactPoint( ref btVector3 normalOnBInWorld, ref btVector3 pointInWorld, double depth )
{
bool isSwapped = m_manifoldPtr.m_body0 != m_body0Wrap.m_collisionObject;
btVector3 pointA; pointInWorld.AddScale( ref normalOnBInWorld, depth, out pointA );
btVector3 localA;
btVector3 localB;
if( isSwapped )
{
m_body1Wrap.m_collisionObject.m_worldTransform.invXform( ref pointA, out localA );
m_body0Wrap.m_collisionObject.m_worldTransform.invXform( ref pointInWorld, out localB );
}
else
{
m_body0Wrap.m_collisionObject.m_worldTransform.invXform( ref pointA, out localA );
m_body1Wrap.m_collisionObject.m_worldTransform.invXform( ref pointInWorld, out localB );
}
btManifoldPoint newPt = BulletGlobals.ManifoldPointPool.Get();
newPt.Initialize( ref localA, ref localB, ref normalOnBInWorld, depth );
btScalar.Dbg( "2 add contact point " + pointA.ToString() + " and " + pointInWorld.ToString() );
newPt.m_positionWorldOnA = pointA;
newPt.m_positionWorldOnB = pointInWorld;
//BP mod, store contact triangles.
if( isSwapped )
{
newPt.m_partId0 = m_partId1;
newPt.m_partId1 = m_partId0;
newPt.m_index0 = m_index1;
newPt.m_index1 = m_index0;
}
else
{
newPt.m_partId0 = m_partId0;
newPt.m_partId1 = m_partId1;
newPt.m_index0 = m_index0;
newPt.m_index1 = m_index1;
}
//experimental feature info, for per-triangle material etc.
btCollisionObjectWrapper obj0Wrap = isSwapped ? m_body1Wrap : m_body0Wrap;
btCollisionObjectWrapper obj1Wrap = isSwapped ? m_body0Wrap : m_body1Wrap;
m_resultCallback.addSingleResult( newPt, obj0Wrap, newPt.m_partId0, newPt.m_index0, obj1Wrap, newPt.m_partId1, newPt.m_index1 );
}
// See also geometrictools.com
// Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv
double SegmentSqrDistance( ref btVector3 from, ref btVector3 to, ref btVector3 p, out btVector3 nearest )
{
btVector3 diff = p - from;
btVector3 v = to - from;
double t = v.dot( diff );
if( t > 0 )
{
double dotVV = v.dot( v );
if( t < dotVV )
{
t /= dotVV;
diff.AddScale( ref v, -t, out diff );
//diff -= t * v;
}
else
{
t = 1;
diff.Sub( ref v, out diff );
//diff -= v;
}
}
else
t = 0;
from.AddScale( ref v, t, out nearest );
//nearest = from + t * v;
return diff.dot( ref diff );
}
public virtual void addContactPoint( ref btVector3 normalOnBInWorld, ref btVector3 pointInWorld, double depth )
{
Debug.Assert( m_manifoldPtr != null );
//order in manifold needs to match
if( depth > m_manifoldPtr.getContactBreakingThreshold() )
// if (depth > m_manifoldPtr.getContactProcessingThreshold())
return;
bool isSwapped = m_manifoldPtr.m_body0 != m_body0Wrap.m_collisionObject;
btVector3 pointA;// = pointInWorld + normalOnBInWorld * depth;
pointInWorld.AddScale( ref normalOnBInWorld, depth, out pointA );
btVector3 localA;
btVector3 localB;
if( isSwapped )
{
m_body1Wrap.m_collisionObject.m_worldTransform.invXform( ref pointA, out localA );
m_body0Wrap.m_collisionObject.m_worldTransform.invXform( ref pointInWorld, out localB );
}
else
{
m_body0Wrap.m_collisionObject.m_worldTransform.invXform( ref pointA, out localA );
m_body1Wrap.m_collisionObject.m_worldTransform.invXform( ref pointInWorld, out localB );
}
btManifoldPoint newPt = BulletGlobals.ManifoldPointPool.Get();
newPt.Initialize( ref localA, ref localB, ref normalOnBInWorld, depth );
btScalar.Dbg( "add contact point " + pointA + " and " + pointInWorld );
newPt.m_positionWorldOnA = pointA;
newPt.m_positionWorldOnB = pointInWorld;
int insertIndex = m_manifoldPtr.getCacheEntry( ref newPt );
newPt.m_combinedFriction = calculateCombinedFriction( m_body0Wrap.m_collisionObject, m_body1Wrap.m_collisionObject );
newPt.m_combinedRestitution = calculateCombinedRestitution( m_body0Wrap.m_collisionObject, m_body1Wrap.m_collisionObject );
newPt.m_combinedRollingFriction = calculateCombinedRollingFriction( m_body0Wrap.m_collisionObject, m_body1Wrap.m_collisionObject );
btVector3.btPlaneSpace1( ref newPt.m_normalWorldOnB, out newPt.m_lateralFrictionDir1, out newPt.m_lateralFrictionDir2 );
//BP mod, store contact triangles.
if( isSwapped )
{
newPt.m_partId0 = m_partId1;
newPt.m_partId1 = m_partId0;
newPt.m_index0 = m_index1;
newPt.m_index1 = m_index0;
}
else
{
newPt.m_partId0 = m_partId0;
newPt.m_partId1 = m_partId1;
newPt.m_index0 = m_index0;
newPt.m_index1 = m_index1;
}
//Console.WriteLine("depth=%f\n",depth);
///@todo, check this for any side effects
if( insertIndex >= 0 )
{
//btManifoldPoint oldPoint = m_manifoldPtr.getContactPoint(insertIndex);
m_manifoldPtr.replaceContactPoint( newPt, insertIndex );
}
else
{
insertIndex = m_manifoldPtr.addManifoldPoint( newPt );
}
//User can override friction and/or restitution
if( gContactAddedCallback != null &&
//and if either of the two bodies requires custom material
( ( m_body0Wrap.m_collisionObject.getCollisionFlags() & btCollisionObject.CollisionFlags.CF_CUSTOM_MATERIAL_CALLBACK ) != 0 ||
( m_body1Wrap.m_collisionObject.getCollisionFlags() & btCollisionObject.CollisionFlags.CF_CUSTOM_MATERIAL_CALLBACK ) != 0 ) )
{
//experimental feature info, for per-triangle material etc.
btCollisionObjectWrapper obj0Wrap = isSwapped ? m_body1Wrap : m_body0Wrap;
btCollisionObjectWrapper obj1Wrap = isSwapped ? m_body0Wrap : m_body1Wrap;
gContactAddedCallback( m_manifoldPtr.getContactPoint( insertIndex ), obj0Wrap, newPt.m_partId0, newPt.m_index0, obj1Wrap, newPt.m_partId1, newPt.m_index1 );
}
}
