public MultiBodyDynamicsWorld(Dispatcher dispatcher, BroadphaseInterface pairCache, MultiBodyConstraintSolver constraintSolver, CollisionConfiguration collisionConfiguration) { IntPtr native = btMultiBodyDynamicsWorld_new(dispatcher.Native, pairCache.Native, constraintSolver.Native, collisionConfiguration.Native); InitializeUserOwned(native); InitializeMembers(dispatcher, pairCache, constraintSolver); }
public MultiBodyDynamicsWorld(Dispatcher dispatcher, BroadphaseInterface pairCache, MultiBodyConstraintSolver constraintSolver, CollisionConfiguration collisionConfiguration) : base(btMultiBodyDynamicsWorld_new(dispatcher._native, pairCache._native, constraintSolver._native, collisionConfiguration._native), dispatcher, pairCache) { _constraintSolver = constraintSolver; _bodies = new List <MultiBody>(); _constraints = new List <MultiBodyConstraint>(); }
public MultiBodyDynamicsWorld(Dispatcher dispatcher, BroadphaseInterface pairCache, MultiBodyConstraintSolver constraintSolver, CollisionConfiguration collisionConfiguration) : base(btMultiBodyDynamicsWorld_new(dispatcher._native, pairCache._native, constraintSolver._native, collisionConfiguration._native)) { _constraintSolver = constraintSolver; _dispatcher = dispatcher; _broadphase = pairCache; _bodies = new List<MultiBody>(); _constraints = new List<MultiBodyConstraint>(); }
protected override void OnInitializePhysics() { // collision configuration contains default setup for memory, collision setup CollisionConf = new DefaultCollisionConfiguration(); Dispatcher = new CollisionDispatcher(CollisionConf); Broadphase = new DbvtBroadphase(); Solver = new MultiBodyConstraintSolver(); World = new MultiBodyDynamicsWorld(Dispatcher, Broadphase, Solver as MultiBodyConstraintSolver, CollisionConf); World.Gravity = new Vector3(0, -10, 0); // create a few basic rigid bodies BoxShape groundShape = new BoxShape(50, 50, 50); //groundShape.InitializePolyhedralFeatures(); //CollisionShape groundShape = new StaticPlaneShape(new Vector3(0,1,0), 50); CollisionShapes.Add(groundShape); CollisionObject ground = LocalCreateRigidBody(0, Matrix.Translation(0, -50, 0), groundShape); ground.UserObject = "Ground"; // create a few dynamic rigidbodies const float mass = 1.0f; BoxShape colShape = new BoxShape(1); CollisionShapes.Add(colShape); Vector3 localInertia = colShape.CalculateLocalInertia(mass); const float start_x = StartPosX - ArraySizeX / 2; const float start_y = StartPosY; const float start_z = StartPosZ - ArraySizeZ / 2; int k, i, j; for (k = 0; k < ArraySizeY; k++) { for (i = 0; i < ArraySizeX; i++) { for (j = 0; j < ArraySizeZ; j++) { Matrix startTransform = Matrix.Translation( 3 * i + start_x, 3 * k + start_y, 3 * j + start_z ); // using motionstate is recommended, it provides interpolation capabilities // and only synchronizes 'active' objects DefaultMotionState myMotionState = new DefaultMotionState(startTransform); using (var rbInfo = new RigidBodyConstructionInfo(mass, myMotionState, colShape, localInertia)) { var body = new RigidBody(rbInfo); World.AddRigidBody(body); } } } } var settings = new MultiBodySettings() { BasePosition = new Vector3(60, 29.5f, -2) * Scaling, CanSleep = true, CreateConstraints = true, DisableParentCollision = true, // the self-collision has conflicting/non-resolvable contact normals IsFixedBase = false, NumLinks = 2, UsePrismatic = true }; var multiBodyA = CreateFeatherstoneMultiBody(World as MultiBodyDynamicsWorld, settings); settings.NumLinks = 10; settings.BasePosition = new Vector3(0, 29.5f, -settings.NumLinks * 4); settings.IsFixedBase = true; settings.UsePrismatic = false; var multiBodyB = CreateFeatherstoneMultiBody(World as MultiBodyDynamicsWorld, settings); settings.BasePosition = new Vector3(-20 * Scaling, 29.5f * Scaling, -settings.NumLinks * 4 * Scaling); settings.IsFixedBase = false; var multiBodyC = CreateFeatherstoneMultiBody(World as MultiBodyDynamicsWorld, settings); settings.BasePosition = new Vector3(-20, 9.5f, -settings.NumLinks * 4); settings.IsFixedBase = true; settings.UsePrismatic = true; settings.DisableParentCollision = true; var multiBodyPrim = CreateFeatherstoneMultiBody(World as MultiBodyDynamicsWorld, settings); }
protected override void OnInitializePhysics() { // collision configuration contains default setup for memory, collision setup CollisionConf = new DefaultCollisionConfiguration(); Dispatcher = new CollisionDispatcher(CollisionConf); Broadphase = new DbvtBroadphase(); Solver = new MultiBodyConstraintSolver(); World = new MultiBodyDynamicsWorld(Dispatcher, Broadphase, Solver as MultiBodyConstraintSolver, CollisionConf); World.Gravity = new Vector3(0, -10, 0); // create a few basic rigid bodies BoxShape groundShape = new BoxShape(50, 50, 50); //groundShape.InitializePolyhedralFeatures(); //CollisionShape groundShape = new StaticPlaneShape(new Vector3(0,1,0), 50); CollisionShapes.Add(groundShape); CollisionObject ground = LocalCreateRigidBody(0, Matrix.Translation(0, -51.55f, 0), groundShape); ground.UserObject = "Ground"; int numLinks = 5; bool spherical = true; bool floatingBase = false; Vector3 basePosition = new Vector3(-0.4f, 3.0f, 0.0f); Vector3 baseHalfExtents = new Vector3(0.05f, 0.37f, 0.1f); Vector3 linkHalfExtents = new Vector3(0.05f, 0.37f, 0.1f); var mb = CreateFeatherstoneMultiBody(World as MultiBodyDynamicsWorld, numLinks, basePosition, baseHalfExtents, linkHalfExtents, spherical, floatingBase); floatingBase = !floatingBase; mb.CanSleep = true; mb.HasSelfCollision = false; mb.UseGyroTerm = true; bool damping = true; if (damping) { mb.LinearDamping = 0.1f; mb.AngularDamping = 0.9f; } else { mb.LinearDamping = 0; mb.AngularDamping = 0; } if (numLinks > 0) { float q0 = 45.0f * (float)Math.PI / 180.0f; if (spherical) { Quaternion quat0 = Quaternion.RotationAxis(new Vector3(1, 1, 0).Normalized, q0); quat0.Normalize(); mb.SetJointPosMultiDof(0, new float[] { quat0.X, quat0.Y, quat0.Z, quat0.W }); } else { mb.SetJointPosMultiDof(0, new float[] { q0 }); } } AddColliders(mb, baseHalfExtents, linkHalfExtents); LocalCreateRigidBody(1, Matrix.Translation(0, -0.95f, 0), new BoxShape(0.5f, 0.5f, 0.5f)); }
protected override void OnInitializePhysics() { // collision configuration contains default setup for memory, collision setup CollisionConf = new DefaultCollisionConfiguration(); Dispatcher = new CollisionDispatcher(CollisionConf); Broadphase = new DbvtBroadphase(); Solver = new MultiBodyConstraintSolver(); World = new MultiBodyDynamicsWorld(Dispatcher, Broadphase, Solver as MultiBodyConstraintSolver, CollisionConf); World.Gravity = new Vector3(0, -9.81f, 0); const bool floating = false; const bool gyro = false; const int numLinks = 1; const bool canSleep = false; const bool selfCollide = false; Vector3 linkHalfExtents = new Vector3(0.05f, 0.5f, 0.1f); Vector3 baseHalfExtents = new Vector3(0.05f, 0.5f, 0.1f); Vector3 baseInertiaDiag = Vector3.Zero; const float baseMass = 0; multiBody = new MultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep); //multiBody.UseRK4Integration = true; //multiBody.BaseWorldTransform = Matrix.Identity; //init the links Vector3 hingeJointAxis = new Vector3(1, 0, 0); //y-axis assumed up Vector3 parentComToCurrentCom = new Vector3(0, -linkHalfExtents[1], 0); Vector3 currentPivotToCurrentCom = new Vector3(0, -linkHalfExtents[1], 0); Vector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; for(int i = 0; i < numLinks; i++) { const float linkMass = 10; Vector3 linkInertiaDiag = Vector3.Zero; using (var shape = new SphereShape(radius)) { shape.CalculateLocalInertia(linkMass, out linkInertiaDiag); } multiBody.SetupRevolute(i, linkMass, linkInertiaDiag, i - 1, Quaternion.Identity, hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false); } multiBody.FinalizeMultiDof(); (World as MultiBodyDynamicsWorld).AddMultiBody(multiBody); multiBody.CanSleep = canSleep; multiBody.HasSelfCollision = selfCollide; multiBody.UseGyroTerm = gyro; #if PENDULUM_DAMPING multiBody.LinearDamping = 0.1f; multiBody.AngularDamping = 0.9f; #else multiBody.LinearDamping = 0; multiBody.AngularDamping = 0; #endif for (int i = 0; i < numLinks; i++) { var shape = new SphereShape(radius); CollisionShapes.Add(shape); var col = new MultiBodyLinkCollider(multiBody, i); col.CollisionShape = shape; const bool isDynamic = true; CollisionFilterGroups collisionFilterGroup = isDynamic ? CollisionFilterGroups.DefaultFilter : CollisionFilterGroups.StaticFilter; CollisionFilterGroups collisionFilterMask = isDynamic ? CollisionFilterGroups.AllFilter : CollisionFilterGroups.AllFilter & ~CollisionFilterGroups.StaticFilter; World.AddCollisionObject(col, collisionFilterGroup, collisionFilterMask); multiBody.GetLink(i).Collider = col; } }