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