/// <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>
/// 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);
}