public DOFDesc(string name, JointDesc description, DOFType type, float min, float max, bool isVelocityDrive,
float defaultDriveValue)
{
Name = name;
Description = description;
Type = type;
Minimum = min;
Maximum = max;
Scale = 100f / (max - min);
IsVelocityDrive = isVelocityDrive;
DefaultDriveValue = defaultDriveValue;
}
void AddJoint(Joint joint, simengine.VisualEntity entity)
{
if (!(joint is PhysicsJoint))
{
return;
}
if (!_state.Joints.ContainsKey(joint.State.Name))
{
JointDesc description = new JointDesc(joint.State.Name, entity, joint as PhysicsJoint);
string[] DOFNames = joint.State.Name.Split(';');
int which = 0;
if (joint.State.Angular != null)
{
if (joint.State.Angular.TwistMode != JointDOFMode.Locked && IsNotUselessDrive(joint.State.Angular.TwistDrive))
{
AddDOF(DOFNames, which++, description, DOFType.Twist);
}
if (joint.State.Angular.Swing1Mode != JointDOFMode.Locked && IsNotUselessDrive(joint.State.Angular.SwingDrive))
{
AddDOF(DOFNames, which++, description, DOFType.Swing1);
}
if (joint.State.Angular.Swing2Mode != JointDOFMode.Locked && IsNotUselessDrive(joint.State.Angular.SlerpDrive))
{
AddDOF(DOFNames, which++, description, DOFType.Swing2);
}
}
if (joint.State.Linear != null)
{
if (joint.State.Linear.XMotionMode != JointDOFMode.Locked && IsNotUselessDrive(joint.State.Linear.XDrive))
{
AddDOF(DOFNames, which++, description, DOFType.X);
}
if (joint.State.Linear.YMotionMode != JointDOFMode.Locked && IsNotUselessDrive(joint.State.Linear.YDrive))
{
AddDOF(DOFNames, which++, description, DOFType.Y);
}
if (joint.State.Linear.ZMotionMode != JointDOFMode.Locked && IsNotUselessDrive(joint.State.Linear.ZDrive))
{
AddDOF(DOFNames, which++, description, DOFType.Z);
}
}
}
}
void AddDOF(string[] DOFNames, int which, JointDesc desc, DOFType type)
{
bool isVelocityDrive = false;
float defaultDriveValue = 0.0f;
float min = -180, max = 180;
if (which < DOFNames.Length)
{
string[] subNames = DOFNames[which].Split('|');
try
{
if (subNames.Length > 1)
{
min = Single.Parse(subNames[1]);
}
if (subNames.Length > 2)
{
max = Single.Parse(subNames[2]);
}
}
catch
{
}
if (IsVelocityDrive(desc.Joint.State, type))
{
float angularVelocityLength = 0;
if (Math.Abs(desc.Joint.State.Angular.DriveTargetVelocity.X) > Math.Abs(angularVelocityLength))
{
angularVelocityLength = desc.Joint.State.Angular.DriveTargetVelocity.X;
}
if (Math.Abs(desc.Joint.State.Angular.DriveTargetVelocity.Y) > Math.Abs(angularVelocityLength))
{
angularVelocityLength = desc.Joint.State.Angular.DriveTargetVelocity.Y;
}
if (Math.Abs(desc.Joint.State.Angular.DriveTargetVelocity.Z) > Math.Abs(angularVelocityLength))
{
angularVelocityLength = desc.Joint.State.Angular.DriveTargetVelocity.Z;
}
if (Math.Abs(angularVelocityLength) < float.Epsilon)
{
min = -2.0f;
max = 2.0f;
}
else
{
min = Math.Min(-angularVelocityLength, angularVelocityLength);
max = Math.Max(-angularVelocityLength, angularVelocityLength);
defaultDriveValue = -angularVelocityLength;
}
isVelocityDrive = true;
}
_state.Joints.Add(subNames[0], new DOFDesc(subNames[0], desc, type, min, max, isVelocityDrive, defaultDriveValue));
}
else
{
string name = DOFNames[0];
switch (which)
{
case 0: name += " Twist"; break;
case 1: name += " Swing1"; break;
case 2: name += " Swing2"; break;
case 3: name += " X"; min = -2; max = 2; break;
case 4: name += " Y"; min = -2; max = 2; break;
case 5: name += " Z"; min = -2; max = 2; break;
}
_state.Joints.Add(name, new DOFDesc(name, desc, type, min, max, isVelocityDrive, defaultDriveValue));
}
}
void MoveJoint(MoveJoint move)
{
DOFDesc dof = _state.Joints[move.Name];
JointDesc desc = dof.Description;
Vector3 targetVelocity = new Vector3(0, 0, 0);
switch (dof.Type)
{
case DOFType.Twist:
desc.TwistAngle = (float)move.Angle;
targetVelocity = new Vector3((float)-move.Angle, 0, 0);
break;
case DOFType.Swing1:
desc.Swing1Angle = (float)move.Angle;
targetVelocity = new Vector3(0, (float)-move.Angle, 0);
break;
case DOFType.Swing2:
desc.Swing2Angle = (float)move.Angle;
targetVelocity = new Vector3(0, 0, (float)-move.Angle);
break;
case DOFType.X:
desc.X = (float)move.Angle;
targetVelocity = new Vector3((float)-move.Angle, 0, 0);
break;
case DOFType.Y:
desc.Y = (float)move.Angle;
targetVelocity = new Vector3(0, (float)-move.Angle, 0);
break;
case DOFType.Z:
desc.Z = (float)move.Angle;
targetVelocity = new Vector3(0, 0, (float)-move.Angle);
break;
}
PhysicsJoint thisJoint = null;
if (desc.Joint != null)
{
thisJoint = (PhysicsJoint)desc.Joint;
}
else
{
thisJoint = (PhysicsJoint)desc.JointEntity.ParentJoint;
}
if (IsVelocityDrive(thisJoint.State, dof.Type))
{
_entity.DeferredTaskQueue.Post(new Task(() => SetDriveInternal(thisJoint, targetVelocity)));
}
else
{
Task <Physics.PhysicsJoint, Quaternion, Vector3> deferredTask =
new Task <Physics.PhysicsJoint, Quaternion, Vector3>(thisJoint, desc.JointOrientation, desc.JointPosition, SetDriveInternal);
_entity.DeferredTaskQueue.Post(deferredTask);
}
}
