public virtual void drawArc( ref btVector3 center, ref btVector3 normal, ref btVector3 axis
, double radiusA, double radiusB, double minAngle, double maxAngle,
ref btVector3 color, bool drawSect, double stepDegrees = btScalar.BT_TEN )
{
btVector3 vx = axis;
btVector3 vy; normal.cross( ref axis, out vy );
double step = stepDegrees * btScalar.SIMD_RADS_PER_DEG;
int nSteps = (int)btScalar.btFabs( ( maxAngle - minAngle ) / step );
if( nSteps == 0 ) nSteps = 1;
btVector3 prev;
center.AddScale( ref vx, radiusA * btScalar.btCos( minAngle ), out prev );
prev.AddScale( ref vy, radiusB * btScalar.btSin( minAngle ), out prev );
if( drawSect )
{
drawLine( ref center, ref prev, ref color );
}
for( int i = 1; i <= nSteps; i++ )
{
double angle = minAngle + ( maxAngle - minAngle ) * i / (double)nSteps;
btVector3 next;
center.AddScale( ref vx, radiusA * btScalar.btCos( angle ), out next );
next.AddScale( ref vy, radiusB * btScalar.btSin( angle ), out next );
drawLine( ref prev, ref next, ref color );
prev = next;
}
if( drawSect )
{
drawLine( ref center, ref prev, ref color );
}
}
public 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;
}
public void evalEulerEqn( ref btVector3 w1, ref btVector3 w0, ref btVector3 T, double dt,
ref btMatrix3x3 I, out btVector3 result )
{
btVector3 Iw0;
btVector3 dtT;
btVector3 Iw1;
btVector3 cross;
T.Mult( dt, out dtT );
I.Apply( ref w0, out Iw0 );
dtT.Add( ref Iw0, out Iw0 );
// Iw0 = ( T * dt + I * w0 )
I.Apply( ref w1, out Iw1 );
w1.cross( ref Iw1, out cross );
cross.Mult( dt, out cross );
// cross = w1.cross( I * w1 ) * dt
Iw1.Add( ref cross, out Iw1 );
// Iw1 = Iw1 + cross
Iw1.Sub( ref Iw0, out result );
//btVector3 w2; = I * w1 + w1.cross( I * w1 ) * dt - ( T * dt + I * w0 );
//return w2;
}
/// 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;
}
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 );
}
internal void setupContactConstraint( btSolverConstraint solverConstraint,
btSolverBody bodyA, btSolverBody bodyB,
btManifoldPoint cp, btContactSolverInfo infoGlobal,
out double relaxation,
ref btVector3 rel_pos1, ref btVector3 rel_pos2 )
{
// ref btVector3 pos1 = cp.getPositionWorldOnA();
// ref btVector3 pos2 = cp.getPositionWorldOnB();
btRigidBody rb0 = bodyA.m_originalBody;
btRigidBody rb1 = bodyB.m_originalBody;
// btVector3 rel_pos1 = pos1 - colObj0.getWorldTransform().getOrigin();
// btVector3 rel_pos2 = pos2 - colObj1.getWorldTransform().getOrigin();
//rel_pos1 = pos1 - bodyA.getWorldTransform().getOrigin();
//rel_pos2 = pos2 - bodyB.getWorldTransform().getOrigin();
relaxation = 1;
btVector3 torqueAxis0; rel_pos1.cross( ref cp.m_normalWorldOnB, out torqueAxis0 );
btVector3 tmp;
//solverConstraint.m_angularComponentA = rb0 != null ? rb0.m_invInertiaTensorWorld * torqueAxis0 * rb0.getAngularFactor() : btVector3.Zero;
if( rb0 != null )
{
rb0.m_invInertiaTensorWorld.Apply( ref torqueAxis0, out tmp );
tmp.Mult( ref rb0.m_angularFactor, out solverConstraint.m_angularComponentA );
}
else
solverConstraint.m_angularComponentA = btVector3.Zero;
btVector3 torqueAxis1; rel_pos2.cross( ref cp.m_normalWorldOnB, out torqueAxis1 );
torqueAxis1.Invert( out torqueAxis1 );
//solverConstraint.m_angularComponentB = rb1 != null ? rb1.m_invInertiaTensorWorld * -torqueAxis1 * rb1.getAngularFactor() : btVector3.Zero;
if( rb1 != null )
{
rb1.m_invInertiaTensorWorld.Apply( ref torqueAxis1, out tmp );
tmp.Mult( ref rb1.m_angularFactor, out solverConstraint.m_angularComponentB );
}
else
solverConstraint.m_angularComponentB = btVector3.Zero;
{
#if COMPUTE_IMPULSE_DENOM
double denom0 = rb0.computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
double denom1 = rb1.computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
#else
btVector3 vec;
double denom0 = 0;
double denom1 = 0;
if( rb0 != null )
{
( solverConstraint.m_angularComponentA ).cross( ref rel_pos1, out vec );
denom0 = rb0.getInvMass() + cp.m_normalWorldOnB.dot( vec );
}
if( rb1 != null )
{
solverConstraint.m_angularComponentB.Invert( out tmp );
tmp.cross( ref rel_pos2, out vec );
denom1 = rb1.getInvMass() + cp.m_normalWorldOnB.dot( vec );
}
#endif //COMPUTE_IMPULSE_DENOM
double denom = relaxation / ( denom0 + denom1 );
btScalar.Dbg( "m_jacDiagABInv 3 set to " + denom.ToString( "g17" ) );
solverConstraint.m_jacDiagABInv = denom;
}
if( rb0 != null )
{
solverConstraint.m_contactNormal1 = cp.m_normalWorldOnB;
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
btScalar.Dbg( "Torque Axis to relpos1 " + torqueAxis0 );
}
else
{
solverConstraint.m_contactNormal1 = btVector3.Zero;
solverConstraint.m_relpos1CrossNormal = btVector3.Zero;
}
if( rb1 != null )
{
cp.m_normalWorldOnB.Invert( out solverConstraint.m_contactNormal2 );
solverConstraint.m_relpos2CrossNormal = torqueAxis1;
btScalar.Dbg( "Torque Axis to relpos2 " + torqueAxis1 );
}
else
{
solverConstraint.m_contactNormal2 = btVector3.Zero;
solverConstraint.m_relpos2CrossNormal = btVector3.Zero;
}
double restitution = 0;
double penetration = cp.getDistance() + infoGlobal.m_linearSlop;
{
btVector3 vel1, vel2;
vel1 = rb0 != null ? rb0.getVelocityInLocalPoint( ref rel_pos1 ) : btVector3.Zero;
vel2 = rb1 != null ? rb1.getVelocityInLocalPoint( ref rel_pos2 ) : btVector3.Zero;
// btVector3 vel2 = rb1 ? rb1.getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
btVector3 vel; vel1.Sub( ref vel2, out vel );
double rel_vel = cp.m_normalWorldOnB.dot( ref vel );
solverConstraint.m_friction = cp.m_combinedFriction;
restitution = restitutionCurve( rel_vel, cp.m_combinedRestitution );
if( restitution <= btScalar.BT_ZERO )
{
restitution = 0;
};
}
///warm starting (or zero if disabled)
if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_WARMSTARTING ) != 0 )
{
solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
if( rb0 != null )
{
solverConstraint.m_contactNormal1.Mult2( ref bodyA.m_invMass, ref rb0.m_linearFactor, out tmp );
bodyA.applyImpulse( ref tmp, ref solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse );
}
if( rb1 != null )
{
solverConstraint.m_contactNormal2.Mult2( ref rb1.m_linearFactor, ref bodyB.m_invMass, out tmp );
tmp.Invert( out tmp );
btVector3 tmp2; solverConstraint.m_angularComponentB.Invert( out tmp2 );
bodyB.applyImpulse( ref tmp, ref tmp2, -(double)solverConstraint.m_appliedImpulse );
}
}
else
{
solverConstraint.m_appliedImpulse = 0;
}
solverConstraint.m_appliedPushImpulse = 0;
{
btVector3 externalForceImpulseA = bodyA.m_originalBody != null ? bodyA.m_externalForceImpulse : btVector3.Zero;
btVector3 externalTorqueImpulseA = bodyA.m_originalBody != null ? bodyA.m_externalTorqueImpulse : btVector3.Zero;
btVector3 externalForceImpulseB = bodyB.m_originalBody != null ? bodyB.m_externalForceImpulse : btVector3.Zero;
btVector3 externalTorqueImpulseB = bodyB.m_originalBody != null ? bodyB.m_externalTorqueImpulse : btVector3.Zero;
btScalar.Dbg( "external torque impulses " + externalTorqueImpulseA + externalTorqueImpulseB );
double vel1Dotn = solverConstraint.m_contactNormal1.dotAdded( ref bodyA.m_linearVelocity, ref externalForceImpulseA )
+ solverConstraint.m_relpos1CrossNormal.dotAdded( ref bodyA.m_angularVelocity, ref externalTorqueImpulseA );
double vel2Dotn = solverConstraint.m_contactNormal2.dotAdded( ref bodyB.m_linearVelocity, ref externalForceImpulseB )
+ solverConstraint.m_relpos2CrossNormal.dotAdded( ref bodyB.m_angularVelocity, ref externalTorqueImpulseB );
double rel_vel = vel1Dotn + vel2Dotn;
double positionalError = 0;
double velocityError = restitution - rel_vel;// * damping;
double erp = infoGlobal.m_erp2;
if( !infoGlobal.m_splitImpulse || ( penetration > infoGlobal.m_splitImpulsePenetrationThreshold ) )
{
erp = infoGlobal.m_erp;
}
if( penetration > 0 )
{
positionalError = 0;
velocityError -= penetration / infoGlobal.m_timeStep;
}
else
{
positionalError = -penetration * erp / infoGlobal.m_timeStep;
}
double penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
double velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
if( !infoGlobal.m_splitImpulse || ( penetration > infoGlobal.m_splitImpulsePenetrationThreshold ) )
{
//combine position and velocity into rhs
solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;//-solverConstraint.m_contactNormal1.dot(bodyA.m_externalForce*bodyA.m_invMass-bodyB.m_externalForce/bodyB.m_invMass)*solverConstraint.m_jacDiagABInv;
btScalar.Dbg( "Constraint 3 m_rhs " + solverConstraint.m_rhs.ToString( "g17" ) );
solverConstraint.m_rhsPenetration = 0;
}
else
{
//split position and velocity into rhs and m_rhsPenetration
solverConstraint.m_rhs = velocityImpulse;
btScalar.Dbg( "Constraint 4 m_rhs " + solverConstraint.m_rhs.ToString( "g17" ) );
solverConstraint.m_rhsPenetration = penetrationImpulse;
}
solverConstraint.m_cfm = 0;
solverConstraint.m_lowerLimit = 0;
solverConstraint.m_upperLimit = 1e10f;
}
}
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();
}
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;
}
}
static void orth( ref btVector3 v, out btVector3 result )
{
btVector3 a; v.cross( ref btVector3.Forward, out a );
btVector3 b; v.cross( ref btVector3.yAxis, out b );
if( a.length() > b.length() )
{
a.normalized( out result );
}
else
{
b.normalized( out result );
}
}
public static double DistanceBetweenLines( ref btVector3 ustart, ref btVector3 udir
, ref btVector3 vstart, ref btVector3 vdir
, out btVector3 upoint, out btVector3 vpoint )
{
btVector3 cp;
udir.cross( ref vdir, out cp );
cp.normalize();
double distu = -cp.dot( ref ustart );
double distv = -cp.dot( ref vstart );
double dist = btScalar.btFabs( distu - distv );
btPlane plane;
btVector3 tmp;
//if( upoint )
{
vdir.cross( ref cp, out plane.normal );
plane.normal.normalize();
plane.dist = -plane.normal.dot( ref vstart );
ustart.Add( ref udir, out tmp );
plane.PlaneLineIntersection( ref ustart, ref tmp, out upoint );
}
//if( vpoint )
{
udir.cross( ref cp, out plane.normal );
plane.normal.normalize();
plane.dist = -plane.normal.dot( ref ustart );
ustart.Add( ref vdir, out tmp );
plane.PlaneLineIntersection( ref vstart, ref tmp, out vpoint );
}
return dist;
}
/*@brief Return the cross product of two vectors */
public static void btCross( ref btVector3 v1, ref btVector3 v2, out btVector3 result )
{
v1.cross( ref v2, out result );
}
/*@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 );
}
