opensim/OpenSim/Region/PhysicsModules/BulletS/BSActorLockAxis.cs

/*
 * Copyright (c) Contributors, http://opensimulator.org/
 * See CONTRIBUTORS.TXT for a full list of copyright holders.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyrightD
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the OpenSimulator Project nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

using OMV = OpenMetaverse;

namespace OpenSim.Region.PhysicsModule.BulletS
{
    public class BSActorLockAxis : BSActor
    {
        private BSConstraint LockAxisConstraint = null;
        private bool HaveRegisteredForBeforeStepCallback = false;

        // The lock access flags (which axises were locked) when the contraint was built.
        // Used to see if locking has changed since when the constraint was built.
        OMV.Vector3 LockAxisLinearFlags;
        OMV.Vector3 LockAxisAngularFlags;

        public BSActorLockAxis(BSScene physicsScene, BSPhysObject pObj, string actorName)
            : base(physicsScene, pObj, actorName)
        {
            m_physicsScene.DetailLog("{0},BSActorLockAxis,constructor", m_controllingPrim.LocalID);
            LockAxisConstraint = null;
            HaveRegisteredForBeforeStepCallback = false;
        }

        // BSActor.isActive
        public override bool isActive
        {
            get { return Enabled && m_controllingPrim.IsPhysicallyActive; }
        }

        // Release any connections and resources used by the actor.
        // BSActor.Dispose()
        public override void Dispose()
        {
            Enabled = false;
            UnRegisterForBeforeStepCallback();
            RemoveAxisLockConstraint();
        }

        // Called when physical parameters (properties set in Bullet) need to be re-applied.
        // Called at taint-time.
        // BSActor.Refresh()
        public override void Refresh()
        {
            // Since the axis logging is done with a constraint, Refresh() time is good for
            //    changing parameters but this needs to wait until the prim/linkset is physically
            //    constructed. Therefore, the constraint itself is placed at pre-step time.
    
            // If all the axis are free, we don't need to exist
            // Refresh() only turns off. Enabling is done by InitializeAxisActor()
            //      whenever parameters are changed.
            //      This leaves 'enable' free to turn off an actor when it is not wanted to run.
            if (m_controllingPrim.LockedAngularAxis == m_controllingPrim.LockedAxisFree
                    && m_controllingPrim.LockedLinearAxis == m_controllingPrim.LockedAxisFree)
            {
                Enabled = false;
            }
    
            if (isActive)
            {
                RegisterForBeforeStepCallback();
            }
            else
            {
                RemoveDependencies();
                UnRegisterForBeforeStepCallback();
            }
        }

        // The object's physical representation is being rebuilt so pick up any physical dependencies (constraints, ...).
        //     Register a prestep action to restore physical requirements before the next simulation step.
        // Called at taint-time.
        // BSActor.RemoveDependencies()
        public override void RemoveDependencies()
        {
            RemoveAxisLockConstraint();
        }

        private void RegisterForBeforeStepCallback()
        {
            if (!HaveRegisteredForBeforeStepCallback)
            {
                m_physicsScene.BeforeStep += PhysicsScene_BeforeStep;
                HaveRegisteredForBeforeStepCallback = true;
            }
        }

        private void UnRegisterForBeforeStepCallback()
        {
            if (HaveRegisteredForBeforeStepCallback)
            {
                m_physicsScene.BeforeStep -= PhysicsScene_BeforeStep;
                HaveRegisteredForBeforeStepCallback = false;
            }
        }

        private void PhysicsScene_BeforeStep(float timestep)
        {
            // If all the axis are free, we don't need to exist
            if (m_controllingPrim.LockedAngularAxis == m_controllingPrim.LockedAxisFree
                    && m_controllingPrim.LockedLinearAxis == m_controllingPrim.LockedAxisFree)
            {
                Enabled = false;
            }
    
            // If the object is physically active, add the axis locking constraint
            if (isActive)
            {
                // Check to see if the locking parameters have changed
                if (m_controllingPrim.LockedLinearAxis != this.LockAxisLinearFlags
                    || m_controllingPrim.LockedAngularAxis != this.LockAxisAngularFlags)
                {
                    // The locking has changed. Remove the old constraint and build a new one
                    RemoveAxisLockConstraint();
                }
    
                AddAxisLockConstraint();
            }
            else
            {
                RemoveAxisLockConstraint();
            }
        }

        // Note that this relies on being called at TaintTime
        private void AddAxisLockConstraint()
        {
            if (LockAxisConstraint == null)
            {
                // Lock that axis by creating a 6DOF constraint that has one end in the world and
                //    the other in the object.
                // http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?p=20817
                // http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?p=26380
    
                // Remove any existing axis constraint (just to be sure)
                RemoveAxisLockConstraint();
    
                BSConstraint6Dof axisConstrainer = new BSConstraint6Dof(m_physicsScene.World, m_controllingPrim.PhysBody,
                                    OMV.Vector3.Zero, OMV.Quaternion.Identity,
                                    false /* useLinearReferenceFrameB */, true /* disableCollisionsBetweenLinkedBodies */);
                LockAxisConstraint = axisConstrainer;
                m_physicsScene.Constraints.AddConstraint(LockAxisConstraint);
    
                // Remember the clocking being inforced so we can notice if they have changed
                LockAxisLinearFlags = m_controllingPrim.LockedLinearAxis;
                LockAxisAngularFlags = m_controllingPrim.LockedAngularAxis;
    
                // The constraint is tied to the world and oriented to the prim.
    
                if (!axisConstrainer.SetLinearLimits(m_controllingPrim.LockedLinearAxisLow, m_controllingPrim.LockedLinearAxisHigh))
                {
                    m_physicsScene.DetailLog("{0},BSActorLockAxis.AddAxisLockConstraint,failedSetLinearLimits",
                            m_controllingPrim.LocalID);
                }
    
                if (!axisConstrainer.SetAngularLimits(m_controllingPrim.LockedAngularAxisLow, m_controllingPrim.LockedAngularAxisHigh))
                {
                    m_physicsScene.DetailLog("{0},BSActorLockAxis.AddAxisLockConstraint,failedSetAngularLimits",
                            m_controllingPrim.LocalID);
                }
    
                m_physicsScene.DetailLog("{0},BSActorLockAxis.AddAxisLockConstraint,create,linLow={1},linHi={2},angLow={3},angHi={4}",
                                            m_controllingPrim.LocalID,
                                            m_controllingPrim.LockedLinearAxisLow,
                                            m_controllingPrim.LockedLinearAxisHigh,
                                            m_controllingPrim.LockedAngularAxisLow,
                                            m_controllingPrim.LockedAngularAxisHigh);
    
                // Constants from one of the posts mentioned above and used in Bullet's ConstraintDemo.
                axisConstrainer.TranslationalLimitMotor(true /* enable */, 5.0f, 0.1f);
    
                axisConstrainer.RecomputeConstraintVariables(m_controllingPrim.RawMass);
    
                RegisterForBeforeStepCallback();
            }
        }

        private void RemoveAxisLockConstraint()
        {
            UnRegisterForBeforeStepCallback();
            if (LockAxisConstraint != null)
            {
                m_physicsScene.Constraints.RemoveAndDestroyConstraint(LockAxisConstraint);
                LockAxisConstraint = null;
                m_physicsScene.DetailLog("{0},BSActorLockAxis.RemoveAxisLockConstraint,destroyingConstraint", m_controllingPrim.LocalID);
            }
        }
    }
}