private void SetBaseReference(MQuaternion baseGlobalRot, MVector3 baseGlobalPos)
{
this.isMapped = true;
this.targetBP = baseGlobalPos.Clone();
this.targetGBR = baseGlobalRot.Clone();
this.isBP = this.GetGlobalPosition();
this.isGBR = this.GetGlobalRotation();
this.invTargetGBR = MQuaternionExtensions.Inverse(this.targetGBR);
this.invIsGBR = MQuaternionExtensions.Inverse(this.isGBR);
this.isOffset = this.invIsGBR.Multiply(this.targetBP.Subtract(this.isBP));
this.targetOffset = this.invTargetGBR.Multiply(this.isBP.Subtract(this.targetBP));
this.inverseOffsetRotation = MQuaternionExtensions.Inverse(this.GetOffsetRotation());
}
/// <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);
}
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);
}
