/// <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());
}
}
}
}
public static MVector3 GetVector3(this MTranslationConstraint tConstraint)
{
return(new MVector3(tConstraint.X(), tConstraint.Y(), tConstraint.Z()));
}
/// <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);
}