Esempio n. 1
0
        protected internal void CreateRigidBody(BulletXScene parent_scene, IMesh mesh, OpenMetaverse.Vector3 pos,
                                                OpenMetaverse.Vector3 size)
        {
            //For RigidBody Constructor. The next values might change
            float _linearDamping = 0.0f;
            float _angularDamping = 0.0f;
            float _friction = 1.0f;
            float _restitution = 0.0f;
            Matrix _startTransform = Matrix.Identity;
            Matrix _centerOfMassOffset = Matrix.Identity;
            //added by jed zhu
            _mesh = mesh;

            lock (BulletXScene.BulletXLock)
            {
                _startTransform.Translation = BulletXMaths.PhysicsVectorToXnaVector3(pos);
                //For now all prims are boxes
                CollisionShape _collisionShape;
                if (mesh == null)
                {
                    _collisionShape = new BoxShape(BulletXMaths.PhysicsVectorToXnaVector3(size)/2.0f);
                }
                else
                {
                    int iVertexCount = mesh.getVertexList().Count;
                    int[] indices = mesh.getIndexListAsInt();
                    Vector3[] v3Vertices = new Vector3[iVertexCount];
                    for (int i = 0; i < iVertexCount; i++)
                    {
                        OpenMetaverse.Vector3 v = mesh.getVertexList()[i];
                        if (v != null) // Note, null has special meaning. See meshing code for details
                            v3Vertices[i] = BulletXMaths.PhysicsVectorToXnaVector3(v);
                        else
                            v3Vertices[i] = Vector3.Zero;
                    }
                    TriangleIndexVertexArray triMesh = new TriangleIndexVertexArray(indices, v3Vertices);

                    _collisionShape = new TriangleMeshShape(triMesh);
                }
                DefaultMotionState _motionState = new DefaultMotionState(_startTransform, _centerOfMassOffset);
                Vector3 _localInertia = new Vector3();
                if (_physical) _collisionShape.CalculateLocalInertia(Mass, out _localInertia); //Always when mass > 0
                rigidBody =
                    new RigidBody(Mass, _motionState, _collisionShape, _localInertia, _linearDamping, _angularDamping,
                                  _friction, _restitution);
                //rigidBody.ActivationState = ActivationState.DisableDeactivation;
                //It's seems that there are a bug with rigidBody constructor and its CenterOfMassPosition
                Vector3 _vDebugTranslation;
                _vDebugTranslation = _startTransform.Translation - rigidBody.CenterOfMassPosition;
                rigidBody.Translate(_vDebugTranslation);
                //---
                parent_scene.ddWorld.AddRigidBody(rigidBody);
            }
        }
Esempio n. 2
0
        public CcdDemo()
        {
            NumObjects = 120;

            Shapes[0] = new BoxShape(new Vector3(200, 10, 200));

            for (int i = 0; i < NumObjects; i++)
            {
                CollisionShape shape = Shapes[ShapeIndex[i]];
                shape.Margin = CollisionMargin;

                bool isDynamic = i > 0;

                Matrix trans = Matrix.Identity;

                if (i > 0)
                {
                    //stack them
                    int colsize = 10;
                    int row = (i * HalfExtents * 2) / (colsize * 2 * HalfExtents);
                    int row2 = row;
                    int col = (i) % (colsize) - colsize / 2;

                    if (col > 3)
                    {
                        col = 11;
                        row2 |= 1;
                    }

                    Vector3 pos;
                    if (NumObjects > 2)
                    {
                        pos = new Vector3(col * 2 * HalfExtents + (row2 % 2) * HalfExtents,
                            row * 2 * HalfExtents + HalfExtents + ExtraHeight, 0);
                    }
                    else
                    {
                        pos = new Vector3(0, -30, 0);
                    }

                    if (shape is BoxShape)
                    {
                        trans.Right = new Vector3(0, -1, 0);
                        trans.Up = new Vector3(1, 0, 0);
                        trans.Backward = new Vector3(0, 0, 1);
                    }

                    trans *= Matrix.CreateRotationZ(Microsoft.Xna.Framework.MathHelper.PiOver2);
                    trans.Translation = pos;
                }
                else
                {
                    trans.Translation = new Vector3(0, -30, 0);
                }

                float mass = 1.0f;
                if (!isDynamic)
                    mass = 0.0f;

                RigidBody body = CreateRigidBody(mass, trans, shape);

                body.CcdSquareMotionThreshold = HalfExtents;
                body.CcdSweptSphereRadius = 0.2f * HalfExtents;
            }
        }
Esempio n. 3
0
        private void shootBox(Vector3 destination)
        {
            if (_world != null)
            {
                float mass = 1f;
                Matrix startTransform = Matrix.Identity;
                Vector3 camPos = _camera.Position;
                startTransform.Translation = camPos;
                //CollisionShape boxShape = new SphereShape(1);
                CollisionShape boxShape = new BoxShape(Vector3.One);
                RigidBody body = CreateRigidBody(mass, startTransform, boxShape);

                Vector3 linVel = new Vector3(destination.X - camPos.X, destination.Y - camPos.Y, destination.Z - camPos.Z);
                linVel.Normalize();
                linVel *= _shootBoxInitialSpeed;

                //body.getWorldTransform().setOrigin(camPos);
                //body.getWorldTransform().setRotation(btQuaternion(0, 0, 0, 1));
                body.LinearVelocity = linVel;
                body.AngularVelocity = Vector3.Zero;
            }
        }
Esempio n. 4
0
        /// <summary>
        /// Reinitializes physics.
        /// </summary>
        public void ResetPhysics()
        {
            _collisionDispatcher = new CollisionDispatcher();

            if (_useSweepAndPrune)
            {
                Vector3 worldAabbMin = new Vector3(-10000, -10000, -10000);
                Vector3 worldAabbMax = new Vector3(10000, 10000, 10000);
                const int maxProxies = 32766;
                _broadphase = new AxisSweep3(worldAabbMin, worldAabbMax, maxProxies);
            }
            else
                _broadphase = new SimpleBroadphase();

            _solver = new SequentialImpulseConstraintSolver();
            _world = new DiscreteDynamicsWorld(_collisionDispatcher, _broadphase, _solver);

            //world.setConstraintSolver(solver);
            _world.Gravity = new Vector3(0, -10, 0);

            _shapePtr = new CollisionShape[4];
            _shapePtr[0] = new BoxShape(new Vector3(50, 10, 50));
            _shapePtr[1] = new CylinderShape(new Vector3(_cubeHalfExtent - _collisionMargin, _cubeHalfExtent - _collisionMargin, _cubeHalfExtent - _collisionMargin));
            _shapePtr[2] = new SphereShape(_cubeHalfExtent);
            _shapePtr[3] = new BoxShape(new Vector3(_cubeHalfExtent, _cubeHalfExtent, _cubeHalfExtent));

            _shapeIndex = new int[_maxNumObjects];
            Matrix tr = Matrix.Identity;

            for (int i = 0; i < _numObjects; i++)
            {
                if (i > 0)
                    // set shape
                    _shapeIndex[i] = 1;
                else
                    _shapeIndex[i] = 0;
            }

            GC.Collect();
        }
Esempio n. 5
0
        public ConstraintDemo()
        {
            CollisionShape shape = new BoxShape(new Vector3(HalfExtents, HalfExtents, HalfExtents));
            Matrix trans = Matrix.Identity;
            trans.Translation = new Vector3(0, 20, 0);

            float mass = 1f;
            //Point to Point constraint (ball socket)
            {
                RigidBody bodyA = CreateRigidBody(mass, trans, shape);
                trans.Translation = new Vector3(2 * HalfExtents, 20, 0);

                mass = 1f;
                RigidBody bodyB = null;
                //RigidBody bodyB = CreateRigidBody(mass, trans, shape);
                //bodyB.ActivationState = ActivationState.DisableDeactivation;
                //bodyB.SetDamping(0.3f, 0.3f);

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

                Vector3 pivotInB = bodyB != null ? MathHelper.MatrixToVector(MathHelper.InvertMatrix(bodyB.CenterOfMassTransform), MathHelper.MatrixToVector(bodyA.CenterOfMassTransform, pivotInA)) : pivotInA;
                Vector3 axisInB = bodyB != null ?
                    Vector3.TransformNormal(Vector3.TransformNormal(axisInA, bodyB.CenterOfMassTransform), MathHelper.InvertMatrix(bodyB.CenterOfMassTransform)) :
                Vector3.TransformNormal(axisInA, bodyA.CenterOfMassTransform);

                //TypedConstraint p2p = new Point2PointConstraint(bodyA, bodyB, pivotInA, pivotInB);
                //TypedConstraint hinge = new HingeConstraint(bodyA, bodyB, pivotInA, pivotInB, axisInA, axisInB);
                HingeConstraint hinge = new HingeConstraint(bodyA, pivotInA, axisInA);

                //use zero targetVelocity and a small maxMotorImpulse to simulate joint friction
                //float	targetVelocity = 0.0f;
                //float	maxMotorImpulse = 0.01;
                float targetVelocity = 1.0f;
                float maxMotorImpulse = 1.0f;
                hinge.EnableAngularMotor(true, targetVelocity, maxMotorImpulse);

                PhysicsWorld.AddConstraint(hinge);
            }

            // create a slider, using the generic D6 constraint
            {
                mass = 1f;
                Vector3 sliderWorldPos = new Vector3(0, 10, 0);
                Vector3 sliderAxis = new Vector3(1, 0, 0);
                float angle = 0;
                Matrix sliderOrientation = Matrix.CreateFromQuaternion(new Quaternion(sliderAxis, angle));

                trans = Matrix.Identity;
                trans.Translation = sliderWorldPos;
                //trans.setBasis(sliderOrientation);
                _sliderTransform = trans;

                _d6BodyA = CreateRigidBody(mass, trans, shape);
                _d6BodyA.ActivationState = ActivationState.DisableDeactivation;
                RigidBody fixedBody1 = CreateRigidBody(0, trans, null);

                Matrix frameInA, frameInB;
                frameInA = Matrix.Identity;
                frameInB = Matrix.Identity;

                Generic6DofConstraint slider = new Generic6DofConstraint(_d6BodyA, fixedBody1, frameInA, frameInB);
                slider.SetLinearLowerLimit(_lowerSliderLimit);
                slider.SetLinearUpperLimit(_hiSliderLimit);

                //range should be small, otherwise singularities will 'explode' the constraint
                slider.SetAngularLowerLimit(new Vector3(10, 0, 0));
                slider.SetAngularUpperLimit(new Vector3(0, 0, 0));

                PhysicsWorld.AddConstraint(slider);
            }
        }
        public float GetSphereDistance(CollisionObject boxObject, out Vector3 pointOnBox, out Vector3 pointOnSphere, Vector3 sphereCenter, float radius)
        {
            pointOnBox    = new Vector3();
            pointOnSphere = new Vector3();

            float margins;

            Vector3[] bounds   = new Vector3[2];
            BoxShape  boxShape = boxObject.CollisionShape as BoxShape;

            bounds[0] = -boxShape.HalfExtents;
            bounds[1] = boxShape.HalfExtents;

            margins = boxShape.Margin;             //also add sphereShape margin?

            Matrix m44T = boxObject.WorldTransform;

            Vector3[] boundsVec = new Vector3[2];
            float     penetration;

            boundsVec[0] = bounds[0];
            boundsVec[1] = bounds[1];

            Vector3 marginsVec = new Vector3(margins, margins, margins);

            // add margins
            bounds[0] += marginsVec;
            bounds[1] -= marginsVec;

            /////////////////////////////////////////////////

            Vector3 tmp, prel, normal, v3P;

            Vector3[] n = new Vector3[6];
            float     sep = 10000000.0f, sepThis;

            n[0] = new Vector3(-1.0f, 0.0f, 0.0f);
            n[1] = new Vector3(0.0f, -1.0f, 0.0f);
            n[2] = new Vector3(0.0f, 0.0f, -1.0f);
            n[3] = new Vector3(1.0f, 0.0f, 0.0f);
            n[4] = new Vector3(0.0f, 1.0f, 0.0f);
            n[5] = new Vector3(0.0f, 0.0f, 1.0f);

            // convert  point in local space
            prel = MathHelper.InvXForm(m44T, sphereCenter);

            bool found = false;

            v3P = prel;

            for (int i = 0; i < 6; i++)
            {
                int j = i < 3 ? 0 : 1;
                if ((sepThis = (Vector3.Dot(v3P - bounds[j], n[i]))) > 0.0f)
                {
                    v3P   = v3P - n[i] * sepThis;
                    found = true;
                }
            }

            //

            if (found)
            {
                bounds[0] = boundsVec[0];
                bounds[1] = boundsVec[1];

                normal        = Vector3.Normalize(prel - v3P);
                pointOnBox    = v3P + normal * margins;
                pointOnSphere = prel - normal * radius;

                if ((Vector3.Dot(pointOnSphere - pointOnBox, normal)) > 0.0f)
                {
                    return(1.0f);
                }

                // transform back in world space
                tmp           = MathHelper.MatrixToVector(m44T, pointOnBox);
                pointOnBox    = tmp;
                tmp           = MathHelper.MatrixToVector(m44T, pointOnSphere);
                pointOnSphere = tmp;
                float seps2 = (pointOnBox - pointOnSphere).LengthSquared();

                //if this fails, fallback into deeper penetration case, below
                if (seps2 > MathHelper.Epsilon)
                {
                    sep     = -(float)Math.Sqrt(seps2);
                    normal  = (pointOnBox - pointOnSphere);
                    normal *= 1f / sep;
                }
                return(sep);
            }

            //////////////////////////////////////////////////
            // Deep penetration case

            penetration = GetSpherePenetration(boxObject, ref pointOnBox, ref pointOnSphere, sphereCenter, radius, bounds[0], bounds[1]);

            bounds[0] = boundsVec[0];
            bounds[1] = boundsVec[1];

            if (penetration <= 0.0f)
            {
                return(penetration - margins);
            }
            else
            {
                return(1.0f);
            }
        }