/// <summary>
/// Performs the actual motion planning
/// </summary>
/// <param name="velocity"></param>
/// <param name="angularVelocity"></param>
/// <param name="time"></param>
/// <param name="currentPosition"></param>
/// <param name="currentRotation"></param>
/// <param name="targetPosition"></param>
/// <param name="targetRotation"></param>
/// <returns></returns>
private MTransform DoLocalMotionPlanning(float velocity, float angularVelocity, TimeSpan time, MVector3 currentPosition, MQuaternion currentRotation, MVector3 targetPosition, MQuaternion targetRotation)
{
MTransform result = new MTransform();
MVector3 delta = targetPosition.Subtract(currentPosition);
double angle = Math.Abs(MQuaternionExtensions.Angle(currentRotation, targetRotation));
double maxTranslationDelta = velocity * time.TotalSeconds;
if (delta.Magnitude() >= maxTranslationDelta)
{
delta = delta.Normalize();
delta = delta.Multiply(maxTranslationDelta);
}
//To do consider self collision
double maxAngle = angularVelocity * time.TotalSeconds;
if (angle < maxAngle)
{
angle = maxAngle;
}
double weight = Math.Min(1, maxAngle / angle);
result.Position = currentPosition.Add(delta);
result.Rotation = MQuaternionExtensions.Slerp(currentRotation, targetRotation, (float)weight);
//result.Time = time;
return(result);
}
/// <summary>
/// Performs a local motion planning to estimate the next pose
/// </summary>
/// <param name="velocity"></param>
/// <param name="time"></param>
/// <param name="currentPosition"></param>
/// <param name="currentRotation"></param>
/// <param name="targetPosition"></param>
/// <param name="targetRotation"></param>
/// <returns></returns>
private MTransform DoLocalMotionPlanning(double velocity, TimeSpan time, MVector3 currentPosition, MQuaternion currentRotation, MVector3 targetPosition, MQuaternion targetRotation)
{
//Create a resulting transform
MTransform result = new MTransform();
//Estimate the delta
MVector3 deltaPosition = targetPosition.Subtract(currentPosition);
//Estimate the meximum allowed delta
double maxTranslationDelta = velocity * time.TotalSeconds;
//Limit the maximum
if (deltaPosition.Magnitude() >= maxTranslationDelta)
{
deltaPosition = deltaPosition.Normalize();
deltaPosition = deltaPosition.Multiply(maxTranslationDelta);
}
float angularVelocityReach = 100f;
double angle = Math.Abs(MQuaternionExtensions.Angle(currentRotation, targetRotation));
double maxAngle = angularVelocityReach * time.TotalSeconds;
//Estimate the blendweihgt for the oreitnation blending
double weight = Math.Min(1, maxAngle / angle);
result.Position = currentPosition.Add(deltaPosition);
result.Rotation = MQuaternionExtensions.Slerp(currentRotation, targetRotation, (float)weight);
//result.Time = time;
return(result);
}
/// <summary>
/// Performs local motion planning to reach the defined point
/// </summary>
/// <param name="velocity"></param>
/// <param name="time"></param>
/// <param name="currentPosition"></param>
/// <param name="currentRotation"></param>
/// <param name="targetPosition"></param>
/// <param name="targetRotation"></param>
/// <returns></returns>
private MTransform DoLocalMotionPlanning(double velocity, double angularVelocity, TimeSpan time, MVector3 currentPosition, MQuaternion currentRotation, MVector3 targetPosition, MQuaternion targetRotation)
{
//Create a new transform representing the result
MTransform result = new MTransform();
//Estimate the vector to reach the goal
MVector3 delta = targetPosition.Subtract(currentPosition);
float distance = delta.Magnitude();
//Determine the angular distance
double angle = Math.Abs(MQuaternionExtensions.Angle(currentRotation, targetRotation));
//Determine the max translation delta and max angle
double maxTranslationDelta = velocity * time.TotalSeconds;
double maxAngle = angularVelocity * time.TotalSeconds;
//Compute the translation weight
float translationWeight = (float)Math.Min(1, maxTranslationDelta / delta.Magnitude());
//Compute the rotation weight
float rotationWeight = (float)Math.Min(1, maxAngle / angle);
//Limit the translation
if (delta.Magnitude() >= maxTranslationDelta)
{
delta = delta.Normalize();
delta = delta.Multiply(maxTranslationDelta);
}
//Compute the new position
result.Position = currentPosition.Add(delta);
if (angularVelocity == 0)
{
result.Rotation = MQuaternionExtensions.Slerp(currentRotation, targetRotation, translationWeight);
}
else
{
result.Rotation = MQuaternionExtensions.Slerp(currentRotation, targetRotation, rotationWeight);
}
return(result);
}
/// <summary>
/// Performs the position step for a single hand
/// </summary>
/// <param name="result"></param>
/// <param name="time"></param>
/// <param name="hand"></param>
/// <returns></returns>
private void PositionObjectSingleHand(ref MSimulationResult result, double time, HandContainer hand)
{
//Compute the root velocity
double rootVelocity = this.ComputeRootVelocity(time);
//The current hand transform (the approved result of the last frame)
MTransform currentHandTransform = GetTransform(simulationState.Initial, hand.Type);
//The desired hand transform (of the underlying animation)
MTransform targetHandTransform = GetTransform(simulationState.Current, hand.Type);
//Check if for the hand a carry target is defined
if (hand.CarryTargetName != null)
{
//Get the target transform if a carry target is defined
MTransform carryTargetTransform = SceneAccess.GetTransformByID(this.CarryTargetName);
//Compute the global position of the respective hand based on the object
targetHandTransform.Position = carryTargetTransform.TransformPoint(hand.HandOffset.Position);
targetHandTransform.Rotation = carryTargetTransform.TransformRotation(hand.HandOffset.Rotation);
}
rootVelocity = 0f;
//Compute the new hand pose at the end of this frame
MTransform nextHandTransform = this.DoLocalMotionPlanning(rootVelocity + hand.Velocity, TimeSpan.FromSeconds(time), currentHandTransform.Position, currentHandTransform.Rotation, targetHandTransform.Position, targetHandTransform.Rotation);
//Compute the object transform
result.SceneManipulations.Add(new MSceneManipulation()
{
Transforms = new List <MTransformManipulation>()
{
new MTransformManipulation()
{
Target = hand.Instruction.Properties["TargetID"],
//Compute the object location with respect to the offset
Position = nextHandTransform.TransformPoint(hand.ObjectOffset.Position),
Rotation = nextHandTransform.TransformRotation(hand.ObjectOffset.Rotation)
}
}
});
float translationDistance = targetHandTransform.Position.Subtract(nextHandTransform.Position).Magnitude();
float angularDistance = (float)MQuaternionExtensions.Angle(nextHandTransform.Rotation, targetHandTransform.Rotation);
//Check if goal reached -> change state
if (translationDistance < 0.01f && angularDistance < 2)
{
result.Events.Add(new MSimulationEvent("PositioningFinished", "PositioningFinished", hand.Instruction.ID));
//Finally in carry state
hand.State = CarryState.Carry;
//Set the constraint if the carry ik is enabled
if (UseCarryIK || hand.CarryTargetName != null)
{
//Remove the endeffector constraint no carry ik
this.constraintManager.SetEndeffectorConstraint(hand.JointType, nextHandTransform.Position, nextHandTransform.Rotation, hand.ConstraintID);
}
else
{
//Remove the endeffector constraint no carry ik
this.constraintManager.RemoveEndeffectorConstraints(hand.JointType);
}
}
//Not finished
else
{
//Set the position and rotation parameters of the ik
this.constraintManager.SetEndeffectorConstraint(hand.JointType, nextHandTransform.Position, nextHandTransform.Rotation, hand.ConstraintID);
}
}
/// <summary>
/// Method is responsible for modeling the positiong the object and hands which is the first part of the carry for both handed objects
/// </summary>
/// <param name="result"></param>
/// <param name="time"></param>
private void PositionObjectBothHanded(ref MSimulationResult result, double time)
{
double rootVelocity = this.ComputeRootVelocity(time);
MAvatarPostureValues avatarPose = this.simulationState.Initial;
MTransform currentObjectTransform = this.SceneAccess.GetTransformByID(this.instruction.Properties["TargetID"]);
//Move the object to a central spot in front of the avatar
//Create a new transform for the target object transform
MTransform targetObjectTransform = new MTransform();
if (this.CarryTargetName != null && this.CarryTargetName.Length > 0)
{
MTransform targetTransform = SceneAccess.GetTransformByID(this.CarryTargetName);
targetObjectTransform.Position = targetTransform.Position;
targetObjectTransform.Rotation = targetTransform.Rotation;
}
else
{
MTransform refTransform = GetTransform(this.simulationState.Initial, bothHandedCarryReferenceJoint);
MVector3 forward = GetRootForwad(this.simulationState.Initial);
//Determine the ref transform rotation
refTransform.Rotation = MQuaternionExtensions.FromEuler(new MVector3(0, Extensions.SignedAngle(new MVector3(0, 0, 1), forward, new MVector3(0, 1, 0)), 0));
targetObjectTransform.Position = refTransform.TransformPoint(this.internalCarryTransform.Position);
targetObjectTransform.Rotation = refTransform.TransformRotation(this.internalCarryTransform.Rotation);
}
MTransform nextObjectPose = this.DoLocalMotionPlanning(rootVelocity + positionObjectVelocity, TimeSpan.FromSeconds(time), currentObjectTransform.Position, currentObjectTransform.Rotation, targetObjectTransform.Position, targetObjectTransform.Rotation);
MTransform nextObjectTransform = new MTransform("", nextObjectPose.Position, nextObjectPose.Rotation);
//Update the position of the object
result.SceneManipulations.Add(new MSceneManipulation()
{
Transforms = new List <MTransformManipulation>()
{
new MTransformManipulation()
{
Target = instruction.Properties["TargetID"],
Position = nextObjectPose.Position,
Rotation = nextObjectPose.Rotation
}
}
});
//Update the hands
foreach (HandContainer hand in this.ActiveHands)
{
//Update the hands
MTransform nextHandPose = new MTransform("", nextObjectTransform.TransformPoint(hand.HandOffset.Position), nextObjectTransform.TransformRotation(hand.HandOffset.Rotation));
//Set a new endeffector constraint
this.constraintManager.SetEndeffectorConstraint(hand.JointType, nextHandPose.Position, nextHandPose.Rotation, hand.ConstraintID);
//Assign the hand pose to preserve finger rotations
result.Posture = AssignHandPose(result.Posture, hand.HandPose, hand.Type);
}
//Check if position is finished
if ((targetObjectTransform.Position.Subtract(nextObjectPose.Position)).Magnitude() < 0.01f && MQuaternionExtensions.Angle(targetObjectTransform.Rotation, nextObjectPose.Rotation) < 0.1f)
{
result.Events.Add(new MSimulationEvent("PositioningFinished", "PositioningFinished", instruction.ID));
//Only consider the rotation around y axis
double yRotation = this.GetRootRotation(this.simulationState.Initial).ToEuler().Y;
MTransform rootTransform = new MTransform("", this.GetRootPosition(this.simulationState.Initial), MQuaternionExtensions.FromEuler(new MVector3(0, yRotation, 0)));
//Update the new relative coordinates
this.relativeObjectRotation = rootTransform.InverseTransformRotation(nextObjectTransform.Rotation);
this.relativeObjectPosition = rootTransform.InverseTransformPoint(nextObjectTransform.Position);
this.bothHandedState = CarryState.Carry;
//Get the joint constraints
List <MConstraint> jointConstraints = this.constraintManager.GetJointConstraints();
//Solve using ik if constraints are defined
if (jointConstraints.Count > 0)
{
MIKServiceResult ikResult = this.ServiceAccess.IKService.CalculateIKPosture(result.Posture, jointConstraints, new Dictionary <string, string>());
result.Posture = ikResult.Posture;
}
}
}
/// <summary>
/// Do step routine in which the actual simulation result is generated
/// </summary>
/// <param name="time"></param>
/// <param name="simulationState"></param>
/// <returns></returns>
public override MSimulationResult DoStep(double time, MSimulationState simulationState)
{
//Create a new simulation result
MSimulationResult result = new MSimulationResult()
{
Events = new List <MSimulationEvent>(),
Constraints = simulationState.Constraints ?? new List <MConstraint>(),
SceneManipulations = new List <MSceneManipulation>(),
Posture = simulationState.Current
};
//Compute the target transform at the beginning of each frame
this.targetTransform = this.ComputeTargetTransform();
//The presently active constraints
List <MConstraint> globalConstraints = result.Constraints;
//The local constraints defined within the MMU
List <MConstraint> localConstraints = new List <MConstraint>();
//Use the constraint manager to manage the local constraints
constraintManager.SetConstraints(ref localConstraints);
//Set the channel data to the approved state of the last frame (all MMUs were executed including the low prio grasp/positioning)
this.SkeletonAccess.SetChannelData(simulationState.Initial);
//Get the current hand position and rotation
MVector3 currentHandPosition = this.SkeletonAccess.GetGlobalJointPosition(this.AvatarDescription.AvatarID, this.handJoint);
MQuaternion currentHandRotation = this.SkeletonAccess.GetGlobalJointRotation(this.AvatarDescription.AvatarID, this.handJoint);
//The next pose
MTransform nextPose = null;
//The current velocity used for path planning
float currentVelocity = this.velocity;// + this.ComputeRootVelocity(time, simulationState);
//Use the trajectory if defined
if (this.trajectory != null)
{
//If a trajectory is used -> The target transform is the last point of the trajectory
this.targetTransform = this.trajectory.Last();
//Compute the next pose
nextPose = this.DoLocalMotionPlanning(currentVelocity, this.angularVelocity, TimeSpan.FromSeconds(time), currentHandPosition, currentHandRotation, this.trajectory[trajectoryIndex].Position, this.trajectory[trajectoryIndex].Rotation);
//Check if close to current target -> move to next target -> To do consider rotation
if ((nextPose.Position.Subtract(trajectory[trajectoryIndex].Position)).Magnitude() < this.translationThreshold && MQuaternionExtensions.Angle(nextPose.Rotation, trajectory[trajectoryIndex].Rotation) < this.rotationThreshold && trajectoryIndex < trajectory.Count - 1)
{
trajectoryIndex++;
}
}
else
{
//Compute the next pose
nextPose = this.DoLocalMotionPlanning(currentVelocity, this.angularVelocity, TimeSpan.FromSeconds(time), currentHandPosition, currentHandRotation, this.targetTransform.Position, this.targetTransform.Rotation);
}
//Get the current distance
float currentDistance = (nextPose.Position.Subtract(targetTransform.Position)).Magnitude();
float currentAngularDistance = (float)MQuaternionExtensions.Angle(nextPose.Rotation, targetTransform.Rotation);
//Check if the ik is only computed once and blending is performed subsequently
if (this.singleShotIK)
{
//Estimate the weight for blending
float weight = (float)Math.Min(1, (currentVelocity * time) / currentDistance);
//To check -> Why is a deep copy required?
result.Posture = Blending.PerformBlend((IntermediateSkeleton)this.SkeletonAccess, simulationState.Initial, this.singleShotIKTargetPosture.Copy(), weight, false);
if (weight >= 1 - 1e-3)
{
result.Events.Add(new MSimulationEvent(this.instruction.Name, mmiConstants.MSimulationEvent_End, this.instruction.ID));
constraintManager.SetEndeffectorConstraint(new MJointConstraint(this.handJoint)
{
GeometryConstraint = new MGeometryConstraint()
{
ParentObjectID = "",
ParentToConstraint = new MTransform(System.Guid.NewGuid().ToString(), targetTransform.Position, targetTransform.Rotation)
}
});
}
}
//Default scenario -> IK is computed for each frame
else
{
if (currentDistance <= this.translationThreshold && currentAngularDistance <= this.rotationThreshold)
{
//Set the target
nextPose.Position = targetTransform.Position;
nextPose.Rotation = targetTransform.Rotation;
MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_INFO, "Reach finished");
result.Events.Add(new MSimulationEvent(this.instruction.Name, mmiConstants.MSimulationEvent_End, this.instruction.ID));
}
//Set the desired endeffector constraints
constraintManager.SetEndeffectorConstraint(this.handJoint, nextPose.Position, nextPose.Rotation);
}
//Create a list with the specific constraints for the reach MMU -> Only get the specific ones that must be solved (local constraints)
List <MConstraint> ikConstraints = constraintManager.GetJointConstraints();
//Only solve if at least one constraint is defined
if (ikConstraints.Count > 0)
{
int ikIterations = 1;
MIKServiceResult ikResult = null;
//Use the ik to compute a posture fulfilling the requested constraints
//To do -> Try with different initial postures / compute suitability of the generated posture
for (int i = 0; i < ikIterations; i++)
{
//Compute twice
ikResult = this.ServiceAccess.IKService.CalculateIKPosture(result.Posture, ikConstraints, new Dictionary <string, string>());
result.Posture = ikResult.Posture;
}
}
//Update the constraint manager to operate on the global constraints
constraintManager.SetConstraints(ref globalConstraints);
//Integrate the newly defined constraints in the global ones
constraintManager.Combine(localConstraints);
//Just for better understanding -> Assign the previous constraints + integrated ones to the result (this is not neccessary since the constraint manager is operating on the reference)
result.Constraints = globalConstraints;
//Return the result
return(result);
}
/// <summary>
/// Basic do step routine that is executed for each frame and generates the actual motion.
/// </summary>
/// <param name="time"></param>
/// <param name="simulationState"></param>
/// <returns></returns>
private MSimulationResult DoStepBlending(double time, MSimulationState simulationState)
{
//Create a new simulation result
MSimulationResult result = new MSimulationResult()
{
Events = simulationState.Events ?? new List <MSimulationEvent>(),
Constraints = simulationState.Constraints,
SceneManipulations = simulationState.SceneManipulations ?? new List <MSceneManipulation>(),
Posture = simulationState.Current
};
//Directly operate on the global constraints -> since no ik is computed
List <MConstraint> globalConstraints = result.Constraints;
//Assign the global constraints to the constraint manager
this.constraintManager.SetConstraints(ref globalConstraints);
//Use the initial state (approved posture of last frame)
this.SkeletonAccess.SetChannelData(simulationState.Initial);
//Get the current hand position and rotation
MVector3 currentHandPosition = this.SkeletonAccess.GetGlobalJointPosition(this.AvatarDescription.AvatarID, this.handJoint);
MQuaternion currentHandRotation = this.SkeletonAccess.GetGlobalJointRotation(this.AvatarDescription.AvatarID, this.handJoint);
MJointConstraint jointConstraint = null;
switch (this.handJoint)
{
case MJointType.LeftWrist:
jointConstraint = this.constraintManager.GetEndeffectorConstraint(MJointType.LeftWrist);
break;
case MJointType.RightWrist:
jointConstraint = this.constraintManager.GetEndeffectorConstraint(MJointType.RightWrist);
break;
}
//Handle the joint constraint
if (jointConstraint != null)
{
//Get the current hand positon based on the constraint
currentHandPosition = jointConstraint.GeometryConstraint.GetGlobalPosition(this.SceneAccess);
currentHandRotation = jointConstraint.GeometryConstraint.GetGlobalRotation(this.SceneAccess);
}
//Set the skeleton to the current state
this.SkeletonAccess.SetChannelData(simulationState.Current);
//Determine the target hand position (either underlying MMU or given via boundary constraints)
MVector3 targetHandPosition = this.SkeletonAccess.GetGlobalJointPosition(this.AvatarDescription.AvatarID, this.handJoint);
MQuaternion targetHandRotation = this.SkeletonAccess.GetGlobalJointRotation(this.AvatarDescription.AvatarID, this.handJoint);
//Move the hand from the current position to the target position
MVector3 deltaPosition = targetHandPosition.Subtract(currentHandPosition);
float angle = (float)MQuaternionExtensions.Angle(currentHandRotation, targetHandRotation);
//Compute the distance of the hand to the target hand position
float distanceToGoal = deltaPosition.Magnitude();
//Compute the max distance which can be covered within the current frame
float maxDistance = (float)(time * this.velocity);
//Compute the max allowed angle
float maxAngle = (float)(time * this.angularVelocity);
//Compute the weight for slerping (weight increases with shrinking distance to target)
float translationWeight = Math.Min(1.0f, maxDistance / distanceToGoal);
//Compute the rotation weight
float rotationWeight = Math.Min(1.0f, maxAngle / angle);
//Blend from the current rotation to the target
result.Posture = Blending.PerformBlend((IntermediateSkeleton)this.SkeletonAccess, simulationState.Initial, simulationState.Current, Math.Min(translationWeight, rotationWeight), true);
this.SkeletonAccess.SetChannelData(result.Posture);
if (distanceToGoal < 0.01f)
{
result.Events.Add(new MSimulationEvent()
{
Name = "Release Finished",
Reference = this.instruction.ID,
Type = mmiConstants.MSimulationEvent_End
});
//Remove all constraints for the respective hand
this.constraintManager.RemoveEndeffectorConstraints(this.handJoint);
}
else
{
//Remove the endeffector constraint
this.constraintManager.RemoveEndeffectorConstraints(this.handJoint);
//Update the constraint
this.constraintManager.SetEndeffectorConstraint(this.handJoint, this.SkeletonAccess.GetGlobalJointPosition(result.Posture.AvatarID, this.handJoint), this.SkeletonAccess.GetGlobalJointRotation(result.Posture.AvatarID, this.handJoint));
}
return(result);
}
/// <summary>
/// Basic do step routine that is executed for each frame and generates the actual motion.
/// </summary>
/// <param name="time"></param>
/// <param name="simulationState"></param>
/// <returns></returns>
private MSimulationResult DoStepIK(double time, MSimulationState simulationState)
{
//Create a default result
MSimulationResult result = new MSimulationResult()
{
Events = new List <MSimulationEvent>(),
Constraints = simulationState.Constraints ?? new List <MConstraint>(),
SceneManipulations = new List <MSceneManipulation>(),
Posture = simulationState.Current
};
//The presently active constraints
List <MConstraint> globalConstraints = result.Constraints;
//The local constraints defined within the MMU
List <MConstraint> localConstraints = new List <MConstraint>();
//Use the constraint manager to manage the local constraints
constraintManager.SetConstraints(ref localConstraints);
//Set the channel data to the approved state of the last frame (all MMUs were executed including the low prio grasp/positioning)
this.SkeletonAccess.SetChannelData(simulationState.Initial);
//Get the current hand position and rotation
MVector3 currentHandPosition = this.SkeletonAccess.GetGlobalJointPosition(this.AvatarDescription.AvatarID, this.handJoint);
MQuaternion currentHandRotation = this.SkeletonAccess.GetGlobalJointRotation(this.AvatarDescription.AvatarID, this.handJoint);
//Set the skeleton acess data to the current
this.SkeletonAccess.SetChannelData(simulationState.Current);
//Determine the target hand position (either underlying MMU or given via boundary constraints)
MTransform targetTransform = new MTransform()
{
Position = this.SkeletonAccess.GetGlobalJointPosition(this.AvatarDescription.AvatarID, this.handJoint),
Rotation = this.SkeletonAccess.GetGlobalJointRotation(this.AvatarDescription.AvatarID, this.handJoint)
};
MTransform nextPose = null;
float translationWeight = 0;
//Use the trajectory if defined
if (this.trajectory != null && this.trajectory.Count > 0)
{
//Update the last element dynamically
trajectory.Last().Position = targetTransform.Position;
trajectory.Last().Rotation = targetTransform.Rotation;
//Compute the next pose
nextPose = this.DoLocalMotionPlanning(this.velocity, this.angularVelocity, TimeSpan.FromSeconds(time), currentHandPosition, currentHandRotation, this.trajectory[trajectoryIndex].Position, this.trajectory[trajectoryIndex].Rotation, out translationWeight);
//Check if close to current target -> move to next target -> To do consider rotation
if ((nextPose.Position.Subtract(trajectory[trajectoryIndex].Position)).Magnitude() < 0.1f && MQuaternionExtensions.Angle(nextPose.Rotation, trajectory[trajectoryIndex].Rotation) < 1f && trajectoryIndex < trajectory.Count - 1)
{
trajectoryIndex++;
}
}
else
{
//Compute the next pose
nextPose = this.DoLocalMotionPlanning(this.velocity, this.angularVelocity, TimeSpan.FromSeconds(time), currentHandPosition, currentHandRotation, targetTransform.Position, targetTransform.Rotation, out translationWeight);
}
////Determine the next pose using local motion planning
//MTransform nextPose = this.DoLocalMotionPlanning(this.velocity, this.angularVelocity, TimeSpan.FromSeconds(time), currentHandPosition, currentHandRotation, targetHandPosition, targetHandRotation, out float translationWeight);
//Perform a partial blend
//result.Posture = this.PerformPartialBlend(this.handJoint, translationWeight, simulationState);
//Get the current distance
float currentDistance = (nextPose.Position.Subtract(targetTransform.Position)).Magnitude();
float currentAngularDistance = (float)MQuaternionExtensions.Angle(nextPose.Rotation, targetTransform.Rotation);
//Handle the present state (either in ik mode oder blending)
switch (this.state)
{
case ReleaseMotionState.IK:
if (currentDistance < 0.02f && currentAngularDistance < 5f)
{
//Switch to blending to realize the final share
this.state = ReleaseMotionState.Blending;
this.elapsedBlendTime = 0;
//Set to global constraints
this.constraintManager.SetConstraints(ref globalConstraints);
//Remove all constraints for the respective hand
this.constraintManager.RemoveEndeffectorConstraints(this.handJoint);
}
else
{
//Update the constraint
this.constraintManager.SetEndeffectorConstraint(this.handJoint, nextPose.Position, nextPose.Rotation);
}
break;
}
//Use the local constraint to compute the ik
this.constraintManager.SetConstraints(ref localConstraints);
//React depending on the given state
switch (this.state)
{
//In ik mode the ik solver must be called
case ReleaseMotionState.IK:
//Create a list with the specific constraints for the reach MMU -> Only get the specific ones that must be solved (local constraints)
List <MConstraint> ikConstraints = constraintManager.GetJointConstraints();
//Only solve if at least one constraint is defined
if (ikConstraints.Count > 0)
{
//Compute twice
MIKServiceResult ikResult = this.ServiceAccess.IKService.CalculateIKPosture(result.Posture, ikConstraints, new Dictionary <string, string>());
result.Posture = ikResult.Posture;
}
break;
//In blending mode, motion blending must be performed
case ReleaseMotionState.Blending:
//Perform a blend
elapsedBlendTime += (float)time;
float blendWeight = Math.Min(1, elapsedBlendTime / endBlendDuration);
result.Posture = MMICSharp.Common.Tools.Blending.PerformBlend(this.SkeletonAccess as IntermediateSkeleton, simulationState.Initial, simulationState.Current, blendWeight, true);
//Perform a partial blend
//result.Posture = this.PerformPartialBlend(this.handJoint, blendWeight, simulationState);
if (blendWeight >= 1f)
{
result.Events.Add(new MSimulationEvent()
{
Name = "Release Finished",
Reference = this.instruction.ID,
Type = mmiConstants.MSimulationEvent_End
});
}
break;
}
//Combine the constraints
this.constraintManager.SetConstraints(ref globalConstraints);
this.constraintManager.Combine(localConstraints);
return(result);
}
/// <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);
}
/// <summary>
/// Method performs a local motion planning and tries to reach the specified goal position and rotation using the given velocity,angular velocity and time.
/// </summary>
/// <param name="velocity"></param>
/// <param name="angularVelocity"></param>
/// <param name="time"></param>
/// <param name="currentPosition"></param>
/// <param name="currentRotation"></param>
/// <param name="targetPosition"></param>
/// <param name="targetRotation"></param>
/// <returns></returns>
private MTransform DoLocalMotionPlanning(double velocity, double angularVelocity, TimeSpan time, MVector3 currentPosition, MQuaternion currentRotation, MVector3 targetPosition, MQuaternion targetRotation, bool collisionAvoidance)
{
//Create a new transform representing the result
MTransform result = new MTransform();
//Estimate the delta
MVector3 delta = targetPosition.Subtract(currentPosition);
//Determine the current delta angle
double angle = Math.Abs(MQuaternionExtensions.Angle(currentRotation, targetRotation));
//Determine the max translation delta and max angle in the current frame
double maxTranslationDelta = velocity * time.TotalSeconds;
double maxAngle = angularVelocity * time.TotalSeconds;
//Estimate the blend weight for the rotation and position
float rotationWeight = (float)Math.Min(1, maxAngle / angle);
float positionWeight = (float)Math.Min(1, maxTranslationDelta / delta.Magnitude());
//Limit the max translation
if (delta.Magnitude() >= maxTranslationDelta)
{
delta = delta.Normalize();
delta = delta.Multiply(maxTranslationDelta);
}
if (collisionAvoidance)
{
MVector3 collisionAvoidanceForce = this.ComputCollisionAvoidance(currentPosition, delta);
//if (collisionAvoidanceForce.Magnitude() > 0)
// MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_INFO, "Collision avoidance force: " + collisionAvoidanceForce.Magnitude());
//Add the collision avoidance force on top
delta = delta.Add(collisionAvoidanceForce);
//Limit the max translation
if (delta.Magnitude() >= maxTranslationDelta)
{
delta = delta.Normalize();
delta = delta.Multiply(maxTranslationDelta);
}
}
//Compute the new position
result.Position = currentPosition.Add(delta);
//Compute the new rotation by interpolating towards the target rotation
if (angularVelocity > 0)
{
result.Rotation = MQuaternionExtensions.Slerp(currentRotation, targetRotation, rotationWeight);
}
//Use the rotation weight
else
{
result.Rotation = MQuaternionExtensions.Slerp(currentRotation, targetRotation, positionWeight);
}
//Return the simulation result
return(result);
}
/// <summary>
/// Basic to step routine which computes the result of the current frame
/// </summary>
/// <param name="time"></param>
/// <returns></returns>
public override MSimulationResult DoStep(double time, MSimulationState simulationState)
{
//Create a new result
MSimulationResult result = new MSimulationResult()
{
Events = simulationState.Events ?? new List <MSimulationEvent>(),
DrawingCalls = new List <MDrawingCall>(),
SceneManipulations = simulationState.SceneManipulations ?? new List <MSceneManipulation>(),
Posture = simulationState.Current,
Constraints = simulationState.Constraints ?? new List <MConstraint>()
};
//The presently active constraints
List <MConstraint> globalConstraints = new List <MConstraint>(result.Constraints);
//The local constraints defined within the MMU
List <MConstraint> localConstraints = new List <MConstraint>();
//Setup the constraint manager and use the local constraints
this.constraintManager.SetConstraints(ref localConstraints);
//Handle each active hand
for (int i = this.activeHands.Count - 1; i >= 0; i--)
{
//Get the current hand
HandContainer hand = this.activeHands[i];
//Skip if hand is not initialized
if (!hand.Initialized)
{
continue;
}
//Get the transform of the object to be positioned
MTransform currentObjectTransform = this.SceneAccess.GetTransformByID(hand.Instruction.Properties["SubjectID"]);
//Get the transform of the target
MTransform targetObjectTransform = null;
//Determine the next location of the object (at the end of the frame)
MTransform nextObjectTransform = null;
//Check if trajectory is defined
if (hand.Trajectory != null)
{
//The last point is the target transform
targetObjectTransform = hand.Trajectory.Last();
//The current rajectory point
MTransform currentTrajectoryPoint = hand.Trajectory[hand.TrajectoryIndex];
//Estimate the next transfom based on local motion planning
nextObjectTransform = this.DoLocalMotionPlanning(hand.Velocity, hand.AngularVelocity, TimeSpan.FromSeconds(time), currentObjectTransform.Position, currentObjectTransform.Rotation, currentTrajectoryPoint.Position, currentTrajectoryPoint.Rotation, hand.CollisionAvoidance);
//Get the current distance
float currentDistance = nextObjectTransform.Position.Subtract(hand.Trajectory[hand.TrajectoryIndex].Position).Magnitude();
float currentAngularDistance = (float)MQuaternionExtensions.Angle(nextObjectTransform.Rotation, hand.Trajectory[hand.TrajectoryIndex].Rotation);
//Check if close to current target -> move to next target
if (currentDistance < this.translationThreshold && currentAngularDistance < this.rotationThreshold && hand.TrajectoryIndex < hand.Trajectory.Count - 1)
{
hand.TrajectoryIndex++;
}
}
//Default behavior if no trajectory is specified
else
{
targetObjectTransform = this.ComputeTargetTransform(hand);
//Estimate the next pose of the scene object
nextObjectTransform = this.DoLocalMotionPlanning(hand.Velocity, hand.AngularVelocity, TimeSpan.FromSeconds(time), currentObjectTransform.Position, currentObjectTransform.Rotation, targetObjectTransform.Position, targetObjectTransform.Rotation, hand.CollisionAvoidance);
}
//Set the pose of the object to the next estimated pose
result.SceneManipulations.Add(new MSceneManipulation()
{
Transforms = new List <MTransformManipulation>()
{
new MTransformManipulation()
{
Target = hand.Instruction.Properties.GetValue("SubjectID", "subjectID"),
Position = nextObjectTransform.Position,
Rotation = nextObjectTransform.Rotation
}
}
});
//Compute the next handpose based on the offset
MTransform nextHandTransform = new MTransform("", nextObjectTransform.TransformPoint(hand.Offset.Position), nextObjectTransform.TransformRotation(hand.Offset.Rotation));
//Set the ik constraints
constraintManager.SetEndeffectorConstraint(hand.Type, nextHandTransform.Position, nextHandTransform.Rotation);
//To do add constraints
float distance = (nextObjectTransform.Position.Subtract(targetObjectTransform.Position)).Magnitude();
float angularDistance = (float)MQuaternionExtensions.Angle(nextObjectTransform.Rotation, targetObjectTransform.Rotation);
//Check if goal criteria fulfilled
if (distance < this.translationThreshold && angularDistance < this.rotationThreshold)
{
//Increment the time
hand.ElapsedHoldTime += time;
if (hand.ElapsedHoldTime < hand.HoldTime)
{
continue;
}
this.activeHands.RemoveAt(i);
//Add new finished event
if (hand.BothHanded)
{
if (activeHands.Count == 0)
{
result.Events.Add(new MSimulationEvent(hand.Instruction.Name, mmiConstants.MSimulationEvent_End, hand.Instruction.ID));
}
}
//Single handed grasp
else
{
result.Events.Add(new MSimulationEvent(hand.Instruction.Name, mmiConstants.MSimulationEvent_End, hand.Instruction.ID));
}
}
}
//Get the properties from the constraint manager
List <MConstraint> jointConstraints = this.constraintManager.GetJointConstraints();
//Use the ik service if at least one constraint must be solved
if (jointConstraints.Count > 0)
{
MIKServiceResult ikResult = this.ServiceAccess.IKService.CalculateIKPosture(simulationState.Current, jointConstraints, new Dictionary <string, string>());
result.Posture = ikResult.Posture;
}
//Configure the constraint manager to operate on the global constraints
constraintManager.SetConstraints(ref globalConstraints);
//Combine the global with the local ones
constraintManager.Combine(localConstraints);
//Provide the combined constraints as result
result.Constraints = globalConstraints;
//Return the simulation result
return(result);
}
/// <summary>
/// Basic to step routine which computes the result of the current frame
/// </summary>
/// <param name="time"></param>
/// <returns></returns>
public override MSimulationResult DoStep(double time, MSimulationState simulationState)
{
//Create a new result
MSimulationResult result = new MSimulationResult()
{
Events = this.simulationState.Events ?? new List <MSimulationEvent>(),
DrawingCalls = new List <MDrawingCall>(),
SceneManipulations = this.simulationState.SceneManipulations ?? new List <MSceneManipulation>(),
Posture = this.simulationState.Current,
Constraints = this.simulationState.Constraints ?? new List <MConstraint>()
};
List <MConstraint> constraints = result.Constraints;
//Setup the constraint manager
this.constraintManager.SetConstraints(ref constraints);
//Set the simulation sate
this.simulationState = simulationState;
//Handle each active hand
for (int i = this.ActiveHands.Count - 1; i >= 0; i--)
{
//Get the current hand
HandContainer hand = this.ActiveHands[i];
if (!hand.Initialized)
{
continue;
}
//Get the transform of the object to be positioned
MTransform currentObjectTransform = hand.Subject.Transform;
//Get the transform of the target
MTransform targetObjectTransform = null;
//Determine the next location of the object (at the end of the frame)
MTransform nextObjectTransform = null;
//Check if trajectory is defined
if (hand.Trajectory != null)
{
//The last point is the target transform
targetObjectTransform = hand.Trajectory.Last();
//Estimate the next transfom based on local motion planning
nextObjectTransform = this.DoLocalMotionPlanning(hand.Velocity, hand.AngularVelocity, TimeSpan.FromSeconds(time), currentObjectTransform.Position, currentObjectTransform.Rotation, hand.Trajectory[hand.TrajectoryIndex].Position, hand.Trajectory[hand.TrajectoryIndex].Rotation);
float translationDistance = (nextObjectTransform.Position.Subtract(hand.Trajectory[hand.TrajectoryIndex].Position)).Magnitude();
double angularDistance = MQuaternionExtensions.Angle(nextObjectTransform.Rotation, hand.Trajectory[hand.TrajectoryIndex].Rotation);
//Check if close to current target -> move to next target
if (translationDistance < 0.01f && angularDistance < 0.5f && hand.TrajectoryIndex < hand.Trajectory.Count - 1)
{
hand.TrajectoryIndex++;
}
}
//Default behavior if no trajectory is specified
else
{
targetObjectTransform = hand.Target.Transform;
//Estimate the next pose of the scene object
nextObjectTransform = this.DoLocalMotionPlanning(hand.Velocity, hand.AngularVelocity, TimeSpan.FromSeconds(time), currentObjectTransform.Position, currentObjectTransform.Rotation, targetObjectTransform.Position, targetObjectTransform.Rotation);
}
//Set the pose of the object to the next estimated pose
result.SceneManipulations.Add(new MSceneManipulation()
{
Transforms = new List <MTransformManipulation>()
{
new MTransformManipulation()
{
Target = hand.Subject.ID,
Position = nextObjectTransform.Position,
Rotation = nextObjectTransform.Rotation
}
}
});
//Compute the next handpose based on the offset
MTransform nextHandTransform = new MTransform("", nextObjectTransform.TransformPoint(hand.Offset.Position), nextObjectTransform.TransformRotation(hand.Offset.Rotation));
//Set the ik constraints
constraintManager.SetEndeffectorConstraint(new MJointConstraint(hand.JointType)
{
GeometryConstraint = new MGeometryConstraint("")
{
ParentToConstraint = new MTransform(Guid.NewGuid().ToString(), nextHandTransform.Position, nextHandTransform.Rotation)
}
});
//To do add constraints
float distance = (nextObjectTransform.Position.Subtract(targetObjectTransform.Position)).Magnitude();
double angularDist = MQuaternionExtensions.Angle(nextObjectTransform.Rotation, targetObjectTransform.Rotation);
if (hand.Trajectory != null && hand.TrajectoryIndex < hand.Trajectory.Count - 2)
{
//Do nothing
}
else
{
if (distance < 0.01f && angularDist < 0.5f)
{
//Increment the time
hand.ElapsedHoldTime += time;
if (hand.ElapsedHoldTime < hand.HoldTime)
{
continue;
}
this.ActiveHands.RemoveAt(i);
//Add new finished event
if (hand.BothHanded)
{
if (ActiveHands.Count == 0)
{
result.Events.Add(new MSimulationEvent(hand.Instruction.Name, mmiConstants.MSimulationEvent_End, hand.Instruction.ID));
}
}
//Single handed grasp
else
{
result.Events.Add(new MSimulationEvent(hand.Instruction.Name, mmiConstants.MSimulationEvent_End, hand.Instruction.ID));
}
}
}
}
//Use the ik service
if (result.Constraints.Count > 0)
{
MIKServiceResult ikResult = ServiceAccess.IKService.CalculateIKPosture(this.simulationState.Current, result.Constraints, new Dictionary <string, string>());
result.Posture = ikResult.Posture;
}
return(result);
}
public override MSimulationResult DoStep(double time, MSimulationState simulationState)
{
//Create a new result
MSimulationResult result = new MSimulationResult()
{
Events = simulationState.Events ?? new List <MSimulationEvent>(),
DrawingCalls = new List <MDrawingCall>(),
SceneManipulations = simulationState.SceneManipulations ?? new List <MSceneManipulation>(),
Posture = simulationState.Current,
Constraints = simulationState.Constraints ?? new List <MConstraint>()
};
//Cast to intermediate skeleton to get all functions
IntermediateSkeleton iS = this.SkeletonAccess as IntermediateSkeleton;
//Assign the approved posture of the last frame (proper finger rotations)
this.SkeletonAccess.SetChannelData(simulationState.Initial);
//First estimate the finger rotations
//------------------------------------------------------------------------------------------------------------------
//Handle each active hand
for (int i = this.ActiveHands.Count - 1; i >= 0; i--)
{
//Get the current hand
HandContainer hand = this.ActiveHands[i];
if (hand.Release)
{
//Use all finger joint if in release mode
foreach (MJointType jointType in fingerJoints)
{
//Get the current rotation of the finger
if (!hand.CurrentFingerRotations.ContainsKey(jointType))
{
hand.CurrentFingerRotations.Add(jointType, iS.GetLocalJointRotation(this.AvatarDescription.AvatarID, jointType));
}
else
{
hand.CurrentFingerRotations[jointType] = iS.GetLocalJointRotation(this.AvatarDescription.AvatarID, jointType);
}
}
}
else
{
//Handle all joint constraints -> Use only the fingers that are constrained
foreach (MJointConstraint joint in hand.FinalPosture.JointConstraints)
{
//Skip if the joint is no finger joint
if (!this.IsFingerJoint(joint.JointType))
{
continue;
}
//Get the current rotation of the finger
if (!hand.CurrentFingerRotations.ContainsKey(joint.JointType))
{
hand.CurrentFingerRotations.Add(joint.JointType, iS.GetLocalJointRotation(this.AvatarDescription.AvatarID, joint.JointType));
}
else
{
hand.CurrentFingerRotations[joint.JointType] = iS.GetLocalJointRotation(this.AvatarDescription.AvatarID, joint.JointType);
}
}
}
}
//Perform the blending
//------------------------------------------------------------------------------------------------------------------
//Assign the approved posture of the last frame (proper finger rotations)
this.SkeletonAccess.SetChannelData(simulationState.Current);
//Handle each active hand
for (int i = this.ActiveHands.Count - 1; i >= 0; i--)
{
//Get the current hand
HandContainer hand = this.ActiveHands[i];
//Flag which indicates whether the fingers are positioned
bool positioned = true;
if (hand.Release)
{
//If in release mode again use all joints being finger joints
foreach (MJointType jointType in fingerJoints)
{
//Get the current rotation of the finger
MQuaternion currentRotation = hand.CurrentFingerRotations[jointType];
//The current rotation is the result of the last frame
MQuaternion desiredRotation = iS.GetLocalJointRotation(this.AvatarDescription.AvatarID, jointType);
//The weight used for blending
double weight = 0;
//The angle between the current rotation and the desired one
double angle = MQuaternionExtensions.Angle(currentRotation, desiredRotation);
//Compute the weight based on the elapsed time if the duration is set
if (hand.HasDuration)
{
weight = hand.Elapsed / hand.Duration;
}
//By default use the angular velocity
else
{
//The max allowed angle in this frame
double maxAngle = time * hand.AngularVelocity;
weight = Math.Min(1, maxAngle / angle);
}
//Compute the new rotation
MQuaternion newRotation = MQuaternionExtensions.Slerp(currentRotation, desiredRotation, (float)weight);
//Set the local joint rotation of the intermediate skeleton
iS.SetLocalJointRotation(this.AvatarDescription.AvatarID, jointType, newRotation);
//Goal criteria
if (angle > 0.1f)
{
positioned = false;
}
}
}
else
{
//Handle all joint constraints
foreach (MJointConstraint joint in hand.FinalPosture.JointConstraints)
{
//Skip if the joint is no finger joint
if (!this.IsFingerJoint(joint.JointType))
{
continue;
}
//Get the current rotation of the finger
MQuaternion currentRotation = hand.CurrentFingerRotations[joint.JointType];
//Get the desired rotation
MQuaternion desiredRotation = joint.GeometryConstraint.ParentToConstraint.Rotation;
//The weight used for blending
double weight = 0;
//The angle between the current rotation and the desired one
double angle = MQuaternionExtensions.Angle(currentRotation, desiredRotation);
//Compute the weight based on the elapsed time if the duration is set
if (hand.HasDuration)
{
weight = hand.Elapsed / hand.Duration;
}
//By default use the angular velocity
else
{
//The max allowed angle in this frame
double maxAngle = time * hand.AngularVelocity;
weight = Math.Min(1, maxAngle / angle);
}
//Compute the new rotation
MQuaternion newRotation = MQuaternionExtensions.Slerp(currentRotation, desiredRotation, (float)weight);
//Set the local joint rotation of the intermediate skeleton
iS.SetLocalJointRotation(this.AvatarDescription.AvatarID, joint.JointType, newRotation);
//Goal criteria
if (angle > 0.1f)
{
positioned = false;
}
}
}
//Provide event if positioned successfully
if (positioned && !hand.Positioned)
{
hand.Positioned = true;
result.Events.Add(new MSimulationEvent()
{
Name = "MoveFingersMMU",
Type = "FingersPositioned",
Reference = hand.Instruction.ID
});
}
//Increment the time
hand.Elapsed += (float)time;
}
//Recompute the posture given the performed changes
result.Posture = iS.RecomputeCurrentPostureValues(this.AvatarDescription.AvatarID);
//Return the simulation result for the given frame
return(result);
}