/// <summary>
/// Performs the do step realted computations for the single hand mode
/// </summary>
/// <param name="result"></param>
/// <param name="time"></param>
/// <param name="simulationState"></param>
/// <returns></returns>
private MSimulationResult DoStepSingleHanded(MSimulationResult result, double time, MSimulationState simulationState)
{
//The presently active constraints
List <MConstraint> globalConstraints = result.Constraints;
//Operate on the local constraints
this.constraintManager.SetConstraints(ref globalConstraints);
//Handle each active hand
for (int i = this.ActiveHands.Count - 1; i >= 0; i--)
{
//Get the current hand container
HandContainer hand = this.ActiveHands[i];
//Handle the state
switch (hand.State)
{
case CarryState.Positioning:
//Call positioning of single hand and add the resulting ik properties (e.g. hand position/Rotation)
this.PositionObjectSingleHand(ref result, time, hand);
break;
case CarryState.Carry:
//Call the single handed carry and add the resulting ik properties (e.g. hand position/rotation)
this.SingleHandedCarry(ref result, time, hand);
break;
}
}
//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;
}
return(result);
}
/// <summary>
/// To do in future use instruction as argument
/// Because it is not know which instruction should be aborted
/// </summary>
/// <returns></returns>
public override MBoolResponse Abort(string instructionID)
{
//To do -> identify the correct hand which should be aborted
HandContainer hand = this.ActiveHands.Find(s => s.Instruction.ID == instructionID);
if (hand != null)
{
this.ActiveHands.Remove(hand);
}
else
{
Console.WriteLine("Carry aborted");
this.ActiveHands.Clear();
}
return(new MBoolResponse(true));
}
/// <summary>
/// Setups the hand
/// </summary>
/// <param name="type"></param>
/// <param name="instruction"></param>
/// <returns></returns>
private HandContainer SetupHand(HandType 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)
{
BlendStartPosture = this.simulationState.Initial.Copy(),
ElapsedBlendTime = 0,
CarryTargetName = null
};
//Compute the blend time based on the distance and velocity
hand.BlendDuration = GetHandDistance(hand.Type) / hand.Velocity;
//First extract all parameters
if (instruction.Properties.ContainsKey("Velocity"))
{
hand.Velocity = float.Parse(instruction.Properties["Velocity"], System.Globalization.CultureInfo.InvariantCulture);
}
//Use the carry target if defined
if (instruction.Properties.ContainsKey("CarryTarget"))
{
hand.CarryTargetName = instruction.Properties["CarryTarget"];
}
this.ActiveHands.Add(hand);
return(hand);
}
/// <summary>
/// Performs a single handed carry
/// Just sets the object relative to the hand
/// </summary>
/// <param name="result"></param>
/// <param name="hand"></param>
/// <returns></returns>
private List <MIKProperty> SingleHandedCarry(ref MSimulationResult result, double time, HandContainer hand)
{
List <MIKProperty> ikProperties = new List <MIKProperty>();
//Check if a carry target is specified
if (hand.CarryTargetName != null)
{
//Compute the root velocity
double rootVelocity = this.ComputeRootVelocity(time);
//Get the current transform of the carry target for the respective hand
MTransform targetTr = SceneAccess.GetTransformByID(this.CarryTargetName);
//The transform of the carry target
MTransform targetTransform = new MTransform("", targetTr.Position, targetTr.Rotation);
//Compute the global position of the respective hand based on the object
MVector3 targetHandPosition = targetTransform.TransformPoint(hand.HandOffset.Position);
MQuaternion targetHandRotation = targetTransform.TransformRotation(hand.HandOffset.Rotation);
//Get the current hand transform
MTransform currentHandTransform = GetTransform(simulationState.Initial, hand.Type);
//Compute the new hand pose
MTransform nextHandPose = this.DoLocalMotionPlanning(rootVelocity + hand.Velocity, TimeSpan.FromSeconds(time), currentHandTransform.Position, currentHandTransform.Rotation, targetHandPosition, targetHandRotation);
//Compute the object transform
result.SceneManipulations.Add(new MSceneManipulation()
{
Transforms = new List <MTransformManipulation>()
{
new MTransformManipulation()
{
Target = hand.Instruction.Properties["TargetID"],
Position = nextHandPose.TransformPoint(hand.ObjectOffset.Position),
Rotation = nextHandPose.TransformRotation(hand.ObjectOffset.Rotation)
}
}
});
//Set the position and rotation parameters of the ik
this.constraintManager.SetEndeffectorConstraint(hand.JointType, nextHandPose.Position, nextHandPose.Rotation, hand.ConstraintID);
}
//Just set the object relative to the current hand
else
{
//Create a transform representing the hand transform for the planned frame
MTransform handTransform = GetTransform(simulationState.Current, hand.Type);
if (this.UseCarryIK)
{
this.constraintManager.SetEndeffectorConstraint(hand.JointType, handTransform.Position, handTransform.Rotation, hand.ConstraintID);
}
//Compute the object transform
result.SceneManipulations.Add(new MSceneManipulation()
{
Transforms = new List <MTransformManipulation>()
{
new MTransformManipulation()
{
Target = hand.Instruction.Properties["TargetID"],
Position = handTransform.TransformPoint(hand.ObjectOffset.Position),
Rotation = handTransform.TransformRotation(hand.ObjectOffset.Rotation)
}
}
});
}
//To do optionally consider self-collisions
return(ikProperties);
}
/// <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);
}
}
