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

using OMV = OpenMetaverse;

namespace OpenSim.Region.Physics.BulletSPlugin
{

// When a child is linked, the relationship position of the child to the parent
//    is remembered so the child's world position can be recomputed when it is
//    removed from the linkset.
sealed class BSLinksetCompoundInfo : BSLinksetInfo
{
    public int Index;
    public OMV.Vector3 OffsetFromRoot;
    public OMV.Vector3 OffsetFromCenterOfMass;
    public OMV.Quaternion OffsetRot;
    public BSLinksetCompoundInfo(int indx, OMV.Vector3 p, OMV.Quaternion r)
    {
        Index = indx;
        OffsetFromRoot = p;
        OffsetFromCenterOfMass = p;
        OffsetRot = r;
    }
    // 'centerDisplacement' is the distance from the root the the center-of-mass (Bullet 'zero' of the shape)
    public BSLinksetCompoundInfo(int indx, BSPrimLinkable root, BSPrimLinkable child, OMV.Vector3 centerDisplacement)
    {
        // Each child position and rotation is given relative to the center-of-mass.
        OMV.Quaternion invRootOrientation = OMV.Quaternion.Inverse(root.RawOrientation);
        OMV.Vector3 displacementFromRoot = (child.RawPosition - root.RawPosition) * invRootOrientation;
        OMV.Vector3 displacementFromCOM = displacementFromRoot - centerDisplacement;
        OMV.Quaternion displacementRot = child.RawOrientation * invRootOrientation;

        // Save relative position for recomputing child's world position after moving linkset.
        Index = indx;
        OffsetFromRoot = displacementFromRoot;
        OffsetFromCenterOfMass = displacementFromCOM;
        OffsetRot = displacementRot;
    }
    public override void Clear()
    {
        Index = 0;
        OffsetFromRoot = OMV.Vector3.Zero;
        OffsetFromCenterOfMass = OMV.Vector3.Zero;
        OffsetRot = OMV.Quaternion.Identity;
    }
    public override string ToString()
    {
        StringBuilder buff = new StringBuilder();
        buff.Append("<i=");
        buff.Append(Index.ToString());
        buff.Append(",p=");
        buff.Append(OffsetFromRoot.ToString());
        buff.Append(",m=");
        buff.Append(OffsetFromCenterOfMass.ToString());
        buff.Append(",r=");
        buff.Append(OffsetRot.ToString());
        buff.Append(">");
        return buff.ToString();
    }
};

public sealed class BSLinksetCompound : BSLinkset
{
    private static string LogHeader = "[BULLETSIM LINKSET COMPOUND]";

    private BSShape LinksetShape;

    public BSLinksetCompound(BSScene scene, BSPrimLinkable parent)
        : base(scene, parent)
    {
        LinksetShape = new BSShapeNull();
    }

    // When physical properties are changed the linkset needs to recalculate
    //   its internal properties.
    public override void Refresh(BSPrimLinkable requestor)
    {
        base.Refresh(requestor);

        // Something changed so do the rebuilding thing
        // ScheduleRebuild();
    }

    // Schedule a refresh to happen after all the other taint processing.
    private void ScheduleRebuild(BSPrimLinkable requestor)
    {
        DetailLog("{0},BSLinksetCompound.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)
        {
            m_physicsScene.PostTaintObject("BSLinksetCompound.ScheduleRebuild", LinksetRoot.LocalID, delegate()
            {
                if (HasAnyChildren)
                    RecomputeLinksetCompound();
            });
        }
    }

    // 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},BSLinksetCompound.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);
        }

        // The origional prims are removed from the world as the shape of the root compound
        //     shape takes over.
        m_physicsScene.PE.AddToCollisionFlags(child.PhysBody, CollisionFlags.CF_NO_CONTACT_RESPONSE);
        m_physicsScene.PE.ForceActivationState(child.PhysBody, ActivationState.DISABLE_SIMULATION);
        // We don't want collisions from the old linkset children.
        m_physicsScene.PE.RemoveFromCollisionFlags(child.PhysBody, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);

        child.PhysBody.collisionType = CollisionType.LinksetChild;

        ret = true;

        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},BSLinksetCompound.MakeStatic,call,IsRoot={1}", child.LocalID, IsRoot(child));
        if (IsRoot(child))
        {
            ScheduleRebuild(LinksetRoot);
        }
        else
        {
            // The non-physical children can come back to life.
            m_physicsScene.PE.RemoveFromCollisionFlags(child.PhysBody, CollisionFlags.CF_NO_CONTACT_RESPONSE);

            child.PhysBody.collisionType = CollisionType.LinksetChild;

            // Don't force activation so setting of DISABLE_SIMULATION can stay if used.
            m_physicsScene.PE.Activate(child.PhysBody, false);
            ret = true;
        }
        return ret;
    }

    // 'physicalUpdate' is true if these changes came directly from the physics engine. Don't need to rebuild then.
    // Called at taint-time.
    public override void UpdateProperties(UpdatedProperties whichUpdated, BSPrimLinkable updated)
    {
        if (!LinksetRoot.IsPhysicallyActive)
        {
            // No reason to do this physical stuff for static linksets.
            DetailLog("{0},BSLinksetCompound.UpdateProperties,notPhysical", LinksetRoot.LocalID);
            return;
        }

        // The user moving a child around requires the rebuilding of the linkset compound shape
        // One problem is this happens when a border is crossed -- the simulator implementation
        //    stores the position into the group which causes the move of the object
        //    but it also means all the child positions get updated.
        //    What would cause an unnecessary rebuild so we make sure the linkset is in a
        //    region before bothering to do a rebuild.
        if (!IsRoot(updated) && m_physicsScene.TerrainManager.IsWithinKnownTerrain(LinksetRoot.RawPosition))
        {
            // If a child of the linkset is updating only the position or rotation, that can be done
            //    without rebuilding the linkset.
            // If a handle for the child can be fetch, we update the child here. If a rebuild was
            //    scheduled by someone else, the rebuild will just replace this setting.

            bool updatedChild = false;
            // Anything other than updating position or orientation usually means a physical update
            //     and that is caused by us updating the object.
            if ((whichUpdated & ~(UpdatedProperties.Position | UpdatedProperties.Orientation)) == 0)
            {
                // Find the physical instance of the child
                if (LinksetRoot.PhysShape.HasPhysicalShape && m_physicsScene.PE.IsCompound(LinksetRoot.PhysShape.physShapeInfo))
                {
                    // It is possible that the linkset is still under construction and the child is not yet
                    //    inserted into the compound shape. A rebuild of the linkset in a pre-step action will
                    //    build the whole thing with the new position or rotation.
                    // The index must be checked because Bullet references the child array but does no validity
                    //    checking of the child index passed.
                    int numLinksetChildren = m_physicsScene.PE.GetNumberOfCompoundChildren(LinksetRoot.PhysShape.physShapeInfo);
                    if (updated.LinksetChildIndex < numLinksetChildren)
                    {
                        BulletShape linksetChildShape = m_physicsScene.PE.GetChildShapeFromCompoundShapeIndex(LinksetRoot.PhysShape.physShapeInfo, updated.LinksetChildIndex);
                        if (linksetChildShape.HasPhysicalShape)
                        {
                            // Found the child shape within the compound shape
                            m_physicsScene.PE.UpdateChildTransform(LinksetRoot.PhysShape.physShapeInfo, updated.LinksetChildIndex,
                                                                        updated.RawPosition - LinksetRoot.RawPosition,
                                                                        updated.RawOrientation * OMV.Quaternion.Inverse(LinksetRoot.RawOrientation),
                                                                        true /* shouldRecalculateLocalAabb */);
                            updatedChild = true;
                            DetailLog("{0},BSLinksetCompound.UpdateProperties,changeChildPosRot,whichUpdated={1},pos={2},rot={3}",
                                                        updated.LocalID, whichUpdated, updated.RawPosition, updated.RawOrientation);
                        }
                        else    // DEBUG DEBUG
                        {       // DEBUG DEBUG
                            DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild,noChildShape,shape={1}",
                                                                        updated.LocalID, linksetChildShape);
                        }       // DEBUG DEBUG
                    }
                    else    // DEBUG DEBUG
                    {       // DEBUG DEBUG
                        // the child is not yet in the compound shape. This is non-fatal.
                        DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild,childNotInCompoundShape,numChildren={1},index={2}",
                                                                    updated.LocalID, numLinksetChildren, updated.LinksetChildIndex);
                    }       // DEBUG DEBUG
                }
                else    // DEBUG DEBUG
                {       // DEBUG DEBUG
                    DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild,noBodyOrNotCompound", updated.LocalID);
                }       // DEBUG DEBUG

                if (!updatedChild)
                {
                    // If couldn't do the individual child, the linkset needs a rebuild to incorporate the new child info.
                    // Note: there are several ways through this code that will not update the child if
                    //    the linkset is being rebuilt. In this case, scheduling a rebuild is a NOOP since
                    //    there will already be a rebuild scheduled.
                    DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild.schedulingRebuild,whichUpdated={1}",
                                                                    updated.LocalID, whichUpdated);
                    updated.LinksetInfo = null; // setting to 'null' causes relative position to be recomputed.
                    ScheduleRebuild(updated);
                }
            }
        }
    }

    // Routine called when rebuilding the body of some member of the linkset.
    // Since we don't keep in world relationships, do nothing unless it's a child changing.
    // 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},BSLinksetCompound.RemoveBodyDependencies,refreshIfChild,rID={1},rBody={2},isRoot={3}",
                        child.LocalID, LinksetRoot.LocalID, LinksetRoot.PhysBody, IsRoot(child));

        if (!IsRoot(child))
        {
            // Because it is a convenient time, recompute child world position and rotation based on
            //    its position in the linkset.
            RecomputeChildWorldPosition(child, true /* inTaintTime */);
            child.LinksetInfo = null;
        }

        // Cannot schedule a refresh/rebuild here because this routine is called when
        //     the linkset is being rebuilt.
        // InternalRefresh(LinksetRoot);

        return ret;
    }

    // When the linkset is built, the child shape is added to the compound shape relative to the
    //    root shape. The linkset then moves around but this does not move the actual child
    //    prim. The child prim's location must be recomputed based on the location of the root shape.
    private void RecomputeChildWorldPosition(BSPrimLinkable child, bool inTaintTime)
    {
        // For the moment (20130201), disable this computation (converting the child physical addr back to
        //    a region address) until we have a good handle on center-of-mass offsets and what the physics
        //    engine moving a child actually means.
        // The simulator keeps track of where children should be as the linkset moves. Setting
        //    the pos/rot here does not effect that knowledge as there is no good way for the
        //    physics engine to send the simulator an update for a child.

        /*
        BSLinksetCompoundInfo lci = child.LinksetInfo as BSLinksetCompoundInfo;
        if (lci != null)
        {
            if (inTaintTime)
            {
                OMV.Vector3 oldPos = child.RawPosition;
                child.ForcePosition = LinksetRoot.RawPosition + lci.OffsetFromRoot;
                child.ForceOrientation = LinksetRoot.RawOrientation * lci.OffsetRot;
                DetailLog("{0},BSLinksetCompound.RecomputeChildWorldPosition,oldPos={1},lci={2},newPos={3}",
                                            child.LocalID, oldPos, lci, child.RawPosition);
            }
            else
            {
                // TaintedObject is not used here so the raw position is set now and not at taint-time.
                child.Position = LinksetRoot.RawPosition + lci.OffsetFromRoot;
                child.Orientation = LinksetRoot.RawOrientation * lci.OffsetRot;
            }
        }
        else
        {
            // This happens when children have been added to the linkset but the linkset
            //     has not been constructed yet. So like, at taint time, adding children to a linkset
            //     and then changing properties of the children (makePhysical, for instance)
            //     but the post-print action of actually rebuilding the linkset has not yet happened.
            // PhysicsScene.Logger.WarnFormat("{0} Restoring linkset child position failed because of no relative position computed. ID={1}",
            //                                 LogHeader, child.LocalID);
            DetailLog("{0},BSLinksetCompound.recomputeChildWorldPosition,noRelativePositonInfo", child.LocalID);
        }
        */
    }

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

    // 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);

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

            // Rebuild the compound shape with the new child shape included
            ScheduleRebuild(child);
        }
        return;
    }

    // Remove the specified child from the linkset.
    // Safe to call even if the child is not really in the linkset.
    protected override void RemoveChildFromLinkset(BSPrimLinkable child)
    {
        child.ClearDisplacement();

        if (m_children.Remove(child))
        {
            DetailLog("{0},BSLinksetCompound.RemoveChildFromLinkset,call,rID={1},rBody={2},cID={3},cBody={4}",
                            child.LocalID,
                            LinksetRoot.LocalID, LinksetRoot.PhysBody.AddrString,
                            child.LocalID, child.PhysBody.AddrString);

            // Cause the child's body to be rebuilt and thus restored to normal operation
            RecomputeChildWorldPosition(child, false);
            child.LinksetInfo = null;
            child.ForceBodyShapeRebuild(false);

            if (!HasAnyChildren)
            {
                // The linkset is now empty. The root needs rebuilding.
                LinksetRoot.ForceBodyShapeRebuild(false);
            }
            else
            {
                // Rebuild the compound shape with the child removed
                ScheduleRebuild(LinksetRoot);
            }
        }
        return;
    }

    // Called before the simulation step to make sure the compound based linkset
    //    is all initialized.
    // Constraint linksets are rebuilt every time.
    // Note that this works for rebuilding just the root after a linkset is taken apart.
    // Called at taint time!!
    private bool disableCOM = true;     // DEBUG DEBUG: disable until we get this debugged
    private void RecomputeLinksetCompound()
    {
        if (!LinksetRoot.IsPhysicallyActive)
        {
            // There is no reason to build all this physical stuff for a non-physical linkset
            DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,notPhysical", LinksetRoot.LocalID);
            return;
        }

        try
        {
            // This replaces the physical shape of the root prim with a compound shape made up of the root
            // shape and all the children.

            // RootPrim.PhysShape is the shape of the root prim.
            // Here we build the compound shape made up of all the children.

            // Free up any shape we'd previously built.
            LinksetShape.Dereference(m_physicsScene);

            // Get a new compound shape to build the linkset shape in.
            LinksetShape = BSShapeCompound.GetReference(m_physicsScene);

            // The center of mass for the linkset is the geometric center of the group.
            // Compute a displacement for each component so it is relative to the center-of-mass.
            // Bullet presumes an object's origin (relative <0,0,0>) is its center-of-mass
            OMV.Vector3 centerOfMassW = ComputeLinksetCenterOfMass();

            OMV.Quaternion invRootOrientation = OMV.Quaternion.Inverse(LinksetRoot.RawOrientation);

            // 'centerDisplacement' is the value to subtract from children to give physical offset position
            OMV.Vector3 centerDisplacement = (centerOfMassW - LinksetRoot.RawPosition) * invRootOrientation;
            LinksetRoot.SetEffectiveCenterOfMassW(centerDisplacement);

            // TODO: add phantom root shape to be the center-of-mass

            // Add a shape for each of the other children in the linkset
            int memberIndex = 1;
            ForEachMember(delegate(BSPrimLinkable cPrim)
            {
                if (IsRoot(cPrim))
                {
                    cPrim.LinksetChildIndex = 0;
                }
                else
                {
                    cPrim.LinksetChildIndex = memberIndex;
                }

                BSShape childShape = cPrim.PhysShape.GetReference(m_physicsScene, cPrim);
                OMV.Vector3 offsetPos = (cPrim.RawPosition - LinksetRoot.RawPosition) * invRootOrientation - centerDisplacement;
                OMV.Quaternion offsetRot = cPrim.RawOrientation * invRootOrientation;
                m_physicsScene.PE.AddChildShapeToCompoundShape(LinksetShape.physShapeInfo, childShape.physShapeInfo, offsetPos, offsetRot);
                DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addChild,indx={1},rShape={2},cShape={3},offPos={4},offRot={5}",
                                    LinksetRoot.LocalID, memberIndex, LinksetRoot.PhysShape, cPrim.PhysShape, offsetPos, offsetRot);

                memberIndex++;

                return false;   // 'false' says to move onto the next child in the list
            });

            // Sneak the built compound shape in as the shape of the root prim.
            // Note this doesn't touch the root prim's PhysShape so be sure the manage the difference.
            // Object removed and added to world to get collision cache rebuilt for new shape.
            m_physicsScene.PE.RemoveObjectFromWorld(m_physicsScene.World, LinksetRoot.PhysBody);
            m_physicsScene.PE.SetCollisionShape(m_physicsScene.World, LinksetRoot.PhysBody, LinksetShape.physShapeInfo);
            m_physicsScene.PE.AddObjectToWorld(m_physicsScene.World, LinksetRoot.PhysBody);

            // With all of the linkset packed into the root prim, it has the mass of everyone.
            LinksetMass = ComputeLinksetMass();
            LinksetRoot.UpdatePhysicalMassProperties(LinksetMass, true);

            // Enable the physical position updator to return the position and rotation of the root shape
            m_physicsScene.PE.AddToCollisionFlags(LinksetRoot.PhysBody, CollisionFlags.BS_RETURN_ROOT_COMPOUND_SHAPE);
        }
        finally
        {
            Rebuilding = false;
        }

        // See that the Aabb surrounds the new shape
        m_physicsScene.PE.RecalculateCompoundShapeLocalAabb(LinksetRoot.PhysShape.physShapeInfo);
    }
}
}