path: root/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.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 class BSLinkset
{
    // private static string LogHeader = "[BULLETSIM LINKSET]";

    public BSPhysObject LinksetRoot { get; protected set; }

    public BSScene PhysicsScene { get; private set; }

    static int m_nextLinksetID = 1;
    public int LinksetID { get; private set; }

    // The children under the root in this linkset.
    // There are two lists of children: the current children at runtime
    //    and the children at taint-time. For instance, if you delink a
    //    child from the linkset, the child is removed from m_children
    //    but the constraint won't be removed until taint time.
    //    Two lists lets this track the 'current' children and
    //    the physical 'taint' children separately.
    // After taint processing and before the simulation step, these
    //    two lists must be the same.
    private HashSet<BSPhysObject> m_children;
    private HashSet<BSPhysObject> m_taintChildren;

    // We lock the diddling of linkset classes to prevent any badness.
    // This locks the modification of the instances of this class. Changes
    //    to the physical representation is done via the tainting mechenism.
    private object m_linksetActivityLock = new Object();

    // We keep the prim's mass in the linkset structure since it could be dependent on other prims
    private float m_mass;
    public float LinksetMass
    {
        get
        {
            m_mass = ComputeLinksetMass();
            return m_mass;
        }
    }

    public OMV.Vector3 CenterOfMass
    {
        get { return ComputeLinksetCenterOfMass(); }
    }

    public OMV.Vector3 GeometricCenter
    {
        get { return ComputeLinksetGeometricCenter(); }
    }

    public BSLinkset(BSScene scene, BSPhysObject parent)
    {
        // A simple linkset of one (no children)
        LinksetID = m_nextLinksetID++;
        // We create LOTS of linksets.
        if (m_nextLinksetID <= 0)
            m_nextLinksetID = 1;
        PhysicsScene = scene;
        LinksetRoot = parent;
        m_children = new HashSet<BSPhysObject>();
        m_taintChildren = new HashSet<BSPhysObject>();
        m_mass = parent.MassRaw;
    }

    // Link to a linkset where the child knows the parent.
    // Parent changing should not happen so do some sanity checking.
    // We return the parent's linkset so the child can track its membership.
    // Called at runtime.
    public BSLinkset AddMeToLinkset(BSPhysObject child)
    {
        lock (m_linksetActivityLock)
        {
            // Don't add the root to its own linkset
            if (!IsRoot(child))
                AddChildToLinkset(child);
        }
        return this;
    }

    // Remove a child from a linkset.
    // Returns a new linkset for the child which is a linkset of one (just the
    //    orphened child).
    // Called at runtime.
    public BSLinkset RemoveMeFromLinkset(BSPhysObject child)
    {
        lock (m_linksetActivityLock)
        {
            if (IsRoot(child))
            {
                // Cannot remove the root from a linkset.
                return this;
            }

            RemoveChildFromLinkset(child);
        }

        // The child is down to a linkset of just itself
        return new BSLinkset(PhysicsScene, child);
    }

    // Return 'true' if the passed object is the root object of this linkset
    public bool IsRoot(BSPhysObject requestor)
    {
        return (requestor.LocalID == LinksetRoot.LocalID);
    }

    public int NumberOfChildren { get { return m_children.Count; } }

    // Return 'true' if this linkset has any children (more than the root member)
    public bool HasAnyChildren { get { return (m_children.Count > 0); } }

    // Return 'true' if this child is in this linkset
    public bool HasChild(BSPhysObject child)
    {
        bool ret = false;
        lock (m_linksetActivityLock)
        {
            foreach (BSPhysObject bp in m_children)
            {
                if (child.LocalID == bp.LocalID)
                {
                    ret = true;
                    break;
                }
            }
        }
        return ret;
    }

    // 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 bool MakeDynamic(BSPhysObject child)
    {
        // What is done for each object in BSPrim is what we want.
        return false;
    }

    // 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.
    // Called at taint-time!
    public bool MakeStatic(BSPhysObject child)
    {
        // What is done for each object in BSPrim is what we want.
        return false;
    }

    // When physical properties are changed the linkset needs to recalculate
    //   its internal properties.
    // Called at runtime.
    public void Refresh(BSPhysObject requestor)
    {
        // If there are no children, there can't be any constraints to recompute
        if (!HasAnyChildren)
            return;

        // Only the root does the recomputation
        if (IsRoot(requestor))
        {
            PhysicsScene.TaintedObject("BSLinkSet.Refresh", delegate()
            {
                RecomputeLinksetConstraintVariables();
                DetailLog("{0},BSLinkset.Refresh,complete,rBody={1}", 
                                LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"));
            });
        }
    }

    // Routine used when rebuilding the body of the root of the linkset
    // Destroy all the constraints have have been made to root.
    // This is called when the root body is changing.
    // Returns 'true' of something eas actually removed and would need restoring
    // Called at taint-time!!
    public bool RemoveBodyDependencies(BSPrim child)
    {
        bool ret = false;

        lock (m_linksetActivityLock)
        {
            if (IsRoot(child))
            {
                // If the one with the dependency is root, must undo all children
                DetailLog("{0},BSLinkset.RemoveBodyDependencies,removeChildrenForRoot,rID={1},rBody={2}",
                                                child.LocalID, LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"));

                ret = PhysicallyUnlinkAllChildrenFromRoot(LinksetRoot);
            }
            else
            {
                DetailLog("{0},BSLinkset.RemoveBodyDependencies,removeSingleChild,rID={1},rBody={2},cID={3},cBody={4}",
                                                child.LocalID,
                                                LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"),
                                                child.LocalID, child.BSBody.ptr.ToString("X"));
                // ret = PhysicallyUnlinkAChildFromRoot(LinksetRoot, child);
                // Despite the function name, this removes any link to the specified object.
                ret = PhysicallyUnlinkAllChildrenFromRoot(child);
            }
        }
        return ret;
    }

    // Routine used when rebuilding the body of the root of the linkset
    // This is called after RemoveAllLinksToRoot() to restore all the constraints.
    // This is called when the root body has been changed.
    // Called at taint-time!!
    public void RestoreBodyDependencies(BSPrim child)
    {
        lock (m_linksetActivityLock)
        {
            if (IsRoot(child))
            {
                DetailLog("{0},BSLinkset.RestoreBodyDependencies,restoreChildrenForRoot,rID={1},numChild={2}",
                                                child.LocalID, LinksetRoot.LocalID, m_taintChildren.Count);
                foreach (BSPhysObject bpo in m_taintChildren)
                {
                    PhysicallyLinkAChildToRoot(LinksetRoot, bpo);
                }
            }
            else
            {
                DetailLog("{0},BSLinkset.RestoreBodyDependencies,restoreSingleChild,rID={1},rBody={2},cID={3},cBody={4}",
                                                LinksetRoot.LocalID,
                                                LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"),
                                                child.LocalID, child.BSBody.ptr.ToString("X"));
                PhysicallyLinkAChildToRoot(LinksetRoot, child);
            }
        }
    }

    // ================================================================
    // Below this point is internal magic

    private float ComputeLinksetMass()
    {
        float mass;
        lock (m_linksetActivityLock)
        {
            mass = LinksetRoot.MassRaw;
            foreach (BSPhysObject bp in m_taintChildren)
            {
                mass += bp.MassRaw;
            }
        }
        return mass;
    }

    private OMV.Vector3 ComputeLinksetCenterOfMass()
    {
        OMV.Vector3 com;
        lock (m_linksetActivityLock)
        {
            com = LinksetRoot.Position * LinksetRoot.MassRaw;
            float totalMass = LinksetRoot.MassRaw;

            foreach (BSPhysObject bp in m_taintChildren)
            {
                com += bp.Position * bp.MassRaw;
                totalMass += bp.MassRaw;
            }
            if (totalMass != 0f)
                com /= totalMass;
        }

        return com;
    }

    private OMV.Vector3 ComputeLinksetGeometricCenter()
    {
        OMV.Vector3 com;
        lock (m_linksetActivityLock)
        {
            com = LinksetRoot.Position;

            foreach (BSPhysObject bp in m_taintChildren)
            {
                com += bp.Position * bp.MassRaw;
            }
            com /= (m_taintChildren.Count + 1);
        }

        return com;
    }

    // I am the root of a linkset and a new child is being added
    // Called while LinkActivity is locked.
    private void AddChildToLinkset(BSPhysObject child)
    {
        if (!HasChild(child))
        {
            m_children.Add(child);

            BSPhysObject rootx = LinksetRoot; // capture the root as of now
            BSPhysObject childx = child;

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

            PhysicsScene.TaintedObject("AddChildToLinkset", delegate()
            {
                DetailLog("{0},AddChildToLinkset,taint,rID={1},rBody={2},cID={3},cBody={4}", 
                                rootx.LocalID,
                                rootx.LocalID, rootx.BSBody.ptr.ToString("X"),
                                childx.LocalID, childx.BSBody.ptr.ToString("X"));
                // Since this is taint-time, the body and shape could have changed for the child
                rootx.ForcePosition = rootx.Position;   // DEBUG
                childx.ForcePosition = childx.Position;   // DEBUG
                PhysicallyLinkAChildToRoot(rootx, childx);
                m_taintChildren.Add(child);
            });
        }
        return;
    }

    // Forcefully removing a child from a linkset.
    // This is not being called by the child so we have to make sure the child doesn't think
    //    it's still connected to the linkset.
    // Normal OpenSimulator operation will never do this because other SceneObjectPart information
    //    also has to be updated (like pointer to prim's parent).
    private void RemoveChildFromOtherLinkset(BSPhysObject pchild)
    {
        pchild.Linkset = new BSLinkset(PhysicsScene, pchild);
        RemoveChildFromLinkset(pchild);
    }

    // I am the root of a linkset and one of my children is being removed.
    // Safe to call even if the child is not really in my linkset.
    private void RemoveChildFromLinkset(BSPhysObject child)
    {
        if (m_children.Remove(child))
        {
            BSPhysObject rootx = LinksetRoot; // capture the root and body as of now
            BSPhysObject childx = child;

            DetailLog("{0},RemoveChildFromLinkset,call,rID={1},rBody={2},cID={3},cBody={4}", 
                            childx.LocalID,
                            rootx.LocalID, rootx.BSBody.ptr.ToString("X"),
                            childx.LocalID, childx.BSBody.ptr.ToString("X"));

            PhysicsScene.TaintedObject("RemoveChildFromLinkset", delegate()
            {
                m_taintChildren.Remove(child);
                PhysicallyUnlinkAChildFromRoot(rootx, childx);
                RecomputeLinksetConstraintVariables();
            });

        }
        else
        {
            // This will happen if we remove the root of the linkset first. 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(BSPhysObject rootPrim, BSPhysObject childPrim)
    {
        // Zero motion for children so they don't interpolate
        childPrim.ZeroMotion();

        // Relative position normalized to the root prim
        // Essentually a vector pointing from center of rootPrim to center of childPrim
        OMV.Vector3 childRelativePosition = childPrim.Position - rootPrim.Position;

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

        DetailLog("{0},BSLinkset.PhysicallyLinkAChildToRoot,taint,root={1},rBody={2},child={3},cBody={4},rLoc={5},cLoc={6},midLoc={7}",
                                        rootPrim.LocalID,
                                        rootPrim.LocalID, rootPrim.BSBody.ptr.ToString("X"),
                                        childPrim.LocalID, childPrim.BSBody.ptr.ToString("X"),
                                        rootPrim.Position, childPrim.Position, midPoint);

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

        BS6DofConstraint constrain = new BS6DofConstraint(
                            PhysicsScene.World, rootPrim.BSBody, childPrim.BSBody, 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 as a warning to future programmers.
        // ==================================================================================
        // relative position normalized to the root prim
        OMV.Quaternion invThisOrientation = OMV.Quaternion.Inverse(rootPrim.Orientation);
        OMV.Vector3 childRelativePosition = (childPrim.Position - rootPrim.Position) * invThisOrientation;

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

        // create a constraint that allows no freedom of movement between the two objects
        // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
        DetailLog("{0},BSLinkset.PhysicallyLinkAChildToRoot,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, childPrim.LocalID);
        BS6DofConstraint constrain = new BS6DofConstraint(
                        PhysicsScene.World, rootPrim.Body, childPrim.Body,
                        OMV.Vector3.Zero,
                        OMV.Quaternion.Inverse(rootPrim.Orientation),
                        OMV.Vector3.Zero,
                        OMV.Quaternion.Inverse(childPrim.Orientation),
                        // A point half way between the parent and child
                        // childRelativePosition/2,
                        // childRelativeRotation,
                        // childRelativePosition/2,
                        // inverseChildRelativeRotation,
                        true,
                        true
                        );
        // ==================================================================================
        */

        PhysicsScene.Constraints.AddConstraint(constrain);

        // zero linear and angular limits makes the objects unable to move in relation to each other
        constrain.SetLinearLimits(OMV.Vector3.Zero, OMV.Vector3.Zero);
        constrain.SetAngularLimits(OMV.Vector3.Zero, OMV.Vector3.Zero);

        // tweek the constraint to increase stability
        constrain.UseFrameOffset(PhysicsScene.BoolNumeric(PhysicsScene.Params.linkConstraintUseFrameOffset));
        constrain.TranslationalLimitMotor(PhysicsScene.BoolNumeric(PhysicsScene.Params.linkConstraintEnableTransMotor),
                        PhysicsScene.Params.linkConstraintTransMotorMaxVel,
                        PhysicsScene.Params.linkConstraintTransMotorMaxForce);
        constrain.SetCFMAndERP(PhysicsScene.Params.linkConstraintCFM, PhysicsScene.Params.linkConstraintERP);
        if (PhysicsScene.Params.linkConstraintSolverIterations != 0f)
        {
            constrain.SetSolverIterations(PhysicsScene.Params.linkConstraintSolverIterations);
        }
    }

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

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

        return ret;
    }

    // Remove linkage between myself and any possible children I might have.
    // Called at taint time!
    private bool PhysicallyUnlinkAllChildrenFromRoot(BSPhysObject rootPrim)
    {
        DetailLog("{0},BSLinkset.PhysicallyUnlinkAllChildren,taint", rootPrim.LocalID);
        bool ret = false;

        if (PhysicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.BSBody))
        {
            ret = true;
        }
        return ret;
    }

    // Call each of the constraints that make up this linkset and recompute the
    //    various transforms and variables. Used when objects are added or removed
    //    from a linkset to make sure the constraints know about the new mass and
    //    geometry.
    // Must only be called at taint time!!
    private void RecomputeLinksetConstraintVariables()
    {
        float linksetMass = LinksetMass;
        foreach (BSPhysObject child in m_taintChildren)
        {
            BSConstraint constrain;
            if (PhysicsScene.Constraints.TryGetConstraint(LinksetRoot.BSBody, child.BSBody, out constrain))
            {
                // DetailLog("{0},BSLinkset.RecomputeLinksetConstraintVariables,taint,child={1},mass={2},A={3},B={4}",
                //         LinksetRoot.LocalID, child.LocalID, linksetMass, constrain.Body1.ID, constrain.Body2.ID);
                constrain.RecomputeConstraintVariables(linksetMass);
            }
            else
            {
                // Non-fatal error that happens when children are being added to the linkset but
                //    their constraints have not been created yet.
                break;
            }
        }

        // If the whole linkset is not here, doesn't make sense to recompute linkset wide values
        if (m_children.Count == m_taintChildren.Count)
        {
            // If this is a multiple object linkset, set everybody's center of mass to the set's center of mass
            OMV.Vector3 centerOfMass = ComputeLinksetCenterOfMass();
            BulletSimAPI.SetCenterOfMassByPosRot2(LinksetRoot.BSBody.ptr, 
                                centerOfMass, OMV.Quaternion.Identity);
            DetailLog("{0},BSLinkset.RecomputeLinksetConstraintVariables,setCenterOfMass,COM={1},rBody={2}",
                                LinksetRoot.LocalID, centerOfMass, LinksetRoot.BSBody.ptr.ToString("X"));
            foreach (BSPhysObject child in m_taintChildren)
            {
                BulletSimAPI.SetCenterOfMassByPosRot2(child.BSBody.ptr, 
                                centerOfMass, OMV.Quaternion.Identity);
            }

            // BulletSimAPI.DumpAllInfo2(PhysicsScene.World.ptr);  // DEBUG DEBUG DEBUG
        }
        return;
    }


    // Invoke the detailed logger and output something if it's enabled.
    private void DetailLog(string msg, params Object[] args)
    {
        if (PhysicsScene.PhysicsLogging.Enabled)
            PhysicsScene.DetailLog(msg, args);
    }

}
}