* Applying Nlin's NINJA Joint patch. v2. Mantis# 2874

* Thanks nlin!
* To try it out, set ninja joints active in the ODEPhysicsSettings
and use the example at:
* http://forge.opensimulator.org/gf/download/frsrelease/142/304/demo-playground.tgz.
* Don't forget to change the .tgz to .oar and load it with load-oar.

1 files changed, 165 insertions, 2 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
+                }
+            }
+        }
     }
 }