C# BoxShape.CalculateLocalInertia方法代码示例

        public Physics()
        {
            // collision configuration contains default setup for memory, collision setup
            collisionConf = new DefaultCollisionConfiguration();
            Dispatcher = new CollisionDispatcher(collisionConf);

            Broadphase = new DbvtBroadphase();

            World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, collisionConf);
            World.Gravity = new Vector3(0, -10, 0);

            CollisionShapes = new List<CollisionShape>();

            // create the ground
            CollisionShape groundShape = new BoxShape(50, 1, 50);
            CollisionShapes.Add(groundShape);
            CollisionObject ground = LocalCreateRigidBody(0, Matrix.Identity, groundShape);
            ground.UserObject = "Ground";

            // create a few dynamic rigidbodies
            float mass = 1.0f;

            CollisionShape colShape = new BoxShape(1);
            CollisionShapes.Add(colShape);
            Vector3 localInertia = colShape.CalculateLocalInertia(mass);

            float start_x = StartPosX - ArraySizeX / 2;
            float start_y = StartPosY;
            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.CreateTranslation(
                            new Vector3(
                                2*i + start_x,
                                2*k + start_y,
                                2*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);

                        // make it drop from a height
                        body.Translate(new Vector3(0, 20, 0));

                        World.AddRigidBody(body);
                    }
                }
            }
        }

 static void TestAlignment()
 {
     const float mass = 1.0f;
     Vector3WriteTest vTest = new Vector3WriteTest();
     vTest.Value1 = 2.0f;
     vTest.Value2 = 3.0f;
     using (BoxShape shape = new BoxShape(1))
     {
         shape.CalculateLocalInertia(mass, out vTest.Vector);
     }
     if (vTest.Value1 != 2.0f || vTest.Value2 != 3.0f)
     {
         Console.WriteLine("Vector3 value was overwritten with padding!");
     }
 }

        public bool Build(
            FeatureUpdateContext updateContext,
            [ParentModel] Point[] Points,
            double Size)
        {
            //-- Upkeep
              //--
              if( physics == null )
              {
            physics = new Sutd.Physics( );
              }
              else
              {
            Regenerate( );
            physics.Reset( );
              }

              //-- Set Gravity
              //--
              physics.world.Gravity = new Vec3D( 0, 0, -10 );

              //-- Create Ground
              //--
              var body = new Sutd.Physics.Body( );
              {
            //-- Define Infinite Plane
            //--
            var shape = new StaticPlaneShape( new Vec3D( 0, 0, 1 ), 0 );
            physics.shapes.Add( shape );

            //-- Set Physics State / Bullet
            //-- Fixed bodies have zero mass and inertia
            //--
            var param = new RigidBodyConstructionInfo(
              mass: 0.0f, motionState: new DefaultMotionState( Mat4D.Identity ),
              collisionShape: shape, localInertia: Vec3D.Zero );
            body.rigid = new RigidBody( param );
            param.Dispose( );

            physics.world.AddRigidBody( body.rigid );
            body.matrix = body.rigid.WorldTransform;

            //-- Set Visual State / Rhino
            //-- Create a very thin but wide finite box
            //--
            var transform = DTransform3d.Identity;
            AddBox( transform, new DVector3d( 50, 50, 0.01 ) );
            body.solid = geometry[geometry.Count - 1];
              }
              physics.bodies.Add( body );

              //-- Create 3D Grid of Boxes
              //--
              float half = (float)( Size * 0.5 );
              foreach( var point in Points )
              {
            body = new Sutd.Physics.Body( );
            {
              //-- Collision Shape
              //--
              var shape = new BoxShape( half, half, half );
              physics.shapes.Add( shape );

              //-- Mass Properties
              //--
              var inertia = Vec3D.Zero;
              shape.CalculateLocalInertia( mass: 1.0f, inertia: out inertia );

              //-- Physics State
              //--
              var param = new RigidBodyConstructionInfo(
            mass: 1.0f, motionState: new DefaultMotionState( Mat4D.Identity ),
            collisionShape: shape, localInertia: inertia );
              body.rigid = new RigidBody( param );
              param.Dispose( );

              physics.world.AddRigidBody( body.rigid );

              body.rigid.Translate( new Vec3D( (float)point.X, (float)point.Y, (float)point.Z ) );
              body.matrix = body.rigid.WorldTransform;

              //-- Visual State
              //--
              var transform = DTransform3d.Identity;
              transform.Translation = point.DPoint3d;
              AddBox( transform, new DVector3d( Size, Size, Size ) );
              body.solid = geometry[geometry.Count - 1];
            }
            physics.bodies.Add( body );
              }

              return true;
        }

        void CreateTowerCircle(Vector3 offsetPosition, int stackSize, int rotSize, Vector3 boxSize)
        {
            BoxShape blockShape = new BoxShape(boxSize[0] - collisionRadius, boxSize[1] - collisionRadius, boxSize[2] - collisionRadius);

            float mass = 1.0f;
            Vector3 localInertia;
            blockShape.CalculateLocalInertia(mass, out localInertia);

            float radius = 1.3f * rotSize * boxSize[0] / (float)Math.PI;

            // create active boxes
            float posY = boxSize[1];
            float rot = 0;

            for (int i = 0; i < stackSize; i++)
            {
                for (int j = 0; j < rotSize; j++)
                {
                    Matrix trans = Matrix.Translation(0, 0, radius);
                    trans *= Matrix.RotationY(rot);
                    trans *= Matrix.Translation(offsetPosition + new Vector3(0, posY, 0));
                    LocalCreateRigidBody(mass, trans, blockShape);

                    rot += (2.0f * (float)Math.PI) / rotSize;
                }

                posY += boxSize[1] * 2.0f;
                rot += (float)Math.PI / (float)rotSize;
            }
        }

        /*
        void MyContactCallback(object sender, ContactAddedEventArgs e)
        {
            if (e.CollisionObject0Wrapper.CollisionObject.CollisionShape.ShapeType == BroadphaseNativeType.CompoundShape)
            {
                CompoundShape compound = e.CollisionObject0Wrapper.CollisionObject.CollisionShape as CompoundShape;
                CollisionShape childShape = compound.GetChildShape(e.Index0);
            }

            if (e.CollisionObject1Wrapper.CollisionObject.CollisionShape.ShapeType == BroadphaseNativeType.CompoundShape)
            {
                CompoundShape compound = e.CollisionObject1Wrapper.CollisionObject.CollisionShape as CompoundShape;
                CollisionShape childShape = compound.GetChildShape(e.Index1);
            }

            e.IsContactModified = true;
        }
        */
        public void SetupEmptyDynamicsWorld()
        {
            // collision configuration contains default setup for memory, collision setup
            CollisionConf = new DefaultCollisionConfiguration();
            Dispatcher = new CollisionDispatcher(CollisionConf);

            CompoundCollisionAlgorithm.CompoundChildShapePairCallback = MyCompoundChildShapeCallback;

            convexDecompositionObjectOffset = new Vector3(10, 0, 0);

            Broadphase = new AxisSweep3(new Vector3(-10000, -10000, -10000), new Vector3(10000, 10000, 10000));
            //Broadphase = new SimpleBroadphase();

            Solver = new SequentialImpulseConstraintSolver();
            World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, Solver, CollisionConf);

            // create the ground
            CollisionShape groundShape = new BoxShape(30, 2, 30);
            CollisionShapes.Add(groundShape);
            CollisionObject ground = LocalCreateRigidBody(0, Matrix.Translation(0, -4.5f, 0), groundShape);
            ground.UserObject = "Ground";

            // create a few dynamic rigidbodies
            float mass = 1.0f;

            CollisionShape colShape = new BoxShape(1);
            CollisionShapes.Add(colShape);
            Vector3 localInertia = colShape.CalculateLocalInertia(mass);
        }

        protected override void OnInitializePhysics()
        {
            // collision configuration contains default setup for memory, collision setup
            CollisionConf = new DefaultCollisionConfiguration();
            Dispatcher = new CollisionDispatcher(CollisionConf);

            Broadphase = new DbvtBroadphase();

            World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, CollisionConf);
            World.Gravity = new Vector3(0, -10, 0);

            // create the ground
            BoxShape groundShape = new BoxShape(50, 1, 50);
            //groundShape.InitializePolyhedralFeatures();
            //CollisionShape groundShape = new StaticPlaneShape(new Vector3(0,1,0), 50);

            CollisionShapes.Add(groundShape);
            CollisionObject ground = LocalCreateRigidBody(0, Matrix.Identity, 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 startX = StartPosX - ArraySizeX / 2;
            const float startY = StartPosY;
            const float startZ = StartPosZ - ArraySizeZ / 2;

            RigidBodyConstructionInfo rbInfo =
                new RigidBodyConstructionInfo(mass, null, colShape, localInertia);

            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(
                            2 * i + startX,
                            2 * k + startY,
                            2 * j + startZ
                        );

                        // using motionstate is recommended, it provides interpolation capabilities
                        // and only synchronizes 'active' objects
                        rbInfo.MotionState = new DefaultMotionState(startTransform);
                        RigidBody body = new RigidBody(rbInfo);

                        // make it drop from a height
                        body.Translate(new Vector3(0, 20, 0));

                        World.AddRigidBody(body);
                    }
                }
            }
            rbInfo.Dispose();
        }

        protected override void OnInitializePhysics()
        {
            // collision configuration contains default setup for memory, collision setup
            CollisionConf = new DefaultCollisionConfiguration();
            Dispatcher = new CollisionDispatcher(CollisionConf);

            Broadphase = new DbvtBroadphase();

            World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, CollisionConf);
            World.Gravity = new Vector3(0, -10, 0);

            // create the ground
            CollisionShape groundShape = new BoxShape(20, 50, 10);
            CollisionShapes.Add(groundShape);
            CollisionObject ground = LocalCreateRigidBody(0,
                Matrix.RotationAxis(new Vector3(0, 0, 1), (float)Math.PI * 0.03f) * Matrix.Translation(0, -50, 0),
                groundShape);
            ground.Friction = 1;
            ground.RollingFriction = 1;
            ground.UserObject = "Ground";

            groundShape = new BoxShape(100, 50, 100);
            CollisionShapes.Add(groundShape);
            ground = LocalCreateRigidBody(0, Matrix.Translation(0, -54, 0), groundShape);
            ground.Friction = 1;
            ground.RollingFriction = 1;
            ground.UserObject = "Ground";

            // create a few dynamic rigidbodies
            CollisionShape[] colShapes = {
                new SphereShape(1),
                new CapsuleShape(0.5f,1),
                new CapsuleShapeX(0.5f,1),
                new CapsuleShapeZ(0.5f,1),
                new ConeShape(0.5f,1),
                new ConeShapeX(0.5f,1),
                new ConeShapeZ(0.5f,1),
                new CylinderShape(new Vector3(0.5f,1,0.5f)),
                new CylinderShapeX(new Vector3(1,0.5f,0.5f)),
                new CylinderShapeZ(new Vector3(0.5f,0.5f,1)),
            };
            foreach (var collisionShape in colShapes)
            {
                CollisionShapes.Add(collisionShape);
            }

            const float mass = 1.0f;

            CollisionShape colShape = new BoxShape(1);
            CollisionShapes.Add(colShape);
            Vector3 localInertia = colShape.CalculateLocalInertia(mass);

            var rbInfo = new RigidBodyConstructionInfo(mass, null, null, localInertia);

            const float startX = StartPosX - ArraySizeX / 2;
            const float startY = StartPosY;
            const float startZ = StartPosZ - ArraySizeZ / 2;

            int shapeIndex = 0;
            for (int k = 0; k < ArraySizeY; k++)
            {
                for (int i = 0; i < ArraySizeX; i++)
                {
                    for (int j = 0; j < ArraySizeZ; j++)
                    {
                        Matrix startTransform = Matrix.Translation(
                            2 * i + startX,
                            2 * k + startY + 20,
                            2 * j + startZ
                        );
                        shapeIndex++;

                        // using motionstate is recommended, it provides interpolation capabilities
                        // and only synchronizes 'active' objects
                        rbInfo.MotionState = new DefaultMotionState(startTransform);
                        rbInfo.CollisionShape = colShapes[shapeIndex % colShapes.Length];

                        RigidBody body = new RigidBody(rbInfo);
                        body.Friction = 1;
                        body.RollingFriction = 0.3f;
                        body.SetAnisotropicFriction(colShape.AnisotropicRollingFrictionDirection, AnisotropicFrictionFlags.RollingFriction);

                        World.AddRigidBody(body);
                    }
                }
            }

            rbInfo.Dispose();
        }

        void CreateWall(Vector3 offsetPosition, int stackSize, Vector3 boxSize)
        {
            BoxShape blockShape = new BoxShape(boxSize[0] - collisionRadius, boxSize[1] - collisionRadius, boxSize[2] - collisionRadius);

            float mass = 1.0f;
            Vector3 localInertia;
            blockShape.CalculateLocalInertia(mass, out localInertia);

            //	float diffX = boxSize[0] * 1.0f;
            float diffY = boxSize[1] * 1.0f;
            float diffZ = boxSize[2] * 1.0f;

            float offset = -stackSize * (diffZ * 2.0f) * 0.5f;
            Vector3 pos = new Vector3(0.0f, diffY, 0.0f);

            while (stackSize > 0)
            {
                for (int i = 0; i < stackSize; i++)
                {
                    pos[2] = offset + (float)i * (diffZ * 2.0f);
                    LocalCreateRigidBody(mass, Matrix.Translation(offsetPosition + pos), blockShape);
                }
                offset += diffZ;
                pos[1] += (diffY * 2.0f);
                stackSize--;
            }
        }

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

        void CreatePyramid(Vector3 offsetPosition, int stackSize, Vector3 boxSize)
        {
            float space = 0.0001f;
            Vector3 pos = new Vector3(0.0f, boxSize[1], 0.0f);

            BoxShape blockShape = new BoxShape(boxSize[0] - collisionRadius, boxSize[1] - collisionRadius, boxSize[2] - collisionRadius);

            float mass = 1.0f;
            Vector3 localInertia;
            blockShape.CalculateLocalInertia(mass, out localInertia);

            float diffX = boxSize[0] * 1.02f;
            float diffY = boxSize[1] * 1.02f;
            float diffZ = boxSize[2] * 1.02f;

            float offsetX = -stackSize * (diffX * 2.0f + space) * 0.5f;
            float offsetZ = -stackSize * (diffZ * 2.0f + space) * 0.5f;
            while (stackSize > 0)
            {
                for (int j = 0; j < stackSize; j++)
                {
                    pos[2] = offsetZ + (float)j * (diffZ * 2.0f + space);
                    for (int i = 0; i < stackSize; i++)
                    {
                        pos[0] = offsetX + (float)i * (diffX * 2.0f + space);
                        RigidBody body = LocalCreateRigidBody(mass, Matrix.Translation(offsetPosition + pos), blockShape);
                    }
                }
                offsetX += diffX;
                offsetZ += diffZ;
                pos[1] += (diffY * 2.0f + space);
                stackSize--;
            }
        }

        public Physics()
        {
            // collision configuration contains default setup for memory, collision setup
            DefaultCollisionConstructionInfo cci = new DefaultCollisionConstructionInfo();
            cci.DefaultMaxPersistentManifoldPoolSize = 32768;
            CollisionConf = new DefaultCollisionConfiguration(cci);

            if (UseParallelDispatcherBenchmark)
            {
                int maxNumOutstandingTasks = 4;

                Win32ThreadConstructionInfo info = new Win32ThreadConstructionInfo("collision",
                    Win32ThreadFunc.ProcessCollisionTask, Win32LSMemorySetupFunc.CreateCollisionLocalStoreMemory,
                    maxNumOutstandingTasks);

                Win32ThreadSupport threadSupportCollision = new Win32ThreadSupport(info);
                Dispatcher = new SpuGatheringCollisionDispatcher(threadSupportCollision, 1, CollisionConf);
            }
            else
            {
                Dispatcher = new CollisionDispatcher(CollisionConf);
                Dispatcher.DispatcherFlags = DispatcherFlags.DisableContactPoolDynamicAllocation;
            }

            // the maximum size of the collision world. Make sure objects stay within these boundaries
            // Don't make the world AABB size too large, it will harm simulation quality and performance
            Vector3 worldAabbMin = new Vector3(-1000, -1000, -1000);
            Vector3 worldAabbMax = new Vector3(1000, 1000, 1000);

            HashedOverlappingPairCache pairCache = new HashedOverlappingPairCache();
            Broadphase = new AxisSweep3(worldAabbMin, worldAabbMax, 3500, pairCache);
            //Broadphase = new DbvtBroadphase();

            if (UseParallelDispatcherBenchmark)
            {
                ThreadSupportInterface thread = CreateSolverThreadSupport(4);
                Solver = new ParallelConstraintSolver(thread);
            }
            else
            {
                Solver = new SequentialImpulseConstraintSolver();
            }

            World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, Solver, CollisionConf);
            World.Gravity = new Vector3(0, -10, 0);

            if (UseParallelDispatcherBenchmark)
            {
                ((DiscreteDynamicsWorld)World).SimulationIslandManager.SplitIslands = false;
            }
            World.SolverInfo.SolverMode |= SolverModes.EnableFrictionDirectionCaching;
            World.SolverInfo.NumIterations = 5;

            if (benchmark < 5)
            {
                // create the ground
                CollisionShape groundShape = new BoxShape(250, 50, 250);
                CollisionShapes.Add(groundShape);
                CollisionObject ground = base.LocalCreateRigidBody(0, Matrix.Translation(0, -50, 0), groundShape);
                ground.UserObject = "Ground";
            }

            float cubeSize = 1.0f;
            float spacing = cubeSize;
            float mass = 1.0f;
            int size = 8;
            Vector3 localInertia;
            Vector3 pos = new Vector3(0.0f, cubeSize * 2, 0.0f);
            float offset = -size * (cubeSize * 2.0f + spacing) * 0.5f;

            switch (benchmark)
            {
                case 1:
                    // 3000

                    BoxShape blockShape = new BoxShape(cubeSize - collisionRadius);
                    mass = 2.0f;
                    blockShape.CalculateLocalInertia(mass, out localInertia);

                    for (int k = 0; k < 47; k++)
                    {
                        for (int j = 0; j < size; j++)
                        {
                            pos[2] = offset + (float)j * (cubeSize * 2.0f + spacing);
                            for (int i = 0; i < size; i++)
                            {
                                pos[0] = offset + (float)i * (cubeSize * 2.0f + spacing);
                                RigidBody cmbody = LocalCreateRigidBody(mass, Matrix.Translation(pos), blockShape);
                            }
                        }
                        offset -= 0.05f * spacing * (size - 1);
                        // spacing *= 1.01f;
                        pos[1] += (cubeSize * 2.0f + spacing);
                    }
                    break;

                case 2:
                    CreatePyramid(new Vector3(-20, 0, 0), 12, new Vector3(cubeSize));
                    CreateWall(new Vector3(-2.0f, 0.0f, 0.0f), 12, new Vector3(cubeSize));
                    CreateWall(new Vector3(4.0f, 0.0f, 0.0f), 12, new Vector3(cubeSize));
//.........这里部分代码省略.........

        public Physics(SceneManager sceneMgr)
        {
            // collision configuration contains default setup for memory, collision setup
            collisionConf = new DefaultCollisionConfiguration();
            Dispatcher = new CollisionDispatcher(collisionConf);

            Broadphase = new DbvtBroadphase();

            World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, collisionConf);
            World.Gravity = new Vector3(0, -10, 0);

            // create the ground
            CollisionShape groundShape = new BoxShape(50, 1, 50);
            CollisionShapes.Add(groundShape);
            CollisionObject ground = LocalCreateRigidBody(0, Matrix4.IDENTITY, groundShape);
            ground.UserObject = "Ground";

            // create a few dynamic rigidbodies
            float mass = 1.0f;

            CollisionShape colShape = new BoxShape(1);
            CollisionShapes.Add(colShape);
            Vector3 localInertia = colShape.CalculateLocalInertia(mass);

            var rbInfo = new RigidBodyConstructionInfo(mass, null, colShape, localInertia);

            float start_x = StartPosX - ArraySizeX / 2;
            float start_y = StartPosY;
            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++)
                    {
                        Matrix4 startTransform = new Matrix4();
                        startTransform.MakeTrans(
                            new Vector3(
                                2*i + start_x,
                                2*k + start_y,
                                2*j + start_z
                                )
                            );

                        // using motionstate is recommended, it provides interpolation capabilities
                        // and only synchronizes 'active' objects
                        int index = (k * ArraySizeX + i) * ArraySizeZ + j;
                        Entity box = sceneMgr.CreateEntity("Box" + index.ToString(), "box.mesh");
                        box.SetMaterialName("BoxMaterial/Active");
                        SceneNode boxNode = sceneMgr.RootSceneNode.CreateChildSceneNode("BoxNode" + index.ToString());
                        boxNode.AttachObject(box);
                        boxNode.Scale(new Vector3(2, 2, 2));
                        var mogreMotionState = new MogreMotionState(box, boxNode, startTransform);
                        rbInfo.MotionState = mogreMotionState;

                        RigidBody body = new RigidBody(rbInfo);
                        mogreMotionState.Body = body;

                        // make it drop from a height
                        body.Translate(new Vector3(0, 20, 0));

                        World.AddRigidBody(body);
                    }
                }
            }

            rbInfo.Dispose();
        }

        void AddBoxes()
        {
            // 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 startX = StartPosX - ArraySizeX / 2;
            const float startY = StartPosY;
            const float startZ = 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 + startX,
                            3 * k + startY,
                            3 * j + startZ
                        );

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

        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;
        }