3 files changed, 412 insertions, 54 deletions

diff --git a/OpenSim/Region/Environment/Scenes/Scene.cs b/OpenSim/Region/Environment/Scenes/Scene.cs
index d4d5b90..2d0743c 100644
--- a/OpenSim/Region/Environment/Scenes/Scene.cs
+++ b/OpenSim/Region/Environment/Scenes/Scene.cs
@@ -200,8 +200,30 @@ namespace OpenSim.Region.Environment.Scenes
         // an instance to the physics plugin's Scene object.
         public PhysicsScene PhysicsScene
         {
-            set { m_sceneGraph.PhysicsScene = value; }
             get { return m_sceneGraph.PhysicsScene; }
+            set
+            {
+                // If we're not doing the initial set
+                // Then we've got to remove the previous
+                // event handler
+                if (PhysicsScene != null && PhysicsScene.SupportsNINJAJoints)
+                {
+                    PhysicsScene.OnJointMoved -= jointMoved;
+                    PhysicsScene.OnJointDeactivated -= jointDeactivated;
+                    PhysicsScene.OnJointErrorMessage -= jointErrorMessage;
+                }
+
+                m_sceneGraph.PhysicsScene = value;
+
+                if (PhysicsScene != null && m_sceneGraph.PhysicsScene.SupportsNINJAJoints)
+                {
+                    // register event handlers to respond to joint movement/deactivation
+                    PhysicsScene.OnJointMoved += jointMoved;
+                    PhysicsScene.OnJointDeactivated += jointDeactivated;
+                    PhysicsScene.OnJointErrorMessage += jointErrorMessage;
+                }
+
+            }
         }
 
         // This gets locked so things stay thread safe.
@@ -1848,7 +1870,11 @@ namespace OpenSim.Region.Environment.Scenes
 
             foreach (SceneObjectPart part in group.Children.Values)
             {
-                if (part.PhysActor != null)
+                if (part.IsJoint() && ((part.ObjectFlags&(uint)PrimFlags.Physics) != 0) )
+                {
+                    PhysicsScene.RequestJointDeletion(part.Name); // FIXME: what if the name changed?
+                }
+                else if (part.PhysActor != null)
                 {
                     PhysicsScene.RemovePrim(part.PhysActor);
                     part.PhysActor = null;
@@ -4408,5 +4434,142 @@ namespace OpenSim.Region.Environment.Scenes
 
             return 0;
         }
+
+        // This callback allows the PhysicsScene to call back to its caller (the SceneGraph) and
+        // update non-physical objects like the joint proxy objects that represent the position
+        // of the joints in the scene.
+
+        // This routine is normally called from within a lock(OdeLock) from within the OdePhysicsScene
+        // WARNING: be careful of deadlocks here if you manipulate the scene. Remember you are being called
+        // from within the OdePhysicsScene. 
+
+        protected internal void jointMoved(PhysicsJoint joint)
+        {
+
+            // m_parentScene.PhysicsScene.DumpJointInfo(); // non-thread-locked version; we should already be in a lock(OdeLock) when this callback is invoked
+            // FIXME: this causes a sequential lookup of all objects in the scene; use a dictionary
+            SceneObjectPart jointProxyObject = GetSceneObjectPart(joint.ObjectNameInScene);
+            if (jointProxyObject == null)
+            {
+                jointErrorMessage(joint, "WARNING, joint proxy not found, name " + joint.ObjectNameInScene);
+                return;
+            }
+
+            // now update the joint proxy object in the scene to have the position of the joint as returned by the physics engine
+            SceneObjectPart trackedBody = GetSceneObjectPart(joint.TrackedBodyName); // FIXME: causes a sequential lookup
+            if (trackedBody == null) return; // the actor may have been deleted but the joint still lingers around a few frames waiting for deletion. during this time, trackedBody is NULL to prevent further motion of the joint proxy.
+            jointProxyObject.Velocity = trackedBody.Velocity;
+            jointProxyObject.RotationalVelocity = trackedBody.RotationalVelocity;
+            switch (joint.Type)
+            {
+                case PhysicsJointType.Ball:
+                    {
+                        PhysicsVector jointAnchor = PhysicsScene.GetJointAnchor(joint);
+                        Vector3 proxyPos = new Vector3(jointAnchor.X, jointAnchor.Y, jointAnchor.Z);
+                        jointProxyObject.ParentGroup.UpdateGroupPosition(proxyPos); // schedules the entire group for a terse update
+                    }
+                    break;
+
+                case PhysicsJointType.Hinge:
+                    {
+                        PhysicsVector jointAnchor = PhysicsScene.GetJointAnchor(joint);
+
+                        // Normally, we would just ask the physics scene to return the axis for the joint.
+                        // Unfortunately, ODE sometimes returns <0,0,0> for the joint axis, which should
+                        // never occur. Therefore we cannot rely on ODE to always return a correct joint axis.
+                        // Therefore the following call does not always work:
+                        //PhysicsVector phyJointAxis = _PhyScene.GetJointAxis(joint);
+
+                        // instead we compute the joint orientation by saving the original joint orientation
+                        // relative to one of the jointed bodies, and applying this transformation
+                        // to the current position of the jointed bodies (the tracked body) to compute the
+                        // current joint orientation.
+
+                        if (joint.TrackedBodyName == null)
+                        {
+                            jointErrorMessage(joint, "joint.TrackedBodyName is null, joint " + joint.ObjectNameInScene);
+                        }
+
+                        Vector3 proxyPos = new Vector3(jointAnchor.X, jointAnchor.Y, jointAnchor.Z);
+                        Quaternion q = trackedBody.RotationOffset * joint.LocalRotation;
+
+                        jointProxyObject.ParentGroup.UpdateGroupPosition(proxyPos); // schedules the entire group for a terse update
+                        jointProxyObject.ParentGroup.UpdateGroupRotation(q); // schedules the entire group for a terse update
+                    }
+                    break;
+            }
+        }
+
+        // This callback allows the PhysicsScene to call back to its caller (the SceneGraph) and
+        // update non-physical objects like the joint proxy objects that represent the position
+        // of the joints in the scene.
+
+        // This routine is normally called from within a lock(OdeLock) from within the OdePhysicsScene
+        // WARNING: be careful of deadlocks here if you manipulate the scene. Remember you are being called
+        // from within the OdePhysicsScene.
+        protected internal void jointDeactivated(PhysicsJoint joint)
+        {
+            //m_log.Debug("[NINJA] SceneGraph.jointDeactivated, joint:" + joint.ObjectNameInScene);
+            // FIXME: this causes a sequential lookup of all objects in the scene; use a dictionary
+            SceneObjectPart jointProxyObject = GetSceneObjectPart(joint.ObjectNameInScene);
+            if (jointProxyObject == null)
+            {
+                jointErrorMessage(joint, "WARNING, trying to deactivate (stop interpolation of) joint proxy, but not found, name " + joint.ObjectNameInScene);
+                return;
+            }
+
+            // turn the proxy non-physical, which also stops its client-side interpolation
+            bool wasUsingPhysics = ((jointProxyObject.ObjectFlags & (uint)PrimFlags.Physics) != 0);
+            if (wasUsingPhysics)
+            {
+                jointProxyObject.UpdatePrimFlags(false, false, true, false); // FIXME: possible deadlock here; check to make sure all the scene alterations set into motion here won't deadlock
+            }
+        }
+
+        // This callback allows the PhysicsScene to call back to its caller (the SceneGraph) and
+        // alert the user of errors by using the debug channel in the same way that scripts alert
+        // the user of compile errors.
+
+        // This routine is normally called from within a lock(OdeLock) from within the OdePhysicsScene
+        // WARNING: be careful of deadlocks here if you manipulate the scene. Remember you are being called
+        // from within the OdePhysicsScene.
+        public void jointErrorMessage(PhysicsJoint joint, string message)
+        {
+            // FIXME: this causes a sequential lookup of all objects in the scene; use a dictionary
+            if (joint != null)
+            {
+                if (joint.ErrorMessageCount > PhysicsJoint.maxErrorMessages) 
+                    return;
+
+                SceneObjectPart jointProxyObject = GetSceneObjectPart(joint.ObjectNameInScene);
+                if (jointProxyObject != null)
+                {
+                    SimChat(Utils.StringToBytes("[NINJA] " + message),
+                        ChatTypeEnum.DebugChannel,
+                        2147483647,
+                        jointProxyObject.AbsolutePosition,
+                        jointProxyObject.Name,
+                        jointProxyObject.UUID,
+                        false);
+
+                    joint.ErrorMessageCount++;
+
+                    if (joint.ErrorMessageCount > PhysicsJoint.maxErrorMessages)
+                    {
+                        SimChat(Utils.StringToBytes("[NINJA] Too many messages for this joint, suppressing further messages."),
+                            ChatTypeEnum.DebugChannel,
+                            2147483647,
+                            jointProxyObject.AbsolutePosition,
+                            jointProxyObject.Name,
+                            jointProxyObject.UUID,
+                            false);
+                    }
+                }
+                else
+                {
+                    // couldn't find the joint proxy object; the error message is silently suppressed
+                }
+            }
+        }
     }
 }
diff --git a/OpenSim/Region/Environment/Scenes/SceneGraph.cs b/OpenSim/Region/Environment/Scenes/SceneGraph.cs
index dbb8539..d998dbb 100644
--- a/OpenSim/Region/Environment/Scenes/SceneGraph.cs
+++ b/OpenSim/Region/Environment/Scenes/SceneGraph.cs
@@ -159,6 +159,22 @@ namespace OpenSim.Region.Environment.Scenes
         {
             lock (m_syncRoot)
             {
+                // Here is where the Scene calls the PhysicsScene. This is a one-way
+                // interaction; the PhysicsScene cannot access the calling Scene directly.
+                // But with joints, we want a PhysicsActor to be able to influence a
+                // non-physics SceneObjectPart. In particular, a PhysicsActor that is connected
+                // with a joint should be able to move the SceneObjectPart which is the visual
+                // representation of that joint (for editing and serialization purposes).
+                // However the PhysicsActor normally cannot directly influence anything outside
+                // of the PhysicsScene, and the non-physical SceneObjectPart which represents
+                // the joint in the Scene does not exist in the PhysicsScene.
+                //
+                // To solve this, we have an event in the PhysicsScene that is fired when a joint
+                // has changed position (because one of its associated PhysicsActors has changed 
+                // position).
+                //
+                // Therefore, JointMoved and JointDeactivated events will be fired as a result of the following Simulate().
+
                 return _PhyScene.Simulate((float)elapsed);
             }
         }
@@ -875,6 +891,7 @@ namespace OpenSim.Region.Environment.Scenes
         {
             List<EntityBase> EntityList = GetEntities();
 
+            // FIXME: use a dictionary here
             foreach (EntityBase ent in EntityList)
             {
                 if (ent is SceneObjectGroup)
diff --git a/OpenSim/Region/Environment/Scenes/SceneObjectPart.cs b/OpenSim/Region/Environment/Scenes/SceneObjectPart.cs
index 3491645..0f3e065 100644
--- a/OpenSim/Region/Environment/Scenes/SceneObjectPart.cs
+++ b/OpenSim/Region/Environment/Scenes/SceneObjectPart.cs
@@ -419,7 +419,14 @@ namespace OpenSim.Region.Environment.Scenes
         public virtual string Name
         {
             get { return m_name; }
-            set { m_name = value; }
+            set 
+            { 
+                m_name = value;
+                if (PhysActor != null)
+                {
+                    PhysActor.SOPName = value;
+                }
+            }
         }
 
         public byte Material
@@ -681,7 +688,14 @@ namespace OpenSim.Region.Environment.Scenes
         public string Description
         {
             get { return m_description; }
-            set { m_description = value; }
+            set 
+            {
+                m_description = value;
+                if (PhysActor != null)
+                {
+                    PhysActor.SOPDescription = value;
+                }
+            }
         }
 
         public Color Color
@@ -1287,30 +1301,39 @@ if (m_shape != null) {
             bool isPhysical = (((rootObjectFlags & (uint) PrimFlags.Physics) != 0) && m_physicalPrim);
             bool isPhantom = ((rootObjectFlags & (uint) PrimFlags.Phantom) != 0);
 
-            // Special case for VolumeDetection: If VolumeDetection is set, the phantom flag is locally ignored
-            if (VolumeDetectActive)
-                isPhantom = false;
-
-            // Added clarification..   since A rigid body is an object that you can kick around, etc.
-            bool RigidBody = isPhysical && !isPhantom;
-
-            // The only time the physics scene shouldn't know about the prim is if it's phantom or an attachment, which is phantom by definition
-            if (!isPhantom && !IsAttachment)
+            if (IsJoint())
+            {
+                DoPhysicsPropertyUpdate(isPhysical, true);
+            }
+            else
             {
-                PhysActor = m_parentGroup.Scene.PhysicsScene.AddPrimShape(
-                    Name,
-                    Shape,
-                    new PhysicsVector(AbsolutePosition.X, AbsolutePosition.Y, AbsolutePosition.Z),
-                    new PhysicsVector(Scale.X, Scale.Y, Scale.Z),
-                    RotationOffset,
-                    RigidBody);
+                // Special case for VolumeDetection: If VolumeDetection is set, the phantom flag is locally ignored
+                if (VolumeDetectActive)
+                    isPhantom = false;
 
-                // Basic Physics returns null..  joy joy joy.
-                if (PhysActor != null)
+                // Added clarification..   since A rigid body is an object that you can kick around, etc.
+                bool RigidBody = isPhysical && !isPhantom;
+
+                // The only time the physics scene shouldn't know about the prim is if it's phantom or an attachment, which is phantom by definition
+                if (!isPhantom && !IsAttachment)
                 {
-                    PhysActor.LocalID = LocalId;
-                    DoPhysicsPropertyUpdate(RigidBody, true);
-                    PhysActor.SetVolumeDetect(VolumeDetectActive ? 1 : 0);
+                    PhysActor = m_parentGroup.Scene.PhysicsScene.AddPrimShape(
+                        Name,
+                        Shape,
+                        new PhysicsVector(AbsolutePosition.X, AbsolutePosition.Y, AbsolutePosition.Z),
+                        new PhysicsVector(Scale.X, Scale.Y, Scale.Z),
+                        RotationOffset,
+                        RigidBody);
+
+                    // Basic Physics returns null..  joy joy joy.
+                    if (PhysActor != null)
+                    {
+                        PhysActor.SOPName = this.Name; // save object name and desc into the PhysActor so ODE internals know the joint/body info
+                        PhysActor.SOPDescription = this.Description;
+                        PhysActor.LocalID = LocalId;
+                        DoPhysicsPropertyUpdate(RigidBody, true);
+                        PhysActor.SetVolumeDetect(VolumeDetectActive ? 1 : 0);
+                    }
                 }
             }
         }
@@ -1421,57 +1444,160 @@ if (m_shape != null) {
 
         public void DoPhysicsPropertyUpdate(bool UsePhysics, bool isNew)
         {
-            if (PhysActor != null)
+            if (IsJoint())
             {
-                if (UsePhysics != PhysActor.IsPhysical || isNew)
+                if (UsePhysics)
                 {
-                    if (PhysActor.IsPhysical)
+                    // by turning a joint proxy object physical, we cause creation of a joint in the ODE scene.
+                    // note that, as a special case, joints have no bodies or geoms in the physics scene, even though they are physical.
+
+                    PhysicsJointType jointType;
+                    if (IsHingeJoint())
+                    {
+                        jointType = PhysicsJointType.Hinge;
+                    }
+                    else if (IsBallJoint())
+                    {
+                        jointType = PhysicsJointType.Ball;
+                    }
+                    else
+                    {
+                        jointType = PhysicsJointType.Ball;
+                    }
+
+                    List<string> bodyNames = new List<string>();
+                    string RawParams = Description;
+                    string[] jointParams = RawParams.Split(' ');
+                    string trackedBodyName = null;
+                    if (jointParams.Length >= 2)
+                    {
+                        for (int iBodyName = 0; iBodyName < 2; iBodyName++)
+                        {
+                            string bodyName = jointParams[iBodyName];
+                            bodyNames.Add(bodyName);
+                            if (bodyName != "NULL")
+                            {
+                                if (trackedBodyName == null)
+                                {
+                                    trackedBodyName = bodyName;
+                                }
+                            }
+                        }
+                    }
+
+                    SceneObjectPart trackedBody = m_parentGroup.Scene.GetSceneObjectPart(trackedBodyName); // FIXME: causes a sequential lookup
+                    Quaternion localRotation = Quaternion.Identity;
+                    if (trackedBody != null)
                     {
-                        if (!isNew)
-                            ParentGroup.Scene.RemovePhysicalPrim(1);
+                        localRotation = Quaternion.Inverse(trackedBody.RotationOffset) * this.RotationOffset;
+                    }
+                    else
+                    {
+                        // error, output it below
+                    }
 
-                        PhysActor.OnRequestTerseUpdate -= PhysicsRequestingTerseUpdate;
-                        PhysActor.OnOutOfBounds -= PhysicsOutOfBounds;
-                        PhysActor.delink();
+                    PhysicsJoint joint;
+
+                    joint = m_parentGroup.Scene.PhysicsScene.RequestJointCreation(Name, jointType,
+                        new PhysicsVector(AbsolutePosition.X, AbsolutePosition.Y, AbsolutePosition.Z),
+                        this.RotationOffset,
+                        Description,
+                        bodyNames,
+                        trackedBodyName,
+                        localRotation);
+
+                    if (trackedBody == null)
+                    {
+                        ParentGroup.Scene.jointErrorMessage(joint, "warning: tracked body name not found! joint location will not be updated properly. joint: " + Name);
                     }
 
-                    if (!UsePhysics && !isNew)
+                }
+                else
+                {
+                    if (isNew)
                     {
-                        // reset velocity to 0 on physics switch-off. Without that, the client thinks the
-                        // prim still has velocity and continues to interpolate its position along the old
-                        // velocity-vector.
+                        // if the joint proxy is new, and it is not physical, do nothing. There is no joint in ODE to
+                        // delete, and if we try to delete it, due to asynchronous processing, the deletion request
+                        // will get processed later at an indeterminate time, which could cancel a later-arriving
+                        // joint creation request.
+                    }
+                    else
+                    {
+                        // here we turn off the joint object, so remove the joint from the physics scene
+                        m_parentGroup.Scene.PhysicsScene.RequestJointDeletion(Name); // FIXME: what if the name changed?
+
+                        // make sure client isn't interpolating the joint proxy object
                         Velocity = new Vector3(0, 0, 0);
+                        RotationalVelocity = new Vector3(0, 0, 0);
                         Acceleration = new Vector3(0, 0, 0);
-                        AngularVelocity = new Vector3(0, 0, 0);
                     }
+                }
+            }
+            else
+            {
+                if (PhysActor != null)
+                {
+                    if (UsePhysics != PhysActor.IsPhysical || isNew)
+                    {
+                        if (PhysActor.IsPhysical) // implies UsePhysics==false for this block
+                        {
+                            if (!isNew)
+                                ParentGroup.Scene.RemovePhysicalPrim(1);
 
-                    PhysActor.IsPhysical = UsePhysics;
+                            PhysActor.OnRequestTerseUpdate -= PhysicsRequestingTerseUpdate;
+                            PhysActor.OnOutOfBounds -= PhysicsOutOfBounds;
+                            PhysActor.delink();
 
+                            if (ParentGroup.Scene.PhysicsScene.SupportsNINJAJoints && (!isNew))
+                            {
+                                // destroy all joints connected to this now deactivated body
+                                m_parentGroup.Scene.PhysicsScene.RemoveAllJointsConnectedToActorThreadLocked(PhysActor);
+                            }
 
-                    // If we're not what we're supposed to be in the physics scene, recreate ourselves.
-                    //m_parentGroup.Scene.PhysicsScene.RemovePrim(PhysActor);
-                    /// that's not wholesome.  Had to make Scene public
-                    //PhysActor = null;
+                            // stop client-side interpolation of all joint proxy objects that have just been deleted
+                            // this is done because RemoveAllJointsConnectedToActor invokes the OnJointDeactivated callback,
+                            // which stops client-side interpolation of deactivated joint proxy objects.
+                        }
 
-                    if ((ObjectFlags & (uint) PrimFlags.Phantom) == 0)
-                    {
-                        if (UsePhysics)
+                        if (!UsePhysics && !isNew)
                         {
-                            ParentGroup.Scene.AddPhysicalPrim(1);
+                            // reset velocity to 0 on physics switch-off. Without that, the client thinks the
+                            // prim still has velocity and continues to interpolate its position along the old
+                            // velocity-vector.
+                            Velocity = new Vector3(0, 0, 0);
+                            Acceleration = new Vector3(0, 0, 0);
+                            AngularVelocity = new Vector3(0, 0, 0);
+                            //RotationalVelocity = new Vector3(0, 0, 0);
+                        }
+
+                        PhysActor.IsPhysical = UsePhysics;
 
-                            PhysActor.OnRequestTerseUpdate += PhysicsRequestingTerseUpdate;
-                            PhysActor.OnOutOfBounds += PhysicsOutOfBounds;
-                            if (_parentID != 0 && _parentID != LocalId)
+
+                        // If we're not what we're supposed to be in the physics scene, recreate ourselves.
+                        //m_parentGroup.Scene.PhysicsScene.RemovePrim(PhysActor);
+                        /// that's not wholesome.  Had to make Scene public
+                        //PhysActor = null;
+
+                        if ((ObjectFlags & (uint)PrimFlags.Phantom) == 0)
+                        {
+                            if (UsePhysics)
                             {
-                                if (ParentGroup.RootPart.PhysActor != null)
+                                ParentGroup.Scene.AddPhysicalPrim(1);
+
+                                PhysActor.OnRequestTerseUpdate += PhysicsRequestingTerseUpdate;
+                                PhysActor.OnOutOfBounds += PhysicsOutOfBounds;
+                                if (_parentID != 0 && _parentID != LocalId)
                                 {
-                                    PhysActor.link(ParentGroup.RootPart.PhysActor);
+                                    if (ParentGroup.RootPart.PhysActor != null)
+                                    {
+                                        PhysActor.link(ParentGroup.RootPart.PhysActor);
+                                    }
                                 }
                             }
                         }
                     }
+                    m_parentGroup.Scene.PhysicsScene.AddPhysicsActorTaint(PhysActor);
                 }
-                m_parentGroup.Scene.PhysicsScene.AddPhysicsActorTaint(PhysActor);
             }
         }
 
@@ -3190,6 +3316,53 @@ if (m_shape != null) {
             }
         }
 
+        public bool IsHingeJoint()
+        {
+            // For now, we use the NINJA naming scheme for identifying joints.
+            // In the future, we can support other joint specification schemes such as a 
+            // custom checkbox in the viewer GUI.
+            if (m_parentGroup.Scene.PhysicsScene.SupportsNINJAJoints)
+            {
+                string hingeString = "hingejoint";
+                return (Name.Length >= hingeString.Length && Name.Substring(0, hingeString.Length) == hingeString);
+            }
+            else
+            {
+                return false;
+            }
+        }
+
+        public bool IsBallJoint()
+        {
+            // For now, we use the NINJA naming scheme for identifying joints.
+            // In the future, we can support other joint specification schemes such as a 
+            // custom checkbox in the viewer GUI.
+            if (m_parentGroup.Scene.PhysicsScene.SupportsNINJAJoints)
+            {
+                string ballString = "balljoint";
+                return (Name.Length >= ballString.Length && Name.Substring(0, ballString.Length) == ballString);
+            }
+            else
+            {
+                return false;
+            }
+        }
+
+        public bool IsJoint()
+        {
+            // For now, we use the NINJA naming scheme for identifying joints.
+            // In the future, we can support other joint specification schemes such as a 
+            // custom checkbox in the viewer GUI.
+            if (m_parentGroup.Scene.PhysicsScene.SupportsNINJAJoints)
+            {
+                return IsHingeJoint() || IsBallJoint();
+            }
+            else
+            {
+                return false;
+            }
+        }
+
         public void UpdatePrimFlags(bool UsePhysics, bool IsTemporary, bool IsPhantom, bool IsVD)
         {
             bool wasUsingPhysics = ((ObjectFlags & (uint) PrimFlags.Physics) != 0);
@@ -3230,6 +3403,11 @@ if (m_shape != null) {
 
             }
 
+            if (UsePhysics && IsJoint())
+            {
+                IsPhantom = true;
+            }
+
             if (UsePhysics)
             {
                 AddFlag(PrimFlags.Physics);
@@ -3258,7 +3436,7 @@ if (m_shape != null) {
             }
 
             
-            if (IsPhantom || IsAttachment)
+            if (IsPhantom || IsAttachment) // note: this may have been changed above in the case of joints
             {
                 AddFlag(PrimFlags.Phantom);
                 if (PhysActor != null)