From ec2dc354b485491d7879686b4a78027971e3ed92 Mon Sep 17 00:00:00 2001 From: Teravus Ovares Date: Fri, 26 Dec 2008 12:58:02 +0000 Subject: * 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. --- OpenSim/Region/Environment/Scenes/Scene.cs | 167 +++++- OpenSim/Region/Environment/Scenes/SceneGraph.cs | 17 + .../Region/Environment/Scenes/SceneObjectPart.cs | 282 ++++++++-- OpenSim/Region/Physics/Manager/PhysicsActor.cs | 3 + OpenSim/Region/Physics/Manager/PhysicsJoint.cs | 55 ++ OpenSim/Region/Physics/Manager/PhysicsScene.cs | 64 +++ OpenSim/Region/Physics/OdePlugin/ODEPrim.cs | 29 +- .../Region/Physics/OdePlugin/OdePhysicsJoint.cs | 49 ++ OpenSim/Region/Physics/OdePlugin/OdePlugin.cs | 571 +++++++++++++++++++++ 9 files changed, 1181 insertions(+), 56 deletions(-) create mode 100644 OpenSim/Region/Physics/Manager/PhysicsJoint.cs create mode 100644 OpenSim/Region/Physics/OdePlugin/OdePhysicsJoint.cs (limited to 'OpenSim/Region') 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 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 bodyNames = new List(); + 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 BodyNames = new List(); + 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 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 _collisionEventPrim = new List(); public Dictionary geom_name_map = new Dictionary(); public Dictionary actor_name_map = new Dictionary(); + private bool m_NINJA_physics_joints_enabled = false; + //private Dictionary jointpart_name_map = new Dictionary(); + private Dictionary> joints_connecting_actor = new Dictionary>(); private d.ContactGeom[] contacts = new d.ContactGeom[80]; + private List requestedJointsToBeCreated = new List(); // lock only briefly. accessed by external code (to request new joints) and by OdeScene.Simulate() to move those joints into pending/active + private List pendingJoints = new List(); // can lock for longer. accessed only by OdeScene. + private List activeJoints = new List(); // can lock for longer. accessed only by OdeScene. + private List requestedJointsToBeDeleted = new List(); // 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 SOPName_to_activeJoint = new Dictionary(); + private Dictionary SOPName_to_pendingJoint = new Dictionary(); 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 myRequestedJointsToBeDeleted; + lock (externalJointRequestsLock) + { + // make a local copy of the shared list for processing (threading issues) + myRequestedJointsToBeDeleted = new List(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 myRequestedJointsToBeCreated; + lock (externalJointRequestsLock) + { + // make a local copy of the shared list for processing (threading issues) + myRequestedJointsToBeCreated = new List(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()); + } + 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 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(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 jointsToRemove = new List(); + //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 successfullyProcessedPendingJoints = new List(); + //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 jointBodies = new List(); + 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? -- cgit v1.1