opensim/OpenSim/Region/PhysicsModules/BulletS/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.PhysicsModule.BulletS
{
    public sealed class BSLinksetCompound : BSLinkset
    {
#pragma warning disable 414
        private static string LogHeader = "[BULLETSIM LINKSET COMPOUND]";
#pragma warning restore 414

        public BSLinksetCompound(BSScene scene, BSPrimLinkable parent)
            : base(scene, parent)
        {
            LinksetImpl = LinksetImplementation.Compound;
        }
	
        // ================================================================
        // Changing the physical property of the linkset only needs to change the root
        public override void SetPhysicalFriction(float friction)
        {
            if (LinksetRoot.PhysBody.HasPhysicalBody)
                m_physicsScene.PE.SetFriction(LinksetRoot.PhysBody, friction);
        }
        public override void SetPhysicalRestitution(float restitution)
        {
            if (LinksetRoot.PhysBody.HasPhysicalBody)
                m_physicsScene.PE.SetRestitution(LinksetRoot.PhysBody, restitution);
        }
        public override void SetPhysicalGravity(OMV.Vector3 gravity)
        {
            if (LinksetRoot.PhysBody.HasPhysicalBody)
                m_physicsScene.PE.SetGravity(LinksetRoot.PhysBody, gravity);
        }
        public override void ComputeAndSetLocalInertia(OMV.Vector3 inertiaFactor, float linksetMass)
        {
            OMV.Vector3 inertia = m_physicsScene.PE.CalculateLocalInertia(LinksetRoot.PhysShape.physShapeInfo, linksetMass);
            LinksetRoot.Inertia = inertia * inertiaFactor;
            m_physicsScene.PE.SetMassProps(LinksetRoot.PhysBody, linksetMass, LinksetRoot.Inertia);
            m_physicsScene.PE.UpdateInertiaTensor(LinksetRoot.PhysBody);
        }
        public override void SetPhysicalCollisionFlags(CollisionFlags collFlags)
        {
            if (LinksetRoot.PhysBody.HasPhysicalBody)
                m_physicsScene.PE.SetCollisionFlags(LinksetRoot.PhysBody, collFlags);
        }
        public override void AddToPhysicalCollisionFlags(CollisionFlags collFlags)
        {
            if (LinksetRoot.PhysBody.HasPhysicalBody)
                m_physicsScene.PE.AddToCollisionFlags(LinksetRoot.PhysBody, collFlags);
        }
        public override void RemoveFromPhysicalCollisionFlags(CollisionFlags collFlags)
        {
            if (LinksetRoot.PhysBody.HasPhysicalBody)
                m_physicsScene.PE.RemoveFromCollisionFlags(LinksetRoot.PhysBody, collFlags);
        }
        // ================================================================
	
        // When physical properties are changed the linkset needs to recalculate
        //   its internal properties.
        public override void Refresh(BSPrimLinkable requestor)
        {
            // Something changed so do the rebuilding thing
            ScheduleRebuild(requestor);
            base.Refresh(requestor);
        }
	
        // Schedule a refresh to happen after all the other taint processing.
        private void ScheduleRebuild(BSPrimLinkable requestor)
        {
            // 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.
            lock (m_linksetActivityLock)
            {
                if (!RebuildScheduled && !Rebuilding && HasAnyChildren)
                {
                    InternalScheduleRebuild(requestor);
                }
            }
        }
	
        // Must be called with m_linksetActivityLock or race conditions will haunt you.
        private void InternalScheduleRebuild(BSPrimLinkable requestor)
        {
            DetailLog("{0},BSLinksetCompound.InternalScheduleRebuild,,rebuilding={1},hasChildren={2}",
                                requestor.LocalID, Rebuilding, HasAnyChildren);
            RebuildScheduled = true;
            m_physicsScene.PostTaintObject("BSLinksetCompound.ScheduleRebuild", LinksetRoot.LocalID, delegate()
            {
                if (HasAnyChildren)
                {
                    if (this.AllPartsComplete)
                    {
                        RecomputeLinksetCompound();
                    }
                    else
                    {
                        DetailLog("{0},BSLinksetCompound.InternalScheduleRebuild,,rescheduling because not all children complete",
                                                        requestor.LocalID);
                        InternalScheduleRebuild(requestor);
                    }
                }
                RebuildScheduled = false;
            });
        }
	
        // 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.
                Refresh(LinksetRoot);
            }
            return ret;
        }
	
        // The object is going static (non-physical). We do not do anything for static linksets.
        // Return 'true' if any properties updated on the passed object.
        // Called at taint-time!
        public override bool MakeStatic(BSPrimLinkable child)
        {
            bool ret = false;
	
            DetailLog("{0},BSLinksetCompound.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.
                Refresh(LinksetRoot);
            }
            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 (!RebuildScheduled   // if rebuilding, let the rebuild do it
                            && !LinksetRoot.IsIncomplete    // if waiting for assets or whatever, don't change
                            && LinksetRoot.PhysShape.HasPhysicalShape   // there must be a physical shape assigned
                            && 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);
                        Refresh(updated);
                    }
                }
            }
        }
	
        // Routine called when rebuilding the body of some member of the linkset.
        // If one of the bodies is being changed, the linkset needs rebuilding.
        // For instance, a linkset is built and then a mesh asset is read in and the mesh is recreated.
        // 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.RemoveDependencies,refreshIfChild,rID={1},rBody={2},isRoot={3}",
                            child.LocalID, LinksetRoot.LocalID, LinksetRoot.PhysBody, IsRoot(child));
	
            Refresh(child);
	
            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 BSLinkInfo(child));
	
                DetailLog("{0},BSLinksetCompound.AddChildToLinkset,call,child={1}", LinksetRoot.LocalID, child.LocalID);
	
                // Rebuild the compound shape with the new child shape included
                Refresh(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, bool inTaintTime)
        {
            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
                child.ForceBodyShapeRebuild(inTaintTime);
	
                if (!HasAnyChildren)
                {
                    // The linkset is now empty. The root needs rebuilding.
                    LinksetRoot.ForceBodyShapeRebuild(inTaintTime);
                }
                else
                {
                    // Rebuild the compound shape with the child removed
                    Refresh(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 UseBulletSimRootOffsetHack = false;    // Attempt to have Bullet track the coords of root compound shape
        private void RecomputeLinksetCompound()
        {
            try
            {
                Rebuilding = true;
	
                // No matter what is being done, force the root prim's PhysBody and PhysShape to get set
                //     to what they should be as if the root was not in a linkset.
                // Not that bad since we only get into this routine if there are children in the linkset and
                //     something has been updated/changed.
                // Have to do the rebuild before checking for physical because this might be a linkset
                //     being destructed and going non-physical.
                LinksetRoot.ForceBodyShapeRebuild(true);
	
                // There is no reason to build all this physical stuff for a non-physical or empty linkset.
                if (!LinksetRoot.IsPhysicallyActive || !HasAnyChildren)
                {
                    DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,notPhysicalOrNoChildren", LinksetRoot.LocalID);
                    return; // Note the 'finally' clause at the botton which will get executed.
                }
	
                // Get a new compound shape to build the linkset shape in.
                BSShape linksetShape = BSShapeCompound.GetReference(m_physicsScene);
	
                // 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.Normalize(OMV.Quaternion.Inverse(LinksetRoot.RawOrientation));
                OMV.Vector3 origRootPosition = LinksetRoot.RawPosition;
	
                // 'centerDisplacementV' is the vehicle relative distance from the simulator root position to the center-of-mass
                OMV.Vector3 centerDisplacementV = (centerOfMassW - LinksetRoot.RawPosition) * invRootOrientation;
                if (UseBulletSimRootOffsetHack || !BSParam.LinksetOffsetCenterOfMass)
                {
                    // Zero everything if center-of-mass displacement is not being done.
                    centerDisplacementV = OMV.Vector3.Zero;
                    LinksetRoot.ClearDisplacement();
                }
                else
                {
                    // The actual center-of-mass could have been set by the user.
                    centerDisplacementV = LinksetRoot.SetEffectiveCenterOfMassDisplacement(centerDisplacementV);
                }
	
                DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,COM,rootPos={1},com={2},comDisp={3}",
                                    LinksetRoot.LocalID, origRootPosition, centerOfMassW, centerDisplacementV);
	
                // Add the shapes of all the components of the linkset
                int memberIndex = 1;
                ForEachMember((cPrim) =>
                {
                    if (IsRoot(cPrim))
                    {
                        // Root shape is always index zero.
                        cPrim.LinksetChildIndex = 0;
                    }
                    else
                    {
                        cPrim.LinksetChildIndex = memberIndex;
                        memberIndex++;
                    }
	
                    // Get a reference to the shape of the child for adding of that shape to the linkset compound shape
                    BSShape childShape = cPrim.PhysShape.GetReference(m_physicsScene, cPrim);
	
                    // Offset the child shape from the center-of-mass and rotate it to root relative.
                    OMV.Vector3 offsetPos = (cPrim.RawPosition - origRootPosition) * invRootOrientation - centerDisplacementV;
                    OMV.Quaternion offsetRot = OMV.Quaternion.Normalize(cPrim.RawOrientation) * invRootOrientation;
	
                    // Add the child shape to the compound shape being built
                    if (childShape.physShapeInfo.HasPhysicalShape)
                    {
                        m_physicsScene.PE.AddChildShapeToCompoundShape(linksetShape.physShapeInfo, childShape.physShapeInfo, offsetPos, offsetRot);
                        DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addChild,indx={1},cShape={2},offPos={3},offRot={4}",
                                    LinksetRoot.LocalID, cPrim.LinksetChildIndex, childShape, offsetPos, offsetRot);
	
                        // Since we are borrowing the shape of the child, disable the origional child body
                        if (!IsRoot(cPrim))
                        {
                            m_physicsScene.PE.AddToCollisionFlags(cPrim.PhysBody, CollisionFlags.CF_NO_CONTACT_RESPONSE);
                            m_physicsScene.PE.ForceActivationState(cPrim.PhysBody, ActivationState.DISABLE_SIMULATION);
                            // We don't want collisions from the old linkset children.
                            m_physicsScene.PE.RemoveFromCollisionFlags(cPrim.PhysBody, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
                            cPrim.PhysBody.collisionType = CollisionType.LinksetChild;
                        }
                    }
                    else
                    {
                        // The linkset must be in an intermediate state where all the children have not yet
                        //    been constructed. This sometimes happens on startup when everything is getting
                        //    built and some shapes have to wait for assets to be read in.
                        // Just skip this linkset for the moment and cause the shape to be rebuilt next tick.
                        // One problem might be that the shape is broken somehow and it never becomes completely
                        //    available. This might cause the rebuild to happen over and over.
                        InternalScheduleRebuild(LinksetRoot);
                        DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addChildWithNoShape,indx={1},cShape={2},offPos={3},offRot={4}",
                                        LinksetRoot.LocalID, cPrim.LinksetChildIndex, childShape, offsetPos, offsetRot);
                        // Output an annoying warning. It should only happen once but if it keeps coming out,
                        //    the user knows there is something wrong and will report it.
                        m_physicsScene.Logger.WarnFormat("{0} Linkset rebuild warning. If this happens more than one or two times, please report in Mantis 7191", LogHeader);
                        m_physicsScene.Logger.WarnFormat("{0} pName={1}, childIdx={2}, shape={3}",
                                        LogHeader, LinksetRoot.Name, cPrim.LinksetChildIndex, childShape);
	
                        // This causes the loop to bail on building the rest of this linkset.
                        // The rebuild operation will fix it up next tick or declare the object unbuildable.
                        return true;
                    }
	
                    return false;   // 'false' says to move onto the next child in the list
                });
	
                // Replace the root shape with the built compound shape.
                // Object removed and added to world to get collision cache rebuilt for new shape.
                LinksetRoot.PhysShape.Dereference(m_physicsScene);
                LinksetRoot.PhysShape = linksetShape;
                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);
                DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addBody,body={1},shape={2}",
                                            LinksetRoot.LocalID, LinksetRoot.PhysBody, linksetShape);
                m_physicsScene.PE.ResetBroadphasePool(m_physicsScene.World);    // DEBUG DEBUG
	
                // With all of the linkset packed into the root prim, it has the mass of everyone.
                LinksetMass = ComputeLinksetMass();
                LinksetRoot.UpdatePhysicalMassProperties(LinksetMass, true);
	
                if (UseBulletSimRootOffsetHack)
                {
                    // Enable the physical position updator to return the position and rotation of the root shape.
                    // This enables a feature in the C++ code to return the world coordinates of the first shape in the
                    //     compound shape. This aleviates the need to offset the returned physical position by the
                    //     center-of-mass offset.
                    // TODO: either debug this feature or remove it.
                    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);
        }
    }
}