public void addContactPoint( ref btVector3 normalOnBInWorld, ref btVector3 pointInWorldOrg, double depthOrg )
{
m_reportedDistance = depthOrg;
m_reportedNormalOnWorld = normalOnBInWorld;
btVector3 adjustedPointB; pointInWorldOrg.SubScale( ref normalOnBInWorld, m_marginOnB, out adjustedPointB );
m_reportedDistance = depthOrg + ( m_marginOnA + m_marginOnB );
if( m_reportedDistance < 0 )
{
m_foundResult = true;
}
m_originalResult.addContactPoint( ref normalOnBInWorld, ref adjustedPointB, m_reportedDistance );
}
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;
}