* 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.0.6.2-post-fixes
Teravus Ovares
parent
8a58d998d9
commit
ec2dc354b4
|
|
@ -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
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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,6 +1301,12 @@ if (m_shape != null) {
|
|||
bool isPhysical = (((rootObjectFlags & (uint) PrimFlags.Physics) != 0) && m_physicalPrim);
|
||||
bool isPhantom = ((rootObjectFlags & (uint) PrimFlags.Phantom) != 0);
|
||||
|
||||
if (IsJoint())
|
||||
{
|
||||
DoPhysicsPropertyUpdate(isPhysical, true);
|
||||
}
|
||||
else
|
||||
{
|
||||
// Special case for VolumeDetection: If VolumeDetection is set, the phantom flag is locally ignored
|
||||
if (VolumeDetectActive)
|
||||
isPhantom = false;
|
||||
|
|
@ -1308,12 +1328,15 @@ if (m_shape != null) {
|
|||
// 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public void ClearUndoState()
|
||||
{
|
||||
|
|
@ -1420,12 +1443,103 @@ if (m_shape != null) {
|
|||
}
|
||||
|
||||
public void DoPhysicsPropertyUpdate(bool UsePhysics, bool isNew)
|
||||
{
|
||||
if (IsJoint())
|
||||
{
|
||||
if (UsePhysics)
|
||||
{
|
||||
// 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)
|
||||
{
|
||||
localRotation = Quaternion.Inverse(trackedBody.RotationOffset) * this.RotationOffset;
|
||||
}
|
||||
else
|
||||
{
|
||||
// error, output it below
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
if (isNew)
|
||||
{
|
||||
// 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (PhysActor != null)
|
||||
{
|
||||
if (UsePhysics != PhysActor.IsPhysical || isNew)
|
||||
{
|
||||
if (PhysActor.IsPhysical)
|
||||
if (PhysActor.IsPhysical) // implies UsePhysics==false for this block
|
||||
{
|
||||
if (!isNew)
|
||||
ParentGroup.Scene.RemovePhysicalPrim(1);
|
||||
|
|
@ -1433,6 +1547,16 @@ if (m_shape != null) {
|
|||
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);
|
||||
}
|
||||
|
||||
// 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 (!UsePhysics && !isNew)
|
||||
|
|
@ -1443,6 +1567,7 @@ if (m_shape != null) {
|
|||
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;
|
||||
|
|
@ -1453,7 +1578,7 @@ if (m_shape != null) {
|
|||
/// that's not wholesome. Had to make Scene public
|
||||
//PhysActor = null;
|
||||
|
||||
if ((ObjectFlags & (uint) PrimFlags.Phantom) == 0)
|
||||
if ((ObjectFlags & (uint)PrimFlags.Phantom) == 0)
|
||||
{
|
||||
if (UsePhysics)
|
||||
{
|
||||
|
|
@ -1474,6 +1599,7 @@ if (m_shape != null) {
|
|||
m_parentGroup.Scene.PhysicsScene.AddPhysicsActorTaint(PhysActor);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Restore this part from the serialized xml representation.
|
||||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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
|
||||
}
|
||||
}
|
||||
|
|
@ -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"
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
@ -1183,6 +1184,23 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
// 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)
|
||||
{
|
||||
disableBodySoft();
|
||||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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; }
|
||||
}
|
||||
}
|
||||
|
|
@ -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,10 +2575,38 @@ 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?
|
||||
|
|
|
|||
|
|
@ -434,6 +434,13 @@
|
|||
;; append to existing physics logfile, or overwrite existing logfiles?
|
||||
;physics_logging_append_existing_logfile = true
|
||||
|
||||
; ##
|
||||
; ## Joint support
|
||||
; ##
|
||||
|
||||
; if you would like physics joints to be enabled through a special naming convention in the client, set this to true. (see NINJA Physics documentation, http://opensimulator.org/wiki/NINJA_Physics)
|
||||
;use_NINJA_physics_joints = true
|
||||
|
||||
|
||||
[RemoteAdmin]
|
||||
enabled = false
|
||||
|
|
|
|||
Loading…
Reference in New Issue