protected override void OnInitialize()
        {
            Freelook.SetEyeTarget(eye, target);

            Graphics.SetFormText("BulletSharp - Concave Convexcast Demo");
            Graphics.SetInfoText("Move using mouse and WASD+shift\n" +
                "F3 - Toggle debug\n" +
                //"F11 - Toggle fullscreen\n" +
                "Space - Shoot box");

            DebugDrawMode = debugMode;

            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();
            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);
                }
            }
            indices.Dispose();

            indexVertexArrays.AddIndexedMesh(mesh);

            convexcastBatch = new ConvexcastBatch(40.0f, 0.0f, -10.0f, 80.0f);
            //convexcastBatch = new ConvexcastBatch(true, 40.0f, -50.0f, 50.0f);
        }
        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;
        }
        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";
        }
        public TriangleIndexVertexArray CreateMeshInterface(byte[] meshData, int offset, Dictionary<long, byte[]> libPointers)
        {
            TriangleIndexVertexArray meshInterface = CreateTriangleMeshContainer();
            byte[] meshParts;
            Vector3 scaling;
            int numMeshParts;
            using (MemoryStream shapeStream = new MemoryStream(meshData, false))
            {
                using (BulletReader shapeReader = new BulletReader(shapeStream))
                {
                    shapeStream.Position += offset;
                    long meshPartsPtr = shapeReader.ReadPtr();
                    meshParts = libPointers[meshPartsPtr];
                    scaling = shapeReader.ReadVector3();
                    numMeshParts = shapeReader.ReadInt32();
                }
            }
            using (MemoryStream meshStream = new MemoryStream(meshParts, false))
            {
                using (BulletReader meshReader = new BulletReader(meshStream))
                {
                    for (int i = 0; i < numMeshParts; i++)
                    {
                        int meshOffset = i * Marshal.SizeOf(typeof(MeshPartData));

                        IndexedMesh meshPart = new IndexedMesh();
                        long vertices3f = meshReader.ReadPtr(meshOffset + MeshPartData.Offset("Vertices3F"));
                        long vertices3d = meshReader.ReadPtr(meshOffset + MeshPartData.Offset("Vertices3D"));
                        long indices32 = meshReader.ReadPtr(meshOffset + MeshPartData.Offset("Indices32"));
                        meshPart.NumTriangles = meshReader.ReadInt32(meshOffset + MeshPartData.Offset("NumTriangles"));
                        meshPart.NumVertices = meshReader.ReadInt32(meshOffset + MeshPartData.Offset("NumVertices"));
                        meshPart.Allocate(meshPart.NumTriangles, meshPart.NumVertices, sizeof(int) * 3, sizeof(float) * 4);

                        if (indices32 != 0)
                        {
                            using (Stream triangleStream = meshPart.GetTriangleStream())
                            {
                                byte[] indices = libPointers[indices32];
                                triangleStream.Write(indices, 0, indices.Length);
                            }
                        }
                        else
                        {
                            throw new NotImplementedException();
                            long indices16 = meshReader.ReadPtr(meshOffset + MeshPartData.Offset("Indices16"));
                        }

                        if (vertices3f != 0)
                        {
                            using (Stream vertexStream = meshPart.GetVertexStream())
                            {
                                byte[] vertices = libPointers[vertices3f];
                                vertexStream.Write(vertices, 0, vertices.Length);
                            }
                        }
                        else
                        {
                            throw new NotImplementedException();
                        }
                        if (meshPart.TriangleIndexBase != IntPtr.Zero && meshPart.VertexBase != IntPtr.Zero)
                        {
                            meshInterface.AddIndexedMesh(meshPart, meshPart.IndexType);
                        }
                        //meshPart.Dispose();
                    }
                }
            }
            return meshInterface;
        }