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