path: root/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.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.Text;

using OMV = OpenMetaverse;

namespace OpenSim.Region.Physics.BulletSPlugin
{
public sealed class BSLinksetConstraints : BSLinkset
{
    // private static string LogHeader = "[BULLETSIM LINKSET CONSTRAINTS]";

    public class BSLinkInfoConstraint : BSLinkInfo
    {
        public ConstraintType constraintType;
        public BSConstraint constraint;
        public OMV.Vector3 linearLimitLow;
        public OMV.Vector3 linearLimitHigh;
        public OMV.Vector3 angularLimitLow;
        public OMV.Vector3 angularLimitHigh;
        public bool useFrameOffset;
        public bool enableTransMotor;
        public float transMotorMaxVel;
        public float transMotorMaxForce;
        public float cfm;
        public float erp;
        public float solverIterations;
        //
        public OMV.Vector3 frameInAloc;
        public OMV.Quaternion frameInArot;
        public OMV.Vector3 frameInBloc;
        public OMV.Quaternion frameInBrot;
        // Spring
        public float springDamping;
        public float springStiffness;



        public BSLinkInfoConstraint(BSPrimLinkable pMember)
            : base(pMember)
        {
            constraint = null;
            ResetLink();
        }

        // Set all the parameters for this constraint to a fixed, non-movable constraint.
        public override void ResetLink()
        {
            constraintType = ConstraintType.D6_CONSTRAINT_TYPE;
            linearLimitLow = OMV.Vector3.Zero;
            linearLimitHigh = OMV.Vector3.Zero;
            angularLimitLow = OMV.Vector3.Zero;
            angularLimitHigh = OMV.Vector3.Zero;
            useFrameOffset = BSParam.LinkConstraintUseFrameOffset;
            enableTransMotor = BSParam.LinkConstraintEnableTransMotor;
            transMotorMaxVel = BSParam.LinkConstraintTransMotorMaxVel;
            transMotorMaxForce = BSParam.LinkConstraintTransMotorMaxForce;
            cfm = BSParam.LinkConstraintCFM;
            erp = BSParam.LinkConstraintERP;
            solverIterations = BSParam.LinkConstraintSolverIterations;
            frameInAloc = OMV.Vector3.Zero;
            frameInArot = OMV.Quaternion.Identity;
            frameInBloc = OMV.Vector3.Zero;
            frameInBrot = OMV.Quaternion.Identity;
            springDamping = -1f;
            springStiffness = -1f;
        }

        // Given a constraint, apply the current constraint parameters to same.
        public override void SetLinkParameters(BSConstraint constrain)
        {
            switch (constraintType)
            {
                case ConstraintType.D6_CONSTRAINT_TYPE:
                    BSConstraint6Dof constrain6dof = constrain as BSConstraint6Dof;
                    if (constrain6dof != null)
                    {
                        // NOTE: D6_SPRING_CONSTRAINT_TYPE should be updated if you change any of this code.
                        // zero linear and angular limits makes the objects unable to move in relation to each other
                        constrain6dof.SetLinearLimits(linearLimitLow, linearLimitHigh);
                        constrain6dof.SetAngularLimits(angularLimitLow, angularLimitHigh);

                        // tweek the constraint to increase stability
                        constrain6dof.UseFrameOffset(useFrameOffset);
                        constrain6dof.TranslationalLimitMotor(enableTransMotor, transMotorMaxVel, transMotorMaxForce);
                        constrain6dof.SetCFMAndERP(cfm, erp);
                        if (solverIterations != 0f)
                        {
                            constrain6dof.SetSolverIterations(solverIterations);
                        }
                    }
                    break;
                case ConstraintType.D6_SPRING_CONSTRAINT_TYPE:
                    BSConstraintSpring constrainSpring = constrain as BSConstraintSpring;
                    if (constrainSpring != null)
                    {
                        // zero linear and angular limits makes the objects unable to move in relation to each other
                        constrainSpring.SetLinearLimits(linearLimitLow, linearLimitHigh);
                        constrainSpring.SetAngularLimits(angularLimitLow, angularLimitHigh);

                        // tweek the constraint to increase stability
                        constrainSpring.UseFrameOffset(useFrameOffset);
                        constrainSpring.TranslationalLimitMotor(enableTransMotor, transMotorMaxVel, transMotorMaxForce);
                        constrainSpring.SetCFMAndERP(cfm, erp);
                        if (solverIterations != 0f)
                        {
                            constrainSpring.SetSolverIterations(solverIterations);
                        }
                        for (int ii = 0; ii < 6; ii++)
                        {
                            if (springDamping != -1)
                                constrainSpring.SetDamping(ii, springDamping);
                            if (springStiffness != -1)
                            constrainSpring.SetStiffness(ii, springStiffness);
                        }
                    }
                    break;
                default:
                    break;
            }
        }
    }

    public BSLinksetConstraints(BSScene scene, BSPrimLinkable parent) : base(scene, parent)
    {
        LinksetImpl = LinksetImplementation.Constraint;
    }

    // When physical properties are changed the linkset needs to recalculate
    //   its internal properties.
    // This is queued in the 'post taint' queue so the
    //   refresh will happen once after all the other taints are applied.
    public override void Refresh(BSPrimLinkable requestor)
    {
        base.Refresh(requestor);

    }

    private void ScheduleRebuild(BSPrimLinkable requestor)
    {
        DetailLog("{0},BSLinksetConstraint.ScheduleRebuild,,rebuilding={1},hasChildren={2},actuallyScheduling={3}",
                            requestor.LocalID, Rebuilding, HasAnyChildren, (!Rebuilding && HasAnyChildren));

        // When rebuilding, it is possible to set properties that would normally require a rebuild.
        //    If already rebuilding, don't request another rebuild.
        //    If a linkset with just a root prim (simple non-linked prim) don't bother rebuilding.
        if (!Rebuilding && HasAnyChildren)
        {
            // Queue to happen after all the other taint processing
            m_physicsScene.PostTaintObject("BSLinksetContraints.Refresh", requestor.LocalID, delegate()
                {
                    if (HasAnyChildren)
                        RecomputeLinksetConstraints();
                });
        }
    }

    // The object is going dynamic (physical). Do any setup necessary
    //     for a dynamic linkset.
    // Only the state of the passed object can be modified. The rest of the linkset
    //     has not yet been fully constructed.
    // Return 'true' if any properties updated on the passed object.
    // Called at taint-time!
    public override bool MakeDynamic(BSPrimLinkable child)
    {
        bool ret = false;
        DetailLog("{0},BSLinksetConstraints.MakeDynamic,call,IsRoot={1}", child.LocalID, IsRoot(child));
        if (IsRoot(child))
        {
            // The root is going dynamic. Rebuild the linkset so parts and mass get computed properly.
            ScheduleRebuild(LinksetRoot);
        }
        return ret;
    }

    // The object is going static (non-physical). Do any setup necessary for a static linkset.
    // Return 'true' if any properties updated on the passed object.
    // This doesn't normally happen -- OpenSim removes the objects from the physical
    //     world if it is a static linkset.
    // Called at taint-time!
    public override bool MakeStatic(BSPrimLinkable child)
    {
        bool ret = false;

        DetailLog("{0},BSLinksetConstraint.MakeStatic,call,IsRoot={1}", child.LocalID, IsRoot(child));
        child.ClearDisplacement();
        if (IsRoot(child))
        {
            // Schedule a rebuild to verify that the root shape is set to the real shape.
            ScheduleRebuild(LinksetRoot);
        }
        return ret;
    }

    // Called at taint-time!!
    public override void UpdateProperties(UpdatedProperties whichUpdated, BSPrimLinkable pObj)
    {
        // Nothing to do for constraints on property updates
    }

    // Routine called when rebuilding the body of some member of the linkset.
    // Destroy all the constraints have have been made to root and set
    //     up to rebuild the constraints before the next simulation step.
    // Returns 'true' of something was actually removed and would need restoring
    // Called at taint-time!!
    public override bool RemoveDependencies(BSPrimLinkable child)
    {
        bool ret = false;

        DetailLog("{0},BSLinksetConstraint.RemoveDependencies,removeChildrenForRoot,rID={1},rBody={2}",
                                    child.LocalID, LinksetRoot.LocalID, LinksetRoot.PhysBody.AddrString);

        lock (m_linksetActivityLock)
        {
            // Just undo all the constraints for this linkset. Rebuild at the end of the step.
            ret = PhysicallyUnlinkAllChildrenFromRoot(LinksetRoot);
            // Cause the constraints, et al to be rebuilt before the next simulation step.
            ScheduleRebuild(LinksetRoot);
        }
        return ret;
    }

    // ================================================================

    // Add a new child to the linkset.
    // Called while LinkActivity is locked.
    protected override void AddChildToLinkset(BSPrimLinkable child)
    {
        if (!HasChild(child))
        {
            m_children.Add(child, new BSLinkInfoConstraint(child));

            DetailLog("{0},BSLinksetConstraints.AddChildToLinkset,call,child={1}", LinksetRoot.LocalID, child.LocalID);

            // Cause constraints and assorted properties to be recomputed before the next simulation step.
            ScheduleRebuild(LinksetRoot);
        }
        return;
    }

    // Remove the specified child from the linkset.
    // Safe to call even if the child is not really in my linkset.
    protected override void RemoveChildFromLinkset(BSPrimLinkable child, bool inTaintTime)
    {
        if (m_children.Remove(child))
        {
            BSPrimLinkable rootx = LinksetRoot; // capture the root and body as of now
            BSPrimLinkable childx = child;

            DetailLog("{0},BSLinksetConstraints.RemoveChildFromLinkset,call,rID={1},rBody={2},cID={3},cBody={4}",
                            childx.LocalID,
                            rootx.LocalID, rootx.PhysBody.AddrString,
                            childx.LocalID, childx.PhysBody.AddrString);

            m_physicsScene.TaintedObject(inTaintTime, "BSLinksetConstraints.RemoveChildFromLinkset", delegate()
            {
                PhysicallyUnlinkAChildFromRoot(rootx, childx);
            });
            // See that the linkset parameters are recomputed at the end of the taint time.
            ScheduleRebuild(LinksetRoot);
        }
        else
        {
            // Non-fatal occurance.
            // PhysicsScene.Logger.ErrorFormat("{0}: Asked to remove child from linkset that was not in linkset", LogHeader);
        }
        return;
    }

    // Create a constraint between me (root of linkset) and the passed prim (the child).
    // Called at taint time!
    private void PhysicallyLinkAChildToRoot(BSPrimLinkable rootPrim, BSPrimLinkable childPrim)
    {
        // Don't build the constraint when asked. Put it off until just before the simulation step.
        Refresh(rootPrim);
    }

    // Create a static constraint between the two passed objects
    private BSConstraint BuildConstraint(BSPrimLinkable rootPrim, BSLinkInfo li)
    {
        BSLinkInfoConstraint linkInfo = li as BSLinkInfoConstraint;
        if (linkInfo == null)
            return null;

        // Zero motion for children so they don't interpolate
        li.member.ZeroMotion(true);

        BSConstraint constrain = null;

        switch (linkInfo.constraintType)
        {
            case ConstraintType.D6_CONSTRAINT_TYPE:
                // Relative position normalized to the root prim
                // Essentually a vector pointing from center of rootPrim to center of li.member
                OMV.Vector3 childRelativePosition = linkInfo.member.Position - rootPrim.Position;

                // real world coordinate of midpoint between the two objects
                OMV.Vector3 midPoint = rootPrim.Position + (childRelativePosition / 2);

                DetailLog("{0},BSLinksetConstraint.BuildConstraint,6Dof,rBody={1},cBody={2},rLoc={3},cLoc={4},midLoc={7}",
                                                rootPrim.LocalID, rootPrim.PhysBody, linkInfo.member.PhysBody,
                                                rootPrim.Position, linkInfo.member.Position, midPoint);

                // create a constraint that allows no freedom of movement between the two objects
                // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818

                constrain = new BSConstraint6Dof(
                                    m_physicsScene.World, rootPrim.PhysBody, linkInfo.member.PhysBody, midPoint, true, true );

                /* NOTE: below is an attempt to build constraint with full frame computation, etc.
                 *     Using the midpoint is easier since it lets the Bullet code manipulate the transforms
                 *     of the objects.
                 * Code left for future programmers.
                // ==================================================================================
                // relative position normalized to the root prim
                OMV.Quaternion invThisOrientation = OMV.Quaternion.Inverse(rootPrim.Orientation);
                OMV.Vector3 childRelativePosition = (liConstraint.member.Position - rootPrim.Position) * invThisOrientation;

                // relative rotation of the child to the parent
                OMV.Quaternion childRelativeRotation = invThisOrientation * liConstraint.member.Orientation;
                OMV.Quaternion inverseChildRelativeRotation = OMV.Quaternion.Inverse(childRelativeRotation);

                DetailLog("{0},BSLinksetConstraint.PhysicallyLinkAChildToRoot,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, liConstraint.member.LocalID);
                constrain = new BS6DofConstraint(
                                PhysicsScene.World, rootPrim.Body, liConstraint.member.Body,
                                OMV.Vector3.Zero,
                                OMV.Quaternion.Inverse(rootPrim.Orientation),
                                OMV.Vector3.Zero,
                                OMV.Quaternion.Inverse(liConstraint.member.Orientation),
                                true,
                                true
                                );
                // ==================================================================================
                */

                break;
            case ConstraintType.D6_SPRING_CONSTRAINT_TYPE:
                constrain = new BSConstraintSpring(m_physicsScene.World, rootPrim.PhysBody, linkInfo.member.PhysBody,
                                linkInfo.frameInAloc, linkInfo.frameInArot, linkInfo.frameInBloc, linkInfo.frameInBrot,
                                true /*useLinearReferenceFrameA*/,
                                true /*disableCollisionsBetweenLinkedBodies*/);
                DetailLog("{0},BSLinksetConstraint.BuildConstraint,spring,root={1},rBody={2},child={3},cBody={4},rLoc={5},cLoc={6},midLoc={7}",
                                                rootPrim.LocalID,
                                                rootPrim.LocalID, rootPrim.PhysBody.AddrString,
                                                linkInfo.member.LocalID, linkInfo.member.PhysBody.AddrString,
                                                rootPrim.Position, linkInfo.member.Position);

                break;
            default:
                break;
        }

        linkInfo.SetLinkParameters(constrain);

        m_physicsScene.Constraints.AddConstraint(constrain);

        return constrain;
    }

    // Remove linkage between the linkset root and a particular child
    // The root and child bodies are passed in because we need to remove the constraint between
    //      the bodies that were present at unlink time.
    // Called at taint time!
    private bool PhysicallyUnlinkAChildFromRoot(BSPrimLinkable rootPrim, BSPrimLinkable childPrim)
    {
        bool ret = false;
        DetailLog("{0},BSLinksetConstraint.PhysicallyUnlinkAChildFromRoot,taint,root={1},rBody={2},child={3},cBody={4}",
                            rootPrim.LocalID,
                            rootPrim.LocalID, rootPrim.PhysBody.AddrString,
                            childPrim.LocalID, childPrim.PhysBody.AddrString);

        // Find the constraint for this link and get rid of it from the overall collection and from my list
        if (m_physicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.PhysBody, childPrim.PhysBody))
        {
            // Make the child refresh its location
            m_physicsScene.PE.PushUpdate(childPrim.PhysBody);
            ret = true;
        }

        return ret;
    }

    // Remove linkage between myself and any possible children I might have.
    // Returns 'true' of any constraints were destroyed.
    // Called at taint time!
    private bool PhysicallyUnlinkAllChildrenFromRoot(BSPrimLinkable rootPrim)
    {
        DetailLog("{0},BSLinksetConstraint.PhysicallyUnlinkAllChildren,taint", rootPrim.LocalID);

        return m_physicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.PhysBody);
    }

    // Call each of the constraints that make up this linkset and recompute the
    //    various transforms and variables. Create constraints of not created yet.
    // Called before the simulation step to make sure the constraint based linkset
    //    is all initialized.
    // Called at taint time!!
    private void RecomputeLinksetConstraints()
    {
        float linksetMass = LinksetMass;
        LinksetRoot.UpdatePhysicalMassProperties(linksetMass, true);

        DetailLog("{0},BSLinksetConstraint.RecomputeLinksetConstraints,set,rBody={1},linksetMass={2}",
                            LinksetRoot.LocalID, LinksetRoot.PhysBody.AddrString, linksetMass);

        try
        {
            Rebuilding = true;

            // There is no reason to build all this physical stuff for a non-physical linkset.
            if (!LinksetRoot.IsPhysicallyActive || !HasAnyChildren)
            {
                DetailLog("{0},BSLinksetConstraint.RecomputeLinksetCompound,notPhysicalOrNoChildren", LinksetRoot.LocalID);
                return; // Note the 'finally' clause at the botton which will get executed.
            }

            ForEachLinkInfo((li) =>
            {
                // A child in the linkset physically shows the mass of the whole linkset.
                // This allows Bullet to apply enough force on the child to move the whole linkset.
                // (Also do the mass stuff before recomputing the constraint so mass is not zero.)
                li.member.UpdatePhysicalMassProperties(linksetMass, true);

                BSConstraint constrain;
                if (!m_physicsScene.Constraints.TryGetConstraint(LinksetRoot.PhysBody, li.member.PhysBody, out constrain))
                {
                    // If constraint doesn't exist yet, create it.
                    constrain = BuildConstraint(LinksetRoot, li);
                }
                li.SetLinkParameters(constrain);
                constrain.RecomputeConstraintVariables(linksetMass);

                // PhysicsScene.PE.DumpConstraint(PhysicsScene.World, constrain.Constraint);    // DEBUG DEBUG
                return false;   // 'false' says to keep processing other members
            });
        }
        finally
        {
            Rebuilding = false;
        }
    }
}
}