/// <summary> /// Creates a posture constraint given the UnityHandPose /// </summary> /// <returns></returns> public MPostureConstraint GetPostureConstraint() { MPostureConstraint postureConstraint = new MPostureConstraint() { JointConstraints = new List <MJointConstraint>(), Posture = new MAvatarPostureValues("", new List <double>()) }; foreach (MJoint joint in this.GetPose().Joints) { MJointConstraint jointConstraint = new MJointConstraint(joint.Type) { GeometryConstraint = new MGeometryConstraint() { ParentObjectID = "", ParentToConstraint = new MTransform() { ID = Guid.NewGuid().ToString(), Position = joint.Position, Rotation = joint.Rotation } } }; postureConstraint.JointConstraints.Add(jointConstraint); } return(postureConstraint); }
/// <summary> /// Sets a new endeffector constraint and replaces old ones if available /// </summary> /// <param name="newConstraint"></param> /// <returns></returns> public virtual void SetEndeffectorConstraint(MJointConstraint newConstraint, String id = null) { //Create a new id if null if (id == null) { id = Guid.NewGuid().ToString(); } this.RemoveEndeffectorConstraints(newConstraint.JointType); //Create a new constraint MConstraint constraint = new MConstraint(id) { JointConstraint = newConstraint }; this.constraints.Add(constraint); }
/// <summary> /// Visualizes the constraints /// </summary> /// <param name="constraints"></param> private void ShowConstraints(List <MConstraint> constraints) { for (int i = constraintObjects.Count - 1; i >= 0; i--) { constraintObjects[i].SetActive(false); } int index = 0; foreach (MConstraint constraint in constraints) { if (constraint.JointConstraint != null) { MJointConstraint endeffectorConstraint = constraint.JointConstraint; if (endeffectorConstraint.GeometryConstraint != null) { MTranslationConstraint posConstraint = endeffectorConstraint.GeometryConstraint.TranslationConstraint; GameObject p = null; if (index < constraintObjects.Count) { p = constraintObjects[index]; p.SetActive(true); } else { p = GameObject.CreatePrimitive(PrimitiveType.Sphere); p.GetComponent <Renderer>().material = new Material(Shader.Find("Standard")); p.GetComponent <Renderer>().material.color = Color.red; p.transform.localScale = new Vector3(0.05f, 0.05f, 0.05f); constraintObjects.Add(p); } index++; p.transform.position = new Vector3((float)posConstraint.X(), (float)posConstraint.Y(), (float)posConstraint.Z()); } } } }
/// <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); }
/// <summary> /// Returns all ik constraints which are violated (avove specified threshold) /// </summary> /// <param name="constraints"></param> /// <param name="currentPosture"></param> /// <returns></returns> private List <MConstraint> GetViolatedIKConstraints(List <MConstraint> constraints, MAvatarPostureValues currentPosture) { List <MConstraint> violated = new List <MConstraint>(); if (constraints != null) { // Apply result posture values to the skeleton skeletonAccess.SetChannelData(currentPosture); string avatarID = currentPosture.AvatarID; //Check each joint constraint foreach (MConstraint mconstraint in constraints) { if (mconstraint.JointConstraint != null) { MJointConstraint endeffectorConstraint = mconstraint.JointConstraint; //Skip if no gemometry constraint is defined if (endeffectorConstraint.GeometryConstraint == null) { continue; } double distance = 0f; double angularDistance = 0f; //Default (parent to constraint is set) if (endeffectorConstraint.GeometryConstraint.ParentToConstraint != null) { MVector3 position = endeffectorConstraint.GeometryConstraint.ParentToConstraint.Position; MQuaternion rotation = endeffectorConstraint.GeometryConstraint.ParentToConstraint.Rotation; switch (endeffectorConstraint.JointType) { case MJointType.LeftWrist: distance = MVector3Extensions.Distance(position, this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.LeftWrist)); angularDistance = MQuaternionExtensions.Angle(rotation, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.LeftWrist)); break; case MJointType.RightWrist: distance = MVector3Extensions.Distance(position, this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.RightWrist)); angularDistance = MQuaternionExtensions.Angle(rotation, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.RightWrist)); break; case MJointType.LeftBall: distance = MVector3Extensions.Distance(position, this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.LeftAnkle)); angularDistance = MQuaternionExtensions.Angle(rotation, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.LeftAnkle)); break; case MJointType.RightBall: distance = MVector3Extensions.Distance(position, this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.RightAnkle)); angularDistance = MQuaternionExtensions.Angle(rotation, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.RightAnkle)); break; case MJointType.PelvisCentre: distance = MVector3Extensions.Distance(position, this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.PelvisCentre)); angularDistance = MQuaternionExtensions.Angle(rotation, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.PelvisCentre)); break; } } //Legacy fallback mechanism -> Remove in future else { MTranslationConstraint positionConstraint = endeffectorConstraint.GeometryConstraint.TranslationConstraint; if (endeffectorConstraint.GeometryConstraint.TranslationConstraint != null) { switch (endeffectorConstraint.JointType) { case MJointType.LeftWrist: distance = MVector3Extensions.Distance(new MVector3(positionConstraint.X(), positionConstraint.Y(), positionConstraint.Z()), this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.LeftWrist)); break; case MJointType.RightWrist: distance = MVector3Extensions.Distance(new MVector3(positionConstraint.X(), positionConstraint.Y(), positionConstraint.Z()), this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.RightWrist)); break; case MJointType.LeftBall: distance = MVector3Extensions.Distance(new MVector3(positionConstraint.X(), positionConstraint.Y(), positionConstraint.Z()), this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.LeftAnkle)); break; case MJointType.RightBall: distance = MVector3Extensions.Distance(new MVector3(positionConstraint.X(), positionConstraint.Y(), positionConstraint.Z()), this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.RightAnkle)); break; case MJointType.PelvisCentre: distance = MVector3Extensions.Distance(new MVector3(positionConstraint.X(), positionConstraint.Y(), positionConstraint.Z()), this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.PelvisCentre)); break; } } //Handle the rotation constraint if (endeffectorConstraint.GeometryConstraint.RotationConstraint != null) { MRotationConstraint rotationConstraint = endeffectorConstraint.GeometryConstraint.RotationConstraint; //Compute a quaternion based on the euler angles MQuaternion quaternion = MQuaternionExtensions.FromEuler(new MVector3(rotationConstraint.X(), rotationConstraint.Y(), rotationConstraint.Z())); if (endeffectorConstraint.GeometryConstraint.ParentObjectID == null || endeffectorConstraint.GeometryConstraint.ParentObjectID == "") { switch (endeffectorConstraint.JointType) { case MJointType.LeftWrist: angularDistance = MQuaternionExtensions.Angle(quaternion, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.LeftWrist)); break; case MJointType.RightWrist: angularDistance = MQuaternionExtensions.Angle(quaternion, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.RightWrist)); break; case MJointType.LeftBall: angularDistance = MQuaternionExtensions.Angle(quaternion, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.LeftAnkle)); break; case MJointType.RightBall: angularDistance = MQuaternionExtensions.Angle(quaternion, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.RightAnkle)); break; case MJointType.PelvisCentre: angularDistance = MQuaternionExtensions.Angle(quaternion, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.PelvisCentre)); break; } } } } //Check if solving is required if (distance > this.PositionThreshold || angularDistance > this.RotationThreshold) { violated.Add(mconstraint); } } } } return(violated); }
// Update is called once per frame void LateUpdate() { if (EnableIK) { MMIAvatar avatar = this.GetComponent <MMIAvatar>(); if (NewInterface) { List <MConstraint> constraints = new List <MConstraint>(); if (LeftHandTarget != null) { MJointConstraint leftHandConstraint = new MJointConstraint(MJointType.LeftWrist) { GeometryConstraint = new MGeometryConstraint() { ParentObjectID = "", ParentToConstraint = this.LeftHandTarget.MSceneObject.Transform } }; constraints.Add(new MConstraint(System.Guid.NewGuid().ToString()) { JointConstraint = leftHandConstraint }); } if (RightHandTarget != null) { MJointConstraint rightHandConstraint = new MJointConstraint(MJointType.RightWrist) { GeometryConstraint = new MGeometryConstraint() { ParentObjectID = "", ParentToConstraint = this.RightHandTarget.MSceneObject.Transform } }; constraints.Add(new MConstraint(System.Guid.NewGuid().ToString()) { JointConstraint = rightHandConstraint }); } MIKServiceResult result = avatar.MMUAccess.ServiceAccess.IKService.CalculateIKPosture(avatar.GetRetargetedPosture(), constraints, new Dictionary <string, string>()); avatar.AssignPostureValues(result.Posture); } else { List <MIKProperty> ikProperties = new List <MIKProperty>(); if (LeftHandTarget != null) { ikProperties.Add(new MIKProperty() { OperationType = MIKOperationType.SetPosition, Target = MEndeffectorType.LeftHand, Values = new List <double>() { LeftHandTarget.transform.position.x, LeftHandTarget.transform.position.y, LeftHandTarget.transform.position.z }, Weight = 1 }); ikProperties.Add(new MIKProperty() { OperationType = MIKOperationType.SetRotation, Target = MEndeffectorType.LeftHand, Values = new List <double>() { LeftHandTarget.transform.rotation.x, LeftHandTarget.transform.rotation.y, LeftHandTarget.transform.rotation.z, LeftHandTarget.transform.rotation.w }, Weight = 1 }); } if (RightHandTarget != null) { ikProperties.Add(new MIKProperty() { OperationType = MIKOperationType.SetPosition, Target = MEndeffectorType.RightHand, Values = new List <double>() { RightHandTarget.transform.position.x, RightHandTarget.transform.position.y, RightHandTarget.transform.position.z }, Weight = 1 }); ikProperties.Add(new MIKProperty() { OperationType = MIKOperationType.SetRotation, Target = MEndeffectorType.RightHand, Values = new List <double>() { RightHandTarget.transform.rotation.x, RightHandTarget.transform.rotation.y, RightHandTarget.transform.rotation.z, RightHandTarget.transform.rotation.w }, Weight = 1 }); } MAvatarPostureValues posture = avatar.MMUAccess.ServiceAccess.IKService.ComputeIK(avatar.GetRetargetedPosture(), ikProperties); avatar.AssignPostureValues(posture); } } }