/// <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); }