refactor: Simplify reading OdeScene.Simulate() loop by shunting all the NINJA joints stuff into its own method.
Now if ninja joints isn't active (which is the default) don't have to wade through a lot of massively indented irrelevant code.bulletsim
Justin Clark-Casey (justincc)
parent
9c6227da66
commit
3f0d8f3cbf
|
|
@ -2738,186 +2738,7 @@ Console.WriteLine("AddPhysicsActorTaint to " + taintedprim.Name);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (SupportsNINJAJoints)
|
if (SupportsNINJAJoints)
|
||||||
{
|
SimulateNINJAJoints();
|
||||||
// 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(" ".ToCharArray(), System.StringSplitOptions.RemoveEmptyEntries);
|
|
||||||
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)
|
if (processedtaints)
|
||||||
//Console.WriteLine("Simulate calls Clear of _taintedPrim list");
|
//Console.WriteLine("Simulate calls Clear of _taintedPrim list");
|
||||||
|
|
@ -3117,6 +2938,194 @@ Console.WriteLine("AddPhysicsActorTaint to " + taintedprim.Name);
|
||||||
return fps;
|
return fps;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Simulate NINJA joints.
|
||||||
|
/// </summary>
|
||||||
|
/// <remarks>
|
||||||
|
/// Called by the main Simulate() loop if NINJA joints are active. Should not be called from anywhere else.
|
||||||
|
/// </remarks>
|
||||||
|
protected void SimulateNINJAJoints()
|
||||||
|
{
|
||||||
|
// 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(" ".ToCharArray(), System.StringSplitOptions.RemoveEmptyEntries);
|
||||||
|
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");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
public override void GetResults()
|
public override void GetResults()
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue