Beispiel #1
0
        protected override void Dispose(bool isdisposing)
        {
            if (isInWorld && isdisposing && m_linkCollider != null)
            {
                BPhysicsWorld pw = BPhysicsWorld.Get();
                if (pw != null && pw.world != null)
                {
                    //constraints must be removed before rigid body is removed

                    /*
                     * for (int i = m_linkCollider.NumConstraintRefs; i > 0; i--)
                     * {
                     *  BTypedConstraint tc = (BTypedConstraint)m_rigidBody.GetConstraintRef(i - 1).Userobject;
                     *  ((DiscreteDynamicsWorld)pw.world).RemoveConstraint(tc.GetConstraint());
                     * }
                     */

                    ((DiscreteDynamicsWorld)pw.world).RemoveCollisionObject(m_linkCollider);
                }
            }
            if (m_linkCollider != null)
            {
                m_linkCollider.Dispose();
                m_linkCollider = null;
            }
        }
Beispiel #2
0
 protected override void Dispose(bool isdisposing)
 {
     if (isInWorld && isdisposing && m_multibody != null)
     {
         BPhysicsWorld pw = BPhysicsWorld.Get();
         if (pw != null && pw.world != null)
         {
             if (m_baseCollider != null)
             {
                 pw.world.RemoveCollisionObject(m_baseCollider);
             }
             if (m_multibody != null)
             {
                 ((MultiBodyDynamicsWorld)pw.world).RemoveMultiBody(m_multibody);
             }
         }
     }
     if (m_multibody != null)
     {
         m_multibody.Dispose();
         m_multibody = null;
     }
     if (m_baseCollider != null)
     {
         m_baseCollider.Dispose();
         m_baseCollider = null;
     }
     if (m_links != null)
     {
         m_links.Clear();
         m_links = null;
     }
 }
Beispiel #3
0
        /**
         * Creates or configures a RigidBody based on the current settings. Does not alter the internal state of this component in any way.
         * Can be used to create copies of this BRigidBody for use in other physics simulations.
         */
        public virtual bool CreateMultiBody(ref MultiBody mb, ref BulletSharp.Math.Vector3 localInertia, CollisionShape cs)
        {
            localInertia = BulletSharp.Math.Vector3.Zero;

            if (_mass > 0)
            {
                cs.CalculateLocalInertia(_mass, out localInertia);
            }

            if (mb == null)
            {
                int nbLinks = Links.Count;
                foreach (BMultiBodyLink link in Links)
                {
                    nbLinks += link.NbLinks;
                }

                Debug.Log("Adding multibody with " + nbLinks + " links");
                mb = new MultiBody(nbLinks, _mass, localInertia, false, false);
                mb.BaseWorldTransform = transform.localToWorldMatrix.ToBullet();
                mb.HasSelfCollision   = SelfCollision;
                var collider = new MultiBodyLinkCollider(mb, -1);
                collider.CollisionShape = cs;
                collider.WorldTransform = transform.localToWorldMatrix.ToBullet();
                collider.CollisionFlags = collisionFlags;
                collider.UserObject     = UserObject ?? this;
                BPhysicsWorld.Get().world.AddCollisionObject(collider, groupsIBelongTo, collisionMask);
                mb.BaseCollider   = collider;
                m_collisionObject = collider;

                BulletMultiBodyLinkColliderProxy baseProxy = gameObject.GetComponent <BulletMultiBodyLinkColliderProxy>();
                if (baseProxy == null)
                {
                    baseProxy = gameObject.AddComponent <BulletMultiBodyLinkColliderProxy>();
                }
                baseProxy.target = collider;

                try
                {
                    int currentLinkIndex = 0;
                    for (int i = 0; i < Links.Count; ++i)
                    {
                        currentLinkIndex += Links[i].AddLinkToMultiBody(this, currentLinkIndex, -1, transform);
                    }

                    mb.FinalizeMultiDof();
                }
                catch (Exception e) // if an error occurs, don't add the object, otherwise unity will crash
                {
                    Debug.LogErrorFormat("Error occured while setting MultiBody : {0}\n{1}", e.Message, e.StackTrace);
                    BPhysicsWorld.Get().world.RemoveCollisionObject(collider);
                    return(false);
                }
                mb.CanSleep   = false;
                mb.UserObject = UserObject ?? this;
            }
            return(true);
        }
Beispiel #4
0
 private void Update()
 {
     if (m_baseCollider != null && isInWorld)
     {
         Matrix4x4 m = m_baseCollider.WorldTransform.ToUnity();
         transform.position = BSExtensionMethods2.ExtractTranslationFromMatrix(ref m);
         transform.rotation = BSExtensionMethods2.ExtractRotationFromMatrix(ref m);
         for (int i = 0; i < m_links.Count; i++)
         {
             MultiBodyLinkCollider linkCollider = m_links[i].GetLinkCollider();
             m = linkCollider.WorldTransform.ToUnity();
             m_links[i].transform.position = BSExtensionMethods2.ExtractTranslationFromMatrix(ref m);
             m_links[i].transform.rotation = BSExtensionMethods2.ExtractRotationFromMatrix(ref m);
         }
     }
 }
Beispiel #5
0
 void DoEverything()
 {
     OnInitializePhysics();
     for (int i = 0; i < 100; i++)
     {
         OnUpdate();
         for (int j = 0; j < numLinks; j++)
         {
             MultiBodyLinkCollider linkCollider = links[j];
             UnityEngine.Matrix4x4 m            = linkCollider.WorldTransform.ToUnity();
             UnityEngine.Vector3   p            = BSExtensionMethods2.ExtractTranslationFromMatrix(ref m);
             linkPositions[j] = p;
             UnityEngine.Debug.Log("pos " + p.ToString("f3") + " " + j);
         }
     }
     ExitPhysics();
 }
        void AddColliders(MultiBody multiBody, Vector3 baseHalfExtents, Vector3 linkHalfExtents)
        {
            // Add a collider for the base
            Quaternion[] worldToLocal = new Quaternion[multiBody.NumLinks + 1];
            Vector3[]    localOrigin  = new Vector3[multiBody.NumLinks + 1];

            worldToLocal[0] = multiBody.WorldToBaseRot;
            localOrigin[0]  = multiBody.BasePosition;

            //if (true)
            {
                var collider = new MultiBodyLinkCollider(multiBody, -1);
                collider.CollisionShape = new BoxShape(baseHalfExtents);

                Matrix tr = Matrix.RotationQuaternion(worldToLocal[0].Inverse);
                tr.Origin = localOrigin[0];
                collider.WorldTransform = tr;

                World.AddCollisionObject(collider, CollisionFilterGroups.StaticFilter,
                                         CollisionFilterGroups.DefaultFilter | CollisionFilterGroups.StaticFilter);
                BulletExampleRunner.Get().CreateUnityMultiBodyLinkColliderProxy(collider);
                collider.Friction      = Friction;
                multiBody.BaseCollider = collider;
            }

            for (int i = 0; i < multiBody.NumLinks; i++)
            {
                int parent = multiBody.GetParent(i);
                worldToLocal[i + 1] = multiBody.GetParentToLocalRot(i) * worldToLocal[parent + 1];
                localOrigin[i + 1]  = localOrigin[parent + 1] + (worldToLocal[i + 1].Inverse.Rotate(multiBody.GetRVector(i)));
            }

            for (int i = 0; i < multiBody.NumLinks; i++)
            {
                var collider = new MultiBodyLinkCollider(multiBody, i);
                collider.CollisionShape = new BoxShape(linkHalfExtents);
                Matrix tr = Matrix.RotationQuaternion(worldToLocal[i + 1].Inverse) * Matrix.Translation(localOrigin[i + 1]);
                collider.WorldTransform = tr;
                World.AddCollisionObject(collider, CollisionFilterGroups.StaticFilter,
                                         CollisionFilterGroups.DefaultFilter | CollisionFilterGroups.StaticFilter);
                collider.Friction = Friction;
                BulletExampleRunner.Get().CreateUnityMultiBodyLinkColliderProxy(collider);
                multiBody.GetLink(i).Collider = collider;
            }
        }
        private void PickMultiBody(MultiBodyLinkCollider collider, ref Vector3 pickPosition)
        {
            MultiBody multiBody = collider.MultiBody;

            if (multiBody == null)
            {
                return;
            }

            _prevCanSleep      = multiBody.CanSleep;
            multiBody.CanSleep = false;
            Vector3 pivotInA = multiBody.WorldPosToLocal(collider.Link, pickPosition);

            var p2p = new MultiBodyPoint2Point(multiBody, collider.Link, null, pivotInA, pickPosition)
            {
                MaxAppliedImpulse = 2
            };

            var world = _demo.Simulation.World as MultiBodyDynamicsWorld;

            world.AddMultiBodyConstraint(p2p);
            _multiBodyPickConstraint = p2p;
        }
Beispiel #8
0
        /// <summary>
        /// A MultiBody object needs to be created and added a controled sequence
        ///    1) The multibody needs to be added to the PhysicsWorld.
        ///    2) Then the base colliders and other colliders can be created and added. They
        ///       depend on the multibody reference.
        ///    3) Then collision objects can be added.
        /// </summary>
        internal void CreateColliders()
        {
            if (m_multibody == null)
            {
                Debug.LogError("Multibody must exist before creating colliders");
                return;
            }
            m_baseCollider            = new MultiBodyLinkCollider(m_multibody, -1);
            m_baseCollider.UserObject = this;
            //todo validate that shape exists on base and all
            BCollisionShape shape = GetComponent <BCollisionShape>();

            m_baseCollider.CollisionShape = shape.GetCollisionShape();
            Matrix worldTrans = Matrix.RotationQuaternion(transform.rotation.ToBullet());

            worldTrans.Origin             = transform.position.ToBullet();
            m_baseCollider.WorldTransform = worldTrans;
            bool isDynamic = (baseMass > 0 && !fixedBase);

            m_groupsIBelongTo        = isDynamic ? (m_groupsIBelongTo) : (m_groupsIBelongTo | BulletSharp.CollisionFilterGroups.StaticFilter);
            m_collisionMask          = isDynamic ? (m_collisionMask) : (m_collisionMask | BulletSharp.CollisionFilterGroups.StaticFilter);
            m_multibody.BaseCollider = m_baseCollider;

            for (int i = 0; i < m_links.Count; i++)
            {
                //create colliders
                MultiBodyLinkCollider col = new MultiBodyLinkCollider(m_multibody, m_links[i].index);
                m_links[i].AssignMultiBodyLinkColliderOnCreation(this, col);
                col.UserObject     = m_links[i];
                shape              = m_links[i].GetComponent <BCollisionShape>();
                col.CollisionShape = shape.GetCollisionShape();
                worldTrans         = Matrix.RotationQuaternion(m_links[i].transform.rotation.ToBullet());
                worldTrans.Origin  = m_links[i].transform.position.ToBullet();
                col.WorldTransform = worldTrans;
                m_multibody.GetLink(i).Collider = col;
            }
        }
Beispiel #9
0
    IEnumerator myCoroutine()
    {
        Debug.Log("Waiting to start");
        while (!Input.GetKeyDown(KeyCode.Space))
        {
            yield return(null);
        }
        OnInitializePhysics();

        Debug.Log("Initialized");
        int frame = 0;

        while (frame < 10)
        {
            if (Input.GetKeyDown(KeyCode.Space))
            {
                frame++;
                Debug.Log("Do Update " + frame);
                for (int j = 0; j < numLinks; j++)
                {
                    MultiBodyLinkCollider linkCollider = links[j];
                    for (int k = 0; k < 5; k++)
                    {
                        OnUpdate();
                    }
                    UnityEngine.Matrix4x4 m = linkCollider.WorldTransform.ToUnity();
                    UnityEngine.Vector3   p = BSExtensionMethods2.ExtractTranslationFromMatrix(ref m);
                    linkPositions[j] = p;
                    UnityEngine.Debug.Log("pos " + p.ToString("f3") + " " + j);
                }
            }
            yield return(null);
        }
        Debug.Log("Finalize");
        ExitPhysics();
    }
Beispiel #10
0
    public void CreateUnityMultiBodyLinkColliderProxy(MultiBodyLinkCollider body)
    {
        GameObject     cube = Instantiate <GameObject>(cubePrefab);
        CollisionShape cs   = body.CollisionShape;

        if (cs is BoxShape)
        {
            BoxShape bxcs = cs as BoxShape;
            BulletSharp.Math.Vector3 s  = bxcs.HalfExtentsWithMargin;
            MeshRenderer             mr = cube.GetComponentInChildren <MeshRenderer>();
            mr.transform.localScale = s.ToUnity() * 2f;
            Matrix4x4 m = body.WorldTransform.ToUnity();
            cube.transform.position   = BSExtensionMethods2.ExtractTranslationFromMatrix(ref m);
            cube.transform.rotation   = BSExtensionMethods2.ExtractRotationFromMatrix(ref m);
            cube.transform.localScale = BSExtensionMethods2.ExtractScaleFromMatrix(ref m);
            Destroy(cube.GetComponent <BulletRigidBodyProxy>());
            BulletMultiBodyLinkColliderProxy cp = cube.AddComponent <BulletMultiBodyLinkColliderProxy>();
            cp.target = body;
        }
        else
        {
            Debug.LogError("Not implemented");
        }
    }
        protected override void OnInitializePhysics()
        {
            // collision configuration contains default setup for memory, collision setup
            CollisionConf = new DefaultCollisionConfiguration();
            Dispatcher    = new CollisionDispatcher(CollisionConf);

            Broadphase = new DbvtBroadphase();
            Solver     = new MultiBodyConstraintSolver();

            World         = new MultiBodyDynamicsWorld(Dispatcher, Broadphase, Solver as MultiBodyConstraintSolver, CollisionConf);
            World.Gravity = new Vector3(0, -9.81f, 0);

            const bool floating        = false;
            const bool gyro            = false;
            const int  numLinks        = 1;
            const bool canSleep        = false;
            const bool selfCollide     = false;
            Vector3    linkHalfExtents = new Vector3(0.05f, 0.5f, 0.1f);
            //Vector3 baseHalfExtents = new Vector3(0.05f, 0.5f, 0.1f);

            Vector3     baseInertiaDiag = Vector3.Zero;
            const float baseMass        = 0;

            multiBody = new MultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
            //multiBody.UseRK4Integration = true;
            //multiBody.BaseWorldTransform = Matrix.Identity;

            //init the links
            Vector3 hingeJointAxis = new Vector3(1, 0, 0);

            //y-axis assumed up
            Vector3 parentComToCurrentCom    = new Vector3(0, -linkHalfExtents[1], 0);
            Vector3 currentPivotToCurrentCom = new Vector3(0, -linkHalfExtents[1], 0);
            Vector3 parentComToCurrentPivot  = parentComToCurrentCom - currentPivotToCurrentCom;

            for (int i = 0; i < numLinks; i++)
            {
                const float linkMass        = 10;
                Vector3     linkInertiaDiag = Vector3.Zero;
                using (var shape = new SphereShape(radius))
                {
                    shape.CalculateLocalInertia(linkMass, out linkInertiaDiag);
                }

                multiBody.SetupRevolute(i, linkMass, linkInertiaDiag, i - 1, Quaternion.Identity,
                                        hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);
            }

            multiBody.FinalizeMultiDof();

            (World as MultiBodyDynamicsWorld).AddMultiBody(multiBody);
            multiBody.CanSleep         = canSleep;
            multiBody.HasSelfCollision = selfCollide;
            multiBody.UseGyroTerm      = gyro;

#if PENDULUM_DAMPING
            multiBody.LinearDamping  = 0.1f;
            multiBody.AngularDamping = 0.9f;
#else
            multiBody.LinearDamping  = 0;
            multiBody.AngularDamping = 0;
#endif

            for (int i = 0; i < numLinks; i++)
            {
                var shape = new SphereShape(radius);
                CollisionShapes.Add(shape);
                var col = new MultiBodyLinkCollider(multiBody, i);
                col.CollisionShape = shape;
                //const bool isDynamic = true;
                CollisionFilterGroups collisionFilterGroup = CollisionFilterGroups.DefaultFilter; // : CollisionFilterGroups.StaticFilter;
                CollisionFilterGroups collisionFilterMask  = CollisionFilterGroups.AllFilter;     // : CollisionFilterGroups.AllFilter & ~CollisionFilterGroups.StaticFilter;
                World.AddCollisionObject(col, collisionFilterGroup, collisionFilterMask);
                multiBody.GetLink(i).Collider = col;
            }
        }
Beispiel #12
0
    public int AddLinkToMultiBody(BMultiBody mb, int currentLinkIndex, int parentIndex, Transform parent)
    {
        if (isLinked)
        {
            Debug.LogErrorFormat("Cannot add link {0} to multibody {1} bacause it is already linked", name, mb.name);
            return(0);
        }

        BCollisionShape collisionShape = GetComponent <BCollisionShape>();

        if (collisionShape == null)
        {
            throw new MissingComponentException("Could not find " + typeof(BCollisionShape).Name + " component on BodyLink " + name);
        }

        multiBody       = mb;
        linkId          = currentLinkIndex;
        parentLinkId    = parentIndex;
        parentTransform = parent;

        CollisionShape shape = collisionShape.GetCollisionShape();

        if (shape == null)
        {
            throw new MissingComponentException("Could not get collision shape from " + collisionShape.GetType().Name + " shape component on BodyLink " + name);
        }
        BulletSharp.Math.Vector3 linkInertia;
        shape.CalculateLocalInertia(Mass, out linkInertia);

        if (BPhysicsWorld.Get().debugType >= BulletUnity.Debugging.BDebug.DebugType.Debug)
        {
            Debug.LogFormat(this, "Adding link {0} : {1} to parent {2} of multibody {3}", currentLinkIndex, name, parentIndex, mb.name);
        }


        SetupLink(linkInertia);

        linkCollider = new MultiBodyLinkCollider(mb.MultiBody, currentLinkIndex);
        linkCollider.CollisionShape  = shape;
        linkCollider.WorldTransform  = transform.localToWorldMatrix.ToBullet();
        linkCollider.CollisionFlags  = collisionFlags;
        linkCollider.Friction        = Friction;
        linkCollider.RollingFriction = RollingFriction;
        linkCollider.Restitution     = Restitution;
        linkCollider.UserObject      = this;
        BPhysicsWorld.Get().world.AddCollisionObject(linkCollider, groupsIBelongTo, collisionMask);
        m_collisionObject = linkCollider;

        BulletMultiBodyLinkColliderProxy proxy = gameObject.GetComponent <BulletMultiBodyLinkColliderProxy>();

        if (proxy == null)
        {
            proxy = gameObject.AddComponent <BulletMultiBodyLinkColliderProxy>();
        }

        mb.MultiBody.GetLink(currentLinkIndex).Collider = linkCollider;
        proxy.target = linkCollider;

        isLinked = true;

        foreach (BMultiBodyConstraint mbc in Constraints)
        {
            mbc.AddConstraintToMultiBody(MultiBody, LinkId);
        }

        int addedLinks = 1;

        for (int i = 0; i < Links.Count; ++i)
        {
            addedLinks += Links[i].AddLinkToMultiBody(mb, i + currentLinkIndex + 1, currentLinkIndex, transform);
        }
        return(addedLinks);
    }
Beispiel #13
0
        public virtual void OnHandleInput()
        {
            if (Input.KeysPressed.Count != 0)
            {
                switch (Input.KeysPressed[0])
                {
                case Keys.Escape:
                case Keys.Q:
                    Graphics.Form.Close();
                    return;

                case Keys.F1:
                    MessageBox.Show(
                        "Move using WASD + shift\n" +
                        "Left click - point camera\n" +
                        "Right click - pick up an object using a Point2PointConstraint\n" +
                        "Right click + shift - pick up an object using a fixed Generic6DofConstraint\n" +
                        "Space - shoot box\n" +
                        "Q - quit\n" +
                        Graphics.InfoText,
                        "Help");
                    // Key release won't be captured
                    Input.KeysDown.Remove(Keys.F1);
                    break;

                case Keys.F3:
                    IsDebugDrawEnabled = !IsDebugDrawEnabled;
                    break;

                case Keys.F8:
                    Input.ClearKeyCache();
                    GraphicsLibraryManager.ExitWithReload = true;
                    Graphics.Form.Close();
                    break;

                case Keys.F11:
                    Graphics.IsFullScreen = !Graphics.IsFullScreen;
                    break;

                case (Keys.Control | Keys.F):
                    const int maxSerializeBufferSize = 1024 * 1024 * 5;
                    using (var serializer = new DefaultSerializer(maxSerializeBufferSize))
                    {
                        World.Serialize(serializer);
                        byte[] dataBytes = new byte[serializer.CurrentBufferSize];
                        System.Runtime.InteropServices.Marshal.Copy(serializer.BufferPointer, dataBytes, 0,
                                                                    dataBytes.Length);
                        using (var file = new System.IO.FileStream("world.bullet", System.IO.FileMode.Create))
                        {
                            file.Write(dataBytes, 0, dataBytes.Length);
                        }
                    }
                    break;

                case Keys.G:
                    //shadowsEnabled = !shadowsEnabled;
                    break;

                case Keys.Space:
                    ShootBox(Freelook.Eye, GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, Graphics.FieldOfView));
                    break;

                case Keys.Return:
                    ClientResetScene();
                    break;
                }
            }

            if (Input.MousePressed != MouseButtons.None)
            {
                Vector3 rayTo = GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, Graphics.FieldOfView);

                if (Input.MousePressed == MouseButtons.Right)
                {
                    if (World != null)
                    {
                        Vector3 rayFrom = Freelook.Eye;

                        ClosestRayResultCallback rayCallback = new ClosestRayResultCallback(ref rayFrom, ref rayTo);
                        World.RayTest(ref rayFrom, ref rayTo, rayCallback);
                        if (rayCallback.HasHit)
                        {
                            Vector3   pickPos = rayCallback.HitPointWorld;
                            RigidBody body    = rayCallback.CollisionObject as RigidBody;
                            if (body != null)
                            {
                                if (!(body.IsStaticObject || body.IsKinematicObject))
                                {
                                    pickedBody = body;
                                    pickedBody.ActivationState = ActivationState.DisableDeactivation;

                                    Vector3 localPivot = Vector3.TransformCoordinate(pickPos, Matrix.Invert(body.CenterOfMassTransform));

                                    if (Input.KeysDown.Contains(Keys.ShiftKey))
                                    {
                                        Generic6DofConstraint dof6 = new Generic6DofConstraint(body, Matrix.Translation(localPivot), false)
                                        {
                                            LinearLowerLimit  = Vector3.Zero,
                                            LinearUpperLimit  = Vector3.Zero,
                                            AngularLowerLimit = Vector3.Zero,
                                            AngularUpperLimit = Vector3.Zero
                                        };

                                        World.AddConstraint(dof6);
                                        pickConstraint = dof6;

                                        dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 0);
                                        dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 1);
                                        dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 2);
                                        dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 3);
                                        dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 4);
                                        dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 5);

                                        dof6.SetParam(ConstraintParam.StopErp, 0.1f, 0);
                                        dof6.SetParam(ConstraintParam.StopErp, 0.1f, 1);
                                        dof6.SetParam(ConstraintParam.StopErp, 0.1f, 2);
                                        dof6.SetParam(ConstraintParam.StopErp, 0.1f, 3);
                                        dof6.SetParam(ConstraintParam.StopErp, 0.1f, 4);
                                        dof6.SetParam(ConstraintParam.StopErp, 0.1f, 5);
                                    }
                                    else
                                    {
                                        Point2PointConstraint p2p = new Point2PointConstraint(body, localPivot);
                                        World.AddConstraint(p2p);
                                        pickConstraint           = p2p;
                                        p2p.Setting.ImpulseClamp = 30;
                                        //very weak constraint for picking
                                        p2p.Setting.Tau = 0.001f;

                                        /*
                                         * p2p.SetParam(ConstraintParams.Cfm, 0.8f, 0);
                                         * p2p.SetParam(ConstraintParams.Cfm, 0.8f, 1);
                                         * p2p.SetParam(ConstraintParams.Cfm, 0.8f, 2);
                                         * p2p.SetParam(ConstraintParams.Erp, 0.1f, 0);
                                         * p2p.SetParam(ConstraintParams.Erp, 0.1f, 1);
                                         * p2p.SetParam(ConstraintParams.Erp, 0.1f, 2);
                                         */
                                    }
                                }
                            }
                            else
                            {
                                MultiBodyLinkCollider multiCol = rayCallback.CollisionObject as MultiBodyLinkCollider;
                                if (multiCol != null && multiCol.MultiBody != null)
                                {
                                    MultiBody mb = multiCol.MultiBody;

                                    prevCanSleep = mb.CanSleep;
                                    mb.CanSleep  = false;
                                    Vector3 pivotInA = mb.WorldPosToLocal(multiCol.Link, pickPos);

                                    MultiBodyPoint2Point p2p = new MultiBodyPoint2Point(mb, multiCol.Link, null, pivotInA, pickPos);
                                    p2p.MaxAppliedImpulse = 2;

                                    (World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(p2p);
                                    pickingMultiBodyPoint2Point = p2p;
                                }
                            }
                            oldPickingDist = (pickPos - rayFrom).Length;
                        }
                        rayCallback.Dispose();
                    }
                }
            }
            else if (Input.MouseReleased == MouseButtons.Right)
            {
                RemovePickingConstraint();
            }

            // Mouse movement
            if (Input.MouseDown == MouseButtons.Right)
            {
                MovePickedBody();
            }
        }
Beispiel #14
0
        MultiBody CreateFeatherstoneMultiBody(MultiBodyDynamicsWorld world, MultiBodySettings settings)
        {
            int     nLinks  = settings.NumLinks;
            float   mass    = 13.5f * Scaling;
            Vector3 inertia = new Vector3(91, 344, 253) * Scaling * Scaling;

            var body = new MultiBody(nLinks, mass, inertia, settings.IsFixedBase, settings.CanSleep);
            //body.HasSelfCollision = false;

            //Quaternion orn = new Quaternion(0, 0, 1, -0.125f * Math.PI);
            Quaternion orn = new Quaternion(0, 0, 0, 1);

            body.BasePosition   = settings.BasePosition;
            body.WorldToBaseRot = orn;
            body.BaseVelocity   = Vector3.Zero;


            Vector3    joint_axis_hinge           = new Vector3(1, 0, 0);
            Vector3    joint_axis_prismatic       = new Vector3(0, 0, 1);
            Quaternion parent_to_child            = orn.Inverse();
            Vector3    joint_axis_child_prismatic = parent_to_child.Rotate(joint_axis_prismatic);
            Vector3    joint_axis_child_hinge     = parent_to_child.Rotate(joint_axis_hinge);

            int this_link_num    = -1;
            int link_num_counter = 0;

            Vector3 pos = new Vector3(0, 0, 9.0500002f) * Scaling;
            Vector3 joint_axis_position = new Vector3(0, 0, 4.5250001f) * Scaling;

            for (int i = 0; i < nLinks; i++)
            {
                float initial_joint_angle = 0.3f;
                if (i > 0)
                {
                    initial_joint_angle = -0.06f;
                }

                int child_link_num = link_num_counter++;

                if (settings.UsePrismatic) // i == (nLinks - 1))
                {
                    body.SetupPrismatic(child_link_num, mass, inertia, this_link_num,
                                        parent_to_child, joint_axis_child_prismatic, parent_to_child.Rotate(pos), Vector3.Zero, settings.DisableParentCollision);
                }
                else
                {
                    body.SetupRevolute(child_link_num, mass, inertia, this_link_num,
                                       parent_to_child, joint_axis_child_hinge, joint_axis_position, parent_to_child.Rotate(pos - joint_axis_position), settings.DisableParentCollision);
                }
                body.SetJointPos(child_link_num, initial_joint_angle);
                this_link_num = i;

                /*if (false) //!useGroundShape && i == 4)
                 * {
                 *  Vector3 pivotInAworld = new Vector3(0, 20, 46);
                 *  Vector3 pivotInAlocal = body.WorldPosToLocal(i, pivotInAworld);
                 *  Vector3 pivotInBworld = pivotInAworld;
                 *  MultiBodyPoint2Point p2p = new MultiBodyPoint2Point(body, i, TypedConstraint.FixedBody, pivotInAlocal, pivotInBworld);
                 *  (World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(p2p);
                 * }*/

                if (settings.UsePrismatic)
                {
                    //MultiBodyConstraint con = new MultiBodyJointLimitConstraint(body, nLinks - 1, 2, 3);

                    if (settings.CreateConstraints)
                    {
                        MultiBodyConstraint con = new MultiBodyJointLimitConstraint(body, i, -1, 1);
                        (World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(con);
                    }
                }
                else
                {
                    //if (true)
                    {
                        var con = new MultiBodyJointMotor(body, i, 0, 50000);
                        (World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(con);
                    }

                    var con2 = new MultiBodyJointLimitConstraint(body, i, -1, 1);
                    (World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(con2);
                }
            }

            // Add a collider for the base
            Quaternion[] worldToLocal = new Quaternion[nLinks + 1];
            Vector3[]    localOrigin  = new Vector3[nLinks + 1];

            worldToLocal[0] = body.WorldToBaseRot;
            localOrigin[0]  = body.BasePosition;

            //Vector3 halfExtents = new Vector3(7.5f, 0.05f, 4.5f);
            Vector3 halfExtents = new Vector3(7.5f, 0.45f, 4.5f);

            float[] posB = new float[] { localOrigin[0].X, localOrigin[0].Y, localOrigin[0].Z, 1 };
            //float[] quatB = new float[] { worldToLocal[0].X, worldToLocal[0].Y, worldToLocal[0].Z, worldToLocal[0].W };

            //if (true)
            {
                CollisionShape box      = new BoxShape(halfExtents * Scaling);
                var            bodyInfo = new RigidBodyConstructionInfo(mass, null, box, inertia);
                RigidBody      bodyB    = new RigidBody(bodyInfo);
                var            collider = new MultiBodyLinkCollider(body, -1);

                collider.CollisionShape = box;
                Matrix tr = Matrix.RotationQuaternion(worldToLocal[0].Inverse()) * Matrix.Translation(localOrigin[0]);
                collider.WorldTransform = tr;
                bodyB.WorldTransform    = tr;

                World.AddCollisionObject(collider, CollisionFilterGroups.StaticFilter,
                                         CollisionFilterGroups.DefaultFilter | CollisionFilterGroups.StaticFilter);
                collider.Friction = Friction;
                body.BaseCollider = collider;
            }


            for (int i = 0; i < body.NumLinks; i++)
            {
                int parent = body.GetParent(i);
                worldToLocal[i + 1] = body.GetParentToLocalRot(i) * worldToLocal[parent + 1];
                localOrigin[i + 1]  = localOrigin[parent + 1] + (worldToLocal[i + 1].Inverse().Rotate(body.GetRVector(i)));
            }

            for (int i = 0; i < body.NumLinks; i++)
            {
                CollisionShape box      = new BoxShape(halfExtents * Scaling);
                var            collider = new MultiBodyLinkCollider(body, i);

                collider.CollisionShape = box;
                Matrix tr = Matrix.RotationQuaternion(worldToLocal[i + 1].Inverse()) * Matrix.Translation(localOrigin[i + 1]);
                collider.WorldTransform = tr;
                World.AddCollisionObject(collider, CollisionFilterGroups.StaticFilter,
                                         CollisionFilterGroups.DefaultFilter | CollisionFilterGroups.StaticFilter);
                collider.Friction = Friction;

                body.GetLink(i).Collider = collider;
                //World.DebugDrawer.DrawBox(halfExtents, pos, quat);
            }

            (World as MultiBodyDynamicsWorld).AddMultiBody(body);
            return(body);
        }
        public PendulumDemoSimulation()
        {
            CollisionConfiguration = new DefaultCollisionConfiguration();
            Dispatcher             = new CollisionDispatcher(CollisionConfiguration);

            Broadphase = new DbvtBroadphase();
            _solver    = new MultiBodyConstraintSolver();

            MultiBodyWorld = new MultiBodyDynamicsWorld(Dispatcher, Broadphase, _solver, CollisionConfiguration);
            World.SetInternalTickCallback(TickCallback, null, true);

            const bool floating        = false;
            const bool gyro            = false;
            const int  numLinks        = 1;
            const bool canSleep        = false;
            const bool selfCollide     = false;
            var        linkHalfExtents = new Vector3(0.05f, 0.5f, 0.1f);
            var        baseHalfExtents = new Vector3(0.05f, 0.5f, 0.1f);

            var         baseInertiaDiag = Vector3.Zero;
            const float baseMass        = 0;

            MultiBody = new MultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
            //MultiBody.UseRK4Integration = true;
            //MultiBody.BaseWorldTransform = Matrix.Identity;

            //init the links
            var hingeJointAxis = new Vector3(1, 0, 0);

            //y-axis assumed up
            Vector3 parentComToCurrentCom    = new Vector3(0, -linkHalfExtents[1], 0);
            Vector3 currentPivotToCurrentCom = new Vector3(0, -linkHalfExtents[1], 0);
            Vector3 parentComToCurrentPivot  = parentComToCurrentCom - currentPivotToCurrentCom;

            for (int i = 0; i < numLinks; i++)
            {
                const float linkMass        = 10;
                Vector3     linkInertiaDiag = Vector3.Zero;
                using (var shape = new SphereShape(radius))
                {
                    shape.CalculateLocalInertia(linkMass, out linkInertiaDiag);
                }

                MultiBody.SetupRevolute(i, linkMass, linkInertiaDiag, i - 1, Quaternion.Identity,
                                        hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);
            }

            MultiBody.FinalizeMultiDof();

            MultiBodyWorld.AddMultiBody(MultiBody);
            MultiBody.CanSleep         = canSleep;
            MultiBody.HasSelfCollision = selfCollide;
            MultiBody.UseGyroTerm      = gyro;

#if PENDULUM_DAMPING
            MultiBody.LinearDamping  = 0.1f;
            MultiBody.AngularDamping = 0.9f;
#else
            MultiBody.LinearDamping  = 0;
            MultiBody.AngularDamping = 0;
#endif

            for (int i = 0; i < numLinks; i++)
            {
                var shape = new SphereShape(radius);
                var col   = new MultiBodyLinkCollider(MultiBody, i);
                col.CollisionShape = shape;
                const bool            isDynamic            = true;
                CollisionFilterGroups collisionFilterGroup = isDynamic ? CollisionFilterGroups.DefaultFilter : CollisionFilterGroups.StaticFilter;
                CollisionFilterGroups collisionFilterMask  = isDynamic ? CollisionFilterGroups.AllFilter : CollisionFilterGroups.AllFilter & ~CollisionFilterGroups.StaticFilter;
                World.AddCollisionObject(col, collisionFilterGroup, collisionFilterMask);
                MultiBody.GetLink(i).Collider = col;
            }
        }
Beispiel #16
0
 /// <summary>
 /// BMultiBodyLinks do not create their own native parts. That happens as part of the MultiBody creation.
 /// After the native MultiBody has been created. The references to the native MultiBodyLinkColliders can be
 /// obtatined and are assigned using this function.
 /// </summary>
 internal void AssignMultiBodyLinkColliderOnCreation(BMultiBody mb, MultiBodyLinkCollider linkCollider)
 {
     m_linkCollider = linkCollider;
 }
Beispiel #17
0
        //todo(erwincoumans) Quick hack, reference to InvertedPendulumPDControl implementation. Will create a separate header/source file for this.
        public static MultiBody createInvertedPendulumMultiBody(float radius, MultiBodyDynamicsWorld world, Matrix baseWorldTrans, bool fixedBase)
        {
            BulletSharp.Math.Vector4[] colors = new BulletSharp.Math.Vector4[]
            {
                new BulletSharp.Math.Vector4(1, 0, 0, 1),
                new BulletSharp.Math.Vector4(0, 1, 0, 1),
                new BulletSharp.Math.Vector4(0, 1, 1, 1),
                new BulletSharp.Math.Vector4(1, 1, 0, 1),
            };
            int curColor = 0;

            bool damping     = false;
            bool gyro        = false;
            int  numLinks    = 2;
            bool spherical   = false;               //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
            bool canSleep    = false;
            bool selfCollide = false;

            BulletSharp.Math.Vector3 linkHalfExtents = new BulletSharp.Math.Vector3(0.05f, 0.37f, 0.1f);
            BulletSharp.Math.Vector3 baseHalfExtents = new BulletSharp.Math.Vector3(0.04f, 0.35f, 0.08f);


            //mbC.forceMultiDof();							//if !spherical, you can comment this line to check the 1DoF algorithm
            //init the base
            BulletSharp.Math.Vector3 baseInertiaDiag = new BulletSharp.Math.Vector3(0.0f, 0.0f, 0.0f);
            float baseMass = fixedBase ? 0.0f : 10.0f;

            if (baseMass != 0)
            {
                //CollisionShape *shape = new btSphereShape(baseHalfExtents[0]);// btBoxShape(BulletSharp.Math.Vector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
                CollisionShape shape = new BoxShape(new BulletSharp.Math.Vector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
                shape.CalculateLocalInertia(baseMass, out baseInertiaDiag);
                shape.Dispose();
            }


            MultiBody pMultiBody = new MultiBody(numLinks, 0, baseInertiaDiag, fixedBase, canSleep);

            pMultiBody.BaseWorldTransform = baseWorldTrans;
            BulletSharp.Math.Vector3 vel = new BulletSharp.Math.Vector3(0, 0, 0);
            //	pMultiBody.setBaseVel(vel);

            //init the links
            BulletSharp.Math.Vector3 hingeJointAxis = new BulletSharp.Math.Vector3(1, 0, 0);

            //y-axis assumed up
            BulletSharp.Math.Vector3 parentComToCurrentCom    = new BulletSharp.Math.Vector3(0, -linkHalfExtents[1] * 2.0f, 0); //par body's COM to cur body's COM offset
            BulletSharp.Math.Vector3 currentPivotToCurrentCom = new BulletSharp.Math.Vector3(0, -linkHalfExtents[1], 0);        //cur body's COM to cur body's PIV offset
            BulletSharp.Math.Vector3 parentComToCurrentPivot  = parentComToCurrentCom - currentPivotToCurrentCom;               //par body's COM to cur body's PIV offset

            //////
            float q0 = 1.0f * Mathf.PI / 180.0f;

            BulletSharp.Math.Quaternion quat0 = new BulletSharp.Math.Quaternion(new BulletSharp.Math.Vector3(1, 0, 0), q0);
            quat0.Normalize();
            /////

            for (int i = 0; i < numLinks; ++i)
            {
                float linkMass = 1.0f;
                //if (i==3 || i==2)
                //	linkMass= 1000;
                BulletSharp.Math.Vector3 linkInertiaDiag = new BulletSharp.Math.Vector3(0.0f, 0.0f, 0.0f);

                CollisionShape shape = null;
                if (i == 0)
                {
                    shape = new BoxShape(new BulletSharp.Math.Vector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));//
                }
                else
                {
                    shape = new SphereShape(radius);
                }
                shape.CalculateLocalInertia(linkMass, out linkInertiaDiag);
                shape.Dispose();


                if (!spherical)
                {
                    //pMultiBody.setupRevolute(i, linkMass, linkInertiaDiag, i - 1, BulletSharp.Math.Quaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);

                    if (i == 0)
                    {
                        pMultiBody.SetupRevolute(i, linkMass, linkInertiaDiag, i - 1,
                                                 new BulletSharp.Math.Quaternion(0.0f, 0.0f, 0.0f, 1.0f),
                                                 hingeJointAxis,
                                                 parentComToCurrentPivot,
                                                 currentPivotToCurrentCom, false);
                    }
                    else
                    {
                        parentComToCurrentCom    = new BulletSharp.Math.Vector3(0, -radius * 2.0f, 0); //par body's COM to cur body's COM offset
                        currentPivotToCurrentCom = new BulletSharp.Math.Vector3(0, -radius, 0);        //cur body's COM to cur body's PIV offset
                        parentComToCurrentPivot  = parentComToCurrentCom - currentPivotToCurrentCom;   //par body's COM to cur body's PIV offset


                        pMultiBody.SetupFixed(i, linkMass, linkInertiaDiag, i - 1,
                                              new BulletSharp.Math.Quaternion(0.0f, 0.0f, 0.0f, 1.0f),
                                              parentComToCurrentPivot,
                                              currentPivotToCurrentCom);
                    }
                }
                else
                {
                    //pMultiBody.setupPlanar(i, linkMass, linkInertiaDiag, i - 1, BulletSharp.Math.Quaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, BulletSharp.Math.Vector3(1, 0, 0), parentComToCurrentPivot*2, false);
                    pMultiBody.SetupSpherical(i, linkMass, linkInertiaDiag, i - 1, new BulletSharp.Math.Quaternion(0.0f, 0.0f, 0.0f, 1.0f), parentComToCurrentPivot, currentPivotToCurrentCom, false);
                }
            }

            pMultiBody.FinalizeMultiDof();
            world.AddMultiBody(pMultiBody);
            MultiBody mbC = pMultiBody;

            mbC.CanSleep         = (canSleep);
            mbC.HasSelfCollision = (selfCollide);
            mbC.UseGyroTerm      = (gyro);
            //
            if (!damping)
            {
                mbC.LinearDamping  = (0.0f);
                mbC.AngularDamping = (0.0f);
            }
            else
            {
                mbC.LinearDamping  = (0.1f);
                mbC.AngularDamping = (0.9f);
            }


            if (numLinks > 0)
            {
                q0 = 180.0f * Mathf.PI / 180.0f;
                if (!spherical)
                {
                    mbC.SetJointPosMultiDof(0, new float[] { q0 });
                }
                else
                {
                    BulletSharp.Math.Vector3 vv = new BulletSharp.Math.Vector3(1, 1, 0);
                    vv.Normalize();
                    quat0 = new BulletSharp.Math.Quaternion(vv, q0);
                    quat0.Normalize();
                    float[] quat0fs = new float[] { quat0.X, quat0.Y, quat0.Z, quat0.W };
                    mbC.SetJointPosMultiDof(0, quat0fs);
                }
            }
            ///
            BulletSharp.Math.Quaternion[] world_to_local; //btAlignedObjectArray<BulletSharp.Math.Quaternion>
            world_to_local = new BulletSharp.Math.Quaternion[pMultiBody.NumLinks + 1];

            BulletSharp.Math.Vector3[] local_origin; //btAlignedObjectArray<BulletSharp.Math.Vector3>
            local_origin      = new BulletSharp.Math.Vector3[pMultiBody.NumLinks + 1];
            world_to_local[0] = pMultiBody.WorldToBaseRot;
            local_origin[0]   = pMultiBody.BasePosition;
            //  double friction = 1;
            {
                if (true)
                {
                    CollisionShape shape = new BoxShape(new BulletSharp.Math.Vector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); //new btSphereShape(baseHalfExtents[0]);
                                                                                                                                                   // guiHelper.createCollisionShapeGraphicsObject(shape);

                    MultiBodyLinkCollider col = new MultiBodyLinkCollider(pMultiBody, -1);
                    col.CollisionShape = shape;

                    Matrix tr = new Matrix();
                    tr.ScaleVector = BulletSharp.Math.Vector3.One;
                    //if we don't set the initial pose of the btCollisionObject, the simulator will do this
                    //when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider

                    tr.Origin = local_origin[0];
                    BulletSharp.Math.Quaternion orn = new BulletSharp.Math.Quaternion(new BulletSharp.Math.Vector3(0, 0, 1), 0.25f * 3.1415926538f);

                    tr.Rotation        = (orn);
                    col.WorldTransform = (tr);

                    bool isDynamic = (baseMass > 0 && !fixedBase);
                    CollisionFilterGroups collisionFilterGroup = isDynamic ? CollisionFilterGroups.DefaultFilter : CollisionFilterGroups.StaticFilter;
                    CollisionFilterGroups collisionFilterMask  = isDynamic ? CollisionFilterGroups.AllFilter : CollisionFilterGroups.AllFilter ^ CollisionFilterGroups.StaticFilter;


                    world.AddCollisionObject(col, collisionFilterGroup, collisionFilterMask);//, 2,1+2);

                    BulletSharp.Math.Vector4 color = new BulletSharp.Math.Vector4(0.0f, 0.0f, 0.5f, 1f);
                    //guiHelper.createCollisionObjectGraphicsObject(col, color);

                    //                col.setFriction(friction);
                    pMultiBody.BaseCollider = (col);
                }
            }


            for (int i = 0; i < pMultiBody.NumLinks; ++i)
            {
                int parent = pMultiBody.GetParent(i);
                world_to_local[i + 1] = pMultiBody.GetParentToLocalRot(i) * world_to_local[parent + 1];
                BulletSharp.Math.Vector3 vv = world_to_local[i + 1].Inverse.Rotate(pMultiBody.GetRVector(i));
                local_origin[i + 1] = local_origin[parent + 1] + vv;
            }


            for (int i = 0; i < pMultiBody.NumLinks; ++i)
            {
                BulletSharp.Math.Vector3 posr = local_origin[i + 1];
                //	float pos[4]={posr.x(),posr.y(),posr.z(),1};

                float[]        quat  = new float[] { -world_to_local[i + 1].X, -world_to_local[i + 1].Y, -world_to_local[i + 1].Z, world_to_local[i + 1].W };
                CollisionShape shape = null;

                if (i == 0)
                {
                    shape = new BoxShape(new BulletSharp.Math.Vector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));//btSphereShape(linkHalfExtents[0]);
                }
                else
                {
                    shape = new SphereShape(radius);
                }

                //guiHelper.createCollisionShapeGraphicsObject(shape);
                MultiBodyLinkCollider col = new MultiBodyLinkCollider(pMultiBody, i);
                col.CollisionShape = (shape);
                Matrix tr = new Matrix();
                tr.ScaleVector     = new BulletSharp.Math.Vector3();
                tr.Origin          = (posr);
                tr.Rotation        = (new BulletSharp.Math.Quaternion(quat[0], quat[1], quat[2], quat[3]));
                col.WorldTransform = (tr);
                //       col.setFriction(friction);
                bool isDynamic = true;//(linkMass > 0);
                CollisionFilterGroups collisionFilterGroup = isDynamic ? CollisionFilterGroups.DefaultFilter : CollisionFilterGroups.StaticFilter;
                CollisionFilterGroups collisionFilterMask  = isDynamic ? CollisionFilterGroups.AllFilter : CollisionFilterGroups.AllFilter ^ CollisionFilterGroups.StaticFilter;

                //if (i==0||i>numLinks-2)
                {
                    world.AddCollisionObject(col, collisionFilterGroup, collisionFilterMask);//,2,1+2);
                    BulletSharp.Math.Vector4 color = colors[curColor];
                    curColor++;
                    curColor &= 3;
                    //guiHelper.createCollisionObjectGraphicsObject(col, color);


                    pMultiBody.GetLink(i).Collider = col;
                }
            }

            return(pMultiBody);
        }
Beispiel #18
0
        public virtual void OnHandleInput()
        {
            if (Input.KeysPressed.Count != 0)
            {
                switch (Input.KeysPressed[0])
                {
                case Keys.Escape:
                case Keys.Q:
                    Graphics.Form.Close();
                    return;

                case Keys.F3:
                    IsDebugDrawEnabled = !IsDebugDrawEnabled;
                    break;

                case Keys.F8:
                    Input.ClearKeyCache();
                    GraphicsLibraryManager.ExitWithReload = true;
                    Graphics.Form.Close();
                    break;

                case Keys.F11:
                    Graphics.IsFullScreen = !Graphics.IsFullScreen;
                    break;

                case Keys.G:
                    //shadowsEnabled = !shadowsEnabled;
                    break;

                case Keys.Space:
                    ShootBox(Freelook.Eye, GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, Graphics.FieldOfView));
                    break;

                case Keys.Return:
                    ClientResetScene();
                    break;
                }
            }

            if (Input.MousePressed != MouseButtons.None)
            {
                Vector3 rayTo = GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, Graphics.FieldOfView);

                if (Input.MousePressed == MouseButtons.Right)
                {
                    if (_world != null)
                    {
                        Vector3 rayFrom = Freelook.Eye;

                        ClosestRayResultCallback rayCallback = new ClosestRayResultCallback(ref rayFrom, ref rayTo);
                        _world.RayTest(ref rayFrom, ref rayTo, rayCallback);
                        if (rayCallback.HasHit)
                        {
                            Vector3   pickPos = rayCallback.HitPointWorld;
                            RigidBody body    = rayCallback.CollisionObject as RigidBody;
                            if (body != null)
                            {
                                if (!(body.IsStaticObject || body.IsKinematicObject))
                                {
                                    pickedBody = body;
                                    pickedBody.ActivationState = ActivationState.DisableDeactivation;

                                    Vector3 localPivot = Vector3.TransformCoordinate(pickPos, Matrix.Invert(body.CenterOfMassTransform));

                                    if (Input.KeysDown.Contains(Keys.ShiftKey))
                                    {
                                        Generic6DofConstraint dof6 = new Generic6DofConstraint(body, Matrix.Translation(localPivot), false)
                                        {
                                            LinearLowerLimit  = Vector3.Zero,
                                            LinearUpperLimit  = Vector3.Zero,
                                            AngularLowerLimit = Vector3.Zero,
                                            AngularUpperLimit = Vector3.Zero
                                        };

                                        _world.AddConstraint(dof6);
                                        pickConstraint = dof6;

                                        dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 0);
                                        dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 1);
                                        dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 2);
                                        dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 3);
                                        dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 4);
                                        dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 5);

                                        dof6.SetParam(ConstraintParam.StopErp, 0.1f, 0);
                                        dof6.SetParam(ConstraintParam.StopErp, 0.1f, 1);
                                        dof6.SetParam(ConstraintParam.StopErp, 0.1f, 2);
                                        dof6.SetParam(ConstraintParam.StopErp, 0.1f, 3);
                                        dof6.SetParam(ConstraintParam.StopErp, 0.1f, 4);
                                        dof6.SetParam(ConstraintParam.StopErp, 0.1f, 5);
                                    }
                                    else
                                    {
                                        Point2PointConstraint p2p = new Point2PointConstraint(body, localPivot);
                                        _world.AddConstraint(p2p);
                                        pickConstraint           = p2p;
                                        p2p.Setting.ImpulseClamp = 30;
                                        //very weak constraint for picking
                                        p2p.Setting.Tau = 0.001f;

                                        /*
                                         * p2p.SetParam(ConstraintParams.Cfm, 0.8f, 0);
                                         * p2p.SetParam(ConstraintParams.Cfm, 0.8f, 1);
                                         * p2p.SetParam(ConstraintParams.Cfm, 0.8f, 2);
                                         * p2p.SetParam(ConstraintParams.Erp, 0.1f, 0);
                                         * p2p.SetParam(ConstraintParams.Erp, 0.1f, 1);
                                         * p2p.SetParam(ConstraintParams.Erp, 0.1f, 2);
                                         */
                                    }
                                }
                            }
                            else
                            {
                                MultiBodyLinkCollider multiCol = rayCallback.CollisionObject as MultiBodyLinkCollider;
                                if (multiCol != null && multiCol.MultiBody != null)
                                {
                                    MultiBody mb = multiCol.MultiBody;

                                    prevCanSleep = mb.CanSleep;
                                    mb.CanSleep  = false;
                                    Vector3 pivotInA = mb.WorldPosToLocal(multiCol.Link, pickPos);

                                    MultiBodyPoint2Point p2p = new MultiBodyPoint2Point(mb, multiCol.Link, null, pivotInA, pickPos);
                                    p2p.MaxAppliedImpulse = 2;

                                    (World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(p2p);
                                    pickingMultiBodyPoint2Point = p2p;
                                }
                            }
                            oldPickingDist = (pickPos - rayFrom).Length;
                        }
                        rayCallback.Dispose();
                    }
                }
            }
            else if (Input.MouseReleased == MouseButtons.Right)
            {
                RemovePickingConstraint();
            }

            // Mouse movement
            if (Input.MouseDown == MouseButtons.Right)
            {
                MovePickedBody();
            }
        }