public override MBoolResponse AssignInstruction(MInstruction instruction, MSimulationState simulationState)
{
//Assign the instruction
this.instruction = instruction;
MBoolResponse response = new MBoolResponse(true);
if (instruction.Constraints.Count > 0 && instruction.Constraints[0].GeometryConstraint != null)
{
MSceneObject parent = this.SceneAccess.GetSceneObjectByID(instruction.Constraints[0].GeometryConstraint.ParentObjectID);
if (instruction.Constraints[0].GeometryConstraint.ParentToConstraint != null)
{
MTransform ptc = instruction.Constraints[0].GeometryConstraint.ParentToConstraint;
String gtp = ptc.Parent;
if (gtp != null && this.SceneAccess.GetSceneObjectByID(gtp) != null)
{
// transform parent takes precedent.
this.targetTransform = ptc.LocalToGlobal(this.SceneAccess);
}
else if (parent != null)
{
// parent to constraint has not valid parent, thus the geometry constraint parent is
this.targetTransform = ptc.Multiply(parent.Transform.LocalToGlobal(this.SceneAccess));
}
else
{
this.targetTransform = ptc;
}
}
else
{
MVector3 pos = new MVector3(0, 0, 0);
if (instruction.Constraints[0].GeometryConstraint.TranslationConstraint != null)
{
MTranslationConstraint trlCstr = instruction.Constraints[0].GeometryConstraint.TranslationConstraint;
if (parent != null)
{
pos = parent.Transform.Position.Add(trlCstr.GetVector3());
}
else
{
pos = trlCstr.GetVector3();
}
}
MQuaternion rot = new MQuaternion(0, 0, 0, 1);
if (instruction.Constraints[0].GeometryConstraint.RotationConstraint != null)
{
MRotationConstraint rtCstr = instruction.Constraints[0].GeometryConstraint.RotationConstraint;
if (parent != null)
{
rot = rtCstr.GetQuaternion().Multiply(parent.Transform.Rotation);
}
else
{
rot = rtCstr.GetQuaternion();
}
}
this.targetTransform = new MTransform("", pos, rot);
}
}
else
{
response = new MBoolResponse(false)
{
LogData = new List <string>()
{
"Required target constraint (MGeometryConstraint) not defined"
}
};
}
//Extract the velocity if defined
if (instruction.Properties.ContainsKey("Velocity"))
{
Console.WriteLine("vel: " + instruction.Properties["Velocity"]);
float.TryParse(instruction.Properties["Velocity"], System.Globalization.NumberStyles.Float, System.Globalization.CultureInfo.InvariantCulture, out velocity);
}
else
{
velocity = -1.0f;
}
/*
* //Get the target id
* if (instruction.Properties.ContainsKey("TargetID"))
* this.targetTransform = this.SceneAccess.GetTransformByID(instruction.Properties["TargetID"]);
* //Error id not available
* else
* {
* return new MBoolResponse(false)
* {
* };
* }*/
//Return true/success
return(response);
}
public static double X(this MRotationConstraint rConstraint)
{
return(rConstraint.Limits.X());
}
public static MQuaternion GetQuaternion(this MRotationConstraint rConstraint)
{
return(MQuaternionExtensions.FromEuler(rConstraint.GetVector3()));
}
public static MVector3 GetVector3(this MRotationConstraint rConstraint)
{
return(new MVector3(rConstraint.X(), rConstraint.Y(), rConstraint.Z()));
}
/// <summary>
/// Determines whether the constraint is fulflled
/// </summary>
/// <param name="desiredRotation"></param>
/// <param name="currentRotation"></param>
/// <param name="rotationConstraint"></param>
/// <returns></returns>
private bool IsFulfilled(MQuaternion desiredRotation, MQuaternion currentRotation, MRotationConstraint rotationConstraint)
{
//By default return true -> It is assumed that interval is [-inv,+inv]
if (rotationConstraint == null)
{
return(true);
}
MVector3 currentRotationEuler = MQuaternionExtensions.ToEuler(currentRotation);
//Determine the global min and max coordinate
MVector3 min = new MVector3(rotationConstraint.Limits.X.Min, rotationConstraint.Limits.Y.Min, rotationConstraint.Limits.Z.Min).Add(currentRotationEuler);
MVector3 max = new MVector3(rotationConstraint.Limits.X.Max, rotationConstraint.Limits.Y.Max, rotationConstraint.Limits.Z.Max).Add(currentRotationEuler);
return(currentRotationEuler.X >= min.X && currentRotationEuler.Y >= min.Y && currentRotationEuler.Z >= min.Z && currentRotationEuler.X <= max.X && currentRotationEuler.Y <= max.Y && currentRotationEuler.Z <= max.Z);
}
/// <summary>
/// Returns all ik constraints which are violated (avove specified threshold)
/// </summary>
/// <param name="constraints"></param>
/// <param name="currentPosture"></param>
/// <returns></returns>
private List <MConstraint> GetViolatedIKConstraints(List <MConstraint> constraints, MAvatarPostureValues currentPosture)
{
List <MConstraint> violated = new List <MConstraint>();
if (constraints != null)
{
// Apply result posture values to the skeleton
skeletonAccess.SetChannelData(currentPosture);
string avatarID = currentPosture.AvatarID;
//Check each joint constraint
foreach (MConstraint mconstraint in constraints)
{
if (mconstraint.JointConstraint != null)
{
MJointConstraint endeffectorConstraint = mconstraint.JointConstraint;
//Skip if no gemometry constraint is defined
if (endeffectorConstraint.GeometryConstraint == null)
{
continue;
}
double distance = 0f;
double angularDistance = 0f;
//Default (parent to constraint is set)
if (endeffectorConstraint.GeometryConstraint.ParentToConstraint != null)
{
MVector3 position = endeffectorConstraint.GeometryConstraint.ParentToConstraint.Position;
MQuaternion rotation = endeffectorConstraint.GeometryConstraint.ParentToConstraint.Rotation;
switch (endeffectorConstraint.JointType)
{
case MJointType.LeftWrist:
distance = MVector3Extensions.Distance(position, this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.LeftWrist));
angularDistance = MQuaternionExtensions.Angle(rotation, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.LeftWrist));
break;
case MJointType.RightWrist:
distance = MVector3Extensions.Distance(position, this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.RightWrist));
angularDistance = MQuaternionExtensions.Angle(rotation, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.RightWrist));
break;
case MJointType.LeftBall:
distance = MVector3Extensions.Distance(position, this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.LeftAnkle));
angularDistance = MQuaternionExtensions.Angle(rotation, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.LeftAnkle));
break;
case MJointType.RightBall:
distance = MVector3Extensions.Distance(position, this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.RightAnkle));
angularDistance = MQuaternionExtensions.Angle(rotation, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.RightAnkle));
break;
case MJointType.PelvisCentre:
distance = MVector3Extensions.Distance(position, this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.PelvisCentre));
angularDistance = MQuaternionExtensions.Angle(rotation, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.PelvisCentre));
break;
}
}
//Legacy fallback mechanism -> Remove in future
else
{
MTranslationConstraint positionConstraint = endeffectorConstraint.GeometryConstraint.TranslationConstraint;
if (endeffectorConstraint.GeometryConstraint.TranslationConstraint != null)
{
switch (endeffectorConstraint.JointType)
{
case MJointType.LeftWrist:
distance = MVector3Extensions.Distance(new MVector3(positionConstraint.X(), positionConstraint.Y(), positionConstraint.Z()), this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.LeftWrist));
break;
case MJointType.RightWrist:
distance = MVector3Extensions.Distance(new MVector3(positionConstraint.X(), positionConstraint.Y(), positionConstraint.Z()), this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.RightWrist));
break;
case MJointType.LeftBall:
distance = MVector3Extensions.Distance(new MVector3(positionConstraint.X(), positionConstraint.Y(), positionConstraint.Z()), this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.LeftAnkle));
break;
case MJointType.RightBall:
distance = MVector3Extensions.Distance(new MVector3(positionConstraint.X(), positionConstraint.Y(), positionConstraint.Z()), this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.RightAnkle));
break;
case MJointType.PelvisCentre:
distance = MVector3Extensions.Distance(new MVector3(positionConstraint.X(), positionConstraint.Y(), positionConstraint.Z()), this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.PelvisCentre));
break;
}
}
//Handle the rotation constraint
if (endeffectorConstraint.GeometryConstraint.RotationConstraint != null)
{
MRotationConstraint rotationConstraint = endeffectorConstraint.GeometryConstraint.RotationConstraint;
//Compute a quaternion based on the euler angles
MQuaternion quaternion = MQuaternionExtensions.FromEuler(new MVector3(rotationConstraint.X(), rotationConstraint.Y(), rotationConstraint.Z()));
if (endeffectorConstraint.GeometryConstraint.ParentObjectID == null || endeffectorConstraint.GeometryConstraint.ParentObjectID == "")
{
switch (endeffectorConstraint.JointType)
{
case MJointType.LeftWrist:
angularDistance = MQuaternionExtensions.Angle(quaternion, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.LeftWrist));
break;
case MJointType.RightWrist:
angularDistance = MQuaternionExtensions.Angle(quaternion, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.RightWrist));
break;
case MJointType.LeftBall:
angularDistance = MQuaternionExtensions.Angle(quaternion, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.LeftAnkle));
break;
case MJointType.RightBall:
angularDistance = MQuaternionExtensions.Angle(quaternion, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.RightAnkle));
break;
case MJointType.PelvisCentre:
angularDistance = MQuaternionExtensions.Angle(quaternion, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.PelvisCentre));
break;
}
}
}
}
//Check if solving is required
if (distance > this.PositionThreshold || angularDistance > this.RotationThreshold)
{
violated.Add(mconstraint);
}
}
}
}
return(violated);
}
