示例#1
0
        private CompoundShape CreateCompoundShape(Hacd hacd, Vector3 localScaling)
        {
            var wavefrontWriter     = new WavefrontWriter("file_convex.obj");
            var convexDecomposition = new ConvexDecomposition(wavefrontWriter)
            {
                LocalScaling = localScaling
            };

            for (int c = 0; c < hacd.NClusters; c++)
            {
                int trianglesLen = hacd.GetNTrianglesCH(c) * 3;
                if (trianglesLen == 0)
                {
                    continue;
                }
                var triangles = new long[trianglesLen];

                int nVertices = hacd.GetNPointsCH(c);
                var points    = new double[nVertices * 3];

                hacd.GetCH(c, points, triangles);

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

                convexDecomposition.Result(verticesArray, triangles);
            }

            wavefrontWriter.Dispose();

            // Combine convex shapes into a compound shape
            var compoundShape = new CompoundShape();

            for (int i = 0; i < convexDecomposition.ConvexShapes.Count; i++)
            {
                Vector3 centroid    = convexDecomposition.ConvexCentroids[i];
                var     convexShape = convexDecomposition.ConvexShapes[i];
                Matrix  trans       = Matrix.Translation(centroid);
                if (_enableSat)
                {
                    convexShape.InitializePolyhedralFeatures();
                }
                compoundShape.AddChildShape(trans, convexShape);

                PhysicsHelper.CreateBody(1.0f, trans, convexShape, World);
            }

            return(compoundShape);
        }
示例#2
0
        private CompoundShape CreateCompoundShape(Hacd hacd, Vector3 localScaling)
        {
            var wavefrontWriter     = new WavefrontWriter("file_convex.obj");
            var convexDecomposition = new ConvexDecomposition(wavefrontWriter)
            {
                LocalScaling = localScaling
            };

            for (int c = 0; c < hacd.NClusters; c++)
            {
                int trianglesLen = hacd.GetNTrianglesCH(c) * 3;
                if (trianglesLen == 0)
                {
                    continue;
                }

                Vector3[] points;
                int[]     triangles;
                hacd.GetCH(c, out points, out triangles);

                convexDecomposition.Result(points, triangles);
            }

            wavefrontWriter.Dispose();

            // Combine convex shapes into a compound shape
            var compoundShape = new CompoundShape();

            for (int i = 0; i < convexDecomposition.ConvexShapes.Count; i++)
            {
                Vector3 centroid    = convexDecomposition.ConvexCentroids[i];
                var     convexShape = convexDecomposition.ConvexShapes[i];
                Matrix  trans       = Matrix.Translation(centroid);
                if (_enableSat)
                {
                    convexShape.InitializePolyhedralFeatures();
                }
                compoundShape.AddChildShape(trans, convexShape);

                LocalCreateRigidBody(1.0f, trans, convexShape);
            }

            return(compoundShape);
        }
        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
        }
示例#4
0
        protected override void OnInitializePhysics()
        {
            ManifoldPoint.ContactAdded += MyContactCallback;

            SetupEmptyDynamicsWorld();
            CreateGround();

            //CompoundCollisionAlgorithm.CompoundChildShapePairCallback = MyCompoundChildShapeCallback;

            var wo = WavefrontObj.Load("data/file.obj");

            if (wo.Indices.Count == 0)
            {
                return;
            }

            var localScaling = new Vector3(6, 6, 6);

            triangleMesh = CreateTriangleMesh(wo.Indices, wo.Vertices, localScaling);

            // Convex hull approximation
            ConvexHullShape convexShape = CreateHullApproximation(triangleMesh);

            CollisionShapes.Add(convexShape);
            float mass = 1.0f;

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

            // Non-moving body
            Vector3    convexDecompositionObjectOffset = new Vector3(10, 0, 0);
            const bool useQuantization = true;
            var        concaveShape    = new BvhTriangleMeshShape(triangleMesh, useQuantization);

            CollisionShapes.Add(concaveShape);
            LocalCreateRigidBody(0, Matrix.Translation(convexDecompositionObjectOffset), concaveShape);


            var hacd = new Hacd()
            {
                VerticesPerConvexHull = 100,
                CompacityWeight       = 0.1,
                VolumeWeight          = 0,

                // Recommended HACD parameters
                NClusters               = 2,
                Concavity               = 100,
                AddExtraDistPoints      = false,
                AddFacesPoints          = false,
                AddNeighboursDistPoints = false
            };

            hacd.SetPoints(wo.Vertices);
            hacd.SetTriangles(wo.Indices);

            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)
            {
                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.Result(verticesArray, trianglesInt);
            }

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


            // Combine convex shapes into a compound shape
            var compound = new CompoundShape();

            for (int i = 0; i < convexDecomposition.convexShapes.Count; i++)
            {
                Vector3 centroid     = convexDecomposition.convexCentroids[i];
                var     convexShape2 = convexDecomposition.convexShapes[i];
                Matrix  trans        = Matrix.Translation(centroid);
                if (enableSat)
                {
                    convexShape2.InitializePolyhedralFeatures();
                }
                CollisionShapes.Add(convexShape2);
                compound.AddChildShape(trans, convexShape2);

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

#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
        }
示例#5
0
        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
        }
        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];
                Matrix  trans        = Matrix.Translation(centroid);
                var     convexShape2 = convexDecomposition.convexShapes[i] as ConvexHullShape;
                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
        }
        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
        }