9 files changed, 1181 insertions, 56 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)
diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs
index fd02057..3ba5ce5 100644
--- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs
+++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs
@@ -127,6 +127,9 @@ namespace OpenSim.Region.Physics.Manager
 
         public abstract bool Selected { set; }
 
+        public String SOPName { get; set; }
+        public String SOPDescription { get; set; }
+
         public abstract void CrossingFailure();
 
         public abstract void link(PhysicsActor obj);
diff --git a/OpenSim/Region/Physics/Manager/PhysicsJoint.cs b/OpenSim/Region/Physics/Manager/PhysicsJoint.cs
new file mode 100644
index 0000000..52f7e5f
--- /dev/null
+++ b/OpenSim/Region/Physics/Manager/PhysicsJoint.cs
@@ -0,0 +1,55 @@
+/*

+ * 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 copyright

+ *       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 OpenSim 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 OpenSim.Framework;

+using OpenMetaverse;

+

+namespace OpenSim.Region.Physics.Manager

+{

+    public enum PhysicsJointType : int

+    {

+        Ball = 0,

+        Hinge = 1

+    }

+

+    public class PhysicsJoint

+    {

+        public virtual bool IsInPhysicsEngine { get { return false; } } // set internally to indicate if this joint has already been passed to the physics engine or is still pending

+        public PhysicsJointType Type { get; set; }

+        public string RawParams { get; set; }

+        public List<string> BodyNames = new List<string>();

+        public PhysicsVector Position { get; set; } // global coords

+        public Quaternion Rotation { get; set; } // global coords

+        public string ObjectNameInScene { get; set; } // proxy object in scene that represents the joint position/orientation

+        public string TrackedBodyName { get; set; } // body name that this joint is attached to (ObjectNameInScene will follow TrackedBodyName)

+        public Quaternion LocalRotation { get; set; } // joint orientation relative to one of the involved bodies, the tracked body

+        public int ErrorMessageCount { get; set; } // total # of error messages printed for this joint since its creation. if too many, further error messages are suppressed to prevent flooding.

+        public const int maxErrorMessages = 100; // no more than this # of error messages will be printed for each joint

+    }

+}

diff --git a/OpenSim/Region/Physics/Manager/PhysicsScene.cs b/OpenSim/Region/Physics/Manager/PhysicsScene.cs
index f104632..2cf4d5a 100644
--- a/OpenSim/Region/Physics/Manager/PhysicsScene.cs
+++ b/OpenSim/Region/Physics/Manager/PhysicsScene.cs
@@ -73,6 +73,67 @@ namespace OpenSim.Region.Physics.Manager
         public abstract PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, PhysicsVector position,
                                                   PhysicsVector size, Quaternion rotation, bool isPhysical);
 
+        public virtual bool SupportsNINJAJoints
+        {
+            get { return false; }
+        }
+
+        public virtual PhysicsJoint RequestJointCreation(string objectNameInScene, PhysicsJointType jointType, PhysicsVector position,
+                                            Quaternion rotation, string parms, List<string> bodyNames, string trackedBodyName, Quaternion localRotation)
+        { return null; }
+
+        public virtual void RequestJointDeletion(string objectNameInScene)
+        { return; }
+
+        public virtual void RemoveAllJointsConnectedToActorThreadLocked(PhysicsActor actor)
+        { return; }
+
+        public virtual void DumpJointInfo()
+        { return; }
+
+        public event JointMoved OnJointMoved;
+
+        protected virtual void DoJointMoved(PhysicsJoint joint)
+        {
+            // We need this to allow subclasses (but not other classes) to invoke the event; C# does
+            // not allow subclasses to invoke the parent class event.
+            if (OnJointMoved != null)
+            {
+                OnJointMoved(joint);
+            }
+        }
+
+        public event JointDeactivated OnJointDeactivated;
+
+        protected virtual void DoJointDeactivated(PhysicsJoint joint)
+        {
+            // We need this to allow subclasses (but not other classes) to invoke the event; C# does
+            // not allow subclasses to invoke the parent class event.
+            if (OnJointDeactivated != null)
+            {
+                OnJointDeactivated(joint);
+            }
+        }
+
+        public event JointErrorMessage OnJointErrorMessage;
+
+        protected virtual void DoJointErrorMessage(PhysicsJoint joint, string message)
+        {
+            // We need this to allow subclasses (but not other classes) to invoke the event; C# does
+            // not allow subclasses to invoke the parent class event.
+            if (OnJointErrorMessage != null)
+            {
+                OnJointErrorMessage(joint, message);
+            }
+        }
+
+        public virtual PhysicsVector GetJointAnchor(PhysicsJoint joint)
+        { return null; }
+
+        public virtual PhysicsVector GetJointAxis(PhysicsJoint joint)
+        { return null; }
+
+
         public abstract void AddPhysicsActorTaint(PhysicsActor prim);
 
         public abstract float Simulate(float timeStep);
@@ -181,4 +242,7 @@ namespace OpenSim.Region.Physics.Manager
             }
         }
     }
+    public delegate void JointMoved(PhysicsJoint joint);
+    public delegate void JointDeactivated(PhysicsJoint joint);
+    public delegate void JointErrorMessage(PhysicsJoint joint, string message); // this refers to an "error message due to a problem", not "amount of joint constraint violation"
 }
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
index b9c0936..8dc8f78 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
@@ -50,6 +50,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         private PhysicsVector _torque = new PhysicsVector(0,0,0);
         private PhysicsVector m_lastVelocity = new PhysicsVector(0.0f, 0.0f, 0.0f);
         private PhysicsVector m_lastposition = new PhysicsVector(0.0f, 0.0f, 0.0f);
+        private Quaternion m_lastorientation = new Quaternion();
         private PhysicsVector m_rotationalVelocity;
         private PhysicsVector _size;
         private PhysicsVector _acceleration;
@@ -1182,6 +1183,23 @@ namespace OpenSim.Region.Physics.OdePlugin
                 // in between the disabling and the collision properties setting
                 // which would wake the physical body up from a soft disabling and potentially cause it to fall
                 // through the ground.
+                
+                // NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select
+                // just one part of the assembly, the rest of the assembly is non-selected and still simulating,
+                // so that causes the selected part to wake up and continue moving.
+
+                // even if you select all parts of a jointed assembly, it is not guaranteed that the entire
+                // assembly will stop simulating during the selection, because of the lack of atomicity
+                // of select operations (their processing could be interrupted by a thread switch, causing
+                // simulation to continue before all of the selected object notifications trickle down to
+                // the physics engine).
+
+                // e.g. we select 100 prims that are connected by joints. non-atomically, the first 50 are
+                // selected and disabled. then, due to a thread switch, the selection processing is
+                // interrupted and the physics engine continues to simulate, so the last 50 items, whose
+                // selection was not yet processed, continues to simulate. this wakes up ALL of the 
+                // first 50 again. then the last 50 are disabled. then the first 50, which were just woken
+                // up, start simulating again, which in turn wakes up the last 50.
 
                 if (m_isphysical)
                 {
@@ -2398,7 +2416,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             {
                 PhysicsVector pv = new PhysicsVector(0, 0, 0);
                 bool lastZeroFlag = _zeroFlag;
-                if (Body != (IntPtr)0)
+                if (Body != (IntPtr)0) // FIXME -> or if it is a joint
                 {
                     d.Vector3 vec = d.BodyGetPosition(Body);
                     d.Quaternion ori = d.BodyGetQuaternion(Body);
@@ -2407,6 +2425,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                     d.Vector3 torque = d.BodyGetTorque(Body);
                     _torque.setValues(torque.X, torque.Y, torque.Z);
                     PhysicsVector l_position = new PhysicsVector();
+                    Quaternion l_orientation = new Quaternion();
 
                     //  kluge to keep things in bounds.  ODE lets dead avatars drift away (they should be removed!)
                     //if (vec.X < 0.0f) { vec.X = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
@@ -2415,10 +2434,15 @@ namespace OpenSim.Region.Physics.OdePlugin
                     //if (vec.Y > 255.95f) { vec.Y = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
 
                     m_lastposition = _position;
+                    m_lastorientation = _orientation;
 
                     l_position.X = vec.X;
                     l_position.Y = vec.Y;
                     l_position.Z = vec.Z;
+                    l_orientation.X = ori.X;
+                    l_orientation.Y = ori.Y;
+                    l_orientation.Z = ori.Z;
+                    l_orientation.W = ori.W;
 
                     if (l_position.X > 255.95f || l_position.X < 0f || l_position.Y > 255.95f || l_position.Y < 0f)
                     {
@@ -2474,7 +2498,8 @@ namespace OpenSim.Region.Physics.OdePlugin
 
                     if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
                         && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
-                        && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02))
+                        && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02)
+                        && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01 ))
                     {
                         _zeroFlag = true;
                         m_throttleUpdates = false;
diff --git a/OpenSim/Region/Physics/OdePlugin/OdePhysicsJoint.cs b/OpenSim/Region/Physics/OdePlugin/OdePhysicsJoint.cs
new file mode 100644
index 0000000..754ca2b
--- /dev/null
+++ b/OpenSim/Region/Physics/OdePlugin/OdePhysicsJoint.cs
@@ -0,0 +1,49 @@
+/*

+ * 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 copyright

+ *       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 OpenSim 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 OpenMetaverse;

+using Ode.NET;

+using OpenSim.Framework;

+using OpenSim.Region.Physics.Manager;

+using OpenSim.Region.Physics.Manager;

+using OpenSim.Region.Physics.OdePlugin;

+

+namespace OpenSim.Region.Physics.OdePlugin

+{

+    class OdePhysicsJoint : PhysicsJoint

+    {

+        public override bool IsInPhysicsEngine

+        {

+            get

+            {

+                return (jointID != IntPtr.Zero);

+            }

+        }

+        public IntPtr jointID { get; set; }

+    }

+}

diff --git a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
index c0b4b45..acd2569 100644
--- a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
+++ b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
@@ -32,6 +32,7 @@ using System.Reflection;
 using System.Runtime.InteropServices;
 using System.Threading;
 using System.IO;
+using System.Diagnostics;
 using log4net;
 using Nini.Config;
 using Ode.NET;
@@ -218,7 +219,17 @@ namespace OpenSim.Region.Physics.OdePlugin
         private List<PhysicsActor> _collisionEventPrim = new List<PhysicsActor>();
         public Dictionary<IntPtr, String> geom_name_map = new Dictionary<IntPtr, String>();
         public Dictionary<IntPtr, PhysicsActor> actor_name_map = new Dictionary<IntPtr, PhysicsActor>();
+        private bool m_NINJA_physics_joints_enabled = false;
+        //private Dictionary<String, IntPtr> jointpart_name_map = new Dictionary<String,IntPtr>();
+        private Dictionary<String, List<PhysicsJoint>> joints_connecting_actor = new Dictionary<String, List<PhysicsJoint>>();
         private d.ContactGeom[] contacts = new d.ContactGeom[80];
+        private List<PhysicsJoint> requestedJointsToBeCreated = new List<PhysicsJoint>(); // lock only briefly. accessed by external code (to request new joints) and by OdeScene.Simulate() to move those joints into pending/active
+        private List<PhysicsJoint> pendingJoints = new List<PhysicsJoint>(); // can lock for longer. accessed only by OdeScene.
+        private List<PhysicsJoint> activeJoints = new List<PhysicsJoint>(); // can lock for longer. accessed only by OdeScene.
+        private List<string> requestedJointsToBeDeleted = new List<string>(); // lock only briefly. accessed by external code (to request deletion of joints) and by OdeScene.Simulate() to move those joints out of pending/active
+        private Object externalJointRequestsLock = new Object();
+        private Dictionary<String, PhysicsJoint> SOPName_to_activeJoint = new Dictionary<String, PhysicsJoint>();
+        private Dictionary<String, PhysicsJoint> SOPName_to_pendingJoint = new Dictionary<String, PhysicsJoint>();
 
         private d.Contact contact;
         private d.Contact TerrainContact;
@@ -415,6 +426,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                     physics_logging = physicsconfig.GetBoolean("physics_logging", false);
                     physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0);
                     physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false);
+
+                    m_NINJA_physics_joints_enabled = physicsconfig.GetBoolean("use_NINJA_physics_joints", false);
+
                 }
             }
 
@@ -1347,6 +1361,341 @@ namespace OpenSim.Region.Physics.OdePlugin
             return result;
         }
 
+        public override bool SupportsNINJAJoints
+        {
+            get { return m_NINJA_physics_joints_enabled; }
+        }
+
+        // internal utility function: must be called within a lock(OdeLock)
+        private void InternalAddActiveJoint(PhysicsJoint joint)
+        {
+            activeJoints.Add(joint);
+            SOPName_to_activeJoint.Add(joint.ObjectNameInScene, joint);
+        }
+
+        // internal utility function: must be called within a lock(OdeLock)
+        private void InternalAddPendingJoint(OdePhysicsJoint joint)
+        {
+            pendingJoints.Add(joint);
+            SOPName_to_pendingJoint.Add(joint.ObjectNameInScene, joint);
+        }
+
+        // internal utility function: must be called within a lock(OdeLock)
+        private void InternalRemovePendingJoint(PhysicsJoint joint)
+        {
+            pendingJoints.Remove(joint);
+            SOPName_to_pendingJoint.Remove(joint.ObjectNameInScene);
+        }
+
+        // internal utility function: must be called within a lock(OdeLock)
+        private void InternalRemoveActiveJoint(PhysicsJoint joint)
+        {
+            activeJoints.Remove(joint);
+            SOPName_to_activeJoint.Remove(joint.ObjectNameInScene);
+        }
+
+        public override void DumpJointInfo()
+        {
+            string hdr = "[NINJA] JOINTINFO: ";
+            foreach (PhysicsJoint j in pendingJoints)
+            {
+                m_log.Debug(hdr + " pending joint, Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
+            }
+            m_log.Debug(hdr + pendingJoints.Count + " total pending joints");
+            foreach (string jointName in SOPName_to_pendingJoint.Keys)
+            {
+                m_log.Debug(hdr + " pending joints dict contains Name: " + jointName);
+            }
+            m_log.Debug(hdr + SOPName_to_pendingJoint.Keys.Count + " total pending joints dict entries");
+            foreach (PhysicsJoint j in activeJoints)
+            {
+                m_log.Debug(hdr + " active joint, Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
+            }
+            m_log.Debug(hdr + activeJoints.Count + " total active joints");
+            foreach (string jointName in SOPName_to_activeJoint.Keys)
+            {
+                m_log.Debug(hdr + " active joints dict contains Name: " + jointName);
+            }
+            m_log.Debug(hdr + SOPName_to_activeJoint.Keys.Count + " total active joints dict entries");
+
+            m_log.Debug(hdr + " Per-body joint connectivity information follows.");
+            m_log.Debug(hdr + joints_connecting_actor.Keys.Count + " bodies are connected by joints.");
+            foreach (string actorName in joints_connecting_actor.Keys)
+            {
+                m_log.Debug(hdr + " Actor " + actorName + " has the following joints connecting it");
+                foreach (PhysicsJoint j in joints_connecting_actor[actorName])
+                {
+                    m_log.Debug(hdr + " * joint Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
+                }
+                m_log.Debug(hdr + joints_connecting_actor[actorName].Count + " connecting joints total for this actor");
+            }
+        }
+
+        public override void RequestJointDeletion(string ObjectNameInScene)
+        {
+            lock (externalJointRequestsLock)
+            {
+                if (!requestedJointsToBeDeleted.Contains(ObjectNameInScene)) // forbid same deletion request from entering twice to prevent spurious deletions processed asynchronously
+                {
+                    requestedJointsToBeDeleted.Add(ObjectNameInScene);
+                }
+            }
+        }
+
+        private void DeleteRequestedJoints()
+        {
+            List<string> myRequestedJointsToBeDeleted;
+            lock (externalJointRequestsLock)
+            {
+                // make a local copy of the shared list for processing (threading issues)
+                myRequestedJointsToBeDeleted = new List<string>(requestedJointsToBeDeleted);
+            }
+
+            foreach (string jointName in myRequestedJointsToBeDeleted)
+            {
+                lock (OdeLock)
+                {
+                    //m_log.Debug("[NINJA] trying to deleting requested joint " + jointName);
+                    if (SOPName_to_activeJoint.ContainsKey(jointName) || SOPName_to_pendingJoint.ContainsKey(jointName))
+                    {
+                        OdePhysicsJoint joint = null;
+                        if (SOPName_to_activeJoint.ContainsKey(jointName))
+                        {
+                            joint = SOPName_to_activeJoint[jointName] as OdePhysicsJoint;
+                            InternalRemoveActiveJoint(joint);
+                        }
+                        else if (SOPName_to_pendingJoint.ContainsKey(jointName))
+                        {
+                            joint = SOPName_to_pendingJoint[jointName] as OdePhysicsJoint;
+                            InternalRemovePendingJoint(joint);
+                        }
+
+                        if (joint != null)
+                        {
+                            //m_log.Debug("joint.BodyNames.Count is " + joint.BodyNames.Count + " and contents " + joint.BodyNames);
+                            for (int iBodyName = 0; iBodyName < 2; iBodyName++)
+                            {
+                                string bodyName = joint.BodyNames[iBodyName];
+                                if (bodyName != "NULL")
+                                {
+                                    joints_connecting_actor[bodyName].Remove(joint);
+                                    if (joints_connecting_actor[bodyName].Count == 0)
+                                    {
+                                        joints_connecting_actor.Remove(bodyName);
+                                    }
+                                }
+                            }
+
+                            DoJointDeactivated(joint);
+                            if (joint.jointID != IntPtr.Zero)
+                            {
+                                d.JointDestroy(joint.jointID);
+                                joint.jointID = IntPtr.Zero;
+                                //DoJointErrorMessage(joint, "successfully destroyed joint " + jointName);
+                            }
+                            else
+                            {
+                                //m_log.Warn("[NINJA] Ignoring re-request to destroy joint " + jointName);
+                            }
+                        }
+                        else
+                        {
+                            // DoJointErrorMessage(joint, "coult not find joint to destroy based on name " + jointName);
+                        }
+                    }
+                    else
+                    {
+                        // DoJointErrorMessage(joint, "WARNING - joint removal failed, joint " + jointName);
+                    }
+                }
+            }
+
+            // remove processed joints from the shared list
+            lock (externalJointRequestsLock)
+            {
+                foreach (string jointName in myRequestedJointsToBeDeleted)
+                {
+                    requestedJointsToBeDeleted.Remove(jointName);
+                }
+            }
+        }
+
+        // for pending joints we don't know if their associated bodies exist yet or not.
+        // the joint is actually created during processing of the taints
+        private void CreateRequestedJoints()
+        {
+            List<PhysicsJoint> myRequestedJointsToBeCreated;
+            lock (externalJointRequestsLock)
+            {
+                // make a local copy of the shared list for processing (threading issues)
+                myRequestedJointsToBeCreated = new List<PhysicsJoint>(requestedJointsToBeCreated);
+            }
+
+            foreach (PhysicsJoint joint in myRequestedJointsToBeCreated)
+            {
+                lock (OdeLock)
+                {
+                    if (SOPName_to_pendingJoint.ContainsKey(joint.ObjectNameInScene) && SOPName_to_pendingJoint[joint.ObjectNameInScene] != null)
+                    {
+                        DoJointErrorMessage(joint, "WARNING: ignoring request to re-add already pending joint Name:" + joint.ObjectNameInScene + " type:" + joint.Type + " parms: " + joint.RawParams + " pos: " + joint.Position + " rot:" + joint.Rotation);
+                        continue;
+                    }
+                    if (SOPName_to_activeJoint.ContainsKey(joint.ObjectNameInScene) && SOPName_to_activeJoint[joint.ObjectNameInScene] != null)
+                    {
+                        DoJointErrorMessage(joint, "WARNING: ignoring request to re-add already active joint Name:" + joint.ObjectNameInScene + " type:" + joint.Type + " parms: " + joint.RawParams + " pos: " + joint.Position + " rot:" + joint.Rotation);
+                        continue;
+                    }
+
+                    InternalAddPendingJoint(joint as OdePhysicsJoint);
+
+                    if (joint.BodyNames.Count >= 2)
+                    {
+                        for (int iBodyName = 0; iBodyName < 2; iBodyName++)
+                        {
+                            string bodyName = joint.BodyNames[iBodyName];
+                            if (bodyName != "NULL")
+                            {
+                                if (!joints_connecting_actor.ContainsKey(bodyName))
+                                {
+                                    joints_connecting_actor.Add(bodyName, new List<PhysicsJoint>());
+                                }
+                                joints_connecting_actor[bodyName].Add(joint);
+                            }
+                        }
+                    }
+                }
+            }
+
+            // remove processed joints from shared list
+            lock (externalJointRequestsLock)
+            {
+                foreach (PhysicsJoint joint in myRequestedJointsToBeCreated)
+                {
+                    requestedJointsToBeCreated.Remove(joint);
+                }
+            }
+
+        }
+
+        // public function to add an request for joint creation
+        // this joint will just be added to a waiting list that is NOT processed during the main
+        // Simulate() loop (to avoid deadlocks). After Simulate() is finished, we handle unprocessed joint requests.
+
+        public override PhysicsJoint RequestJointCreation(string objectNameInScene, PhysicsJointType jointType, PhysicsVector position,
+                                            Quaternion rotation, string parms, List<string> bodyNames, string trackedBodyName, Quaternion localRotation)
+
+        {
+
+            OdePhysicsJoint joint = new OdePhysicsJoint();
+            joint.ObjectNameInScene = objectNameInScene;
+            joint.Type = jointType;
+            joint.Position = new PhysicsVector(position.X, position.Y, position.Z);
+            joint.Rotation = rotation;
+            joint.RawParams = parms;
+            joint.BodyNames = new List<string>(bodyNames);
+            joint.TrackedBodyName = trackedBodyName; 
+            joint.LocalRotation = localRotation;
+            joint.jointID = IntPtr.Zero;
+            joint.ErrorMessageCount = 0;
+
+            lock (externalJointRequestsLock)
+            {
+                if (!requestedJointsToBeCreated.Contains(joint)) // forbid same creation request from entering twice 
+                {
+                    requestedJointsToBeCreated.Add(joint);
+                }
+            }
+            return joint;
+        }
+
+        private void RemoveAllJointsConnectedToActor(PhysicsActor actor)
+        {
+            //m_log.Debug("RemoveAllJointsConnectedToActor: start");
+            if (actor.SOPName != null && joints_connecting_actor.ContainsKey(actor.SOPName) && joints_connecting_actor[actor.SOPName] != null)
+            {
+
+                List<PhysicsJoint> jointsToRemove = new List<PhysicsJoint>();
+                //TODO: merge these 2 loops (originally it was needed to avoid altering a list being iterated over, but it is no longer needed due to the joint request queue mechanism)
+                foreach (PhysicsJoint j in joints_connecting_actor[actor.SOPName])
+                {
+                    jointsToRemove.Add(j);
+                }
+                foreach (PhysicsJoint j in jointsToRemove)
+                {
+                    //m_log.Debug("RemoveAllJointsConnectedToActor: about to request deletion of " + j.ObjectNameInScene);
+                    RequestJointDeletion(j.ObjectNameInScene);
+                    //m_log.Debug("RemoveAllJointsConnectedToActor: done request deletion of " + j.ObjectNameInScene);
+                    j.TrackedBodyName = null; // *IMMEDIATELY* prevent any further movement of this joint (else a deleted actor might cause spurious tracking motion of the joint for a few frames, leading to the joint proxy object disappearing)
+                }
+            }
+        }
+
+        public override void RemoveAllJointsConnectedToActorThreadLocked(PhysicsActor actor)
+        {
+            //m_log.Debug("RemoveAllJointsConnectedToActorThreadLocked: start");
+            lock (OdeLock)
+            {
+                //m_log.Debug("RemoveAllJointsConnectedToActorThreadLocked: got lock");
+                RemoveAllJointsConnectedToActor(actor);
+            }
+        }
+
+        // normally called from within OnJointMoved, which is called from within a lock(OdeLock)
+        public override PhysicsVector GetJointAnchor(PhysicsJoint joint)
+        {
+            Debug.Assert(joint.IsInPhysicsEngine);
+            d.Vector3 pos = new d.Vector3();
+
+            if (!(joint is OdePhysicsJoint))
+            {
+                DoJointErrorMessage(joint, "warning: non-ODE joint requesting anchor: " + joint.ObjectNameInScene);
+            }
+            else
+            {
+                OdePhysicsJoint odeJoint = (OdePhysicsJoint)joint;
+                switch (odeJoint.Type)
+                {
+                    case PhysicsJointType.Ball:
+                        d.JointGetBallAnchor(odeJoint.jointID, out pos);
+                        break;
+                    case PhysicsJointType.Hinge:
+                        d.JointGetHingeAnchor(odeJoint.jointID, out pos);
+                        break;
+                }
+            }
+            return new PhysicsVector(pos.X, pos.Y, pos.Z);
+        }
+
+        // normally called from within OnJointMoved, which is called from within a lock(OdeLock)
+        // WARNING: ODE sometimes returns <0,0,0> as the joint axis! Therefore this function
+        // appears to be unreliable. Fortunately we can compute the joint axis ourselves by
+        // keeping track of the joint's original orientation relative to one of the involved bodies.
+        public override PhysicsVector GetJointAxis(PhysicsJoint joint)
+        {
+            Debug.Assert(joint.IsInPhysicsEngine);
+            d.Vector3 axis = new d.Vector3();
+
+            if (!(joint is OdePhysicsJoint))
+            {
+                DoJointErrorMessage(joint, "warning: non-ODE joint requesting anchor: " + joint.ObjectNameInScene);
+            }
+            else
+            {
+                OdePhysicsJoint odeJoint = (OdePhysicsJoint)joint;
+                switch (odeJoint.Type)
+                {
+                    case PhysicsJointType.Ball:
+                        DoJointErrorMessage(joint, "warning - axis requested for ball joint: " + joint.ObjectNameInScene);
+                        break;
+                    case PhysicsJointType.Hinge:
+                        d.JointGetHingeAxis(odeJoint.jointID, out axis);
+                        break;
+                }
+            }
+            return new PhysicsVector(axis.X, axis.Y, axis.Z);
+        }
+
+
         public void remActivePrim(OdePrim deactivatePrim)
         {
             lock (_activeprims)
@@ -1468,6 +1817,11 @@ namespace OpenSim.Region.Physics.OdePlugin
                         //}
                         //}
                         //}
+
+                        if (SupportsNINJAJoints)
+                        {
+                            RemoveAllJointsConnectedToActorThreadLocked(prim);
+                        }
                     }
                 }
             }
@@ -1873,6 +2227,13 @@ namespace OpenSim.Region.Physics.OdePlugin
             {
                 m_physicsiterations = 10;
             }
+
+            if (SupportsNINJAJoints)
+            {
+                DeleteRequestedJoints(); // this must be outside of the lock(OdeLock) to avoid deadlocks
+                CreateRequestedJoints(); // this must be outside of the lock(OdeLock) to avoid deadlocks
+            }
+
             lock (OdeLock)
             {
                 // Process 10 frames if the sim is running normal..
@@ -1944,6 +2305,188 @@ namespace OpenSim.Region.Physics.OdePlugin
                                         prim.m_collisionscore = 0;
                                     }
 
+                                    if (SupportsNINJAJoints)
+                                    {
+                                        // Create pending joints, if possible
+
+                                        // joints can only be processed after ALL bodies are processed (and exist in ODE), since creating
+                                        // a joint requires specifying the body id of both involved bodies
+                                        if (pendingJoints.Count > 0)
+                                        {
+                                            List<PhysicsJoint> successfullyProcessedPendingJoints = new List<PhysicsJoint>();
+                                            //DoJointErrorMessage(joints_connecting_actor, "taint: " + pendingJoints.Count + " pending joints");
+                                            foreach (PhysicsJoint joint in pendingJoints)
+                                            {
+                                                //DoJointErrorMessage(joint, "taint: time to create joint with parms: " + joint.RawParams);
+                                                string[] jointParams = joint.RawParams.Split(' ');
+                                                List<IntPtr> jointBodies = new List<IntPtr>();
+                                                bool allJointBodiesAreReady = true;
+                                                foreach (string jointParam in jointParams)
+                                                {
+                                                    if (jointParam == "NULL")
+                                                    {
+                                                        //DoJointErrorMessage(joint, "attaching NULL joint to world");
+                                                        jointBodies.Add(IntPtr.Zero);
+                                                    }
+                                                    else
+                                                    {
+                                                        //DoJointErrorMessage(joint, "looking for prim name: " + jointParam);
+                                                        bool foundPrim = false;
+                                                        lock (_prims)
+                                                        {
+                                                            foreach (OdePrim prim in _prims) // FIXME: inefficient
+                                                            {
+                                                                if (prim.SOPName == jointParam)
+                                                                {
+                                                                    //DoJointErrorMessage(joint, "found for prim name: " + jointParam);
+                                                                    if (prim.IsPhysical && prim.Body != IntPtr.Zero)
+                                                                    {
+                                                                        jointBodies.Add(prim.Body);
+                                                                        foundPrim = true;
+                                                                        break;
+                                                                    }
+                                                                    else
+                                                                    {
+                                                                        DoJointErrorMessage(joint, "prim name " + jointParam +
+                                                                            " exists but is not (yet) physical; deferring joint creation. " +
+                                                                            "IsPhysical property is " + prim.IsPhysical +
+                                                                            " and body is " + prim.Body);
+                                                                        foundPrim = false;
+                                                                        break;
+                                                                    }
+                                                                }
+                                                            }
+                                                        }
+                                                        if (foundPrim)
+                                                        {
+                                                            // all is fine
+                                                        }
+                                                        else
+                                                        {
+                                                            allJointBodiesAreReady = false;
+                                                            break;
+                                                        }
+                                                    }
+                                                }
+                                                if (allJointBodiesAreReady)
+                                                {
+                                                    //DoJointErrorMessage(joint, "allJointBodiesAreReady for " + joint.ObjectNameInScene + " with parms " + joint.RawParams);
+                                                    if (jointBodies[0] == jointBodies[1])
+                                                    {
+                                                        DoJointErrorMessage(joint, "ERROR: joint cannot be created; the joint bodies are the same, body1==body2. Raw body is " + jointBodies[0] + ". raw parms: " + joint.RawParams);
+                                                    }
+                                                    else
+                                                    {
+                                                        switch (joint.Type)
+                                                        {
+                                                            case PhysicsJointType.Ball:
+                                                                {
+                                                                    IntPtr odeJoint;
+                                                                    //DoJointErrorMessage(joint, "ODE creating ball joint ");
+                                                                    odeJoint = d.JointCreateBall(world, IntPtr.Zero);
+                                                                    //DoJointErrorMessage(joint, "ODE attaching ball joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]);
+                                                                    d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]);
+                                                                    //DoJointErrorMessage(joint, "ODE setting ball anchor: " + odeJoint + " to vec:" + joint.Position);
+                                                                    d.JointSetBallAnchor(odeJoint,
+                                                                                        joint.Position.X,
+                                                                                        joint.Position.Y,
+                                                                                        joint.Position.Z);
+                                                                    //DoJointErrorMessage(joint, "ODE joint setting OK");
+                                                                    //DoJointErrorMessage(joint, "The ball joint's bodies are here: b0: ");
+                                                                    //DoJointErrorMessage(joint, "" + (jointBodies[0] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[0]) : "fixed environment"));
+                                                                    //DoJointErrorMessage(joint, "The ball joint's bodies are here: b1: ");
+                                                                    //DoJointErrorMessage(joint, "" + (jointBodies[1] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[1]) : "fixed environment"));
+
+                                                                    if (joint is OdePhysicsJoint)
+                                                                    {
+                                                                        ((OdePhysicsJoint)joint).jointID = odeJoint;
+                                                                    }
+                                                                    else
+                                                                    {
+                                                                        DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!");
+                                                                    }
+                                                                }
+                                                                break;
+                                                            case PhysicsJointType.Hinge:
+                                                                {
+                                                                    IntPtr odeJoint;
+                                                                    //DoJointErrorMessage(joint, "ODE creating hinge joint ");
+                                                                    odeJoint = d.JointCreateHinge(world, IntPtr.Zero);
+                                                                    //DoJointErrorMessage(joint, "ODE attaching hinge joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]);
+                                                                    d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]);
+                                                                    //DoJointErrorMessage(joint, "ODE setting hinge anchor: " + odeJoint + " to vec:" + joint.Position);
+                                                                    d.JointSetHingeAnchor(odeJoint,
+                                                                                          joint.Position.X,
+                                                                                          joint.Position.Y,
+                                                                                          joint.Position.Z);
+                                                                    // We use the orientation of the x-axis of the joint's coordinate frame
+                                                                    // as the axis for the hinge. 
+
+                                                                    // Therefore, we must get the joint's coordinate frame based on the
+                                                                    // joint.Rotation field, which originates from the orientation of the 
+                                                                    // joint's proxy object in the scene.
+
+                                                                    // The joint's coordinate frame is defined as the transformation matrix
+                                                                    // that converts a vector from joint-local coordinates into world coordinates.
+                                                                    // World coordinates are defined as the XYZ coordinate system of the sim,
+                                                                    // as shown in the top status-bar of the viewer.
+
+                                                                    // Once we have the joint's coordinate frame, we extract its X axis (AtAxis)
+                                                                    // and use that as the hinge axis.
+
+                                                                    //joint.Rotation.Normalize();
+                                                                    Matrix4 proxyFrame = Matrix4.CreateFromQuaternion(joint.Rotation);
+
+                                                                    // Now extract the X axis of the joint's coordinate frame.
+
+                                                                    // Do not try to use proxyFrame.AtAxis or you will become mired in the
+                                                                    // tar pit of transposed, inverted, and generally messed-up orientations.
+                                                                    // (In other words, Matrix4.AtAxis() is borked.)
+                                                                    // Vector3 jointAxis = proxyFrame.AtAxis; <--- this path leadeth to madness
+
+                                                                    // Instead, compute the X axis of the coordinate frame by transforming
+                                                                    // the (1,0,0) vector. At least that works.
+
+                                                                    //m_log.Debug("PHY: making axis: complete matrix is " + proxyFrame);
+                                                                    Vector3 jointAxis = Vector3.Transform(Vector3.UnitX, proxyFrame);
+                                                                    //m_log.Debug("PHY: making axis: hinge joint axis is " + jointAxis); 
+                                                                    //DoJointErrorMessage(joint, "ODE setting hinge axis: " + odeJoint + " to vec:" + jointAxis);
+                                                                    d.JointSetHingeAxis(odeJoint,
+                                                                                        jointAxis.X,
+                                                                                        jointAxis.Y,
+                                                                                        jointAxis.Z);
+                                                                    //d.JointSetHingeParam(odeJoint, (int)dParam.CFM, 0.1f);
+                                                                    if (joint is OdePhysicsJoint)
+                                                                    {
+                                                                        ((OdePhysicsJoint)joint).jointID = odeJoint;
+                                                                    }
+                                                                    else
+                                                                    {
+                                                                        DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!");
+                                                                    }
+                                                                }
+                                                                break;
+                                                        }
+                                                        successfullyProcessedPendingJoints.Add(joint);
+                                                    }
+                                                }
+                                                else
+                                                {
+                                                    DoJointErrorMessage(joint, "joint could not yet be created; still pending");
+                                                }
+                                            }
+                                            foreach (PhysicsJoint successfullyProcessedJoint in successfullyProcessedPendingJoints)
+                                            {
+                                                //DoJointErrorMessage(successfullyProcessedJoint, "finalizing succesfully procsssed joint " + successfullyProcessedJoint.ObjectNameInScene + " parms " + successfullyProcessedJoint.RawParams);
+                                                //DoJointErrorMessage(successfullyProcessedJoint, "removing from pending");
+                                                InternalRemovePendingJoint(successfullyProcessedJoint);
+                                                //DoJointErrorMessage(successfullyProcessedJoint, "adding to active");
+                                                InternalAddActiveJoint(successfullyProcessedJoint);
+                                                //DoJointErrorMessage(successfullyProcessedJoint, "done");
+                                            }
+                                        }
+                                    }
+
                                     if (processedtaints)
                                         _taintedPrim.Clear();
                                 }
@@ -2032,11 +2575,39 @@ namespace OpenSim.Region.Physics.OdePlugin
                             if (actor.IsPhysical && (d.BodyIsEnabled(actor.Body) || !actor._zeroFlag))
                             {
                                 actor.UpdatePositionAndVelocity();
+
+                                if (SupportsNINJAJoints)
+                                {
+                                    // If an actor moved, move its joint proxy objects as well.
+                                    // There seems to be an event PhysicsActor.OnPositionUpdate that could be used
+                                    // for this purpose but it is never called! So we just do the joint
+                                    // movement code here.
+
+                                    if (actor.SOPName != null &&
+                                        joints_connecting_actor.ContainsKey(actor.SOPName) &&
+                                        joints_connecting_actor[actor.SOPName] != null &&
+                                        joints_connecting_actor[actor.SOPName].Count > 0)
+                                    {
+                                        foreach (PhysicsJoint affectedJoint in joints_connecting_actor[actor.SOPName])
+                                        {
+                                            if (affectedJoint.IsInPhysicsEngine)
+                                            {
+                                                DoJointMoved(affectedJoint);
+                                            }
+                                            else
+                                            {
+                                                DoJointErrorMessage(affectedJoint, "a body connected to a joint was moved, but the joint doesn't exist yet! this will lead to joint error. joint was: " + affectedJoint.ObjectNameInScene + " parms:" + affectedJoint.RawParams);
+                                            }
+                                        }
+                                    }
+                                }
                             }
                         }
                     }
                 }
 
+                //DumpJointInfo();
+
                 // Finished with all sim stepping. If requested, dump world state to file for debugging.
                 // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed?
                 // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots?