OpenSim-Test/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 BSLinksetConstraints(BSScene scene, BSPhysObject parent)
    {
        base.Initialize(scene, parent);
    }

    // When physical properties are changed the linkset needs to recalculate
    //   its internal properties.
    // May be called at runtime or taint-time (just pass the appropriate flag).
    public override void Refresh(BSPhysObject requestor, bool inTaintTime)
    {
        // If there are no children or not root, I am not the one that recomputes the constraints
        if (!HasAnyChildren || !IsRoot(requestor))
            return;

        BSScene.TaintCallback refreshOperation = delegate()
            {
                RecomputeLinksetConstraintVariables();
                DetailLog("{0},BSLinkset.Refresh,complete,rBody={1}",
                                LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"));
            };
        if (inTaintTime)
            refreshOperation();
        else
            PhysicsScene.TaintedObject("BSLinkSet.Refresh", refreshOperation);
    }

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

    // Called at taint-time!!
    public override void UpdateProperties(BSPhysObject updated)
    {
        // Nothing to do for constraints on property updates
    }

    // 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 override 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;
    }

    // Companion to RemoveBodyDependencies(). If RemoveBodyDependencies() returns 'true',
    // this routine will restore the removed constraints.
    // Called at taint-time!!
    public override 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

    // I am the root of a linkset and a new child is being added
    // Called while LinkActivity is locked.
    protected override 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).
    protected override void RemoveChildFromOtherLinkset(BSPhysObject pchild)
    {
        pchild.Linkset = BSLinkset.Factory(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.
    protected override 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;
    }
}
}