//-------------------------------------------------------
public void Reset()
{
btVector3 origin = new btVector3( -3, 10, 0 );
btTransform init = new btTransform( ref btQuaternion.Identity, ref origin );
fallingRigidBody.setWorldTransform( ref init );
origin.x = 3;
init.setOrigin( ref origin );
fallingRigidBody2.setWorldTransform( ref init );
step = 0;
}
void Setup()
{
world = new btDiscreteDynamicsWorld();
world.setDebugDrawer( Program.Drawer );
btVector3 tmp; btVector3.yAxis.Add( ref btVector3.xAxis, out tmp );
tmp.normalized( out tmp );
btCollisionShape groundShape;
if( sloped )
groundShape = new btStaticPlaneShape( ref btVector3.Zero, ref tmp );
else
groundShape = new btStaticPlaneShape( ref btVector3.Zero, ref btVector3.yAxis );
btDefaultMotionState groundMotionState = new btDefaultMotionState();
btRigidBody.btRigidBodyConstructionInfo
groundRigidBodyCI = new btRigidBody.btRigidBodyConstructionInfo( 0, groundMotionState
, groundShape, ref btVector3.Zero );
btRigidBody groundRigidBody = new btRigidBody( groundRigidBodyCI );
world.addRigidBody( groundRigidBody );
//-------------------------------------------------------
{
btCollisionShape fallShape = new btBoxShape( ref btVector3.One );
btVector3 origin = new btVector3( -1, 10, 0 );
btTransform init = new btTransform( ref btQuaternion.Identity, ref origin );
btDefaultMotionState fallMotionState = new btDefaultMotionState( ref init );
btScalar mass = 1;
btVector3 fallInertia;
fallShape.calculateLocalInertia( mass, out fallInertia );
btRigidBody.btRigidBodyConstructionInfo
fallingRigidBodyCI = new btRigidBody.btRigidBodyConstructionInfo( mass, fallMotionState
, fallShape, ref fallInertia );
fallingRigidBody = new btRigidBody( fallingRigidBodyCI );
world.addRigidBody( fallingRigidBody );
}
//-------------------------------------------------------
{
btCollisionShape staticShape = new btBoxShape( ref btVector3.One );
btVector3 origin = new btVector3( -1, 4, 0 );
btTransform init = new btTransform( ref btQuaternion.Identity, ref origin );
btDefaultMotionState fallMotionState = new btDefaultMotionState( ref init );
btScalar mass = 0;
btVector3 fallInertia;
staticShape.calculateLocalInertia( mass, out fallInertia );
btRigidBody.btRigidBodyConstructionInfo
fallingRigidBodyCI = new btRigidBody.btRigidBodyConstructionInfo( mass, fallMotionState
, staticShape, ref fallInertia );
staticRigidBody[0] = new btRigidBody( fallingRigidBodyCI );
world.addRigidBody( staticRigidBody[0] );
}
//-------------------------------------------------------
{
btCompoundShape fallShape2 = new btCompoundShape();
btVector3 origin = new btVector3( 3, 40, 0 );
btTransform init = new btTransform( btQuaternion.Identity, origin );
btDefaultMotionState fallMotionState = new btDefaultMotionState( ref init );
btCollisionShape part = new btBoxShape( ref btVector3.One );
{
int x, y, z;
for( x = -2; x <= 2; x++ )
for( y = -2; y <= 2; y++ )
for( z = -2; z <= 2; z++ )
{
tmp = btVector3.Zero;
tmp.AddScale( ref btVector3.xAxis, x * 2, out tmp );
tmp.AddScale( ref btVector3.yAxis, y * 2, out tmp );
tmp.AddScale( ref btVector3.zAxis, z * 2, out tmp );
init.setOrigin( ref tmp );
fallShape2.addChildShape( ref init, part );
}
}
double mass = 125;// .001;
btVector3 fallInertia;
fallShape2.calculateLocalInertia( mass, out fallInertia ); // fills fallInertia
btRigidBody.btRigidBodyConstructionInfo
fallingRigidBodyCI = new btRigidBody.btRigidBodyConstructionInfo( mass, fallMotionState
, fallShape2, ref fallInertia );
fallingRigidBody2 = new btRigidBody( fallingRigidBodyCI );
world.addRigidBody( fallingRigidBody2 );
}
if( false )
{
//---------------------------------------------------
// Hinge them together
btVector3 pivotInA; btVector3.xAxis.Mult( 3, out pivotInA );
btVector3 axisInA = btVector3.yAxis;
btVector3 pivotInB; btVector3.xAxis.Mult( -3, out pivotInB );
btVector3 axisInB = btVector3.yAxis;
btHingeConstraint constraint = new btHingeConstraint( fallingRigidBody, fallingRigidBody2
, ref pivotInA, ref pivotInB, ref axisInA, ref axisInB );
//constraint.enableMotor( true );
//constraint.setMotorTargetVelocity( 1 );
//constraint.setLimit( -btScalar.SIMD_2_PI, btScalar.SIMD_2_PI );
constraint.enableAngularMotor( true, -1, -0.1 );
world.addConstraint( constraint );
}
}
///computes the exact moment of inertia and the transform from the coordinate system defined by the principal axes of the moment of inertia
///and the center of mass to the current coordinate system. "masses" points to an array of masses of the children. The resulting transform
///"principal" has to be applied inversely to all children transforms in order for the local coordinate system of the compound
///shape to be centered at the center of mass and to coincide with the principal axes. This also necessitates a correction of the world transform
///of the collision object by the principal transform.
void calculatePrincipalAxisTransform( double[] masses, ref btTransform principal, ref btVector3 inertia )
{
int n = m_children.Count;
double totalMass = 0;
btVector3 center = btVector3.Zero;
int k;
for( k = 0; k < n; k++ )
{
Debug.Assert( masses[k] > 0 );
center.AddScale( ref m_children[k].m_transform.m_origin, masses[k], out center );
totalMass += masses[k];
}
Debug.Assert( totalMass > 0 );
center /= totalMass;
principal.setOrigin( ref center );
btMatrix3x3 tensor = new btMatrix3x3( 0, 0, 0, 0, 0, 0, 0, 0, 0);
for( k = 0; k < n; k++ )
{
btVector3 i;
m_children[k].m_childShape.calculateLocalInertia( masses[k], out i );
btTransform t = m_children[k].m_transform;
btVector3 o; t.m_origin.Sub(ref center, out o );
//compute inertia tensor in coordinate system of compound shape
btMatrix3x3 j; t.m_basis.transpose( out j);
j.m_el0 *= i.x;
j.m_el1 *= i.y;
j.m_el2 *= i.z;
btMatrix3x3 j2;
t.m_basis.Mult( ref j, out j2 );
//add inertia tensor
tensor.m_el0 += j2.m_el0;
tensor.m_el1 += j2.m_el1;
tensor.m_el2 += j2.m_el2;
//compute inertia tensor of pointmass at o
double o2 = o.length2();
j[0].setValue( o2, 0, 0 );
j[1].setValue( 0, o2, 0 );
j[2].setValue( 0, 0, o2 );
j.m_el0 += o * -o.x;
j.m_el1 += o * -o.y;
j.m_el2 += o * -o.z;
//add inertia tensor of pointmass
tensor.m_el0.AddScale( ref j.m_el0, masses[k], out tensor.m_el0 );
tensor.m_el1.AddScale( ref j.m_el1, masses[k], out tensor.m_el1 );
tensor.m_el2.AddScale( ref j.m_el2, masses[k], out tensor.m_el2 );
}
tensor.diagonalize( out principal.m_basis, (double)( 0.00001 ), 20 );
inertia.setValue( tensor[0][0], tensor[1][1], tensor[2][2] );
}
//-------------------------------------------------------
public void Reset()
{
btVector3 origin = new btVector3( -1, 10, 0 );
btTransform init = new btTransform( ref btQuaternion.Identity, ref origin );
fallingRigidBody.setWorldTransform( ref init );
fallingRigidBody.setAngularVelocity( ref btVector3.Zero );
fallingRigidBody.setLinearVelocity( ref btVector3.Zero );
origin.x = 3;
origin.y = 50;
init.setOrigin( ref origin );
fallingRigidBody2.setWorldTransform( ref init );
fallingRigidBody2.setAngularVelocity( ref btVector3.Zero );
fallingRigidBody2.setLinearVelocity( ref btVector3.Zero );
step = 0;
}
public btConeTwistConstraint( btRigidBody rbA, ref btTransform rbAFrame )
: base( btObjectTypes.CONETWIST_CONSTRAINT_TYPE, rbA )
{
m_rbAFrame = ( rbAFrame );
m_angularOnly = ( false );
m_useSolveConstraintObsolete = ( CONETWIST_USE_OBSOLETE_SOLVER );
m_rbBFrame = m_rbAFrame;
m_rbBFrame.setOrigin( ref btVector3.Zero );
init();
}
