/// <summary>
/// Performs the actual solving
/// </summary>
/// <param name="input"></param>
/// <param name="mmuResults"></param>
/// <returns></returns>
public virtual MSimulationResult Solve(MSimulationResult input, List <MSimulationResult> mmuResults, float timespan)
{
MSimulationResult result = input;
//By default use all constraints
List <MConstraint> constraints = new List <MConstraint>(input.Constraints);
//Only solve the constraints which are not already fulfilled
if (this.SolveViolatedConstraintsOnly)
{
constraints = this.GetViolatedIKConstraints(input.Constraints, result.Posture);
}
//Compute the ik if at least one constraint is not fulfilled
if (constraints.Count > 0)
{
MAvatarPostureValues currentPosture = input.Posture;
//Solve n times
for (int i = 0; i < this.Iterations; i++)
{
MIKServiceResult ikResult = this.serviceAccess.IKService.CalculateIKPosture(currentPosture, constraints, new Dictionary <string, string>());
currentPosture = ikResult.Posture;
}
result.Posture = currentPosture;
}
return(result);
}
/// <summary>
/// Recomputes the present posture values for the given avatar id.
/// This function has to be called after any set-function call in order
/// to get the MAvatarPosture for the current configuration.
/// If the posture is to be set permantently, SetChannelData has to be called
/// with the generated MAvatarPostureValues
/// </summary>
/// <param name="avatarID"></param>
/// <returns></returns>
public MAvatarPostureValues RecomputeCurrentPostureValues(string avatarID)
{
MAvatarPostureValues ret = this.GetCurrentPostureValues(avatarID);
ret.PostureData = this.hierarchies[avatarID].GetAvatarPostureValues();
return(ret);
}
/// <summary>
/// Retargets the global posture to the intermediate skeleton
/// </summary>
/// <param name="globalTarget"></param>
/// <returns></returns>
public MAvatarPostureValues RetargetToIntermediate(MAvatarPosture globalTarget)
{
RJoint root = ((RJoint)this.skeleton.GetRoot(globalTarget.AvatarID));
bool rootFound = false;
foreach (MJoint j in globalTarget.Joints)
{
if (j.Type != MJointType.Undefined)
{
RJoint rj = ((RJoint)root.GetChild(j.Type));
rj.RetargetPositionToIS(j.Position, j.Rotation);
rj.RetargetRotationToIS(j.Rotation);
if (!rootFound)
{
rootFound = true;
if (j.Type == MJointType.Root)
{
}
else
{
MVector3 globalPos = rj.GetGlobalPosManually();
root.SetGlobalPosManually(new MVector3(globalPos.X, 0, globalPos.Z));
//rj.SetGlobalPosManually(new MVector3(0, globalPos.Y, 0));
}
}
}
}
root.RecomputeLocalTransformations();
MAvatarPostureValues ret = new MAvatarPostureValues(globalTarget.AvatarID, root.GetAvatarPostureValues());
return(ret);
}
/// <summary>
/// Returns partial MAvatarPostureValues for the defined joint list of the lastPostureValues.
/// </summary>
/// <param name="avatarID"></param>
/// <param name="joints"></param>
/// <returns></returns>
public MAvatarPostureValues GetCurrentPostureValuesPartial(string avatarID, List <MJointType> joints)
{
RJoint root = this.hierarchies[avatarID];
List <double> postureValues = root.GetAvatarPostureValuesPartial(joints);
MAvatarPostureValues values = new MAvatarPostureValues(avatarID, postureValues);
return(values);
}
/// <summary>
/// Method to setup the actual retargeting
/// </summary>
protected virtual void SetupRetargeting()
{
//Only do the retargeting setup if not happened in before
if (!setupRetargeting)
{
setupRetargeting = true;
// find and load retargeting configuration file
//Create a unique id for the avatar (only valid in the current session -> otherwise UUID required)
string id = UnitySceneAccess.CreateAvatarID();
//Only load the configuration if defined
if (LoadRetargetingConfiguration)
{
if (!System.IO.File.Exists(Application.dataPath + "/" + this.ConfigurationFilePath))
{
Debug.LogError($"Problem setting up retargeting: The required file: {Application.dataPath + "/" + this.ConfigurationFilePath} is not available");
return;
}
string skelConf = System.IO.File.ReadAllText(Application.dataPath + "/" + this.ConfigurationFilePath);
MAvatarPosture p = MMICSharp.Common.Communication.Serialization.FromJsonString <MAvatarPosture>(skelConf);//JsonConvert.DeserializeObject<MAvatarPosture>(skelConf);
p.AvatarID = id;
this.SetupRetargeting(id, p);
this.AssignPostureValues(retargetingService.RetargetToIntermediate(p));
}
//If not defined use the global posture
else
{
this.SetupRetargeting(id);
}
MAvatarDescription avatarDescription = this.GetSkeletonAccess().GetAvatarDescription(id);
//Create a new MAvatar (the representation within MMI framework)
MAvatarPostureValues zeroPosture = this.GetSkeletonAccess().GetCurrentPostureValues(id);
//Create the avatar
this.MAvatar = new MAvatar()
{
Name = this.name,
ID = id,
Description = avatarDescription,
PostureValues = zeroPosture,
Properties = new Dictionary <string, string>(),
SceneObjects = new List <string>()
};
//Add the avatar to the scene access
UnitySceneAccess.AddAvatar(this.MAvatar);
Debug.Log("Retargeting successfully set up");
}
}
/// <summary>
/// Returns the current posture values for the avatar by recomputing the values based on the internal hierarchy.
/// </summary>
/// <param name="avatarID"></param>
/// <returns></returns>
public MAvatarPostureValues GetCurrentPostureValues(string avatarID)
{
RJoint root = this.hierarchies[avatarID];
List <double> postureValues = root.GetAvatarPostureValues();
MAvatarPostureValues values = new MAvatarPostureValues(avatarID, postureValues);
return(values);
//return this.lastPostureValues[avatarID];
}
/// <summary>
/// Returns the root transform of the specific posture
/// </summary>
/// <param name="posture"></param>
/// <returns></returns>
private MTransform GetRootTransform(MAvatarPostureValues posture)
{
this.SkeletonAccess.SetChannelData(posture);
return(new MTransform()
{
ID = "",
Position = this.SkeletonAccess.GetRootPosition(this.avatarDescription.AvatarID),
Rotation = this.SkeletonAccess.GetRootRotation(this.avatarDescription.AvatarID)
});
}
/// <summary>
/// Returns the forward vector of the root transform
/// </summary>
/// <param name="posture"></param>
/// <returns></returns>
private MVector3 GetRootForwad(MAvatarPostureValues posture)
{
MTransform rootTransform = this.GetRootTransform(posture);
//Compute the forwad vector of the root transform
MVector3 rootForward = rootTransform.Rotation.Multiply(new MVector3(0, 0, 1));
rootForward.Y = 0;
rootForward = rootForward.Normalize();
return(rootForward);
}
/// <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>
/// Returns the forward vector of the root transform
/// </summary>
/// <param name="posture"></param>
/// <returns></returns>
private MVector3 GetGlobalDirection(MAvatarPostureValues posture, MVector3 localDireciton)
{
MTransform rootTransform = this.GetRootTransform(posture);
//Compute the forwad vector of the root transform
MVector3 rootForward = rootTransform.Rotation.Multiply(localDireciton);
rootForward.Y = 0;
rootForward = rootForward.Normalize();
return(rootForward);
}
/// <summary>
/// Applies the transform manipulations to be reflected in the intermediate skeleton
/// </summary>
public virtual void ApplyTransformManipulations()
{
//Update the intermediate skeleton with the new values
//Get the global posture
MAvatarPosture globalPosture = this.GenerateGlobalPosture();
//Perform a retargeting to the intermediate skeleton
MAvatarPostureValues intermediatePostureValues = retargetingService.RetargetToIntermediate(globalPosture);
//Apply the posture values
this.skeleton.SetChannelData(intermediatePostureValues);
}
/// <summary>
/// Returns the retargeted posture represented in the intermediate skeleton used in the MMI framework
/// </summary>
/// <returns></returns>
public MAvatarPostureValues GetRetargetedPosture()
{
// Return retargeted posture from target Avatar to intermediate skeleton.
MAvatarPosture p = this.GenerateGlobalPosture();
MAvatarPostureValues vals = this.retargetingService.RetargetToIntermediate(p);
if (this.skelVis != null)
{
this.skelVis.AssignPostureValues();
}
return(vals);
}
/// <summary>
/// Computes the new (desired) hand position considering the offset of the collider (to avoid self-collisions)
/// </summary>
/// <param name="targetHandPosition"></param>
/// <param name="currentPosture"></param>
/// <returns></returns>
private MVector3 ComputeNewPositionWithOffset(MVector3 targetHandPosition, MAvatarPostureValues currentPosture)
{
//Optionally ensure that the object does not intersect the avatar
MCollider collider = this.SceneAccess.GetColliderById(this.objectTransform.ID);
//Determine the offset based on the respective collider
float offset = 0;
if (collider.SphereColliderProperties != null)
{
offset = (float)collider.SphereColliderProperties.Radius;
}
if (collider.BoxColliderProperties != null)
{
offset = (float)collider.BoxColliderProperties.Size.Magnitude();
}
if (collider.CapsuleColliderProperties != null)
{
offset = Math.Max((float)collider.CapsuleColliderProperties.Height, (float)collider.CapsuleColliderProperties.Radius);
}
//The offset could be also dynamically determined (using the mesh intersection distance or using Physics Compute Pentration in unity)
this.SkeletonAccess.SetChannelData(currentPosture);
//Get the shoulder positions
MVector3 leftShoulderPosition = this.SkeletonAccess.GetGlobalJointPosition(this.AvatarDescription.AvatarID, MJointType.LeftShoulder);
MVector3 rightShoulderPosition = this.SkeletonAccess.GetGlobalJointPosition(this.AvatarDescription.AvatarID, MJointType.RightShoulder);
//Compute the direction vector pointing from the avatar towards the respective hand
MVector3 dir = new MVector3(0, 0, 0);
switch (this.handJoint)
{
case MJointType.LeftWrist:
dir = leftShoulderPosition.Subtract(rightShoulderPosition).Normalize();
break;
case MJointType.RightWrist:
dir = rightShoulderPosition.Subtract(leftShoulderPosition).Normalize();
break;
}
//Add an offset on top of the position
return(targetHandPosition.Add(dir.Multiply(offset)));
}
public override MBoolResponse AssignInstruction(MInstruction instruction, MSimulationState simulationState)
{
//Setup the ik
this.ServiceAccess.IKService.Setup(this.AvatarDescription, new Dictionary <string, string>());
//Reset all flags/states
this.constraintManager = new ConstraintManager(this.SceneAccess);
this.singleShotIK = false;
this.singleShotIKTargetPosture = null;
//Assign the instruction
this.instruction = instruction;
//Parse the parameters
MBoolResponse result = this.ParseParameters(instruction);
if (!result.Successful)
{
return(result);
}
//Compute the target posture
if (this.singleShotIK)
{
List <MConstraint> tempConstraints = new List <MConstraint>();
constraintManager.SetConstraints(ref tempConstraints);
//Set the ik constraints
constraintManager.SetEndeffectorConstraint(this.handJoint, targetTransform.Position, targetTransform.Rotation);
//Compute the posture
MIKServiceResult ikResult = this.ServiceAccess.IKService.CalculateIKPosture(simulationState.Current, constraintManager.GetJointConstraints(), new Dictionary <string, string>());
this.singleShotIKTargetPosture = ikResult.Posture.Copy();
//Clear the constraints in the constraint manager
tempConstraints.Clear();
}
//Return true/success
return(new MBoolResponse(true));
}
/// <summary>
/// Assigns the pose to the avatar.
/// </summary>
/// <param name="pose"></param>
/// <param name="assignLocalPositions">Specifies whether the local positions are assigned as well</param>
public virtual void AssignPostureValues(MAvatarPostureValues pose, bool assignLocalPositions = true)
{
//Perform a retargeting to the specific skeleton using the retargeting service
MAvatarPosture p = this.retargetingService.RetargetToTarget(pose);
//Update the values of the skeletal representation
skeleton.SetChannelData(pose);
//Compute the root transformation if defined
/*
* if (this.AutoComputeRootTransform)
* {
* MVector3 globalRootBonePosition = skeleton.GetRootPosition(this.AvatarID);
* MQuaternion globalRootBoneRotation = skeleton.GetRootRotation(this.AvatarID);
*
*
* //Compute the root transform
* this.RootTransform.position = new Vector3((float)globalRootBonePosition.X, this.RootTransform.position.y, (float)globalRootBonePosition.Z);
* //this.RootBone.transform.position = globalRootBonePosition.ToVector3();
*
*
* Vector3 currentEulerRoot = this.RootTransform.eulerAngles;
* this.RootTransform.rotation = Quaternion.Euler(currentEulerRoot.x, globalRootBoneRotation.ToQuaternion().eulerAngles.y - 90.0f, currentEulerRoot.z);
* //this.RootBone.transform.rotation = globalRootBoneRotation.ToQuaternion();
*
* MTransform CharTransform = new MTransform("tmp", this.RootTransform.position.ToMVector3(), this.RootTransform.rotation.ToMQuaternion());
* }
*/
// Update root transform
this.RootTransform.transform.position = p.Joints[0].Position.ToVector3();
this.RootTransform.transform.rotation = p.Joints[0].Rotation.ToQuaternion();
//Update the transforms/visualization by applying the global transformations
this.Pelvis.ApplyGlobalJoints(p.Joints);
//Update the skeleton visualization if enabled
if (this.skelVis != null)
{
this.skelVis.root.ApplyPostureValues();
}
}
//Callback which is called for each frame
private void CoSimulator_OnResult(object sender, MSimulationResult e)
{
//Aquire the mutex during the writing of the new results
this.simulationEventMutex.WaitOne();
currentEvents.Events = new List <MSimulationEvent>(e.Events);
currentEvents.FrameNumber = (int)this.coSimulator.FrameNumber;
currentEvents.SimulationTime = this.coSimulator.Time;
MAvatarPostureValues postureValues = e.Posture.Copy();
//Add the data
data.TryAdd(currentEvents.FrameNumber, currentEvents);
this.simulationEventMutex.ReleaseMutex();
//Perform in a new thread
Task.Run(() =>
{
//Handle each event type which is registered
foreach (var entry in callbackClients)
{
//Handle each client
foreach (CoSimulationEventCallbackClient client in entry.Value)
{
if (entry.Key == "OnFrameEnd")
{
client.Access.OnFrameEnd(postureValues);
}
//Check if events available
if (currentEvents.Events != null)
{
var relevantEvents = currentEvents.Events.Where(s => s.Type == entry.Key).ToList();
if (relevantEvents.Count > 0)
{
client.Access.OnEvent(new MCoSimulationEvents(relevantEvents, currentEvents.SimulationTime, currentEvents.FrameNumber));
}
}
}
}
});
}
/// <summary>
/// Sets the root position of an avatar. In addition, this function re-aligns the pelvis
/// and the root joint. Meaning, the root joint is set to the target position and
/// the translation animation of the pelvis is removed.
/// </summary>
/// <param name="avatarId"></param>
/// <param name="position"></param>
public void SetRootPosition(string avatarId, MVector3 position)
{
MAvatarPostureValues values = this.GetCurrentPostureValues(avatarId);
position = position.Subtract(this.hierarchies[avatarId].GetMJoint().Position);
// Set root position
values.PostureData[0] = position.X;
values.PostureData[1] = position.Y;
values.PostureData[2] = position.Z;
// reset pelvis position to be positioned above the root
values.PostureData[7] = 0;
values.PostureData[8] = 0;
values.PostureData[9] = 0;
this.hierarchies[values.AvatarID].SetAvatarPostureValues(values, this.animatedJoints[values.AvatarID]);
// I avoid using the SetChannelData here, to not overwrite the last set channel values.
// this.SetChannelData(values);
}
public void TestRootPosition()
{
MAvatarDescription desc = IntermediateSkeleton.GenerateFromDescriptionFile("TestAvatar");
IntermediateSkeleton skeleton = new IntermediateSkeleton();
skeleton.InitializeAnthropometry(desc);
List <double> rotationValues = new List <double>()
{
0, 0, 0, 0.86041, -0.01303, 0.50383, 0.07533, //S1L5
0.00000, 0.00000, 0.00000, 1.00000, 0.00000, 0.00000, 0.00000, // T12L12
0.00000, 0.00000, 0.00000, 1.00000, 0.00000, 0.00000, 0.00000, // T1T2
0.00000, 0.00000, 0.00000, 1.00000, 0.00000, 0.00000, 0.00000, //C4C5
0.00000, 0.00000, 0.00000, 0.98890, 0.04908, -0.13945, -0.01508, // Head
0.00000, 0.00000, 0.00000, 0.74914, -0.35251, 0.02895, 0.56007, // LeftShoulder
0.98560, 0.11136, -0.00962, 0.12689, // Left ELbow
0.96542, -0.01250, 0.25953, 0.02139, // Left Wrist
0.00000, 0.00000, 0.00000, 0.74411, 0.10420, 0.26279, -0.60530,
0.95158, 0.28073, 0.07735, -0.09850, // Right Elbow
0.99256, -0.00379, 0.11897, -0.02548, // right wrist
0.94999, -0.28306, 0.12805, 0.03154, // Left hip
0.97503, 0.22205, 0.00000, -0.00001, // Knee
0.99439, -0.07404, 0.06580, 0.03709, // Ankle
1.00000, 0.00000, 0.00000, 0.00000, // Toes
0.99694, 0.07053, -0.02371, 0.02406, // Right Hip
0.91716, 0.39852, 0.00000, 0.00000, // Knee
0.99076, -0.12976, 0.02501, 0.03048, // Ankle
1.00000, 0.00000, 0.00000, 0.00000
}; // Toes
MAvatarPostureValues values = new MAvatarPostureValues(desc.AvatarID, rotationValues);
skeleton.SetChannelData(values);
MVector3 pos = skeleton.GetGlobalJointPosition(desc.AvatarID, MJointType.S1L5Joint);
MVector3 gt = skeleton.GetRoot(desc.AvatarID).GetMJoint().Position;
gt = new MVector3(gt.X + rotationValues[0], gt.Y + rotationValues[1], gt.Z + rotationValues[2]);
System.Console.WriteLine("pos: {0}, {1}, {2}", pos.X, pos.Y, pos.Z);
System.Console.WriteLine("gt: {0}, {1}, {2}", gt.X, gt.Y, gt.Z);
Assert.IsTrue(System.Math.Abs(pos.X - gt.X) < 0.001);
Assert.IsTrue(System.Math.Abs(pos.Y - gt.Y) < 0.001);
Assert.IsTrue(System.Math.Abs(pos.Z - gt.Z) < 0.001);
}
/// <summary>
/// Sets the root rotation of an avatar.
/// In this function, the root joint is set to the target rotation and the rotation animation
/// of the pelvis is removed.
///
/// TODO: This has to be improved, as all animation of the pelvis is removed. As the roots forward direction (z-axis)
/// should point to the front of the avatar, only this part of the rotation should be removed from the pelvis.
/// </summary>
/// <param name="avatarId"></param>
/// <param name="rotation"></param>
public void SetRootRotation(string avatarId, MQuaternion rotation)
{
MAvatarPostureValues values = this.GetCurrentPostureValues(avatarId);
// set root rotation
values.PostureData[3] = rotation.W;
values.PostureData[4] = rotation.X;
values.PostureData[5] = rotation.Y;
values.PostureData[6] = rotation.Z;
// reset pelvis rotation to be oriented along with the root.
values.PostureData[10] = 1;
values.PostureData[11] = 0;
values.PostureData[12] = 0;
values.PostureData[13] = 0;
this.hierarchies[values.AvatarID].SetAvatarPostureValues(values, this.animatedJoints[values.AvatarID]);
// I avoid using the SetChannelData here, to not overwrite the last set channel values.
// this.SetChannelData(values);
}
/// <summary>
/// Initializes the anthropometry from a given description.
/// Has to be performed prior to all other interactions with the
/// intermediate skeleton (e.g. query for joint positions, etc.).
/// </summary>
/// <param name="description"></param>
public void InitializeAnthropometry(MAvatarDescription description)
{
this.avatarDescriptions[description.AvatarID] = description;
//update joint offsets
this.hierarchies[description.AvatarID] = RJoint.Initialize(description.ZeroPosture.Joints);
List <double> zero_rotations = new List <double>();
this.hierarchies[description.AvatarID].CreateZeroVector(zero_rotations);
MAvatarPostureValues values = new MAvatarPostureValues(description.AvatarID, zero_rotations);
this.lastPostureValues[description.AvatarID] = values;
List <MJointType> animatedJoints = new List <MJointType>();
foreach (MJoint j in description.ZeroPosture.Joints)
{
animatedJoints.Add(j.Type);
}
this.SetAnimatedJoints(description.AvatarID, animatedJoints);
}
/// <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 GetRootPosition(MAvatarPostureValues posture)
{
this.SkeletonAccess.SetChannelData(posture);
return(this.SkeletonAccess.GetRootPosition(this.avatarDescription.AvatarID));
}
public void SetToZero(string AvatarID)
{
MAvatarPostureValues zero = this.GetZeroPosture(AvatarID);
this.j.SetAvatarPostureValues(zero);
}
/// <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>
/// 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));
}
private MTransform GetTransform(MAvatarPostureValues posture, HandType type)
{
return(GetTransform(posture, type == HandType.Left ? MJointType.LeftWrist : MJointType.RightWrist));
}
private MQuaternion GetGlobalRotation(MAvatarPostureValues posture, HandType type)
{
return(GetGlobalRotation(posture, type == HandType.Left ? MJointType.LeftWrist : MJointType.RightWrist));
}
private MVector3 GetGlobalPosition(MAvatarPostureValues posture, HandType type)
{
return(GetGlobalPosition(posture, type == HandType.Left ? MJointType.LeftWrist : MJointType.RightWrist));
}
/// <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;
}
}
}
//To do
private MAvatarPostureValues AssignHandPose(MAvatarPostureValues posture, MTransform handPose, HandType type)
{
//this.SkeletonAccess.SetChannelData(posture);
return(posture);
}
//To do
private MAvatarPostureValues AssignBoneRotations(MAvatarPostureValues target, MAvatarPostureValues source, List <MJointType> joints)
{
//To do
return(target);
}
