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); }
/** * Creates or configures a RigidBody based on the current settings. Does not alter the internal state of this component in any way. * Can be used to create copies of this BRigidBody for use in other physics simulations. */ public virtual bool CreateMultiBody(ref MultiBody mb, ref BulletSharp.Math.Vector3 localInertia, CollisionShape cs) { localInertia = BulletSharp.Math.Vector3.Zero; if (_mass > 0) { cs.CalculateLocalInertia(_mass, out localInertia); } if (mb == null) { int nbLinks = Links.Count; foreach (BMultiBodyLink link in Links) { nbLinks += link.NbLinks; } Debug.Log("Adding multibody with " + nbLinks + " links"); mb = new MultiBody(nbLinks, _mass, localInertia, false, false); mb.BaseWorldTransform = transform.localToWorldMatrix.ToBullet(); mb.HasSelfCollision = SelfCollision; var collider = new MultiBodyLinkCollider(mb, -1); collider.CollisionShape = cs; collider.WorldTransform = transform.localToWorldMatrix.ToBullet(); collider.CollisionFlags = collisionFlags; collider.UserObject = UserObject ?? this; BPhysicsWorld.Get().world.AddCollisionObject(collider, groupsIBelongTo, collisionMask); mb.BaseCollider = collider; m_collisionObject = collider; BulletMultiBodyLinkColliderProxy baseProxy = gameObject.GetComponent <BulletMultiBodyLinkColliderProxy>(); if (baseProxy == null) { baseProxy = gameObject.AddComponent <BulletMultiBodyLinkColliderProxy>(); } baseProxy.target = collider; try { int currentLinkIndex = 0; for (int i = 0; i < Links.Count; ++i) { currentLinkIndex += Links[i].AddLinkToMultiBody(this, currentLinkIndex, -1, transform); } mb.FinalizeMultiDof(); } catch (Exception e) // if an error occurs, don't add the object, otherwise unity will crash { Debug.LogErrorFormat("Error occured while setting MultiBody : {0}\n{1}", e.Message, e.StackTrace); BPhysicsWorld.Get().world.RemoveCollisionObject(collider); return(false); } mb.CanSleep = false; mb.UserObject = UserObject ?? this; } return(true); }
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; } }
void CreateOrConfigureMultiBody(ref MultiBody mb, float baseMass, BCollisionShape[] shapes, BMultiBodyConstraint[] constraints) { BulletSharp.Math.Vector3 inertia = BulletSharp.Math.Vector3.Zero; if (baseMass != 0) { CollisionShape cs = m_baseCollisionShape.GetCollisionShape(); cs.CalculateLocalInertia(baseMass, out inertia); } mb = new MultiBody(m_links.Count, baseMass, inertia, fixedBase, canSleep); mb.BasePosition = transform.position.ToBullet(); UnityEngine.Quaternion r = UnityEngine.Quaternion.Inverse(transform.rotation); mb.WorldToBaseRot = r.ToBullet(); for (int i = 0; i < m_links.Count; i++) { //Debug.Log("Found link " + i + " parent " + m_links[i].parentIndex + " index " + m_links[i].index); BMultiBodyLink link = m_links[i]; CollisionShape cs = shapes[i].GetCollisionShape(); if (cs != null) { cs.CalculateLocalInertia(link.mass, out inertia); } else { inertia = BulletSharp.Math.Vector3.Zero; } FeatherstoneJointType jt = link.jointType; int parentIdx = link.parentIndex; // Vector from parent pivot (COM) to the joint pivot point in parent's frame UnityEngine.Vector3 parentCOM2ThisPivotOffset; link.FreezeJointAxis(); if (link.parentIndex >= 0) { parentCOM2ThisPivotOffset = link.parentCOM2JointPivotOffset; } else { parentCOM2ThisPivotOffset = transform.InverseTransformPoint(link.transform.TransformPoint(link.localPivotPosition)); } // Vector from the joint pivot point to link's pivot point (COM) in link's frame. UnityEngine.Vector3 thisPivotToThisCOMOffset = link.thisPivotToJointCOMOffset; // Should rotate vectors in parent frame to vectors in local frame UnityEngine.Quaternion parentToThisRotation = link.parentToJointRotation; switch (jt) { case FeatherstoneJointType.Fixed: mb.SetupFixed(i, link.mass, inertia, link.parentIndex, parentToThisRotation.ToBullet(), parentCOM2ThisPivotOffset.ToBullet(), thisPivotToThisCOMOffset.ToBullet(), false); break; case FeatherstoneJointType.Planar: mb.SetupPlanar(i, link.mass, inertia, link.parentIndex, parentToThisRotation.ToBullet(), link.rotationAxis.ToBullet(), thisPivotToThisCOMOffset.ToBullet(), false); break; case FeatherstoneJointType.Prismatic: mb.SetupPrismatic(i, link.mass, inertia, link.parentIndex, parentToThisRotation.ToBullet(), link.rotationAxis.ToBullet(), parentCOM2ThisPivotOffset.ToBullet(), thisPivotToThisCOMOffset.ToBullet(), false); break; case FeatherstoneJointType.Revolute: mb.SetupRevolute(i, link.mass, inertia, link.parentIndex, parentToThisRotation.ToBullet(), link.rotationAxis.ToBullet(), parentCOM2ThisPivotOffset.ToBullet(), thisPivotToThisCOMOffset.ToBullet(), false); break; case FeatherstoneJointType.Spherical: mb.SetupSpherical(i, link.mass, inertia, link.parentIndex, parentToThisRotation.ToBullet(), parentCOM2ThisPivotOffset.ToBullet(), thisPivotToThisCOMOffset.ToBullet(), false); break; default: Debug.LogError("Invalid joint type for link " + link.name); break; } } 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; } mb.FinalizeMultiDof(); m_multibody = mb; }
//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); }
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; } }