//------------------------------------------------------- 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(); }