MultiBody CreateFeatherstoneMultiBody(MultiBodyDynamicsWorld world, int numLinks, Vector3 basePosition, Vector3 baseHalfExtents, Vector3 linkHalfExtents, bool spherical, bool floating) { float mass = 1; Vector3 inertia = Vector3.Zero; if (mass != 0) { using (var box = new BoxShape(baseHalfExtents)) { box.CalculateLocalInertia(mass, out inertia); } } var mb = new MultiBody(numLinks, mass, inertia, !floating, false); //body.HasSelfCollision = false; //body.BaseVelocity = Vector3.Zero; mb.BasePosition = basePosition; //body.WorldToBaseRot = new Quaternion(0, 0, 1, -0.125f * (float)Math.PI); mb.WorldToBaseRot = Quaternion.Identity; float linkMass = 1; Vector3 linkInertia = Vector3.Zero; if (linkMass != 0) { using (var box = new BoxShape(linkHalfExtents)) { box.CalculateLocalInertia(linkMass, out linkInertia); } } //y-axis assumed up Vector3 parentComToCurrentCom = new Vector3(0, -linkHalfExtents[1] * 2.0f, 0); //par body's COM to cur body's COM offset Vector3 currentPivotToCurrentCom = new Vector3(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset Vector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset for (int i = 0; i < numLinks; i++) { if (spherical) { mb.SetupSpherical(i, linkMass, linkInertia, i - 1, Quaternion.Identity, parentComToCurrentPivot, currentPivotToCurrentCom, false); } else { Vector3 hingeJointAxis = new Vector3(1, 0, 0); mb.SetupRevolute(i, linkMass, linkInertia, i - 1, Quaternion.Identity, hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false); } } mb.FinalizeMultiDof(); (World as MultiBodyDynamicsWorld).AddMultiBody(mb); return(mb); }
public FeatherStoneDemoSimulation() { CollisionConfiguration = new DefaultCollisionConfiguration(); Dispatcher = new CollisionDispatcher(CollisionConfiguration); Broadphase = new DbvtBroadphase(); _solver = new MultiBodyConstraintSolver(); MultiBodyWorld = new MultiBodyDynamicsWorld(Dispatcher, Broadphase, _solver, CollisionConfiguration); CreateGround(); int numLinks = 5; bool spherical = true; bool floatingBase = false; var basePosition = new Vector3(-0.4f, 3.0f, 0.0f); var baseHalfExtents = new Vector3(0.05f, 0.37f, 0.1f); var linkHalfExtents = new Vector3(0.05f, 0.37f, 0.1f); var multiBody = CreateFeatherstoneMultiBody(MultiBodyWorld, numLinks, basePosition, baseHalfExtents, linkHalfExtents, spherical, floatingBase); bool damping = true; if (damping) { multiBody.LinearDamping = 0.1f; multiBody.AngularDamping = 0.9f; } else { multiBody.LinearDamping = 0; multiBody.AngularDamping = 0; } if (numLinks > 0) { float q0 = 45.0f * (float)Math.PI / 180.0f; if (spherical) { Quaternion quat0 = Quaternion.RotationAxis(Vector3.Normalize(new Vector3(1, 1, 0)), q0); quat0.Normalize(); multiBody.SetJointPosMultiDof(0, new float[] { quat0.X, quat0.Y, quat0.Z, quat0.W }); } else { multiBody.SetJointPosMultiDof(0, new float[] { q0 }); } } CreateColliders(multiBody, baseHalfExtents, linkHalfExtents); CreateRigidBody(1, Matrix.Translation(0, -0.95f, 0), new BoxShape(0.5f, 0.5f, 0.5f)); }
//GUIHelperInterface m_guiHelper; public CommonMultiBodyBase() { //m_filterCallback = null; m_pairCache = null; m_broadphase = null; m_dispatcher = null; m_solver = null; m_collisionConfiguration = null; m_dynamicsWorld = null; //m_pickedBody = null; //m_pickedConstraint = null; //m_pickingMultiBodyPoint2Point = null; m_prevCanSleep = false; //m_guiHelper = null; }
private static void CleanupMultiBodyWorld(MultiBodyDynamicsWorld world) { for (int i = world.NumMultiBodyConstraints - 1; i >= 0; i--) { MultiBodyConstraint multiBodyConstraint = world.GetMultiBodyConstraint(i); world.RemoveMultiBodyConstraint(multiBodyConstraint); multiBodyConstraint.Dispose(); } for (int i = world.NumMultibodies - 1; i >= 0; i--) { MultiBody multiBody = world.GetMultiBody(i); world.RemoveMultiBody(multiBody); multiBody.Dispose(); } }
public Physics() { ActiveBodies = new List <PhysicalBody>(); AddBodyQueue = new List <PhysicalBody>(); RemoveBodyQueue = new List <PhysicalBody>(); collisionConf = new DefaultCollisionConfiguration(); dispatcher = new CollisionDispatcher(collisionConf); broadphase = new DbvtBroadphase(); var w = new MultiBodyDynamicsWorld(dispatcher, broadphase, new MultiBodyConstraintSolver(), collisionConf); w.SolverInfo.SolverMode = SolverModes.CacheFriendly; w.SolverInfo.Restitution = 0; w.Gravity = new Vector3(0, -9.81f, 0); World = w; }
public virtual void createEmptyDynamicsWorld() { ///collision configuration contains default setup for memory, collision setup CollisionConf = new DefaultCollisionConfiguration(); ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) Dispatcher = new CollisionDispatcher(CollisionConf); m_pairCache = new HashedOverlappingPairCache(); Broadphase = new DbvtBroadphase(m_pairCache); m_solver = new MultiBodyConstraintSolver(); World = m_dynamicsWorld = new MultiBodyDynamicsWorld(Dispatcher, Broadphase, m_solver, CollisionConf); m_dynamicsWorld.Gravity = (new BulletSharp.Math.Vector3(0, -10, 0)); }
public virtual void createEmptyDynamicsWorld() { ///collision configuration contains default setup for memory, collision setup m_collisionConfiguration = new DefaultCollisionConfiguration(); //m_collisionConfiguration.setConvexConvexMultipointIterations(); //m_filterCallback = new MyOverlapFilterCallback2(); ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) m_dispatcher = new CollisionDispatcher(m_collisionConfiguration); m_pairCache = new HashedOverlappingPairCache(); //m_pairCache.OverlapFilterCallback = (m_filterCallback); m_broadphase = new DbvtBroadphase(m_pairCache); //btSimpleBroadphase(); m_solver = new MultiBodyConstraintSolver(); m_dynamicsWorld = new MultiBodyDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration); m_dynamicsWorld.Gravity = (new BulletSharp.Math.Vector3(0, -10, 0)); }
public override void ExitPhysics() { if (m_inverseModel != null) { Debug.Log("Dispose inverse model " + m_inverseModel.NumBodies); m_inverseModel.Dispose(); } Debug.Log("InverseDynamicsExitPhysics"); //cleanup in the reverse order of creation/initialization //remove the rigidbodies from the dynamics world and delete them if (m_dynamicsWorld == null) { int i; for (i = m_dynamicsWorld.NumConstraints - 1; i >= 0; i--) { TypedConstraint tc = m_dynamicsWorld.GetConstraint(i); m_dynamicsWorld.RemoveConstraint(tc); tc.Dispose(); } for (i = m_dynamicsWorld.NumMultiBodyConstraints - 1; i >= 0; i--) { MultiBodyConstraint mbc = m_dynamicsWorld.GetMultiBodyConstraint(i); m_dynamicsWorld.RemoveMultiBodyConstraint(mbc); mbc.Dispose(); } for (i = m_dynamicsWorld.NumMultibodies - 1; i >= 0; i--) { MultiBody mb = m_dynamicsWorld.GetMultiBody(i); m_dynamicsWorld.RemoveMultiBody(mb); mb.Dispose(); } for (i = m_dynamicsWorld.NumCollisionObjects - 1; i >= 0; i--) { CollisionObject obj = m_dynamicsWorld.CollisionObjectArray[i]; RigidBody body = RigidBody.Upcast(obj); if (body != null && body.MotionState != null) { body.MotionState.Dispose(); } m_dynamicsWorld.RemoveCollisionObject(obj); obj.Dispose(); } } if (m_multiBody != null) { m_multiBody.Dispose(); } //delete collision shapes for (int j = 0; j < CollisionShapes.Count; j++) { CollisionShape shape = CollisionShapes[j]; shape.Dispose(); } CollisionShapes.Clear(); m_dynamicsWorld.Dispose(); m_dynamicsWorld = null; m_solver.Dispose(); m_solver = null; Broadphase.Dispose(); Broadphase = null; Dispatcher.Dispose(); Dispatcher = null; m_pairCache.Dispose(); m_pairCache = null; CollisionConf.Dispose(); CollisionConf = null; Debug.Log("After dispose B"); }
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 = CollisionFilterGroups.DefaultFilter; // : CollisionFilterGroups.StaticFilter; CollisionFilterGroups collisionFilterMask = CollisionFilterGroups.AllFilter; // : CollisionFilterGroups.AllFilter & ~CollisionFilterGroups.StaticFilter; World.AddCollisionObject(col, collisionFilterGroup, collisionFilterMask); multiBody.GetLink(i).Collider = col; } }
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); RigidBodyConstructionInfo rbInfo = new RigidBodyConstructionInfo(mass, myMotionState, colShape, localInertia); RigidBody body = new RigidBody(rbInfo); rbInfo.Dispose(); 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); }
MultiBody CreateFeatherstoneMultiBody(MultiBodyDynamicsWorld world, MultiBodySettings settings) { int nLinks = settings.NumLinks; float mass = 13.5f * Scaling; Vector3 inertia = new Vector3(91, 344, 253) * Scaling * Scaling; var body = new MultiBody(nLinks, mass, inertia, settings.IsFixedBase, settings.CanSleep); //body.HasSelfCollision = false; //Quaternion orn = new Quaternion(0, 0, 1, -0.125f * Math.PI); Quaternion orn = new Quaternion(0, 0, 0, 1); body.BasePosition = settings.BasePosition; body.WorldToBaseRot = orn; body.BaseVelocity = Vector3.Zero; Vector3 joint_axis_hinge = new Vector3(1, 0, 0); Vector3 joint_axis_prismatic = new Vector3(0, 0, 1); Quaternion parent_to_child = orn.Inverse(); Vector3 joint_axis_child_prismatic = parent_to_child.Rotate(joint_axis_prismatic); Vector3 joint_axis_child_hinge = parent_to_child.Rotate(joint_axis_hinge); int this_link_num = -1; int link_num_counter = 0; Vector3 pos = new Vector3(0, 0, 9.0500002f) * Scaling; Vector3 joint_axis_position = new Vector3(0, 0, 4.5250001f) * Scaling; for (int i = 0; i < nLinks; i++) { float initial_joint_angle = 0.3f; if (i > 0) { initial_joint_angle = -0.06f; } int child_link_num = link_num_counter++; if (settings.UsePrismatic) // i == (nLinks - 1)) { body.SetupPrismatic(child_link_num, mass, inertia, this_link_num, parent_to_child, joint_axis_child_prismatic, parent_to_child.Rotate(pos), Vector3.Zero, settings.DisableParentCollision); } else { body.SetupRevolute(child_link_num, mass, inertia, this_link_num, parent_to_child, joint_axis_child_hinge, joint_axis_position, parent_to_child.Rotate(pos - joint_axis_position), settings.DisableParentCollision); } body.SetJointPos(child_link_num, initial_joint_angle); this_link_num = i; /*if (false) //!useGroundShape && i == 4) * { * Vector3 pivotInAworld = new Vector3(0, 20, 46); * Vector3 pivotInAlocal = body.WorldPosToLocal(i, pivotInAworld); * Vector3 pivotInBworld = pivotInAworld; * MultiBodyPoint2Point p2p = new MultiBodyPoint2Point(body, i, TypedConstraint.FixedBody, pivotInAlocal, pivotInBworld); * (World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(p2p); * }*/ if (settings.UsePrismatic) { //MultiBodyConstraint con = new MultiBodyJointLimitConstraint(body, nLinks - 1, 2, 3); if (settings.CreateConstraints) { MultiBodyConstraint con = new MultiBodyJointLimitConstraint(body, i, -1, 1); (World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(con); } } else { //if (true) { var con = new MultiBodyJointMotor(body, i, 0, 50000); (World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(con); } var con2 = new MultiBodyJointLimitConstraint(body, i, -1, 1); (World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(con2); } } // Add a collider for the base Quaternion[] worldToLocal = new Quaternion[nLinks + 1]; Vector3[] localOrigin = new Vector3[nLinks + 1]; worldToLocal[0] = body.WorldToBaseRot; localOrigin[0] = body.BasePosition; //Vector3 halfExtents = new Vector3(7.5f, 0.05f, 4.5f); Vector3 halfExtents = new Vector3(7.5f, 0.45f, 4.5f); float[] posB = new float[] { localOrigin[0].X, localOrigin[0].Y, localOrigin[0].Z, 1 }; //float[] quatB = new float[] { worldToLocal[0].X, worldToLocal[0].Y, worldToLocal[0].Z, worldToLocal[0].W }; //if (true) { CollisionShape box = new BoxShape(halfExtents * Scaling); var bodyInfo = new RigidBodyConstructionInfo(mass, null, box, inertia); RigidBody bodyB = new RigidBody(bodyInfo); var collider = new MultiBodyLinkCollider(body, -1); collider.CollisionShape = box; Matrix tr = Matrix.RotationQuaternion(worldToLocal[0].Inverse()) * Matrix.Translation(localOrigin[0]); collider.WorldTransform = tr; bodyB.WorldTransform = tr; World.AddCollisionObject(collider, CollisionFilterGroups.StaticFilter, CollisionFilterGroups.DefaultFilter | CollisionFilterGroups.StaticFilter); collider.Friction = Friction; body.BaseCollider = collider; } for (int i = 0; i < body.NumLinks; i++) { int parent = body.GetParent(i); worldToLocal[i + 1] = body.GetParentToLocalRot(i) * worldToLocal[parent + 1]; localOrigin[i + 1] = localOrigin[parent + 1] + (worldToLocal[i + 1].Inverse().Rotate(body.GetRVector(i))); } for (int i = 0; i < body.NumLinks; i++) { CollisionShape box = new BoxShape(halfExtents * Scaling); var collider = new MultiBodyLinkCollider(body, i); collider.CollisionShape = box; Matrix tr = Matrix.RotationQuaternion(worldToLocal[i + 1].Inverse()) * Matrix.Translation(localOrigin[i + 1]); collider.WorldTransform = tr; World.AddCollisionObject(collider, CollisionFilterGroups.StaticFilter, CollisionFilterGroups.DefaultFilter | CollisionFilterGroups.StaticFilter); collider.Friction = Friction; body.GetLink(i).Collider = collider; //World.DebugDrawer.DrawBox(halfExtents, pos, quat); } (World as MultiBodyDynamicsWorld).AddMultiBody(body); return(body); }
public virtual void exitPhysics() { removePickingConstraint(); //cleanup in the reverse order of creation/initialization //remove the rigidbodies from the dynamics world and delete them if (m_dynamicsWorld == null) { int i; for (i = m_dynamicsWorld.NumConstraints - 1; i >= 0; i--) { m_dynamicsWorld.RemoveConstraint(m_dynamicsWorld.GetConstraint(i)); } for (i = m_dynamicsWorld.NumMultiBodyConstraints - 1; i >= 0; i--) { MultiBodyConstraint mbc = m_dynamicsWorld.GetMultiBodyConstraint(i); m_dynamicsWorld.RemoveMultiBodyConstraint(mbc); mbc.Dispose(); } for (i = m_dynamicsWorld.NumMultibodies - 1; i >= 0; i--) { MultiBody mb = m_dynamicsWorld.GetMultiBody(i); m_dynamicsWorld.RemoveMultiBody(mb); mb.Dispose(); } for (i = m_dynamicsWorld.NumCollisionObjects - 1; i >= 0; i--) { CollisionObject obj = m_dynamicsWorld.CollisionObjectArray[i]; RigidBody body = RigidBody.Upcast(obj); if (body != null && body.MotionState != null) { body.MotionState.Dispose(); } m_dynamicsWorld.RemoveCollisionObject(obj); obj.Dispose(); } } //delete collision shapes for (int j = 0; j < m_collisionShapes.Count; j++) { CollisionObject shape = m_collisionShapes[j]; shape.Dispose(); } m_collisionShapes.Clear(); m_dynamicsWorld.Dispose(); m_dynamicsWorld = null; m_solver.Dispose(); m_solver = null; m_broadphase.Dispose(); m_broadphase = null; m_dispatcher.Dispose(); m_dispatcher = null; m_pairCache.Dispose(); m_pairCache = null; //m_filterCallback.Dispose(); //m_filterCallback = null; m_collisionConfiguration.Dispose(); m_collisionConfiguration = null; }
public void ConvertURDF2BulletInternal( URDFImporterInterface u2b, MultiBodyCreationInterface creation, URDF2BulletCachedData cache, int urdfLinkIndex, Matrix parentTransformInWorldSpace, GameObject parentGameObject, MultiBodyDynamicsWorld world1, bool createMultiBody, string pathPrefix, bool enableConstraints, ConvertURDFFlags flags = 0) { Matrix linkTransformInWorldSpace = Matrix.Identity; int mbLinkIndex = cache.getMbIndexFromUrdfIndex(urdfLinkIndex); int urdfParentIndex = cache.getParentUrdfIndex(urdfLinkIndex); int mbParentIndex = cache.getMbIndexFromUrdfIndex(urdfParentIndex); RigidBody parentRigidBody = null; //b3Printf(); if (debugLevel >= BDebug.DebugType.Debug) { Debug.LogFormat("mb link index = {0}\n", mbLinkIndex); } Matrix parentLocalInertialFrame = Matrix.Identity; float parentMass = (1); BulletSharp.Math.Vector3 parentLocalInertiaDiagonal = new BulletSharp.Math.Vector3(1, 1, 1); if (urdfParentIndex == -2) { if (debugLevel >= BDebug.DebugType.Debug) { Debug.LogFormat("root link has no parent\n"); } } else { if (debugLevel >= BDebug.DebugType.Debug) { Debug.LogFormat("urdf parent index = {0}", urdfParentIndex); Debug.LogFormat("mb parent index = {0}", mbParentIndex); } parentRigidBody = cache.getRigidBodyFromLink(urdfParentIndex); u2b.getMassAndInertia(urdfParentIndex, out parentMass, out parentLocalInertiaDiagonal, out parentLocalInertialFrame); } float mass = 0; Matrix localInertialFrame = Matrix.Identity; BulletSharp.Math.Vector3 localInertiaDiagonal = new BulletSharp.Math.Vector3(0, 0, 0); u2b.getMassAndInertia(urdfLinkIndex, out mass, out localInertiaDiagonal, out localInertialFrame); Matrix parent2joint = Matrix.Identity; UrdfJointTypes jointType; BulletSharp.Math.Vector3 jointAxisInJointSpace; float jointLowerLimit; float jointUpperLimit; float jointDamping; float jointFriction; float jointMaxForce; float jointMaxVelocity; bool hasParentJoint = u2b.getJointInfo2(urdfLinkIndex, out parent2joint, out linkTransformInWorldSpace, out jointAxisInJointSpace, out jointType, out jointLowerLimit, out jointUpperLimit, out jointDamping, out jointFriction, out jointMaxForce, out jointMaxVelocity); string linkName = u2b.getLinkName(urdfLinkIndex); if ((flags & ConvertURDFFlags.CUF_USE_SDF) != 0) { Matrix tmp = new Matrix(); Matrix.Invert(ref parentTransformInWorldSpace, out tmp); Matrix.Multiply(ref tmp, ref linkTransformInWorldSpace, out parent2joint); } else { if ((flags & ConvertURDFFlags.CUF_USE_MJCF) != 0) { linkTransformInWorldSpace = parentTransformInWorldSpace * linkTransformInWorldSpace; } else { linkTransformInWorldSpace = parentTransformInWorldSpace * parent2joint; } } GameObject gameObject = new GameObject(linkName); if (parentGameObject != null) { gameObject.transform.parent = parentGameObject.transform; } //-------------------- /* * bool hasParentJoint = loader.getJointInfo2(linkIndex, out parent2joint, out linkTransformInWorldSpace, out jointAxisInJointSpace, out jointType, out jointLowerLimit, out jointUpperLimit, out jointDamping, out jointFriction, out jointMaxForce, out jointMaxVelocity); * string linkName = loader.getLinkName(linkIndex); * * if ((flags & ConvertURDFFlags.CUF_USE_SDF) != 0) * { * Matrix tmp = parentTransformInWorldSpace.Inverse(); * Matrix.Multiply(ref tmp, ref linkTransformInWorldSpace, out parent2joint); * } * else * { * if ((flags & ConvertURDFFlags.CUF_USE_MJCF) != 0) * { * linkTransformInWorldSpace = parentTransformInWorldSpace * linkTransformInWorldSpace; * } * else * { * linkTransformInWorldSpace = parentTransformInWorldSpace * parent2joint; * } * } * * lgo.transform.position = linkTransformInWorldSpace.Origin.ToUnity(); */ //-------------------- gameObject.transform.position = linkTransformInWorldSpace.Origin.ToUnity(); gameObject.transform.rotation = linkTransformInWorldSpace.Rotation.ToUnity(); BCollisionShape compoundShape = u2b.convertLinkCollisionShapes(urdfLinkIndex, pathPrefix, ref localInertialFrame, gameObject); int graphicsIndex; { graphicsIndex = u2b.convertLinkVisualShapes(urdfLinkIndex, pathPrefix, ref localInertialFrame); } if (compoundShape != null) { UrdfMaterialColor matColor; Color color2 = Color.red; Color specular = new Color(0.5f, 0.5f, 0.5f); if (u2b.getLinkColor2(urdfLinkIndex, out matColor)) { color2 = matColor.m_rgbaColor; specular = matColor.m_specularColor; } if (mass != 0) { if ((flags & ConvertURDFFlags.CUF_USE_URDF_INERTIA) == 0) { compoundShape.GetCollisionShape().CalculateLocalInertia(mass, out localInertiaDiagonal); Debug.Assert(localInertiaDiagonal[0] < 1e10); Debug.Assert(localInertiaDiagonal[1] < 1e10); Debug.Assert(localInertiaDiagonal[2] < 1e10); } URDFLinkContactInfo contactInfo; u2b.getLinkContactInfo(urdfLinkIndex, out contactInfo); //temporary inertia scaling until we load inertia from URDF if ((contactInfo.m_flags & URDF_LinkContactFlags.URDF_CONTACT_HAS_INERTIA_SCALING) != 0) { localInertiaDiagonal *= contactInfo.m_inertiaScaling; } } RigidBody linkRigidBody = null; Matrix inertialFrameInWorldSpace = linkTransformInWorldSpace * localInertialFrame; if (!createMultiBody) { RigidBody body = creation.allocateRigidBody(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrameInWorldSpace, compoundShape.GetCollisionShape()); linkRigidBody = body; world1.AddRigidBody(body); compoundShape.GetCollisionShape().UserIndex = (graphicsIndex); URDFLinkContactInfo contactInfo; u2b.getLinkContactInfo(urdfLinkIndex, out contactInfo); ProcessContactParameters(contactInfo, body); creation.createRigidBodyGraphicsInstance2(urdfLinkIndex, body, color2, specular, graphicsIndex); cache.registerRigidBody(urdfLinkIndex, body, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape.GetCollisionShape(), ref localInertialFrame); //untested: u2b.convertLinkVisualShapes2(linkIndex,urdfLinkIndex,pathPrefix,localInertialFrame,body); } else { if (cache.m_bulletMultiBody == null) { // creating base bool canSleep = false; bool isFixedBase = (mass == 0);//todo: figure out when base is fixed int totalNumJoints = cache.m_totalNumJoints1; //cache.m_bulletMultiBody = creation.allocateMultiBody(urdfLinkIndex, totalNumJoints, mass, localInertiaDiagonal, isFixedBase, canSleep); BMultiBody bmm = cache.m_bulletMultiBody = gameObject.AddComponent <BMultiBody>(); bmm.fixedBase = isFixedBase; bmm.canSleep = canSleep; bmm.baseMass = mass; //new btMultiBody(totalNumJoints, mass, localInertiaDiagonal, isFixedBase, canSleep); //if ((flags & ConvertURDFFlags.CUF_USE_MJCF) != 0) //{ // cache.m_bulletMultiBody.BaseWorldTransform = (linkTransformInWorldSpace); //} //cache.registerMultiBody(urdfLinkIndex, cache.m_bulletMultiBody, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape.GetCollisionShape(), ref localInertialFrame); } } //create a joint if necessary BMultiBodyLink bmbl = null; if (hasParentJoint) { //==================== //btTransform offsetInA, offsetInB; //offsetInA = parentLocalInertialFrame.inverse() * parent2joint; //offsetInB = localInertialFrame.inverse(); //btQuaternion parentRotToThis = offsetInB.getRotation() * offsetInA.inverse().getRotation(); //===================== Matrix offsetInA = new Matrix(), offsetInB = new Matrix(); Matrix.Invert(ref parentLocalInertialFrame, out offsetInA); offsetInA = offsetInA * parent2joint; Matrix.Invert(ref localInertialFrame, out offsetInB); Matrix offsetInAInv = new Matrix(); Matrix.Invert(ref offsetInA, out offsetInAInv); BulletSharp.Math.Quaternion parentRotToThis = offsetInB.GetRotation() * offsetInAInv.GetRotation(); Matrix tmp = new Matrix(); Matrix.Invert(ref parentTransformInWorldSpace, out tmp); Matrix tmp2 = new Matrix(); Matrix.Invert(ref parent2joint, out tmp2); Matrix tmp3 = new Matrix(); Matrix tmp4 = linkTransformInWorldSpace; Matrix link2joint = new Matrix(); Matrix.Multiply(ref tmp, ref tmp2, out tmp3); Matrix.Multiply(ref tmp3, ref tmp4, out link2joint); if (debugLevel >= BDebug.DebugType.Debug) { Matrix linkTransformInWorldSpaceInv = new Matrix(), parentTransformInWorldSpaceInv = new Matrix(); Matrix.Invert(ref linkTransformInWorldSpace, out linkTransformInWorldSpaceInv); Matrix.Invert(ref parentTransformInWorldSpace, out parentTransformInWorldSpaceInv); Debug.Log("Creating link " + linkName + " offsetInA=" + offsetInA.Origin + " offsetInB=" + offsetInB.Origin + " parentLocalInertialFrame=" + parentLocalInertialFrame.Origin + " localInertialFrame=" + localInertialFrame.Origin + " localTrnaform=" + " linkTransInWorldSpace=" + linkTransformInWorldSpace.Origin + " linkTransInWorldSpaceInv=" + linkTransformInWorldSpaceInv.Origin + " parentTransInWorldSpace=" + parentTransformInWorldSpace.Origin + " parentTransInWorldSpaceInv=" + parentTransformInWorldSpaceInv.Origin + " link2joint=" + link2joint.Origin + " jointType=" + jointType); } bool disableParentCollision = true; switch (jointType) { case UrdfJointTypes.URDFFloatingJoint: case UrdfJointTypes.URDFPlanarJoint: case UrdfJointTypes.URDFFixedJoint: { if ((jointType == UrdfJointTypes.URDFFloatingJoint) || (jointType == UrdfJointTypes.URDFPlanarJoint)) { Debug.Log("Warning: joint unsupported, creating a fixed joint instead."); } //creation.addLinkMapping(urdfLinkIndex, mbLinkIndex); if (createMultiBody) { //todo: adjust the center of mass transform and pivot axis properly /* * cache.m_bulletMultiBody.SetupFixed(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex, * parentRotToThis, offsetInA.Origin, -offsetInB.Origin); */ bmbl = gameObject.AddComponent <BMultiBodyLink>(); bmbl.jointType = FeatherstoneJointType.Fixed; bmbl.mass = mass; bmbl.localPivotPosition = offsetInB.Origin.ToUnity(); } else { //b3Printf("Fixed joint\n"); Debug.LogError("TODO Setup 6dof "); /* * Generic6DofSpring2Constraint dof6 = null; * * //backward compatibility * if ((flags & ConvertURDFFlags.CUF_RESERVED) != 0) * { * dof6 = creation.createFixedJoint(urdfLinkIndex, parentRigidBody, linkRigidBody, offsetInA, offsetInB); * } * else * { * dof6 = creation.createFixedJoint(urdfLinkIndex, linkRigidBody, parentRigidBody, offsetInB, offsetInA); * } * if (enableConstraints) * world1.AddConstraint(dof6, true); */ } break; } case UrdfJointTypes.URDFContinuousJoint: case UrdfJointTypes.URDFRevoluteJoint: { //creation.addLinkMapping(urdfLinkIndex, mbLinkIndex); if (createMultiBody) { //cache.m_bulletMultiBody.SetupRevolute(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex, // parentRotToThis, (offsetInB.GetRotation().Rotate(jointAxisInJointSpace)), offsetInA.Origin,//parent2joint.getOrigin(), // -offsetInB.Origin, // disableParentCollision); bmbl = gameObject.AddComponent <BMultiBodyLink>(); bmbl.jointType = FeatherstoneJointType.Revolute; bmbl.mass = mass; bmbl.localPivotPosition = offsetInB.Origin.ToUnity(); bmbl.rotationAxis = offsetInB.Rotation.Rotate(jointAxisInJointSpace).ToUnity(); if (jointType == UrdfJointTypes.URDFRevoluteJoint && jointLowerLimit <= jointUpperLimit) { //string name = u2b.getLinkName(urdfLinkIndex); //printf("create btMultiBodyJointLimitConstraint for revolute link name=%s urdf link index=%d (low=%f, up=%f)\n", name.c_str(), urdfLinkIndex, jointLowerLimit, jointUpperLimit); BMultiBodyJointLimitConstraint mbc = gameObject.AddComponent <BMultiBodyJointLimitConstraint>(); mbc.m_jointLowerLimit = jointLowerLimit; mbc.m_jointUpperLimit = jointUpperLimit; } /* * Debug.Log("=========== Creating joint for: " + gameObject.name); * Debug.Log("parentRotateToThis: " + parentRotToThis.ToUnity().eulerAngles); * Debug.Log("rotationAxis: " + bmbl.rotationAxis); * Debug.Log("offsetInA: " + offsetInA.Origin); * Debug.Log("negOffsetInB: " + -offsetInB.Origin); */ } else { Generic6DofSpring2Constraint dof6 = null; //backwards compatibility if ((flags & ConvertURDFFlags.CUF_RESERVED) != 0) { dof6 = creation.createRevoluteJoint(urdfLinkIndex, parentRigidBody, linkRigidBody, offsetInA, offsetInB, jointAxisInJointSpace, jointLowerLimit, jointUpperLimit); } else { dof6 = creation.createRevoluteJoint(urdfLinkIndex, linkRigidBody, parentRigidBody, offsetInB, offsetInA, jointAxisInJointSpace, jointLowerLimit, jointUpperLimit); } if (enableConstraints) { world1.AddConstraint(dof6, true); } //b3Printf("Revolute/Continuous joint\n"); } break; } case UrdfJointTypes.URDFPrismaticJoint: { //creation.addLinkMapping(urdfLinkIndex, mbLinkIndex); if (createMultiBody) { //cache.m_bulletMultiBody.SetupPrismatic(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex, // parentRotToThis, (offsetInB.GetRotation().Rotate(jointAxisInJointSpace)), offsetInA.Origin,//parent2joint.getOrigin(), // -offsetInB.Origin, // disableParentCollision); bmbl = gameObject.AddComponent <BMultiBodyLink>(); bmbl.jointType = FeatherstoneJointType.Prismatic; bmbl.mass = mass; bmbl.localPivotPosition = offsetInB.Origin.ToUnity(); bmbl.rotationAxis = offsetInB.Rotation.Rotate(jointAxisInJointSpace).ToUnity(); if (jointLowerLimit <= jointUpperLimit) { //string name = u2b.getLinkName(urdfLinkIndex); //printf("create btMultiBodyJointLimitConstraint for prismatic link name=%s urdf link index=%d (low=%f, up=%f)\n", name.c_str(), urdfLinkIndex, jointLowerLimit,jointUpperLimit); BMultiBodyJointLimitConstraint mbc = gameObject.AddComponent <BMultiBodyJointLimitConstraint>(); mbc.m_jointLowerLimit = jointLowerLimit; mbc.m_jointUpperLimit = jointUpperLimit; } //printf("joint lower limit=%d, upper limit = %f\n", jointLowerLimit, jointUpperLimit); } else { Generic6DofSpring2Constraint dof6 = creation.createPrismaticJoint(urdfLinkIndex, parentRigidBody, linkRigidBody, offsetInA, offsetInB, jointAxisInJointSpace, jointLowerLimit, jointUpperLimit); if (enableConstraints) { world1.AddConstraint(dof6, true); } //b3Printf("Prismatic\n"); } break; } default: { //b3Printf("Error: unsupported joint type in URDF (%d)\n", jointType); Debug.Assert(false); break; } } } if (createMultiBody) { if (bmbl != null) { bmbl.jointDamping = jointDamping; bmbl.jointFriction = jointFriction; //bmbl.jointLowerLimit = jointLowerLimit; //bmbl.jointUpperLimit = jointUpperLimit; //bmbl.jointMaxForce = jointMaxForce; //bmbl.jointMaxVelocity = jointMaxVelocity; } { /* * MultiBodyLinkCollider col = creation.allocateMultiBodyLinkCollider(urdfLinkIndex, mbLinkIndex, cache.m_bulletMultiBody); * * compoundShape.GetCollisionShape().UserIndex = (graphicsIndex); * * col.CollisionShape = (compoundShape.GetCollisionShape()); * * Matrix tr = Matrix.Identity; * * tr = linkTransformInWorldSpace; * //if we don't set the initial pose of the btCollisionObject, the simulator will do this * //when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider * * col.WorldTransform = (tr); * * //base and fixed? . static, otherwise flag as dynamic * bool isDynamic = (mbLinkIndex < 0 && cache.m_bulletMultiBody.HasFixedBase) ? false : true; * CollisionFilterGroups collisionFilterGroup = isDynamic ? (CollisionFilterGroups.DefaultFilter) : (CollisionFilterGroups.StaticFilter); * CollisionFilterGroups collisionFilterMask = isDynamic ? (CollisionFilterGroups.AllFilter) : (CollisionFilterGroups.AllFilter ^ CollisionFilterGroups.StaticFilter); * * CollisionFilterGroups colGroup = 0, colMask = 0; * UrdfCollisionFlags collisionFlags = u2b.getCollisionGroupAndMask(urdfLinkIndex, out colGroup, out colMask); * if ((collisionFlags & UrdfCollisionFlags.URDF_HAS_COLLISION_GROUP) != 0) * { * collisionFilterGroup = colGroup; * } * if ((collisionFlags & UrdfCollisionFlags.URDF_HAS_COLLISION_MASK) != 0) * { * collisionFilterMask = colMask; * } * world1.AddCollisionObject(col, collisionFilterGroup, collisionFilterMask); * * color2 = Color.red;//(0.0,0.0,0.5); * Color specularColor = new Color(1, 1, 1); * UrdfMaterialColor matCol; * if (u2b.getLinkColor2(urdfLinkIndex, out matCol)) * { * color2 = matCol.m_rgbaColor; * specularColor = matCol.m_specularColor; * } * { * * * creation.createCollisionObjectGraphicsInstance2(urdfLinkIndex, col, color2, specularColor); * } * { * * * u2b.convertLinkVisualShapes2(mbLinkIndex, urdfLinkIndex, pathPrefix, ref localInertialFrame, col, u2b.getBodyUniqueId()); * } * URDFLinkContactInfo contactInfo; * u2b.getLinkContactInfo(urdfLinkIndex, out contactInfo); * * * ProcessContactParameters(contactInfo, col); * * if (mbLinkIndex >= 0) //???? double-check +/- 1 * { * cache.m_bulletMultiBody.GetLink(mbLinkIndex).Collider = col; * if ((flags & ConvertURDFFlags.CUF_USE_SELF_COLLISION_EXCLUDE_PARENT) != 0) * { * cache.m_bulletMultiBody.GetLink(mbLinkIndex).Flags |= (int)btMultiBodyLinkFlags.BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; * } * if ((flags & ConvertURDFFlags.CUF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS) != 0) * { * cache.m_bulletMultiBody.GetLink(mbLinkIndex).Flags |= (int)btMultiBodyLinkFlags.BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION; * } * } * else * { * cache.m_bulletMultiBody.BaseCollider = (col); * } */ } } else { //u2b.convertLinkVisualShapes2(urdfLinkIndex,urdfIndex,pathPrefix,localInertialFrame,compoundShape); } } List <int> urdfChildIndices = new List <int>(); u2b.getLinkChildIndices(urdfLinkIndex, urdfChildIndices); int numChildren = urdfChildIndices.Count; for (int i = 0; i < numChildren; i++) { int urdfChildLinkIndex = urdfChildIndices[i]; ConvertURDF2BulletInternal(u2b, creation, cache, urdfChildLinkIndex, linkTransformInWorldSpace, gameObject, world1, createMultiBody, pathPrefix, enableConstraints, flags); } }
/* * Does not set any local variables. Is safe to use to create duplicate physics worlds for independant simulation. */ public bool CreatePhysicsWorld(out CollisionWorld world, out CollisionConfiguration collisionConfig, out CollisionDispatcher dispatcher, out BroadphaseInterface broadphase, out ConstraintSolver solver, out SoftBodyWorldInfo softBodyWorldInfo) { bool success = true; if (m_worldType == WorldType.SoftBodyAndRigidBody && m_collisionType == CollisionConfType.DefaultDynamicsWorldCollisionConf) { Debug.LogError("For World Type = SoftBodyAndRigidBody collisionType must be collisionType=SoftBodyRigidBodyCollisionConf. Switching"); m_collisionType = CollisionConfType.SoftBodyRigidBodyCollisionConf; success = false; } collisionConfig = null; if (m_collisionType == CollisionConfType.DefaultDynamicsWorldCollisionConf) { collisionConfig = new DefaultCollisionConfiguration(); } else if (m_collisionType == CollisionConfType.SoftBodyRigidBodyCollisionConf) { collisionConfig = new SoftBodyRigidBodyCollisionConfiguration(); } dispatcher = new CollisionDispatcher(collisionConfig); if (m_broadphaseType == BroadphaseType.DynamicAABBBroadphase) { broadphase = new DbvtBroadphase(); } else if (m_broadphaseType == BroadphaseType.Axis3SweepBroadphase) { broadphase = new AxisSweep3(m_axis3SweepBroadphaseMin.ToBullet(), m_axis3SweepBroadphaseMax.ToBullet(), axis3SweepMaxProxies); } else if (m_broadphaseType == BroadphaseType.Axis3SweepBroadphase_32bit) { broadphase = new AxisSweep3_32Bit(m_axis3SweepBroadphaseMin.ToBullet(), m_axis3SweepBroadphaseMax.ToBullet(), axis3SweepMaxProxies); } else { broadphase = null; } world = null; softBodyWorldInfo = null; solver = null; if (m_worldType == WorldType.CollisionOnly) { world = new CollisionWorld(dispatcher, broadphase, collisionConfig); } else if (m_worldType == WorldType.RigidBodyDynamics) { switch (solverType) { case SolverType.MLCP: DantzigSolver dtsolver = new DantzigSolver(); solver = new MlcpSolver(dtsolver); break; default: break; } world = new DiscreteDynamicsWorld(dispatcher, broadphase, solver, collisionConfig); } else if (m_worldType == WorldType.MultiBodyWorld) { MultiBodyConstraintSolver mbConstraintSolver = null; switch (solverType) { /* case SolverType.MLCP: * DantzigSolver dtsolver = new DantzigSolver(); * mbConstraintSolver = new MultiBodyMLCPConstraintSolver(dtsolver); * break;*/ default: mbConstraintSolver = new MultiBodyConstraintSolver(); break; } world = new MultiBodyDynamicsWorld(dispatcher, broadphase, mbConstraintSolver, collisionConfig); } else if (m_worldType == WorldType.SoftBodyAndRigidBody) { SequentialImpulseConstraintSolver siConstraintSolver = new SequentialImpulseConstraintSolver(); constraintSolver = siConstraintSolver; siConstraintSolver.RandSeed = sequentialImpulseConstraintSolverRandomSeed; m_world = new SoftRigidDynamicsWorld(Dispatcher, Broadphase, siConstraintSolver, CollisionConf); _ddWorld = (DiscreteDynamicsWorld)m_world;; SoftRigidDynamicsWorld _sworld = (SoftRigidDynamicsWorld)m_world; m_world.DispatchInfo.EnableSpu = true; _sworld.WorldInfo.SparseSdf.Initialize(); _sworld.WorldInfo.SparseSdf.Reset(); _sworld.WorldInfo.AirDensity = 1.2f; _sworld.WorldInfo.WaterDensity = 0; _sworld.WorldInfo.WaterOffset = 0; _sworld.WorldInfo.WaterNormal = BulletSharp.Math.Vector3.Zero; _sworld.WorldInfo.Gravity = m_gravity.ToBullet(); } if (world is DiscreteDynamicsWorld) { ((DiscreteDynamicsWorld)world).Gravity = m_gravity.ToBullet(); } if (_doDebugDraw) { DebugDrawUnity db = new DebugDrawUnity(); db.DebugMode = _debugDrawMode; world.DebugDrawer = db; } return(success); }
//todo(erwincoumans) Quick hack, reference to InvertedPendulumPDControl implementation. Will create a separate header/source file for this. public static MultiBody createInvertedPendulumMultiBody(float radius, MultiBodyDynamicsWorld world, Matrix baseWorldTrans, bool fixedBase) { BulletSharp.Math.Vector4[] colors = new BulletSharp.Math.Vector4[] { new BulletSharp.Math.Vector4(1, 0, 0, 1), new BulletSharp.Math.Vector4(0, 1, 0, 1), new BulletSharp.Math.Vector4(0, 1, 1, 1), new BulletSharp.Math.Vector4(1, 1, 0, 1), }; int curColor = 0; bool damping = false; bool gyro = false; int numLinks = 2; bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals bool canSleep = false; bool selfCollide = false; BulletSharp.Math.Vector3 linkHalfExtents = new BulletSharp.Math.Vector3(0.05f, 0.37f, 0.1f); BulletSharp.Math.Vector3 baseHalfExtents = new BulletSharp.Math.Vector3(0.04f, 0.35f, 0.08f); //mbC.forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm //init the base BulletSharp.Math.Vector3 baseInertiaDiag = new BulletSharp.Math.Vector3(0.0f, 0.0f, 0.0f); float baseMass = fixedBase ? 0.0f : 10.0f; if (baseMass != 0) { //CollisionShape *shape = new btSphereShape(baseHalfExtents[0]);// btBoxShape(BulletSharp.Math.Vector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); CollisionShape shape = new BoxShape(new BulletSharp.Math.Vector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); shape.CalculateLocalInertia(baseMass, out baseInertiaDiag); shape.Dispose(); } MultiBody pMultiBody = new MultiBody(numLinks, 0, baseInertiaDiag, fixedBase, canSleep); pMultiBody.BaseWorldTransform = baseWorldTrans; BulletSharp.Math.Vector3 vel = new BulletSharp.Math.Vector3(0, 0, 0); // pMultiBody.setBaseVel(vel); //init the links BulletSharp.Math.Vector3 hingeJointAxis = new BulletSharp.Math.Vector3(1, 0, 0); //y-axis assumed up BulletSharp.Math.Vector3 parentComToCurrentCom = new BulletSharp.Math.Vector3(0, -linkHalfExtents[1] * 2.0f, 0); //par body's COM to cur body's COM offset BulletSharp.Math.Vector3 currentPivotToCurrentCom = new BulletSharp.Math.Vector3(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset BulletSharp.Math.Vector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset ////// float q0 = 1.0f * Mathf.PI / 180.0f; BulletSharp.Math.Quaternion quat0 = new BulletSharp.Math.Quaternion(new BulletSharp.Math.Vector3(1, 0, 0), q0); quat0.Normalize(); ///// for (int i = 0; i < numLinks; ++i) { float linkMass = 1.0f; //if (i==3 || i==2) // linkMass= 1000; BulletSharp.Math.Vector3 linkInertiaDiag = new BulletSharp.Math.Vector3(0.0f, 0.0f, 0.0f); CollisionShape shape = null; if (i == 0) { shape = new BoxShape(new BulletSharp.Math.Vector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));// } else { shape = new SphereShape(radius); } shape.CalculateLocalInertia(linkMass, out linkInertiaDiag); shape.Dispose(); if (!spherical) { //pMultiBody.setupRevolute(i, linkMass, linkInertiaDiag, i - 1, BulletSharp.Math.Quaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false); if (i == 0) { pMultiBody.SetupRevolute(i, linkMass, linkInertiaDiag, i - 1, new BulletSharp.Math.Quaternion(0.0f, 0.0f, 0.0f, 1.0f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false); } else { parentComToCurrentCom = new BulletSharp.Math.Vector3(0, -radius * 2.0f, 0); //par body's COM to cur body's COM offset currentPivotToCurrentCom = new BulletSharp.Math.Vector3(0, -radius, 0); //cur body's COM to cur body's PIV offset parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset pMultiBody.SetupFixed(i, linkMass, linkInertiaDiag, i - 1, new BulletSharp.Math.Quaternion(0.0f, 0.0f, 0.0f, 1.0f), parentComToCurrentPivot, currentPivotToCurrentCom); } } else { //pMultiBody.setupPlanar(i, linkMass, linkInertiaDiag, i - 1, BulletSharp.Math.Quaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, BulletSharp.Math.Vector3(1, 0, 0), parentComToCurrentPivot*2, false); pMultiBody.SetupSpherical(i, linkMass, linkInertiaDiag, i - 1, new BulletSharp.Math.Quaternion(0.0f, 0.0f, 0.0f, 1.0f), parentComToCurrentPivot, currentPivotToCurrentCom, false); } } pMultiBody.FinalizeMultiDof(); world.AddMultiBody(pMultiBody); MultiBody mbC = pMultiBody; mbC.CanSleep = (canSleep); mbC.HasSelfCollision = (selfCollide); mbC.UseGyroTerm = (gyro); // if (!damping) { mbC.LinearDamping = (0.0f); mbC.AngularDamping = (0.0f); } else { mbC.LinearDamping = (0.1f); mbC.AngularDamping = (0.9f); } if (numLinks > 0) { q0 = 180.0f * Mathf.PI / 180.0f; if (!spherical) { mbC.SetJointPosMultiDof(0, new float[] { q0 }); } else { BulletSharp.Math.Vector3 vv = new BulletSharp.Math.Vector3(1, 1, 0); vv.Normalize(); quat0 = new BulletSharp.Math.Quaternion(vv, q0); quat0.Normalize(); float[] quat0fs = new float[] { quat0.X, quat0.Y, quat0.Z, quat0.W }; mbC.SetJointPosMultiDof(0, quat0fs); } } /// BulletSharp.Math.Quaternion[] world_to_local; //btAlignedObjectArray<BulletSharp.Math.Quaternion> world_to_local = new BulletSharp.Math.Quaternion[pMultiBody.NumLinks + 1]; BulletSharp.Math.Vector3[] local_origin; //btAlignedObjectArray<BulletSharp.Math.Vector3> local_origin = new BulletSharp.Math.Vector3[pMultiBody.NumLinks + 1]; world_to_local[0] = pMultiBody.WorldToBaseRot; local_origin[0] = pMultiBody.BasePosition; // double friction = 1; { if (true) { CollisionShape shape = new BoxShape(new BulletSharp.Math.Vector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); //new btSphereShape(baseHalfExtents[0]); // guiHelper.createCollisionShapeGraphicsObject(shape); MultiBodyLinkCollider col = new MultiBodyLinkCollider(pMultiBody, -1); col.CollisionShape = shape; Matrix tr = new Matrix(); tr.ScaleVector = BulletSharp.Math.Vector3.One; //if we don't set the initial pose of the btCollisionObject, the simulator will do this //when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider tr.Origin = local_origin[0]; BulletSharp.Math.Quaternion orn = new BulletSharp.Math.Quaternion(new BulletSharp.Math.Vector3(0, 0, 1), 0.25f * 3.1415926538f); tr.Rotation = (orn); col.WorldTransform = (tr); bool isDynamic = (baseMass > 0 && !fixedBase); CollisionFilterGroups collisionFilterGroup = isDynamic ? CollisionFilterGroups.DefaultFilter : CollisionFilterGroups.StaticFilter; CollisionFilterGroups collisionFilterMask = isDynamic ? CollisionFilterGroups.AllFilter : CollisionFilterGroups.AllFilter ^ CollisionFilterGroups.StaticFilter; world.AddCollisionObject(col, collisionFilterGroup, collisionFilterMask);//, 2,1+2); BulletSharp.Math.Vector4 color = new BulletSharp.Math.Vector4(0.0f, 0.0f, 0.5f, 1f); //guiHelper.createCollisionObjectGraphicsObject(col, color); // col.setFriction(friction); pMultiBody.BaseCollider = (col); } } for (int i = 0; i < pMultiBody.NumLinks; ++i) { int parent = pMultiBody.GetParent(i); world_to_local[i + 1] = pMultiBody.GetParentToLocalRot(i) * world_to_local[parent + 1]; BulletSharp.Math.Vector3 vv = world_to_local[i + 1].Inverse.Rotate(pMultiBody.GetRVector(i)); local_origin[i + 1] = local_origin[parent + 1] + vv; } for (int i = 0; i < pMultiBody.NumLinks; ++i) { BulletSharp.Math.Vector3 posr = local_origin[i + 1]; // float pos[4]={posr.x(),posr.y(),posr.z(),1}; float[] quat = new float[] { -world_to_local[i + 1].X, -world_to_local[i + 1].Y, -world_to_local[i + 1].Z, world_to_local[i + 1].W }; CollisionShape shape = null; if (i == 0) { shape = new BoxShape(new BulletSharp.Math.Vector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));//btSphereShape(linkHalfExtents[0]); } else { shape = new SphereShape(radius); } //guiHelper.createCollisionShapeGraphicsObject(shape); MultiBodyLinkCollider col = new MultiBodyLinkCollider(pMultiBody, i); col.CollisionShape = (shape); Matrix tr = new Matrix(); tr.ScaleVector = new BulletSharp.Math.Vector3(); tr.Origin = (posr); tr.Rotation = (new BulletSharp.Math.Quaternion(quat[0], quat[1], quat[2], quat[3])); col.WorldTransform = (tr); // col.setFriction(friction); bool isDynamic = true;//(linkMass > 0); CollisionFilterGroups collisionFilterGroup = isDynamic ? CollisionFilterGroups.DefaultFilter : CollisionFilterGroups.StaticFilter; CollisionFilterGroups collisionFilterMask = isDynamic ? CollisionFilterGroups.AllFilter : CollisionFilterGroups.AllFilter ^ CollisionFilterGroups.StaticFilter; //if (i==0||i>numLinks-2) { world.AddCollisionObject(col, collisionFilterGroup, collisionFilterMask);//,2,1+2); BulletSharp.Math.Vector4 color = colors[curColor]; curColor++; curColor &= 3; //guiHelper.createCollisionObjectGraphicsObject(col, color); pMultiBody.GetLink(i).Collider = col; } } return(pMultiBody); }
private void CreateWorld() { if (physicWorldParameters.worldType == WorldType.SoftBodyAndRigidBody && physicWorldParameters.collisionType != CollisionConfType.SoftBodyRigidBodyCollisionConf) { Debug.LogError("For World Type = SoftBodyAndRigidBody collisionType must be collisionType = SoftBodyRigidBodyCollisionConf."); return; } isDisposed = false; switch (physicWorldParameters.collisionType) { case CollisionConfType.DefaultDynamicsWorldCollisionConf: collisionConf = new DefaultCollisionConfiguration(); break; case CollisionConfType.SoftBodyRigidBodyCollisionConf: collisionConf = new SoftBodyRigidBodyCollisionConfiguration(); break; } dispatcher = new CollisionDispatcher(collisionConf); switch (physicWorldParameters.broadphaseType) { default: case BroadphaseType.DynamicAABBBroadphase: broadphase = new DbvtBroadphase(); break; case BroadphaseType.Axis3SweepBroadphase: broadphase = new AxisSweep3(physicWorldParameters.axis3SweepBroadphaseMin, physicWorldParameters.axis3SweepBroadphaseMax, physicWorldParameters.axis3SweepMaxProxies); break; case BroadphaseType.Axis3SweepBroadphase_32bit: broadphase = new AxisSweep3_32Bit(physicWorldParameters.axis3SweepBroadphaseMin, physicWorldParameters.axis3SweepBroadphaseMax, physicWorldParameters.axis3SweepMaxProxies); break; } switch (physicWorldParameters.worldType) { case WorldType.CollisionOnly: World = new CollisionWorld(dispatcher, broadphase, collisionConf); break; case WorldType.RigidBodyDynamics: World = new DiscreteDynamicsWorld(dispatcher, broadphase, null, collisionConf); break; case WorldType.MultiBodyWorld: MultiBodyConstraintSolver mbConstraintSolver = new MultiBodyConstraintSolver(); constraintSolver = mbConstraintSolver; World = new MultiBodyDynamicsWorld(dispatcher, broadphase, mbConstraintSolver, collisionConf); break; case WorldType.SoftBodyAndRigidBody: SequentialImpulseConstraintSolver siConstraintSolver = new SequentialImpulseConstraintSolver(); constraintSolver = siConstraintSolver; siConstraintSolver.RandSeed = physicWorldParameters.sequentialImpulseConstraintSolverRandomSeed; World = new SoftRigidDynamicsWorld(dispatcher, broadphase, siConstraintSolver, collisionConf); _world.DispatchInfo.EnableSpu = true; SoftWorld.WorldInfo.SparseSdf.Initialize(); SoftWorld.WorldInfo.SparseSdf.Reset(); SoftWorld.WorldInfo.AirDensity = 1.2f; SoftWorld.WorldInfo.WaterDensity = 0; SoftWorld.WorldInfo.WaterOffset = 0; SoftWorld.WorldInfo.WaterNormal = BulletSharp.Math.Vector3.Zero; SoftWorld.WorldInfo.Gravity = physicWorldParameters.gravity; break; } if (_world is DiscreteDynamicsWorld) { ((DiscreteDynamicsWorld)_world).Gravity = physicWorldParameters.gravity; } Debug.LogFormat("Physic World of type {0} Instanced", physicWorldParameters.worldType); if (physicWorldParameters.debug) { Debug.Log("Physic World Debug is active"); DebugDrawUnity db = new DebugDrawUnity(); db.DebugMode = physicWorldParameters.debugDrawMode; _world.DebugDrawer = db; } }
//Does not set any local variables. Is safe to use to create duplicate physics worlds for independant simulation. public void CreatePhysicsWorld ( out CollisionWorld world, out CollisionConfiguration collisionConfig, out CollisionDispatcher dispatcher, out BroadphaseInterface broadphase, out ConstraintSolver solver, out SoftBodyWorldInfo softBodyWorldInfo ) { collisionConfig = null; switch (physicWorldParameters.collisionType) { default: case CollisionConfType.DefaultDynamicsWorldCollisionConf: collisionConfig = new DefaultCollisionConfiguration(); break; case CollisionConfType.SoftBodyRigidBodyCollisionConf: collisionConfig = new SoftBodyRigidBodyCollisionConfiguration(); break; } dispatcher = new CollisionDispatcher(collisionConfig); switch (physicWorldParameters.broadphaseType) { default: case BroadphaseType.DynamicAABBBroadphase: broadphase = new DbvtBroadphase(); break; case BroadphaseType.Axis3SweepBroadphase: broadphase = new AxisSweep3( physicWorldParameters.axis3SweepBroadphaseMin, physicWorldParameters.axis3SweepBroadphaseMax, physicWorldParameters.axis3SweepMaxProxies ); break; case BroadphaseType.Axis3SweepBroadphase_32bit: broadphase = new AxisSweep3_32Bit( physicWorldParameters.axis3SweepBroadphaseMin, physicWorldParameters.axis3SweepBroadphaseMax, physicWorldParameters.axis3SweepMaxProxies ); break; } world = null; softBodyWorldInfo = null; solver = null; switch (physicWorldParameters.worldType) { case WorldType.CollisionOnly: world = new CollisionWorld(dispatcher, broadphase, collisionConfig); break; default: case WorldType.RigidBodyDynamics: world = new DiscreteDynamicsWorld(dispatcher, broadphase, null, collisionConfig); break; case WorldType.MultiBodyWorld: MultiBodyConstraintSolver mbConstraintSolver = new MultiBodyConstraintSolver(); constraintSolver = mbConstraintSolver; world = new MultiBodyDynamicsWorld(dispatcher, broadphase, mbConstraintSolver, collisionConfig); break; case WorldType.SoftBodyAndRigidBody: SequentialImpulseConstraintSolver siConstraintSolver = new SequentialImpulseConstraintSolver(); constraintSolver = siConstraintSolver; siConstraintSolver.RandSeed = physicWorldParameters.sequentialImpulseConstraintSolverRandomSeed; world = new SoftRigidDynamicsWorld(this.dispatcher, this.broadphase, siConstraintSolver, collisionConf); world.DispatchInfo.EnableSpu = true; SoftRigidDynamicsWorld _sworld = (SoftRigidDynamicsWorld)world; _sworld.WorldInfo.SparseSdf.Initialize(); _sworld.WorldInfo.SparseSdf.Reset(); _sworld.WorldInfo.AirDensity = 1.2f; _sworld.WorldInfo.WaterDensity = 0; _sworld.WorldInfo.WaterOffset = 0; _sworld.WorldInfo.WaterNormal = BulletSharp.Math.Vector3.Zero; _sworld.WorldInfo.Gravity = Gravity.ToBullet(); break; } if (world is DiscreteDynamicsWorld) { ((DiscreteDynamicsWorld)world).Gravity = Gravity.ToBullet(); } }
protected void Dispose(bool disposing) { if (debugType >= BDebug.DebugType.Debug) { Debug.Log("BDynamicsWorld Disposing physics."); } if (PhysicsWorldHelper != null) { PhysicsWorldHelper.m_ddWorld = null; PhysicsWorldHelper.m_world = null; } if (m_world != null) { //remove/dispose constraints int i; if (_ddWorld != null) { if (debugType >= BDebug.DebugType.Debug) { Debug.LogFormat("Removing Constraints {0}", _ddWorld.NumConstraints); } for (i = _ddWorld.NumConstraints - 1; i >= 0; i--) { TypedConstraint constraint = _ddWorld.GetConstraint(i); _ddWorld.RemoveConstraint(constraint); if (constraint.Userobject is BTypedConstraint) { ((BTypedConstraint)constraint.Userobject).m_isInWorld = false; } if (debugType >= BDebug.DebugType.Debug) { Debug.LogFormat("Removed Constaint {0}", constraint.Userobject); } constraint.Dispose(); } } if (debugType >= BDebug.DebugType.Debug) { Debug.LogFormat("Removing Collision Objects {0}", m_world.NumCollisionObjects); } //remove the rigidbodies from the dynamics world and delete them for (i = m_world.NumCollisionObjects - 1; i >= 0; i--) { CollisionObject obj = m_world.CollisionObjectArray[i]; RigidBody body = obj as RigidBody; if (body != null && body.MotionState != null) { Debug.Assert(body.NumConstraintRefs == 0, "Rigid body still had constraints"); body.MotionState.Dispose(); } m_world.RemoveCollisionObject(obj); if (obj.UserObject is BCollisionObject) { ((BCollisionObject)obj.UserObject).isInWorld = false; } if (debugType >= BDebug.DebugType.Debug) { Debug.LogFormat("Removed CollisionObject {0}", obj.UserObject); } obj.Dispose(); } MultiBodyDynamicsWorld _mbdWorld = m_world as MultiBodyDynamicsWorld; if (_mbdWorld != null) { if (debugType >= BDebug.DebugType.Debug) { Debug.LogFormat("Removing MultiBody Constraints {0}", _mbdWorld.NumMultiBodyConstraints); } for (i = _mbdWorld.NumMultiBodyConstraints - 1; i >= 0; i--) { MultiBodyConstraint constraint = _mbdWorld.GetMultiBodyConstraint(i); _mbdWorld.RemoveMultiBodyConstraint(constraint); /* if (constraint.Userobject is BTypedConstraint) ((BTypedConstraint)constraint.Userobject).m_isInWorld = false; * if (debugType >= BDebug.DebugType.Debug) Debug.LogFormat("Removed Constaint {0}", constraint.Userobject);*/ constraint.Dispose(); } if (debugType >= BDebug.DebugType.Debug) { Debug.LogFormat("Removing Multibodies {0}", _mbdWorld.NumMultibodies); } //remove the rigidbodies from the dynamics world and delete them for (i = _mbdWorld.NumMultibodies - 1; i >= 0; i--) { MultiBody mb = _mbdWorld.GetMultiBody(i); _mbdWorld.RemoveMultiBody(mb); if (mb.UserObject is BMultiBody bmb) { bmb.isInWorld = false; } if (debugType >= BDebug.DebugType.Debug) { Debug.LogFormat("Removed CollisionObject {0}", mb.UserObject); } mb.Dispose(); } } if (m_world.DebugDrawer != null) { if (m_world.DebugDrawer is IDisposable) { IDisposable dis = (IDisposable)m_world.DebugDrawer; dis.Dispose(); } } m_world.Dispose(); Broadphase.Dispose(); Dispatcher.Dispose(); CollisionConf.Dispose(); _ddWorld = null; m_world = null; } if (Broadphase != null) { Broadphase.Dispose(); Broadphase = null; } if (Dispatcher != null) { Dispatcher.Dispose(); Dispatcher = null; } if (CollisionConf != null) { CollisionConf.Dispose(); CollisionConf = null; } if (constraintSolver != null) { constraintSolver.Dispose(); constraintSolver = null; } if (softBodyWorldInfo != null) { softBodyWorldInfo.Dispose(); softBodyWorldInfo = null; } _isDisposed = true; singleton = null; }
public PendulumDemoSimulation() { CollisionConfiguration = new DefaultCollisionConfiguration(); Dispatcher = new CollisionDispatcher(CollisionConfiguration); Broadphase = new DbvtBroadphase(); _solver = new MultiBodyConstraintSolver(); MultiBodyWorld = new MultiBodyDynamicsWorld(Dispatcher, Broadphase, _solver, CollisionConfiguration); World.SetInternalTickCallback(TickCallback, null, true); const bool floating = false; const bool gyro = false; const int numLinks = 1; const bool canSleep = false; const bool selfCollide = false; var linkHalfExtents = new Vector3(0.05f, 0.5f, 0.1f); var baseHalfExtents = new Vector3(0.05f, 0.5f, 0.1f); var 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 var 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(); MultiBodyWorld.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); 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; } }