protected void UpdateActivationState(float timeStep) { BulletGlobals.StartProfile("updateActivationState"); int length = m_nonStaticRigidBodies.Count; for (int i = 0; i < length; ++i) { RigidBody body = m_nonStaticRigidBodies[i]; if (body != null) { body.UpdateDeactivation(timeStep); if (body.WantsSleeping()) { if (body.IsStaticOrKinematicObject()) { body.SetActivationState(ActivationState.ISLAND_SLEEPING); } else { if (body.GetActivationState() == ActivationState.ACTIVE_TAG) { body.SetActivationState(ActivationState.WANTS_DEACTIVATION); } if (body.GetActivationState() == ActivationState.ISLAND_SLEEPING) { IndexedVector3 zero = IndexedVector3.Zero; body.SetAngularVelocity(ref zero); body.SetLinearVelocity(ref zero); if (body.GetMotionState() != null) { body.GetMotionState().SetWorldTransform(body.GetWorldTransform()); } } } } else { if (body.GetActivationState() != ActivationState.DISABLE_DEACTIVATION) { body.SetActivationState(ActivationState.ACTIVE_TAG); } } } } BulletGlobals.StopProfile(); }
public override void AddRigidBody(RigidBody body) { if (!body.IsStaticOrKinematicObject() && 0 == (body.GetFlags() & RigidBodyFlags.BT_DISABLE_WORLD_GRAVITY)) { body.SetGravity(ref m_gravity); } if (body.GetCollisionShape() != null) { if (!body.IsStaticObject()) { m_nonStaticRigidBodies.Add(body); } else { body.SetActivationState(ActivationState.ISLAND_SLEEPING); } bool isDynamic = !(body.IsStaticObject() || body.IsKinematicObject()); CollisionFilterGroups collisionFilterGroup = isDynamic ? CollisionFilterGroups.DefaultFilter : CollisionFilterGroups.StaticFilter; CollisionFilterGroups collisionFilterMask = isDynamic ? CollisionFilterGroups.AllFilter : (CollisionFilterGroups.AllFilter ^ CollisionFilterGroups.StaticFilter); AddCollisionObject(body, collisionFilterGroup, collisionFilterMask); } }
public override void AddRigidBody(RigidBody body, CollisionFilterGroups group, CollisionFilterGroups mask) { if (!body.IsStaticOrKinematicObject() && 0 == (body.GetFlags() & RigidBodyFlags.BT_DISABLE_WORLD_GRAVITY)) { body.SetGravity(ref m_gravity); } if (body.GetCollisionShape() != null) { if (!body.IsStaticObject()) { if (!m_nonStaticRigidBodies.Contains(body)) { m_nonStaticRigidBodies.Add(body); } } else { body.SetActivationState(ActivationState.ISLAND_SLEEPING); } AddCollisionObject(body, group, mask); } }
protected override void Initialize() { base.Initialize(); SetCameraDistance(50.0f); ///collision configuration contains default setup for memory, collision setup m_collisionConfiguration = new DefaultCollisionConfiguration(); //m_collisionConfiguration.setConvexConvexMultipointIterations(); ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) m_dispatcher = new CollisionDispatcher(m_collisionConfiguration); m_broadphase = new DbvtBroadphase(); ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded) m_constraintSolver = new SequentialImpulseConstraintSolver(); m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration); IndexedVector3 gravity = new IndexedVector3(0, -10, 0); m_dynamicsWorld.SetGravity(ref gravity); // NEW => btGhostPairCallback ================================= m_ghostPairCallback = new GhostPairCallback(); m_dynamicsWorld.GetBroadphase().GetOverlappingPairCache().SetInternalGhostPairCallback(m_ghostPairCallback); // Needed once to enable ghost objects inside Bullet // NEW => btGhostObject ======================================= m_ghostObject = new GhostObject(); CollisionShape shape = new BoxShape(new IndexedVector3(5f)); // As far as I know only the world aabb of the shape will be used (i.e. a box always parallel to the x,y,z axis of variable size) m_collisionShapes.Add(shape); m_ghostObject.SetCollisionShape(shape); m_ghostObject.SetCollisionFlags(CollisionFlags.CF_NO_CONTACT_RESPONSE); // We can choose to make it "solid" if we want... m_dynamicsWorld.AddCollisionObject(m_ghostObject, CollisionFilterGroups.SensorTrigger, CollisionFilterGroups.AllFilter & ~CollisionFilterGroups.SensorTrigger); //m_ghostObject.setWorldTransform(btTransform(btQuaternion::getIdentity(),btVector3(0,5,-15))); IndexedMatrix im = IndexedMatrix.CreateFromQuaternion(quatDeg45Y); im._origin = new IndexedVector3(0, 5, -15); m_ghostObject.SetWorldTransform(im); // NEW => btPairCachingGhostObject ============================ m_pairCachingGhostObject = new PairCachingGhostObject(); shape = new ConeShape(7.0f, 14.0f); m_collisionShapes.Add(shape); m_pairCachingGhostObject.SetCollisionShape(shape); m_pairCachingGhostObject.SetCollisionFlags(CollisionFlags.CF_NO_CONTACT_RESPONSE); // We can choose to make it "solid" if we want... m_dynamicsWorld.AddCollisionObject(m_pairCachingGhostObject, CollisionFilterGroups.SensorTrigger, CollisionFilterGroups.AllFilter & ~CollisionFilterGroups.SensorTrigger); //m_pairCachingGhostObject.setWorldTransform(btTransform(btQuaternion::getIdentity(),btVector3(0,5,15))); im._origin = new IndexedVector3(0, 7, 15); m_pairCachingGhostObject.SetWorldTransform(im); //============================================================= ///create a few basic rigid bodies CollisionShape groundShape = new BoxShape(new IndexedVector3(50)); m_collisionShapes.Add(groundShape); IndexedMatrix groundTransform = IndexedMatrix.Identity; groundTransform._origin = new IndexedVector3(0, -50, 0); float mass = 0.0f; LocalCreateRigidBody(mass, groundTransform, groundShape); // spawn some cubes (code pasted from appBasicDemo...) if(true) { //create a few dynamic rigidbodies CollisionShape colShape = new BoxShape(new IndexedVector3(SCALING, SCALING, SCALING)); //btCollisionShape* colShape = new btSphereShape(btScalar(1.)); //CollisionShape colShape = new CylinderShape(new IndexedVector3(1f, 1, 1f)); m_collisionShapes.Add(colShape); /// Create Dynamic Objects IndexedMatrix startTransform = IndexedMatrix.Identity; mass = 1f; //rigidbody is dynamic if and only if mass is non zero, otherwise static bool isDynamic = mass != 0f; IndexedVector3 localInertia = IndexedVector3.Zero; if (isDynamic) { colShape.CalculateLocalInertia(mass, out localInertia); } float start_x = START_POS_X - ARRAY_SIZE_X / 2; float start_y = START_POS_Y; float start_z = START_POS_Z - ARRAY_SIZE_Z / 2; for (int k = 0; k < ARRAY_SIZE_Y; k++) { for (int i = 0; i < ARRAY_SIZE_X; i++) { for (int j = 0; j < ARRAY_SIZE_Z; j++) { startTransform._origin = (new IndexedVector3(2.0f * i + start_x, 20 + 2.0f * k + start_y, 2.0f * j + start_z) * SCALING); //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects DefaultMotionState myMotionState = new DefaultMotionState(startTransform, IndexedMatrix.Identity); RigidBodyConstructionInfo rbInfo = new RigidBodyConstructionInfo(mass, myMotionState, colShape, localInertia); RigidBody body = new RigidBody(rbInfo); //body.setContactProcessingThreshold(colShape.getContactBreakingThreshold()); body.SetActivationState(ActivationState.ISLAND_SLEEPING); m_dynamicsWorld.AddRigidBody(body); body.SetActivationState(ActivationState.ISLAND_SLEEPING); body.SetUserPointer(String.Format("Box X{0} Y{1} Z{2}", k, i, j)); } } } } ClientResetScene(); }
//---------------------------------------------------------------------------------------------- public virtual void MouseFunc(ref MouseState oldMouseState, ref MouseState newMouseState) { IndexedVector3 rayTo = GetRayTo(newMouseState.X, newMouseState.Y); if (WasReleased(ref oldMouseState,ref newMouseState,2)) { ShootBox(rayTo); } else if (WasReleased(ref oldMouseState,ref newMouseState,1)) { //apply an impulse if (m_dynamicsWorld != null) { ClosestRayResultCallback rayCallback = new ClosestRayResultCallback(new IndexedVector3(m_cameraPosition), rayTo); IndexedVector3 ivPos = new IndexedVector3(m_cameraPosition); IndexedVector3 ivTo = new IndexedVector3(rayTo); m_dynamicsWorld.RayTest(ref ivPos,ref ivTo, rayCallback); if (rayCallback.HasHit()) { RigidBody body = RigidBody.Upcast(rayCallback.m_collisionObject); if (body != null) { body.SetActivationState(ActivationState.ACTIVE_TAG); IndexedVector3 impulse = rayTo; impulse.Normalize(); float impulseStrength = 10f; impulse *= impulseStrength; IndexedVector3 relPos = rayCallback.m_hitPointWorld - body.GetCenterOfMassPosition(); body.ApplyImpulse(ref impulse, ref relPos); } } } } else if (newMouseState.LeftButton == ButtonState.Pressed) { //add a point to point constraint for picking if (m_dynamicsWorld != null) { IndexedVector3 rayFrom; if (m_ortho) { rayFrom = rayTo; rayFrom.Z = -100.0f; } else { rayFrom = new IndexedVector3(m_cameraPosition); } ClosestRayResultCallback rayCallback = new ClosestRayResultCallback(ref rayFrom, ref rayTo); IndexedVector3 ivPos = new IndexedVector3(m_cameraPosition); IndexedVector3 ivTo = new IndexedVector3(rayTo); m_dynamicsWorld.RayTest(ref ivPos, ref ivTo, rayCallback); if (rayCallback.HasHit()) { RigidBody body = RigidBody.Upcast(rayCallback.m_collisionObject); if (body != null) { //other exclusions? if (!(body.IsStaticObject() || body.IsKinematicObject())) { pickedBody = body; pickedBody.SetActivationState(ActivationState.DISABLE_DEACTIVATION); IndexedVector3 pickPos = rayCallback.m_hitPointWorld; IndexedVector3 localPivot = body.GetCenterOfMassTransform().Inverse() * pickPos; if (use6Dof) { IndexedMatrix tr = IndexedMatrix.Identity; tr._origin = localPivot; Generic6DofConstraint dof6 = new Generic6DofConstraint(body, ref tr, false); dof6.SetLinearLowerLimit(new IndexedVector3(0, 0, 0)); dof6.SetLinearUpperLimit(new IndexedVector3(0, 0, 0)); dof6.SetAngularLowerLimit(new IndexedVector3(0, 0, 0)); dof6.SetAngularUpperLimit(new IndexedVector3(0, 0, 0)); m_dynamicsWorld.AddConstraint(dof6); m_pickConstraint = dof6; dof6.SetParam(ConstraintParams.BT_CONSTRAINT_STOP_CFM, 0.8f, 0); dof6.SetParam(ConstraintParams.BT_CONSTRAINT_STOP_CFM, 0.8f, 1); dof6.SetParam(ConstraintParams.BT_CONSTRAINT_STOP_CFM, 0.8f, 2); dof6.SetParam(ConstraintParams.BT_CONSTRAINT_STOP_CFM, 0.8f, 3); dof6.SetParam(ConstraintParams.BT_CONSTRAINT_STOP_CFM, 0.8f, 4); dof6.SetParam(ConstraintParams.BT_CONSTRAINT_STOP_CFM, 0.8f, 5); dof6.SetParam(ConstraintParams.BT_CONSTRAINT_STOP_ERP, 0.1f, 0); dof6.SetParam(ConstraintParams.BT_CONSTRAINT_STOP_ERP, 0.1f, 1); dof6.SetParam(ConstraintParams.BT_CONSTRAINT_STOP_ERP, 0.1f, 2); dof6.SetParam(ConstraintParams.BT_CONSTRAINT_STOP_ERP, 0.1f, 3); dof6.SetParam(ConstraintParams.BT_CONSTRAINT_STOP_ERP, 0.1f, 4); dof6.SetParam(ConstraintParams.BT_CONSTRAINT_STOP_ERP, 0.1f, 5); } else { Point2PointConstraint p2p = new Point2PointConstraint(body, ref localPivot); m_dynamicsWorld.AddConstraint(p2p, false); p2p.m_setting.m_impulseClamp = mousePickClamping; p2p.m_setting.m_tau = 0.001f; if (m_pickConstraint != null) { int ibreak = 0; } m_pickConstraint = p2p; } //save mouse position for dragging gOldPickingPos = rayTo; gHitPos = pickPos; gOldPickingDist = (pickPos - rayFrom).Length(); //very weak constraint for picking } } } } } else if (WasReleased(ref oldMouseState,ref newMouseState,0)) { if (m_pickConstraint != null && m_dynamicsWorld != null) { m_dynamicsWorld.RemoveConstraint(m_pickConstraint); m_pickConstraint = null; //printf("removed constraint %i",gPickingConstraintId); m_pickConstraint = null; pickedBody.ForceActivationState(ActivationState.ACTIVE_TAG); pickedBody.SetDeactivationTime(0f); pickedBody = null; } } }
public override void InitializeDemo() { SetCameraDistance(SCALING * 50f); //string filename = @"E:\users\man\bullet\xna-basic-output-1.txt"; //FileStream filestream = File.Open(filename, FileMode.Create, FileAccess.Write, FileShare.Read); //BulletGlobals.g_streamWriter = new StreamWriter(filestream); ///collision configuration contains default setup for memory, collision setup m_collisionConfiguration = new DefaultCollisionConfiguration(); ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) m_dispatcher = new CollisionDispatcher(m_collisionConfiguration); m_broadphase = new DbvtBroadphase(); IOverlappingPairCache pairCache = null; //pairCache = new SortedOverlappingPairCache(); //m_broadphase = new SimpleBroadphase(1000, pairCache); ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded) SequentialImpulseConstraintSolver sol = new SequentialImpulseConstraintSolver(); m_constraintSolver = sol; m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration); IndexedVector3 gravity = new IndexedVector3(0, -10, 0); m_dynamicsWorld.SetGravity(ref gravity); ///create a few basic rigid bodies IndexedVector3 halfExtents = new IndexedVector3(50, 50, 50); //IndexedVector3 halfExtents = new IndexedVector3(10, 10, 10); CollisionShape groundShape = new BoxShape(ref halfExtents); //CollisionShape groundShape = new StaticPlaneShape(new IndexedVector3(0,1,0), 50); m_collisionShapes.Add(groundShape); IndexedMatrix groundTransform = IndexedMatrix.CreateTranslation(new IndexedVector3(0, -50, 0)); //IndexedMatrix groundTransform = IndexedMatrix.CreateTranslation(new IndexedVector3(0,-10,0)); float mass = 0f; LocalCreateRigidBody(mass, ref groundTransform, groundShape); { //create a few dynamic rigidbodies CollisionShape colShape = new BoxShape(new IndexedVector3(SCALING, SCALING, SCALING)); //btCollisionShape* colShape = new btSphereShape(btScalar(1.)); //CollisionShape colShape = new CylinderShape(new IndexedVector3(1f, 1, 1f)); m_collisionShapes.Add(colShape); /// Create Dynamic Objects IndexedMatrix startTransform = IndexedMatrix.Identity; mass = 1f; //rigidbody is dynamic if and only if mass is non zero, otherwise static bool isDynamic = mass != 0f; IndexedVector3 localInertia = IndexedVector3.Zero; if (isDynamic) { colShape.CalculateLocalInertia(mass, out localInertia); } float start_x = START_POS_X - ARRAY_SIZE_X/2; float start_y = START_POS_Y; float start_z = START_POS_Z - ARRAY_SIZE_Z/2; for (int k=0;k<ARRAY_SIZE_Y;k++) { for (int i=0;i<ARRAY_SIZE_X;i++) { for(int j = 0;j<ARRAY_SIZE_Z;j++) { startTransform._origin = (new IndexedVector3(2.0f * i + start_x, 20 + 2.0f * k + start_y, 2.0f * j + start_z) * SCALING); //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects DefaultMotionState myMotionState = new DefaultMotionState(startTransform, IndexedMatrix.Identity); RigidBodyConstructionInfo rbInfo = new RigidBodyConstructionInfo(mass, myMotionState, colShape, localInertia); RigidBody body = new RigidBody(rbInfo); //body->setContactProcessingThreshold(colShape->getContactBreakingThreshold()); body.SetActivationState(ActivationState.ISLAND_SLEEPING); m_dynamicsWorld.AddRigidBody(body); body.SetActivationState(ActivationState.ISLAND_SLEEPING); body.SetUserPointer(String.Format("Box X{0} Y{1} Z{2}", k, i, j)); } } } } ClientResetScene(); }
public override void InitializeDemo() { //string filename = @"E:\users\man\bullet\xna-constraint-output.txt"; //FileStream filestream = File.Open(filename, FileMode.Create, FileAccess.Write, FileShare.Read); //BulletGlobals.g_streamWriter = new StreamWriter(filestream); //maxiterations = 100; m_collisionConfiguration = new DefaultCollisionConfiguration(); m_dispatcher = new CollisionDispatcher(m_collisionConfiguration); IndexedVector3 worldMin = new IndexedVector3(-1000,-1000,-1000); IndexedVector3 worldMax = new IndexedVector3(1000,1000,1000); m_broadphase = new AxisSweep3Internal(ref worldMin, ref worldMax, 0xfffe, 0xffff, 16384, null, false); m_constraintSolver = new SequentialImpulseConstraintSolver(); m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_constraintSolver,m_collisionConfiguration); m_dynamicsWorld.SetDebugDrawer(m_debugDraw); SetCameraDistance(26f); //CollisionShape groundShape = new BoxShape(new IndexedVector3(50f, 40f, 50f)); CollisionShape groundShape = new StaticPlaneShape(new IndexedVector3(0, 1, 0), 40); m_collisionShapes.Add(groundShape); IndexedMatrix groundTransform = IndexedMatrix.Identity; groundTransform._origin = new IndexedVector3(0, -56, 0); RigidBody groundBody = LocalCreateRigidBody(0, ref groundTransform, groundShape); CollisionShape shape = new BoxShape(new IndexedVector3(CUBE_HALF_EXTENTS, CUBE_HALF_EXTENTS, CUBE_HALF_EXTENTS)); m_collisionShapes.Add(shape); IndexedMatrix trans = IndexedMatrix.Identity; trans._origin = new IndexedVector3(0, 20, 0); float mass = 1f; #if true //point to point constraint with a breaking threshold { trans = IndexedMatrix.Identity; trans._origin = new IndexedVector3(1,30,-5); LocalCreateRigidBody( mass,trans,shape); trans._origin = new IndexedVector3(0,0,-5); RigidBody body0 = LocalCreateRigidBody( mass,trans,shape); trans._origin = new IndexedVector3(2*CUBE_HALF_EXTENTS,20,0); mass = 1.0f; RigidBody body1 = null;//localCreateRigidBody( mass,trans,shape); IndexedVector3 pivotInA = new IndexedVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,0); TypedConstraint p2p = new Point2PointConstraint(body0,ref pivotInA); m_dynamicsWorld.AddConstraint(p2p); p2p.SetBreakingImpulseThreshold(10.2f); p2p.SetDbgDrawSize(5.0f); } #endif #if true //point to point constraint (ball socket) //SEEMS OK { //trans = IndexedMatrix.Identity; RigidBody body0 = LocalCreateRigidBody( mass,ref trans,shape); trans._origin = new IndexedVector3(2*CUBE_HALF_EXTENTS,20,0); mass = 1f; RigidBody body1 = null;//localCreateRigidBody( mass,trans,shape); IndexedVector3 pivotInA = new IndexedVector3(CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS); IndexedVector3 axisInA = new IndexedVector3(0,0,1); IndexedVector3 pivotInB = body1 != null ? body1.GetCenterOfMassTransform().Inverse() * (body0.GetCenterOfMassTransform() * (pivotInA)) : pivotInA; IndexedVector3 axisInB = body1 != null ? (body1.GetCenterOfMassTransform()._basis.Inverse() * (body1.GetCenterOfMassTransform()._basis * axisInA)) : body0.GetCenterOfMassTransform()._basis * axisInA; #if P2P TypedConstraint p2p = new Point2PointConstraint(body0,ref pivotInA); //btTypedConstraint* p2p = new btPoint2PointConstraint(*body0,*body1,pivotInA,pivotInB); //btTypedConstraint* hinge = new btHingeConstraint(*body0,*body1,pivotInA,pivotInB,axisInA,axisInB); m_dynamicsWorld.AddConstraint(p2p); p2p.SetDbgDrawSize(5.0f); #else HingeConstraint hinge = new HingeConstraint(body0,ref pivotInA,ref axisInA,false); float targetVelocity = 1f; float maxMotorImpulse = 1.0f; hinge.EnableAngularMotor(true,targetVelocity,maxMotorImpulse); m_dynamicsWorld.AddConstraint(hinge);//p2p); hinge.SetDbgDrawSize(5f); #endif } #endif #if true //create a slider, using the generic D6 constraint // SEEMS OK { mass = 1f; IndexedVector3 sliderWorldPos = new IndexedVector3(0,10,0); IndexedVector3 sliderAxis = new IndexedVector3(1,0,0); float angle=0f;//SIMD_RADS_PER_DEG * 10.f; IndexedBasisMatrix sliderOrientation = new IndexedBasisMatrix(new IndexedQuaternion(sliderAxis,angle)); trans = IndexedMatrix.Identity; trans._origin = sliderWorldPos; //trans.setBasis(sliderOrientation); sliderTransform = trans; d6body0 = LocalCreateRigidBody( mass,ref trans,shape); d6body0.SetActivationState(ActivationState.DISABLE_DEACTIVATION); RigidBody fixedBody1 = LocalCreateRigidBody(0,ref trans,null); m_dynamicsWorld.AddRigidBody(fixedBody1); IndexedMatrix frameInA, frameInB; frameInA = IndexedMatrix.Identity; frameInB = IndexedMatrix.Identity; frameInA._origin = new IndexedVector3(0, 5, 0); frameInB._origin = new IndexedVector3(0, 5, 0); // bool useLinearReferenceFrameA = false;//use fixed frame B for linear llimits bool useLinearReferenceFrameA = true;//use fixed frame A for linear llimits spSlider6Dof = new Generic6DofConstraint(fixedBody1, d6body0,ref frameInA,ref frameInB,useLinearReferenceFrameA); spSlider6Dof.SetLinearLowerLimit(ref lowerSliderLimit); spSlider6Dof.SetLinearUpperLimit(ref hiSliderLimit); //range should be small, otherwise singularities will 'explode' the constraint IndexedVector3 angularLower = new IndexedVector3(-1.5f,0,0); IndexedVector3 angularUpper = -angularLower; spSlider6Dof.SetAngularLowerLimit(ref angularLower); spSlider6Dof.SetAngularUpperLimit(ref angularUpper); // slider.setAngularLowerLimit(IndexedVector3(0,0,0)); // slider.setAngularUpperLimit(IndexedVector3(0,0,0)); spSlider6Dof.SetAngularLowerLimit(new IndexedVector3(-MathUtil.SIMD_PI, 0, 0)); spSlider6Dof.SetAngularUpperLimit(new IndexedVector3(1.5f, 0, 0)); spSlider6Dof.GetTranslationalLimitMotor().m_enableMotor[0] = true; spSlider6Dof.GetTranslationalLimitMotor().m_targetVelocity.X = -5.0f; spSlider6Dof.GetTranslationalLimitMotor().m_maxMotorForce.X = 0.1f; m_dynamicsWorld.AddConstraint(spSlider6Dof); spSlider6Dof.SetDbgDrawSize(5f); } #endif #if true { // create a door using hinge constraint attached to the world CollisionShape pDoorShape = new BoxShape(new IndexedVector3(2.0f, 5.0f, 0.2f)); m_collisionShapes.Add(pDoorShape); IndexedMatrix doorTrans = IndexedMatrix.Identity; doorTrans._origin = new IndexedVector3(-5.0f, -2.0f, 0.0f); RigidBody pDoorBody = LocalCreateRigidBody( 1.0f, ref doorTrans, pDoorShape); pDoorBody.SetActivationState(ActivationState.DISABLE_DEACTIVATION); IndexedVector3 btPivotA = new IndexedVector3( 10f+2.1f, -2.0f, 0.0f ); // right next to the door slightly outside IndexedVector3 btAxisA = new IndexedVector3( 0.0f, 1.0f, 0.0f ); // pointing upwards, aka Y-axis spDoorHinge = new HingeConstraint( pDoorBody, ref btPivotA, ref btAxisA,false ); spDoorHinge.SetLimit(-MathUtil.SIMD_PI * 0.25f, MathUtil.SIMD_PI * 0.25f); m_dynamicsWorld.AddConstraint(spDoorHinge); spDoorHinge.SetDbgDrawSize(5.0f); } #endif #if true { // create a generic 6DOF constraint // SEEMS OK - But debug draw a bit wrong? IndexedMatrix tr = IndexedMatrix.Identity; tr._origin = new IndexedVector3(10f, 6f, 0f); //tr.getBasis().setEulerZYX(0,0,0); // RigidBody pBodyA = localCreateRigidBody( mass, tr, shape); RigidBody pBodyA = LocalCreateRigidBody( 0.0f, ref tr, shape); // RigidBody pBodyA = localCreateRigidBody( 0.0, tr, 0); pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION); tr = IndexedMatrix.Identity; tr._origin = new IndexedVector3(0f, 6f, 0f); //tr.getBasis().setEulerZYX(0,0,0); RigidBody pBodyB = LocalCreateRigidBody(mass, ref tr, shape); pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION); IndexedMatrix frameInA, frameInB; frameInA = IndexedMatrix.CreateTranslation(-5,0,0); frameInB = IndexedMatrix.CreateTranslation(5,0,0); Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, ref frameInA, ref frameInB, true); // btGeneric6DofConstraint* pGen6DOF = new btGeneric6DofConstraint(*pBodyA, *pBodyB, frameInA, frameInB, false); IndexedVector3 linearLower = new IndexedVector3(-10, -2, -1); pGen6DOF.SetLinearLowerLimit(ref linearLower); IndexedVector3 linearUpper = new IndexedVector3(10,2,1); pGen6DOF.SetLinearUpperLimit(ref linearUpper); // ? why again? //linearLower = new IndexedVector3(-10,0,0); //pGen6DOF.setLinearLowerLimit(ref linearLower); // pGen6DOF.setLinearUpperLimit(IndexedVector3(10., 0., 0.)); // pGen6DOF.setLinearLowerLimit(IndexedVector3(0., 0., 0.)); // pGen6DOF.setLinearUpperLimit(IndexedVector3(0., 0., 0.)); // pGen6DOF.getTranslationalLimitMotor().m_enableMotor[0] = true; // pGen6DOF.getTranslationalLimitMotor().m_targetVelocity[0] = 5.0f; // pGen6DOF.getTranslationalLimitMotor().m_maxMotorForce[0] = 0.1f; // pGen6DOF.setAngularLowerLimit(IndexedVector3(0., SIMD_HALF_PI*0.9, 0.)); // pGen6DOF.setAngularUpperLimit(IndexedVector3(0., -SIMD_HALF_PI*0.9, 0.)); // pGen6DOF.setAngularLowerLimit(IndexedVector3(0., 0., -SIMD_HALF_PI)); // pGen6DOF.setAngularUpperLimit(IndexedVector3(0., 0., SIMD_HALF_PI)); IndexedVector3 angularLower = new IndexedVector3(-MathUtil.SIMD_HALF_PI * 0.5f, -0.75f, -MathUtil.SIMD_HALF_PI * 0.8f); IndexedVector3 angularUpper = -angularLower; pGen6DOF.SetAngularLowerLimit(ref angularLower); pGen6DOF.SetAngularUpperLimit(ref angularUpper); // pGen6DOF.setAngularLowerLimit(IndexedVector3(0.f, -0.75, SIMD_HALF_PI * 0.8f)); // pGen6DOF.setAngularUpperLimit(IndexedVector3(0.f, 0.75, -SIMD_HALF_PI * 0.8f)); // pGen6DOF.setAngularLowerLimit(IndexedVector3(0.f, -SIMD_HALF_PI * 0.8f, SIMD_HALF_PI * 1.98f)); // pGen6DOF.setAngularUpperLimit(IndexedVector3(0.f, SIMD_HALF_PI * 0.8f, -SIMD_HALF_PI * 1.98f)); // pGen6DOF.setAngularLowerLimit(IndexedVector3(-0.75,-0.5, -0.5)); // pGen6DOF.setAngularUpperLimit(IndexedVector3(0.75,0.5, 0.5)); // pGen6DOF.setAngularLowerLimit(IndexedVector3(-0.75,0., 0.)); // pGen6DOF.setAngularUpperLimit(IndexedVector3(0.75,0., 0.)); m_dynamicsWorld.AddConstraint(pGen6DOF, true); pGen6DOF.SetDbgDrawSize(5.0f); } #endif #if true { // create a ConeTwist constraint IndexedMatrix tr = IndexedMatrix.CreateTranslation(-10,5,0); RigidBody pBodyA = LocalCreateRigidBody( 1.0f, ref tr, shape); pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION); tr = IndexedMatrix.CreateTranslation(-10,-5,0); RigidBody pBodyB = LocalCreateRigidBody(0.0f, ref tr, shape); IndexedMatrix frameInA, frameInB; frameInA = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI); frameInA._origin = new IndexedVector3(0, -5, 0); frameInB = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI); frameInB._origin = new IndexedVector3(0, 5, 0); ConeTwistConstraint pCT = new ConeTwistConstraint(pBodyA, pBodyB, ref frameInA, ref frameInB); pCT.SetLimit(MathUtil.SIMD_QUARTER_PI * 0.6f, MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_PI * 0.8f, 0.5f); // soft limit == hard limit m_dynamicsWorld.AddConstraint(pCT, true); pCT.SetDbgDrawSize(5.0f); } #endif #if true { // Hinge connected to the world, with motor (to hinge motor with new and old constraint solver) // WORKS OK IndexedMatrix tr = IndexedMatrix.Identity; RigidBody pBody = LocalCreateRigidBody( 1.0f, ref tr, shape); pBody.SetActivationState(ActivationState.DISABLE_DEACTIVATION); IndexedVector3 btPivotA = new IndexedVector3( 10.0f, 0.0f, 0.0f ); IndexedVector3 btAxisA = new IndexedVector3( 0.0f, 0.0f, 1.0f ); HingeConstraint pHinge = new HingeConstraint(pBody, ref btPivotA, ref btAxisA,false); // pHinge.enableAngularMotor(true, -1.0, 0.165); // use for the old solver pHinge.EnableAngularMotor(true, -1.0f, 1.65f); // use for the new SIMD solver m_dynamicsWorld.AddConstraint(pHinge); pHinge.SetDbgDrawSize(5.0f); } #endif #if true { // WORKS OK // create a universal joint using generic 6DOF constraint // create two rigid bodies // static bodyA (parent) on top: IndexedMatrix tr = IndexedMatrix.CreateTranslation(20,4,0); RigidBody pBodyA = LocalCreateRigidBody( 0.0f, ref tr, shape); pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION); // dynamic bodyB (child) below it : tr = IndexedMatrix.CreateTranslation(20,0,0); RigidBody pBodyB = LocalCreateRigidBody(1.0f, ref tr, shape); pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION); // add some (arbitrary) data to build constraint frames IndexedVector3 parentAxis = new IndexedVector3(1.0f, 0.0f, 0.0f); IndexedVector3 childAxis = new IndexedVector3(0.0f, 0.0f, 1.0f); IndexedVector3 anchor = new IndexedVector3(20.0f, 2.0f, 0.0f); UniversalConstraint pUniv = new UniversalConstraint(pBodyA, pBodyB, ref anchor, ref parentAxis, ref childAxis); pUniv.SetLowerLimit(-MathUtil.SIMD_HALF_PI * 0.5f, -MathUtil.SIMD_HALF_PI * 0.5f); pUniv.SetUpperLimit(MathUtil.SIMD_HALF_PI * 0.5f, MathUtil.SIMD_HALF_PI * 0.5f); // add constraint to world m_dynamicsWorld.AddConstraint(pUniv, true); // draw constraint frames and limits for debugging pUniv.SetDbgDrawSize(5.0f); } #endif #if true // WORKS OK { // create a generic 6DOF constraint with springs IndexedMatrix tr = IndexedMatrix.CreateTranslation(-20f,16f,0f); //tr.setIdentity(); //tr.setOrigin(btVector3(btScalar(-20.), btScalar(16.), btScalar(0.))); //tr.getBasis().setEulerZYX(0,0,0); RigidBody pBodyA = LocalCreateRigidBody( 0.0f, ref tr, shape); pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION); //tr.setIdentity(); //tr.setOrigin(btVector3(btScalar(-10.), btScalar(16.), btScalar(0.))); //tr.getBasis().setEulerZYX(0,0,0); tr = IndexedMatrix.CreateTranslation(-10,16,0); RigidBody pBodyB = LocalCreateRigidBody(1.0f, ref tr, shape); pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION); IndexedMatrix frameInA = IndexedMatrix.CreateTranslation(10f,0f,0f); IndexedMatrix frameInB = IndexedMatrix.CreateTranslation(0f,0f,0f); Generic6DofSpringConstraint pGen6DOFSpring = new Generic6DofSpringConstraint(pBodyA, pBodyB, ref frameInA, ref frameInB, true); pGen6DOFSpring.SetLinearUpperLimit(new IndexedVector3(5f, 0f, 0f)); pGen6DOFSpring.SetLinearLowerLimit(new IndexedVector3(-5f, 0f, 0f)); pGen6DOFSpring.SetAngularLowerLimit(new IndexedVector3(0f, 0f, -1.5f)); pGen6DOFSpring.SetAngularUpperLimit(new IndexedVector3(0f, 0f, 1.5f)); m_dynamicsWorld.AddConstraint(pGen6DOFSpring, true); pGen6DOFSpring.SetDbgDrawSize(5.0f); pGen6DOFSpring.EnableSpring(0, true); pGen6DOFSpring.SetStiffness(0, 39.478f); pGen6DOFSpring.SetDamping(0, 0.5f); pGen6DOFSpring.EnableSpring(5, true); pGen6DOFSpring.SetStiffness(5, 39.478f); pGen6DOFSpring.SetDamping(0, 0.3f); pGen6DOFSpring.SetEquilibriumPoint(); } #endif #if true { // WORKS OK // create a Hinge2 joint // create two rigid bodies // static bodyA (parent) on top: IndexedMatrix tr = IndexedMatrix.CreateTranslation(-20f,4f,0f); RigidBody pBodyA = LocalCreateRigidBody( 0.0f, ref tr, shape); pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION); // dynamic bodyB (child) below it : tr = IndexedMatrix.CreateTranslation(-20f,0f,0f); RigidBody pBodyB = LocalCreateRigidBody(1.0f, ref tr, shape); pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION); // add some data to build constraint frames IndexedVector3 parentAxis = new IndexedVector3(0.0f, 1.0f, 0.0f); IndexedVector3 childAxis = new IndexedVector3(1.0f, 0.0f, 0.0f); IndexedVector3 anchor = new IndexedVector3(-20.0f, 0.0f, 0.0f); Hinge2Constraint pHinge2 = new Hinge2Constraint(pBodyA, pBodyB, ref anchor, ref parentAxis, ref childAxis); pHinge2.SetLowerLimit(-MathUtil.SIMD_HALF_PI * 0.5f); pHinge2.SetUpperLimit(MathUtil.SIMD_HALF_PI * 0.5f); // add constraint to world m_dynamicsWorld.AddConstraint(pHinge2, true); // draw constraint frames and limits for debugging pHinge2.SetDbgDrawSize(5.0f); } #endif #if true { // WORKS OK // create a Hinge joint between two dynamic bodies // create two rigid bodies // static bodyA (parent) on top: IndexedMatrix tr = IndexedMatrix.CreateTranslation(-20f,-2f,0f); RigidBody pBodyA = LocalCreateRigidBody( 1.0f, ref tr, shape); pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION); // dynamic bodyB: tr = IndexedMatrix.CreateTranslation(-30f,-2f,0f); RigidBody pBodyB = LocalCreateRigidBody(10.0f, ref tr, shape); pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION); // add some data to build constraint frames IndexedVector3 axisA = new IndexedVector3(0.0f, 1.0f, 0.0f); IndexedVector3 axisB = new IndexedVector3(0.0f, 1.0f, 0.0f); IndexedVector3 pivotA = new IndexedVector3(-5.0f, 0.0f, 0.0f); IndexedVector3 pivotB = new IndexedVector3(5.0f, 0.0f, 0.0f); spHingeDynAB = new HingeConstraint(pBodyA, pBodyB, ref pivotA, ref pivotB, ref axisA, ref axisB); spHingeDynAB.SetLimit(-MathUtil.SIMD_HALF_PI * 0.5f, MathUtil.SIMD_HALF_PI * 0.5f); // add constraint to world m_dynamicsWorld.AddConstraint(spHingeDynAB, true); // draw constraint frames and limits for debugging spHingeDynAB.SetDbgDrawSize(5.0f); } #endif #if true { // 6DOF connected to the world, with motor IndexedMatrix tr = IndexedMatrix.CreateTranslation(10,-15,0); RigidBody pBody = LocalCreateRigidBody( 1.0f, ref tr, shape); pBody.SetActivationState(ActivationState.DISABLE_DEACTIVATION); IndexedMatrix frameB = IndexedMatrix.Identity; Generic6DofConstraint pGen6Dof = new Generic6DofConstraint(pBody, ref frameB, false ); m_dynamicsWorld.AddConstraint(pGen6Dof); pGen6Dof.SetDbgDrawSize(5.0f); pGen6Dof.SetAngularLowerLimit(new IndexedVector3(0,0,0)); pGen6Dof.SetAngularUpperLimit(new IndexedVector3(0,0,0)); pGen6Dof.SetLinearLowerLimit(new IndexedVector3(-10.0f, 0, 0)); pGen6Dof.SetLinearUpperLimit(new IndexedVector3(10.0f, 0, 0)); pGen6Dof.GetTranslationalLimitMotor().m_enableMotor[0] = true; pGen6Dof.GetTranslationalLimitMotor().m_targetVelocity[0] = 5.0f; pGen6Dof.GetTranslationalLimitMotor().m_maxMotorForce[0] = 0.1f; } #endif }