internal static void calculateVelocityQuaternion( ref btVector3 pos0, ref btVector3 pos1, ref btQuaternion orn0, ref btQuaternion orn1, double timeStep, out btVector3 linVel, out btVector3 angVel )
{
btVector3 tmp;
pos1.Sub( ref pos0, out tmp );
tmp.Div( timeStep, out linVel );
btVector3 axis;
double angle;
if( !orn0.Equals(ref orn1 ) )
{
calculateDiffAxisAngleQuaternion( ref orn0, ref orn1, out axis, out angle );
axis.Mult( angle, out tmp );
tmp.Div( timeStep, out angVel );
}
else
{
angVel = btVector3.Zero;
}
}
public static void TriNormal( ref btVector3 v0, ref btVector3 v1, ref btVector3 v2, out btVector3 result )
{
// return the normal of the triangle
// inscribed by v0, v1, and v2
btVector3 tmp1;
btVector3 tmp2;
v1.Sub( ref v0, out tmp1 );
v2.Sub( ref v1, out tmp2 );
btVector3 cp;
tmp1.cross( ref tmp2, out cp );
double m = cp.length();
if( m == 0 )
{
result = btVector3.xAxis;
return;
}
cp.Mult( ( 1.0 / m ), out result );
}
bool pointInTriangle(
ref btVector3 vertice0,
ref btVector3 vertice1,
ref btVector3 vertice2,
ref btVector3 normal, ref btVector3 p )
{
btVector3 edge1; vertice1.Sub( ref vertice0, out edge1 );// ( *p2 - *p1 );
btVector3 edge2; vertice2.Sub( ref vertice1, out edge2 );//( *p3 - *p2 );
btVector3 edge3; vertice0.Sub( ref vertice2, out edge3 );//( *p1 - *p3 );
btVector3 p1_to_p; p.Sub( ref vertice0, out p1_to_p );// ( *p - *p1 );
btVector3 p2_to_p; p.Sub( ref vertice1, out p2_to_p );// ( *p - *p2 );
btVector3 p3_to_p; p.Sub( ref vertice2, out p3_to_p );// ( *p - *p3 );
btVector3 edge1_normal; edge1.cross( ref normal, out edge1_normal );
btVector3 edge2_normal; edge2.cross( ref normal, out edge2_normal );
btVector3 edge3_normal; edge3.cross( ref normal, out edge3_normal );
double r1, r2, r3;
r1 = edge1_normal.dot( ref p1_to_p );
r2 = edge2_normal.dot( ref p2_to_p );
r3 = edge3_normal.dot( ref p3_to_p );
if( ( r1 > 0 && r2 > 0 && r3 > 0 ) ||
( r1 <= 0 && r2 <= 0 && r3 <= 0 ) )
return true;
return false;
}
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 static void rayTest( btDbvtNode root,
ref btVector3 rayFrom,
ref btVector3 rayTo,
ICollide policy )
{
using( btDbvtStackDataBlock stackDataBlock = new btDbvtStackDataBlock() )
{
if( root != null )
{
btVector3 rayDir = ( rayTo - rayFrom );
rayDir.normalize();
///what about division by zero? -. just set rayDirection[i] to INF/BT_LARGE_FLOAT
btVector3 rayDirectionInverse = new btVector3(
rayDir.x == 0.0f ? btScalar.BT_LARGE_FLOAT : 1.0f / rayDir.x,
rayDir.y == 0.0f ? btScalar.BT_LARGE_FLOAT : 1.0f / rayDir.y,
rayDir.z == 0.0f ? btScalar.BT_LARGE_FLOAT : 1.0f / rayDir.z );
stackDataBlock.signs[0] = rayDirectionInverse.x < 0.0f ? (uint)1 :0;
stackDataBlock.signs[1] = rayDirectionInverse.y < 0.0f ? (uint)1 :0;
stackDataBlock.signs[2] = rayDirectionInverse.z < 0.0f ? (uint)1 :0;
btVector3 tmp; rayTo.Sub( ref rayFrom, out tmp );
double lambda_max = btVector3.dot( ref rayDir, ref tmp );
int depth = 1;
int treshold = DOUBLE_STACKSIZE - 2;
stackDataBlock.stack.Count = ( DOUBLE_STACKSIZE );
stackDataBlock.stack[0] = root;
do
{
btDbvtNode node = stackDataBlock.stack[--depth];
stackDataBlock.bounds0 = node.volume._min;
stackDataBlock.bounds1 = node.volume._max;
double tmin = 1.0f, lambda_min = 0.0f;
bool result1 = btAabbUtil.btRayAabb2( ref rayFrom, ref rayDirectionInverse, stackDataBlock.signs
, ref stackDataBlock.bounds0
, ref stackDataBlock.bounds1
, out tmin, lambda_min, lambda_max );
#if COMPARE_BTRAY_AABB2
double param=1.0f;
bool result2 = AabbUtil.RayAabb(ref rayFrom,ref rayTo,node.volume.Mins(),node.volume.Maxs(),param,resultNormal);
Debug.Assert(result1 == result2);
#endif //TEST_BTRAY_AABB2
if( result1 )
{
if( node.IsInternal() )
{
if( depth > treshold )
{
stackDataBlock.stack.Count = ( stackDataBlock.stack.Count * 2 );
treshold = stackDataBlock.stack.Count - 2;
}
stackDataBlock.stack[depth++] = node._children0;
stackDataBlock.stack[depth++] = node._children1;
}
else
{
policy.Process( node );
}
}
} while( depth != 0 );
}
}
}
static double projectorigin( ref btVector3 a,
ref btVector3 b,
ref btVector3 c,
double[] w, out uint m )
{
m = 100;
int[] imd3 = { 1, 2, 0 };
btVector3[] vt = { a, b, c };
btVector3[] dl = new btVector3[3];
a.Sub( ref b, out dl[0] );
b.Sub( ref c, out dl[1] );
c.Sub( ref a, out dl[2] );
btVector3 n; btVector3.btCross( ref dl[0], ref dl[1], out n );
double l = n.length2();
if( l > GJK_SIMPLEX3_EPS )
{
double mindist = -1;
double[] subw = { 0, 0 };
uint subm = ( 0 );
for( int i = 0; i < 3; ++i )
{
btVector3 tmp;
btVector3.btCross( ref dl[i], ref n, out tmp );
if( btVector3.btDot( ref vt[i], ref tmp ) > 0 )
{
int j = imd3[i];
double subd = ( projectorigin( ref vt[i], ref vt[j], subw, out subm ) );
if( ( mindist < 0 ) || ( subd < mindist ) )
{
mindist = subd;
m = (uint)( ( ( subm & 1 ) != 0 ? 1 << i : 0 ) + ( ( subm & 2 ) != 0 ? 1 << j : 0 ) );
w[i] = subw[0];
w[j] = subw[1];
w[imd3[j]] = 0;
}
}
}
if( mindist < 0 )
{
double d = btVector3.btDot( ref a, ref n );
double s = btScalar.btSqrt( l );
btVector3 p; n.Mult( ( d / l ), out p );
mindist = p.length2();
m = 7;
btVector3 tmp, tmp2;
b.Sub( ref p, out tmp );
dl[1].cross( ref tmp, out tmp2 );
w[0] = ( tmp2 ).length() / s;
c.Sub( ref p, out tmp );
dl[2].cross( ref tmp, out tmp2 );
w[1] = ( tmp2 ).length() / s;
w[2] = 1 - ( w[0] + w[1] );
}
return ( mindist );
}
return ( -1 );
}
static public void segmentsClosestPoints(
out btVector3 ptsVector,
out btVector3 offsetA,
out btVector3 offsetB,
out double tA, out double tB,
ref btVector3 translation,
ref btVector3 dirA, double hlenA,
ref btVector3 dirB, double hlenB )
{
// compute the parameters of the closest points on each line segment
double dirA_dot_dirB = btVector3.btDot( ref dirA, ref dirB );
double dirA_dot_trans = btVector3.btDot( ref dirA, ref translation );
double dirB_dot_trans = btVector3.btDot( ref dirB, ref translation );
double denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB;
if( denom == 0.0f )
{
tA = 0.0f;
}
else
{
tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom;
if( tA < -hlenA )
tA = -hlenA;
else if( tA > hlenA )
tA = hlenA;
}
tB = tA * dirA_dot_dirB - dirB_dot_trans;
if( tB < -hlenB )
{
tB = -hlenB;
tA = tB * dirA_dot_dirB + dirA_dot_trans;
if( tA < -hlenA )
tA = -hlenA;
else if( tA > hlenA )
tA = hlenA;
}
else if( tB > hlenB )
{
tB = hlenB;
tA = tB * dirA_dot_dirB + dirA_dot_trans;
if( tA < -hlenA )
tA = -hlenA;
else if( tA > hlenA )
tA = hlenA;
}
// compute the closest points relative to segment centers.
dirA.Mult( tA, out offsetA );
dirB.Mult( tB, out offsetB );
btVector3 tmp;
translation.Sub( ref offsetA, out tmp );
tmp.Add( ref offsetB, out ptsVector );
//ptsVector = translation - offsetA + offsetB;
}
public void PlaneProject( btVector3 point, out btVector3 result )
{
btVector3 tmp;
normal.Mult( ( point.dot( ref normal ) + dist ), out tmp );
point.Sub( ref tmp, out result );
}
static double calcArea4Points( ref btVector3 p0, ref btVector3 p1, ref btVector3 p2, ref btVector3 p3 )
{
// It calculates possible 3 area constructed from random 4 points and returns the biggest one.
btVector3 a1, a2, a3;
btVector3 b1, b2, b3;
p0.Sub( ref p1, out a1 );
p0.Sub( ref p2, out a2 );
p0.Sub( ref p3, out a3 );
p2.Sub( ref p3, out b1 );
p1.Sub( ref p2, out b2 );
p1.Sub( ref p3, out b3 );
//todo: Following 3 cross production can be easily optimized by SIMD.
btVector3 tmp0; a1.cross( ref b1, out tmp0 );
btVector3 tmp1; a2.cross( ref b2, out tmp1 );
btVector3 tmp2; a3.cross( ref b3, out tmp2 );
return btScalar.btMax( btScalar.btMax( tmp0.length2(), tmp1.length2() ), tmp2.length2() );
}
public static double btDelLength( ref btVector3 linVelB, ref btVector3 linVelA )
{
btVector3 tmp;
linVelB.Sub( ref linVelA, out tmp );
return tmp.length();
}
public static void btCross2Del( ref btVector3 a, ref btVector3 b, ref btVector3 c, ref btVector3 d, out btVector3 result )
{
btVector3 tmp1;
btVector3 tmp2;
a.Sub( ref b, out tmp1 );
c.Sub( ref d, out tmp2 );
btCross( ref tmp1, ref tmp2, out result );
}
/*@brief Return the distance between the ends of this and another vector
This is symantically treating the vector like a point */
public double distance( ref btVector3 v )
{
btVector3 tmp;
v.Sub( ref this, out tmp );
return tmp.length();
}
public static double btDelLength2( ref btVector3 max, ref btVector3 min )
{
btVector3 tmp;
max.Sub( ref min, out tmp );
return tmp.dot( ref tmp );
}
public static double BetweenLength2( ref btVector3 min, ref btVector3 max )
{
btVector3 tmp;
max.Sub( ref min, out tmp );
return tmp.dot( ref tmp );
}
static bool above( btVector3[] vertices, int3 t, ref btVector3 p, double epsilon )
{
btVector3 n;
btPlane.TriNormal( ref vertices[t[0]], ref vertices[t[1]], ref vertices[t[2]], out n );
p.Sub( ref vertices[t[0]], out p );
return ( n.dot( ref p ) > epsilon ); // btScalar.SIMD_EPSILON???
}
public void PlaneLineIntersection( ref btVector3 p0, ref btVector3 p1, out btVector3 result )
{
// returns the point where the line p0-p1 intersects the plane n&d
btVector3 dif;
p1.Sub( ref p0, out dif );
double dn = normal.dot( ref dif );
double t = -( dist + normal.dot( ref p0 ) ) / dn;
dif.Mult( t, out dif );
p0.Add( ref dif, out result );
}
public void invXform( ref btVector3 inVec, out btVector3 result )
{
btVector3 v;
inVec.Sub( ref m_origin, out v );
btMatrix3x3 tmp;
m_basis.transpose( out tmp );
tmp.Apply( ref v, out result );
}
public void SetCenterOfMass( btVector3 position )
{
btVector3 delta; position.Sub( ref m_centerOfMass, out delta );
m_centerOfMass = position;
delta.Invert( out delta );
foreach( btCompoundShapeChild shape in m_children )
{
shape.m_transform.Move( ref delta );
}
}
///calculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep)
///result is conservative
public void calculateTemporalAabb( ref btTransform curTrans, ref btVector3 linvel, ref btVector3 angvel, double timeStep
, out btVector3 temporalAabbMin, out btVector3 temporalAabbMax )
{
//start with static aabb
getAabb( ref curTrans, out temporalAabbMin, out temporalAabbMax );
double temporalAabbMaxx = temporalAabbMax.x;
double temporalAabbMaxy = temporalAabbMax.y;
double temporalAabbMaxz = temporalAabbMax.z;
double temporalAabbMinx = temporalAabbMin.x;
double temporalAabbMiny = temporalAabbMin.y;
double temporalAabbMinz = temporalAabbMin.z;
// add linear motion
btVector3 linMotion;
linvel.Mult( timeStep, out linMotion );
///@todo: simd would have a vector max/min operation, instead of per-element access
if( linMotion.x > btScalar.BT_ZERO )
temporalAabbMaxx += linMotion.x;
else
temporalAabbMinx += linMotion.x;
if( linMotion.y > btScalar.BT_ZERO )
temporalAabbMaxy += linMotion.y;
else
temporalAabbMiny += linMotion.y;
if( linMotion.z > btScalar.BT_ZERO )
temporalAabbMaxz += linMotion.z;
else
temporalAabbMinz += linMotion.z;
//add conservative angular motion
double angularMotion = angvel.length() * getAngularMotionDisc() * timeStep;
btVector3 angularMotion3d = new btVector3( angularMotion, angularMotion, angularMotion );
temporalAabbMin = new btVector3( temporalAabbMinx, temporalAabbMiny, temporalAabbMinz );
temporalAabbMax = new btVector3( temporalAabbMaxx, temporalAabbMaxy, temporalAabbMaxz );
temporalAabbMin.Sub( ref angularMotion3d, out temporalAabbMin );
temporalAabbMax.Add( ref angularMotion3d, out temporalAabbMax );
}
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 bool updateClosestVectorAndPoints()
{
if( m_needsUpdate )
{
m_cachedBC.reset();
m_needsUpdate = false;
switch( m_numVertices )
{
case 0:
m_cachedValidClosest = false;
break;
case 1:
{
m_cachedP1 = m_simplexPointsP0;
m_cachedP2 = m_simplexPointsQ0;
m_cachedP1.Sub( ref m_cachedP2, out m_cachedV ); //== m_simplexVectorW[0]
m_cachedBC.reset();
m_cachedBC.setBarycentricCoordinates( btScalar.BT_ONE, btScalar.BT_ZERO, btScalar.BT_ZERO, btScalar.BT_ZERO );
m_cachedValidClosest = m_cachedBC.isValid();
break;
};
case 2:
{
//closest point origin from line segment
//btVector3 from = ;
//btVector3 to = m_simplexVectorW[1];
btVector3 nearest;
//btVector3 p = btVector3.Zero;
btVector3 diff; m_simplexVectorW0.Invert( out diff );
btVector3 v; m_simplexVectorW1.Sub( ref m_simplexVectorW0, out v );
double t = v.dot( ref diff );
btVector3 tmp;
if( t > 0 )
{
double dotVV = v.dot( ref v );
if( t < dotVV )
{
t /= dotVV;
diff.AddScale( ref v, -t, out diff );// -= t * v;
m_cachedBC.m_usedVertices |= btUsageBitfield.usedVertexA;
m_cachedBC.m_usedVertices |= btUsageBitfield.usedVertexB;
}
else
{
t = 1;
diff.Sub( ref v, out diff );
//reduce to 1 point
m_cachedBC.m_usedVertices |= btUsageBitfield.usedVertexB;
}
}
else
{
t = 0;
//reduce to 1 point
m_cachedBC.m_usedVertices |= btUsageBitfield.usedVertexA;
}
m_cachedBC.setBarycentricCoordinates( 1 - t, t );
v.Mult( t, out tmp );
m_simplexVectorW0.Add( ref tmp, out nearest );
m_simplexPointsP1.Sub( ref m_simplexPointsP0, out tmp );
m_simplexPointsP0.AddScale( ref tmp, t, out m_cachedP1 );
m_simplexPointsQ1.Sub( ref m_simplexPointsQ0, out tmp );
m_simplexPointsQ0.AddScale( ref tmp, t, out m_cachedP2 );
m_cachedP1.Sub( ref m_cachedP2, out m_cachedV );
reduceVertices( m_cachedBC.m_usedVertices );
m_cachedValidClosest = m_cachedBC.isValid();
break;
}
case 3:
{
//closest point origin from triangle
btVector3 p = btVector3.Zero;
closestPtPointTriangle( ref p, ref m_simplexVectorW0, ref m_simplexVectorW1, ref m_simplexVectorW2, ref m_cachedBC );
btVector3 tmp, tmp2;
m_simplexPointsP0.Mult( m_cachedBC.m_barycentricCoord0, out tmp );
m_simplexPointsP1.Mult( m_cachedBC.m_barycentricCoord1, out tmp2 );
tmp.Add( ref tmp2, out tmp );
m_simplexPointsP2.Mult( m_cachedBC.m_barycentricCoord2, out tmp2 );
tmp.Add( ref tmp2, out m_cachedP1 );
//m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoord0 +
// m_simplexPointsP[1] * m_cachedBC.m_barycentricCoord1 +
// m_simplexPointsP[2] * m_cachedBC.m_barycentricCoord2;
m_simplexPointsQ0.Mult( m_cachedBC.m_barycentricCoord0, out tmp );
m_simplexPointsQ1.Mult( m_cachedBC.m_barycentricCoord1, out tmp2 );
tmp.Add( ref tmp2, out tmp );
m_simplexPointsQ2.Mult( m_cachedBC.m_barycentricCoord2, out tmp2 );
tmp.Add( ref tmp2, out m_cachedP2 );
//m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoord0 +
// m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoord1 +
// m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoord2;
m_cachedP1.Sub( ref m_cachedP2, out m_cachedV );
//m_cachedV = m_cachedP1 - m_cachedP2;
reduceVertices( m_cachedBC.m_usedVertices );
m_cachedValidClosest = m_cachedBC.isValid();
break;
}
case 4:
{
btVector3 p = btVector3.Zero;
bool hasSeperation = closestPtPointTetrahedron( ref p, ref m_simplexVectorW0, ref m_simplexVectorW1, ref m_simplexVectorW2, ref m_simplexVectorW3, m_cachedBC );
if( hasSeperation )
{
btVector3 tmp, tmp2;
m_simplexPointsP0.Mult( m_cachedBC.m_barycentricCoord0, out tmp );
m_simplexPointsP1.Mult( m_cachedBC.m_barycentricCoord1, out tmp2 );
tmp.Add( ref tmp2, out tmp );
m_simplexPointsP2.Mult( m_cachedBC.m_barycentricCoord2, out tmp2 );
tmp.Add( ref tmp2, out tmp );
m_simplexPointsP3.Mult( m_cachedBC.m_barycentricCoord3, out tmp2 );
tmp.Add( ref tmp2, out m_cachedP1 );
//m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoord0 +
// m_simplexPointsP[1] * m_cachedBC.m_barycentricCoord1 +
// m_simplexPointsP[2] * m_cachedBC.m_barycentricCoord2 +
// m_simplexPointsP[3] * m_cachedBC.m_barycentricCoord3;
m_simplexPointsQ0.Mult( m_cachedBC.m_barycentricCoord0, out tmp );
m_simplexPointsQ1.Mult( m_cachedBC.m_barycentricCoord1, out tmp2 );
tmp.Add( ref tmp2, out tmp );
m_simplexPointsQ2.Mult( m_cachedBC.m_barycentricCoord2, out tmp2 );
tmp.Add( ref tmp2, out tmp );
m_simplexPointsQ3.Mult( m_cachedBC.m_barycentricCoord3, out tmp2 );
tmp.Add( ref tmp2, out m_cachedP2 );
//m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoord0 +
// m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoord1 +
// m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoord2 +
// m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoord3;
m_cachedP1.Sub( ref m_cachedP2, out m_cachedV );
//m_cachedV = m_cachedP1 - m_cachedP2;
reduceVertices( m_cachedBC.m_usedVertices );
}
else
{
// Console.WriteLine("sub distance got penetration\n");
if( m_cachedBC.m_degenerate )
{
m_cachedValidClosest = false;
}
else
{
m_cachedValidClosest = true;
//degenerate case == false, penetration = true + zero
m_cachedV.setValue( btScalar.BT_ZERO, btScalar.BT_ZERO, btScalar.BT_ZERO );
}
break;
}
m_cachedValidClosest = m_cachedBC.isValid();
//closest point origin from tetrahedron
break;
}
default:
{
m_cachedValidClosest = false;
}
break;
};
}
return m_cachedValidClosest;
}
static double projectorigin( ref btVector3 a,
ref btVector3 b,
ref btVector3 c,
ref btVector3 d,
double[] w, out uint m )
{
m = 100;
uint[] imd3 = { 1, 2, 0 };
btVector3[] vt = { a, b, c, d };
btVector3[] dl = new btVector3[3];// { a - d, b - d, c - d };
a.Sub( ref d, out dl[0] );
b.Sub( ref d, out dl[1] );
c.Sub( ref d, out dl[2] );
double vl = btVector3.det( ref dl[0], ref dl[1], ref dl[2] );
btVector3 tmp;
btVector3.btCross2Del( ref b, ref c, ref a, ref b, out tmp );
bool ng = ( vl * btVector3.btDot( ref a, ref tmp ) ) <= 0;
if( ng && ( btScalar.btFabs( vl ) > GJK_SIMPLEX4_EPS ) )
{
double mindist = -1;
double[] subw = { 0, 0, 0 };
uint subm = ( 0 );
for( int i = 0; i < 3; ++i )
{
uint j = imd3[i];
btVector3 tmp2;
btVector3.btCross( ref dl[i], ref dl[j], out tmp2 );
double s = vl * btVector3.btDot( ref d, ref tmp2 );
if( s > 0 )
{
double subd = projectorigin( ref vt[i], ref vt[j], ref d, subw, out subm );
if( ( mindist < 0 ) || ( subd < mindist ) )
{
mindist = subd;
m = (uint)( ( ( ( subm & 1 ) != 0 ) ? 1 << i : 0 ) +
( ( ( subm & 2 ) != 0 ) ? 1 << (int)j : 0 ) +
( ( ( subm & 4 ) != 0 ) ? 8 : 0 ) );
w[i] = subw[0];
w[j] = subw[1];
w[imd3[j]] = 0;
w[3] = subw[2];
}
}
}
if( mindist < 0 )
{
mindist = 0;
m = 15;
w[0] = btVector3.det( ref c, ref b, ref d ) / vl;
w[1] = btVector3.det( ref a, ref c, ref d ) / vl;
w[2] = btVector3.det( ref b, ref a, ref d ) / vl;
w[3] = 1 - ( w[0] + w[1] + w[2] );
}
return ( mindist );
}
return ( -1 );
}
public bool closestPtPointTriangle( ref btVector3 p, ref btVector3 a, ref btVector3 b, ref btVector3 c, ref btSubSimplexClosestResult result )
{
result.m_usedVertices = 0;//.reset();
// Check if P in vertex region outside A
btVector3 ab; b.Sub( ref a, out ab );
btVector3 ac; c.Sub( ref a, out ac );
btVector3 ap; p.Sub( ref a, out ap );
double d1 = ab.dot( ref ap );
double d2 = ac.dot( ref ap );
if( d1 <= (double)( 0.0 ) && d2 <= (double)( 0.0 ) )
{
result.m_closestPointOnSimplex = a;
result.m_usedVertices |= btUsageBitfield.usedVertexA;
result.setBarycentricCoordinates( 1, 0, 0 );
return true;// a; // barycentric coordinates (1,0,0)
}
// Check if P in vertex region outside B
btVector3 bp; p.Sub( ref b, out bp );
double d3 = ab.dot( ref bp );
double d4 = ac.dot( ref bp );
if( d3 >= (double)( 0.0 ) && d4 <= d3 )
{
result.m_closestPointOnSimplex = b;
result.m_usedVertices |= btUsageBitfield.usedVertexB;
result.setBarycentricCoordinates( 0, 1, 0 );
return true; // b; // barycentric coordinates (0,1,0)
}
// Check if P in edge region of AB, if so return projection of P onto AB
double vc = d1 * d4 - d3 * d2;
btVector3 tmp;
if( vc <= (double)( 0.0 ) && d1 >= (double)( 0.0 ) && d3 <= (double)( 0.0 ) )
{
double v = d1 / ( d1 - d3 );
ab.Mult( v, out tmp );
a.Add( ref tmp, out result.m_closestPointOnSimplex );
result.m_usedVertices |= btUsageBitfield.usedVertexA;
result.m_usedVertices |= btUsageBitfield.usedVertexB;
result.setBarycentricCoordinates( 1 - v, v, 0 );
return true;
//return a + v * ab; // barycentric coordinates (1-v,v,0)
}
// Check if P in vertex region outside C
btVector3 cp; p.Sub( ref c, out cp );
double d5 = ab.dot( ref cp );
double d6 = ac.dot( ref cp );
if( d6 >= (double)( 0.0 ) && d5 <= d6 )
{
result.m_closestPointOnSimplex = c;
result.m_usedVertices |= btUsageBitfield.usedVertexC;
result.setBarycentricCoordinates( 0, 0, 1 );
return true;//c; // barycentric coordinates (0,0,1)
}
// Check if P in edge region of AC, if so return projection of P onto AC
double vb = d5 * d2 - d1 * d6;
if( vb <= (double)( 0.0 ) && d2 >= (double)( 0.0 ) && d6 <= (double)( 0.0 ) )
{
double w = d2 / ( d2 - d6 );
ac.Mult( w, out tmp );
a.Add( ref tmp, out result.m_closestPointOnSimplex );
result.m_usedVertices |= btUsageBitfield.usedVertexA;
result.m_usedVertices |= btUsageBitfield.usedVertexC;
result.setBarycentricCoordinates( 1 - w, 0, w );
return true;
//return a + w * ac; // barycentric coordinates (1-w,0,w)
}
// Check if P in edge region of BC, if so return projection of P onto BC
double va = d3 * d6 - d5 * d4;
if( va <= (double)( 0.0 ) && ( d4 - d3 ) >= (double)( 0.0 ) && ( d5 - d6 ) >= (double)( 0.0 ) )
{
double w = ( d4 - d3 ) / ( ( d4 - d3 ) + ( d5 - d6 ) );
btVector3 tmp2;
c.Sub( ref b, out tmp2 );
tmp2.Mult( w, out tmp );
b.Add( ref tmp, out result.m_closestPointOnSimplex );
//result.m_closestPointOnSimplex = b + w * ( c - b );
result.m_usedVertices |= btUsageBitfield.usedVertexB;
result.m_usedVertices |= btUsageBitfield.usedVertexC;
result.setBarycentricCoordinates( 0, 1 - w, w );
return true;
// return b + w * (c - b); // barycentric coordinates (0,1-w,w)
}
// P inside face region. Compute Q through its barycentric coordinates (u,v,w)
{
double denom = (double)( 1.0 ) / ( va + vb + vc );
double v = vb * denom;
double w = vc * denom;
ab.Mult( v, out ab );
ac.Mult( w, out ac );
a.Add( ref ab, out tmp );
tmp.Add( ref ac, out result.m_closestPointOnSimplex );
//result.m_closestPointOnSimplex = a + ab * v + ac * w;
result.m_usedVertices |= btUsageBitfield.usedVertexA;
result.m_usedVertices |= btUsageBitfield.usedVertexB;
result.m_usedVertices |= btUsageBitfield.usedVertexC;
result.setBarycentricCoordinates( 1 - v - w, v, w );
}
return true;
// return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = (double)(1.0) - v - w
}
///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;
}
}
}
}
/// Test if point p and d lie on opposite sides of plane through abc
public int pointOutsideOfPlane( ref btVector3 p, ref btVector3 a, ref btVector3 b, ref btVector3 c, ref btVector3 d )
{
btVector3 tmp, tmp2;
b.Sub( ref a, out tmp );
c.Sub( ref a, out tmp2 );
btVector3 normal; ( tmp ).cross( ref tmp2, out normal );
p.Sub( ref a, out tmp );
double signp = ( tmp ).dot( ref normal ); // [AP AB AC]
d.Sub( ref a, out tmp );
double signd = ( tmp ).dot( ref normal ); // [AD AB AC]
#if CATCH_DEGENERATE_TETRAHEDRON
#if BT_USE_DOUBLE_PRECISION
if (signd * signd < ((double)(1e-8) * (double)(1e-8)))
{
return -1;
}
#else
if( signd * signd < ( (double)( 1e-4 ) * (double)( 1e-4 ) ) )
{
// Console.WriteLine("affine dependent/degenerate\n");//
return -1;
}
#endif
#endif
// Points on opposite sides if expression signs are opposite
return ( signp * signd < btScalar.BT_ZERO ) ? 1 : 0;
}
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 );
}
bool collide( ref btVector3 sphereCenter, out btVector3 point, out btVector3 resultNormal, out double depth, double timeOfImpact, double contactBreakingThreshold )
{
//btVector3[] vertices = m_triangle.getVertexPtr( 0 );
double radius = m_sphere.getRadius();
double radiusWithThreshold = radius + contactBreakingThreshold;
btVector3 normal;
btVector3.btCross2Del( ref m_triangle.m_vertices2, ref m_triangle.m_vertices1
, ref m_triangle.m_vertices3, ref m_triangle.m_vertices1
, out normal );
normal.normalize();
btVector3 p1ToCentre; sphereCenter.Sub( ref m_triangle.m_vertices1, out p1ToCentre );
double distanceFromPlane = p1ToCentre.dot( ref normal );
if( distanceFromPlane < btScalar.BT_ZERO )
{
//triangle facing the other way
distanceFromPlane *= btScalar.BT_NEG_ONE;
normal *= btScalar.BT_NEG_ONE;
}
bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold;
// Check for contact / intersection
bool hasContact = false;
btVector3 contactPoint = btVector3.Zero;
if( isInsideContactPlane )
{
if( pointInTriangle(
ref m_triangle.m_vertices1
, ref m_triangle.m_vertices2
, ref m_triangle.m_vertices3
, ref normal
, ref sphereCenter
) )
{
// Inside the contact wedge - touches a point on the shell plane
hasContact = true;
contactPoint = sphereCenter - normal * distanceFromPlane;
}
else
{
// Could be inside one of the contact capsules
double contactCapsuleRadiusSqr = radiusWithThreshold * radiusWithThreshold;
btVector3 nearestOnEdge;
for( int i = 0; i < m_triangle.getNumEdges(); i++ )
{
btVector3 pa;
btVector3 pb;
m_triangle.getEdge( i, out pa, out pb );
double distanceSqr = SegmentSqrDistance( ref pa, ref pb, ref sphereCenter, out nearestOnEdge );
if( distanceSqr < contactCapsuleRadiusSqr )
{
// Yep, we're inside a capsule
hasContact = true;
contactPoint = nearestOnEdge;
}
}
}
}
if( hasContact )
{
btVector3 contactToCentre; sphereCenter.Sub( ref contactPoint, out contactToCentre );
double distanceSqr = contactToCentre.length2();
if( distanceSqr < radiusWithThreshold * radiusWithThreshold )
{
if( distanceSqr > btScalar.SIMD_EPSILON )
{
double distance = btScalar.btSqrt( distanceSqr );
resultNormal = contactToCentre;
resultNormal.normalize();
point = contactPoint;
depth = -( radius - distance );
}
else
{
resultNormal = normal;
point = contactPoint;
depth = -radius;
}
return true;
}
}
depth = 0;
point = btVector3.Zero;
resultNormal = btVector3.Zero;
return false;
}
