/// <summary>
/// Performs a local motion planning to estimate the next pose
/// </summary>
/// <param name="velocity"></param>
/// <param name="time"></param>
/// <param name="currentPosition"></param>
/// <param name="currentRotation"></param>
/// <param name="targetPosition"></param>
/// <param name="targetRotation"></param>
/// <returns></returns>
private MTransform DoLocalMotionPlanning(double velocity, TimeSpan time, MVector3 currentPosition, MQuaternion currentRotation, MVector3 targetPosition, MQuaternion targetRotation)
{
//Create a resulting transform
MTransform result = new MTransform();
//Estimate the delta
MVector3 deltaPosition = targetPosition.Subtract(currentPosition);
//Estimate the meximum allowed delta
double maxTranslationDelta = velocity * time.TotalSeconds;
//Limit the maximum
if (deltaPosition.Magnitude() >= maxTranslationDelta)
{
deltaPosition = deltaPosition.Normalize();
deltaPosition = deltaPosition.Multiply(maxTranslationDelta);
}
float angularVelocityReach = 100f;
double angle = Math.Abs(MQuaternionExtensions.Angle(currentRotation, targetRotation));
double maxAngle = angularVelocityReach * time.TotalSeconds;
//Estimate the blendweihgt for the oreitnation blending
double weight = Math.Min(1, maxAngle / angle);
result.Position = currentPosition.Add(deltaPosition);
result.Rotation = MQuaternionExtensions.Slerp(currentRotation, targetRotation, (float)weight);
//result.Time = time;
return(result);
}
/// <summary>
/// Performs the actual motion planning
/// </summary>
/// <param name="velocity"></param>
/// <param name="angularVelocity"></param>
/// <param name="time"></param>
/// <param name="currentPosition"></param>
/// <param name="currentRotation"></param>
/// <param name="targetPosition"></param>
/// <param name="targetRotation"></param>
/// <returns></returns>
private MTransform DoLocalMotionPlanning(float velocity, float angularVelocity, TimeSpan time, MVector3 currentPosition, MQuaternion currentRotation, MVector3 targetPosition, MQuaternion targetRotation)
{
MTransform result = new MTransform();
MVector3 delta = targetPosition.Subtract(currentPosition);
double angle = Math.Abs(MQuaternionExtensions.Angle(currentRotation, targetRotation));
double maxTranslationDelta = velocity * time.TotalSeconds;
if (delta.Magnitude() >= maxTranslationDelta)
{
delta = delta.Normalize();
delta = delta.Multiply(maxTranslationDelta);
}
//To do consider self collision
double maxAngle = angularVelocity * time.TotalSeconds;
if (angle < maxAngle)
{
angle = maxAngle;
}
double weight = Math.Min(1, maxAngle / angle);
result.Position = currentPosition.Add(delta);
result.Rotation = MQuaternionExtensions.Slerp(currentRotation, targetRotation, (float)weight);
//result.Time = time;
return(result);
}
/// <summary>
/// Performs local motion planning to reach the defined point
/// </summary>
/// <param name="velocity"></param>
/// <param name="time"></param>
/// <param name="currentPosition"></param>
/// <param name="currentRotation"></param>
/// <param name="targetPosition"></param>
/// <param name="targetRotation"></param>
/// <returns></returns>
private MTransform DoLocalMotionPlanning(double velocity, double angularVelocity, TimeSpan time, MVector3 currentPosition, MQuaternion currentRotation, MVector3 targetPosition, MQuaternion targetRotation)
{
//Create a new transform representing the result
MTransform result = new MTransform();
//Estimate the vector to reach the goal
MVector3 delta = targetPosition.Subtract(currentPosition);
float distance = delta.Magnitude();
//Determine the angular distance
double angle = Math.Abs(MQuaternionExtensions.Angle(currentRotation, targetRotation));
//Determine the max translation delta and max angle
double maxTranslationDelta = velocity * time.TotalSeconds;
double maxAngle = angularVelocity * time.TotalSeconds;
//Compute the translation weight
float translationWeight = (float)Math.Min(1, maxTranslationDelta / delta.Magnitude());
//Compute the rotation weight
float rotationWeight = (float)Math.Min(1, maxAngle / angle);
//Limit the translation
if (delta.Magnitude() >= maxTranslationDelta)
{
delta = delta.Normalize();
delta = delta.Multiply(maxTranslationDelta);
}
//Compute the new position
result.Position = currentPosition.Add(delta);
if (angularVelocity == 0)
{
result.Rotation = MQuaternionExtensions.Slerp(currentRotation, targetRotation, translationWeight);
}
else
{
result.Rotation = MQuaternionExtensions.Slerp(currentRotation, targetRotation, rotationWeight);
}
return(result);
}
private MAvatarPostureValues PerformPartialBlend(MJointType bodySide, float weight, MSimulationState simulationState)
{
List <MJointType> toConsider = new List <MJointType>();
switch (bodySide)
{
case MJointType.LeftWrist:
toConsider.Add(MJointType.LeftShoulder);
toConsider.Add(MJointType.LeftElbow);
toConsider.Add(MJointType.LeftWrist);
break;
case MJointType.RightWrist:
toConsider.Add(MJointType.RightShoulder);
toConsider.Add(MJointType.RightElbow);
toConsider.Add(MJointType.RightWrist);
break;
}
Dictionary <MJointType, MQuaternion> rotations = new Dictionary <MJointType, MQuaternion>();
foreach (MJointType jt in toConsider)
{
this.SkeletonAccess.SetChannelData(simulationState.Initial);
MQuaternion rot1 = this.SkeletonAccess.GetLocalJointRotation(this.AvatarDescription.AvatarID, jt);
this.SkeletonAccess.SetChannelData(simulationState.Current);
MQuaternion rot2 = this.SkeletonAccess.GetLocalJointRotation(this.AvatarDescription.AvatarID, jt);
MQuaternion interpolatedRotation = MQuaternionExtensions.Slerp(rot1, rot2, weight);
rotations.Add(jt, interpolatedRotation);
}
this.SkeletonAccess.SetChannelData(simulationState.Current);
foreach (var entry in rotations)
{
this.SkeletonAccess.SetLocalJointRotation(this.AvatarDescription.AvatarID, entry.Key, entry.Value);
}
return(this.SkeletonAccess.RecomputeCurrentPostureValues(this.AvatarDescription.AvatarID));
}
/// <summary>
/// Method performs a local motion planning and tries to reach the specified goal position and rotation using the given velocity,angular velocity and time.
/// </summary>
/// <param name="velocity"></param>
/// <param name="angularVelocity"></param>
/// <param name="time"></param>
/// <param name="currentPosition"></param>
/// <param name="currentRotation"></param>
/// <param name="targetPosition"></param>
/// <param name="targetRotation"></param>
/// <returns></returns>
private MTransform DoLocalMotionPlanning(double velocity, double angularVelocity, TimeSpan time, MVector3 currentPosition, MQuaternion currentRotation, MVector3 targetPosition, MQuaternion targetRotation, bool collisionAvoidance)
{
//Create a new transform representing the result
MTransform result = new MTransform();
//Estimate the delta
MVector3 delta = targetPosition.Subtract(currentPosition);
//Determine the current delta angle
double angle = Math.Abs(MQuaternionExtensions.Angle(currentRotation, targetRotation));
//Determine the max translation delta and max angle in the current frame
double maxTranslationDelta = velocity * time.TotalSeconds;
double maxAngle = angularVelocity * time.TotalSeconds;
//Estimate the blend weight for the rotation and position
float rotationWeight = (float)Math.Min(1, maxAngle / angle);
float positionWeight = (float)Math.Min(1, maxTranslationDelta / delta.Magnitude());
//Limit the max translation
if (delta.Magnitude() >= maxTranslationDelta)
{
delta = delta.Normalize();
delta = delta.Multiply(maxTranslationDelta);
}
if (collisionAvoidance)
{
MVector3 collisionAvoidanceForce = this.ComputCollisionAvoidance(currentPosition, delta);
//if (collisionAvoidanceForce.Magnitude() > 0)
// MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_INFO, "Collision avoidance force: " + collisionAvoidanceForce.Magnitude());
//Add the collision avoidance force on top
delta = delta.Add(collisionAvoidanceForce);
//Limit the max translation
if (delta.Magnitude() >= maxTranslationDelta)
{
delta = delta.Normalize();
delta = delta.Multiply(maxTranslationDelta);
}
}
//Compute the new position
result.Position = currentPosition.Add(delta);
//Compute the new rotation by interpolating towards the target rotation
if (angularVelocity > 0)
{
result.Rotation = MQuaternionExtensions.Slerp(currentRotation, targetRotation, rotationWeight);
}
//Use the rotation weight
else
{
result.Rotation = MQuaternionExtensions.Slerp(currentRotation, targetRotation, positionWeight);
}
//Return the simulation result
return(result);
}
public override MSimulationResult DoStep(double time, MSimulationState simulationState)
{
//Create a new simulation result
MSimulationResult result = new MSimulationResult()
{
Events = simulationState.Events ?? new List <MSimulationEvent>(),
Constraints = simulationState.Constraints ?? new List <MConstraint>(),
SceneManipulations = simulationState.SceneManipulations ?? new List <MSceneManipulation>()
};
//Assign the constraints to a temp varilable
List <MConstraint> constraints = result.Constraints;
//Use the constraint manager to manage the constraints
constraintManager.SetConstraints(ref constraints);
//Get the hand position and rotation of the last frame (approved result)
this.SkeletonAccess.SetChannelData(simulationState.Initial);
MVector3 currentHandPosition = this.SkeletonAccess.GetGlobalJointPosition(this.AvatarDescription.AvatarID, this.handJoint);
MQuaternion currentHandRotation = this.SkeletonAccess.GetGlobalJointRotation(this.AvatarDescription.AvatarID, this.handJoint);
//Get the desired hand position (of the underlying motion e.g. idle)
this.SkeletonAccess.SetChannelData(simulationState.Current);
MVector3 targetHandPosition = this.SkeletonAccess.GetGlobalJointPosition(this.AvatarDescription.AvatarID, this.handJoint);
MQuaternion targetHandRotation = this.SkeletonAccess.GetGlobalJointRotation(this.AvatarDescription.AvatarID, this.handJoint);
//Add an offset on top of the position if desired
if (this.addOffset)
{
targetHandPosition = ComputeNewPositionWithOffset(targetHandPosition, simulationState.Current);
}
//Move the hand from the current position to the target position
MVector3 deltaPosition = targetHandPosition.Subtract(currentHandPosition);
//Compute the distance of the hand to the target hand position
float distanceToGoal = deltaPosition.Magnitude();
//Create positioning finished event if not already created and distance below threshold
if (distanceToGoal < this.positioningFinishedThreshold && !this.positioningFinished)
{
result.Events.Add(new MSimulationEvent("PositioningFinished", "PositioningFinished", this.instruction.ID));
this.positioningFinished = true;
}
//Compute the current velocity based on the general max velocity and the velocity of the root motion
float currentVelocity = this.velocity + this.ComputeRootVelocity(time, simulationState);
//Compute the max distance which can be covered within the current frame
float maxDistance = (float)(time * currentVelocity);
//Compute the weight for slerping (weight increases with shrinking distance to target)
float weight = Math.Max(0, 1 - distanceToGoal);
//Create a new transform representing the next hand transform
MTransform newHandTransform = new MTransform("", currentHandPosition.Clone(), currentHandRotation.Clone())
{
//Compute the new hand position (normalize delta position and multiply by max distance)
Position = currentHandPosition.Add(deltaPosition.Normalize().Multiply(Math.Min(deltaPosition.Magnitude(), maxDistance))),
//Just perform an interpolation to gather new hand rotation (weight is determined by the translation distance)
Rotation = MQuaternionExtensions.Slerp(currentHandRotation, targetHandRotation, weight)
};
//Compute the corresponding positon/rotation of the object and
//adjust the transformation of the object which should be moved
result.SceneManipulations.Add(new MSceneManipulation()
{
Transforms = new List <MTransformManipulation>()
{
new MTransformManipulation()
{
Target = this.objectTransform.ID,
//Compute the new global position of the object
Position = newHandTransform.TransformPoint(this.objectPositionOffset),
//Compute the new global rotation of the object
Rotation = newHandTransform.TransformRotation(this.objectRotationOffset)
}
}
});
//Set the desired endeffector constraints
constraintManager.SetEndeffectorConstraint(this.handJoint, newHandTransform.Position, newHandTransform.Rotation);
//Generate a new posture using the ik solver and the specified constraints
MIKServiceResult ikResult = this.ServiceAccess.IKService.CalculateIKPosture(simulationState.Current, constraintManager.GetJointConstraints(), new Dictionary <string, string>());
result.Posture = ikResult.Posture;
//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 = simulationState.Events != null ? simulationState.Events : new List <MSimulationEvent>(),
Constraints = simulationState.Constraints,
SceneManipulations = simulationState.SceneManipulations != null ? simulationState.SceneManipulations : new List <MSceneManipulation>()
};
//Create variables representing the next object position/rotation
MTransform nextObjectTransform = subjectTransform.Clone();
//Use the constraint manager to manage the constraints
List <MConstraint> tmpConstraints = result.Constraints;
//Set the constraints
constraintManager.SetConstraints(ref tmpConstraints);
//Compute the new hand position and rotation
MVector3 deltaPosition = this.targetObjectTransform.Position.Subtract(subjectTransform.Position);
float distanceToGoal = deltaPosition.Magnitude();
//Get the current object position
float maxDistance = (float)time * 1.0f;
//Check the current distance to goal
if (distanceToGoal < 0.01f)
{
result.Events.Add(new MSimulationEvent(this.instruction.Name, mmiConstants.MSimulationEvent_End, this.instruction.ID));
}
else
{
//Compute the new hand position (normalize delta position and multiply by max distance)
nextObjectTransform.Position = this.subjectTransform.Position.Add(deltaPosition.Normalize().Multiply(Math.Min(distanceToGoal, maxDistance)));
//Compute the weight for slerping (weight increases with shrinking distance to target)
float weight = Math.Max(0, 1 - distanceToGoal);
//Just perform an interpolation to gather new hand rotation (weight is determined by the translation distance)
nextObjectTransform.Rotation = MQuaternionExtensions.Slerp(this.subjectTransform.Rotation, this.targetObjectTransform.Rotation, weight);
}
//Adjust the transformation of the object which should be moved
result.SceneManipulations.Add(new MSceneManipulation()
{
Transforms = new List <MTransformManipulation>()
{
new MTransformManipulation()
{
Target = this.subjectTransform.ID,
Position = nextObjectTransform.Position,
Rotation = nextObjectTransform.Rotation
}
}
});
//Get the current hand position in global space
MVector3 globalHandPosition = nextObjectTransform.TransformPoint(this.handPositionOffset);
MQuaternion globalHandRotation = nextObjectTransform.TransformRotation(this.handRotationOffset);
//Set the desired endeffector constraints
constraintManager.SetEndeffectorConstraint(this.handJoint, globalHandPosition, globalHandRotation);
MIKServiceResult ikResult = this.ServiceAccess.IKService.CalculateIKPosture(simulationState.Current, constraintManager.GetJointConstraints(), new Dictionary <string, string>());
//Generate a new posture using the ik solver and the specified constraints
result.Posture = ikResult.Posture;
//Return the result
return(result);
}
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>()
};
//Cast to intermediate skeleton to get all functions
IntermediateSkeleton iS = this.SkeletonAccess as IntermediateSkeleton;
//Assign the approved posture of the last frame (proper finger rotations)
this.SkeletonAccess.SetChannelData(simulationState.Initial);
//First estimate the finger rotations
//------------------------------------------------------------------------------------------------------------------
//Handle each active hand
for (int i = this.ActiveHands.Count - 1; i >= 0; i--)
{
//Get the current hand
HandContainer hand = this.ActiveHands[i];
if (hand.Release)
{
//Use all finger joint if in release mode
foreach (MJointType jointType in fingerJoints)
{
//Get the current rotation of the finger
if (!hand.CurrentFingerRotations.ContainsKey(jointType))
{
hand.CurrentFingerRotations.Add(jointType, iS.GetLocalJointRotation(this.AvatarDescription.AvatarID, jointType));
}
else
{
hand.CurrentFingerRotations[jointType] = iS.GetLocalJointRotation(this.AvatarDescription.AvatarID, jointType);
}
}
}
else
{
//Handle all joint constraints -> Use only the fingers that are constrained
foreach (MJointConstraint joint in hand.FinalPosture.JointConstraints)
{
//Skip if the joint is no finger joint
if (!this.IsFingerJoint(joint.JointType))
{
continue;
}
//Get the current rotation of the finger
if (!hand.CurrentFingerRotations.ContainsKey(joint.JointType))
{
hand.CurrentFingerRotations.Add(joint.JointType, iS.GetLocalJointRotation(this.AvatarDescription.AvatarID, joint.JointType));
}
else
{
hand.CurrentFingerRotations[joint.JointType] = iS.GetLocalJointRotation(this.AvatarDescription.AvatarID, joint.JointType);
}
}
}
}
//Perform the blending
//------------------------------------------------------------------------------------------------------------------
//Assign the approved posture of the last frame (proper finger rotations)
this.SkeletonAccess.SetChannelData(simulationState.Current);
//Handle each active hand
for (int i = this.ActiveHands.Count - 1; i >= 0; i--)
{
//Get the current hand
HandContainer hand = this.ActiveHands[i];
//Flag which indicates whether the fingers are positioned
bool positioned = true;
if (hand.Release)
{
//If in release mode again use all joints being finger joints
foreach (MJointType jointType in fingerJoints)
{
//Get the current rotation of the finger
MQuaternion currentRotation = hand.CurrentFingerRotations[jointType];
//The current rotation is the result of the last frame
MQuaternion desiredRotation = iS.GetLocalJointRotation(this.AvatarDescription.AvatarID, jointType);
//The weight used for blending
double weight = 0;
//The angle between the current rotation and the desired one
double angle = MQuaternionExtensions.Angle(currentRotation, desiredRotation);
//Compute the weight based on the elapsed time if the duration is set
if (hand.HasDuration)
{
weight = hand.Elapsed / hand.Duration;
}
//By default use the angular velocity
else
{
//The max allowed angle in this frame
double maxAngle = time * hand.AngularVelocity;
weight = Math.Min(1, maxAngle / angle);
}
//Compute the new rotation
MQuaternion newRotation = MQuaternionExtensions.Slerp(currentRotation, desiredRotation, (float)weight);
//Set the local joint rotation of the intermediate skeleton
iS.SetLocalJointRotation(this.AvatarDescription.AvatarID, jointType, newRotation);
//Goal criteria
if (angle > 0.1f)
{
positioned = false;
}
}
}
else
{
//Handle all joint constraints
foreach (MJointConstraint joint in hand.FinalPosture.JointConstraints)
{
//Skip if the joint is no finger joint
if (!this.IsFingerJoint(joint.JointType))
{
continue;
}
//Get the current rotation of the finger
MQuaternion currentRotation = hand.CurrentFingerRotations[joint.JointType];
//Get the desired rotation
MQuaternion desiredRotation = joint.GeometryConstraint.ParentToConstraint.Rotation;
//The weight used for blending
double weight = 0;
//The angle between the current rotation and the desired one
double angle = MQuaternionExtensions.Angle(currentRotation, desiredRotation);
//Compute the weight based on the elapsed time if the duration is set
if (hand.HasDuration)
{
weight = hand.Elapsed / hand.Duration;
}
//By default use the angular velocity
else
{
//The max allowed angle in this frame
double maxAngle = time * hand.AngularVelocity;
weight = Math.Min(1, maxAngle / angle);
}
//Compute the new rotation
MQuaternion newRotation = MQuaternionExtensions.Slerp(currentRotation, desiredRotation, (float)weight);
//Set the local joint rotation of the intermediate skeleton
iS.SetLocalJointRotation(this.AvatarDescription.AvatarID, joint.JointType, newRotation);
//Goal criteria
if (angle > 0.1f)
{
positioned = false;
}
}
}
//Provide event if positioned successfully
if (positioned && !hand.Positioned)
{
hand.Positioned = true;
result.Events.Add(new MSimulationEvent()
{
Name = "MoveFingersMMU",
Type = "FingersPositioned",
Reference = hand.Instruction.ID
});
}
//Increment the time
hand.Elapsed += (float)time;
}
//Recompute the posture given the performed changes
result.Posture = iS.RecomputeCurrentPostureValues(this.AvatarDescription.AvatarID);
//Return the simulation result for the given frame
return(result);
}