btStaticPlaneShape( ref btVector3 planeOrigin, ref btVector3 planeNormal ) : base()
{
planeNormal.normalized( out m_planeNormal );
m_planeConstant = planeOrigin.dot( ref planeNormal );
m_localScaling = btVector3.Zero;
m_shapeType = BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE;
// Debug.Assert( btFuzzyZero(m_planeNormal.length() - btScalar.BT_ONE) );
}
static bool notExist( ref btVector3 planeEquation, btList<btVector3> planeEquations )
{
int numbrushes = planeEquations.Count;
for( int i = 0; i < numbrushes; i++ )
{
btVector3 N1 = planeEquations[i];
if( planeEquation.dot( ref N1 ) > (double)( 0.999 ) )
{
return false;
}
}
return true;
}
public static bool areVerticesBehindPlane( ref btVector3 planeNormal, btList<btVector3> vertices, double margin )
{
btVector3[] va = vertices.InternalArray;
int numvertices = vertices.Count;
for( int i = 0; i < numvertices; i++ )
{
//ref btVector3 N1 = vertices[i];
double dist = (double)( planeNormal.dot( ref va[i] ) ) + (double)( planeNormal[3] ) - margin;
if( dist > btScalar.BT_ZERO )
{
return false;
}
}
return true;
}
public void Project(ref IndexedMatrix trans, ref IndexedVector3 dir, ref float min, ref float max)
{
#if true
min = float.MaxValue;
max = float.MinValue;
IndexedVector3 witnesPtMin;
IndexedVector3 witnesPtMax;
int numVerts = m_unscaledPoints.Count;
for (int i = 0; i < numVerts; i++)
{
IndexedVector3 vtx = m_unscaledPoints[i] * m_localScaling;
IndexedVector3 pt = trans * vtx;
float dp = pt.Dot(dir);
if (dp < min)
{
min = dp;
witnesPtMin = pt;
}
if (dp > max)
{
max = dp;
witnesPtMax = pt;
}
}
#else
btVector3 localAxis = dir * trans.getBasis();
btVector3 vtx1 = trans(localGetSupportingVertex(localAxis));
btVector3 vtx2 = trans(localGetSupportingVertex(-localAxis));
min = vtx1.dot(dir);
max = vtx2.dot(dir);
#endif
if (min > max)
{
float tmp = min;
min = max;
max = tmp;
}
}
internal int get_limit_motor_info2(
btRotationalLimitMotor limot,
ref btTransform transA, ref btTransform transB, ref btVector3 linVelA, ref btVector3 linVelB, ref btVector3 angVelA, ref btVector3 angVelB,
ref btConstraintInfo2 info, int row, ref btVector3 ax1, bool rotational, bool rotAllowed = false )
{
//int srow = row * info.rowskip;
bool powered = limot.m_enableMotor;
int limit = limot.m_currentLimit;
if( powered || limit != 0 )
{ // if the joint is powered, or has joint limits, add in the extra row
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 );
}
/*
double* J1 = rotational ? info.m_J1angularAxis : info.m_J1linearAxis;
double* J2 = rotational ? info.m_J2angularAxis : info.m_J2linearAxis;
J1[srow + 0] = ax1[0];
J1[srow + 1] = ax1[1];
J1[srow + 2] = ax1[2];
J2[srow + 0] = -ax1[0];
J2[srow + 1] = -ax1[1];
J2[srow + 2] = -ax1[2];
*/
if( ( !rotational ) )
{
if( m_useOffsetForConstraintFrame )
{
btVector3 tmpA, tmpB, relA, relB;
// get vector from bodyB to frameB in WCS
m_calculatedTransformB.m_origin.Sub( ref transB.m_origin, out relB );
// get its projection to constraint axis
btVector3 projB = ax1 * relB.dot( ax1 );
// get vector directed from bodyB to constraint axis (and orthogonal to it)
btVector3 orthoB = relB - projB;
// same for bodyA
m_calculatedTransformA.m_origin.Sub( ref transA.m_origin, out relA );
btVector3 projA = ax1 * relA.dot( ax1 );
btVector3 orthoA = relA - projA;
// get desired offset between frames A and B along constraint axis
double desiredOffs = limot.m_currentPosition - limot.m_currentLimitError;
// desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis
btVector3 totalDist = projA + ax1 * desiredOffs - projB;
// get offset vectors relA and relB
relA = orthoA + totalDist * m_factA;
relB = orthoB - totalDist * m_factB;
tmpA = relA.cross( ax1 );
tmpB = relB.cross( ax1 );
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 );
//for( i = 0; i < 3; i++ ) info.m_J1angularAxis[srow + i] = tmpA[i];
//for( i = 0; i < 3; i++ ) info.m_J2angularAxis[srow + i] = -tmpB[i];
}
else
{
btVector3 ltd; // Linear Torque Decoupling vector
btVector3 c = m_calculatedTransformB.m_origin - transA.m_origin;
ltd = c.cross( ax1 );
info.m_solverConstraints[row].m_relpos1CrossNormal = ltd;
//info.m_J1angularAxis[srow + 0] = ltd[0];
//info.m_J1angularAxis[srow + 1] = ltd[1];
//info.m_J1angularAxis[srow + 2] = ltd[2];
c = m_calculatedTransformB.m_origin - transB.m_origin;
ltd = -c.cross( ax1 );
info.m_solverConstraints[row].m_relpos2CrossNormal = ltd;
//info.m_J2angularAxis[srow + 0] = ltd[0];
//info.m_J2angularAxis[srow + 1] = ltd[1];
//info.m_J2angularAxis[srow + 2] = ltd[2];
}
}
// if we're limited low and high simultaneously, the joint motor is
// ineffective
if( limit != 0 && ( limot.m_loLimit == limot.m_hiLimit ) ) powered = false;
info.m_solverConstraints[row].m_rhs = (double)( 0 );
if( powered )
{
info.m_solverConstraints[row].m_cfm = limot.m_normalCFM;
if( limit == 0 )
{
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_stopERP );
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;
}
}
if( limit != 0 )
{
double k = info.fps * limot.m_stopERP;
if( !rotational )
{
info.m_solverConstraints[row].m_rhs += k * limot.m_currentLimitError;
}
else
{
info.m_solverConstraints[row].m_rhs += -k * limot.m_currentLimitError;
}
info.m_solverConstraints[row].m_cfm = limot.m_stopCFM;
if( limot.m_loLimit == limot.m_hiLimit )
{ // limited low and high simultaneously
info.m_solverConstraints[row].m_lowerLimit = btScalar.BT_MIN_FLOAT;
info.m_solverConstraints[row].m_upperLimit = btScalar.BT_MAX_FLOAT;
}
else
{
if( limit == 1 )
{
info.m_solverConstraints[row].m_lowerLimit = 0;
info.m_solverConstraints[row].m_upperLimit = btScalar.BT_MAX_FLOAT;
}
else
{
info.m_solverConstraints[row].m_lowerLimit = btScalar.BT_MIN_FLOAT;
info.m_solverConstraints[row].m_upperLimit = 0;
}
// deal with bounce
if( limot.m_bounce > 0 )
{
// calculate joint velocity
double vel;
if( rotational )
{
vel = angVelA.dot( ax1 );
//make sure that if no body . angVelB == zero vec
// if (body1)
vel -= angVelB.dot( ax1 );
}
else
{
vel = linVelA.dot( ax1 );
//make sure that if no body . angVelB == zero vec
// if (body1)
vel -= linVelB.dot( ax1 );
}
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if( limit == 1 )
{
if( vel < 0 )
{
double newc = -limot.m_bounce * vel;
if( newc > info.m_solverConstraints[row].m_rhs )
info.m_solverConstraints[row].m_rhs = newc;
}
}
else
{
if( vel > 0 )
{
double newc = -limot.m_bounce * vel;
if( newc < info.m_solverConstraints[row].m_rhs )
info.m_solverConstraints[row].m_rhs = newc;
}
}
}
}
}
return 1;
}
else return 0;
}
public static void shortestArcQuat( ref btVector3 v0, ref btVector3 v1, out btQuaternion result ) // Game Programming Gems 2.10 make sure v0,v1 are normalized
{
btVector3 c;
v0.cross( ref v1, out c );
double d = v0.dot( ref v1 );
if( d < -1.0 + btScalar.SIMD_EPSILON )
{
btVector3 n, unused;
btVector3.btPlaneSpace1( ref v0, out n, out unused );
result.x = n.x; result.y = n.y; result.z = n.z; result.w = 0.0f; // just pick any vector that is orthogonal to v0
return;
}
double s = btScalar.btSqrt( ( 1.0f + d ) * 2.0f );
double rs = 1.0f / s;
result.x = c.x * rs;
result.y = c.y * rs;
result.z = c.z * rs;
result.w = s * 0.5f;
}
void getInfo2InternalUsingFrameOffset( ref btConstraintInfo2 info, ref btTransform transA, ref btTransform transB, ref btVector3 angVelA, ref btVector3 angVelB )
{
//Debug.Assert( !m_useSolveConstraintObsolete );
//int i;
// transforms in world space
btTransform trA; transA.Apply( ref m_rbAFrame, out trA );
btTransform trB; transB.Apply( ref m_rbBFrame, out trB );
// pivot point
// btVector3 pivotAInW = trA.getOrigin();
// btVector3 pivotBInW = trB.getOrigin();
#if true
// difference between frames in WCS
btVector3 ofs; trB.m_origin.Sub( ref trA.m_origin, out ofs );// getOrigin() - trA.getOrigin();
// now get weight factors depending on masses
double miA = getRigidBodyA().getInvMass();
double miB = getRigidBodyB().getInvMass();
bool hasStaticBody = ( miA < btScalar.SIMD_EPSILON ) || ( miB < btScalar.SIMD_EPSILON );
double miS = miA + miB;
double factA, factB;
if( miS > (double)( 0 ) )
{
factA = miB / miS;
}
else
{
factA = (double)( 0.5f );
}
factB = (double)( 1.0f ) - factA;
// get the desired direction of hinge axis
// as weighted sum of Z-orthos of frameA and frameB in WCS
btVector3 ax1A; trA.m_basis.getColumn( 2, out ax1A );
btVector3 ax1B; trB.m_basis.getColumn( 2, out ax1B );
btVector3 tmp;
ax1A.Mult( factA, out tmp );
btVector3 ax1; tmp.AddScale( ref ax1B, factB, out ax1 );
ax1.normalize();
// fill first 3 rows
// we want: velA + wA x relA == velB + wB x relB
btTransform bodyA_trans = transA;
btTransform bodyB_trans = transB;
//int nrow = 2; // last filled row
btVector3 tmpA, tmpB, relA, relB, p, q;
// get vector from bodyB to frameB in WCS
trB.m_origin.Sub( ref bodyB_trans.m_origin, out relB );
// get its projection to hinge axis
btVector3 projB; ax1.Mult( relB.dot( ref ax1 ), out projB );
// get vector directed from bodyB to hinge axis (and orthogonal to it)
btVector3 orthoB; relB.Sub( ref projB, out orthoB );
// same for bodyA
trA.m_origin.Sub( ref bodyA_trans.m_origin, out relA );
btVector3 projA; ax1.Mult( relA.dot( ref ax1 ), out projA );
btVector3 orthoA; relA.Sub( ref projA, out orthoA );
btVector3 totalDist; projA.Sub( ref projB, out totalDist );
// get offset vectors relA and relB
orthoA.AddScale( ref totalDist, factA, out relA );
orthoB.AddScale( ref totalDist, -factB, out relB );
// now choose average ortho to hinge axis
orthoB.Mult( factA, out tmp );
tmp.AddScale( ref orthoA, factB, out p );
double len2 = p.length2();
if( len2 > btScalar.SIMD_EPSILON )
{
p.normalize();
}
else
{
trA.m_basis.getColumn( 1, out p );
}
// make one more ortho
ax1.cross( ref p, out q );
// fill three rows
relA.cross( ref p, out tmpA );
relB.cross( ref p, out tmpB );
info.m_solverConstraints[0].m_relpos1CrossNormal = tmpA;
tmpB.Invert( out info.m_solverConstraints[0].m_relpos2CrossNormal ); // = -tmpB;
relA.cross( ref q, out tmpA );
relB.cross( ref q, out tmpB );
if( hasStaticBody && getSolveLimit() )
{ // to make constraint between static and dynamic objects more rigid
// remove wA (or wB) from equation if angular limit is hit
tmpB.Mult( factB, out tmpB );
tmpA.Mult( factA, out tmpA );
}
info.m_solverConstraints[1].m_relpos1CrossNormal = tmpA;
tmpB.Invert( out info.m_solverConstraints[1].m_relpos2CrossNormal );
relA.cross( ref ax1, out tmpA );
relB.cross( ref ax1, out tmpB );
if( hasStaticBody )
{ // to make constraint between static and dynamic objects more rigid
// remove wA (or wB) from equation
tmpB.Mult( factB, out tmpB );
tmpA.Mult( factA, out tmpA );
}
info.m_solverConstraints[2].m_relpos1CrossNormal = tmpA;
tmpB.Invert( out info.m_solverConstraints[2].m_relpos2CrossNormal );
double normalErp = ( ( m_flags & btHingeFlags.BT_HINGE_FLAGS_ERP_NORM ) != 0 ) ? m_normalERP : info.erp;
double k = info.fps * normalErp;
if( !m_angularOnly )
{
info.m_solverConstraints[0].m_contactNormal1 = p;
info.m_solverConstraints[1].m_contactNormal1 = q;
info.m_solverConstraints[2].m_contactNormal1 = ax1;
p.Invert( out info.m_solverConstraints[0].m_contactNormal2 );
q.Invert( out info.m_solverConstraints[1].m_contactNormal2 );
ax1.Invert( out info.m_solverConstraints[2].m_contactNormal2 );
// compute three elements of right hand side
double rhs = k * p.dot( ofs );
info.m_solverConstraints[0].m_rhs = rhs;
rhs = k * q.dot( ofs );
info.m_solverConstraints[1].m_rhs = rhs;
rhs = k * ax1.dot( ofs );
info.m_solverConstraints[2].m_rhs = rhs;
}
// the hinge axis should be the only unconstrained
// rotational axis, the angular velocity of the two bodies perpendicular to
// the hinge axis should be equal. thus the constraint equations are
// p*w1 - p*w2 = 0
// q*w1 - q*w2 = 0
// where p and q are unit vectors normal to the hinge axis, and w1 and w2
// are the angular velocity vectors of the two bodies.
//int s3 = 3 * s;
//int s4 = 4 * s;
info.m_solverConstraints[3].m_relpos1CrossNormal = p;
info.m_solverConstraints[4].m_relpos1CrossNormal = q;
p.Invert( out info.m_solverConstraints[3].m_relpos2CrossNormal );
q.Invert( out info.m_solverConstraints[4].m_relpos2CrossNormal );
// compute the right hand side of the constraint equation. set relative
// body velocities along p and q to bring the hinge back into alignment.
// if ax1A,ax1B are the unit length hinge axes as computed from bodyA and
// bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2).
// if "theta" is the angle between ax1 and ax2, we need an angular velocity
// along u to cover angle erp*theta in one step :
// |angular_velocity| = angle/time = erp*theta / stepsize
// = (erp*fps) * theta
// angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
// = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
// ...as ax1 and ax2 are unit length. if theta is smallish,
// theta ~= sin(theta), so
// angular_velocity = (erp*fps) * (ax1 x ax2)
// ax1 x ax2 is in the plane space of ax1, so we project the angular
// velocity to p and q to find the right hand side.
k = info.fps * normalErp;//??
btVector3 u; ax1A.cross( ref ax1B, out u );
info.m_solverConstraints[3].m_rhs = k * u.dot( p );
info.m_solverConstraints[4].m_rhs = k * u.dot( q );
#endif
// check angular limits
//int nrow = 4; // last filled row
//int srow;
double limit_err = (double)( 0.0 );
int limit = 0;
if( getSolveLimit() )
{
#if _BT_USE_CENTER_LIMIT_
limit_err = m_limit.getCorrection() * m_referenceSign;
#else
limit_err = m_correction * m_referenceSign;
#endif
limit = ( limit_err > (double)( 0.0 ) ) ? 1 : 2;
}
// if the hinge has joint limits or motor, add in the extra row
bool powered = false;
if( getEnableAngularMotor() )
{
powered = true;
}
if( limit != 0 || powered )
{
info.m_solverConstraints[5].m_relpos1CrossNormal = ax1;
ax1.Invert( out info.m_solverConstraints[5].m_relpos2CrossNormal );
double lostop = getLowerLimit();
double histop = getUpperLimit();
if( limit != 0 && ( lostop == histop ) )
{ // the joint motor is ineffective
powered = false;
}
info.m_solverConstraints[5].m_rhs = (double)( 0.0f );
double currERP = ( ( m_flags & btHingeFlags.BT_HINGE_FLAGS_ERP_STOP ) != 0 ) ? m_stopERP : normalErp;
if( powered )
{
if( ( m_flags & btHingeFlags.BT_HINGE_FLAGS_CFM_NORM ) != 0 )
{
info.m_solverConstraints[5].m_cfm = m_normalCFM;
}
double mot_fact = getMotorFactor( m_hingeAngle, lostop, histop, m_motorTargetVelocity, info.fps * currERP );
info.m_solverConstraints[5].m_rhs += mot_fact * m_motorTargetVelocity * m_referenceSign;
info.m_solverConstraints[5].m_lowerLimit = -m_maxMotorImpulse;
info.m_solverConstraints[5].m_upperLimit = m_maxMotorImpulse;
}
if( limit != 0 )
{
k = info.fps * currERP;
info.m_solverConstraints[5].m_rhs += k * limit_err;
if( ( m_flags & btHingeFlags.BT_HINGE_FLAGS_CFM_STOP ) != 0 )
{
info.m_solverConstraints[5].m_cfm = m_stopCFM;
}
if( lostop == histop )
{
// limited low and high simultaneously
info.m_solverConstraints[5].m_lowerLimit = btScalar.BT_MIN_FLOAT;
info.m_solverConstraints[5].m_upperLimit = btScalar.BT_MAX_FLOAT;
}
else if( limit == 1 )
{ // low limit
info.m_solverConstraints[5].m_lowerLimit = 0;
info.m_solverConstraints[5].m_upperLimit = btScalar.BT_MAX_FLOAT;
}
else
{ // high limit
info.m_solverConstraints[5].m_lowerLimit = btScalar.BT_MIN_FLOAT;
info.m_solverConstraints[5].m_upperLimit = 0;
}
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
#if _BT_USE_CENTER_LIMIT_
double bounce = m_limit.getRelaxationFactor();
#else
double bounce = m_relaxationFactor;
#endif
if( bounce > (double)( 0.0 ) )
{
double vel = angVelA.dot( ax1 );
vel -= angVelB.dot( ax1 );
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if( limit == 1 )
{ // low limit
if( vel < 0 )
{
double newc = -bounce * vel;
if( newc > info.m_solverConstraints[5].m_rhs )
{
info.m_solverConstraints[5].m_rhs = newc;
}
}
}
else
{ // high limit - all those computations are reversed
if( vel > 0 )
{
double newc = -bounce * vel;
if( newc < info.m_solverConstraints[5].m_rhs )
{
info.m_solverConstraints[5].m_rhs = newc;
}
}
}
}
#if _BT_USE_CENTER_LIMIT_
info.m_solverConstraints[5].m_rhs *= m_limit.getBiasFactor();
#else
info.m_solverConstraints[5].m_rhs *= m_biasFactor;
#endif
} // if(limit)
} // if angular limit or powered
}
public btHingeConstraint( btRigidBody rbA, btRigidBody rbB, ref btVector3 pivotInA, ref btVector3 pivotInB,
ref btVector3 axisInA, ref btVector3 axisInB, bool useReferenceFrameA = false )
: base( btObjectTypes.HINGE_CONSTRAINT_TYPE, rbA, rbB )
{
Init();
#if _BT_USE_CENTER_LIMIT_
m_limit = new btAngularLimit();
#endif
m_useReferenceFrameA = ( useReferenceFrameA );
m_rbAFrame.m_origin = pivotInA;
// since no frame is given, assume this to be zero angle and just pick rb transform axis
btVector3 rbAxisA1; rbA.m_worldTransform.m_basis.getColumn( 0, out rbAxisA1 );
btVector3 rbAxisA2;
double projection = axisInA.dot( rbAxisA1 );
if( projection >= 1.0f - btScalar.SIMD_EPSILON )
{
btVector3 tmp;
rbA.m_worldTransform.m_basis.getColumn( 2, out tmp );
tmp.Invert( out rbAxisA1 );
rbA.m_worldTransform.m_basis.getColumn( 1, out rbAxisA2 );
}
else if( projection <= -1.0f + btScalar.SIMD_EPSILON )
{
rbA.m_worldTransform.m_basis.getColumn( 2, out rbAxisA1 );
rbA.m_worldTransform.m_basis.getColumn( 1, out rbAxisA2 );
}
else
{
axisInA.cross( ref rbAxisA1, out rbAxisA2 );
rbAxisA2.cross( ref axisInA, out rbAxisA1 );
}
btMatrix3x3.setValue( out m_rbAFrame.m_basis, rbAxisA1.x, rbAxisA2.x, axisInA.x,
rbAxisA1.y, rbAxisA2.y, axisInA.y,
rbAxisA1.z, rbAxisA2.z, axisInA.z );
btQuaternion rotationArc; btQuaternion.shortestArcQuat( ref axisInA, ref axisInB, out rotationArc );
btVector3 rbAxisB1; btQuaternion.quatRotate( ref rotationArc, ref rbAxisA1, out rbAxisB1 );
btVector3 rbAxisB2; axisInB.cross( ref rbAxisB1, out rbAxisB2 );
m_rbBFrame.m_origin = pivotInB;
m_rbBFrame.m_basis.setValue( ref rbAxisB1, ref rbAxisB2, ref axisInB );
btMatrix3x3.setValue( out m_rbBFrame.m_basis, ref rbAxisB1, ref rbAxisB2, ref axisInB );
m_referenceSign = m_useReferenceFrameA ? (double)( -1 ) : (double)( 1 );
}
///optional method mainly used to generate multiple contact points by clipping polyhedral features (faces/edges)
///experimental/work-in-progress
public virtual bool initializePolyhedralFeatures( bool shiftVerticesByMargin = false )
{
if( m_polyhedron != null )
{
m_polyhedron = null;
}
m_polyhedron = new btConvexPolyhedron();
int count = getNumVertices();
btList<btVector3> orgVertices = new btList<btVector3>( count );
btVector3[] arrVertices = orgVertices.InternalArray;
for( int i = 0; i < count; i++ )
{
getVertex( i, out arrVertices[i] );
}
btConvexHullComputer conv = new btConvexHullComputer();
if( shiftVerticesByMargin )
{
btList<btVector3> planeEquations = new btList<btVector3>();
btGeometryUtil.getPlaneEquationsFromVertices( orgVertices, planeEquations );
btList<btVector3> shiftedPlaneEquations = new btList<btVector3>();
for( int p = 0; p < planeEquations.Count; p++ )
{
btVector3 plane = planeEquations[p];
// double margin = getMargin();
plane[3] -= getMargin();
shiftedPlaneEquations.Add( plane );
}
btList<btVector3> tmpVertices = new btList<btVector3>();
btGeometryUtil.getVerticesFromPlaneEquations( shiftedPlaneEquations, tmpVertices );
conv.compute( tmpVertices, tmpVertices.Count, 0, 0 );
}
else
{
conv.compute( orgVertices, orgVertices.Count, 0, 0 );
}
btList<btVector3> faceNormals = new btList<btVector3>( conv.faces.Count );
int numFaces = conv.faces.Count;
btConvexHullComputer convexUtil = conv;
btVector3[] arr_faceNormals = faceNormals.InternalArray;
btList<btConvexPolyhedron.btFace> tmpFaces = new btList<btConvexPolyhedron.btFace>( numFaces );
int numVertices = convexUtil.vertices.Count;
m_polyhedron.m_vertices.Count = m_polyhedron.m_vertices.Capacity = ( numVertices );
for( int p = 0; p < numVertices; p++ )
{
m_polyhedron.m_vertices[p] = convexUtil.vertices[p];
}
for( int i = 0; i < numFaces; i++ )
{
btConvexHullComputer.Edge face = convexUtil.faces[i];
//Console.WriteLine("face=%d\n",face);
//btConvexHullComputer::Edge* firstEdge = &convexUtil.edges[face];
btConvexHullComputer.Edge edge = face;
btVector3[] edges = new btVector3[3];
int numEdges = 0;
//compute face normals
do
{
int src = edge.getSourceVertex();
tmpFaces[i].m_indices.Add( (short)src );
int targ = edge.getTargetVertex();
btVector3 wa = convexUtil.vertices[src];
btVector3 wb = convexUtil.vertices[targ];
btVector3 newEdge; wb.Sub( ref wa, out newEdge );
newEdge.normalize();
if( numEdges < 2 )
edges[numEdges++] = newEdge;
edge = edge.getNextEdgeOfFace();
} while( edge != face );
double planeEq = btScalar.BT_LARGE_FLOAT;
if( numEdges == 2 )
{
//faceNormals[i]
edges[0].cross( ref edges[1], out faceNormals.InternalArray[i] );
faceNormals[i].normalize();
tmpFaces[i].m_plane[0] = faceNormals[i].x;
tmpFaces[i].m_plane[1] = faceNormals[i].y;
tmpFaces[i].m_plane[2] = faceNormals[i].z;
tmpFaces[i].m_plane[3] = planeEq;
}
else
{
Debug.Assert( false );//degenerate?
faceNormals[i].setZero();
}
for( int v = 0; v < tmpFaces[i].m_indices.Count; v++ )
{
double eq = m_polyhedron.m_vertices[tmpFaces[i].m_indices[v]].dot( ref arr_faceNormals[i] );
if( planeEq > eq )
{
planeEq = eq;
}
}
tmpFaces[i].m_plane[3] = -planeEq;
}
//merge coplanar faces and copy them to m_polyhedron
double faceWeldThreshold = 0.999f;
btList<int> todoFaces = new btList<int>();
for( int i = 0; i < tmpFaces.Count; i++ )
todoFaces.Add( i );
btList<int> coplanarFaceGroup = new btList<int>();
while( todoFaces.Count > 0 )
{
int refFace = todoFaces[todoFaces.Count - 1];
coplanarFaceGroup.Add( refFace );
btConvexPolyhedron.btFace faceA = tmpFaces[refFace];
todoFaces.Count--;
btVector3 faceNormalA = new btVector3( faceA.m_plane[0], faceA.m_plane[1], faceA.m_plane[2] );
for( int j = todoFaces.Count - 1; j >= 0; j-- )
{
int i = todoFaces[j];
btConvexPolyhedron.btFace faceB = tmpFaces[i];
btVector3 faceNormalB = new btVector3( faceB.m_plane[0], faceB.m_plane[1], faceB.m_plane[2] );
if( faceNormalA.dot( ref faceNormalB ) > faceWeldThreshold )
{
coplanarFaceGroup.Add( i );
todoFaces.RemoveAt( i );
}
}
bool did_merge = false;
if( coplanarFaceGroup.Count > 1 )
{
//do the merge: use Graham Scan 2d convex hull
btList<GrahamVector3> orgpoints = new btList<GrahamVector3>();
btVector3 averageFaceNormal = btVector3.Zero;
for( int i = 0; i < coplanarFaceGroup.Count; i++ )
{
// m_polyhedron.m_faces.Add(tmpFaces[coplanarFaceGroup[i]]);
btConvexPolyhedron.btFace face = tmpFaces[coplanarFaceGroup[i]];
btVector3 faceNormal = new btVector3( face.m_plane[0], face.m_plane[1], face.m_plane[2] );
averageFaceNormal.Add( ref faceNormal, out averageFaceNormal );
for( int f = 0; f < face.m_indices.Count; f++ )
{
int orgIndex = face.m_indices[f];
btVector3 pt = m_polyhedron.m_vertices[orgIndex];
bool found = false;
for( int j = 0; j < orgpoints.Count; j++ )
{
//if ((orgpoints[i].m_orgIndex == orgIndex) || ((rotatedPt-orgpoints[i]).length2()<0.0001))
if( orgpoints[j].m_orgIndex == orgIndex )
{
found = true;
break;
}
}
if( !found )
orgpoints.Add( new GrahamVector3( ref pt, orgIndex ) );
}
}
btConvexPolyhedron.btFace combinedFace = new btConvexPolyhedron.btFace();
for( int i = 0; i < 4; i++ )
combinedFace.m_plane[i] = tmpFaces[coplanarFaceGroup[0]].m_plane[i];
btList<GrahamVector3> hull = new btList<GrahamVector3>();
averageFaceNormal.normalize();
GrahamVector3.GrahamScanConvexHull2D( orgpoints, hull, ref averageFaceNormal );
for( int i = 0; i < hull.Count; i++ )
{
combinedFace.m_indices.Add( hull[i].m_orgIndex );
for( int k = 0; k < orgpoints.Count; k++ )
{
if( orgpoints[k].m_orgIndex == hull[i].m_orgIndex )
{
orgpoints[k].m_orgIndex = -1; // invalidate...
break;
}
}
}
// are there rejected vertices?
bool reject_merge = false;
for( int i = 0; i < orgpoints.Count; i++ )
{
if( orgpoints[i].m_orgIndex == -1 )
continue; // this is in the hull...
// this vertex is rejected -- is anybody else using this vertex?
for( int j = 0; j < tmpFaces.Count; j++ )
{
btConvexPolyhedron.btFace face = tmpFaces[j];
// is this a face of the current coplanar group?
bool is_in_current_group = false;
for( int k = 0; k < coplanarFaceGroup.Count; k++ )
{
if( coplanarFaceGroup[k] == j )
{
is_in_current_group = true;
break;
}
}
if( is_in_current_group ) // ignore this face...
continue;
// does this face use this rejected vertex?
for( int v = 0; v < face.m_indices.Count; v++ )
{
if( face.m_indices[v] == orgpoints[i].m_orgIndex )
{
// this rejected vertex is used in another face -- reject merge
reject_merge = true;
break;
}
}
if( reject_merge )
break;
}
if( reject_merge )
break;
}
if( !reject_merge )
{
// do this merge!
did_merge = true;
m_polyhedron.m_faces.Add( combinedFace );
}
}
if( !did_merge )
{
for( int i = 0; i < coplanarFaceGroup.Count; i++ )
{
btConvexPolyhedron.btFace face = tmpFaces[coplanarFaceGroup[i]];
m_polyhedron.m_faces.Add( face );
}
}
}
m_polyhedron.initialize();
return true;
}
public override bool isInside( ref btVector3 pt, double tolerance )
{
btVector3 normal;
calcNormal( out normal );
//distance to plane
double dist = pt.dot( ref normal );
double planeconst = m_vertices1.dot( ref normal );
dist -= planeconst;
if( dist >= -tolerance && dist <= tolerance )
{
//inside check on edge-planes
int i;
for( i = 0; i < 3; i++ )
{
btVector3 pa, pb;
getEdge( i, out pa, out pb );
btVector3 edge; pb.Sub( ref pa, out edge );
btVector3 edgeNormal; edge.cross( ref normal, out edgeNormal );
edgeNormal.normalize();
double dist2 = pt.dot( ref edgeNormal );
double edgeConst = pa.dot( ref edgeNormal );
dist2 -= edgeConst;
if( dist2 < -tolerance )
return false;
}
return true;
}
return false;
}
/*@brief Return a rotated version of this vector
@param wAxis The axis to rotate about
@param angle The angle to rotate by */
public void rotate( ref btVector3 wAxis, double angle, out btVector3 result )
{
btVector3 o;
wAxis.Mult( wAxis.dot( ref this ), out o );
btVector3 _x;
this.Sub( ref o, out _x );
btVector3 _y;
wAxis.cross( ref this, out _y );
btVector3 tmp;
btVector3 tmp2;
_x.Mult( btScalar.btCos( angle ), out tmp );
o.Add( ref tmp, out tmp2 );
_y.Mult( btScalar.btSin( angle ), out tmp );
tmp2.Add( ref tmp, out result );
}
/*@brief Return the dot product between two vectors */
public static double btDot( ref btVector3 v1, ref btVector3 v2 )
{
return v1.dot( ref v2 );
}
internal void setupFrictionConstraint( btSolverConstraint solverConstraint, ref btVector3 normalAxis
//, int solverBodyIdA, int solverBodyIdB
, btSolverBody solverBodyA, btSolverBody solverBodyB
, btManifoldPoint cp, ref btVector3 rel_pos1, ref btVector3 rel_pos2, btCollisionObject colObj0, btCollisionObject colObj1, double relaxation, double desiredVelocity = 0, double cfmSlip = 0.0 )
{
//btSolverBody solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
//btSolverBody solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
btRigidBody body0 = solverBodyA.m_originalBody;
btRigidBody body1 = solverBodyB.m_originalBody;
solverConstraint.m_solverBodyA = solverBodyA;
solverConstraint.m_solverBodyB = solverBodyB;
solverConstraint.m_friction = cp.m_combinedFriction;
solverConstraint.m_originalContactPoint = null;
solverConstraint.m_appliedImpulse = 0;
solverConstraint.m_appliedPushImpulse = 0;
if( body0 != null )
{
solverConstraint.m_contactNormal1 = normalAxis;
rel_pos1.cross( ref solverConstraint.m_contactNormal1, out solverConstraint.m_relpos1CrossNormal );
btVector3 tmp;
body0.m_invInertiaTensorWorld.Apply( ref solverConstraint.m_relpos1CrossNormal, out tmp );
tmp.Mult( ref body0.m_angularFactor, out solverConstraint.m_angularComponentA );
}
else
{
solverConstraint.m_contactNormal1.setZero();
solverConstraint.m_relpos1CrossNormal.setZero();
solverConstraint.m_angularComponentA.setZero();
}
if( body1 != null )
{
normalAxis.Invert( out solverConstraint.m_contactNormal2 );
rel_pos2.cross( ref solverConstraint.m_contactNormal2, out solverConstraint.m_relpos2CrossNormal );
btVector3 tmp;
body1.m_invInertiaTensorWorld.Apply( ref solverConstraint.m_relpos2CrossNormal, out tmp );
tmp.Mult( ref body1.m_angularFactor, out solverConstraint.m_angularComponentB );
}
else
{
solverConstraint.m_contactNormal2 = btVector3.Zero;
solverConstraint.m_relpos2CrossNormal = btVector3.Zero;
solverConstraint.m_angularComponentB = btVector3.Zero;
}
{
btVector3 vec;
double denom0 = 0;
double denom1 = 0;
if( body0 != null )
{
solverConstraint.m_angularComponentA.cross( ref rel_pos1, out vec );
denom0 = body0.getInvMass() + normalAxis.dot( ref vec );
}
if( body1 != null )
{
btVector3 tmp;
solverConstraint.m_angularComponentB.Invert( out tmp );
tmp.cross( ref rel_pos2, out vec );
denom1 = body1.getInvMass() + normalAxis.dot( ref vec );
}
double denom = relaxation / ( denom0 + denom1 );
btScalar.Dbg( "m_jacDiagABInv 1 set to " + denom.ToString( "g17" ) );
solverConstraint.m_jacDiagABInv = denom;
}
{
double rel_vel;
double vel1Dotn;
double vel2Dotn;
//double vel1Dotn = solverConstraint.m_contactNormal1.dot( body0 != null ? solverBodyA.m_linearVelocity + solverBodyA.m_externalForceImpulse : btVector3.Zero )
// + solverConstraint.m_relpos1CrossNormal.dot( body0 != null ? solverBodyA.m_angularVelocity : btVector3.Zero );
if( body0 != null )
vel1Dotn = solverConstraint.m_contactNormal1.dotAdded( ref solverBodyA.m_linearVelocity, ref solverBodyA.m_externalForceImpulse )
+ solverConstraint.m_relpos1CrossNormal.dot( ref solverBodyA.m_angularVelocity );
else
vel1Dotn = 0;
//double vel2Dotn = solverConstraint.m_contactNormal2.dot( body1 != null ? solverBodyB.m_linearVelocity + solverBodyB.m_externalForceImpulse : btVector3.Zero )
// + solverConstraint.m_relpos2CrossNormal.dot( body1 != null ? solverBodyB.m_angularVelocity : btVector3.Zero );
if( body1 != null )
vel2Dotn = solverConstraint.m_contactNormal2.dotAdded( ref solverBodyB.m_linearVelocity, ref solverBodyB.m_externalForceImpulse )
+ solverConstraint.m_relpos2CrossNormal.dot( ref solverBodyB.m_angularVelocity );
else
vel2Dotn = 0;
rel_vel = vel1Dotn + vel2Dotn;
// double positionalError = 0;
double velocityError = desiredVelocity - rel_vel;
double velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
solverConstraint.m_rhs = velocityImpulse;
btScalar.Dbg( "Constraint 1 m_rhs " + solverConstraint.m_rhs.ToString( "g17" ) );
solverConstraint.m_rhsPenetration = 0;
solverConstraint.m_cfm = cfmSlip;
solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
solverConstraint.m_upperLimit = solverConstraint.m_friction;
}
}
//! apply the correction impulses for two bodies
//double solveAngularLimits(double timeStep,ref btVector3 axis, double jacDiagABInv,btRigidBody * body0, btRigidBody * body1);
internal double solveAngularLimits(
double timeStep, ref btVector3 axis, double jacDiagABInv,
btRigidBody body0, btRigidBody body1 )
{
if( needApplyTorques() == false ) return 0.0f;
double target_velocity = m_targetVelocity;
double maxMotorForce = m_maxMotorForce;
//current error correction
if( m_currentLimit != 0 )
{
target_velocity = -m_stopERP * m_currentLimitError / ( timeStep );
maxMotorForce = m_maxLimitForce;
}
maxMotorForce *= timeStep;
// current velocity difference
btVector3 angVelA = body0.getAngularVelocity();
btVector3 angVelB = body1.getAngularVelocity();
btVector3 vel_diff;
vel_diff = angVelA - angVelB;
double rel_vel = axis.dot( vel_diff );
// correction velocity
double motor_relvel = m_limitSoftness * ( target_velocity - m_damping * rel_vel );
if( motor_relvel < btScalar.SIMD_EPSILON && motor_relvel > -btScalar.SIMD_EPSILON )
{
return 0.0f;//no need for applying force
}
// correction impulse
double unclippedMotorImpulse = ( 1 + m_bounce ) * motor_relvel * jacDiagABInv;
// clip correction impulse
double clippedMotorImpulse;
///@todo: should clip against accumulated impulse
if( unclippedMotorImpulse > 0.0f )
{
clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce ? maxMotorForce : unclippedMotorImpulse;
}
else
{
clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce : unclippedMotorImpulse;
}
// sort with accumulated impulses
double lo = (double)( -btScalar.BT_LARGE_FLOAT );
double hi = (double)( btScalar.BT_LARGE_FLOAT );
double oldaccumImpulse = m_accumulatedImpulse;
double sum = oldaccumImpulse + clippedMotorImpulse;
m_accumulatedImpulse = sum > hi ? btScalar.BT_ZERO : sum < lo ? btScalar.BT_ZERO : sum;
clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
btVector3 motorImp; axis.Mult( clippedMotorImpulse, out motorImp );
body0.applyTorqueImpulse( ref motorImp );
btVector3 tmp;
motorImp.Invert( out tmp );
body1.applyTorqueImpulse( ref tmp );
return clippedMotorImpulse;
}
internal void getInfo2NonVirtual( btConstraintInfo2 info, ref btTransform transA, ref btTransform transB, ref btVector3 linVelA, ref btVector3 linVelB, double rbAinvMass, double rbBinvMass )
{
//btITransform m_calculatedTransformB = getCalculatedTransformB();
//Debug.Assert( !m_useSolveConstraintObsolete );
//int i;//, s = info.rowskip;
double signFact = m_useLinearReferenceFrameA ? (double)( 1.0f ) : (double)( -1.0f );
// difference between frames in WCS
btVector3 ofs; m_calculatedTransformB.m_origin.Sub( ref m_calculatedTransformA.m_origin, out ofs );
// now get weight factors depending on masses
double miA = rbAinvMass;
double miB = rbBinvMass;
bool hasStaticBody = ( miA < btScalar.SIMD_EPSILON ) || ( miB < btScalar.SIMD_EPSILON );
double miS = miA + miB;
double factA, factB;
if( miS > (double)( 0 ) )
{
factA = miB / miS;
}
else
{
factA = (double)( 0.5f );
}
factB = (double)( 1.0f ) - factA;
btVector3 ax1, p, q;
btVector3 ax1A = m_calculatedTransformA.m_basis.getColumn( 0 );
btVector3 ax1B = m_calculatedTransformB.m_basis.getColumn( 0 );
if( m_useOffsetForConstraintFrame )
{
// get the desired direction of slider axis
// as weighted sum of X-orthos of frameA and frameB in WCS
ax1 = ax1A * factA + ax1B * factB;
ax1.normalize();
// construct two orthos to slider axis
btVector3.btPlaneSpace1( ref ax1, out p, out q );
}
else
{ // old way - use frameA
ax1 = m_calculatedTransformA.m_basis.getColumn( 0 );
// get 2 orthos to slider axis (Y, Z)
p = m_calculatedTransformA.m_basis.getColumn( 1 );
q = m_calculatedTransformA.m_basis.getColumn( 2 );
}
// make rotations around these orthos equal
// the slider axis should be the only unconstrained
// rotational axis, the angular velocity of the two bodies perpendicular to
// the slider axis should be equal. thus the constraint equations are
// p*w1 - p*w2 = 0
// q*w1 - q*w2 = 0
// where p and q are unit vectors normal to the slider axis, and w1 and w2
// are the angular velocity vectors of the two bodies.
info.m_solverConstraints[0].m_relpos1CrossNormal = p;// m_J1angularAxis[0] = p[0];
info.m_solverConstraints[1].m_relpos1CrossNormal = q;// m_J1angularAxis[0] = p[0];
p.Invert( out info.m_solverConstraints[0].m_relpos2CrossNormal );
q.Invert( out info.m_solverConstraints[1].m_relpos2CrossNormal );
// compute the right hand side of the constraint equation. set relative
// body velocities along p and q to bring the slider back into alignment.
// if ax1A,ax1B are the unit length slider axes as computed from bodyA and
// bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2).
// if "theta" is the angle between ax1 and ax2, we need an angular velocity
// along u to cover angle erp*theta in one step :
// |angular_velocity| = angle/time = erp*theta / stepsize
// = (erp*fps) * theta
// angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
// = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
// ...as ax1 and ax2 are unit length. if theta is smallish,
// theta ~= sin(theta), so
// angular_velocity = (erp*fps) * (ax1 x ax2)
// ax1 x ax2 is in the plane space of ax1, so we project the angular
// velocity to p and q to find the right hand side.
// double k = info.fps * info.erp * getSoftnessOrthoAng();
double currERP = ( ( m_flags & btSliderFlags.BT_SLIDER_FLAGS_ERP_ORTANG ) != 0 ) ? m_softnessOrthoAng : m_softnessOrthoAng * info.erp;
double k = info.fps * currERP;
btVector3 u = ax1A.cross( ax1B );
info.m_solverConstraints[0].m_rhs = k * u.dot( p );
info.m_solverConstraints[1].m_rhs = k * u.dot( q );
if( ( m_flags & btSliderFlags.BT_SLIDER_FLAGS_CFM_ORTANG ) != 0 )
{
info.m_solverConstraints[0].m_cfm = m_cfmOrthoAng;
info.m_solverConstraints[1].m_cfm = m_cfmOrthoAng;
}
int nrow = 1; // last filled row
//int srow;
double limit_err;
int limit;
bool powered;
// next two rows.
// we want: velA + wA x relA == velB + wB x relB ... but this would
// result in three equations, so we project along two orthos to the slider axis
btTransform bodyA_trans = transA;
btTransform bodyB_trans = transB;
nrow++;
//int s2 = nrow * s;
nrow++;
//int s3 = nrow * s;
btVector3 tmpA = btVector3.Zero, tmpB = btVector3.Zero
, relA = btVector3.Zero, relB = btVector3.Zero, c = btVector3.Zero;
if( m_useOffsetForConstraintFrame )
{
// get vector from bodyB to frameB in WCS
relB = m_calculatedTransformB.m_origin - bodyB_trans.m_origin;
// get its projection to slider axis
btVector3 projB = ax1 * relB.dot( ax1 );
// get vector directed from bodyB to slider axis (and orthogonal to it)
btVector3 orthoB = relB - projB;
// same for bodyA
relA = m_calculatedTransformA.m_origin - bodyA_trans.m_origin;
btVector3 projA = ax1 * relA.dot( ax1 );
btVector3 orthoA = relA - projA;
// get desired offset between frames A and B along slider axis
double sliderOffs = m_linPos - m_depth[0];
// desired vector from projection of center of bodyA to projection of center of bodyB to slider axis
btVector3 totalDist = projA + ax1 * sliderOffs - projB;
// get offset vectors relA and relB
relA = orthoA + totalDist * factA;
relB = orthoB - totalDist * factB;
// now choose average ortho to slider axis
p = orthoB * factA + orthoA * factB;
double len2 = p.length2();
if( len2 > btScalar.SIMD_EPSILON )
{
p /= btScalar.btSqrt( len2 );
}
else
{
p = m_calculatedTransformA.m_basis.getColumn( 1 );
}
// make one more ortho
q = ax1.cross( p );
// fill two rows
tmpA = relA.cross( p );
tmpB = relB.cross( p );
info.m_solverConstraints[2].m_relpos1CrossNormal = tmpA;
tmpB.Invert( out info.m_solverConstraints[2].m_relpos2CrossNormal );
//for( i = 0; i < 3; i++ ) info.m_J1angularAxis[s2 + i] = tmpA[i];
//for( i = 0; i < 3; i++ ) info.m_J2angularAxis[s2 + i] = -tmpB[i];
tmpA = relA.cross( q );
tmpB = relB.cross( q );
if( hasStaticBody && getSolveAngLimit() )
{ // to make constraint between static and dynamic objects more rigid
// remove wA (or wB) from equation if angular limit is hit
tmpB *= factB;
tmpA *= factA;
}
info.m_solverConstraints[3].m_relpos1CrossNormal = tmpA;
tmpB.Invert( out info.m_solverConstraints[3].m_relpos2CrossNormal );
//for( i = 0; i < 3; i++ ) info.m_J1angularAxis[s3 + i] = tmpA[i];
//for( i = 0; i < 3; i++ ) info.m_J2angularAxis[s3 + i] = -tmpB[i];
info.m_solverConstraints[2].m_contactNormal1 = p;
info.m_solverConstraints[3].m_contactNormal1 = q;
p.Invert( out info.m_solverConstraints[2].m_contactNormal2 );
q.Invert( out info.m_solverConstraints[3].m_contactNormal2 );
//for( i = 0; i < 3; i++ ) info.m_J1linearAxis[s2 + i] = p[i];
//for( i = 0; i < 3; i++ ) info.m_J1linearAxis[s3 + i] = q[i];
//for( i = 0; i < 3; i++ ) info.m_J2linearAxis[s2 + i] = -p[i];
//for( i = 0; i < 3; i++ ) info.m_J2linearAxis[s3 + i] = -q[i];
}
else
{ // old way - maybe incorrect if bodies are not on the slider axis
// see discussion "Bug in slider constraint" http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=9&t=4024&start=0
c = bodyB_trans.m_origin - bodyA_trans.m_origin;
btVector3 tmp = c.cross( p );
tmp.Mult( factA, out info.m_solverConstraints[2].m_relpos1CrossNormal );
//for( i = 0; i < 3; i++ ) info.m_J1angularAxis[s2 + i] = factA * tmp[i];
tmp.Mult( factB, out info.m_solverConstraints[2].m_relpos2CrossNormal );
//for( i = 0; i < 3; i++ ) info.m_J2angularAxis[s2 + i] = factB * tmp[i];
tmp = c.cross( q );
tmp.Mult( factA, out info.m_solverConstraints[3].m_relpos1CrossNormal );
//for( i = 0; i < 3; i++ ) info.m_J1angularAxis[s3 + i] = factA * tmp[i];
tmp.Mult( factB, out info.m_solverConstraints[3].m_relpos2CrossNormal );
//for( i = 0; i < 3; i++ ) info.m_J2angularAxis[s3 + i] = factB * tmp[i];
info.m_solverConstraints[2].m_contactNormal1 = p;
info.m_solverConstraints[3].m_contactNormal1 = q;
p.Invert( out info.m_solverConstraints[2].m_contactNormal2 );
q.Invert( out info.m_solverConstraints[3].m_contactNormal2 );
//for( i = 0; i < 3; i++ ) info.m_J1linearAxis[s2 + i] = p[i];
//for( i = 0; i < 3; i++ ) info.m_J1linearAxis[s3 + i] = q[i];
//for( i = 0; i < 3; i++ ) info.m_J2linearAxis[s2 + i] = -p[i];
//for( i = 0; i < 3; i++ ) info.m_J2linearAxis[s3 + i] = -q[i];
}
// compute two elements of right hand side
// k = info.fps * info.erp * getSoftnessOrthoLin();
currERP = ( ( m_flags & btSliderFlags.BT_SLIDER_FLAGS_ERP_ORTLIN ) != 0 ) ? m_softnessOrthoLin : m_softnessOrthoLin * info.erp;
k = info.fps * currERP;
double rhs = k * p.dot( ofs );
info.m_solverConstraints[2].m_rhs = rhs;
//info.m_constraintError[s2] = rhs;
rhs = k * q.dot( ofs );
info.m_solverConstraints[3].m_rhs = rhs;
//info.m_constraintError[s3] = rhs;
if( ( m_flags & btSliderFlags.BT_SLIDER_FLAGS_CFM_ORTLIN ) != 0 )
{
info.m_solverConstraints[2].m_cfm = m_cfmOrthoLin;
info.m_solverConstraints[3].m_cfm = m_cfmOrthoLin;
//info.cfm[s2] = m_cfmOrthoLin;
//info.cfm[s3] = m_cfmOrthoLin;
}
// check linear limits
limit_err = (double)( 0.0 );
limit = 0;
if( getSolveLinLimit() )
{
limit_err = getLinDepth() * signFact;
limit = ( limit_err > (double)( 0.0 ) ) ? 2 : 1;
}
powered = false;
if( getPoweredLinMotor() )
{
powered = true;
}
// if the slider has joint limits or motor, add in the extra row
if( limit != 0 || powered )
{
nrow++;
//srow = nrow * info.rowskip;
info.m_solverConstraints[4].m_contactNormal1 = ax1;
//info.m_J1linearAxis[srow + 0] = ax1[0];
//info.m_J1linearAxis[srow + 1] = ax1[1];
//info.m_J1linearAxis[srow + 2] = ax1[2];
ax1.Invert( out info.m_solverConstraints[4].m_contactNormal2 );
//info.m_J2linearAxis[srow + 0] = -ax1[0];
//info.m_J2linearAxis[srow + 1] = -ax1[1];
//info.m_J2linearAxis[srow + 2] = -ax1[2];
// linear torque decoupling step:
//
// we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies
// do not create a torque couple. in other words, the points that the
// constraint force is applied at must lie along the same ax1 axis.
// a torque couple will result in limited slider-jointed free
// bodies from gaining angular momentum.
if( m_useOffsetForConstraintFrame )
{
// this is needed only when bodyA and bodyB are both dynamic.
if( !hasStaticBody )
{
tmpA = relA.cross( ax1 );
tmpB = relB.cross( ax1 );
info.m_solverConstraints[4].m_relpos1CrossNormal = tmpA;
//info.m_J1angularAxis[srow + 0] = tmpA[0];
//info.m_J1angularAxis[srow + 1] = tmpA[1];
//info.m_J1angularAxis[srow + 2] = tmpA[2];
tmpB.Invert( out info.m_solverConstraints[4].m_relpos2CrossNormal );
//info.m_J2angularAxis[srow + 0] = -tmpB[0];
//info.m_J2angularAxis[srow + 1] = -tmpB[1];
//info.m_J2angularAxis[srow + 2] = -tmpB[2];
}
}
else
{ // The old way. May be incorrect if bodies are not on the slider axis
btVector3 ltd; // Linear Torque Decoupling vector (a torque)
ltd = c.cross( ax1 );
ltd.Mult( factA, out info.m_solverConstraints[4].m_relpos1CrossNormal );
//info.m_J1angularAxis[srow + 0] = factA * ltd[0];
//info.m_J1angularAxis[srow + 1] = factA * ltd[1];
//info.m_J1angularAxis[srow + 2] = factA * ltd[2];
ltd.Mult( factB, out info.m_solverConstraints[4].m_relpos2CrossNormal );
//info.m_J2angularAxis[srow + 0] = factB * ltd[0];
//info.m_J2angularAxis[srow + 1] = factB * ltd[1];
//info.m_J2angularAxis[srow + 2] = factB * ltd[2];
}
// right-hand part
double lostop = getLowerLinLimit();
double histop = getUpperLinLimit();
if( ( limit != 0 ) && ( lostop == histop ) )
{ // the joint motor is ineffective
powered = false;
}
info.m_solverConstraints[4].m_rhs = 0;
info.m_solverConstraints[4].m_lowerLimit = 0;
info.m_solverConstraints[4].m_upperLimit = 0;
currERP = ( ( m_flags & btSliderFlags.BT_SLIDER_FLAGS_ERP_LIMLIN ) != 0 ) ? m_softnessLimLin : info.erp;
if( powered )
{
if( ( m_flags & btSliderFlags.BT_SLIDER_FLAGS_CFM_DIRLIN ) != 0 )
{
info.m_solverConstraints[4].m_cfm = m_cfmDirLin;
}
double tag_vel = getTargetLinMotorVelocity();
double mot_fact = getMotorFactor( m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info.fps * currERP );
info.m_solverConstraints[4].m_rhs -= signFact * mot_fact * getTargetLinMotorVelocity();
info.m_solverConstraints[4].m_lowerLimit += -getMaxLinMotorForce() / info.fps;
info.m_solverConstraints[4].m_upperLimit += getMaxLinMotorForce() / info.fps;
}
if( limit != 0 )
{
k = info.fps * currERP;
info.m_solverConstraints[4].m_rhs += k * limit_err;
if( ( m_flags & btSliderFlags.BT_SLIDER_FLAGS_CFM_LIMLIN ) != 0 )
{
info.m_solverConstraints[4].m_cfm = m_cfmLimLin;
}
if( lostop == histop )
{ // limited low and high simultaneously
info.m_solverConstraints[4].m_lowerLimit = btScalar.BT_MIN_FLOAT;
info.m_solverConstraints[4].m_upperLimit = btScalar.BT_MAX_FLOAT;
}
else if( limit == 1 )
{ // low limit
info.m_solverConstraints[4].m_lowerLimit = btScalar.BT_MIN_FLOAT;
info.m_solverConstraints[4].m_upperLimit = 0;
}
else
{ // high limit
info.m_solverConstraints[4].m_lowerLimit = 0;
info.m_solverConstraints[4].m_upperLimit = btScalar.BT_MAX_FLOAT;
}
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that)
double bounce = btScalar.btFabs( (double)( 1.0 ) - getDampingLimLin() );
if( bounce > (double)( 0.0 ) )
{
double vel = linVelA.dot( ax1 );
vel -= linVelB.dot( ax1 );
vel *= signFact;
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if( limit == 1 )
{ // low limit
if( vel < 0 )
{
double newc = -bounce * vel;
if( newc > info.m_solverConstraints[nrow].m_rhs )
{
info.m_solverConstraints[nrow].m_rhs = newc;
}
}
}
else
{ // high limit - all those computations are reversed
if( vel > 0 )
{
double newc = -bounce * vel;
if( newc < info.m_solverConstraints[nrow].m_rhs )
{
info.m_solverConstraints[nrow].m_rhs = newc;
}
}
}
}
info.m_solverConstraints[4].m_rhs *= getSoftnessLimLin();
} // if(limit)
} // if linear limit
// check angular limits
limit_err = (double)( 0.0 );
limit = 0;
if( getSolveAngLimit() )
{
limit_err = getAngDepth();
limit = ( limit_err > (double)( 0.0 ) ) ? 1 : 2;
}
// if the slider has joint limits, add in the extra row
powered = false;
if( getPoweredAngMotor() )
{
powered = true;
}
if( limit != 0 || powered )
{
nrow++;
//srow = nrow * info.rowskip;
info.m_solverConstraints[nrow].m_relpos1CrossNormal = ax1;
//info.m_J1angularAxis[srow + 0] = ax1[0];
//info.m_J1angularAxis[srow + 1] = ax1[1];
//info.m_J1angularAxis[srow + 2] = ax1[2];
ax1.Invert( out info.m_solverConstraints[nrow].m_relpos2CrossNormal );
//info.m_J2angularAxis[srow + 0] = -ax1[0];
//info.m_J2angularAxis[srow + 1] = -ax1[1];
//info.m_J2angularAxis[srow + 2] = -ax1[2];
double lostop = getLowerAngLimit();
double histop = getUpperAngLimit();
if( limit != 0 && ( lostop == histop ) )
{ // the joint motor is ineffective
powered = false;
}
currERP = ( m_flags & btSliderFlags.BT_SLIDER_FLAGS_ERP_LIMANG ) != 0 ? m_softnessLimAng : info.erp;
if( powered )
{
if( ( m_flags & btSliderFlags.BT_SLIDER_FLAGS_CFM_DIRANG ) != 0 )
{
info.m_solverConstraints[nrow].m_cfm = m_cfmDirAng;
}
double mot_fact = getMotorFactor( m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info.fps * currERP );
info.m_solverConstraints[nrow].m_rhs = mot_fact * getTargetAngMotorVelocity();
info.m_solverConstraints[nrow].m_lowerLimit = -getMaxAngMotorForce() / info.fps;
info.m_solverConstraints[nrow].m_upperLimit = getMaxAngMotorForce() / info.fps;
}
if( limit != 0 )
{
k = info.fps * currERP;
info.m_solverConstraints[nrow].m_rhs += k * limit_err;
if( ( m_flags & btSliderFlags.BT_SLIDER_FLAGS_CFM_LIMANG ) != 0 )
{
info.m_solverConstraints[nrow].m_cfm = m_cfmLimAng;
}
if( lostop == histop )
{
// limited low and high simultaneously
info.m_solverConstraints[nrow].m_lowerLimit = btScalar.BT_MIN_FLOAT;
info.m_solverConstraints[nrow].m_upperLimit = btScalar.BT_MAX_FLOAT;
}
else if( limit == 1 )
{ // low limit
info.m_solverConstraints[nrow].m_lowerLimit = 0;
info.m_solverConstraints[nrow].m_upperLimit = btScalar.BT_MAX_FLOAT;
}
else
{ // high limit
info.m_solverConstraints[nrow].m_lowerLimit = btScalar.BT_MIN_FLOAT;
info.m_solverConstraints[nrow].m_upperLimit = 0;
}
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
double bounce = btScalar.btFabs( (double)( 1.0 ) - getDampingLimAng() );
if( bounce > (double)( 0.0 ) )
{
double vel = m_rbA.getAngularVelocity().dot( ax1 );
vel -= m_rbB.getAngularVelocity().dot( ax1 );
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if( limit == 1 )
{ // low limit
if( vel < 0 )
{
double newc = -bounce * vel;
if( newc > info.m_solverConstraints[nrow].m_rhs )
{
info.m_solverConstraints[nrow].m_rhs = newc;
}
}
}
else
{ // high limit - all those computations are reversed
if( vel > 0 )
{
double newc = -bounce * vel;
if( newc < info.m_solverConstraints[nrow].m_rhs )
{
info.m_solverConstraints[nrow].m_rhs = newc;
}
}
}
}
info.m_solverConstraints[nrow].m_rhs *= getSoftnessLimAng();
} // if(limit)
} // if angular limit or powered
}
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 void PlaneProject( btVector3 point, out btVector3 result )
{
btVector3 tmp;
normal.Mult( ( point.dot( ref normal ) + dist ), out tmp );
point.Sub( ref tmp, out result );
}
PlaneTest DoPlaneTest( ref btPlane p, ref btVector3 v )
{
double a = v.dot( ref p.normal ) + p.dist;
PlaneTest flag = ( a > planetestepsilon )
? PlaneTest.OVER
: ( ( a < -planetestepsilon )
? PlaneTest.UNDER
: PlaneTest.COPLANAR );
return flag;
}
void collideSingleContact( bool usePertube, ref btQuaternion perturbeRot
, btCollisionObjectWrapper convexObjWrap
, ref btTransform convexTransform
, btCollisionObjectWrapper planeObjWrap
, ref btTransform planeTransform
, btDispatcherInfo dispatchInfo, btManifoldResult resultOut
, ref btVector3 planeNormal, double planeConstant
)
{
//btCollisionObjectWrapper convexObjWrap = m_swapped ? body1Wrap : body0Wrap;
//btCollisionObjectWrapper planeObjWrap = m_swapped ? body0Wrap : body1Wrap;
btConvexShape convexShape = (btConvexShape)convexObjWrap.getCollisionShape();
btStaticPlaneShape planeShape = (btStaticPlaneShape)planeObjWrap.getCollisionShape();
bool hasCollision = false;
//planeNormal = planeShape.getPlaneNormal().Copy( out planeNormal );
//double planeConstant = planeShape.getPlaneConstant();
btTransform convexWorldTransform = convexTransform;
//btTransform planeWorldTransform = planeObjWrap.m_worldTransform;
btTransform convexInPlaneTrans;
planeTransform.inverseTimes( ref convexWorldTransform, out convexInPlaneTrans );
if( usePertube )
{
//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;
//convexWorldTransform.getBasis() *= btMatrix3x3( perturbeRot );
}
btTransform planeInConvex;
convexTransform.inverseTimes( ref planeObjWrap.m_collisionObject.m_worldTransform, out planeInConvex );
btVector3 tmp, tmp2;
planeNormal.Invert( out tmp );
planeInConvex.m_basis.Apply( ref tmp, out tmp2 );
btVector3 vtx; convexShape.localGetSupportingVertex( ref tmp2, out vtx );
btVector3 vtxInPlane; convexInPlaneTrans.Apply( ref vtx, out vtxInPlane );
double distance = ( planeNormal.dot( ref vtxInPlane ) - planeConstant );
btVector3 vtxInPlaneProjected; vtxInPlane.AddScale( ref planeNormal, -distance, out vtxInPlaneProjected );
btVector3 vtxInPlaneWorld; planeTransform.Apply( ref vtxInPlaneProjected, out vtxInPlaneWorld );
hasCollision = distance < m_manifoldPtr.getContactBreakingThreshold();
resultOut.setPersistentManifold( m_manifoldPtr );
if( hasCollision )
{
/// report a contact. internally this will be kept persistent, and contact reduction is done
btVector3 normalOnSurfaceB; planeTransform.m_basis.Apply( ref planeNormal, out normalOnSurfaceB );
btScalar.Dbg( "Convex plane adds point " + normalOnSurfaceB + " " + vtxInPlaneWorld + " " + distance.ToString( "g17" ) );
resultOut.addContactPoint( ref normalOnSurfaceB, ref vtxInPlaneWorld, distance );
}
}
internal double solveLinearAxis(
double timeStep,
double jacDiagABInv,
btRigidBody body1, btVector3 pointInA,
btRigidBody body2, btVector3 pointInB,
int limit_index,
btVector3 axis_normal_on_a,
btVector3 anchorPos )
{
///find relative velocity
// btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
// btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
btVector3 rel_pos1 = anchorPos - body1.m_worldTransform.m_origin;
btVector3 rel_pos2 = anchorPos - body2.m_worldTransform.m_origin;
btVector3 vel1 = body1.getVelocityInLocalPoint( ref rel_pos1 );
btVector3 vel2 = body2.getVelocityInLocalPoint( ref rel_pos2 );
btVector3 vel = vel1 - vel2;
double rel_vel = axis_normal_on_a.dot( vel );
/// apply displacement correction
//positional error (zeroth order error)
double depth = -( pointInA - pointInB ).dot( axis_normal_on_a );
double lo = (double)( -btScalar.BT_LARGE_FLOAT );
double hi = (double)( btScalar.BT_LARGE_FLOAT );
double minLimit = m_lowerLimit[limit_index];
double maxLimit = m_upperLimit[limit_index];
//handle the limits
if( minLimit < maxLimit )
{
{
if( depth > maxLimit )
{
depth -= maxLimit;
lo = btScalar.BT_ZERO;
}
else
{
if( depth < minLimit )
{
depth -= minLimit;
hi = btScalar.BT_ZERO;
}
else
{
return 0.0f;
}
}
}
}
double normalImpulse = m_limitSoftness * ( m_restitution * depth / timeStep - m_damping * rel_vel ) * jacDiagABInv;
double oldNormalImpulse = m_accumulatedImpulse[limit_index];
double sum = oldNormalImpulse + normalImpulse;
m_accumulatedImpulse[limit_index] = sum > hi ? btScalar.BT_ZERO : sum < lo ? btScalar.BT_ZERO : sum;
normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
body1.applyImpulse( ref impulse_vector, ref rel_pos1 );
btVector3 tmp;
impulse_vector.Invert( out tmp );
body2.applyImpulse( ref tmp, ref rel_pos2 );
return normalImpulse;
}
static void Main(string[] args)
{
btVector3 testvec = new btVector3(-2, 1, 0);
Console.WriteLine(String.Format("Original: {0}", testvec.testStr()));
btVector3 testvec2 = testvec.absolute();
Console.WriteLine(String.Format("absolute: {0}", testvec2.testStr()));
Console.WriteLine(String.Format("angle:{0}", testvec.angle(testvec2)));
Console.WriteLine(String.Format("closestAxis(orig):{0}", testvec.closestAxis()));
btVector3 testvec3 = testvec.cross(testvec2);
Console.WriteLine(String.Format("cross: {0}", testvec3.testStr()));
Console.WriteLine(String.Format("distance: {0}", testvec.distance(testvec2)));
Console.WriteLine(String.Format("distance2: {0}", testvec.distance2(testvec2)));
Console.WriteLine(String.Format("dot: {0}", testvec.dot(testvec2)));
Console.WriteLine(String.Format("furthestAxis(orig): {0}", testvec.furthestAxis()));
btVector3 testvec4 = testvec.normalized();
Console.WriteLine(String.Format("normalized: {0}", testvec4.testStr()));
testvec4.setInterpolate3(testvec, testvec2, 0.5f);
Console.WriteLine(String.Format("interpolate3: {0}", testvec4.testStr()));
testvec4.setValue(7f, -0.09f, 2.5f);
Console.WriteLine(String.Format("setvec: {0}", testvec4.testStr()));
testvec4.setX(5.0f);
testvec4.setY(-0.25f);
testvec4.setZ(90f);
testvec.setValue(0, 0, -1024);
testvec2.setValue(256, 256, 1024);
Console.WriteLine(String.Format("setvecIndividual: {0}", testvec4.testStr()));
btAxisSweep3 testbtAxisSweep3 = new btAxisSweep3(testvec, testvec2, 50);
btDefaultCollisionConfiguration colconfig = new btDefaultCollisionConfiguration();
btCollisionDispatcher coldisp = new btCollisionDispatcher(colconfig);
btSequentialImpulseConstraintSolver seqimpconssol = new btSequentialImpulseConstraintSolver();
btDiscreteDynamicsWorld dynamicsWorld = new btDiscreteDynamicsWorld(coldisp, testbtAxisSweep3, seqimpconssol,
colconfig);
dynamicsWorld.setGravity(new btVector3(0, 0, -9.87f));
Console.WriteLine(String.Format("stepWorld: {0}", dynamicsWorld.stepSimulation((6f / 60), 5, (1f / 60))));
Console.WriteLine(String.Format("stepWorld: {0}", dynamicsWorld.stepSimulation((6f / 60), 5, (1f / 60))));
Console.WriteLine(String.Format("stepWorld: {0}", dynamicsWorld.stepSimulation((6f / 60), 5, (1f / 60))));
Console.WriteLine(String.Format("stepWorld: {0}", dynamicsWorld.stepSimulation((6f / 60), 5, (1f / 60))));
btQuaternion testquat = new btQuaternion(50, 0, 0, 1);
btQuaternion testquatnorm = testquat.normalized();
Console.WriteLine(String.Format("testquat: {0}", testquat.testStr()));
Console.WriteLine(String.Format("testquatnormalize: {0}", testquatnorm.testStr()));
Console.WriteLine(String.Format("testquatLength: {0}", testquat.length()));
Console.WriteLine(String.Format("testquatnormalizeLength: {0}", testquatnorm.length()));
float[] heightdata = new float[256 * 256];
for (int j = 0; j < 256 * 256; j++)
{
if (j % 2 == 0)
{
heightdata[j] = 21f;
}
else
{
heightdata[j] = 28f;
}
}
btHeightfieldTerrainShape obj = new btHeightfieldTerrainShape(256, 256, heightdata, 1.0f, 0, 256,
(int)btHeightfieldTerrainShape.UPAxis.Z,
(int)btHeightfieldTerrainShape.PHY_ScalarType.
PHY_FLOAT, false);
btCapsuleShape cap = new btCapsuleShape(0.23f, 3);
btTriangleMesh testMesh = new btTriangleMesh(true, false);
testMesh.addTriangle(new btVector3(1, 0, 1), new btVector3(1, 0, -1), new btVector3(-1, 0, -1), false);
testMesh.addTriangle(new btVector3(1, -1, 1), new btVector3(1, -1, -1), new btVector3(-1, -1, -1), false);
testMesh.addTriangle(new btVector3(1, -1, 1), new btVector3(1, 0, 1), new btVector3(-1, -1, -1), false);
testMesh.addTriangle(new btVector3(1, 0, 1), new btVector3(1, -1, -1), new btVector3(-1, 0, -1), false);
testMesh.addTriangle(new btVector3(1, -1, -1), new btVector3(-1, 0, -1), new btVector3(-1, -1, -1), false);
testMesh.addTriangle(new btVector3(1, -1, -1), new btVector3(1, 0, -1), new btVector3(-1, 0, -1), false);
testMesh.addTriangle(new btVector3(1, 0, 1), new btVector3(1, -1, -1), new btVector3(1, 0, -1), false);
testMesh.addTriangle(new btVector3(1, -1, 1), new btVector3(1, -1, -1), new btVector3(1, 0, 1), false);
btGImpactMeshShape meshtest = new btGImpactMeshShape(testMesh);
meshtest.updateBound();
btRigidBody groundbody = new btRigidBody(0,
new btDefaultMotionState(
new btTransform(new btQuaternion(0, 0, 0, 1),
new btVector3(128, 128, 256f / 2f))), obj,
new btVector3(0, 0, 0));
btRigidBody capbody = new btRigidBody(200,
new btDefaultMotionState(
new btTransform(new btQuaternion(0, 0, 0, 1),
new btVector3(128, 128, 25))), cap,
new btVector3(0, 0, 0));
btRigidBody meshbody = new btRigidBody(200,
new btDefaultMotionState(
new btTransform(new btQuaternion(0, 0, 0, 1),
new btVector3(128, 128, 29))), meshtest,
new btVector3(0, 0, 0));
btRigidBodyConstructionInfo constructioninfotest = new btRigidBodyConstructionInfo();
constructioninfotest.m_collisionShape = new btBoxShape(new btVector3(0.5f, 0.5f, 0.5f));
constructioninfotest.m_localInertia = new btVector3(0, 0, 0);
constructioninfotest.m_motionState = new btDefaultMotionState(new btTransform(new btQuaternion(0.3f, -0.4f, 0.8f, 0.1f), new btVector3(128.5f, 128, 25)),
new btTransform(new btQuaternion(0, 0, 0, 1), new btVector3(0, 0.25f, 0)));
constructioninfotest.m_startWorldTransform = new btTransform(new btQuaternion(0, 0, 0, 1), new btVector3(0, 0, 0));
constructioninfotest.m_mass = 2000000;
constructioninfotest.m_linearDamping = 0;
constructioninfotest.m_angularDamping = 0;
constructioninfotest.m_friction = 0.1f;
constructioninfotest.m_restitution = 0;
constructioninfotest.m_linearSleepingThreshold = 0.8f;
constructioninfotest.m_angularSleepingThreshold = 1;
constructioninfotest.m_additionalDamping = false;
constructioninfotest.m_additionalDampingFactor = 0.005f;
constructioninfotest.m_additionalLinearDampingThresholdSqr = 0.01f;
constructioninfotest.m_additionalAngularDampingThresholdSqr = 0.01f;
constructioninfotest.m_additionalAngularDampingFactor = 0.01f;
constructioninfotest.commit();
btGImpactCollisionAlgorithm.registerAlgorithm(coldisp);
btRigidBody cubetest = new btRigidBody(constructioninfotest);
dynamicsWorld.addRigidBody(groundbody);
dynamicsWorld.addRigidBody(cubetest);
dynamicsWorld.addRigidBody(capbody);
dynamicsWorld.addRigidBody(meshbody);
int frame = 0;
for (int i = 0; i < 26; i++)
{
int frames = dynamicsWorld.stepSimulation(((i % 60) / 60f), 10, (1f / 60));
frame += frames;
Console.WriteLine(String.Format("Cube: frame {0} frames: {1} POS:{2}, quat:{3}", frame, frames, cubetest.getInterpolationWorldTransform().getOrigin().testStr(), cubetest.getWorldTransform().getRotation().testStr()));
Console.WriteLine(String.Format("Cap: frame {0} frames: {1} POS:{2}, quat:{3}", frame, frames, capbody.getInterpolationWorldTransform().getOrigin().testStr(), capbody.getWorldTransform().getRotation().testStr()));
Console.WriteLine(String.Format("Mesh: frame {0} frames: {1} POS:{2}, quat:{3}", frame, frames, meshbody.getInterpolationWorldTransform().getOrigin().testStr(), meshbody.getWorldTransform().getRotation().testStr()));
}
dynamicsWorld.removeRigidBody(meshbody);
dynamicsWorld.removeRigidBody(capbody);
dynamicsWorld.removeRigidBody(cubetest);
dynamicsWorld.removeRigidBody(groundbody);
cubetest.Dispose();
groundbody.Dispose();
capbody.Dispose();
cap.Dispose();
obj.Dispose();
testbtAxisSweep3.Dispose();
dynamicsWorld.Dispose();
coldisp.Dispose();
colconfig.Dispose();
seqimpconssol.Dispose();
testvec.Dispose();
testvec2.Dispose();
testvec3.Dispose();
testvec4.Dispose();
}
public static void ClipFaceAgainstHull(ref IndexedVector3 separatingNormal, ConvexPolyhedron hullA, ref IndexedMatrix transA, ObjectArray <IndexedVector3> worldVertsB1, float minDist, float maxDist, IDiscreteCollisionDetectorInterfaceResult resultOut)
{
ObjectArray <IndexedVector3> worldVertsB2 = new ObjectArray <IndexedVector3>();
ObjectArray <IndexedVector3> pVtxIn = worldVertsB1;
ObjectArray <IndexedVector3> pVtxOut = worldVertsB2;
pVtxOut.Capacity = pVtxIn.Count;
int closestFaceA = -1;
{
float dmin = float.MaxValue;
for (int face = 0; face < hullA.m_faces.Count; face++)
{
IndexedVector3 Normal = new IndexedVector3(hullA.m_faces[face].m_plane[0], hullA.m_faces[face].m_plane[1], hullA.m_faces[face].m_plane[2]);
IndexedVector3 faceANormalWS = transA._basis * Normal;
float d = IndexedVector3.Dot(faceANormalWS, separatingNormal);
if (d < dmin)
{
dmin = d;
closestFaceA = face;
}
}
}
if (closestFaceA < 0)
{
return;
}
Face polyA = hullA.m_faces[closestFaceA];
// clip polygon to back of planes of all faces of hull A that are adjacent to witness face
int numContacts = pVtxIn.Count;
int numVerticesA = polyA.m_indices.Count;
for (int e0 = 0; e0 < numVerticesA; e0++)
{
IndexedVector3 a = hullA.m_vertices[polyA.m_indices[e0]];
IndexedVector3 b = hullA.m_vertices[polyA.m_indices[(e0 + 1) % numVerticesA]];
IndexedVector3 edge0 = a - b;
IndexedVector3 WorldEdge0 = transA._basis * edge0;
IndexedVector3 worldPlaneAnormal1 = transA._basis * new IndexedVector3(polyA.m_plane[0], polyA.m_plane[1], polyA.m_plane[2]);
IndexedVector3 planeNormalWS1 = -WorldEdge0.Cross(worldPlaneAnormal1);//.cross(WorldEdge0);
IndexedVector3 worldA1 = transA * a;
float planeEqWS1 = -worldA1.Dot(planeNormalWS1);
//int otherFace=0;
#if BLA1
int otherFace = polyA.m_connectedFaces[e0];
btVector3 localPlaneNormal(hullA.m_faces[otherFace].m_plane[0], hullA.m_faces[otherFace].m_plane[1], hullA.m_faces[otherFace].m_plane[2]);
btScalar localPlaneEq = hullA.m_faces[otherFace].m_plane[3];
btVector3 planeNormalWS = transA.getBasis() * localPlaneNormal;
btScalar planeEqWS = localPlaneEq - planeNormalWS.dot(transA.getOrigin());
#else
IndexedVector3 planeNormalWS = planeNormalWS1;
float planeEqWS = planeEqWS1;
#endif //clip face
ClipFace(pVtxIn, pVtxOut, ref planeNormalWS, planeEqWS);
//btSwap(pVtxIn,pVtxOut);
ObjectArray <IndexedVector3> temp = pVtxIn;
pVtxIn = pVtxOut;
pVtxOut = temp;
pVtxOut.Clear();
}
//#define ONLY_REPORT_DEEPEST_POINT
IndexedVector3 point;
// only keep points that are behind the witness face
{
IndexedVector3 localPlaneNormal = new IndexedVector3(polyA.m_plane[0], polyA.m_plane[1], polyA.m_plane[2]);
float localPlaneEq = polyA.m_plane[3];
IndexedVector3 planeNormalWS = transA._basis * localPlaneNormal;
float planeEqWS = localPlaneEq - IndexedVector3.Dot(planeNormalWS, transA._origin);
for (int i = 0; i < pVtxIn.Count; i++)
{
float depth = IndexedVector3.Dot(planeNormalWS, pVtxIn[i]) + planeEqWS;
if (depth <= minDist)
{
// printf("clamped: depth=%f to minDist=%f\n",depth,minDist);
depth = minDist;
}
if (depth <= maxDist && depth >= minDist)
{
IndexedVector3 point2 = pVtxIn[i];
#if ONLY_REPORT_DEEPEST_POINT
curMaxDist = depth;
#else
#if false
if (depth < -3)
{
printf("error in btPolyhedralContactClipping depth = %f\n", depth);
printf("likely wrong separatingNormal passed in\n");
}
#endif
resultOut.AddContactPoint(ref separatingNormal, ref point2, depth);
#endif
}
}
}
#if ONLY_REPORT_DEEPEST_POINT
if (curMaxDist < maxDist)
{
resultOut.AddContactPoint(ref separatingNormal, ref point, curMaxDist);
}
#endif //ONLY_REPORT_DEEPEST_POINT
}
/// Solve A * x = b, where b is a column vector. This is more efficient
/// than computing the inverse in one-shot cases.
///Solve33 is from Box2d, thanks to Erin Catto,
public void solve33( ref btVector3 b, out btVector3 result )
{
btVector3 col1 = getColumn( 0 );
btVector3 col2 = getColumn( 1 );
btVector3 col3 = getColumn( 2 );
btVector3 tmp;
col2.cross( ref col3, out tmp );
double det = col1.dot( ref tmp );
if( btScalar.btFabs( det ) > btScalar.SIMD_EPSILON )
{
det = 1.0f / det;
}
col2.cross( ref col3, out tmp );
result.x = det * b.dot( ref tmp );
b.cross( ref col3, out tmp );
result.y = det * col1.dot( ref tmp );
col2.cross( ref b, out tmp );
result.z = det * col1.dot( ref tmp );
result.w = 0;
}
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;
}
public double computeAngularImpulseDenominator( ref btVector3 axis, btMatrix3x3 invInertiaWorld )
{
btVector3 vec = axis * invInertiaWorld;
return axis.dot( vec );
}
