/// <summary>
/// Basic do step call
/// </summary>
/// <param name="time"></param>
/// <param name="simulationState"></param>
/// <returns></returns>
public override MSimulationResult DoStep(double time, MSimulationState simulationState)
{
//Transmit the scene (if first frame-> transmit full scene otherwise just deltas)
this.mmuAccess.PushScene(this.transmitFullScene);
//Full transmission only required at first frame
this.transmitFullScene = false;
//Perform the do step of the co-simulation
MSimulationResult result = this.coSimulator.DoStep(time, simulationState);
//Write the events
if (result.Events != null && PrintDebugMessages)
{
foreach (MSimulationEvent ev in result.Events)
{
Console.WriteLine("Event: " + ev.Name + " " + ev.Type + " " + ev.Reference);
}
}
//Check the endcondition
if (this.CheckEndCondition != null && this.CheckEndCondition(result))
{
result.Events.Add(new MSimulationEvent("Finished", mmiConstants.MSimulationEvent_End, this.Instruction.ID));
}
return(result);
}
public override MSimulationResult DoStep(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
};
elapsed += (float)time;
float blendWeight = Math.Min(1, elapsed / blendDuration);
//Perform the actual motion blending
result.Posture = MMICSharp.Common.Tools.Blending.PerformBlend(this.SkeletonAccess as IntermediateSkeleton, simulationState.Initial, simulationState.Current, blendWeight, true);
//Provide end event if finished
if (elapsed >= this.blendDuration)
{
result.Events.Add(new MSimulationEvent("Blend finished", mmiConstants.MSimulationEvent_End, this.instruction.ID));
}
return(result);
}
/// <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>
/// Method to compute a new posture for the next frame
/// </summary>
/// <param name="time"></param>
/// <param name="simulationState"></param>
/// <returns></returns>
public override MSimulationResult DoStep(Double time, MSimulationState simulationState)
{
//Create a result to store all the computed data
MSimulationResult result = new MSimulationResult()
{
//Just forward the present constraints
Constraints = simulationState.Constraints ?? new List <MConstraint>(),
Posture = simulationState.Current,
SceneManipulations = simulationState.SceneManipulations
};
//Execute instructions on main thread
this.ExecuteOnMainThread(() =>
{
this.SkeletonAccess.SetChannelData(simulationState.Current);
this.AssignPostureValues(simulationState.Current);
//this.transform.position = this.SkeletonAccess.GetRootPosition(this.AvatarDescription.AvatarID).ToVector3();
//this.transform.rotation = this.SkeletonAccess.GetRootRotation(this.AvatarDescription.AvatarID).ToQuaternion();
this.animator.Update((float)time);
result.Posture = this.GetRetargetedPosture();
});
return(result);
}
/// <summary>
/// Checks whether solving is required
/// </summary>
/// <param name="result"></param>
/// <param name="timeSpan"></param>
/// <returns></returns>
public bool RequiresSolving(MSimulationResult result, float timeSpan)
{
List <MJointConstraint> jointConstraints = new List <MJointConstraint>();
foreach (MConstraint constraint in result.Constraints)
{
if (constraint.JointConstraint != null)
{
jointConstraints.Add(constraint.JointConstraint);
}
if (constraint.PostureConstraint != null && constraint.PostureConstraint.JointConstraints != null)
{
jointConstraints.AddRange(constraint.PostureConstraint.JointConstraints);
}
}
foreach (MJointConstraint constraint in jointConstraints)
{
//Skip if joint is not considered
if (!this.ConsideredTypes.Contains(constraint.JointType))
{
continue;
}
//To do integrate further checks
}
return(true);
}
/// <summary>
/// Assign the posture after compute frame
/// </summary>
/// <param name="result"></param>
public override void PostComputeFrame(MSimulationResult result)
{
if (!this.ComputeAsync)
{
this.avatar.AssignPostureValues(result.Posture);
}
}
/// <summary>
/// Constructor for a mmu container which is initialized by serialized data
/// </summary>
/// <param name="data"></param>
/// <param name="mmus"></param>
/// <param name="tasks"></param>
public MMUContainer(SerializableMMUContainer data, List <IMotionModelUnitAccess> mmus, List <MotionTask> tasks)
{
this.History = new List <MotionTask>();
if (data.History != null)
{
foreach (string id in data.History)
{
MotionTask task = tasks.Find(s => s.ID == id);
if (task != null)
{
this.History.Add(task);
}
}
}
this.IsActive = data.IsActive;
this.LastResult = data.LastResult;
this.LastResults = data.LastResults;
this.MMU = mmus.Find(s => s.ID == data.MMUID);
this.Priority = data.Priority;
this.CurrentTasks = new List <MotionTask>();
if (tasks != null)
{
foreach (string currentTaskId in data.CurrentTaskIDs)
{
MotionTask match = tasks.Find(s => s.ID == currentTaskId);
this.CurrentTasks.Add(match);
}
}
}
/// <summary>
/// Do step method which computes the actual carry postures
/// </summary>
/// <param name="time"></param>
/// <param name="simulationState"></param>
/// <returns></returns>
public override MSimulationResult DoStep(double time, MSimulationState simulationState)
{
//Set the current avatar state
this.simulationState = 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>()
};
//Special case for both handed carry
if (bothHandedCarry)
{
this.DoStepBothHanded(result, time, simulationState);
}
//Single handed carry ->Perform a single handed do step
else
{
result = this.DoStepSingleHanded(result, time, simulationState);
}
//Return the computed result
return(result);
}
public override MSimulationResult DoStep(double time, MSimulationState simulationState)
{
MSimulationResult result = new MSimulationResult()
{
Constraints = simulationState.Constraints,
Events = simulationState.Events ?? new List <MSimulationEvent>(),
Posture = simulationState.Current,
SceneManipulations = simulationState.SceneManipulations,
DrawingCalls = new List <MDrawingCall>()
};
this.elapsed += TimeSpan.FromSeconds(time);
//Add a drwaing call
result.DrawingCalls.Add(new MDrawingCall(MDrawingCallType.DrawText)
{
Properties = new Dictionary <string, string>()
{
{ "text", this.text + this.elapsed.TotalSeconds + " s" }
}
});
//Create the finished event
if (this.elapsed > this.duration)
{
result.Events.Add(new MSimulationEvent("Finished", mmiConstants.MSimulationEvent_End, this.instruction.ID));
}
return(result);
}
/// <summary>
/// Method responsible for the inspector visualization
/// </summary>
public override void OnInspectorGUI()
{
//Get the cosimulation debugger instance
CoSimulationDebugger debugger = this.target as CoSimulationDebugger;
if (debugger.SourceAvatar == null)
{
GUILayout.Label("Source avatar must be set in order to use the CoSimulationDebugger.");
//Call the base inspector
base.OnInspectorGUI();
return;
}
//Check if the present frame is not null
if (debugger.currentFrame != null)
{
GUILayout.Label("Hierachy:");
EditorGUILayout.LabelField("", GUI.skin.horizontalSlider);
//Draw the initial
this.DrawInitial(debugger.currentHierachyIndex == 0);
int index = 0;
for (int i = 0; i < debugger.currentFrame.Results.Count; i++)
{
MSimulationResult result = debugger.currentFrame.Results[i];
MInstruction instruction = debugger.GetInstructionByID(debugger.currentFrame.Instructions[i]);
DrawHierachyEntry(debugger, result, instruction, index + 1);
index++;
}
GUI.color = Color.blue;
for (int i = 0; i < debugger.currentFrame.CoSimulationSolverResults.Count; i++)
{
MSimulationResult result = debugger.currentFrame.CoSimulationSolverResults[i];
DrawSolverEntry(debugger, result, index + 1);
index++;
}
GUI.color = Color.cyan;
//Draw the merged resuls
this.DrawMergedResult(debugger, debugger.currentFrame.MergedResult, debugger.currentHierachyIndex == -1);
GUI.color = GUI.color = Color.white;
}
//Call the base inspector
base.OnInspectorGUI();
}
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>()
};
List <MConstraint> constraints = new List <MConstraint>();
//Apply ik
if (LeftHandTarget != null)
{
constraints.Add(new MConstraint(System.Guid.NewGuid().ToString())
{
JointConstraint = new MJointConstraint()
{
GeometryConstraint = new MGeometryConstraint("")
{
ParentToConstraint = new MTransform(System.Guid.NewGuid().ToString(), LeftHandTarget.Transform.Position, LeftHandTarget.Transform.Rotation),
WeightingFactor = 1.0f,
},
JointType = MJointType.LeftWrist
}
});
}
if (RightHandTarget != null)
{
constraints.Add(new MConstraint(System.Guid.NewGuid().ToString())
{
JointConstraint = new MJointConstraint()
{
GeometryConstraint = new MGeometryConstraint("")
{
ParentToConstraint = new MTransform(System.Guid.NewGuid().ToString(), RightHandTarget.Transform.Position, RightHandTarget.Transform.Rotation),
WeightingFactor = 1.0f
},
JointType = MJointType.RightWrist
},
});
}
if (constraints.Count > 0)
{
MIKServiceResult ikResult = this.ServiceAccess.IKService.CalculateIKPosture(simulationState.Current, constraints, new Dictionary <string, string>());
result.Posture = ikResult.Posture;
}
return(result);
}
/// <summary>
/// Stores the mmu result within the container
/// </summary>
/// <param name="result"></param>
/// <param name="maxQueueLength"></param>
public void StoreResult(MSimulationResult result, int maxQueueLength = 3)
{
this.LastResult = result;
this.LastResults.Enqueue(result);
while (this.LastResults.Count > maxQueueLength)
{
this.LastResults.Dequeue();
}
}
/// <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 simulation cycle for a single frame
/// </summary>
/// <param name="time"></param>
protected virtual void DoStepAsync(float time)
{
//##################### Scene synchronization of each MMU Access ####################################################
this.PushScene();
//##################### Do the Co Simulation for each Avatar ####################################################
//Create a dictionary which contains the avatar states for each MMU
ConcurrentDictionary <MMIAvatar, MSimulationResult> results = new ConcurrentDictionary <MMIAvatar, MSimulationResult>();
//Compute the frame in parallel manner
foreach (MMIAvatar avatar in this.Avatars)
{
MSimulationResult result = avatar.CoSimulator.DoStep(time, new MSimulationState()
{
Current = avatar.LastPosture,
Initial = avatar.LastPosture
});
//Assign the resulting posture
avatar.LastPosture = result.Posture;
//Execute additional method which can be implemented by child class
this.OnResultComputed(result, avatar);
results.TryAdd(avatar, result);
}
//Increment the frame number
this.frameNumber++;
//####################### Incorporate Results ##################################################
//Perform on main thread
MainThreadDispatcher.Instance.ExecuteBlocking(() =>
{
//Apply the manipulations of the scene
this.ApplySceneUpdate(results);
//Update the physics
this.UpdatePhysics(time);
//Assign the posture of each avatar
foreach (MMIAvatar avatar in this.Avatars)
{
avatar.AssignPostureValues(avatar.LastPosture.Copy());
}
});
}
public override MSimulationResult DoStep(double time, MSimulationState simulationState)
{
//The simulation result which is provided as overall result
MSimulationResult result = new MSimulationResult()
{
Posture = simulationState.Current,
Events = new List <MSimulationEvent>(),
SceneManipulations = new List <MSceneManipulation>()
};
//Handle each active MMU (each instruction coressponds to one MMU)
for (int i = instructions.Count - 1; i >= 0; i--)
{
//Update the simulation state
MSimulationResult localResult = mmuInstances[instructions[i]].DoStep(time, simulationState);
//Update the simulation state
//simulationState.Current = localResult.Posture;
//Just forward the constraints
simulationState.Constraints = localResult.Constraints;
//Write the result
result.Constraints = localResult.Constraints;
result.Posture = localResult.Posture;
//Merge the scene manipulations
result.SceneManipulations?.AddRange(localResult.SceneManipulations);
//Add the events
if (localResult.Events != null && localResult.Events.Count > 0)
{
result.Events.AddRange(localResult.Events);
}
//Merge the drawing calls
result.DrawingCalls?.AddRange(localResult.DrawingCalls);
if (localResult.Events.Exists(s => s.Type == mmiConstants.MSimulationEvent_End && s.Reference == instructions[i].ID))
{
//Remove the respective MMU
mmuInstances.Remove(instructions[i]);
//Remove from the list
instructions.RemoveAt(i);
}
}
return(result);
}
private InstructionWindow GetInstructionWindow(MInstruction instruction)
{
InstructionWindow window = new InstructionWindow()
{
StartIndex = -1,
EndIndex = -1,
Instruction = instruction
};
for (int i = 0; i < this.record.Frames.Count; i++)
{
CoSimulationFrame frame = this.record.Frames[i];
if (frame.Instructions.Contains(instruction.ID))
{
if (window.StartIndex == -1)
{
window.StartIndex = i;
}
if (window.EndIndex == -1 || window.EndIndex < i)
{
window.EndIndex = i;
}
MSimulationResult result = frame.MergedResult;
if (result.Events != null && result.Events.Count > 0)
{
foreach (MSimulationEvent simEvent in result.Events)
{
if (simEvent.Reference == instruction.ID)
{
window.Events.Add(i);
window.RawEvents.Add(simEvent);
break;
}
}
}
}
}
return(window);
}
//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>
/// Performs the actual solving
/// </summary>
/// <param name="currentResult"></param>
/// <param name="mmuResults"></param>
/// <param name="timeSpan"></param>
/// <returns></returns>
public MSimulationResult Solve(MSimulationResult currentResult, List <MSimulationResult> mmuResults, float timeSpan)
{
List <MJointConstraint> jointConstraints = new List <MJointConstraint>();
foreach (MConstraint constraint in currentResult.Constraints)
{
if (constraint.JointConstraint != null)
{
jointConstraints.Add(constraint.JointConstraint);
}
if (constraint.PostureConstraint != null && constraint.PostureConstraint.JointConstraints != null)
{
jointConstraints.AddRange(constraint.PostureConstraint.JointConstraints);
}
}
//Set the channel data
this.skeletonAccess.SetChannelData(currentResult.Posture);
foreach (MJointConstraint constraint in jointConstraints)
{
//Skip if joint is not considered
if (!this.ConsideredTypes.Contains(constraint.JointType))
{
continue;
}
MGeometryConstraint geometryConstraint = constraint.GeometryConstraint;
//To do -> Further check the parent
if (geometryConstraint.ParentToConstraint != null)
{
this.skeletonAccess.SetLocalJointPosition(currentResult.Posture.AvatarID, constraint.JointType, geometryConstraint.ParentToConstraint.Position);
this.skeletonAccess.SetLocalJointRotation(currentResult.Posture.AvatarID, constraint.JointType, geometryConstraint.ParentToConstraint.Rotation);
}
}
currentResult.Posture = this.skeletonAccess.RecomputeCurrentPostureValues(currentResult.Posture.AvatarID);
return(currentResult);
}
/// <summary>
/// Performs the do step realted computations for both handed mode
/// </summary>
/// <param name="result"></param>
/// <param name="time"></param>
/// <param name="simulationState"></param>
/// <returns></returns>
private MSimulationResult DoStepBothHanded(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);
switch (this.bothHandedState)
{
//First position the object in order to carry it properly
case CarryState.Positioning:
//Perform both handed positioning
this.PositionObjectBothHanded(ref result, time);
//Solve using ik if constraints are defined
if (this.constraintManager.GetJointConstraints().Count > 0)
{
MIKServiceResult ikResult = this.ServiceAccess.IKService.CalculateIKPosture(result.Posture, this.constraintManager.GetJointConstraints(), new Dictionary <string, string>());
result.Posture = ikResult.Posture;
}
break;
case CarryState.Carry:
//Perform a both handed carry (just preserve the relative position and rotation)
this.BothHandedCarry(ref result);
//Solve using ik if constraints are defined
if (this.constraintManager.GetJointConstraints().Count > 0)
{
MIKServiceResult ikResult = this.ServiceAccess.IKService.CalculateIKPosture(result.Posture, this.constraintManager.GetJointConstraints(), new Dictionary <string, string>());
result.Posture = ikResult.Posture;
}
break;
}
return(result);
}
/// <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>
/// Basic do step call
/// </summary>
/// <param name="time"></param>
/// <param name="simulationState"></param>
/// <returns></returns>
public override MSimulationResult DoStep(double time, MSimulationState simulationState)
{
//Transmit the scene (if first frame-> transmit full scene otherwise just deltas)
this.mmuAccess.PushScene(this.transmitFullScene);
//Full transmission only required at first frame
this.transmitFullScene = false;
//Perform the do step of the co-simulation
MSimulationResult result = this.coSimulator.DoStep(time, simulationState);
if (result.Events != null)
{
foreach (MSimulationEvent ev in result.Events)
{
Console.WriteLine("Event: " + ev.Name + " " + ev.Type + " " + ev.Reference);
}
}
return(result);
}
public SerializableMMUContainer(MMUContainer container)
{
if (container.CurrentTasks != null)
{
foreach (MotionTask task in container.CurrentTasks)
{
this.CurrentTaskIDs.Add(task.ID);
}
}
this.ID = container.ID;
if (container.History != null)
{
this.History = container.History.Select(s => s.ID).ToList();
}
this.IsActive = container.IsActive;
this.LastResult = container.LastResult;
this.LastResults = container.LastResults;
this.MMUID = container.MMU.ID;
this.Priority = container.Priority;
}
public override MSimulationResult DoStep(double time, MSimulationState avatarState)
{
//Call the remote cosimulation
MSimulationResult result = this.remoteCoSimulationMMU.DoStep(time, avatarState);
//Fire events
if (result != null && result.Events != null && result.Events.Count > 0)
{
foreach (MSimulationEvent simEvent in result.Events)
{
this.MSimulationEventHandler?.Invoke(this, simEvent);
}
}
try
{
this.avatar.AssignPostureValues(result.Posture);
}
catch (Exception)
{
Debug.LogError("Problem assigning posture using remote co-simulation");
}
return(result);
}
/// <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);
}
}
/// <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>
/// 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>
/// 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>
/// 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>()
};
this.SkeletonAccess.SetChannelData(simulationState.Current.Copy());
// Target position we want to transform to
MVector3 targetPos = this.targetTransform.Position;
// Fully body posture. Only Pelvis Center (first joint) has to be manipulated.
List <double> posture = simulationState.Current.PostureData;
// Current position and distance to target.
MVector3 currentPos = this.SkeletonAccess.GetRootPosition(simulationState.Initial.AvatarID);
MVector3 distance = targetPos.Subtract(currentPos);
// Current rotation and rotational diff to target
MQuaternion currentRot = this.SkeletonAccess.GetRootRotation(simulationState.Initial.AvatarID);
MQuaternion targetRot = this.targetTransform.Rotation;
MVector3 newPos;
MVector3 deltaDistance = distance.Clone();
if (this.velocity > 0)
{
deltaDistance = distance.Normalize().Multiply(this.velocity * time);
Console.WriteLine("Delta v: " + deltaDistance.Magnitude() + " " + time + " " + this.velocity);
}
// If no velocity set or distance very close, directly morph to target position and rotation.
if (this.velocity <= 0 || distance.Magnitude() < deltaDistance.Magnitude())
{
newPos = targetPos;
// Set rotation
//posture[3] = this.targetTransform.Rotation.W;
//posture[4] = this.targetTransform.Rotation.X;
//posture[5] = this.targetTransform.Rotation.Y;
//posture[6] = this.targetTransform.Rotation.Z;
// Add end event.
Console.WriteLine("Finished with vel " + this.velocity + " at " + distance.Magnitude());
result.Events.Add(new MSimulationEvent(this.instruction.Name, mmiConstants.MSimulationEvent_End, this.instruction.ID));
}
else // if velocity > 0 and distance sufficiently large, we should apply linear translation with the provided velocity.
{
newPos = currentPos.Add(deltaDistance);
Console.WriteLine("Target Location: " + this.targetTransform.Position + " " + currentPos + " " + distance + " " + deltaDistance + " " + newPos);
}
Console.WriteLine("newposrot: " + newPos + " " + targetRot);
this.SkeletonAccess.SetRootPosition(simulationState.Current.AvatarID, newPos);
this.SkeletonAccess.SetRootRotation(simulationState.Current.AvatarID, targetRot);
result.Posture = this.SkeletonAccess.GetCurrentPostureValues(simulationState.Current.AvatarID);
Console.WriteLine("Frame : " + result.Posture.PostureData[0] + " " + result.Posture.PostureData[1] + " " + result.Posture.PostureData[2] + " " + result.Posture.PostureData[3] + " " + result.Posture.PostureData[4] + " " + result.Posture.PostureData[5] + " " + result.Posture.PostureData[6] + " ");
//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);
}
