Ejemplo n.º 1
0
        public void SetUp()
        {
            conf = new DefaultCollisionConfiguration();
            dispatcher = new CollisionDispatcher(conf);
            broadphase = new AxisSweep3(new Vector3(-1000, -1000, -1000), new Vector3(1000, 1000, 1000));
            world = new DiscreteDynamicsWorld(dispatcher, broadphase, null, conf);

            // Initialize TriangleIndexVertexArray with float array
            indexVertexArray = new TriangleIndexVertexArray(TorusMesh.Indices, TorusMesh.Vertices);
            gImpactMeshShape = new GImpactMeshShape(indexVertexArray);
            gImpactMeshShape.CalculateLocalInertia(1.0f);
            gImpactMesh = CreateBody(1.0f, gImpactMeshShape, Vector3.Zero);


            // Initialize TriangleIndexVertexArray with Vector3 array
            Vector3[] torusVertices = new Vector3[TorusMesh.Vertices.Length / 3];
            for (int i = 0; i < torusVertices.Length; i++)
            {
                torusVertices[i] = new Vector3(
                    TorusMesh.Vertices[i * 3],
                    TorusMesh.Vertices[i * 3 + 1],
                    TorusMesh.Vertices[i * 3 + 2]);
            }
            indexVertexArray2 = new TriangleIndexVertexArray(TorusMesh.Indices, torusVertices);
            triangleMeshShape = new BvhTriangleMeshShape(indexVertexArray2, true);
            // CalculateLocalInertia must fail for static shapes (shapes based on TriangleMeshShape)
            //triangleMeshShape.CalculateLocalInertia(1.0f);
            triangleMesh = CreateBody(0.0f, triangleMeshShape, Vector3.Zero);
        }
        public ScaledBvhTriangleMeshShape(BvhTriangleMeshShape childShape, Vector3 localScaling)
        {
            IntPtr native = btScaledBvhTriangleMeshShape_new(childShape.Native, ref localScaling);

            InitializeCollisionShape(native);

            ChildShape = childShape;
        }
 public unsafe static void PartialRefitTree(this BvhTriangleMeshShape obj, ref OpenTK.Vector3 aabbMin, ref OpenTK.Vector3 aabbMax)
 {
     fixed(OpenTK.Vector3 *aabbMinPtr = &aabbMin)
     {
         fixed(OpenTK.Vector3 *aabbMaxPtr = &aabbMax)
         {
             obj.PartialRefitTree(ref *(BulletSharp.Math.Vector3 *)aabbMinPtr, ref *(BulletSharp.Math.Vector3 *)aabbMaxPtr);
         }
     }
 }
Ejemplo n.º 4
0
        public override void Run()
        {
            var conf = new DefaultCollisionConfiguration();
            var dispatcher = new CollisionDispatcher(conf);
            var broadphase = new AxisSweep3(new Vector3(-1000, -1000, -1000), new Vector3(1000, 1000, 1000));
            world = new DiscreteDynamicsWorld(dispatcher, broadphase, null, conf);

            var indexVertexArray = new TriangleIndexVertexArray(TorusMesh.Indices, TorusMesh.Vertices);
            foreach (var indexedMesh in indexVertexArray.IndexedMeshArray)
            {
                indexedMesh.ToString();
            }
            AddToDisposeQueue(indexVertexArray);

            var gImpactMesh = new GImpactMeshShape(indexVertexArray);
            Vector3 aabbMin, aabbMax;
            gImpactMesh.GetAabb(Matrix.Identity, out aabbMin, out aabbMax);
            CreateBody(1.0f, gImpactMesh, Vector3.Zero);
            AddToDisposeQueue(gImpactMesh);
            gImpactMesh = null;

            var triangleMesh = new BvhTriangleMeshShape(indexVertexArray, true);
            triangleMesh.CalculateLocalInertia(1.0f);
            triangleMesh.GetAabb(Matrix.Identity, out aabbMin, out aabbMax);
            CreateBody(1.0f, triangleMesh, Vector3.Zero);
            AddToDisposeQueue(triangleMesh);
            triangleMesh = null;

            indexVertexArray = null;

            AddToDisposeQueue(conf);
            AddToDisposeQueue(dispatcher);
            AddToDisposeQueue(broadphase);
            AddToDisposeQueue(world);

            //conf.Dispose();
            conf = null;
            //dispatcher.Dispose();
            dispatcher = null;
            //broadphase.Dispose();
            broadphase = null;
            for (int i = 0; i < 600; i++)
            {
                world.StepSimulation(1.0f / 60.0f);
            }
            world.Dispose();
            world = null;

            ForceGC();
            TestWeakRefs();
            ClearRefs();
        }
Ejemplo n.º 5
0
        public void SetUp()
        {
            conf = new DefaultCollisionConfiguration();
            dispatcher = new CollisionDispatcher(conf);
            broadphase = new AxisSweep3(new Vector3(-1000, -1000, -1000), new Vector3(1000, 1000, 1000));
            world = new DiscreteDynamicsWorld(dispatcher, broadphase, null, conf);

            indexVertexArray = new TriangleIndexVertexArray(TorusMesh.Indices, TorusMesh.Vertices);

            gImpactMeshShape = new GImpactMeshShape(indexVertexArray);

            triangleMeshShape = new BvhTriangleMeshShape(indexVertexArray, true);
            triangleMeshShape.CalculateLocalInertia(1.0f);

            gImpactMesh = CreateBody(1.0f, gImpactMeshShape, Vector3.Zero);
            triangleMesh = CreateBody(1.0f, triangleMeshShape, Vector3.Zero);
        }
        public BulletTriangleMesh(IModelo model, Vector3 pos, Matrix rotation, Vector3 scale)
        {
            TriangleMesh TriangleMesh = new BulletSharp.TriangleMesh(true,false);
            Vector3[] vertices = null;
            int[] indices = null;
            ExtractData(ref vertices, ref indices, model);                                    
            for (int i = 0; i < indices.Count(); i+=3)
            {

                TriangleMesh.AddTriangle(vertices[indices[i]], vertices[indices[i+1]], vertices[indices[i+2]]);
            }
            vertices = null;
            indices = null;     
                        
            BvhTriangleMeshShape = new BvhTriangleMeshShape(TriangleMesh, true, true);
            this.Shape = BvhTriangleMeshShape;
            
            this.Scale = scale;
            Shape.LocalScaling = scale;
            Object = LocalCreateRigidBody(0, Matrix.CreateTranslation(pos) * rotation, Shape);
        }
Ejemplo n.º 7
0
        protected override void OnInitializePhysics()
        {
            // collision configuration contains default setup for memory, collision setup
            CollisionConf = new DefaultCollisionConfiguration();
            Dispatcher = new CollisionDispatcher(CollisionConf);

            Vector3 worldMin = new Vector3(-1000, -1000, -1000);
            Vector3 worldMax = new Vector3(1000, 1000, 1000);
            Broadphase = new AxisSweep3(worldMin, worldMax);
            Solver = new SequentialImpulseConstraintSolver();

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

            const int totalVerts = NUM_VERTS_X * NUM_VERTS_Y;
            const int totalTriangles = 2 * (NUM_VERTS_X - 1) * (NUM_VERTS_Y - 1);
            indexVertexArrays = new TriangleIndexVertexArray();

            IndexedMesh mesh = new IndexedMesh();
            mesh.Allocate(totalVerts, totalTriangles, 3 * sizeof(int), Vector3.SizeInBytes, PhyScalarType.Int32, PhyScalarType.Single);
            using (var indices = mesh.LockIndices())
            {
                for (int i = 0; i < NUM_VERTS_X - 1; i++)
                {
                    for (int j = 0; j < NUM_VERTS_Y - 1; j++)
                    {
                        indices.Write(j * NUM_VERTS_X + i);
                        indices.Write(j * NUM_VERTS_X + i + 1);
                        indices.Write((j + 1) * NUM_VERTS_X + i + 1);

                        indices.Write(j * NUM_VERTS_X + i);
                        indices.Write((j + 1) * NUM_VERTS_X + i + 1);
                        indices.Write((j + 1) * NUM_VERTS_X + i);
                    }
                }
            }

            indexVertexArrays.AddIndexedMesh(mesh);

            raycastBar = new RaycastBar(4000.0f, 0.0f);
            //raycastBar = new RaycastBar(true, 40.0f, -50.0f, 50.0f);

            CollisionShape colShape = new BoxShape(1);
            CollisionShapes.Add(colShape);

            for (int i = 0; i < 10; i++)
            {
                //CollisionShape colShape = new CapsuleShape(0.5f,2.0f);//boxShape = new SphereShape(1.0f);
                Matrix startTransform = Matrix.Translation(2 * i, 10, 1);
                LocalCreateRigidBody(1.0f, startTransform, colShape);
            }

            SetVertexPositions(waveHeight, 0.0f);

            const bool useQuantizedAabbCompression = true;
            groundShape = new BvhTriangleMeshShape(indexVertexArrays, useQuantizedAabbCompression);
            CollisionShapes.Add(groundShape);

            staticBody = LocalCreateRigidBody(0.0f, Matrix.Identity, groundShape);
            staticBody.CollisionFlags |= CollisionFlags.StaticObject;
            staticBody.UserObject = "Ground";
        }
 public ScaledBvhTriangleMeshShape(BvhTriangleMeshShape childShape, Vector3 localScaling)
     : base(btScaledBvhTriangleMeshShape_new(childShape._native, ref localScaling))
 {
     _childShape = childShape;
 }
Ejemplo n.º 9
0
        protected override void OnInitializePhysics()
        {
            CollisionShape groundShape = new BoxShape(50, 3, 50);
            CollisionShapes.Add(groundShape);

            CollisionConf = new DefaultCollisionConfiguration();
            Dispatcher = new CollisionDispatcher(CollisionConf);
            Solver = new SequentialImpulseConstraintSolver();

            Vector3 worldMin = new Vector3(-10000, -10000, -10000);
            Vector3 worldMax = new Vector3(10000, 10000, 10000);
            Broadphase = new AxisSweep3(worldMin, worldMax);
            //Broadphase = new DbvtBroadphase();

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

            int i;
            Matrix tr;
            Matrix vehicleTr;
            //if (UseTrimeshGround)
            {
                const float scale = 20.0f;

                //create a triangle-mesh ground
                const int NumVertsX = 20;
                const int NumVertsY = 20;
                const int totalVerts = NumVertsX * NumVertsY;

                const int totalTriangles = 2 * (NumVertsX - 1) * (NumVertsY - 1);

                TriangleIndexVertexArray vertexArray = new TriangleIndexVertexArray();
                IndexedMesh mesh = new IndexedMesh();
                mesh.Allocate(totalTriangles, totalVerts);
                mesh.NumTriangles = totalTriangles;
                mesh.NumVertices = totalVerts;
                mesh.TriangleIndexStride = 3 * sizeof(int);
                mesh.VertexStride = Vector3.SizeInBytes;
                using (var indicesStream = mesh.GetTriangleStream())
                {
                    var indices = new BinaryWriter(indicesStream);
                    for (i = 0; i < NumVertsX - 1; i++)
                    {
                        for (int j = 0; j < NumVertsY - 1; j++)
                        {
                            indices.Write(j * NumVertsX + i);
                            indices.Write(j * NumVertsX + i + 1);
                            indices.Write((j + 1) * NumVertsX + i + 1);

                            indices.Write(j * NumVertsX + i);
                            indices.Write((j + 1) * NumVertsX + i + 1);
                            indices.Write((j + 1) * NumVertsX + i);
                        }
                    }
                    indices.Dispose();
                }

                using (var vertexStream = mesh.GetVertexStream())
                {
                    var vertices = new BinaryWriter(vertexStream);
                    for (i = 0; i < NumVertsX; i++)
                    {
                        for (int j = 0; j < NumVertsY; j++)
                        {
                            float wl = .2f;
                            float height = 20.0f * (float)(Math.Sin(i * wl) * Math.Cos(j * wl));

                            vertices.Write((i - NumVertsX * 0.5f) * scale);
                            vertices.Write(height);
                            vertices.Write((j - NumVertsY * 0.5f) * scale);
                        }
                    }
                    vertices.Dispose();
                }

                vertexArray.AddIndexedMesh(mesh);
                groundShape = new BvhTriangleMeshShape(vertexArray, true);

                tr = Matrix.Identity;
                vehicleTr = Matrix.Translation(0, -2, 0);
            }/*
            else
            {
                // Use HeightfieldTerrainShape

                int width = 40, length = 40;
                //int width = 128, length = 128; // Debugging is too slow for this
                float maxHeight = 10.0f;
                float heightScale = maxHeight / 256.0f;
                Vector3 scale = new Vector3(20.0f, maxHeight, 20.0f);

                //PhyScalarType scalarType = PhyScalarType.PhyUChar;
                //FileStream file = new FileStream(heightfieldFile, FileMode.Open, FileAccess.Read);

                // Use float data
                PhyScalarType scalarType = PhyScalarType.PhyFloat;
                byte[] terr = new byte[width * length * 4];
                MemoryStream file = new MemoryStream(terr);
                BinaryWriter writer = new BinaryWriter(file);
                for (i = 0; i < width; i++)
                    for (int j = 0; j < length; j++)
                        writer.Write((float)((maxHeight / 2) + 4 * Math.Sin(j * 0.5f) * Math.Cos(i)));
                writer.Flush();
                file.Position = 0;

                HeightfieldTerrainShape heightterrainShape = new HeightfieldTerrainShape(width, length,
                    file, heightScale, 0, maxHeight, upIndex, scalarType, false);
                heightterrainShape.SetUseDiamondSubdivision(true);

                groundShape = heightterrainShape;
                groundShape.LocalScaling = new Vector3(scale.X, 1, scale.Z);

                tr = Matrix.Translation(new Vector3(-scale.X / 2, scale.Y / 2, -scale.Z / 2));
                vehicleTr = Matrix.Translation(new Vector3(20, 3, -3));


                // Create graphics object

                file.Position = 0;
                BinaryReader reader = new BinaryReader(file);

                int totalTriangles = (width - 1) * (length - 1) * 2;
                int totalVerts = width * length;

                game.groundMesh = new Mesh(game.Device, totalTriangles, totalVerts,
                    MeshFlags.SystemMemory | MeshFlags.Use32Bit, VertexFormat.Position | VertexFormat.Normal);
                SlimDX.DataStream data = game.groundMesh.LockVertexBuffer(LockFlags.None);
                for (i = 0; i < width; i++)
                {
                    for (int j = 0; j < length; j++)
                    {
                        float height;
                        if (scalarType == PhyScalarType.PhyFloat)
                        {
                            // heightScale isn't applied internally for float data
                            height = reader.ReadSingle();
                        }
                        else if (scalarType == PhyScalarType.PhyUChar)
                        {
                            height = file.ReadByte() * heightScale;
                        }
                        else
                        {
                            height = 0.0f;
                        }

                        data.Write((j - length * 0.5f) * scale.X);
                        data.Write(height);
                        data.Write((i - width * 0.5f) * scale.Z);

                        // Normals will be calculated later
                        data.Position += 12;
                    }
                }
                game.groundMesh.UnlockVertexBuffer();
                file.Close();

                data = game.groundMesh.LockIndexBuffer(LockFlags.None);
                for (i = 0; i < width - 1; i++)
                {
                    for (int j = 0; j < length - 1; j++)
                    {
                        // Using diamond subdivision
                        if ((j + i) % 2 == 0)
                        {
                            data.Write(j * width + i);
                            data.Write((j + 1) * width + i + 1);
                            data.Write(j * width + i + 1);

                            data.Write(j * width + i);
                            data.Write((j + 1) * width + i);
                            data.Write((j + 1) * width + i + 1);
                        }
                        else
                        {
                            data.Write(j * width + i);
                            data.Write((j + 1) * width + i);
                            data.Write(j * width + i + 1);

                            data.Write(j * width + i + 1);
                            data.Write((j + 1) * width + i);
                            data.Write((j + 1) * width + i + 1);
                        }

                        / *
                        // Not using diamond subdivision
                        data.Write(j * width + i);
                        data.Write((j + 1) * width + i);
                        data.Write(j * width + i + 1);

                        data.Write(j * width + i + 1);
                        data.Write((j + 1) * width + i);
                        data.Write((j + 1) * width + i + 1);
                        * /
                    }
                }
                game.groundMesh.UnlockIndexBuffer();

                game.groundMesh.ComputeNormals();
            }*/

            CollisionShapes.Add(groundShape);


            //create ground object
            RigidBody ground = LocalCreateRigidBody(0, tr, groundShape);
            ground.UserObject = "Ground";


            CollisionShape chassisShape = new BoxShape(1.0f, 0.5f, 2.0f);
            CollisionShapes.Add(chassisShape);

            CompoundShape compound = new CompoundShape();
            CollisionShapes.Add(compound);

            //localTrans effectively shifts the center of mass with respect to the chassis
            Matrix localTrans = Matrix.Translation(Vector3.UnitY);
            compound.AddChildShape(localTrans, chassisShape);
            RigidBody carChassis = LocalCreateRigidBody(800, Matrix.Identity, compound);
            carChassis.UserObject = "Chassis";
            //carChassis.SetDamping(0.2f, 0.2f);

            //CylinderShapeX wheelShape = new CylinderShapeX(wheelWidth, wheelRadius, wheelRadius);


            // clientResetScene();

            // create vehicle
            VehicleTuning tuning = new VehicleTuning();
            IVehicleRaycaster vehicleRayCaster = new DefaultVehicleRaycaster(World);
            //vehicle = new RaycastVehicle(tuning, carChassis, vehicleRayCaster);
            vehicle = new CustomVehicle(tuning, carChassis, vehicleRayCaster);

            carChassis.ActivationState = ActivationState.DisableDeactivation;
            World.AddAction(vehicle);


            const float connectionHeight = 1.2f;
            bool isFrontWheel = true;

            // choose coordinate system
            vehicle.SetCoordinateSystem(rightIndex, upIndex, forwardIndex);

            BulletSharp.Math.Vector3 connectionPointCS0 = new Vector3(CUBE_HALF_EXTENTS - (0.3f * wheelWidth), connectionHeight, 2 * CUBE_HALF_EXTENTS - wheelRadius);
            vehicle.AddWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, isFrontWheel);

            connectionPointCS0 = new Vector3(-CUBE_HALF_EXTENTS + (0.3f * wheelWidth), connectionHeight, 2 * CUBE_HALF_EXTENTS - wheelRadius);
            vehicle.AddWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, isFrontWheel);

            isFrontWheel = false;
            connectionPointCS0 = new Vector3(-CUBE_HALF_EXTENTS + (0.3f * wheelWidth), connectionHeight, -2 * CUBE_HALF_EXTENTS + wheelRadius);
            vehicle.AddWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, isFrontWheel);

            connectionPointCS0 = new Vector3(CUBE_HALF_EXTENTS - (0.3f * wheelWidth), connectionHeight, -2 * CUBE_HALF_EXTENTS + wheelRadius);
            vehicle.AddWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, isFrontWheel);


            for (i = 0; i < vehicle.NumWheels; i++)
            {
                WheelInfo wheel = vehicle.GetWheelInfo(i);
                wheel.SuspensionStiffness = suspensionStiffness;
                wheel.WheelsDampingRelaxation = suspensionDamping;
                wheel.WheelsDampingCompression = suspensionCompression;
                wheel.FrictionSlip = wheelFriction;
                wheel.RollInfluence = rollInfluence;
            }

            vehicle.RigidBody.WorldTransform = vehicleTr;
        }
        public override void Run()
        {
            #region Create renderers

            // Note: the renderers take care of creating their own
            // device resources and listen for DeviceManager.OnInitialize

            // Create a axis-grid renderer
            var axisGrid = ToDispose(new AxisGridRenderer());
            axisGrid.Initialize(this);

            // Create and initialize the mesh renderer
            var loadedMesh             = Common.Mesh.LoadFromFile("PhysicsScene1.cmo");
            List <MeshRenderer> meshes = new List <MeshRenderer>();
            meshes.AddRange(from mesh in loadedMesh
                            select ToDispose(new MeshRenderer(mesh)));
            foreach (var m in meshes)
            {
                m.Initialize(this);
                m.World = Matrix.Identity;
            }


            // Set the first animation as the current animation and start clock
            foreach (var m in meshes)
            {
                if (m.Mesh.Animations != null && m.Mesh.Animations.Any())
                {
                    m.CurrentAnimation = m.Mesh.Animations.First().Value;
                }
                m.Clock.Start();
            }


            loadedMesh = Common.Mesh.LoadFromFile("SubdividedPlane.cmo");
            var waterMesh = ToDispose(new MeshRenderer(loadedMesh.First()));
            waterMesh.Initialize(this);

            loadedMesh = Common.Mesh.LoadFromFile("Bataux.cmo");
            List <MeshRenderer> shipMeshes = new List <MeshRenderer>();
            shipMeshes.AddRange((from mesh in loadedMesh
                                 select ToDispose(new MeshRenderer(mesh))));
            foreach (var m in shipMeshes)
            {
                m.Initialize(this);
                m.World = Matrix.Scaling(3) * Matrix.RotationAxis(Vector3.UnitY, -1.57079f);
            }

            //var anchor = new SphereRenderer(0.05f);
            //anchor.Initialize(this);
            //var anchorWorld = Matrix.Identity;

            //var sphere = new SphereRenderer();
            //sphere.Initialize(this);
            //var sphereWorld = Matrix.Identity;

            // Create and initialize a Direct2D FPS text renderer
            var fps = ToDispose(new Common.FpsRenderer("Calibri", Color.CornflowerBlue, new Point(8, 8), 16));
            fps.Initialize(this);

            // Create and initialize a general purpose Direct2D text renderer
            // This will display some instructions and the current view and rotation offsets
            var textRenderer = ToDispose(new Common.TextRenderer("Calibri", Color.CornflowerBlue, new Point(8, 40), 12));
            textRenderer.Initialize(this);

            #endregion

            #region Initialize physics engine

            CollisionConfiguration defaultConfig = new DefaultCollisionConfiguration();
            ConstraintSolver       solver        = new SequentialImpulseConstraintSolver();
            BulletSharp.Dispatcher dispatcher    = new CollisionDispatcher(defaultConfig);
            BroadphaseInterface    broadphase    = new DbvtBroadphase();
            DynamicsWorld          world         = null;
            Action initializePhysics             = () =>
            {
                RemoveAndDispose(ref world);
                world         = ToDispose(new BulletSharp.DiscreteDynamicsWorld(dispatcher, broadphase, solver, defaultConfig));
                world.Gravity = new Vector3(0, -10, 0);

                // For each mesh, create a RigidBody and add to "world" for simulation
                meshes.ForEach(m =>
                {
                    // We use the name of the mesh to determine the correct body
                    if (String.IsNullOrEmpty(m.Mesh.Name))
                    {
                        return;
                    }

                    var name   = m.Mesh.Name.ToLower();
                    var extent = m.Mesh.Extent;

                    BulletSharp.CollisionShape shape;

                    #region Create collision shape
                    if (name.Contains("box") || name.Contains("cube"))
                    {
                        // Assumes the box/cube has an axis-aligned neutral orientation
                        shape = new BulletSharp.BoxShape(
                            Math.Abs(extent.Max.Z - extent.Min.Z) / 2.0f,
                            Math.Abs(extent.Max.Y - extent.Min.Y) / 2.0f,
                            Math.Abs(extent.Max.X - extent.Min.X) / 2.0f);
                    }
                    else if (name.Contains("sphere"))
                    {
                        shape = new BulletSharp.SphereShape(extent.Radius);
                    }
                    else // use mesh vertices directly
                    {
                        // for each SubMesh, retrieve the vertex and index buffers
                        // to create a TriangleMeshShape for collision detection.
                        List <Vector3> vertices = new List <Vector3>();
                        List <int> indices      = new List <int>();
                        int vertexOffset        = 0;
                        foreach (var sm in m.Mesh.SubMeshes)
                        {
                            vertexOffset += vertices.Count;
                            indices.AddRange(
                                (from indx in m.Mesh.IndexBuffers[(int)sm.IndexBufferIndex]
                                 select vertexOffset + (int)indx));
                            vertices.AddRange(
                                (from v in m.Mesh
                                 .VertexBuffers[(int)sm.VertexBufferIndex]
                                 select v.Position - extent.Center));
                        }
                        // Create the collision shape
                        var iva = new BulletSharp.TriangleIndexVertexArray(indices.ToArray(), vertices.ToArray());
                        shape   = new BulletSharp.BvhTriangleMeshShape(iva, true);
                    }
                    #endregion

                    m.World = Matrix.Identity; // Reset mesh location
                    float mass; Vector3 vec;
                    shape.GetBoundingSphere(out vec, out mass);
                    var body = new BulletSharp.RigidBody(
                        new BulletSharp.RigidBodyConstructionInfo(name.Contains("static") ? 0 : mass,
                                                                  new MeshMotionState(m),
                                                                  shape, shape.CalculateLocalInertia(mass)));
                    if (body.IsStaticObject)
                    {
                        body.Restitution = 1f;
                        body.Friction    = 0.4f;
                    }
                    // Add to the simulation
                    world.AddRigidBody(body);
                });

#if DEBUG
                world.DebugDrawer           = ToDispose(new PhysicsDebugDraw(this.DeviceManager));
                world.DebugDrawer.DebugMode = DebugDrawModes.DrawAabb | DebugDrawModes.DrawWireframe;
#endif
            };
            initializePhysics();


            // Newton's Cradle

            //var box = new Jitter.Dynamics.RigidBody(new Jitter.Collision.Shapes.BoxShape(7, 1, 2));
            //box.Position = new Jitter.LinearMath.JVector(0, 8, 0);
            //world.AddBody(box);
            //box.IsStatic = true;

            //var anchorBody = new Jitter.Dynamics.RigidBody(new Jitter.Collision.Shapes.SphereShape(0.05f));
            //anchorBody.Position = new Jitter.LinearMath.JVector(0, 4, 0);
            //world.AddBody(anchorBody);
            //anchorBody.IsStatic = true;

            //for (var bodyCount = -3; bodyCount < 4; bodyCount++)
            //{
            //    var testBody = new Jitter.Dynamics.RigidBody(new Jitter.Collision.Shapes.SphereShape(0.501f));
            //    testBody.Position = new Jitter.LinearMath.JVector(bodyCount, 0, 0);

            //    world.AddBody(testBody);

            //    world.AddConstraint(new Jitter.Dynamics.Constraints.PointPointDistance(box, testBody,
            //        testBody.Position + Jitter.LinearMath.JVector.Up * 8f + Jitter.LinearMath.JVector.Forward * 3f + Jitter.LinearMath.JVector.Down * 0.5f,
            //        testBody.Position) { Softness = 1.0f, BiasFactor = 0.8f });

            //    world.AddConstraint(new Jitter.Dynamics.Constraints.PointPointDistance(box, testBody,
            //        testBody.Position + Jitter.LinearMath.JVector.Up * 8f + Jitter.LinearMath.JVector.Backward * 3f + Jitter.LinearMath.JVector.Down * 0.5f,
            //        testBody.Position) { Softness = 1.0f, BiasFactor = 0.8f });

            //    testBody.Material.Restitution = 1.0f;
            //    testBody.Material.StaticFriction = 1.0f;
            //}

            #endregion

            // Initialize the world matrix
            var worldMatrix = Matrix.Identity;

            // Set the camera position slightly behind (z)
            var cameraPosition = new Vector3(0, 1, 10);
            var cameraTarget   = Vector3.Zero;  // Looking at the origin 0,0,0
            var cameraUp       = Vector3.UnitY; // Y+ is Up

            // Prepare matrices
            // Create the view matrix from our camera position, look target and up direction
            var viewMatrix = Matrix.LookAtRH(cameraPosition, cameraTarget, cameraUp);
            viewMatrix.TranslationVector += new Vector3(0, -0.98f, 0);

            // Create the projection matrix
            /* FoV 60degrees = Pi/3 radians */
            // Aspect ratio (based on window size), Near clip, Far clip
            var projectionMatrix = Matrix.PerspectiveFovRH((float)Math.PI / 3f, Width / (float)Height, 0.1f, 100f);

            // Maintain the correct aspect ratio on resize
            Window.Resize += (s, e) =>
            {
                projectionMatrix = Matrix.PerspectiveFovRH((float)Math.PI / 3f, Width / (float)Height, 0.1f, 100f);
            };

            bool debugDraw = false;
            bool paused    = false;

            var simTime = new System.Diagnostics.Stopwatch();
            simTime.Start();
            float time     = 0.0f;
            float timeStep = 0.0f;

            #region Rotation and window event handlers

            // Create a rotation vector to keep track of the rotation
            // around each of the axes
            var rotation = new Vector3(0.0f, 0.0f, 0.0f);

            // We will call this action to update text
            // for the text renderer
            Action updateText = () =>
            {
                textRenderer.Text =
                    String.Format("Rotation ({0}) (Up/Down Left/Right Wheel+-)\nView ({1}) (A/D, W/S, Shift+Wheel+-)"
                                  //+ "\nPress 1,2,3,4,5,6,7,8 to switch shaders"
                                  + "\nTime: {2:0.00} (P to toggle, R to reset scene)"
                                  + "\nPhysics debug draw: {3} (E to toggle)"
                                  + "\nBackspace: toggle between Physics and Waves",
                                  rotation,
                                  viewMatrix.TranslationVector,
                                  simTime.Elapsed.TotalSeconds,
                                  debugDraw);
            };

            Dictionary <Keys, bool> keyToggles = new Dictionary <Keys, bool>();
            keyToggles[Keys.Z]    = false;
            keyToggles[Keys.F]    = false;
            keyToggles[Keys.Back] = false;

            // Support keyboard/mouse input to rotate or move camera view
            var moveFactor      = 0.02f; // how much to change on each keypress
            var shiftKey        = false;
            var ctrlKey         = false;
            var background      = Color.White;
            var showNormals     = false;
            var enableNormalMap = true;
            Window.KeyDown += (s, e) =>
            {
                var context = DeviceManager.Direct3DContext;

                shiftKey = e.Shift;
                ctrlKey  = e.Control;

                switch (e.KeyCode)
                {
                // WASD -> pans view
                case Keys.A:
                    viewMatrix.TranslationVector += new Vector3(moveFactor * 2, 0f, 0f);
                    break;

                case Keys.D:
                    viewMatrix.TranslationVector -= new Vector3(moveFactor * 2, 0f, 0f);
                    break;

                case Keys.S:
                    if (shiftKey)
                    {
                        viewMatrix.TranslationVector += new Vector3(0f, moveFactor * 2, 0f);
                    }
                    else
                    {
                        viewMatrix.TranslationVector -= new Vector3(0f, 0f, 1) * moveFactor * 2;
                    }
                    break;

                case Keys.W:
                    if (shiftKey)
                    {
                        viewMatrix.TranslationVector -= new Vector3(0f, moveFactor * 2, 0f);
                    }
                    else
                    {
                        viewMatrix.TranslationVector += new Vector3(0f, 0f, 1) * moveFactor * 2;
                    }
                    break;

                // Up/Down and Left/Right - rotates around X / Y respectively
                // (Mouse wheel rotates around Z)
                case Keys.Down:
                    worldMatrix *= Matrix.RotationX(moveFactor);
                    rotation    += new Vector3(moveFactor, 0f, 0f);
                    break;

                case Keys.Up:
                    worldMatrix *= Matrix.RotationX(-moveFactor);
                    rotation    -= new Vector3(moveFactor, 0f, 0f);
                    break;

                case Keys.Left:
                    worldMatrix *= Matrix.RotationY(moveFactor);
                    rotation    += new Vector3(0f, moveFactor, 0f);
                    break;

                case Keys.Right:
                    worldMatrix *= Matrix.RotationY(-moveFactor);
                    rotation    -= new Vector3(0f, moveFactor, 0f);
                    break;

                case Keys.T:
                    fps.Show          = !fps.Show;
                    textRenderer.Show = !textRenderer.Show;
                    break;

                case Keys.B:
                    if (background == Color.White)
                    {
                        background = new Color(30, 30, 34);
                    }
                    else
                    {
                        background = Color.White;
                    }
                    break;

                case Keys.G:
                    axisGrid.Show = !axisGrid.Show;
                    break;

                case Keys.P:
                    paused = !paused;
                    if (paused)
                    {
                        simTime.Stop();
                    }
                    else
                    {
                        simTime.Start();
                    }

                    // Pause or resume mesh animation
                    meshes.ForEach(m => {
                        if (m.Clock.IsRunning)
                        {
                            m.Clock.Stop();
                        }
                        else
                        {
                            m.Clock.Start();
                        }
                    });
                    updateText();
                    break;

                case Keys.X:
                    // To test for correct resource recreation
                    // Simulate device reset or lost.
                    System.Diagnostics.Debug.WriteLine(SharpDX.Diagnostics.ObjectTracker.ReportActiveObjects());
                    DeviceManager.Initialize(DeviceManager.Dpi);
                    System.Diagnostics.Debug.WriteLine(SharpDX.Diagnostics.ObjectTracker.ReportActiveObjects());
                    break;

                case Keys.Z:
                    keyToggles[Keys.Z] = !keyToggles[Keys.Z];
                    if (keyToggles[Keys.Z])
                    {
                        context.PixelShader.Set(depthPixelShader);
                    }
                    else
                    {
                        context.PixelShader.Set(pixelShader);
                    }
                    break;

                case Keys.F:
                    keyToggles[Keys.F] = !keyToggles[Keys.F];
                    RasterizerStateDescription rasterDesc;
                    if (context.Rasterizer.State != null)
                    {
                        rasterDesc = context.Rasterizer.State.Description;
                    }
                    else
                    {
                        rasterDesc = new RasterizerStateDescription()
                        {
                            CullMode = CullMode.None,
                            FillMode = FillMode.Solid
                        }
                    };
                    if (keyToggles[Keys.F])
                    {
                        rasterDesc.FillMode      = FillMode.Wireframe;
                        context.Rasterizer.State = ToDispose(new RasterizerState(context.Device, rasterDesc));
                    }
                    else
                    {
                        rasterDesc.FillMode      = FillMode.Solid;
                        context.Rasterizer.State = ToDispose(new RasterizerState(context.Device, rasterDesc));
                    }
                    break;

                case Keys.N:
                    if (!shiftKey)
                    {
                        showNormals = !showNormals;
                    }
                    else
                    {
                        enableNormalMap = !enableNormalMap;
                    }
                    break;

                case Keys.E:
                    debugDraw = !debugDraw;
                    break;

                case Keys.R:

                    //world = new Jitter.World(new Jitter.Collision.CollisionSystemSAP());
                    initializePhysics();
                    if (simTime.IsRunning)
                    {
                        simTime.Restart();
                    }
                    else
                    {
                        simTime.Reset();
                    }
                    break;

                case Keys.D1:
                    context.PixelShader.Set(pixelShader);
                    break;

                case Keys.D2:
                    context.PixelShader.Set(lambertShader);
                    break;

                case Keys.D3:
                    context.PixelShader.Set(phongShader);
                    break;

                case Keys.D4:
                    context.PixelShader.Set(blinnPhongShader);
                    break;

                case Keys.Back:
                    keyToggles[Keys.Back] = !keyToggles[Keys.Back];
                    break;
                }

                updateText();
            };
            Window.KeyUp += (s, e) =>
            {
                // Clear the shift/ctrl keys so they aren't sticky
                if (e.KeyCode == Keys.ShiftKey)
                {
                    shiftKey = false;
                }
                if (e.KeyCode == Keys.ControlKey)
                {
                    ctrlKey = false;
                }
            };
            Window.MouseWheel += (s, e) =>
            {
                if (shiftKey)
                {
                    // Zoom in/out
                    viewMatrix.TranslationVector += new Vector3(0f, 0f, (e.Delta / 120f) * moveFactor * 2);
                }
                else
                {
                    // rotate around Z-axis
                    viewMatrix *= Matrix.RotationZ((e.Delta / 120f) * moveFactor);
                    rotation   += new Vector3(0f, 0f, (e.Delta / 120f) * moveFactor);
                }
                updateText();
            };

            var lastX = 0;
            var lastY = 0;

            Window.MouseDown += (s, e) =>
            {
                if (e.Button == MouseButtons.Left)
                {
                    lastX = e.X;
                    lastY = e.Y;
                }
            };

            Window.MouseMove += (s, e) =>
            {
                if (e.Button == MouseButtons.Left)
                {
                    var yRotate = lastX - e.X;
                    var xRotate = lastY - e.Y;
                    lastY = e.Y;
                    lastX = e.X;

                    // Mouse move changes
                    viewMatrix *= Matrix.RotationX(-xRotate * moveFactor);
                    viewMatrix *= Matrix.RotationY(-yRotate * moveFactor);

                    updateText();
                }
            };

            // Display instructions with initial values
            updateText();

            #endregion

            var clock = new System.Diagnostics.Stopwatch();
            clock.Start();

            #region Render loop

            // Create and run the render loop
            RenderLoop.Run(Window, () =>
            {
                // Update simulation, at 60fps
                if (!paused)
                {
                    if ((float)simTime.Elapsed.TotalSeconds < time)
                    {
                        time     = 0;
                        timeStep = 0;
                    }
                    timeStep = ((float)simTime.Elapsed.TotalSeconds - time);
                    time     = (float)simTime.Elapsed.TotalSeconds;
                    world.StepSimulation(timeStep, 7);
                    // For how to choose the maxSubSteps see:
                    // http://www.bulletphysics.org/mediawiki-1.5.8/index.php/Stepping_The_World
                }

                updateText();
                // Start of frame:

                // Retrieve immediate context
                var context = DeviceManager.Direct3DContext;

                // Clear depth stencil view
                context.ClearDepthStencilView(DepthStencilView, DepthStencilClearFlags.Depth | DepthStencilClearFlags.Stencil, 1.0f, 0);
                // Clear render target view
                context.ClearRenderTargetView(RenderTargetView, background);

                // Create viewProjection matrix
                var viewProjection = Matrix.Multiply(viewMatrix, projectionMatrix);

                // Extract camera position from view
                var camPosition = Matrix.Transpose(Matrix.Invert(viewMatrix)).Column4;
                cameraPosition  = new Vector3(camPosition.X, camPosition.Y, camPosition.Z);

                var perFrame             = new ConstantBuffers.PerFrame();
                perFrame.Light.Color     = new Color(0.8f, 0.8f, 0.8f, 1.0f);
                var lightDir             = Vector3.Transform(new Vector3(1f, -1f, -1f), worldMatrix);
                perFrame.Light.Direction = new Vector3(lightDir.X, lightDir.Y, lightDir.Z);
                perFrame.CameraPosition  = cameraPosition;
                perFrame.Time            = (float)simTime.Elapsed.TotalSeconds; // Provide simulation time to shader
                context.UpdateSubresource(ref perFrame, perFrameBuffer);

                // Render each object

                var perMaterial           = new ConstantBuffers.PerMaterial();
                perMaterial.Ambient       = new Color4(0.2f);
                perMaterial.Diffuse       = Color.White;
                perMaterial.Emissive      = new Color4(0);
                perMaterial.Specular      = Color.White;
                perMaterial.SpecularPower = 20f;
                perMaterial.HasTexture    = 0;
                perMaterial.UVTransform   = Matrix.Identity;
                context.UpdateSubresource(ref perMaterial, perMaterialBuffer);

                var perObject = new ConstantBuffers.PerObject();

                // MESH

                if (!keyToggles[Keys.Back])
                {
                    meshes.ForEach((m) =>
                    {
                        perObject.World = m.World * worldMatrix;
                        // Provide the material constant buffer to the mesh renderer
                        perObject.WorldInverseTranspose = Matrix.Transpose(Matrix.Invert(perObject.World));
                        perObject.WorldViewProjection   = perObject.World * viewProjection;
                        perObject.ViewProjection        = viewProjection;
                        perObject.Transpose();
                        context.UpdateSubresource(ref perObject, perObjectBuffer);

                        m.PerMaterialBuffer = perMaterialBuffer;
                        m.PerArmatureBuffer = perArmatureBuffer;
                        m.Render();

                        if (showNormals)
                        {
                            using (var prevPixelShader = context.PixelShader.Get())
                            {
                                perMaterial.HasTexture  = 0;
                                perMaterial.UVTransform = Matrix.Identity;
                                context.UpdateSubresource(ref perMaterial, perMaterialBuffer);
                                context.PixelShader.Set(pixelShader);

                                context.GeometryShader.Set(debugNormals);

                                m.Render();

                                context.PixelShader.Set(prevPixelShader);
                                context.GeometryShader.Set(null);
                            }
                        }
                    });

                    if (debugDraw)
                    {
                        perObject.World = Matrix.Identity;
                        perObject.WorldInverseTranspose = Matrix.Transpose(Matrix.Invert(perObject.World));
                        perObject.WorldViewProjection   = perObject.World * viewProjection;
                        perObject.ViewProjection        = viewProjection;
                        perObject.Transpose();
                        context.UpdateSubresource(ref perObject, perObjectBuffer);

                        (world.DebugDrawer as PhysicsDebugDraw).DrawDebugWorld(world);
                        context.VertexShader.Set(vertexShader);
                        context.PixelShader.Set(pixelShader);
                        context.InputAssembler.InputLayout = vertexLayout;
                    }
                }
                else
                {
                    perObject.World = waterMesh.World * worldMatrix;
                    perObject.WorldInverseTranspose = Matrix.Transpose(Matrix.Invert(perObject.World));
                    perObject.WorldViewProjection   = perObject.World * viewProjection;
                    perObject.ViewProjection        = viewProjection;
                    perObject.Transpose();
                    context.UpdateSubresource(ref perObject, perObjectBuffer);

                    waterMesh.EnableNormalMap   = enableNormalMap;
                    waterMesh.PerMaterialBuffer = perMaterialBuffer;
                    waterMesh.PerArmatureBuffer = perArmatureBuffer;

                    context.VertexShader.Set(waterVertexShader);
                    waterMesh.Render();

                    if (showNormals)
                    {
                        using (var prevPixelShader = context.PixelShader.Get())
                        {
                            perMaterial.HasTexture  = 0;
                            perMaterial.UVTransform = Matrix.Identity;
                            context.UpdateSubresource(ref perMaterial, perMaterialBuffer);
                            context.PixelShader.Set(pixelShader);

                            context.GeometryShader.Set(debugNormals);

                            waterMesh.Render();

                            context.PixelShader.Set(prevPixelShader);
                            context.GeometryShader.Set(null);
                        }
                    }

                    context.VertexShader.Set(vertexShader);

                    foreach (var m in shipMeshes)
                    {
                        perObject.World = m.World * worldMatrix;
                        perObject.WorldInverseTranspose = Matrix.Transpose(Matrix.Invert(perObject.World));
                        perObject.WorldViewProjection   = perObject.World * viewProjection;
                        perObject.Transpose();
                        context.UpdateSubresource(ref perObject, perObjectBuffer);
                        // Provide the material constant buffer to the mesh renderer
                        perObject.WorldInverseTranspose = Matrix.Transpose(Matrix.Invert(perObject.World));
                        perObject.WorldViewProjection   = perObject.World * viewProjection;
                        perObject.ViewProjection        = viewProjection;
                        perObject.Transpose();
                        context.UpdateSubresource(ref perObject, perObjectBuffer);

                        m.PerMaterialBuffer = perMaterialBuffer;
                        m.PerArmatureBuffer = perArmatureBuffer;
                        m.Render();

                        if (showNormals)
                        {
                            using (var prevPixelShader = context.PixelShader.Get())
                            {
                                perMaterial.HasTexture  = 0;
                                perMaterial.UVTransform = Matrix.Identity;
                                context.UpdateSubresource(ref perMaterial, perMaterialBuffer);
                                context.PixelShader.Set(pixelShader);

                                context.GeometryShader.Set(debugNormals);

                                m.Render();

                                context.PixelShader.Set(prevPixelShader);
                                context.GeometryShader.Set(null);
                            }
                        }
                    }
                }

                perMaterial.Ambient       = new Color4(0.2f);
                perMaterial.Diffuse       = Color.White;
                perMaterial.Emissive      = new Color4(0);
                perMaterial.Specular      = Color.White;
                perMaterial.SpecularPower = 20f;
                perMaterial.UVTransform   = Matrix.Identity;
                context.UpdateSubresource(ref perMaterial, perMaterialBuffer);

                // AXIS GRID
                context.HullShader.Set(null);
                context.DomainShader.Set(null);
                context.GeometryShader.Set(null);

                using (var prevPixelShader = context.PixelShader.Get())
                    using (var prevVertexShader = context.VertexShader.Get())
                    {
                        context.VertexShader.Set(vertexShader);
                        context.PixelShader.Set(pixelShader);
                        perObject.World = worldMatrix;
                        perObject.WorldInverseTranspose = Matrix.Transpose(Matrix.Invert(perObject.World));
                        perObject.WorldViewProjection   = perObject.World * viewProjection;
                        perObject.ViewProjection        = viewProjection;
                        perObject.Transpose();
                        context.UpdateSubresource(ref perObject, perObjectBuffer);
                        axisGrid.Render();
                        context.PixelShader.Set(prevPixelShader);
                        context.VertexShader.Set(prevVertexShader);
                    }

                // Render FPS
                fps.Render();

                // Render instructions + position changes
                textRenderer.Render();

                // Present the frame
                Present();
            });
            #endregion
        }
        protected override void OnInitializePhysics()
        {
            ManifoldPoint.ContactAdded += MyContactCallback;

            SetupEmptyDynamicsWorld();

            WavefrontObj wo = new WavefrontObj();
            int tcount = wo.LoadObj("data/file.obj");
            if (tcount > 0)
            {
                TriangleMesh trimesh = new TriangleMesh();
                trimeshes.Add(trimesh);

                Vector3 localScaling = new Vector3(6, 6, 6);
                List<int> indices = wo.Indices;
                List<Vector3> vertices = wo.Vertices;

                int i;
                for (i = 0; i < tcount; i++)
                {
                    int index0 = indices[i * 3];
                    int index1 = indices[i * 3 + 1];
                    int index2 = indices[i * 3 + 2];

                    Vector3 vertex0 = vertices[index0] * localScaling;
                    Vector3 vertex1 = vertices[index1] * localScaling;
                    Vector3 vertex2 = vertices[index2] * localScaling;

                    trimesh.AddTriangle(vertex0, vertex1, vertex2);
                }

                ConvexShape tmpConvexShape = new ConvexTriangleMeshShape(trimesh);

                //create a hull approximation
                ShapeHull hull = new ShapeHull(tmpConvexShape);
                float margin = tmpConvexShape.Margin;
                hull.BuildHull(margin);
                tmpConvexShape.UserObject = hull;

                ConvexHullShape convexShape = new ConvexHullShape();
                foreach (Vector3 v in hull.Vertices)
                {
                    convexShape.AddPoint(v);
                }

                if (sEnableSAT)
                {
                    convexShape.InitializePolyhedralFeatures();
                }
                tmpConvexShape.Dispose();
                //hull.Dispose();

                CollisionShapes.Add(convexShape);

                float mass = 1.0f;

                LocalCreateRigidBody(mass, Matrix.Translation(0, 2, 14), convexShape);

                const bool useQuantization = true;
                CollisionShape concaveShape = new BvhTriangleMeshShape(trimesh, useQuantization);
                LocalCreateRigidBody(0, Matrix.Translation(convexDecompositionObjectOffset), concaveShape);

                CollisionShapes.Add(concaveShape);

                // Bullet Convex Decomposition

                FileStream outputFile = new FileStream("file_convex.obj", FileMode.Create, FileAccess.Write);
                StreamWriter writer = new StreamWriter(outputFile);

                DecompDesc desc = new DecompDesc
                {
                    mVertices = wo.Vertices.ToArray(),
                    mTcount = tcount,
                    mIndices = wo.Indices.ToArray(),
                    mDepth = 5,
                    mCpercent = 5,
                    mPpercent = 15,
                    mMaxVertices = 16,
                    mSkinWidth = 0.0f
                };

                MyConvexDecomposition convexDecomposition = new MyConvexDecomposition(writer, this);
                desc.mCallback = convexDecomposition;

                // HACD

                Hacd myHACD = new Hacd();
                myHACD.SetPoints(wo.Vertices);
                myHACD.SetTriangles(wo.Indices);
                myHACD.CompacityWeight = 0.1;
                myHACD.VolumeWeight = 0.0;

                // HACD parameters
                // Recommended parameters: 2 100 0 0 0 0
                int nClusters = 2;
                const double concavity = 100;
                //bool invert = false;
                const bool addExtraDistPoints = false;
                const bool addNeighboursDistPoints = false;
                const bool addFacesPoints = false;

                myHACD.NClusters = nClusters;                     // minimum number of clusters
                myHACD.VerticesPerConvexHull = 100;               // max of 100 vertices per convex-hull
                myHACD.Concavity = concavity;                     // maximum concavity
                myHACD.AddExtraDistPoints = addExtraDistPoints;
                myHACD.AddNeighboursDistPoints = addNeighboursDistPoints;
                myHACD.AddFacesPoints = addFacesPoints;

                myHACD.Compute();
                nClusters = myHACD.NClusters;

                myHACD.Save("output.wrl", false);

                if (true)
                {
                    CompoundShape compound = new CompoundShape();
                    CollisionShapes.Add(compound);

                    Matrix trans = Matrix.Identity;

                    for (int c = 0; c < nClusters; c++)
                    {
                        //generate convex result
                        Vector3[] points;
                        int[] triangles;
                        myHACD.GetCH(c, out points, out triangles);

                        ConvexResult r = new ConvexResult(points, triangles);
                        convexDecomposition.ConvexDecompResult(r);
                    }

                    for (i = 0; i < convexDecomposition.convexShapes.Count; i++)
                    {
                        Vector3 centroid = convexDecomposition.convexCentroids[i];
                        trans = Matrix.Translation(centroid);
                        ConvexHullShape convexShape2 = convexDecomposition.convexShapes[i] as ConvexHullShape;
                        compound.AddChildShape(trans, convexShape2);

                        RigidBody body = LocalCreateRigidBody(1.0f, trans, convexShape2);
                    }

            #if true
                    mass = 10.0f;
                    trans = Matrix.Translation(-convexDecompositionObjectOffset);
                    RigidBody body2 = LocalCreateRigidBody(mass, trans, compound);
                    body2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;

                    convexDecompositionObjectOffset.Z = 6;
                    trans = Matrix.Translation(-convexDecompositionObjectOffset);
                    body2 = LocalCreateRigidBody(mass, trans, compound);
                    body2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;

                    convexDecompositionObjectOffset.Z = -6;
                    trans = Matrix.Translation(-convexDecompositionObjectOffset);
                    body2 = LocalCreateRigidBody(mass, trans, compound);
                    body2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;
            #endif
                }

                writer.Dispose();
                outputFile.Dispose();
            }
        }
        protected override void OnInitializePhysics()
        {
            ManifoldPoint.ContactAdded += MyContactCallback;

            SetupEmptyDynamicsWorld();

            CompoundCollisionAlgorithm.CompoundChildShapePairCallback = MyCompoundChildShapeCallback;
            convexDecompositionObjectOffset = new Vector3(10, 0, 0);

            // Load wavefront file
            var wo = new WavefrontObj();
            int tcount = wo.LoadObj("data/file.obj");
            if (tcount == 0)
            {
                return;
            }

            // Convert file data to TriangleMesh
            var trimesh = new TriangleMesh();
            trimeshes.Add(trimesh);

            Vector3 localScaling = new Vector3(6, 6, 6);
            List<int> indices = wo.Indices;
            List<Vector3> vertices = wo.Vertices;

            int i;
            for (i = 0; i < tcount; i++)
            {
                int index0 = indices[i * 3];
                int index1 = indices[i * 3 + 1];
                int index2 = indices[i * 3 + 2];

                Vector3 vertex0 = vertices[index0] * localScaling;
                Vector3 vertex1 = vertices[index1] * localScaling;
                Vector3 vertex2 = vertices[index2] * localScaling;

                trimesh.AddTriangleRef(ref vertex0, ref vertex1, ref vertex2);
            }

            // Create a hull approximation
            ConvexHullShape convexShape;
            using (var tmpConvexShape = new ConvexTriangleMeshShape(trimesh))
            {
                using (var hull = new ShapeHull(tmpConvexShape))
                {
                    hull.BuildHull(tmpConvexShape.Margin);
                    convexShape = new ConvexHullShape(hull.Vertices);
                }
            }
            if (sEnableSAT)
            {
                convexShape.InitializePolyhedralFeatures();
            }
            CollisionShapes.Add(convexShape);

            // Add non-moving body to world
            float mass = 1.0f;
            LocalCreateRigidBody(mass, Matrix.Translation(0, 2, 14), convexShape);

            const bool useQuantization = true;
            var concaveShape = new BvhTriangleMeshShape(trimesh, useQuantization);
            LocalCreateRigidBody(0, Matrix.Translation(convexDecompositionObjectOffset), concaveShape);

            CollisionShapes.Add(concaveShape);

            // HACD
            var hacd = new Hacd();
            hacd.SetPoints(wo.Vertices);
            hacd.SetTriangles(wo.Indices);
            hacd.CompacityWeight = 0.1;
            hacd.VolumeWeight = 0.0;

            // Recommended HACD parameters: 2 100 false false false
            hacd.NClusters = 2;                      // minimum number of clusters
            hacd.Concavity = 100;                    // maximum concavity
            hacd.AddExtraDistPoints = false;
            hacd.AddNeighboursDistPoints = false;
            hacd.AddFacesPoints = false;
            hacd.NumVerticesPerConvexHull = 100;     // max of 100 vertices per convex-hull

            hacd.Compute();
            hacd.Save("output.wrl", false);

            // Generate convex result
            var outputFile = new FileStream("file_convex.obj", FileMode.Create, FileAccess.Write);
            var writer = new StreamWriter(outputFile);

            var convexDecomposition = new ConvexDecomposition(writer, this);
            convexDecomposition.LocalScaling = localScaling;

            for (int c = 0; c < hacd.NClusters; c++)
            {
                Vector3[] points;
                int[] triangles;
                hacd.GetCH(c, out points, out triangles);

                convexDecomposition.ConvexDecompResult(points, triangles);
            }

            // Combine convex shapes into a compound shape
            var compound = new CompoundShape();
            for (i = 0; i < convexDecomposition.convexShapes.Count; i++)
            {
                Vector3 centroid = convexDecomposition.convexCentroids[i];
                var convexShape2 = convexDecomposition.convexShapes[i];
                Matrix trans = Matrix.Translation(centroid);
                if (sEnableSAT)
                {
                    convexShape2.InitializePolyhedralFeatures();
                }
                CollisionShapes.Add(convexShape2);
                compound.AddChildShape(trans, convexShape2);

                LocalCreateRigidBody(1.0f, trans, convexShape2);
            }
            CollisionShapes.Add(compound);

            writer.Dispose();
            outputFile.Dispose();

            #if true
            mass = 10.0f;
            var body2 = LocalCreateRigidBody(mass, Matrix.Translation(-convexDecompositionObjectOffset), compound);
            body2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;

            convexDecompositionObjectOffset.Z = 6;
            body2 = LocalCreateRigidBody(mass, Matrix.Translation(-convexDecompositionObjectOffset), compound);
            body2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;

            convexDecompositionObjectOffset.Z = -6;
            body2 = LocalCreateRigidBody(mass, Matrix.Translation(-convexDecompositionObjectOffset), compound);
            body2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;
            #endif
        }
Ejemplo n.º 13
0
 public BvhTriangleMeshShape GetAccurateCollisionShape(float scale = 1.0f)
 {
     //if (CachedBvhTriangleMeshShape != null) return CachedBvhTriangleMeshShape;
     List<Vector3> vectors = GetOrderedVertices();
     var smesh = new TriangleIndexVertexArray(GetOrderedIndices().ToArray(), vectors.ToArray());
     CachedBvhTriangleMeshShape = new BvhTriangleMeshShape(smesh, false);
     //CachedBvhTriangleMeshShape.LocalScaling = new Vector3(scale);
     return CachedBvhTriangleMeshShape;
 }
Ejemplo n.º 14
0
 public BvhTriangleMeshShape GetAccurateCollisionShape()
 {
     //if (CachedBvhTriangleMeshShape != null) return CachedBvhTriangleMeshShape;
     List<Vector3> vectors = GetRawVertexList();
     var smesh = new TriangleIndexVertexArray(Enumerable.Range(0, Vertices.Count).ToArray(), vectors.Select((a) => a).ToArray());
     CachedBvhTriangleMeshShape = new BvhTriangleMeshShape(smesh, false);
     //CachedBvhTriangleMeshShape.LocalScaling = new Vector3(scale);
     return CachedBvhTriangleMeshShape;
 }
Ejemplo n.º 15
0
 protected override CollisionShape CreateShape()
 {
     //ConvexHullShape shape = new ConvexHullShape(this.vertices);
     //BvhTriangleMeshShape bvh = new BvhTriangleMeshShape(
     //StridingMeshInterface smi = new StridingMeshInterface();
     TriangleIndexVertexArray tiv = new TriangleIndexVertexArray(this.indices, this.vertices);
     BvhTriangleMeshShape bvh = new BvhTriangleMeshShape(tiv, true, true);
     //StridingMeshInterface
     return bvh;
 }
Ejemplo n.º 16
0
        public Physics()
        {
            ManifoldPoint.ContactAdded += CustomMaterialCombinerCallback;

            // collision configuration contains default setup for memory, collision setup
            CollisionConf = new DefaultCollisionConfiguration();
            Dispatcher = new CollisionDispatcher(CollisionConf);

            Broadphase = new AxisSweep3(new Vector3(-10000, -10000, -10000), new Vector3(10000, 10000, 10000));
            Solver = new SequentialImpulseConstraintSolver();

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

            const int totalVerts = NumVertsX * NumVertsY;
            const int totalTriangles = 2 * (NumVertsX - 1) * (NumVertsY - 1);

            gVertices = new Vector3[totalVerts];
            int[] gIndices = new int[totalTriangles * 3];
            BulletMaterial[] gMaterials = new BulletMaterial[2];
            int[] gFaceMaterialIndices = new int[totalTriangles];

            // Explicitly set up the materials.  It's a small array so let's do it bit by bit.
            gMaterials[0].Friction = 0;
            gMaterials[0].Restitution = 0.9f;
            gMaterials[1].Friction = 0.9f;
            gMaterials[1].Restitution = 0.1f;

            int i;
            // Set up the vertex data
            SetVertexPositions(waveheight, 0.0f);
            int index = 0;
            // Set up the face data
            for (i = 0; i < NumVertsX - 1; i++)
            {
                for (int j = 0; j < NumVertsY - 1; j++)
                {
                    gIndices[index++] = j * NumVertsX + i;
                    gIndices[index++] = j * NumVertsX + i + 1;
                    gIndices[index++] = (j + 1) * NumVertsX + i + 1;

                    gIndices[index++] = j * NumVertsX + i;
                    gIndices[index++] = (j + 1) * NumVertsX + i + 1;
                    gIndices[index++] = (j + 1) * NumVertsX + i;
                }
            }

            // Set up the face->material index data
            for (int a = 0; a < totalTriangles; a++)
            {
                // This will give the first half of the faces low friction and high restitution
                // and the second half of the faces high friction and low restitution
                if (a > totalTriangles / 2)
                    gFaceMaterialIndices[a] = 0;
                else
                    gFaceMaterialIndices[a] = 1;
            }

            // Create the array structure
            TriangleIndexVertexMaterialArray indexVertexArrays = new TriangleIndexVertexMaterialArray(
                gIndices, gVertices, gMaterials, gFaceMaterialIndices);

            // Create the multimaterial mesh shape
            trimeshShape = new MultimaterialTriangleMeshShape(indexVertexArrays, true);
            CollisionShapes.Add(trimeshShape);

            // create the ground
            //CollisionShape groundShape = new BoxShape(50, 1, 50);
            //CollisionShapes.PushBack(groundShape);
            CollisionObject ground = LocalCreateRigidBody(0, Matrix.Identity, trimeshShape);
            ground.UserObject = "Ground";
            ground.CollisionFlags |= CollisionFlags.StaticObject | CollisionFlags.CustomMaterialCallback;

            CollisionShape colShape = new BoxShape(0.5f);
            CollisionShapes.Add(colShape);

            for (i = 0; i < 12; i++)
            {
                RigidBody body = LocalCreateRigidBody(1, Matrix.Translation(10 - i, 10, -20 + i * 3), colShape);
                body.CollisionFlags |= CollisionFlags.CustomMaterialCallback;
                body.Friction = 0.9f;
                body.Gravity = new Vector3(0, -20, 0);
                body.ApplyCentralImpulse(new Vector3(-7.7f, 0, 0));
            }
        }
 public ScaledBvhTriangleMeshShape(BvhTriangleMeshShape childShape, Vector3 localScaling)
     : base(btScaledBvhTriangleMeshShape_new(childShape.Native, ref localScaling))
 {
     ChildShape = childShape;
 }
        /// <summary>
        /// Gets a collision shape from a .bullet file.
        /// </summary>
        /// <param name="filename">The full filename, <u>including extension</u></param>
        /// <param name="ent">If the .bullet file does not exist, this is the entity we should use to generate it</param>
        /// <param name="node">If the .bullet file does not exist, this is the node we will use to get info from when generating it</param>
        /// <returns></returns>
        public CollisionShape GetShapeFromFile(string filename, Entity ent, SceneNode node)
        {
            CollisionShape shape;

            if (!Shapes.TryGetValue(filename, out shape)) {
                // check to see if the .bullet file exists
                string bulletfile = GetBulletFile(filename);
                if (bulletfile != null) {
                    // if it does not, import it (make sure we get rid of the extension first)
                    shape = ImportCollisionShape(Path.GetFileNameWithoutExtension(bulletfile));
                }
                else {
                    Launch.Log("[PhysicsMain] " + filename + " does not exist, converting Ogre mesh into physics trimesh and exporting new .bullet file...");
                    // it does not have a file, so we need to convert our ogre mesh
                    shape = new BvhTriangleMeshShape(OgreToBulletMesh.Convert(ent, node), true, true);
                    (shape as BvhTriangleMeshShape).BuildOptimizedBvh();
                    // and then export it as a .bullet file
                    SerializeShape(shape, node.Name);
                }

                // add the shape to the dictionary, including the .bullet extension
                Shapes.Add(filename, shape);
            }

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

            Vector3 worldMin = new Vector3(-1000, -1000, -1000);
            Vector3 worldMax = new Vector3(1000, 1000, 1000);
            Broadphase = new AxisSweep3(worldMin, worldMax);
            Solver = new SequentialImpulseConstraintSolver();

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


            const int totalVerts = NumVertsX * NumVertsY;
            const int totalTriangles = 2 * (NumVertsX - 1) * (NumVertsY - 1);
            indexVertexArrays = new TriangleIndexVertexArray();

            IndexedMesh mesh = new IndexedMesh();
            mesh.NumTriangles = totalTriangles;
            mesh.NumVertices = totalVerts;
            mesh.TriangleIndexStride = 3 * sizeof(int);
            mesh.VertexStride = Vector3.SizeInBytes;
            mesh.TriangleIndexBase = Marshal.AllocHGlobal(mesh.TriangleIndexStride * totalTriangles);
            mesh.VertexBase = Marshal.AllocHGlobal(mesh.VertexStride * totalVerts);
            var indicesStream = mesh.GetTriangleStream();
            using (var indices = new BinaryWriter(indicesStream))
            {
                for (int i = 0; i < NumVertsX - 1; i++)
                {
                    for (int j = 0; j < NumVertsY - 1; j++)
                    {
                        indices.Write(j*NumVertsX + i);
                        indices.Write(j*NumVertsX + i + 1);
                        indices.Write((j + 1)*NumVertsX + i + 1);

                        indices.Write(j*NumVertsX + i);
                        indices.Write((j + 1)*NumVertsX + i + 1);
                        indices.Write((j + 1)*NumVertsX + i);
                    }
                }
            }

            indexVertexArrays.AddIndexedMesh(mesh);

            convexcastBatch = new ConvexcastBatch(40.0f, 0.0f, -10.0f, 80.0f);


            CollisionShape colShape = new BoxShape(1);
            CollisionShapes.Add(colShape);

            for (int j = 0; j < NumDynamicBoxesX; j++)
            {
                for (int i = 0; i < NumDynamicBoxesY; i++)
                {
                    //CollisionShape colShape = new CapsuleShape(0.5f,2.0f);//boxShape = new SphereShape(1.0f);
                    Matrix startTransform = Matrix.Translation(5 * (i - NumDynamicBoxesX / 2), 10, 5 * (j - NumDynamicBoxesY / 2));
                    LocalCreateRigidBody(1.0f, startTransform, colShape);
                }
            }

            SetVertexPositions(WaveHeight, 0.0f);

            const bool useQuantizedAabbCompression = true;
            groundShape = new BvhTriangleMeshShape(indexVertexArrays, useQuantizedAabbCompression);
            CollisionShapes.Add(groundShape);

            staticBody = LocalCreateRigidBody(0.0f, Matrix.Identity, groundShape);
            staticBody.CollisionFlags |= CollisionFlags.StaticObject;
            staticBody.UserObject = "Ground";
        }
Ejemplo n.º 20
0
 public ScaledBvhTriangleMeshShape CreateScaledTriangleMeshShape(BvhTriangleMeshShape meshShape, ref Vector3 localScalingbtBvhTriangleMeshShape)
 {
     ScaledBvhTriangleMeshShape shape = new ScaledBvhTriangleMeshShape(meshShape, localScalingbtBvhTriangleMeshShape);
     _allocatedCollisionShapes.Add(shape);
     return shape;
 }
Ejemplo n.º 21
0
        static void TestGCCollection()
        {
            var conf = new DefaultCollisionConfiguration();
            var dispatcher = new CollisionDispatcher(conf);
            var broadphase = new DbvtBroadphase();
            //var broadphase = new AxisSweep3(new Vector3(-1000, -1000, -1000), new Vector3(1000, 1000, 1000));
            world = new DiscreteDynamicsWorld(dispatcher, broadphase, null, conf);
            world.Gravity = new Vector3(0, -10, 0);
            dispatcher.NearCallback = DispatcherNearCallback;

            CreateBody(0.0f, new BoxShape(50, 1, 50), Vector3.Zero);
            var dynamicObject = CreateBody(10.0f, new SphereShape(1.0f), new Vector3(2, 2, 0));
            var dynamicObject2 = CreateBody(1.0f, new SphereShape(1.0f), new Vector3(0, 2, 0));

            var ghostPairCallback = new GhostPairCallback();
            broadphase.OverlappingPairCache.SetInternalGhostPairCallback(ghostPairCallback);
            AddToDisposeQueue(ghostPairCallback);
            ghostPairCallback = null;
            var ghostObject = new PairCachingGhostObject();
            ghostObject.CollisionShape = new BoxShape(2);
            ghostObject.WorldTransform = Matrix.Translation(2,2,0);
            world.AddCollisionObject(ghostObject);

            var trimesh = new TriangleMesh();
            Vector3 v0 = new Vector3(0, 0, 0);
            Vector3 v1 = new Vector3(1, 0, 0);
            Vector3 v2 = new Vector3(0, 1, 0);
            Vector3 v3 = new Vector3(1, 1, 0);
            trimesh.AddTriangle(v0, v1, v2);
            trimesh.AddTriangle(v1, v3, v2);
            var triangleMeshShape = new BvhTriangleMeshShape(trimesh, false);
            var triMeshObject = CreateBody(0, triangleMeshShape, new Vector3(20,0,20));
            AddToDisposeQueue(triangleMeshShape);
            AddToDisposeQueue(trimesh);
            AddToDisposeQueue(triMeshObject);
            triangleMeshShape = null;
            trimesh = null;

            AddToDisposeQueue(conf);
            AddToDisposeQueue(dispatcher);
            AddToDisposeQueue(broadphase);
            AddToDisposeQueue(world);

            //conf.Dispose();
            conf = null;
            //dispatcher.Dispose();
            dispatcher = null;
            //broadphase.Dispose();
            broadphase = null;
            world.DebugDrawer = new DebugDrawTest();
            AddToDisposeQueue(world.DebugDrawer);
            world.SetInternalTickCallback(WorldPreTickCallback);
            for (int i = 0; i < 600; i++)
            {
                world.StepSimulation(1.0f / 60.0f);
            }

            world.DispatchInfo.DebugDraw = new DebugDrawTest2();
            AddToDisposeQueue(world.DispatchInfo.DebugDraw);
            world.DispatchInfo.DebugDraw = world.DispatchInfo.DebugDraw;
            AddToDisposeQueue(world.DispatchInfo.DebugDraw);
            world.DispatchInfo.DebugDraw = null;
            world.DebugDrawer = null;
            world.DebugDrawer = new DebugDrawTest2();
            world.StepSimulation(1.0f / 60.0f);
            world.DebugDrawWorld();
            AddToDisposeQueue(world.DispatchInfo.DebugDraw);

            world.DebugDrawer = new DebugDrawTest();
            world.DebugDrawWorld();
            AddToDisposeQueue(world.DebugDrawer);
            world.DebugDrawer = null;

            TestContactTest(dynamicObject, dynamicObject2);
            TestGhostObjectPairs(ghostObject);
            TestRayCast(dynamicObject);
            TestTriangleMeshRayCast(triMeshObject);
            dynamicObject = null;
            dynamicObject2 = null;
            triMeshObject = null;

            //world.SetInternalTickCallback(null);
            world.Dispose();
            world = null;

            GC.Collect(GC.MaxGeneration, GCCollectionMode.Forced);
            GC.WaitForPendingFinalizers();

            TestWeakRefs();
            disposeQueue.Clear();
        }
Ejemplo n.º 22
0
 public BvhTriangleMeshShape CreateBvhTriangleMeshShape(StridingMeshInterface trimesh, OptimizedBvh bvh)
 {
     BvhTriangleMeshShape shape;
     if (bvh != null)
     {
         shape = new BvhTriangleMeshShape(trimesh, bvh.IsQuantized, false);
         shape.OptimizedBvh = bvh;
     }
     else
     {
         shape = new BvhTriangleMeshShape(trimesh, true);
     }
     _allocatedCollisionShapes.Add(shape);
     return shape;
 }
        protected override void OnInitializePhysics()
        {
            // collision configuration contains default setup for memory, collision setup
            CollisionConf = new DefaultCollisionConfiguration();
            Dispatcher = new CollisionDispatcher(CollisionConf);

            Vector3 worldMin = new Vector3(-1000, -1000, -1000);
            Vector3 worldMax = new Vector3(1000, 1000, 1000);
            Broadphase = new AxisSweep3(worldMin, worldMax);
            Solver = new SequentialImpulseConstraintSolver();

            World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, Solver, CollisionConf);
            World.SolverInfo.SplitImpulse = 1;
            World.Gravity = new Vector3(0, -10, 0);
            IsDebugDrawEnabled = true;

            CollisionShape colShape = new BoxShape(1);
            CollisionShapes.Add(colShape);

            for (int j = 0; j < NumDynamicBoxesX; j++)
            {
                for (int i = 0; i < NumDynamicBoxesY; i++)
                {
                    //CollisionShape colShape = new CapsuleShape(0.5f,2.0f);//boxShape = new SphereShape(1.0f);
                    Matrix startTransform = Matrix.Translation(5 * (i - NumDynamicBoxesX / 2), 10, 5 * (j - NumDynamicBoxesY / 2));
                    LocalCreateRigidBody(1.0f, startTransform, colShape);
                }
            }

            SetVertexPositions(WaveHeight, 0.0f);

            const bool useQuantizedAabbCompression = true;
            groundShape = new BvhTriangleMeshShape(indexVertexArrays, useQuantizedAabbCompression);
            CollisionShapes.Add(groundShape);

            staticBody = LocalCreateRigidBody(0.0f, Matrix.Identity, groundShape);
            staticBody.CollisionFlags |= CollisionFlags.StaticObject;
            staticBody.UserObject = "Ground";
        }
        protected override void OnInitializePhysics()
        {
            ManifoldPoint.ContactAdded += MyContactCallback;

            SetupEmptyDynamicsWorld();

            //CompoundCollisionAlgorithm.CompoundChildShapePairCallback = MyCompoundChildShapeCallback;
            convexDecompositionObjectOffset = new Vector3(10, 0, 0);

            // Load wavefront file
            var wo = new WavefrontObj();
            //string filename = UnityEngine.Application.dataPath + "/BulletUnity/Examples/Scripts/BulletSharpDemos/ConvexDecompositionDemo/data/file.obj";
            UnityEngine.TextAsset bytes = (UnityEngine.TextAsset)UnityEngine.Resources.Load("file.obj");
            System.IO.Stream byteStream = new System.IO.MemoryStream(bytes.bytes);

            int tcount = wo.LoadObj(byteStream);
            if (tcount == 0)
            {
                return;
            }

            // Convert file data to TriangleMesh
            var trimesh = new TriangleMesh();
            trimeshes.Add(trimesh);

            Vector3 localScaling = new Vector3(6, 6, 6);
            List<int> indices = wo.Indices;
            List<Vector3> vertices = wo.Vertices;

            int i;
            for (i = 0; i < tcount; i++)
            {
                int index0 = indices[i * 3];
                int index1 = indices[i * 3 + 1];
                int index2 = indices[i * 3 + 2];

                Vector3 vertex0 = vertices[index0] * localScaling;
                Vector3 vertex1 = vertices[index1] * localScaling;
                Vector3 vertex2 = vertices[index2] * localScaling;

                trimesh.AddTriangleRef(ref vertex0, ref vertex1, ref vertex2);
            }

            // Create a hull approximation
            ConvexHullShape convexShape;
            using (var tmpConvexShape = new ConvexTriangleMeshShape(trimesh))
            {
                using (var hull = new ShapeHull(tmpConvexShape))
                {
                    hull.BuildHull(tmpConvexShape.Margin);
                    convexShape = new ConvexHullShape(hull.Vertices);
                }
            }
            if (sEnableSAT)
            {
                convexShape.InitializePolyhedralFeatures();
            }
            CollisionShapes.Add(convexShape);

            // Add non-moving body to world
            float mass = 1.0f;
            LocalCreateRigidBody(mass, Matrix.Translation(0, 2, 14), convexShape);

            const bool useQuantization = true;
            var concaveShape = new BvhTriangleMeshShape(trimesh, useQuantization);
            LocalCreateRigidBody(0, Matrix.Translation(convexDecompositionObjectOffset), concaveShape);

            CollisionShapes.Add(concaveShape);

            // HACD
            var hacd = new Hacd();
            hacd.SetPoints(wo.Vertices);
            hacd.SetTriangles(wo.Indices);
            hacd.CompacityWeight = 0.1;
            hacd.VolumeWeight = 0.0;

            // Recommended HACD parameters: 2 100 false false false
            hacd.NClusters = 2;                      // minimum number of clusters
            hacd.Concavity = 100;                    // maximum concavity
            hacd.AddExtraDistPoints = false;
            hacd.AddNeighboursDistPoints = false;
            hacd.AddFacesPoints = false;
            hacd.NVerticesPerCH = 100;     // max of 100 vertices per convex-hull

            hacd.Compute();
            hacd.Save("output.wrl", false);

            // Generate convex result
            var outputFile = new FileStream("file_convex.obj", FileMode.Create, FileAccess.Write);
            var writer = new StreamWriter(outputFile);

            var convexDecomposition = new ConvexDecomposition(writer, this);
            convexDecomposition.LocalScaling = localScaling;

            for (int c = 0; c < hacd.NClusters; c++)
            {
                int nVertices = hacd.GetNPointsCH(c);
                int trianglesLen = hacd.GetNTrianglesCH(c) * 3;
                double[] points = new double[nVertices * 3];
                long[] triangles = new long[trianglesLen];
                hacd.GetCH(c, points, triangles);

                if (trianglesLen == 0)
                {
                    continue;
                }

                Vector3[] verticesArray = new Vector3[nVertices];
                int vi3 = 0;
                for (int vi = 0; vi < nVertices; vi++)
                {
                    verticesArray[vi] = new Vector3(
                        (float)points[vi3], (float)points[vi3 + 1], (float)points[vi3 + 2]);
                    vi3 += 3;
                }

                int[] trianglesInt = new int[trianglesLen];
                for (int ti = 0; ti < trianglesLen; ti++)
                {
                    trianglesInt[ti] = (int)triangles[ti];
                }

                convexDecomposition.ConvexDecompResult(verticesArray, trianglesInt);
            }

            // Combine convex shapes into a compound shape
            var compound = new CompoundShape();
            for (i = 0; i < convexDecomposition.convexShapes.Count; i++)
            {
                Vector3 centroid = convexDecomposition.convexCentroids[i];
                var convexShape2 = convexDecomposition.convexShapes[i];
                Matrix trans = Matrix.Translation(centroid);
                if (sEnableSAT)
                {
                    convexShape2.InitializePolyhedralFeatures();
                }
                CollisionShapes.Add(convexShape2);
                compound.AddChildShape(trans, convexShape2);

                LocalCreateRigidBody(1.0f, trans, convexShape2);
            }
            CollisionShapes.Add(compound);

            writer.Dispose();
            outputFile.Dispose();

            #if true
            mass = 10.0f;
            var body2 = LocalCreateRigidBody(mass, Matrix.Translation(-convexDecompositionObjectOffset), compound);
            body2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;

            convexDecompositionObjectOffset.Z = 6;
            body2 = LocalCreateRigidBody(mass, Matrix.Translation(-convexDecompositionObjectOffset), compound);
            body2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;

            convexDecompositionObjectOffset.Z = -6;
            body2 = LocalCreateRigidBody(mass, Matrix.Translation(-convexDecompositionObjectOffset), compound);
            body2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;
            #endif
        }