/// <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);
}
private Obstacle findMostThreateningObstacle(MVector3 position, MVector3 ahead, MVector3 ahead2, List <Obstacle> obstacles)
{
Obstacle mostThreatening = null;
foreach (Obstacle obstacle in obstacles)
{
bool collision = LineIntersectsCircle(ahead, ahead2, obstacle);
// "position" is the character's current position
if (collision && (mostThreatening == null || MVector3Extensions.Distance(position, obstacle.Center) < MVector3Extensions.Distance(position, mostThreatening.Center)))
{
mostThreatening = obstacle;
}
}
return(mostThreatening);
}
/// <summary>
/// Default do step routine
/// </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 ?? new List <MSimulationEvent>(),
Constraints = simulationState.Constraints ?? new List <MConstraint>(),
SceneManipulations = simulationState.SceneManipulations ?? new List <MSceneManipulation>(),
Posture = simulationState.Current
};
//Set the channel data to reflect to current posture
SkeletonAccess.SetChannelData(simulationState.Current);
//Set the default rotation of the head and neck joints
SkeletonAccess.SetLocalJointRotation(AvatarDescription.AvatarID, MJointType.HeadJoint, initialHeadRotation);
SkeletonAccess.SetLocalJointRotation(AvatarDescription.AvatarID, MJointType.C4C5Joint, initialNeckRotation);
//Create a transform representing the current head location
MTransform currentTransform = new MTransform()
{
ID = "",
Position = SkeletonAccess.GetGlobalJointPosition(AvatarDescription.AvatarID, MJointType.HeadJoint),
Rotation = SkeletonAccess.GetGlobalJointRotation(AvatarDescription.AvatarID, MJointType.HeadJoint)
};
//Create a transform representing the parent location (neck)
MTransform parentTransform = new MTransform()
{
ID = "",
Position = SkeletonAccess.GetGlobalJointPosition(AvatarDescription.AvatarID, MJointType.C4C5Joint),
Rotation = SkeletonAccess.GetGlobalJointRotation(AvatarDescription.AvatarID, MJointType.C4C5Joint)
};
//The current head forward vector
MVector3 currentHeadForward = new MVector3(-1, 0, 0);
//Compute the local position of the desired object (relative to the neck)
MVector3 localPosition = parentTransform.InverseTransformPoint(this.gazeTarget.Position);
//Get the xz distance in local space
float distance = new MVector3(localPosition.X, 0, localPosition.Z).Magnitude();
float height = (float)localPosition.Y;
//Compute the current angle
float currentAngle = (float)(Math.Atan(height / distance) * 180 / Math.PI);
//Limit if below lower limit
if (currentAngle < lowerLimit)
{
localPosition.Y = Math.Tan(lowerLimit * Math.PI / 180) * distance;
}
//Limit if above upper angle limit
if (currentAngle > upperLimit)
{
localPosition.Y = Math.Tan(upperLimit * Math.PI / 180) * distance;
}
float maxYAngle = 80f;
//Limit xz position
float yAngle = (float)MVector3Extensions.Angle(currentHeadForward, new MVector3(localPosition.X, 0, localPosition.Z));
if (yAngle > maxYAngle)
{
//The interpolated direction
MVector3 interpolatedDirection = MVector3Extensions.Lerp(currentHeadForward, new MVector3(localPosition.X, 0, localPosition.Z).Normalize(), (maxYAngle / yAngle)).Normalize();
//Perform correction
MVector3 newLocalPositionsXZ = interpolatedDirection.Multiply(distance);
localPosition.X = newLocalPositionsXZ.X;
localPosition.Z = newLocalPositionsXZ.Z;
}
//Compute the desired and current facing direction
MVector3 desiredHeadForward = localPosition.Normalize();
//Estimate the rotation that is required to rotate from the current head direction towards the desired one
MQuaternion deltaRotation = FromToRotation(currentHeadForward, new MVector3(desiredHeadForward.X, -desiredHeadForward.Y, desiredHeadForward.Z));
//Gather the current location rotation
MQuaternion currentLocalRotation = SkeletonAccess.GetLocalJointRotation(AvatarDescription.AvatarID, MJointType.HeadJoint);
//Update the local joint rotation to adjust the facing direction to the desired values
SkeletonAccess.SetLocalJointRotation(AvatarDescription.AvatarID, MJointType.HeadJoint, currentLocalRotation.Multiply(deltaRotation));
//Set the updated postures
result.Posture = SkeletonAccess.RecomputeCurrentPostureValues(AvatarDescription.AvatarID);
//Return the simulation results
return(result);
}
private bool LineIntersectsCircle(MVector3 ahead, MVector3 ahead2, Obstacle obstacle)
{
return(MVector3Extensions.Distance(obstacle.Center, ahead) < obstacle.Radius || MVector3Extensions.Distance(obstacle.Center, ahead2) < obstacle.Radius);
}
public MAvatarPosture RetargetToTarget(MAvatarPostureValues intermediatePostureValues)
{
string id = intermediatePostureValues.AvatarID;
RJoint root = ((RJoint)this.skeleton.GetRoot(id));
root.SetAvatarPostureValues(intermediatePostureValues);
MAvatarPosture targetOut = new MAvatarPosture();
targetOut.AvatarID = id;
targetOut.Joints = new List <MJoint>();
foreach (MJoint j in this.basePostures[id].Joints)
{
MJoint outJ = new MJoint();
outJ.ID = j.ID;
outJ.Type = j.Type;
outJ.Parent = j.Parent;
if (outJ.Type != MJointType.Undefined)
{
RJoint rj = (RJoint)root.GetChild(j.Type);
outJ.Position = (rj).RetargetPositionToTarget();
outJ.Rotation = (rj).RetargetRotationToTarget();
}
else
{
outJ.Position = j.Position;
outJ.Rotation = j.Rotation;
}
targetOut.Joints.Add(outJ);
}
Dictionary <string, string> _children = this.children[id];
for (int i = 0; i < targetOut.Joints.Count; i++)
{
MJoint outJ = targetOut.Joints[i];
if (outJ.Type == MJointType.Undefined)
{
bool setRot = false;
bool setPos = false;
Console.WriteLine("no jointtype " + outJ.ID);
if (i == 0)
{
// find first joint that is mapped
foreach (MJoint j in targetOut.Joints)
{
if (j.Type != MJointType.Undefined)
{
outJ.Position = new MVector3(j.Position.X, 0, j.Position.Z);
//j.Position.X = 0;
//j.Position.Z = 0;
MVector3 forward = outJ.Rotation.Multiply(new MVector3(0, 0, 1));
forward.Y = 0;
forward.Normalize();
MVector3 currentForward = j.Rotation.Multiply(new MVector3(0, 0, 1));
MQuaternion drot = MVector3Extensions.FromToRotation(currentForward, forward);
outJ.Rotation = drot.Multiply(j.Rotation);
//outJ.Rotation = MQuaternionExtensions.Inverse(drot).Multiply(outJ.Rotation);
setPos = true;
setRot = true;
break;
}
}
}
else
{
/*
* This is disabled for now, as it was not working propperly.
*
* if(_children.ContainsKey(outJ.ID) && _children[outJ.ID] != "")
* {
* for(int jID = i+1; jID < targetOut.Joints.Count; jID ++)
* {
* MJoint j = targetOut.Joints[jID];
* if (j.ID == _children[outJ.ID])
* {
*
* MVector3 srcDir = new MVector3(0, 1, 0);//outJ.Rotation.Multiply(new MVector3(0, 1, 0)).Normalize
* MVector3 trgDir = null;
* MQuaternion parentRot = null;
* if(outJ.Parent != null)
* {
* for(int pID = i-1; pID > 0; pID--)
* {
* if(targetOut.Joints[pID].ID == outJ.Parent)
* {
* if(targetOut.Joints[pID].Type != MJointType.Undefined)
* {
* parentRot = targetOut.Joints[pID].Rotation;
* trgDir = MQuaternionExtensions.Inverse(parentRot).Multiply(j.Position.Subtract(outJ.Position).Normalize());
* }
* }
* }
* }
* if(trgDir != null)
* {
* MQuaternion rot = MVector3Extensions.FromToRotation(srcDir, trgDir);
* outJ.Rotation = parentRot.Multiply(rot);
* outJ.Position = null;
* setRot = true;
* break;
*
* }
*
*
* }
* }
* }*/
}
if (!setRot)
{
outJ.Rotation = null;
}
if (!setPos)
{
outJ.Position = null;
}
}
}
return(targetOut);
}
