public MJoint(string ID, MJointType Type, MMIStandard.MVector3 Position, MMIStandard.MQuaternion Rotation) : this()
{
this.ID = ID;
this.Type = Type;
this.Position = Position;
this.Rotation = Rotation;
}
/// <summary>
/// Performs a blending based on the from posture and the to posture. In particular a blending weight and an additional blending mask is utilized. If the blending mask is set to null, all bones with position + rotation will be used for blending.
/// </summary>
/// <param name="from"></param>
/// <param name="to"></param>
/// <param name="weight"></param>
/// <param name="blendingMask"></param>
/// <returns></returns>
public static MAvatarPostureValues PerformBlend(IntermediateSkeleton skeleton, MAvatarPostureValues from, MAvatarPostureValues to, float weight, Dictionary <MJointType, BlendProperty> blendingMask = null)
{
//MAvatarPosture result = from.Clone();
MAvatarPosture zero = skeleton.GetAvatarDescription(from.AvatarID).ZeroPosture;
skeleton.SetChannelData(from);
List <MQuaternion> fromRot = skeleton.GetLocalJointRotations(from.AvatarID);
skeleton.SetChannelData(to);
List <MQuaternion> toRot = skeleton.GetLocalJointRotations(to.AvatarID);
for (int i = 0; i < zero.Joints.Count; i++)
{
//By default belnd both position and rotation
BlendProperty blendProperty = new BlendProperty(1.0f, 1.0f);
MJointType joint = zero.Joints[i].Type;
if (blendingMask != null && blendingMask.ContainsKey(joint))
{
//Get the bone weight
blendingMask.TryGetValue(joint, out blendProperty);
}
//Perform a linear interpolation of the position
// Does not correspond to intermediate skeleton representation.
// result.Joints[i].Position = result.Joints[i].Position.Lerp(to.Joints[i].Position, weight * blendProperty.PositionWeight);
//Perform a slerp of the rotation
skeleton.SetLocalJointRotation(to.AvatarID, joint, fromRot[i].Slerp(toRot[i], weight * blendProperty.RotationWeight));
}
return(skeleton.RecomputeCurrentPostureValues(to.AvatarID));
}
/// <summary>
/// Basic constructor
/// </summary>
/// <param name="type"></param>
/// <param name="instruction"></param>
/// <param name="isActive"></param>
/// <param name="bothHanded"></param>
public HandContainer(MJointType type, MInstruction instruction, bool isActive, bool bothHanded = false) : this()
{
this.Type = type;
this.Instruction = instruction;
this.IsActive = isActive;
this.BothHanded = bothHanded;
}
private static MJoint NewMJoint(string id, MJointType type, MVector3 offset, MQuaternion rotation, string parentID, List <MChannel> channels)
{
MJoint j = new MJoint(id, type, offset, rotation);
j.Parent = parentID;
j.Channels = channels;
return(j);
}
/// <summary>
/// Returns the endeffector constraint
/// </summary>
/// <param name="type"></param>
/// <returns></returns>
public virtual MJointConstraint GetEndeffectorConstraint(MJointType type)
{
MConstraint constraint = this.constraints.Find(s => s.JointConstraint != null && s.JointConstraint.JointType == type);
if (constraint == null || constraint.JointConstraint == null)
{
return(null);
}
return(constraint.JointConstraint);
}
/// <summary>
/// Returns the global transform of the specific joint type and posture
/// </summary>
/// <param name="posture"></param>
/// <param name="jointType"></param>
/// <returns></returns>
private MTransform GetTransform(MAvatarPostureValues posture, MJointType jointType)
{
this.SkeletonAccess.SetChannelData(posture);
return(new MTransform()
{
ID = "",
Position = this.SkeletonAccess.GetGlobalJointPosition(this.avatarDescription.AvatarID, jointType),
Rotation = this.SkeletonAccess.GetGlobalJointRotation(this.avatarDescription.AvatarID, jointType)
});
}
/// <summary>
/// Removes all endeffector constraints for the specific type
/// </summary>
/// <param name="type"></param>
public virtual void RemoveEndeffectorConstraints(MJointType type)
{
List <MConstraint> conflictingConstraints = this.constraints.Where(s => s.JointConstraint != null && s.JointConstraint.JointType == type).ToList();
if (conflictingConstraints != null)
{
foreach (MConstraint constrToRemove in conflictingConstraints)
{
this.constraints.Remove(constrToRemove);
}
}
}
/// <summary>
/// Sets an endeffector constraint with a given local position and rotation and a parent
/// </summary>
/// <param name="type"></param>
/// <param name="position"></param>
/// <param name="rotation"></param>
/// <param name="parentID">The id of the parent</param>
public virtual void SetEndeffectorConstraint(MJointType type, MVector3 position, MQuaternion rotation, String id = null, String parentID = "")
{
//Create a new endeffector constriant using the MJointConstraint
this.SetEndeffectorConstraint(new MJointConstraint()
{
JointType = type,
GeometryConstraint = new MGeometryConstraint()
{
ParentObjectID = parentID,
ParentToConstraint = new MTransform(Guid.NewGuid().ToString(), position, rotation)
},
}, id);
}
public Joint GetChild(MJointType type)
{
if (this.joint.Type == type)
{
return(this);
}
foreach (Joint c in this.children)
{
Joint j = c.GetChild(type);
if (j != null)
{
return(j);
}
}
return(null);
}
private MAvatarPostureValues PerformPartialBlend(MJointType bodySide, float weight, MSimulationState simulationState)
{
List <MJointType> toConsider = new List <MJointType>();
switch (bodySide)
{
case MJointType.LeftWrist:
toConsider.Add(MJointType.LeftShoulder);
toConsider.Add(MJointType.LeftElbow);
toConsider.Add(MJointType.LeftWrist);
break;
case MJointType.RightWrist:
toConsider.Add(MJointType.RightShoulder);
toConsider.Add(MJointType.RightElbow);
toConsider.Add(MJointType.RightWrist);
break;
}
Dictionary <MJointType, MQuaternion> rotations = new Dictionary <MJointType, MQuaternion>();
foreach (MJointType jt in toConsider)
{
this.SkeletonAccess.SetChannelData(simulationState.Initial);
MQuaternion rot1 = this.SkeletonAccess.GetLocalJointRotation(this.AvatarDescription.AvatarID, jt);
this.SkeletonAccess.SetChannelData(simulationState.Current);
MQuaternion rot2 = this.SkeletonAccess.GetLocalJointRotation(this.AvatarDescription.AvatarID, jt);
MQuaternion interpolatedRotation = MQuaternionExtensions.Slerp(rot1, rot2, weight);
rotations.Add(jt, interpolatedRotation);
}
this.SkeletonAccess.SetChannelData(simulationState.Current);
foreach (var entry in rotations)
{
this.SkeletonAccess.SetLocalJointRotation(this.AvatarDescription.AvatarID, entry.Key, entry.Value);
}
return(this.SkeletonAccess.RecomputeCurrentPostureValues(this.AvatarDescription.AvatarID));
}
/// <summary>
/// Performs a blending based on the from posture and the to posture.
/// </summary>
/// <param name="from"></param>
/// <param name="to"></param>
/// <param name="weight"></param>
/// <param name="rootTransform">Specifies whether the root transform is blended as well</param>
/// <returns></returns>
public static MAvatarPostureValues PerformBlend(IntermediateSkeleton skeleton, MAvatarPostureValues from, MAvatarPostureValues to, float weight, bool rootTransform = true)
{
MAvatarPosture zero = skeleton.GetAvatarDescription(from.AvatarID).ZeroPosture;
skeleton.SetChannelData(from);
List <MQuaternion> fromRot = skeleton.GetLocalJointRotations(from.AvatarID);
skeleton.SetChannelData(to);
List <MQuaternion> toRot = skeleton.GetLocalJointRotations(to.AvatarID);
for (int i = 0; i < zero.Joints.Count; i++)
{
//By default belnd both position and rotation
MJointType joint = zero.Joints[i].Type;
//Perform a linear interpolation of the position
// Does not correspond to intermediate skeleton representation.
// result.Joints[i].Position = result.Joints[i].Position.Lerp(to.Joints[i].Position, weight * blendProperty.PositionWeight);
//Perform a slerp of the rotation
skeleton.SetLocalJointRotation(to.AvatarID, joint, fromRot[i].Slerp(toRot[i], weight));
}
return(skeleton.RecomputeCurrentPostureValues(to.AvatarID));
/*
* MAvatarPosture result = from.Clone();
*
* for (int i = 0; i < result.Joints.Count; i++)
* {
* //Skip if root transform should be ignored
* if (i == 0 && rootTransform)
* result.Joints[i].Position = result.Joints[i].Position.Lerp(to.Joints[i].Position, weight);
*
* //Perform a slerp of the rotation
* result.Joints[i].Rotation = result.Joints[i].Rotation.Slerp(to.Joints[i].Rotation, weight);
* }
*
* return result;
*/
}
public static MEndeffectorType ToEndeffectorType(this MJointType jointType)
{
switch (jointType)
{
case MJointType.LeftWrist:
return(MEndeffectorType.LeftHand);
case MJointType.RightWrist:
return(MEndeffectorType.RightHand);
case MJointType.LeftBall:
return(MEndeffectorType.LeftFoot);
case MJointType.RightBall:
return(MEndeffectorType.RightHand);
case MJointType.PelvisCentre:
return(MEndeffectorType.Root);
}
return(MEndeffectorType.Root);
}
/// <summary>
/// Sets up the respective hand using the given parameters
/// </summary>
/// <param name="jointType"></param>
/// <param name="instruction"></param>
/// <param name="duration"></param>
/// <param name="angularVelocity"></param>
/// <param name="durationSet"></param>
/// <param name="release"></param>
private void SetupHand(MJointType jointType, MInstruction instruction, float duration, float angularVelocity, bool durationSet, bool release)
{
//Create a new hand container
HandContainer hand = new HandContainer()
{
Type = jointType,
Instruction = instruction,
Release = release,
Duration = duration,
AngularVelocity = angularVelocity,
HasDuration = durationSet,
Positioned = false
};
HandContainer old = this.ActiveHands.Find(s => s.Type == jointType);
if (old != null)
{
this.ActiveHands.Remove(old);
}
//Handle the hand pose (if defined)
if (instruction.Properties.ContainsKey("HandPose"))
{
//Get the constraint id
string constraintID = instruction.Properties["HandPose"];
//Get the corresponding hand constraint
MConstraint handConstraint = instruction.Constraints.Find(s => s.ID == constraintID);
//Check if hand constraint is defined and assign as final posture
if (handConstraint != null && handConstraint.PostureConstraint != null)
{
hand.FinalPosture = handConstraint.PostureConstraint;
}
}
//Add as active hand
this.ActiveHands.Add(hand);
}
/// <summary>
/// Methods parses the parameters defined in the MInstruction
/// </summary>
/// <param name="instruction"></param>
/// <returns></returns>
private MBoolResponse ParseParameters(MInstruction instruction)
{
//Extract the single shot ik parameter if defined
if (instruction.Properties.ContainsKey("SingleShotIK"))
{
bool.TryParse(instruction.Properties["SingleShotIK"], out singleShotIK);
}
//Extract the velocity if defined
if (instruction.Properties.ContainsKey("Velocity"))
{
this.velocity = float.Parse(instruction.Properties["Velocity"], System.Globalization.CultureInfo.InvariantCulture);
}
//Extract the angular velocity
if (instruction.Properties.ContainsKey("AngularVelocity"))
{
this.angularVelocity = float.Parse(instruction.Properties["AngularVelocity"], System.Globalization.CultureInfo.InvariantCulture);
}
//Extract the information with regard to the target
if (instruction.Properties.ContainsKey("TargetID"))
{
//Use the constraint (if defined)
if (instruction.Constraints != null && instruction.Constraints.Exists(s => s.ID == instruction.Properties["TargetID"]))
{
MConstraint match = instruction.Constraints.Find(s => s.ID == instruction.Properties["TargetID"]);
this.targetTransform = new MTransform()
{
ID = "target",
Position = match.GeometryConstraint.GetGlobalPosition(this.SceneAccess),
Rotation = match.GeometryConstraint.GetGlobalRotation(this.SceneAccess)
};
MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_DEBUG, "Using MGeometryConstraint for target definition");
}
//Gather from the scene
else
{
this.targetTransform = this.SceneAccess.GetTransformByID(instruction.Properties["TargetID"]);
}
}
//Error id not available
else
{
return(new MBoolResponse(false)
{
LogData = new List <string>()
{
"Required parameter Target ID not defined"
}
});
}
//Get the target hand
if (instruction.Properties.ContainsKey("Hand"))
{
if (instruction.Properties["Hand"] == "Left")
{
this.handJoint = MJointType.LeftWrist;
}
if (instruction.Properties["Hand"] == "Right")
{
this.handJoint = MJointType.RightWrist;
}
}
//Error target hand not specified
else
{
return(new MBoolResponse(false)
{
LogData = new List <string>()
{
"Required parameter hand not defined"
}
});
}
//First extract all parameters
if (instruction.Properties.ContainsKey("Trajectory"))
{
string pathConstraintID = instruction.Properties["Trajectory"];
if (instruction.Constraints != null || instruction.Constraints.Where(s => s.PathConstraint != null && s.ID == pathConstraintID).Count() == 0)
{
//Get the path constraint
MPathConstraint pathConstraint = instruction.Constraints.Find(s => s.PathConstraint != null && s.ID == pathConstraintID).PathConstraint;
//Extract the trajectory
this.trajectory = pathConstraint.GetMTransformList();
MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_INFO, $"Assigned hand trajectory. Number elements: {trajectory.Count}, hand: {this.handJoint}");
}
else
{
MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_ERROR, $"Cannot assign trajectory of hand: {this.handJoint}. No suitable MPathConstraint available.");
}
}
return(new MBoolResponse(true));
}
/// <summary>
/// Returns the global position of the specific joint type and posture
/// </summary>
/// <param name="posture"></param>
/// <param name="jointType"></param>
/// <returns></returns>
private MVector3 GetGlobalPosition(MAvatarPostureValues posture, MJointType jointType)
{
this.SkeletonAccess.SetChannelData(posture);
return(this.SkeletonAccess.GetGlobalJointPosition(this.avatarDescription.AvatarID, jointType));
}
/// <summary>
/// Sets the local translation of a single joint. This function is protected, as it only allows for setting
/// the translation, if the joint actually contains translation channels. If not, the translation information
/// is not applied.
/// </summary>
/// <param name="avatarId"></param>
/// <param name="joint"></param>
/// <param name="position"></param>
public void SetLocalJointPosition(string avatarId, MJointType joint, MVector3 position)
{
this.hierarchies[avatarId].GetChild(joint).SetLocalTranslation(position);
}
public override MBoolResponse AssignInstruction(MInstruction instruction, MSimulationState simulationState)
{
//Reset all values
this.hasTrajectory = false;
// Initialize IK Service
this.ServiceAccess.IKService.Setup(this.AvatarDescription, new Dictionary <string, string>());
//Assign the instruction
this.instruction = instruction;
bool hasTarget = false;
String targetID;
//Get the target id using all synonyms
if (instruction.Properties.GetValue(out targetID, "targetID", "TargetID", "objectID"))
{
this.targetObjectTransform = this.SceneAccess.GetTransformByID(targetID);
hasTarget = true;
}
//Error id not available
if (!hasTarget)
{
return(new MBoolResponse(false)
{
LogData = new List <string>()
{
"Required parameter Target ID not defined"
}
});
}
bool hasSubject = false;
String subjectID;
//Get the subject id using all synonyms
if (instruction.Properties.GetValue(out subjectID, "subjectID", "SubjectID"))
{
this.subjectTransform = this.SceneAccess.GetTransformByID(subjectID);
hasSubject = true;
}
//Error id not available
if (!hasSubject)
{
return(new MBoolResponse(false)
{
LogData = new List <string>()
{
"Required parameter Target ID not defined"
}
});
}
//Get the target hand
if (instruction.Properties.ContainsKey("Hand"))
{
if (instruction.Properties["Hand"] == "Left")
{
this.handJoint = MJointType.LeftWrist;
}
if (instruction.Properties["Hand"] == "Right")
{
this.handJoint = MJointType.RightWrist;
}
}
//Error target hand not specified
else
{
return(new MBoolResponse(false)
{
LogData = new List <string>()
{
"Required parameter hand not defined"
}
});
}
//Handle the trajectory constraint (if defined)
if (instruction.Properties.ContainsKey("trajectory"))
{
string trajectoryID = instruction.Properties["trajectory"];
if (instruction.Constraints != null)
{
MConstraint constraint = instruction.Constraints.Find(s => s.ID == trajectoryID);
if (constraint.PathConstraint != null)
{
this.trajectory = constraint.PathConstraint;
this.hasTrajectory = true;
}
}
}
//Compute the relative position and rotation
this.SkeletonAccess.SetChannelData(simulationState.Initial);
MVector3 currentHandPosition = this.SkeletonAccess.GetGlobalJointPosition(simulationState.Current.AvatarID, this.handJoint);
MQuaternion currentHandRotation = this.SkeletonAccess.GetGlobalJointRotation(simulationState.Current.AvatarID, this.handJoint);
/* Old system could access joint positions from MAvatarPosture directly, now we have to use the intermediate skeleton.
* MVector3 currentHandPosition = simulationState.Initial.GetGlobalPosition(this.handJoint);
* MQuaternion currentHandRotation = simulationState.Initial.GetGlobalRotation(this.handJoint);
*/
//Compute the offsets between hand <-> object
this.handPositionOffset = this.subjectTransform.InverseTransformPoint(currentHandPosition);
this.handRotationOffset = this.subjectTransform.InverseTransformRotation(currentHandRotation);
return(new MBoolResponse(true));
}
/// <summary>
/// Sets the rotation of a specific joint of an avatar in world space. Child
/// joints are not affected by this rotation.
/// </summary>
/// <param name="avatarId"></param>
/// <param name="joint"></param>
/// <param name="rotation"></param>
public void SetGlobalJointRotation(string avatarId, MJointType joint, MQuaternion rotation)
{
this.hierarchies[avatarId].GetChild(joint).SetGlobalRotManually(rotation);
this.RecomputeCurrentPostureValues(avatarId);
}
/// <summary>
/// Returns the position of a joint in a joint local space. The joint local space
/// is the parent space after the transformation by offset position and rotation.
/// If the joint has no translation channels, the return will be a zero vector.
/// </summary>
/// <param name="avatarId"></param>
/// <param name="joint"></param>
/// <returns></returns>
public MVector3 GetLocalJointPosition(string avatarId, MJointType joint)
{
return(this.hierarchies[avatarId].GetChild(joint).GetLocalTranslation());
}
/// <summary>
/// Gets the local joint rotation of a specific joint in a specific avatar.
/// The rotation is in a joint local space, meaning the space of the parent joint
/// after the offset translation and rotations are applied.
/// </summary>
/// <param name="avatarID"></param>
/// <param name="boneType">joint</param>
/// <returns></returns>
public MQuaternion GetLocalJointRotation(string avatarID, MJointType boneType)
{
return(this.hierarchies[avatarID].GetChild(boneType).GetCurrentRotationValues());
}
/// <summary>
/// Method is used to setup the parameters of one individual hand
/// </summary>
/// <param name="type"></param>
/// <param name="instruction"></param>
private void SetupHand(MJointType type, MInstruction instruction)
{
HandContainer hand = this.activeHands.Find(s => s.Type == type);
if (hand != null)
{
this.activeHands.Remove(hand);
}
//Create a new hand
hand = new HandContainer(type, instruction, true);
if (instruction.Properties.ContainsKey("Velocity"))
{
hand.Velocity = float.Parse(instruction.Properties["Velocity"], System.Globalization.CultureInfo.InvariantCulture);
}
if (instruction.Properties.ContainsKey("AngularVelocity"))
{
hand.AngularVelocity = float.Parse(instruction.Properties["AngularVelocity"], System.Globalization.CultureInfo.InvariantCulture);
}
if (instruction.Properties.ContainsKey("Acceleration"))
{
hand.Acceleration = float.Parse(instruction.Properties["Acceleration"], System.Globalization.CultureInfo.InvariantCulture);
}
if (instruction.Properties.ContainsKey("HoldDuration"))
{
hand.HoldTime = float.Parse(instruction.Properties["HoldDuration"], System.Globalization.CultureInfo.InvariantCulture);
}
if (instruction.Properties.ContainsKey("CollisionAvoidance"))
{
hand.CollisionAvoidance = bool.Parse(instruction.Properties["CollisionAvoidance"]);
if (hand.CollisionAvoidance)
{
MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_INFO, $"Using local collision avoidance, hand: {hand.Type}");
}
}
//First extract all parameters
if (instruction.Properties.ContainsKey("Trajectory"))
{
string pathConstraintID = instruction.Properties["Trajectory"];
if (instruction.Constraints != null || instruction.Constraints.Where(s => s.PathConstraint != null && s.ID == pathConstraintID).Count() == 0)
{
//Get the path constraint
MPathConstraint pathConstraint = instruction.Constraints.Find(s => s.PathConstraint != null && s.ID == pathConstraintID).PathConstraint;
//Extract the trajectory
hand.Trajectory = pathConstraint.GetMTransformList();
MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_INFO, $"Assigned hand trajectory. Number elements: {hand.Trajectory.Count}, hand: {hand.Type}");
}
else
{
MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_ERROR, $"Cannot assign trajectory of hand: {hand.Type}. No suitable MPathConstraint available.");
}
}
this.activeHands.Add(hand);
}
/// <summary>
/// Returns the joint rotation in world space for a specific joint and avatar.
/// </summary>
/// <param name="avatarId"></param>
/// <param name="boneType">joint</param>
/// <returns></returns>
public MQuaternion GetGlobalJointRotation(string avatarId, MJointType boneType)
{
return(this.hierarchies[avatarId].GetChild(boneType).GetGlobalRotation());
}
/// <summary>
/// Sets the local joint rotation of a specific joint in a specific avatar.
/// The rotation is to be set in a joint local space, meaning the space of the parent joint
/// after the offset translation and rotations are applied.
/// </summary>
/// <param name="avatarID"></param>
/// <param name="boneType">joint</param>
/// <param name="rotation"></param>
public void SetLocalJointRotation(string avatarID, MJointType boneType, MQuaternion rotation)
{
this.hierarchies[avatarID].GetChild(boneType).SetLocalRotation(rotation);
}
/// <summary>
/// Returns the joint position in world space for a specific joint and avatar.
/// </summary>
/// <param name="avatarId"></param>
/// <param name="boneType">joint</param>
/// <returns></returns>
public MVector3 GetGlobalJointPosition(string avatarId, MJointType boneType)
{
return(this.hierarchies[avatarId].GetChild(boneType).GetGlobalPosition());
}
public override MBoolResponse AssignInstruction(MInstruction instruction, MSimulationState simulationState)
{
//Assign the instruction
this.instruction = instruction;
//Reset the flags and states
this.positioningFinished = false;
this.rootPositionLastFrame = null;
// Initialize IK Service
this.ServiceAccess.IKService.Setup(this.AvatarDescription, new Dictionary <string, string>());
//Get the target id
if (instruction.Properties.ContainsKey("TargetID"))
{
this.objectTransform = this.SceneAccess.GetTransformByID(instruction.Properties["TargetID"]);
}
//Error id not available
else
{
return(new MBoolResponse(false)
{
LogData = new List <string>()
{
"Required parameter TargetID not defined"
}
});
}
//Get the target hand
if (instruction.Properties.ContainsKey("Hand"))
{
if (instruction.Properties["Hand"] == "Left")
{
this.handJoint = MJointType.LeftWrist;
}
if (instruction.Properties["Hand"] == "Right")
{
this.handJoint = MJointType.RightWrist;
}
}
//Error target hand not specified
else
{
return(new MBoolResponse(false)
{
LogData = new List <string>()
{
"Required parameter hand not defined"
}
});
}
//Parse optional property
if (instruction.Properties.ContainsKey("PositioningFinishedThreshold"))
{
float.TryParse(instruction.Properties["PositioningFinishedThreshold"], out this.positioningFinishedThreshold);
}
//Parse optional property
if (instruction.Properties.ContainsKey("AddOffset"))
{
bool.TryParse(instruction.Properties["AddOffset"], out addOffset);
}
//Extract the velocity if defined
if (instruction.Properties.ContainsKey("Velocity"))
{
float.TryParse(instruction.Properties["Velocity"], out velocity);
}
//Get the initial position and rotation
this.SkeletonAccess.SetChannelData(simulationState.Initial);
MVector3 currentHandPosition = this.SkeletonAccess.GetGlobalJointPosition(this.AvatarDescription.AvatarID, this.handJoint);
MQuaternion currentHandRotation = this.SkeletonAccess.GetGlobalJointRotation(this.AvatarDescription.AvatarID, this.handJoint);
//Create a new transform representing the "virtual" transform of the hand
MTransform handTransform = new MTransform("hand", currentHandPosition, currentHandRotation);
//Compute the offsets between hand <-> object
this.objectPositionOffset = handTransform.InverseTransformPoint(objectTransform.Position);
this.objectRotationOffset = handTransform.InverseTransformRotation(objectTransform.Rotation);
return(new MBoolResponse(true));
}
/// <summary>
/// Sets up the respective hand
/// </summary>
/// <param name="type"></param>
/// <param name="instruction"></param>
private void SetupHand(MJointType type, MInstruction instruction)
{
HandContainer hand = this.ActiveHands.Find(s => s.JointType == type);
if (hand != null)
{
this.ActiveHands.Remove(hand);
}
//Create a new hand
hand = new HandContainer(type, instruction, true);
//First extract all parameters
if (!instruction.Properties.GetValue(out hand.Velocity, "Velocity"))
{
hand.Velocity = 1.0f;
}
//First extract all parameters
if (!instruction.Properties.GetValue(out hand.AngularVelocity, "AngularVelocity"))
{
hand.AngularVelocity = 30f;
}
if (!instruction.Properties.GetValue(out hand.Acceleration, "Acceleration"))
{
hand.Acceleration = 1.0f;
}
if (!instruction.Properties.GetValue(out hand.Repetitions, "Repetitions"))
{
hand.Repetitions = 1;
}
if (!instruction.Properties.GetValue(out hand.TurningAngle, "Angle"))
{
hand.TurningAngle = 45f;
}
if (instruction.Properties.GetValue(out hand.MinAngle, "MinAngle"))
{
hand.AngleIntervalDefined = true;
}
if (instruction.Properties.GetValue(out hand.MaxAngle, "MaxAngle"))
{
hand.AngleIntervalDefined = true;
}
instruction.Properties.GetValue(out hand.FixFingerTransformations, "FixFingerTransformations");
if (instruction.Properties.ContainsKey("Axis"))
{
hand.TurningAxis = SceneAccess.GetTransformByID(instruction.Properties["Axis"]);
}
if (instruction.Properties.ContainsKey("SubjectID"))
{
hand.Subject = this.SceneAccess.GetSceneObjectByID(instruction.Properties["SubjectID"]);
}
if (instruction.Properties.ContainsKey("TargetID"))
{
hand.Target = this.SceneAccess.GetSceneObjectByID(instruction.Properties["TargetID"]);
}
this.ActiveHands.Add(hand);
}
/// <summary>
/// Returns the global rotation of the specific joint type and posture
/// </summary>
/// <param name="posture"></param>
/// <param name="jointType"></param>
/// <returns></returns>
private MQuaternion GetGlobalRotation(MAvatarPostureValues posture, MJointType jointType)
{
this.SkeletonAccess.SetChannelData(posture);
return(this.SkeletonAccess.GetGlobalJointRotation(this.avatarDescription.AvatarID, jointType));
}
/// <summary>
/// Sets the joint position of a specific joint for an avatar in world space.
/// This function has to be used with caution, as it may break the skeleton.
/// This function is not protected, meaning it sets the global position as well for joints,
/// where there is not translation channel.
/// </summary>
/// <param name="avatarId"></param>
/// <param name="joint"></param>
/// <param name="position"></param>
public void SetGlobalJointPosition(string avatarId, MJointType joint, MVector3 position)
{
this.hierarchies[avatarId].GetChild(joint).SetGlobalPosManually(position);
this.RecomputeCurrentPostureValues(avatarId);
}
/// <summary>
/// Method parses the parameters
/// </summary>
/// <param name="instruction"></param>
/// <returns></returns>
private MBoolResponse ParseParameters(MInstruction instruction)
{
MBoolResponse response = new MBoolResponse(true);
//Get the target hand
if (instruction.Properties.ContainsKey("Hand"))
{
if (instruction.Properties["Hand"] == "Left")
{
this.handJoint = MJointType.LeftWrist;
}
if (instruction.Properties["Hand"] == "Right")
{
this.handJoint = MJointType.RightWrist;
}
}
//Error target hand not specified
else
{
return(new MBoolResponse(false)
{
LogData = new List <string>()
{
"Required parameter hand not defined"
}
});
}
if (instruction.Properties.ContainsKey("Velocity"))
{
this.velocity = float.Parse(instruction.Properties["Velocity"], System.Globalization.CultureInfo.InvariantCulture);
}
if (instruction.Properties.ContainsKey("AngularVelocity"))
{
this.angularVelocity = float.Parse(instruction.Properties["AngularVelocity"], System.Globalization.CultureInfo.InvariantCulture);
}
if (instruction.Properties.ContainsKey("UseBlending"))
{
bool.TryParse("UseBlending", out bool useBlending);
this.useIK = !useBlending;
}
if (instruction.Properties.ContainsKey("EndBlendDuration"))
{
this.endBlendDuration = float.Parse(instruction.Properties["EndBlendDuration"], System.Globalization.CultureInfo.InvariantCulture);
}
//First extract all parameters
if (instruction.Properties.ContainsKey("Trajectory"))
{
string pathConstraintID = instruction.Properties["Trajectory"];
if (instruction.Constraints != null || instruction.Constraints.Where(s => s.PathConstraint != null && s.ID == pathConstraintID).Count() == 0)
{
//Get the path constraint
MPathConstraint pathConstraint = instruction.Constraints.Find(s => s.PathConstraint != null && s.ID == pathConstraintID).PathConstraint;
//Extract the trajectory
this.trajectory = pathConstraint.GetMTransformList();
//Add the target transform
this.trajectory.Add(new MTransform("targetTransform", new MVector3(0, 0, 0), new MQuaternion(0, 0, 0, 1)));
MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_INFO, $"Assigned hand trajectory. Number elements: {trajectory.Count}, hand: {this.handJoint}");
}
else
{
MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_ERROR, $"Cannot assign trajectory of hand: {this.handJoint}. No suitable MPathConstraint available.");
}
}
return(response);
}
/// <summary>
/// Indicates whether an endeffector constraint for the given type is defined
/// </summary>
/// <param name="type"></param>
/// <returns></returns>
public virtual bool HasEndeffectorConstraint(MJointType type)
{
return(this.constraints.Exists(s => s.JointConstraint != null && s.JointConstraint.JointType == type));
}
