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

            groundShape            = new BoxShape(30, 1, 30);
            ground                 = CreateBody(0, Matrix.Translation(0, -5, 0), groundShape);
            ground.CollisionFlags |= CollisionFlags.CustomMaterialCallback;

            compoundShape = new CompoundShape();
            boxShape      = new BoxShape(1, 1, 1);
            compoundShape.AddChildShape(Matrix.Identity, boxShape);
            boxShape2 = new BoxShape(1, 1, 1);
            compoundShape.AddChildShape(Matrix.Translation(0, -1, 0), boxShape2);
            boxShape3 = new BoxShape(1, 1, 1);
            compoundShape.AddChildShape(Matrix.Translation(0, -2, 0), boxShape3);

            compoundShape2 = new CompoundShape();
            compoundShape2.AddChildShape(Matrix.Identity, compoundShape);

            compound = CreateBody(1, Matrix.Translation(0, 0, 0), compoundShape2);

            ManifoldPoint.ContactAdded += ContactAdded;
        }
Beispiel #2
0
        void CreateRigidBodyStack(int count)
        {
            const float mass = 10.0f;

            CompoundShape  cylinderCompound = new CompoundShape();
            CollisionShape cylinderShape    = new CylinderShapeX(4, 1, 1);
            CollisionShape boxShape         = new BoxShape(4, 1, 1);

            cylinderCompound.AddChildShape(Matrix.Identity, boxShape);
            Quaternion orn            = Quaternion.RotationYawPitchRoll((float)Math.PI / 2.0f, 0.0f, 0.0f);
            Matrix     localTransform = Matrix.RotationQuaternion(orn);

            //localTransform *= Matrix.Translation(new Vector3(1,1,1));
            cylinderCompound.AddChildShape(localTransform, cylinderShape);

            CollisionShape[] shape =
            {
                cylinderCompound,
                new BoxShape(new Vector3(1, 1, 1)),
                new SphereShape(1.5f)
            };

            for (int i = 0; i < count; ++i)
            {
                LocalCreateRigidBody(mass, Matrix.Translation(0, 2 + 6 * i, 0), shape[i % shape.Length]);
            }
        }
Beispiel #3
0
            private void _injectShape(ChVector pos, ChMatrix33 <double> rot, CollisionShape mshape)
            {
                bool centered = true;// (pos.IsNull() && rot.IsIdentity());  // FIX THIS !!!

                // This is needed so later one can access ChModelBullet::GetSafeMargin and ChModelBullet::GetEnvelope
                mshape.SetUserPointer(this);

                // start_vector = ||    -- description is still empty
                if (shapes.Count == 0)
                {
                    if (centered)
                    {
                        shapes.Add(mshape);
                        bt_collision_object.SetCollisionShape(mshape);
                        // end_vector=  | centered shape |
                        return;
                    }
                    else
                    {
                        CompoundShape mcompound = new CompoundShape();
                        shapes.Add(mcompound);
                        shapes.Add(mshape);
                        bt_collision_object.SetCollisionShape(mcompound);
                        IndexedMatrix mtransform = new IndexedMatrix();
                        ChPosMatrToBullet(pos, rot, ref mtransform);
                        mcompound.AddChildShape(ref mtransform, mshape);
                        // vector=  | compound | not centered shape |
                        return;
                    }
                }
                // start_vector = | centered shape |    ----just a single centered shape was added
                if (shapes.Count == 1)
                {
                    IndexedMatrix mtransform = new IndexedMatrix();
                    shapes.Add(shapes[0]);
                    shapes.Add(mshape);
                    CompoundShape mcompound = new CompoundShape(true);
                    shapes[0] = mcompound;
                    bt_collision_object.SetCollisionShape(mcompound);
                    //mtransform.setIdentity();
                    mcompound.AddChildShape(ref mtransform, shapes[1]);
                    ChPosMatrToBullet(pos, rot, ref mtransform);
                    mcompound.AddChildShape(ref mtransform, shapes[2]);
                    // vector=  | compound | old centered shape | new shape | ...
                    return;
                }
                // vector=  | compound | old | old.. |   ----already working with compounds..
                if (shapes.Count > 1)
                {
                    IndexedMatrix mtransform = new IndexedMatrix();
                    shapes.Add(mshape);
                    ChPosMatrToBullet(pos, rot, ref mtransform);
                    CollisionShape mcom = shapes[0];
                    ((CompoundShape)mcom).AddChildShape(ref mtransform, mshape);
                    // vector=  | compound | old | old.. | new shape | ...
                    return;
                }
            }
Beispiel #4
0
                static void UpdateShapes(ref RigidbodyInternal rb)
                {
                    if (rb.ownsCollisionShape && rb.CollisionShape != null)
                    {
                        rb.CollisionShape.Dispose();
                    }

                    CollisionShape resultShape;

                    var collisionShapes = rb.CollisionShapes;

                    if (collisionShapes.Count == 0)
                    {
                        //EmptyShape
                        resultShape = new EmptyShape();
                    }
                    else
                    {
                        //CompoundShape
                        var compoundShape = new CompoundShape();

                        for (int i = 0; i < collisionShapes.Count; i++)
                        {
                            compoundShape.AddChildShape(Matrix4x4.Identity, collisionShapes[i]);
                        }

                        resultShape = compoundShape;
                    }

                    rb.ownsCollisionShape             = true;
                    rb.BulletRigidbody.CollisionShape = resultShape;

                    rb.updateShapes = false;
                    rb.updateMass   = true;
                }
Beispiel #5
0
        private RigidBody CreateGear(float radius, Matrix transform)
        {
            const float mass = 6.28f;

            var shape = new CompoundShape();
            var axle  = new CylinderShape(0.2f, 0.25f, 0.2f);
            var wheel = new CylinderShape(radius, 0.025f, radius);

            shape.AddChildShape(Matrix.Identity, axle);
            shape.AddChildShape(Matrix.Identity, wheel);

            RigidBody body = LocalCreateRigidBody(mass, transform, shape);

            body.LinearFactor = Vector3.Zero;
            return(body);
        }
        //Generate and return an optimized Bullet ConvexHullShape
        private void GenerateCompoundCollider()
        {
            //Get all child collision shapes
            _childShapes = GetComponentsInChildren <BulletCollisionShape>().ToList();

            //Warn the user if no child colliders
            if (_childShapes.Count < 2)
            {
                Debug.LogError("Bullet Compound Shape requires at least two or more child collision shapes!\n" +
                               "Please add two or more primitive collision shapes as child objects.");
                return;
            }

            //Generate the compound collider
            _compoundShape = new CompoundShape {
                LocalScaling = transform.localScale.ToBullet()
            };
            _childTransform = new Matrix();
            foreach (var child in _childShapes)
            {
                if (child == this)
                {
                    continue;
                }
                _childTransform.Origin = child.transform.localPosition.ToBullet();
                _childTransform.SetOrientation(child.transform.rotation.ToBullet());
                _compoundShape.AddChildShape(_childTransform, child.GetCollisionShape());
            }

            //Assign the compound collider to inherited member "Shape"
            Shape = _compoundShape;
        }
Beispiel #7
0
        /// <summary>
        /// Adds a <see cref="CollisionShape"/> to the multi hull shape.
        /// </summary>
        /// <param name="hullShape"></param>
        /// <param name="offset"></param>
        public void AddShape(CollisionShape hullShape, BulletSharp.Math.Matrix offset)
        {
            if (hullShapes.Contains(hullShape))
            {
                return;
            }

            hullShapes.Add(hullShape);
            compoundShape.AddChildShape(offset, hullShape);
        }
Beispiel #8
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);
        }
        /// <summary>
        /// Adds a child shape.
        /// </summary>
        /// <param name="localTransform">The local transform.</param>
        /// <param name="shape">The shape.</param>
        public void AddChildShape(float4x4 localTransform, IBoxShapeImp shape)
        {
            Debug.WriteLine("AddBox");
            var btHalfExtents    = Translator.Float3ToBtVector3(shape.HalfExtents);
            var btChildShape     = new BoxShape(btHalfExtents);
            var btLocalTransform = Translator.Float4X4ToBtMatrix(localTransform);

            BtCompoundShape.AddChildShape(btLocalTransform, btChildShape);
        }
Beispiel #10
0
        void CreateGear(Vector3 pos, float speed)
        {
            Matrix        startTransform = Matrix.Translation(pos);
            CompoundShape shape          = new CompoundShape();

#if true
            shape.AddChildShape(Matrix.Identity, new BoxShape(5, 1, 6));
            shape.AddChildShape(Matrix.RotationZ((float)Math.PI), new BoxShape(5, 1, 6));
#else
            shape.AddChildShape(Matrix.Identity, new CylinderShapeZ(5, 1, 7));
            shape.AddChildShape(Matrix.RotationZ((float)Math.PI), new BoxShape(4, 1, 8));
#endif
            RigidBody body = LocalCreateRigidBody(10, startTransform, shape);
            body.Friction = 1;
            HingeConstraint hinge = new HingeConstraint(body, Matrix.Identity);
            if (speed != 0)
            {
                hinge.EnableAngularMotor(true, speed, 3);
            }
            World.AddConstraint(hinge);
        }
        protected override CollisionShape CreateShape()
        {
            CompoundShape shape = new CompoundShape();

            foreach (AbstractRigidShapeDefinition shapedef in this.children)
            {
                ShapeCustomData sc = new ShapeCustomData();
                sc.Id       = 0;
                sc.ShapeDef = shapedef;
                shape.AddChildShape((Matrix)shapedef.Pose, shapedef.GetShape(sc));
            }
            return(shape);
        }
Beispiel #12
0
 public void SetCollisionConvexMesh(Mesh mesh, Vector3 centerOfMass, Vector3 scale)
 {
     if (mesh == null)
     {
         Log.Error("Could not apply mesh to rigidbody! GameObject: " + GameObject.Name);
     }
     else
     {
         ConvexHullShape hull = new ConvexHullShape(mesh.GetPoints());
         CompoundShape   cs   = new CompoundShape();
         cs.AddChildShape(Matrix4.CreateTranslation(-centerOfMass), hull);
         SetCollisionShape(cs, centerOfMass, scale);
     }
 }
        protected override CollisionShape CreateShape()
        {
            CompoundShape shape = new CompoundShape();

            foreach (AbstractRigidShapeDefinition shapedef in this.children)
            {
                ShapeCustomData sc = new ShapeCustomData();
                sc.Id       = 0;
                sc.ShapeDef = shapedef;

                Matrix tr  = Matrix.Translation(shapedef.Translation);
                Matrix rot = Matrix.RotationQuaternion(shapedef.Rotation);

                shape.AddChildShape(Matrix.Multiply(rot, tr), shapedef.GetShape(sc));
            }
            return(shape);
        }
Beispiel #14
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);
        }
Beispiel #15
0
        /// <summary>
        /// Turns a BXDA mesh into a CompoundShape centered around the origin
        /// </summary>
        /// <param name="mesh"></param>
        /// <returns></returns>
        private static CompoundShape GetShape(BXDAMesh mesh)
        {
            CompoundShape shape = new CompoundShape();

            Vector3[] meshVertices = mesh.AllColliderVertices().ToArray();

            for (int i = 0; i < mesh.colliders.Count; i++)
            {
                BXDAMesh.BXDASubMesh  sub      = mesh.colliders[i];
                Vector3[]             vertices = sub.GetVertexData();
                StridingMeshInterface sMesh    = MeshUtilities.CenteredBulletShapeFromSubMesh(sub);

                //Add the shape at a location relative to the compound shape such that the compound shape is centered at (0, 0) but child shapes are properly placed
                shape.AddChildShape(Matrix4.CreateTranslation(MeshUtilities.MeshCenterRelative(sub, mesh)), new ConvexTriangleMeshShape(sMesh));
                Console.WriteLine("Successfully created and added sub shape");
            }

            return(shape);
        }
Beispiel #16
0
        protected virtual CompoundShape _CreateCompoundShape(bool copyChildren)
        {
            //TODO
            // some of the collider types (non-finite and other compound colliders) are probably not
            // can only be added to game object with rigid body attached.
            // allowed should check for these.
            // what about scaling not sure if it is handled correctly
            CompoundShape cs = new CompoundShape();

            CollisionShapeWithTransform[] collisionShapes = GetSubCollisionShapes(copyChildren);
            for (int i = 0; i < collisionShapes.Length; i++)
            {
                CollisionShape chcs = collisionShapes[i].Shape;

                Vector3 up      = Vector3.up;
                Vector3 origin  = Vector3.zero;
                Vector3 forward = Vector3.forward;
                //to world
                up      = collisionShapes[i].Transform.TransformDirection(up);
                origin  = collisionShapes[i].Transform.TransformPoint(origin);
                forward = collisionShapes[i].Transform.TransformDirection(forward);
                //to compound collider
                up      = transform.InverseTransformDirection(up);
                origin  = transform.InverseTransformPoint(origin);
                forward = transform.InverseTransformDirection(forward);
                Quaternion q = Quaternion.LookRotation(forward, up);

                /*
                 * Some collision shapes can have local scaling applied. Use
                 * btCollisionShape::setScaling(vector3).Non uniform scaling with different scaling
                 * values for each axis, can be used for btBoxShape, btMultiSphereShape,
                 * btConvexShape, btTriangleMeshShape.Note that a non - uniform scaled
                 * sphere can be created by using a btMultiSphereShape with 1 sphere.
                 */

                BulletSharp.Math.Matrix m = BulletSharp.Math.Matrix.AffineTransformation(1f, q.ToBullet(), origin.ToBullet());

                cs.AddChildShape(m, chcs);
            }
            cs.LocalScaling = m_localScaling.ToBullet();
            cs.Margin       = m_Margin;
            return(cs);
        }
Beispiel #17
0
        protected virtual CompoundShape _CreateCompoundShape(bool copyChildren)
        {
            //TODO
            // some of the collider types (non-finite and other compound colliders) are probably not
            // can only be added to game object with rigid body attached.
            // allowed should check for these.
            // what about scaling not sure if it is handled correctly
            CompoundShape cs = new CompoundShape();

            CollisionShapeWithTransform[] collisionShapes = GetSubCollisionShapes(copyChildren);
            for (int i = 0; i < collisionShapes.Length; i++)
            {
                CollisionShape chcs = collisionShapes[i].Shape;
                // we need to invert the scale
                BulletSharp.Math.Matrix m = (this.transform.worldToLocalMatrix * collisionShapes[i].Transform.localToWorldMatrix * Matrix4x4.Scale(collisionShapes[i].Transform.lossyScale).inverse).ToBullet();
                cs.AddChildShape(m, chcs);
            }
            cs.LocalScaling = m_localScaling.ToBullet();
            cs.Margin       = m_Margin;
            return(cs);
        }
Beispiel #18
0
        public Physics(VehicleDemo game)
        {
            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
                int vertStride  = Vector3.SizeInBytes;
                int indexStride = 3 * sizeof(int);

                const int NUM_VERTS_X = 20;
                const int NUM_VERTS_Y = 20;
                const int totalVerts  = NUM_VERTS_X * NUM_VERTS_Y;

                const int totalTriangles = 2 * (NUM_VERTS_X - 1) * (NUM_VERTS_Y - 1);

                TriangleIndexVertexArray vertexArray = new TriangleIndexVertexArray();
                IndexedMesh mesh = new IndexedMesh();
                mesh.Allocate(totalVerts, vertStride, totalTriangles, indexStride, PhyScalarType.Int32, PhyScalarType.Single);

                BulletSharp.DataStream data = mesh.LockVerts();
                for (i = 0; i < NUM_VERTS_X; i++)
                {
                    for (int j = 0; j < NUM_VERTS_Y; j++)
                    {
                        float wl     = .2f;
                        float height = 20.0f * (float)(Math.Sin(i * wl) * Math.Cos(j * wl));

                        data.Write((i - NUM_VERTS_X * 0.5f) * scale);
                        data.Write(height);
                        data.Write((j - NUM_VERTS_Y * 0.5f) * scale);
                    }
                }

                int      index = 0;
                IntArray idata = mesh.TriangleIndices;
                for (i = 0; i < NUM_VERTS_X - 1; i++)
                {
                    for (int j = 0; j < NUM_VERTS_Y - 1; j++)
                    {
                        idata[index++] = j * NUM_VERTS_X + i;
                        idata[index++] = j * NUM_VERTS_X + i + 1;
                        idata[index++] = (j + 1) * NUM_VERTS_X + i + 1;

                        idata[index++] = j * NUM_VERTS_X + i;
                        idata[index++] = (j + 1) * NUM_VERTS_X + i + 1;
                        idata[index++] = (j + 1) * NUM_VERTS_X + i;
                    }
                }

                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.Single;
                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.Single)
                        {
                            // heightScale isn't applied internally for float data
                            height = reader.ReadSingle();
                        }
                        else if (scalarType == PhyScalarType.Byte)
                        {
                            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
            RaycastVehicle.VehicleTuning tuning           = new RaycastVehicle.VehicleTuning();
            IVehicleRaycaster            vehicleRayCaster = new DefaultVehicleRaycaster(World);

            vehicle = new RaycastVehicle(tuning, carChassis, vehicleRayCaster);

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


            float connectionHeight = 1.2f;
            bool  isFrontWheel     = true;

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

            Vector3   connectionPointCS0 = new Vector3(CUBE_HALF_EXTENTS - (0.3f * wheelWidth), connectionHeight, 2 * CUBE_HALF_EXTENTS - wheelRadius);
            WheelInfo a = 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;
        }
Beispiel #19
0
        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();
            }
        }
Beispiel #20
0
        public Character(DiscreteDynamicsWorld world, Vehiculo vehiculo, TGCVector3 position, float rotation, GameModel gameModel)
        {
            //Cargar sonido
            turboSound = new TgcStaticSound();
            turboSound.loadSound(Game.Default.MediaDirectory + Game.Default.FXDirectory + "turbo.wav", gameModel.DirectSound.DsDevice);

            this.vehiculo = vehiculo;

            var loader = new TgcSceneLoader();

            this.mesh  = loader.loadSceneFromFile(vehiculo.ChassisXmlPath).Meshes[0];
            this.wheel = loader.loadSceneFromFile(vehiculo.WheelsXmlPath).Meshes[0];

            Vehiculo.ChangeTextureColor(this.mesh, vehiculo.Color);

            this.mesh.AutoTransform  = false;
            this.wheel.AutoTransform = false;

            maxHitPoints         = float.Parse(mesh.UserProperties["maxHitPoints"], CultureInfo.InvariantCulture);
            engineForce          = -float.Parse(mesh.UserProperties["engineForce"], CultureInfo.InvariantCulture);
            brakeForce           = float.Parse(mesh.UserProperties["brakeForce"], CultureInfo.InvariantCulture);
            steeringAngle        = -float.Parse(mesh.UserProperties["steeringAngle"], CultureInfo.InvariantCulture);
            turboImpulse         = float.Parse(mesh.UserProperties["turboImpulse"], CultureInfo.InvariantCulture);
            frictionSlip         = float.Parse(mesh.UserProperties["frictionSlip"], CultureInfo.InvariantCulture);
            rollInfluence        = float.Parse(mesh.UserProperties["rollInfluence"], CultureInfo.InvariantCulture);
            rearWheelsHeight     = float.Parse(mesh.UserProperties["rearWheelsHeight"], CultureInfo.InvariantCulture);
            frontWheelsHeight    = float.Parse(mesh.UserProperties["frontWheelsHeight"], CultureInfo.InvariantCulture);
            suspensionRestLength = float.Parse(mesh.UserProperties["suspensionRestLength"], CultureInfo.InvariantCulture);
            suspensionStiffness  = float.Parse(mesh.UserProperties["suspensionStiffness"], CultureInfo.InvariantCulture);
            dampingCompression   = float.Parse(mesh.UserProperties["dampingCompression"], CultureInfo.InvariantCulture);
            dampingRelaxation    = float.Parse(mesh.UserProperties["dampingRelaxation"], CultureInfo.InvariantCulture);

            meshAxisRadius = this.mesh.BoundingBox.calculateAxisRadius().ToBsVector;
            var wheelRadius = this.wheel.BoundingBox.calculateAxisRadius().Y;

            //The btBoxShape is centered at the origin
            CollisionShape chassisShape = new BoxShape(meshAxisRadius.X, meshRealHeight, meshAxisRadius.Z);

            //A compound shape is used so we can easily shift the center of gravity of our vehicle to its bottom
            //This is needed to make our vehicle more stable
            CompoundShape compound = new CompoundShape();

            //The center of gravity of the compound shape is the origin. When we add a rigidbody to the compound shape
            //it's center of gravity does not change. This way we can add the chassis rigidbody one unit above our center of gravity
            //keeping it under our chassis, and not in the middle of it
            var localTransform = Matrix.Translation(0, (meshAxisRadius.Y * 1.75f) - (meshRealHeight / 2f), 0);

            compound.AddChildShape(localTransform, chassisShape);
            //Creates a rigid body
            this.rigidBody = CreateChassisRigidBodyFromShape(compound, position, rotation);

            //Adds the vehicle chassis to the world
            world.AddRigidBody(this.rigidBody);
            worldID = world.CollisionObjectArray.IndexOf(this.rigidBody);

            //RaycastVehicle
            DefaultVehicleRaycaster vehicleRayCaster = new DefaultVehicleRaycaster(world);
            VehicleTuning           tuning           = new VehicleTuning();

            //Creates a new instance of the raycast vehicle
            vehicle = new RaycastVehicle(tuning, this.rigidBody, vehicleRayCaster);

            //Never deactivate the vehicle
            this.rigidBody.ActivationState = ActivationState.DisableDeactivation;

            //Adds the vehicle to the world
            world.AddAction(vehicle);

            //Adds the wheels to the vehicle
            AddWheels(meshAxisRadius, vehicle, tuning, wheelRadius);

            //Inicializo puntos
            hitPoints       = maxHitPoints;
            specialPoints   = maxSpecialPoints;
            timerMachineGun = 0f;
        }
        //----------------------------------------------------------------------------------------------------------------

        public override void InitializeDemo()
        {
            CollisionShape groundShape = new BoxShape(new IndexedVector3(50, 3, 50));

            //CollisionShape groundShape = new StaticPlaneShape(IndexedVector3.Up, 0f);


            m_collisionShapes.Add(groundShape);
            m_collisionConfiguration = new DefaultCollisionConfiguration();
            m_dispatcher             = new CollisionDispatcher(m_collisionConfiguration);
            IndexedVector3 worldMin = new IndexedVector3(-1000, -1000, -1000);
            IndexedVector3 worldMax = new IndexedVector3(1000, 1000, 1000);

            //m_broadphase = new AxisSweep3Internal(ref worldMin, ref worldMax, 0xfffe, 0xffff, 16384, null, false);
            m_broadphase = new SimpleBroadphase(100, null);

            m_constraintSolver = new SequentialImpulseConstraintSolver();
            m_dynamicsWorld    = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);

            //m_dynamicsWorld.setGravity(new IndexedVector3(0,0,0));
            IndexedMatrix tr = IndexedMatrix.CreateTranslation(0, -10, 0);

            //either use heightfield or triangle mesh

            //create ground object
            LocalCreateRigidBody(0f, ref tr, groundShape);

            CollisionShape chassisShape = new BoxShape(new IndexedVector3(1.0f, 0.5f, 2.0f));

            m_collisionShapes.Add(chassisShape);

            CompoundShape compound = new CompoundShape();

            m_collisionShapes.Add(compound);
            //localTrans effectively shifts the center of mass with respect to the chassis
            IndexedMatrix localTrans = IndexedMatrix.CreateTranslation(0, 1, 0);

            compound.AddChildShape(ref localTrans, chassisShape);

            {
                CollisionShape suppShape = new BoxShape(new IndexedVector3(0.5f, 0.1f, 0.5f));
                //localTrans effectively shifts the center of mass with respect to the chassis
                IndexedMatrix suppLocalTrans = IndexedMatrix.CreateTranslation(0f, 1.0f, 2.5f);
                compound.AddChildShape(ref suppLocalTrans, suppShape);
            }

            tr._origin = IndexedVector3.Zero;

            m_carChassis = LocalCreateRigidBody(800f, ref tr, compound);//chassisShape);
            //m_carChassis = LocalCreateRigidBody(800f, ref tr, chassisShape);//chassisShape);
            //CollisionShape liftShape = new BoxShape(new IndexedVector3(0.5f, 2.0f, 0.05f));
            //m_collisionShapes.Add(liftShape);
            //m_liftStartPos = new IndexedVector3(0.0f, 2.5f, 3.05f);

            //IndexedMatrix liftTrans = IndexedMatrix.CreateTranslation(m_liftStartPos);
            //m_liftBody = LocalCreateRigidBody(10f, ref liftTrans, liftShape);

            //IndexedMatrix localA = MathUtil.SetEulerZYX(0f, MathUtil.SIMD_HALF_PI, 0f);
            //localA._origin = new IndexedVector3(0f, 1.0f, 3.05f);

            //IndexedMatrix localB = MathUtil.SetEulerZYX(0f, MathUtil.SIMD_HALF_PI, 0f);
            //localB._origin = new IndexedVector3(0f, -1.5f, -0.05f);

            //m_liftHinge = new HingeConstraint(m_carChassis, m_liftBody, ref localA, ref localB);
            ////		m_liftHinge.setLimit(-LIFT_EPS, LIFT_EPS);
            //m_liftHinge.SetLimit(0.0f, 0.0f);
            //m_dynamicsWorld.AddConstraint(m_liftHinge, true);


            //CompoundShape forkCompound = new CompoundShape();
            //m_collisionShapes.Add(forkCompound);

            //IndexedMatrix forkLocalTrans = IndexedMatrix.Identity;
            //CollisionShape forkShapeA = new BoxShape(new IndexedVector3(1.0f, 0.1f, 0.1f));
            //m_collisionShapes.Add(forkShapeA);
            //forkCompound.AddChildShape(ref forkLocalTrans, forkShapeA);

            //CollisionShape forkShapeB = new BoxShape(new IndexedVector3(0.1f, 0.02f, 0.6f));
            //m_collisionShapes.Add(forkShapeB);
            //forkLocalTrans = IndexedMatrix.CreateTranslation(-0.9f, -0.08f, 0.7f);
            //forkCompound.AddChildShape(ref forkLocalTrans, forkShapeB);

            //CollisionShape forkShapeC = new BoxShape(new IndexedVector3(0.1f, 0.02f, 0.6f));
            //m_collisionShapes.Add(forkShapeC);
            //forkLocalTrans = IndexedMatrix.CreateTranslation(0.9f, -0.08f, 0.7f);
            //forkCompound.AddChildShape(ref forkLocalTrans, forkShapeC);

            //m_forkStartPos = new IndexedVector3(0.0f, 0.6f, 3.2f);
            //IndexedMatrix forkTrans = IndexedMatrix.CreateTranslation(m_forkStartPos);

            //m_forkBody = LocalCreateRigidBody(5f, ref forkTrans, forkCompound);

            //localA = MathUtil.SetEulerZYX(0f, 0f, MathUtil.SIMD_HALF_PI);
            //localA._origin = new IndexedVector3(0.0f, -1.9f, 0.05f);

            //IndexedVector3 col0 = MathUtil.matrixColumn(ref localA, 0);
            //IndexedVector3 col1 = MathUtil.matrixColumn(ref localA, 1);
            //IndexedVector3 col2 = MathUtil.matrixColumn(ref localA, 2);



            ////localB = MathUtil.setEulerZYX(0f, 0f, MathUtil.SIMD_HALF_PI);
            //localB = MathUtil.SetEulerZYX(0f, 0f, MathUtil.SIMD_HALF_PI);
            //localB._origin = new IndexedVector3(0.0f, 0.0f, -0.1f);

            //m_forkSlider = new SliderConstraint(m_liftBody, m_forkBody, ref localA, ref localB, true);

            //m_forkSlider.SetLowerLinLimit(0.1f);
            //m_forkSlider.SetUpperLinLimit(0.1f);
            ////		m_forkSlider.setLowerAngLimit(-LIFT_EPS);
            ////		m_forkSlider.setUpperAngLimit(LIFT_EPS);
            //m_forkSlider.SetLowerAngLimit(0.0f);
            //m_forkSlider.SetUpperAngLimit(0.0f);

            //IndexedMatrix localAVec = IndexedMatrix.Identity;
            //IndexedMatrix localBVec = IndexedMatrix.Identity;

            //m_forkSlider2 = new HingeConstraint(m_liftBody, m_forkBody, ref localAVec, ref localBVec);
            //m_dynamicsWorld.AddConstraint(m_forkSlider, true);
            //m_dynamicsWorld.addConstraint(m_forkSlider2, true);


            CompoundShape loadCompound = new CompoundShape(true);

            m_collisionShapes.Add(loadCompound);
            CollisionShape loadShapeA = new BoxShape(new IndexedVector3(2.0f, 0.5f, 0.5f));

            m_collisionShapes.Add(loadShapeA);
            IndexedMatrix loadTrans = IndexedMatrix.Identity;

            loadCompound.AddChildShape(ref loadTrans, loadShapeA);
            CollisionShape loadShapeB = new BoxShape(new IndexedVector3(0.1f, 1.0f, 1.0f));

            m_collisionShapes.Add(loadShapeB);
            loadTrans = IndexedMatrix.CreateTranslation(2.1f, 0.0f, 0.0f);
            loadCompound.AddChildShape(ref loadTrans, loadShapeB);
            CollisionShape loadShapeC = new BoxShape(new IndexedVector3(0.1f, 1.0f, 1.0f));

            m_collisionShapes.Add(loadShapeC);
            loadTrans = IndexedMatrix.CreateTranslation(-2.1f, 0.0f, 0.0f);
            loadCompound.AddChildShape(ref loadTrans, loadShapeC);
            m_loadStartPos = new IndexedVector3(0.0f, -3.5f, 7.0f);
            loadTrans      = IndexedMatrix.CreateTranslation(m_loadStartPos);

            m_loadBody = LocalCreateRigidBody(4f, ref loadTrans, loadCompound);


#if false
            {
                CollisionShape liftShape = new BoxShape(new IndexedVector3(0.5f, 2.0f, 0.05f));
                m_collisionShapes.Add(liftShape);
                IndexedMatrix liftTrans = IndexedMatrix.CreateTranslation(m_liftStartPos);
                m_liftBody = localCreateRigidBody(10f, ref liftTrans, liftShape);

                IndexedMatrix localA = MathUtil.setEulerZYX(0f, MathUtil.SIMD_HALF_PI, 0f);
                localA._origin = new IndexedVector3(0f, 1.0f, 3.05f);

                IndexedMatrix localB = MathUtil.setEulerZYX(0f, MathUtil.SIMD_HALF_PI, 0f);
                localB._origin = new IndexedVector3(0f, -1.5f, -0.05f);

                m_liftHinge = new HingeConstraint(m_carChassis, m_liftBody, ref localA, ref localB);
                //		m_liftHinge.setLimit(-LIFT_EPS, LIFT_EPS);
                m_liftHinge.setLimit(0.0f, 0.0f);
                m_dynamicsWorld.addConstraint(m_liftHinge, true);

                CollisionShape forkShapeA = new BoxShape(new IndexedVector3(1.0f, 0.1f, 0.1f));
                m_collisionShapes.Add(forkShapeA);
                CompoundShape forkCompound = new CompoundShape();
                m_collisionShapes.Add(forkCompound);
                IndexedMatrix forkLocalTrans = IndexedMatrix.Identity;
                forkCompound.addChildShape(ref forkLocalTrans, forkShapeA);

                CollisionShape forkShapeB = new BoxShape(new IndexedVector3(0.1f, 0.02f, 0.6f));
                m_collisionShapes.Add(forkShapeB);
                forkLocalTrans = IndexedMatrix.CreateTranslation(-0.9f, -0.08f, 0.7f);
                forkCompound.addChildShape(ref forkLocalTrans, forkShapeB);

                CollisionShape forkShapeC = new BoxShape(new IndexedVector3(0.1f, 0.02f, 0.6f));
                m_collisionShapes.Add(forkShapeC);
                forkLocalTrans = IndexedMatrix.CreateTranslation(0.9f, -0.08f, 0.7f);
                forkCompound.addChildShape(ref forkLocalTrans, forkShapeC);

                m_forkStartPos = new IndexedVector3(0.0f, 0.6f, 3.2f);
                IndexedMatrix forkTrans = IndexedMatrix.CreateTranslation(m_forkStartPos);

                m_forkBody = localCreateRigidBody(5f, ref forkTrans, forkCompound);

                localA         = MathUtil.setEulerZYX(0f, 0f, MathUtil.SIMD_HALF_PI);
                localA._origin = new IndexedVector3(0.0f, -1.9f, 0.05f);

                localB         = MathUtil.setEulerZYX(0f, 0f, MathUtil.SIMD_HALF_PI);
                localB._origin = new IndexedVector3(0.0f, 0.0f, -0.1f);

                m_forkSlider = new SliderConstraint(m_liftBody, m_forkBody, ref localA, ref localB, true);
                m_forkSlider.setLowerLinLimit(0.1f);
                m_forkSlider.setUpperLinLimit(0.1f);
                //		m_forkSlider.setLowerAngLimit(-LIFT_EPS);
                //		m_forkSlider.setUpperAngLimit(LIFT_EPS);
                m_forkSlider.setLowerAngLimit(0.0f);
                m_forkSlider.setUpperAngLimit(0.0f);
                m_dynamicsWorld.addConstraint(m_forkSlider, true);


                CompoundShape loadCompound = new CompoundShape();
                m_collisionShapes.Add(loadCompound);
                CollisionShape loadShapeA = new BoxShape(new IndexedVector3(2.0f, 0.5f, 0.5f));
                m_collisionShapes.Add(loadShapeA);
                IndexedMatrix loadTrans = IndexedMatrix.Identity;
                loadCompound.addChildShape(ref loadTrans, loadShapeA);
                CollisionShape loadShapeB = new BoxShape(new IndexedVector3(0.1f, 1.0f, 1.0f));
                m_collisionShapes.Add(loadShapeB);
                loadTrans = IndexedMatrix.CreateTranslation(2.1f, 0.0f, 0.0f);
                loadCompound.addChildShape(ref loadTrans, loadShapeB);
                CollisionShape loadShapeC = new BoxShape(new IndexedVector3(0.1f, 1.0f, 1.0f));
                m_collisionShapes.Add(loadShapeC);
                loadTrans = IndexedMatrix.CreateTranslation(-2.1f, 0.0f, 0.0f);
                loadCompound.addChildShape(ref loadTrans, loadShapeC);
                m_loadStartPos = new IndexedVector3(0.0f, -3.5f, 7.0f);
                loadTrans      = IndexedMatrix.CreateTranslation(m_loadStartPos);

                m_loadBody = localCreateRigidBody(4f, ref loadTrans, loadCompound);
            }
#endif
            //m_carChassis.setDamping(0.2f, 0.2f);

            ClientResetScene();

            /// create vehicle

            SetCameraDistance(26.0f);
            SetTexturing(true);
            SetShadows(true);
        }
        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
        }
Beispiel #23
0
        private void CreateVehicle(Matrix transform)
        {
            var chassisShape = new BoxShape(1.0f, 0.5f, 2.0f);

            var compound = new CompoundShape();

            //localTrans effectively shifts the center of mass with respect to the chassis
            Matrix localTrans = Matrix.Translation(Vector3.UnitY);

            compound.AddChildShape(localTrans, chassisShape);
            RigidBody carChassis = PhysicsHelper.CreateBody(800, Matrix.Identity, compound, World);

            carChassis.UserObject = "Chassis";
            //carChassis.SetDamping(0.2f, 0.2f);

            var tuning           = new VehicleTuning();
            var 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;

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

            Vector3 wheelDirection = Vector3.Zero;
            Vector3 wheelAxle      = Vector3.Zero;

            wheelDirection[upIndex] = -1;
            wheelAxle[rightIndex]   = -1;

            bool isFrontWheel    = true;
            var  connectionPoint = new Vector3(CUBE_HALF_EXTENTS - (0.3f * wheelWidth), connectionHeight, 2 * CUBE_HALF_EXTENTS - wheelRadius);

            _vehicle.AddWheel(connectionPoint, wheelDirection, wheelAxle, suspensionRestLength, wheelRadius, tuning, isFrontWheel);

            connectionPoint = new Vector3(-CUBE_HALF_EXTENTS + (0.3f * wheelWidth), connectionHeight, 2 * CUBE_HALF_EXTENTS - wheelRadius);
            _vehicle.AddWheel(connectionPoint, wheelDirection, wheelAxle, suspensionRestLength, wheelRadius, tuning, isFrontWheel);

            isFrontWheel    = false;
            connectionPoint = new Vector3(-CUBE_HALF_EXTENTS + (0.3f * wheelWidth), connectionHeight, -2 * CUBE_HALF_EXTENTS + wheelRadius);
            _vehicle.AddWheel(connectionPoint, wheelDirection, wheelAxle, suspensionRestLength, wheelRadius, tuning, isFrontWheel);

            connectionPoint = new Vector3(CUBE_HALF_EXTENTS - (0.3f * wheelWidth), connectionHeight, -2 * CUBE_HALF_EXTENTS + wheelRadius);
            _vehicle.AddWheel(connectionPoint, wheelDirection, wheelAxle, suspensionRestLength, wheelRadius, tuning, isFrontWheel);


            for (int 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 = transform;
        }
        CompoundShape _CreateCompoundShape(bool copyChildren)
        {
            BCollisionShape[] css = GetComponentsInChildren <BCollisionShape>();
            colliders = new BCollisionShape[css.Length - 1];
            int ii = 0;

            for (int i = 0; i < css.Length; i++)
            {
                if (css[i] == this)
                {
                    //skip
                }
                else
                {
                    colliders[ii] = css[i];
                    ii++;
                }
            }
            if (colliders.Length == 0)
            {
                Debug.LogError("Compound collider");
            }

            //TODO
            // some of the collider types (non-finite and other compound colliders) are probably not
            // can only be added to game object with rigid body attached.
            // allowed should check for these.
            // what about scaling not sure if it is handled correctly
            CompoundShape cs = new CompoundShape();

            for (int i = 0; i < colliders.Length; i++)
            {
                CollisionShape chcs;
                if (copyChildren == true)
                {
                    chcs = colliders[i].CopyCollisionShape();
                }
                else
                {
                    chcs = colliders[i].GetCollisionShape();
                }

                Vector3 up      = Vector3.up;
                Vector3 origin  = Vector3.zero;
                Vector3 forward = Vector3.forward;
                //to world
                up      = colliders[i].transform.TransformDirection(up);
                origin  = colliders[i].transform.TransformPoint(origin);
                forward = colliders[i].transform.TransformDirection(forward);
                //to compound collider
                up      = transform.InverseTransformDirection(up);
                origin  = transform.InverseTransformPoint(origin);
                forward = transform.InverseTransformDirection(forward);
                Quaternion q = Quaternion.LookRotation(forward, up);

                /*
                 * Some collision shapes can have local scaling applied. Use
                 * btCollisionShape::setScaling(vector3).Non uniform scaling with different scaling
                 * values for each axis, can be used for btBoxShape, btMultiSphereShape,
                 * btConvexShape, btTriangleMeshShape.Note that a non - uniform scaled
                 * sphere can be created by using a btMultiSphereShape with 1 sphere.
                 */

                BulletSharp.Math.Matrix m = BulletSharp.Math.Matrix.AffineTransformation(1f, q.ToBullet(), origin.ToBullet());

                cs.AddChildShape(m, chcs);
            }
            cs.LocalScaling = m_localScaling.ToBullet();
            return(cs);
        }
Beispiel #25
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
        }
Beispiel #26
0
        public override void InitializeDemo()
        {
            m_cameraDistance = 10.0f;
            //string filename = @"e:\users\man\bullet\gimpact-demo-xna.txt";
            //FileStream filestream = File.Open(filename, FileMode.Create, FileAccess.Write, FileShare.Read);
            //BulletGlobals.g_streamWriter = new StreamWriter(filestream);

            /// Init Bullet
            m_collisionConfiguration = new DefaultCollisionConfiguration();

            m_dispatcher = new CollisionDispatcher(m_collisionConfiguration);
            //btOverlappingPairCache* broadphase = new btSimpleBroadphase();
            //m_broadphase = new btSimpleBroadphase();

            int            maxProxies   = 1024;
            IndexedVector3 worldAabbMin = new IndexedVector3(-10000, -10000, -10000);
            IndexedVector3 worldAabbMax = new IndexedVector3(10000, 10000, 10000);

            //m_broadphase = new AxisSweep3Internal(ref worldAabbMin, ref worldAabbMax, 0xfffe, 0xffff, 16384, null, false);
            m_broadphase       = new SimpleBroadphase(16384, null);
            m_constraintSolver = new SequentialImpulseConstraintSolver();

            m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);

            //create trimesh model and shape
            InitGImpactCollision();



            /// Create Scene
            float         mass           = 0.0f;
            IndexedMatrix startTransform = IndexedMatrix.Identity;


            CollisionShape staticboxShape1 = new BoxShape(new IndexedVector3(200, 1, 200));//floor

            staticboxShape1.SetUserPointer("Floor");
            CollisionShape staticboxShape2 = new BoxShape(new IndexedVector3(1, 50, 200));//left wall

            staticboxShape1.SetUserPointer("LeftWall");
            CollisionShape staticboxShape3 = new BoxShape(new IndexedVector3(1, 50, 200));//right wall

            staticboxShape1.SetUserPointer("RightWall");
            CollisionShape staticboxShape4 = new BoxShape(new IndexedVector3(200, 50, 1));//front wall

            staticboxShape1.SetUserPointer("FrontWall");
            CollisionShape staticboxShape5 = new BoxShape(new IndexedVector3(200, 50, 1));//back wall

            staticboxShape1.SetUserPointer("BackWall");

            CompoundShape staticScenario = new CompoundShape();//static scenario

            startTransform._origin = new IndexedVector3(0, 0, 0);
            staticScenario.AddChildShape(ref startTransform, staticboxShape1);
            startTransform._origin = new IndexedVector3(-200, 25, 0);
            staticScenario.AddChildShape(ref startTransform, staticboxShape2);
            startTransform._origin = new IndexedVector3(200, 25, 0);
            staticScenario.AddChildShape(ref startTransform, staticboxShape3);
            startTransform._origin = new IndexedVector3(0, 25, 200);
            staticScenario.AddChildShape(ref startTransform, staticboxShape4);
            startTransform._origin = new IndexedVector3(0, 25, -200);
            staticScenario.AddChildShape(ref startTransform, staticboxShape5);

            startTransform._origin = new IndexedVector3(0, 0, 0);

            //RigidBody staticBody = LocalCreateRigidBody(mass, startTransform, staticScenario);
            RigidBody staticBody = LocalCreateRigidBody(mass, startTransform, staticboxShape1);


            staticBody.SetCollisionFlags(staticBody.GetCollisionFlags() | CollisionFlags.CF_STATIC_OBJECT);

            //enable custom material callback
            staticBody.SetCollisionFlags(staticBody.GetCollisionFlags() | CollisionFlags.CF_CUSTOM_MATERIAL_CALLBACK);

            //static plane
            IndexedVector3 normal = new IndexedVector3(0.4f, 1.5f, -0.4f);

            normal.Normalize();
            CollisionShape staticplaneShape6 = new StaticPlaneShape(ref normal, 0.0f);   // A plane

            startTransform._origin = IndexedVector3.Zero;

            RigidBody staticBody2 = LocalCreateRigidBody(mass, startTransform, staticplaneShape6);

            staticBody2.SetCollisionFlags(staticBody2.GetCollisionFlags() | CollisionFlags.CF_STATIC_OBJECT);

            startTransform = IndexedMatrix.Identity;

            /// Create Dynamic Boxes
            {
                int numBoxes = 1;
                for (int i = 0; i < numBoxes; i++)
                {
                    CollisionShape boxShape = new BoxShape(new IndexedVector3(1, 1, 1));
                    //CollisionShape mesh = new BvhTriangleMeshShape(m_indexVertexArrays2,true,true);
                    startTransform._origin = new IndexedVector3(2 * i - (numBoxes - 1), 2, -3);
                    //startTransform._origin = new IndexedVector3(2 * i - 5, 10, -3);
                    //LocalCreateRigidBody(1, startTransform, m_trimeshShape2);
                    LocalCreateRigidBody(1, startTransform, boxShape);
                }
            }
        }
Beispiel #27
0
        public void Evaluate(int SpreadMax)
        {
            IRigidBulletWorld inputWorld = this.worldInput[0];

            SpreadMax = 1;

            if (inputWorld != null)
            {
                this.persistedList.UpdateWorld(inputWorld);

                if (this.chassisShape.IsConnected)
                {
                    for (int i = 0; i < SpreadMax; i++)
                    {
                        if (this.doCreate[i])
                        {
                            RigidBodyPose       initialPose = this.initialPoseInput.IsConnected ? this.initialPoseInput[i] : RigidBodyPose.Default;
                            RigidBodyProperties properties  = this.initialProperties.IsConnected ? this.initialProperties[i] : RigidBodyProperties.Default;

                            ShapeCustomData            shapeData = new ShapeCustomData();
                            DynamicShapeDefinitionBase chassisShapeDefinition = this.chassisShape[i];

                            CollisionShape chassisShape = chassisShapeDefinition.GetShape(shapeData);
                            shapeData.ShapeDef = chassisShapeDefinition;

                            RaycastVehicle vehicle;
                            CompoundShape  compoundShape = new CompoundShape();

                            Matrix localTrans = Matrix.Translation(Vector3.UnitY);
                            compoundShape.AddChildShape(localTrans, chassisShape);

                            //Build mass for dynamic object
                            Vector3 localInertia = Vector3.Zero;
                            if (chassisShapeDefinition.Mass > 0.0f)
                            {
                                compoundShape.CalculateLocalInertia(chassisShapeDefinition.Mass, out localInertia);
                            }

                            Tuple <RigidBody, int> createBodyResult = inputWorld.CreateRigidBody(chassisShape, ref initialPose, ref properties, ref localInertia, chassisShapeDefinition.Mass, this.customString[i]);
                            RigidBody carChassis = createBodyResult.Item1;

                            RaycastVehicle.VehicleTuning tuning           = new RaycastVehicle.VehicleTuning();
                            DefaultVehicleRaycaster      vehicleRayCaster = new DefaultVehicleRaycaster(inputWorld.World);
                            vehicle = new RaycastVehicle(tuning, carChassis, vehicleRayCaster);
                            vehicle.SetCoordinateSystem(rightIndex, upIndex, forwardIndex);

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

                            int wheelCount = this.wheelConstruction.SliceCount;

                            //Add wheels
                            for (int j = 0; j < this.wheelConstruction[i].SliceCount; j++)
                            {
                                WheelConstructionProperties wcs = this.wheelConstruction[i][j];
                                Vector3   connectionPointCS0    = wcs.localPosition.ToBulletVector();
                                WheelInfo wheel = vehicle.AddWheel(connectionPointCS0, wcs.wheelDirection.ToBulletVector(), wcs.wheelAxis.ToBulletVector(), wcs.SuspensionRestLength, wcs.WheelRadius, tuning, wcs.isFrontWheel);
                            }

                            //Set Wheel Properties
                            WheelProperties wis = this.wheelInfoSettings[i] != null ? this.wheelInfoSettings[i] : new WheelProperties();
                            for (int j = 0; j < vehicle.NumWheels; j++)
                            {
                                WheelInfo wheel = vehicle.GetWheelInfo(j);
                                wheel.SuspensionStiffness      = wis.SuspensionStiffness;
                                wheel.WheelsDampingRelaxation  = wis.WheelsDampingRelaxation;
                                wheel.WheelsDampingCompression = wis.WheelsDampingCompression;
                                wheel.FrictionSlip             = wis.FrictionSlip;
                                wheel.RollInfluence            = wis.RollInfluence;
                            }

                            BodyCustomData bd = (BodyCustomData)carChassis.UserObject;
                            bd.Vehicle = vehicle;

                            this.persistedList.Append(createBodyResult.Item1, createBodyResult.Item2);
                        }
                    }


                    List <RigidBody> bodies = this.persistedList.Bodies;
                    this.vehicleOutput.SliceCount = bodies.Count;
                    this.chassisOutput.SliceCount = bodies.Count;
                    for (int i = 0; i < bodies.Count; i++)
                    {
                        BodyCustomData bd = (BodyCustomData)bodies[i].UserObject;
                        this.vehicleOutput[i] = bd.Vehicle;
                        this.chassisOutput[i] = bodies[i];
                    }
                }
            }
            else
            {
                this.vehicleOutput.SliceCount = 0;
                this.chassisOutput.SliceCount = 0;
            }
        }
Beispiel #28
0
        public override void Evaluate(int SpreadMax)
        {
            for (int i = 0; i < SpreadMax; i++)
            {
                if (this.CanCreate(i))
                {
                    wheelRadius       = FwheelRadius[0];
                    wheelWidth        = FwheelWidth[0];
                    CUBE_HALF_EXTENTS = FwheelDistance[0];

                    RaycastVehicle vehicle;

                    AbstractRigidShapeDefinition shapedef = this.FShapes[i];
                    ShapeCustomData sc = new ShapeCustomData();
                    sc.ShapeDef = shapedef;


                    CompoundShape compound = new CompoundShape();



                    CollisionShape chassisShape = shapedef.GetShape(sc);
                    Matrix         localTrans   = Matrix.Translation(Vector3.UnitY);
                    compound.AddChildShape(localTrans, chassisShape);

                    float mass = shapedef.Mass;

                    bool isDynamic = (mass != 0.0f);
                    isFrontWheel = true;

                    Vector3 localInertia = Vector3.Zero;
                    if (isDynamic)
                    {
                        chassisShape.CalculateLocalInertia(mass, out localInertia);
                    }

                    Vector3D pos = this.FPosition[i];
                    Vector4D rot = this.FRotation[i];

                    DefaultMotionState ms = BulletUtils.CreateMotionState(pos.x, pos.y, pos.z, rot.x, rot.y, rot.z, rot.w);


                    RigidBodyConstructionInfo rbInfo = new RigidBodyConstructionInfo(mass, ms, compound, localInertia);
                    RigidBody carChassis             = new RigidBody(rbInfo);

                    BodyCustomData bd = new BodyCustomData();

                    carChassis.UserObject = bd;
                    bd.Id     = this.FWorld[0].GetNewBodyId();
                    bd.Custom = this.FCustom[i];

                    this.FWorld[0].Register(carChassis);


                    RaycastVehicle.VehicleTuning tuning           = new RaycastVehicle.VehicleTuning();
                    VehicleRaycaster             vehicleRayCaster = new DefaultVehicleRaycaster(this.FWorld[0].World);
                    vehicle = new RaycastVehicle(tuning, carChassis, vehicleRayCaster);



                    carChassis.ActivationState = ActivationState.DisableDeactivation;
                    this.FWorld[0].World.AddAction(vehicle);



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

                    Vector3   connectionPointCS0 = new Vector3(CUBE_HALF_EXTENTS - (0.3f * wheelWidth), FconnectionHeight[0], 2 * CUBE_HALF_EXTENTS - wheelRadius);
                    WheelInfo a = vehicle.AddWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, FsuspensionRestLength[0], wheelRadius, tuning, isFrontWheel);

                    connectionPointCS0 = new Vector3(-CUBE_HALF_EXTENTS + (0.3f * wheelWidth), FconnectionHeight[0], 2 * CUBE_HALF_EXTENTS - wheelRadius);
                    vehicle.AddWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, FsuspensionRestLength[0], wheelRadius, tuning, isFrontWheel);

                    isFrontWheel       = false;
                    connectionPointCS0 = new Vector3(-CUBE_HALF_EXTENTS + (0.3f * wheelWidth), FconnectionHeight[0], -2 * CUBE_HALF_EXTENTS + wheelRadius);
                    vehicle.AddWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, FsuspensionRestLength[0], wheelRadius, tuning, isFrontWheel);

                    connectionPointCS0 = new Vector3(CUBE_HALF_EXTENTS - (0.3f * wheelWidth), FconnectionHeight[0], -2 * CUBE_HALF_EXTENTS + wheelRadius);
                    vehicle.AddWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, FsuspensionRestLength[0], wheelRadius, tuning, isFrontWheel);


                    for (i = 0; i < vehicle.NumWheels; i++)
                    {
                        WheelInfo wheel = vehicle.GetWheelInfo(i);
                        wheel.SuspensionStiffness     = FsuspensionStiffness[0];
                        wheel.WheelDampingRelaxation  = FDampingRelaxation[0];
                        wheel.WheelDampingCompression = FDampingCompression[0];
                        wheel.FrictionSlip            = FwheelFriction[0];
                        wheel.RollInfluence           = FrollInfluence[0];
                        wheel.MaxSuspensionTravelCm   = FmaxSuspensionTravelCm[0];
                        wheel.MaxSuspensionForce      = FmaxSuspensionForce[0];
                    }

                    FOutVehicle.SliceCount = 1;
                    FOutVehicle[0]         = vehicle;
                }
            }
        }
Beispiel #29
0
        protected override void OnInitializePhysics()
        {
            SetupEmptyDynamicsWorld();

            CollisionShape groundShape = new BoxShape(50, 1, 50);

            //CollisionShape groundShape = new StaticPlaneShape(Vector3.UnitY, 40);
            CollisionShapes.Add(groundShape);
            RigidBody body = LocalCreateRigidBody(0, Matrix.Translation(0, -16, 0), groundShape);

            body.UserObject = "Ground";

            CollisionShape shape = new BoxShape(new Vector3(CubeHalfExtents));

            CollisionShapes.Add(shape);


            const float THETA = (float)Math.PI / 4.0f;
            float       L_1   = 2 - (float)Math.Tan(THETA);
            float       L_2   = 1 / (float)Math.Cos(THETA);
            float       RATIO = L_2 / L_1;

            RigidBody bodyA;
            RigidBody bodyB;

            CollisionShape cylA = new CylinderShape(0.2f, 0.25f, 0.2f);
            CollisionShape cylB = new CylinderShape(L_1, 0.025f, L_1);
            CompoundShape  cyl0 = new CompoundShape();

            cyl0.AddChildShape(Matrix.Identity, cylA);
            cyl0.AddChildShape(Matrix.Identity, cylB);

            float   mass = 6.28f;
            Vector3 localInertia;

            cyl0.CalculateLocalInertia(mass, out localInertia);
            RigidBodyConstructionInfo ci = new RigidBodyConstructionInfo(mass, null, cyl0, localInertia);

            ci.StartWorldTransform = Matrix.Translation(-8, 1, -8);

            body = new RigidBody(ci); //1,0,cyl0,localInertia);
            World.AddRigidBody(body);
            body.LinearFactor  = Vector3.Zero;
            body.AngularFactor = new Vector3(0, 1, 0);
            bodyA = body;

            cylA = new CylinderShape(0.2f, 0.26f, 0.2f);
            cylB = new CylinderShape(L_2, 0.025f, L_2);
            cyl0 = new CompoundShape();
            cyl0.AddChildShape(Matrix.Identity, cylA);
            cyl0.AddChildShape(Matrix.Identity, cylB);

            mass = 6.28f;
            cyl0.CalculateLocalInertia(mass, out localInertia);
            ci = new RigidBodyConstructionInfo(mass, null, cyl0, localInertia);
            Quaternion orn = Quaternion.RotationAxis(new Vector3(0, 0, 1), -THETA);

            ci.StartWorldTransform = Matrix.RotationQuaternion(orn) * Matrix.Translation(-10, 2, -8);

            body = new RigidBody(ci);//1,0,cyl0,localInertia);
            body.LinearFactor = Vector3.Zero;
            HingeConstraint hinge = new HingeConstraint(body, Vector3.Zero, new Vector3(0, 1, 0), true);

            World.AddConstraint(hinge);
            bodyB = body;
            body.AngularVelocity = new Vector3(0, 3, 0);

            World.AddRigidBody(body);

            Vector3 axisA = new Vector3(0, 1, 0);
            Vector3 axisB = new Vector3(0, 1, 0);

            orn = Quaternion.RotationAxis(new Vector3(0, 0, 1), -THETA);
            Matrix mat = Matrix.RotationQuaternion(orn);

            axisB = new Vector3(mat.M21, mat.M22, mat.M23);

            GearConstraint gear = new GearConstraint(bodyA, bodyB, axisA, axisB, RATIO);

            World.AddConstraint(gear, true);


            mass = 1.0f;

            RigidBody body0 = LocalCreateRigidBody(mass, Matrix.Translation(0, 20, 0), shape);

            RigidBody body1 = null;//LocalCreateRigidBody(mass, Matrix.Translation(2*CUBE_HALF_EXTENTS,20,0), shape);
            //RigidBody body1 = LocalCreateRigidBody(0, Matrix.Translation(2*CUBE_HALF_EXTENTS,20,0), null);
            //body1.ActivationState = ActivationState.DisableDeactivation;
            //body1.SetDamping(0.3f, 0.3f);

            Vector3 pivotInA = new Vector3(CubeHalfExtents, -CubeHalfExtents, -CubeHalfExtents);
            Vector3 axisInA  = new Vector3(0, 0, 1);

            Vector3 pivotInB;

            if (body1 != null)
            {
                Matrix transform = Matrix.Invert(body1.CenterOfMassTransform) * body0.CenterOfMassTransform;
                pivotInB = Vector3.TransformCoordinate(pivotInA, transform);
            }
            else
            {
                pivotInB = pivotInA;
            }

            Vector3 axisInB;

            if (body1 != null)
            {
                Matrix transform = Matrix.Invert(body1.CenterOfMassTransform) * body1.CenterOfMassTransform;
                axisInB = Vector3.TransformCoordinate(axisInA, transform);
            }
            else
            {
                axisInB = Vector3.TransformCoordinate(axisInA, body0.CenterOfMassTransform);
            }

#if P2P
            {
                TypedConstraint p2p = new Point2PointConstraint(body0, pivotInA);
                //TypedConstraint p2p = new Point2PointConstraint(body0, body1, pivotInA, pivotInB);
                //TypedConstraint hinge = new HingeConstraint(body0, body1, pivotInA, pivotInB, axisInA, axisInB);
                World.AddConstraint(p2p);
                p2p.DebugDrawSize = 5;
            }
#else
            {
                hinge = new HingeConstraint(body0, pivotInA, axisInA);

                //use zero targetVelocity and a small maxMotorImpulse to simulate joint friction
                //float	targetVelocity = 0.f;
                //float	maxMotorImpulse = 0.01;
                const float targetVelocity  = 1.0f;
                const float maxMotorImpulse = 1.0f;
                hinge.EnableAngularMotor(true, targetVelocity, maxMotorImpulse);
                World.AddConstraint(hinge);
                hinge.DebugDrawSize = 5;
            }
#endif

            RigidBody pRbA1 = LocalCreateRigidBody(mass, Matrix.Translation(-20, 0, 30), shape);
            //RigidBody pRbA1 = LocalCreateRigidBody(0.0f, Matrix.Translation(-20, 0, 30), shape);
            pRbA1.ActivationState = ActivationState.DisableDeactivation;

            // add dynamic rigid body B1
            RigidBody pRbB1 = LocalCreateRigidBody(mass, Matrix.Translation(-20, 0, 30), shape);
            //RigidBody pRbB1 = LocalCreateRigidBody(0.0f, Matrix.Translation(-20, 0, 30), shape);
            pRbB1.ActivationState = ActivationState.DisableDeactivation;

            // create slider constraint between A1 and B1 and add it to world
            SliderConstraint spSlider1 = new SliderConstraint(pRbA1, pRbB1, Matrix.Identity, Matrix.Identity, true);
            //spSlider1 = new SliderConstraint(pRbA1, pRbB1, Matrix.Identity, Matrix.Identity, false);
            spSlider1.LowerLinearLimit = -15.0f;
            spSlider1.UpperLinearLimit = -5.0f;
            spSlider1.LowerLinearLimit = 5.0f;
            spSlider1.UpperLinearLimit = 15.0f;
            spSlider1.LowerLinearLimit = -10.0f;
            spSlider1.UpperLinearLimit = -10.0f;

            spSlider1.LowerAngularLimit = -(float)Math.PI / 3.0f;
            spSlider1.UpperAngularLimit = (float)Math.PI / 3.0f;

            World.AddConstraint(spSlider1, true);
            spSlider1.DebugDrawSize = 5.0f;


            //create a slider, using the generic D6 constraint
            Vector3     sliderWorldPos = new Vector3(0, 10, 0);
            Vector3     sliderAxis     = Vector3.UnitX;
            const float angle          = 0; //SIMD_RADS_PER_DEG * 10.f;
            Matrix      trans          = Matrix.RotationAxis(sliderAxis, angle) * Matrix.Translation(sliderWorldPos);
            d6body0 = LocalCreateRigidBody(mass, trans, shape);
            d6body0.ActivationState = ActivationState.DisableDeactivation;

            RigidBody fixedBody1 = LocalCreateRigidBody(0, trans, null);
            World.AddRigidBody(fixedBody1);

            Matrix frameInA = Matrix.Translation(0, 5, 0);
            Matrix frameInB = Matrix.Translation(0, 5, 0);

            //bool useLinearReferenceFrameA = false;//use fixed frame B for linear llimits
            const bool useLinearReferenceFrameA = true; //use fixed frame A for linear llimits
            spSlider6Dof = new Generic6DofConstraint(fixedBody1, d6body0, frameInA, frameInB, useLinearReferenceFrameA)
            {
                LinearLowerLimit = lowerSliderLimit,
                LinearUpperLimit = hiSliderLimit,

                //range should be small, otherwise singularities will 'explode' the constraint
                //AngularLowerLimit = new Vector3(-1.5f,0,0),
                //AngularUpperLimit = new Vector3(1.5f,0,0),
                //AngularLowerLimit = new Vector3(0,0,0),
                //AngularUpperLimit = new Vector3(0,0,0),
                AngularLowerLimit = new Vector3((float)-Math.PI, 0, 0),
                AngularUpperLimit = new Vector3(1.5f, 0, 0)
            };

            //spSlider6Dof.TranslationalLimitMotor.EnableMotor[0] = true;
            spSlider6Dof.TranslationalLimitMotor.TargetVelocity = new Vector3(-5.0f, 0, 0);
            spSlider6Dof.TranslationalLimitMotor.MaxMotorForce  = new Vector3(0.1f, 0, 0);

            World.AddConstraint(spSlider6Dof);
            spSlider6Dof.DebugDrawSize = 5;



            // create a door using hinge constraint attached to the world

            CollisionShape pDoorShape = new BoxShape(2.0f, 5.0f, 0.2f);
            CollisionShapes.Add(pDoorShape);
            RigidBody pDoorBody = LocalCreateRigidBody(1.0f, Matrix.Translation(-5.0f, -2.0f, 0.0f), pDoorShape);
            pDoorBody.ActivationState = ActivationState.DisableDeactivation;
            Vector3 btPivotA = new Vector3(10.0f + 2.1f, -2.0f, 0.0f); // right next to the door slightly outside
            Vector3 btAxisA  = Vector3.UnitY;                          // pointing upwards, aka Y-axis

            spDoorHinge = new HingeConstraint(pDoorBody, btPivotA, btAxisA);

            //spDoorHinge.SetLimit(0.0f, (float)Math.PI / 2);
            // test problem values
            //spDoorHinge.SetLimit(-(float)Math.PI, (float)Math.PI * 0.8f);

            //spDoorHinge.SetLimit(1, -1);
            //spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI);
            //spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.3f, 0.0f);
            //spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.01f, 0.0f); // "sticky limits"
            spDoorHinge.SetLimit(-(float)Math.PI * 0.25f, (float)Math.PI * 0.25f);
            //spDoorHinge.SetLimit(0, 0);
            World.AddConstraint(spDoorHinge);
            spDoorHinge.DebugDrawSize = 5;

            RigidBody pDropBody = LocalCreateRigidBody(10.0f, Matrix.Translation(-5.0f, 2.0f, 0.0f), shape);



            // create a generic 6DOF constraint

            //RigidBody pBodyA = LocalCreateRigidBody(mass, Matrix.Translation(10.0f, 6.0f, 0), shape);
            RigidBody pBodyA = LocalCreateRigidBody(0, Matrix.Translation(10, 6, 0), shape);
            //RigidBody pBodyA = LocalCreateRigidBody(0, Matrix.Translation(10, 6, 0), null);
            pBodyA.ActivationState = ActivationState.DisableDeactivation;

            RigidBody pBodyB = LocalCreateRigidBody(mass, Matrix.Translation(0, 6, 0), shape);
            //RigidBody pBodyB = LocalCreateRigidBody(0, Matrix.Translation(0, 6, 0), shape);
            pBodyB.ActivationState = ActivationState.DisableDeactivation;

            frameInA = Matrix.Translation(-5, 0, 0);
            frameInB = Matrix.Translation(5, 0, 0);

            Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, frameInA, frameInB, true);
            //Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, frameInA, frameInB, false);
            pGen6DOF.LinearLowerLimit = new Vector3(-10, -2, -1);
            pGen6DOF.LinearUpperLimit = new Vector3(10, 2, 1);
            //pGen6DOF.LinearLowerLimit = new Vector3(-10, 0, 0);
            //pGen6DOF.LinearUpperLimit = new Vector3(10, 0, 0);
            //pGen6DOF.LinearLowerLimit = new Vector3(0, 0, 0);
            //pGen6DOF.LinearUpperLimit = new Vector3(0, 0, 0);

            //pGen6DOF.TranslationalLimitMotor.EnableMotor[0] = true;
            //pGen6DOF.TranslationalLimitMotor.TargetVelocity = new Vector3(5, 0, 0);
            //pGen6DOF.TranslationalLimitMotor.MaxMotorForce = new Vector3(0.1f, 0, 0);

            //pGen6DOF.AngularLowerLimit = new Vector3(0, (float)Math.PI * 0.9f, 0);
            //pGen6DOF.AngularUpperLimit = new Vector3(0, -(float)Math.PI * 0.9f, 0);
            //pGen6DOF.AngularLowerLimit = new Vector3(0, 0, -(float)Math.PI);
            //pGen6DOF.AngularUpperLimit = new Vector3(0, 0, (float)Math.PI);

            pGen6DOF.AngularLowerLimit = new Vector3(-(float)Math.PI / 4, -0.75f, -(float)Math.PI * 0.4f);
            pGen6DOF.AngularUpperLimit = new Vector3((float)Math.PI / 4, 0.75f, (float)Math.PI * 0.4f);
            //pGen6DOF.AngularLowerLimit = new Vector3(0, -0.75f, (float)Math.PI * 0.8f);
            //pGen6DOF.AngularUpperLimit = new Vector3(0, 0.75f, -(float)Math.PI * 0.8f);
            //pGen6DOF.AngularLowerLimit = new Vector3(0, -(float)Math.PI * 0.8f, (float)Math.PI * 1.98f);
            //pGen6DOF.AngularUpperLimit = new Vector3(0, (float)Math.PI * 0.8f, -(float)Math.PI * 1.98f);

            //pGen6DOF.AngularLowerLimit = new Vector3(-0.75f, -0.5f, -0.5f);
            //pGen6DOF.AngularUpperLimit = new Vector3(0.75f, 0.5f, 0.5f);
            //pGen6DOF.AngularLowerLimit = new Vector3(-0.75f, 0, 0);
            //pGen6DOF.AngularUpperLimit = new Vector3(0.75f, 0, 0);
            //pGen6DOF.AngularLowerLimit = new Vector3(0, -0.7f, 0);
            //pGen6DOF.AngularUpperLimit = new Vector3(0, 0.7f, 0);
            //pGen6DOF.AngularLowerLimit = new Vector3(-1, 0, 0);
            //pGen6DOF.AngularUpperLimit = new Vector3(1, 0, 0);



            // create a ConeTwist constraint

            pBodyA = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, 5, 0), shape);
            //pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-10, 5, 0), shape);
            pBodyA.ActivationState = ActivationState.DisableDeactivation;

            pBodyB = LocalCreateRigidBody(0, Matrix.Translation(-10, -5, 0), shape);
            //pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, -5, 0), shape);

            frameInA  = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2);
            frameInA *= Matrix.Translation(0, -5, 0);
            frameInB  = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2);
            frameInB *= Matrix.Translation(0, 5, 0);

            coneTwist = new ConeTwistConstraint(pBodyA, pBodyB, frameInA, frameInB);
            //coneTwist.SetLimit((float)Math.PI / 4, (float)Math.PI / 4, (float)Math.PI * 0.8f);
            //coneTwist.SetLimit((((float)Math.PI / 4) * 0.6f), (float)Math.PI / 4, (float)Math.PI * 0.8f, 1.0f); // soft limit == hard limit
            coneTwist.SetLimit((((float)Math.PI / 4) * 0.6f), (float)Math.PI / 4, (float)Math.PI * 0.8f, 0.5f);
            World.AddConstraint(coneTwist, true);
            coneTwist.DebugDrawSize = 5;



            // Hinge connected to the world, with motor (to hinge motor with new and old constraint solver)

            RigidBody pBody = LocalCreateRigidBody(1.0f, Matrix.Identity, shape);
            pBody.ActivationState = ActivationState.DisableDeactivation;
            Vector3 pivotA = new Vector3(10.0f, 0.0f, 0.0f);
            btAxisA = new Vector3(0.0f, 0.0f, 1.0f);

            HingeConstraint pHinge = new HingeConstraint(pBody, pivotA, btAxisA);
            //pHinge.EnableAngularMotor(true, -1.0f, 0.165f); // use for the old solver
            pHinge.EnableAngularMotor(true, -1.0f, 1.65f); // use for the new SIMD solver
            World.AddConstraint(pHinge);
            pHinge.DebugDrawSize = 5;



            // create a universal joint using generic 6DOF constraint
            // create two rigid bodies
            // static bodyA (parent) on top:
            pBodyA = LocalCreateRigidBody(0, Matrix.Translation(20, 4, 0), shape);
            pBodyA.ActivationState = ActivationState.DisableDeactivation;
            // dynamic bodyB (child) below it :
            pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(20, 0, 0), shape);
            pBodyB.ActivationState = ActivationState.DisableDeactivation;
            // add some (arbitrary) data to build constraint frames
            Vector3 parentAxis = new Vector3(1, 0, 0);
            Vector3 childAxis  = new Vector3(0, 0, 1);
            Vector3 anchor     = new Vector3(20, 2, 0);

            UniversalConstraint pUniv = new UniversalConstraint(pBodyA, pBodyB, anchor, parentAxis, childAxis);
            pUniv.SetLowerLimit(-(float)Math.PI / 4, -(float)Math.PI / 4);
            pUniv.SetUpperLimit((float)Math.PI / 4, (float)Math.PI / 4);
            // add constraint to world
            World.AddConstraint(pUniv, true);
            // draw constraint frames and limits for debugging
            pUniv.DebugDrawSize = 5;

            World.AddConstraint(pGen6DOF, true);
            pGen6DOF.DebugDrawSize = 5;



            // create a generic 6DOF constraint with springs

            pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-20, 16, 0), shape);
            pBodyA.ActivationState = ActivationState.DisableDeactivation;

            pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, 16, 0), shape);
            pBodyB.ActivationState = ActivationState.DisableDeactivation;

            frameInA = Matrix.Translation(10, 0, 0);
            frameInB = Matrix.Identity;

            Generic6DofSpringConstraint pGen6DOFSpring = new Generic6DofSpringConstraint(pBodyA, pBodyB, frameInA, frameInB, true)
            {
                LinearUpperLimit  = new Vector3(5, 0, 0),
                LinearLowerLimit  = new Vector3(-5, 0, 0),
                AngularLowerLimit = new Vector3(0, 0, -1.5f),
                AngularUpperLimit = new Vector3(0, 0, 1.5f),
                DebugDrawSize     = 5
            };
            World.AddConstraint(pGen6DOFSpring, true);

            pGen6DOFSpring.EnableSpring(0, true);
            pGen6DOFSpring.SetStiffness(0, 39.478f);
            pGen6DOFSpring.SetDamping(0, 0.5f);
            pGen6DOFSpring.EnableSpring(5, true);
            pGen6DOFSpring.SetStiffness(5, 39.478f);
            pGen6DOFSpring.SetDamping(0, 0.3f);
            pGen6DOFSpring.SetEquilibriumPoint();



            // create a Hinge2 joint
            // create two rigid bodies
            // static bodyA (parent) on top:
            pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-20, 4, 0), shape);
            pBodyA.ActivationState = ActivationState.DisableDeactivation;
            // dynamic bodyB (child) below it :
            pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-20, 0, 0), shape);
            pBodyB.ActivationState = ActivationState.DisableDeactivation;
            // add some data to build constraint frames
            parentAxis = new Vector3(0, 1, 0);
            childAxis  = new Vector3(1, 0, 0);
            anchor     = new Vector3(-20, 0, 0);
            Hinge2Constraint pHinge2 = new Hinge2Constraint(pBodyA, pBodyB, anchor, parentAxis, childAxis);
            pHinge2.SetLowerLimit(-(float)Math.PI / 4);
            pHinge2.SetUpperLimit((float)Math.PI / 4);
            // add constraint to world
            World.AddConstraint(pHinge2, true);
            // draw constraint frames and limits for debugging
            pHinge2.DebugDrawSize = 5;



            // create a Hinge joint between two dynamic bodies
            // create two rigid bodies
            // static bodyA (parent) on top:
            pBodyA = LocalCreateRigidBody(1.0f, Matrix.Translation(-20, -2, 0), shape);
            pBodyA.ActivationState = ActivationState.DisableDeactivation;
            // dynamic bodyB:
            pBodyB = LocalCreateRigidBody(10.0f, Matrix.Translation(-30, -2, 0), shape);
            pBodyB.ActivationState = ActivationState.DisableDeactivation;
            // add some data to build constraint frames
            axisA = new Vector3(0, 1, 0);
            axisB = new Vector3(0, 1, 0);
            Vector3 pivotA2 = new Vector3(-5, 0, 0);
            Vector3 pivotB  = new Vector3(5, 0, 0);
            spHingeDynAB = new HingeConstraint(pBodyA, pBodyB, pivotA2, pivotB, axisA, axisB);
            spHingeDynAB.SetLimit(-(float)Math.PI / 4, (float)Math.PI / 4);
            // add constraint to world
            World.AddConstraint(spHingeDynAB, true);
            // draw constraint frames and limits for debugging
            spHingeDynAB.DebugDrawSize = 5;
        }
Beispiel #30
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
        }