/// <summary>
/// Method is repsonsible for modeling the actual carry for both handed objects
/// </summary>
/// <param name="result"></param>
private void BothHandedCarry(ref MSimulationResult result)
{
//Create an empty transform representing the next object transform
MTransform nextObjectTransform = new MTransform();
//Update the desired object transform
if (this.CarryTargetName != null && this.CarryTargetName.Length > 0)
{
MTransform targetTransform = SceneAccess.GetTransformByID(this.CarryTargetName);
nextObjectTransform.Position = targetTransform.Position;
nextObjectTransform.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));
nextObjectTransform.Position = refTransform.TransformPoint(this.internalCarryTransform.Position);
nextObjectTransform.Rotation = refTransform.TransformRotation(this.internalCarryTransform.Rotation);
}
//Compute the object transform
result.SceneManipulations.Add(new MSceneManipulation()
{
Transforms = new List <MTransformManipulation>()
{
new MTransformManipulation()
{
Target = instruction.Properties["TargetID"],
Position = nextObjectTransform.Position,
Rotation = nextObjectTransform.Rotation
}
}
});
List <MIKProperty> ikProperties = new List <MIKProperty>();
//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));
this.constraintManager.SetEndeffectorConstraint(hand.JointType, nextHandPose.Position, nextHandPose.Rotation, hand.ConstraintID);
}
}
/// <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>
/// 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>
/// 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>
/// 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);
}
