public MQuaternion RetargetRotationToIS(MQuaternion globalRot)
{
MQuaternion rot = globalRot.Multiply(this.invTargetGBR).Multiply(this.isGBR);
this.globalRot = rot;
return(rot);
}
/// <summary>
/// Computes the rotation to rotate from one vector to the other
/// </summary>
/// <param name="from">The start direction</param>
/// <param name="to">The desired direction</param>
/// <returns></returns>
private static MQuaternion FromToRotation(MVector3 from, MVector3 to)
{
//Normalize both vectors
from = from.Normalize();
to = to.Normalize();
//Estimate the rotation axis
MVector3 axis = MVector3Extensions.Cross(from, to).Normalize();
//Compute the phi angle
double phi = Math.Acos(MVector3Extensions.Dot(from, to)) / (from.Magnitude() * to.Magnitude());
//Create a new quaternion representing the rotation
MQuaternion result = new MQuaternion()
{
X = Math.Sin(phi / 2) * axis.X,
Y = Math.Sin(phi / 2) * axis.Y,
Z = Math.Sin(phi / 2) * axis.Z,
W = Math.Cos(phi / 2)
};
//Perform is nan check and return identity quaternion
if (double.IsNaN(result.W) || double.IsNaN(result.X) || double.IsNaN(result.Y) || double.IsNaN(result.Z))
{
result = new MQuaternion(0, 0, 0, 1);
}
//Return the estimated rotation
return(result);
}
public MQuaternion RetargetRotationToTarget()
{
MQuaternion globalRot = this.GetGlobalRotation();
MQuaternion rot = globalRot.Multiply(this.invIsGBR).Multiply(this.targetGBR);
return(rot);
}
public MVector3 RetargetPositionToIS(MVector3 globalPos, MQuaternion globalRot)
{
MVector3 pos = globalPos.Add(globalRot.Multiply(this.targetOffset));
this.globalPos = pos;
return(pos);
}
// the following functions can be used to parse MAvatarDescriptions from a bvh-like file. These functions
// are currently not maintained and might be outdated.
#region parsing of MAvatarPosture files
private static MJoint ParseJoint(string[] mos, ref int line_counter, ref List <MJoint> mjointList)
{
// parse lines for current joint
// Todo: Improve parser to consider empty lines and comments
string name = mos[line_counter].Split(' ')[1];
float[] off = parseFloatParameter(mos[line_counter + 2].Split(' '), 3);
MVector3 offset = new MVector3(off[0], off[1], off[2]);
float[] quat = parseFloatParameter(mos[line_counter + 3].Split(' '), 4);
MQuaternion rotation = new MQuaternion(quat[1], quat[2], quat[3], quat[0]);
string[] channels = mos[line_counter + 4].Replace("CHANNELS", "").Split(' ');
List <MChannel> mchannels = MapChannels(channels);
MJoint mjoint = new MJoint(name, MJointTypeMap[name], offset, rotation);
mjoint.Channels = mchannels;
mjointList.Add(mjoint);
line_counter += 5;
while (!mos[line_counter].Contains("}"))
{
MJoint child = ParseJoint(mos, ref line_counter, ref mjointList);
child.Parent = mjoint.ID;
}
line_counter += 1;
return(mjoint);
}
/// <summary>
/// Function taken from:
/// https://gist.github.com/aeroson/043001ca12fe29ee911e
/// </summary>
/// <param name="rotation"></param>
/// <returns></returns>
public static MVector3 ToEulerRad(this MQuaternion rotation)
{
double sqw = rotation.W * rotation.W;
double sqx = rotation.X * rotation.X;
double sqy = rotation.Y * rotation.Y;
double sqz = rotation.Z * rotation.Z;
double unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
double test = rotation.X * rotation.W - rotation.Y * rotation.Z;
MVector3 v = new MVector3();
if (test > 0.4995f * unit)
{ // singularity at north pole
v.Y = 2f * Math.Atan2(rotation.Y, rotation.X);
v.X = Math.PI / 2;
v.Z = 0;
return(NormalizeAngles(v.Multiply(Rad2Deg)));
}
if (test < -0.4995f * unit)
{ // singularity at south pole
v.Y = -2f * Math.Atan2(rotation.Y, rotation.X);
v.X = -Math.PI / 2;
v.Z = 0;
return(NormalizeAngles(v.Multiply(Rad2Deg)));
}
MQuaternion q = new MQuaternion(rotation.W, rotation.Z, rotation.X, rotation.Y);
v.Y = Math.Atan2(2f * q.X * q.W + 2f * q.Y * q.Z, 1 - 2f * (q.Z * q.Z + q.W * q.W)); // Yaw
v.X = Math.Asin(2f * (q.X * q.Z - q.W * q.Y)); // Pitch
v.Z = Math.Atan2(2f * q.X * q.Y + 2f * q.Z * q.W, 1 - 2f * (q.Y * q.Y + q.Z * q.Z)); // Roll
return(NormalizeAngles(v.Multiply(Rad2Deg)));
}
/// <summary>
/// Applies this transform to the other transform.
/// </summary>
/// <param name="transform"></param>
/// <param name="other"></param>
/// <returns></returns>
public static MTransform Multiply(this MTransform transform, MTransform other)
{
MQuaternion q = transform.Rotation.Multiply(other.Rotation);
MVector3 pos = other.Rotation.Multiply(transform.Position).Add(other.Position);
MTransform t = new MTransform(transform.ID, pos, q);
return(t);
}
private static MJoint NewMJoint(string id, MJointType type, MVector3 offset, MQuaternion rotation, string parentID, List <MChannel> channels)
{
MJoint j = new MJoint(id, type, offset, rotation);
j.Parent = parentID;
j.Channels = channels;
return(j);
}
/// <summary>
/// Apply a quaternion rotation on all translation data of this bone. Used to apply the jointOrient.
/// </summary>
/// <param name="quat"></param>
public void RotateTranslation(MQuaternion quat)
{
BaseTranslation = BaseTranslation.rotateBy(quat);
foreach (var list in Translation)
{
for (var i = 0; i < list.Count; i++)
{
list[i] = new Tuple <uint, MVector>(list[i].Item1, list[i].Item2.rotateBy(quat));
}
}
}
public override MBoolResponse Initialize(MAvatarDescription avatarDescription, Dictionary <string, string> properties)
{
base.Initialize(avatarDescription, properties);
//Setuo the skeleton access
this.SkeletonAccess = new IntermediateSkeleton();
this.SkeletonAccess.InitializeAnthropometry(avatarDescription);
//Initial rotations
this.initialHeadRotation = SkeletonAccess.GetLocalJointRotation(AvatarDescription.AvatarID, MJointType.HeadJoint);
this.initialNeckRotation = SkeletonAccess.GetLocalJointRotation(AvatarDescription.AvatarID, MJointType.C4C5Joint);
return(new MBoolResponse(true));
}
/// <summary>
/// Determines whether the constraint is fulflled
/// </summary>
/// <param name="desiredRotation"></param>
/// <param name="currentRotation"></param>
/// <param name="rotationConstraint"></param>
/// <returns></returns>
private bool IsFulfilled(MQuaternion desiredRotation, MQuaternion currentRotation, MRotationConstraint rotationConstraint)
{
//By default return true -> It is assumed that interval is [-inv,+inv]
if (rotationConstraint == null)
{
return(true);
}
MVector3 currentRotationEuler = MQuaternionExtensions.ToEuler(currentRotation);
//Determine the global min and max coordinate
MVector3 min = new MVector3(rotationConstraint.Limits.X.Min, rotationConstraint.Limits.Y.Min, rotationConstraint.Limits.Z.Min).Add(currentRotationEuler);
MVector3 max = new MVector3(rotationConstraint.Limits.X.Max, rotationConstraint.Limits.Y.Max, rotationConstraint.Limits.Z.Max).Add(currentRotationEuler);
return(currentRotationEuler.X >= min.X && currentRotationEuler.Y >= min.Y && currentRotationEuler.Z >= min.Z && currentRotationEuler.X <= max.X && currentRotationEuler.Y <= max.Y && currentRotationEuler.Z <= max.Z);
}
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());
}
public static MSceneObject CreateSceneObject(string name, MVector3 position, MQuaternion rotation, string parent = null)
{
string id = Guid.NewGuid().ToString();
MSceneObject sceneObject = new MSceneObject()
{
ID = id,
Name = name,
Transform = new MTransform(id, position, rotation)
{
Parent = parent
},
Properties = new Dictionary <string, string>(),
};
return(sceneObject);
}
public override MMatrix asMatrix()
{
// Get the current transform matrix
MMatrix m = base.asMatrix();
// Initialize the new matrix we will calculate
MTransformationMatrix tm = new MTransformationMatrix(m);
// Find the current rotation as a quaternion
MQuaternion quat = rotation();
// Convert the rocking value in degrees to radians
DegreeRadianConverter conv = new DegreeRadianConverter();
double newTheta = conv.degreesToRadians(getRockInX());
quat.setToXAxis(newTheta);
// Apply the rocking rotation to the existing rotation
tm.addRotationQuaternion(quat.x, quat.y, quat.z, quat.w, MSpace.Space.kTransform);
// Let Maya know what the matrix should be
return(tm.asMatrixProperty);
}
public MQuaternion GetGlobalRotation()
{
if (this.globalRot == null)
{
MQuaternion parentRotation = new MQuaternion(0, 0, 0, 1);
MVector3 parentPosition = new MVector3(0, 0, 0);
if (this.parentJoint != null)
{
parentRotation = this.parentJoint.globalRot;
parentPosition = this.parentJoint.globalPos;
}
this.globalRot = parentRotation.Multiply(this.joint.Rotation).Multiply(this.currentRotationValues);
}
return(this.globalRot);
}
private MAvatarPostureValues PerformPartialBlend(MJointType bodySide, float weight, MSimulationState simulationState)
{
List <MJointType> toConsider = new List <MJointType>();
switch (bodySide)
{
case MJointType.LeftWrist:
toConsider.Add(MJointType.LeftShoulder);
toConsider.Add(MJointType.LeftElbow);
toConsider.Add(MJointType.LeftWrist);
break;
case MJointType.RightWrist:
toConsider.Add(MJointType.RightShoulder);
toConsider.Add(MJointType.RightElbow);
toConsider.Add(MJointType.RightWrist);
break;
}
Dictionary <MJointType, MQuaternion> rotations = new Dictionary <MJointType, MQuaternion>();
foreach (MJointType jt in toConsider)
{
this.SkeletonAccess.SetChannelData(simulationState.Initial);
MQuaternion rot1 = this.SkeletonAccess.GetLocalJointRotation(this.AvatarDescription.AvatarID, jt);
this.SkeletonAccess.SetChannelData(simulationState.Current);
MQuaternion rot2 = this.SkeletonAccess.GetLocalJointRotation(this.AvatarDescription.AvatarID, jt);
MQuaternion interpolatedRotation = MQuaternionExtensions.Slerp(rot1, rot2, weight);
rotations.Add(jt, interpolatedRotation);
}
this.SkeletonAccess.SetChannelData(simulationState.Current);
foreach (var entry in rotations)
{
this.SkeletonAccess.SetLocalJointRotation(this.AvatarDescription.AvatarID, entry.Key, entry.Value);
}
return(this.SkeletonAccess.RecomputeCurrentPostureValues(this.AvatarDescription.AvatarID));
}
public void Load()
{
for (int i = 0; i < this.Joints.Count; i++)
{
MFnIkJoint ikJoint = new MFnIkJoint();
SKLJoint joint = this.Joints[i];
ikJoint.create();
MDagPath jointDagPath = new MDagPath();
ikJoint.getPath(jointDagPath);
this.JointDagPaths.append(jointDagPath);
ikJoint.set(joint.IsLegacy ? joint.Global : joint.Local);
ikJoint.setName(joint.Name);
}
for (int i = 0; i < this.Joints.Count; i++)
{
SKLJoint joint = this.Joints[i];
if (joint.ParentID == i)
{
MGlobal.displayWarning(string.Format("SKLFile:Load - {0} has invalid Parent ID: {1}", joint.Name, joint.ParentID));
}
else if (joint.ParentID != -1) //Don't need to set up ROOT
{
MFnIkJoint ikParentJoint = new MFnIkJoint(this.JointDagPaths[joint.ParentID]);
MFnIkJoint ikChildJoint = new MFnIkJoint(this.JointDagPaths[i]);
ikParentJoint.addChild(ikChildJoint.objectProperty);
if (this.IsLegacy)
{
MVector position = ikChildJoint.getTranslation(MSpace.Space.kTransform);
MQuaternion rotation = new MQuaternion();
ikChildJoint.getRotation(rotation, MSpace.Space.kWorld);
ikChildJoint.setTranslation(position, MSpace.Space.kWorld);
ikChildJoint.setRotation(rotation, MSpace.Space.kWorld);
}
}
}
}
public MVector3 GetGlobalPosition()
{
if (this.globalPos == null)
{
MQuaternion parentRotation = new MQuaternion(0, 0, 0, 1);
MVector3 parentPosition = new MVector3(0, 0, 0);
if (this.parentJoint != null)
{
parentRotation = this.parentJoint.globalRot;
parentPosition = this.parentJoint.globalPos;
}
MVector3 rotatedOffset = parentRotation.Multiply(this.joint.Position);
MVector3 rotatedTranslation = parentRotation.Multiply(this.joint.Rotation).Multiply(this.translation);
this.globalPos = rotatedTranslation.Add(rotatedOffset).Add(parentPosition);
}
return(this.globalPos);
}
/// <summary>
/// Sets the root rotation of an avatar.
/// In this function, the root joint is set to the target rotation and the rotation animation
/// of the pelvis is removed.
///
/// TODO: This has to be improved, as all animation of the pelvis is removed. As the roots forward direction (z-axis)
/// should point to the front of the avatar, only this part of the rotation should be removed from the pelvis.
/// </summary>
/// <param name="avatarId"></param>
/// <param name="rotation"></param>
public void SetRootRotation(string avatarId, MQuaternion rotation)
{
MAvatarPostureValues values = this.GetCurrentPostureValues(avatarId);
// set root rotation
values.PostureData[3] = rotation.W;
values.PostureData[4] = rotation.X;
values.PostureData[5] = rotation.Y;
values.PostureData[6] = rotation.Z;
// reset pelvis rotation to be oriented along with the root.
values.PostureData[10] = 1;
values.PostureData[11] = 0;
values.PostureData[12] = 0;
values.PostureData[13] = 0;
this.hierarchies[values.AvatarID].SetAvatarPostureValues(values, this.animatedJoints[values.AvatarID]);
// I avoid using the SetChannelData here, to not overwrite the last set channel values.
// this.SetChannelData(values);
}
public void RecomputeLocalTransformations()
{
MQuaternion parentRotation = new MQuaternion(0, 0, 0, 1);
MVector3 parentPosition = new MVector3(0, 0, 0);
if (this.parentJoint != null)
{
parentRotation = this.parentJoint.GetGlobalRotation();
parentPosition = this.parentJoint.GetGlobalPosition();
}
MQuaternion inverseParentRot = MQuaternionExtensions.Inverse(parentRotation);
if (!isMapped && this.parentJoint == null)
{
this.currentRotationValues = new MQuaternion(0, 0, 0, 1);
this.translation = this.globalPos;
}
else if (isMapped)
{
this.currentRotationValues = this.inverseOffsetRotation.Multiply(inverseParentRot).Multiply(this.globalRot);
MVector3 rotatedOffset = parentRotation.Multiply(this.joint.Position);
MVector3 rotatedTranslation = this.globalPos.Subtract(parentPosition).Subtract(rotatedOffset);
this.translation = this.inverseOffsetRotation.Multiply(inverseParentRot).Multiply(rotatedTranslation);
}
else
{
this.currentRotationValues = new MQuaternion(0, 0, 0, 1);
this.translation = new MVector3(0, 0, 0);
this.globalPos = null;
this.globalRot = null;
this.GetGlobalPosition();
this.GetGlobalRotation();
}
foreach (Joint c in this.children)
{
((RJoint)c).RecomputeLocalTransformations();
}
}
public override MMatrix asMatrix(double percent)
{
MPxTransformationMatrix m = new MPxTransformationMatrix(this);
// Apply the percentage to the matrix components
MVector trans = m.translation();
trans *= percent;
m.translateTo(trans);
MPoint rotatePivotTrans = m.rotatePivot();
rotatePivotTrans = rotatePivotTrans * percent;
m.setRotatePivot(rotatePivotTrans);
MPoint scalePivotTrans = new MPoint(m.scalePivotTranslation());
scalePivotTrans = scalePivotTrans * percent;
m.setScalePivotTranslation(new MVector(scalePivotTrans));
// Apply the percentage to the rotate value. Same
// as above + the percentage gets applied
MQuaternion quat = rotation();
DegreeRadianConverter conv = new DegreeRadianConverter();
double newTheta = conv.degreesToRadians(getRockInX());
quat.setToXAxis(newTheta);
m.rotateBy(quat);
MEulerRotation eulRotate = m.eulerRotation();
m.rotateTo(eulRotate.multiply(percent), MSpace.Space.kTransform);
// Apply the percentage to the scale
MVector s = new MVector(scale(MSpace.Space.kTransform));
s.x = 1.0 + (s.x - 1.0) * percent;
s.y = 1.0 + (s.y - 1.0) * percent;
s.z = 1.0 + (s.z - 1.0) * percent;
m.scaleTo(s, MSpace.Space.kTransform);
return(m.asMatrix());
}
/// <summary>
/// Rotates the current transform around the specific point and axis
/// </summary>
/// <param name="center">The rotation center</param>
/// <param name="axis">The rotation axis</param>
/// <param name="angle">The angle to rotate</param>
private static MTransform RotateAround(MTransform transform, MVector3 center, MVector3 axis, float angle)
{
MTransform res = new MTransform()
{
ID = System.Guid.NewGuid().ToString()
};
MVector3 pos = transform.Position;
MQuaternion rot = MQuaternionExtensions.FromEuler(axis.Multiply(angle)); // get the desired rotation
MVector3 dir = pos.Subtract(center); // find current direction relative to center
dir = rot.Multiply(dir); // rotate the direction
res.Position = center.Add(dir); // define new position
MQuaternion myRot = transform.Rotation;
res.Rotation = transform.Rotation.Multiply(MQuaternionExtensions.Inverse(myRot).Multiply(rot).Multiply(myRot));
return(res);
}
private void OrientFix(object sender, RoutedEventArgs e)
{
if (joints == null && joints.Count == 0)
{
return;
}
if (joints.Count == 1)
{
//set for the one
}
MVector lastJointWorldPos = joints[joints.Count - 1].getTranslation(MSpace.Space.kWorld);
MVector firstJointWorldPos = joints[0].getTranslation(MSpace.Space.kWorld);
//string[] lines = System.IO.File.ReadAllLines("D:\temp\testValue.txt");
//float valueX = float.Parse(text_x.Text);
//float valueY = float.Parse(text_y.Text);
//float valueZ = float.Parse(text_z.Text);
//MVector direction = (lastJointWorldPos - firstJointWorldPos).normal;
//MVector yzDirect = direction;
//yzDirect.x = 0;
//yzDirect.normalize();
for (int i = 0; i < joints.Count - 1; i++)
{
//MEulerRotation
//MVector originOrient = joints[i].getOrientation()
MQuaternion mq = new MQuaternion(new MVector(1, 0, 0), lastJointWorldPos - firstJointWorldPos);
MEulerRotation euler = mq.asEulerRotation;
joints[i].setOrientation(mq);
//text_x.Text = euler.x + "";
//text_y.Text = euler.y + "";
//text_z.Text = euler.z + "";
//joints[i].setOrientation(new MEulerRotation(valueX, valueY, valueZ));
}
}
/// <summary>
/// Performs a single handed carry
/// Just sets the object relative to the hand
/// </summary>
/// <param name="result"></param>
/// <param name="hand"></param>
/// <returns></returns>
private List <MIKProperty> SingleHandedCarry(ref MSimulationResult result, double time, HandContainer hand)
{
List <MIKProperty> ikProperties = new List <MIKProperty>();
//Check if a carry target is specified
if (hand.CarryTargetName != null)
{
//Compute the root velocity
double rootVelocity = this.ComputeRootVelocity(time);
//Get the current transform of the carry target for the respective hand
MTransform targetTr = SceneAccess.GetTransformByID(this.CarryTargetName);
//The transform of the carry target
MTransform targetTransform = new MTransform("", targetTr.Position, targetTr.Rotation);
//Compute the global position of the respective hand based on the object
MVector3 targetHandPosition = targetTransform.TransformPoint(hand.HandOffset.Position);
MQuaternion targetHandRotation = targetTransform.TransformRotation(hand.HandOffset.Rotation);
//Get the current hand transform
MTransform currentHandTransform = GetTransform(simulationState.Initial, hand.Type);
//Compute the new hand pose
MTransform nextHandPose = this.DoLocalMotionPlanning(rootVelocity + hand.Velocity, TimeSpan.FromSeconds(time), currentHandTransform.Position, currentHandTransform.Rotation, targetHandPosition, targetHandRotation);
//Compute the object transform
result.SceneManipulations.Add(new MSceneManipulation()
{
Transforms = new List <MTransformManipulation>()
{
new MTransformManipulation()
{
Target = hand.Instruction.Properties["TargetID"],
Position = nextHandPose.TransformPoint(hand.ObjectOffset.Position),
Rotation = nextHandPose.TransformRotation(hand.ObjectOffset.Rotation)
}
}
});
//Set the position and rotation parameters of the ik
this.constraintManager.SetEndeffectorConstraint(hand.JointType, nextHandPose.Position, nextHandPose.Rotation, hand.ConstraintID);
}
//Just set the object relative to the current hand
else
{
//Create a transform representing the hand transform for the planned frame
MTransform handTransform = GetTransform(simulationState.Current, hand.Type);
if (this.UseCarryIK)
{
this.constraintManager.SetEndeffectorConstraint(hand.JointType, handTransform.Position, handTransform.Rotation, hand.ConstraintID);
}
//Compute the object transform
result.SceneManipulations.Add(new MSceneManipulation()
{
Transforms = new List <MTransformManipulation>()
{
new MTransformManipulation()
{
Target = hand.Instruction.Properties["TargetID"],
Position = handTransform.TransformPoint(hand.ObjectOffset.Position),
Rotation = handTransform.TransformRotation(hand.ObjectOffset.Rotation)
}
}
});
}
//To do optionally consider self-collisions
return(ikProperties);
}
public override MBoolResponse AssignInstruction(MInstruction instruction, MSimulationState simulationState)
{
//Reset the properties
this.UseCarryIK = false;
this.bothHandedCarry = false;
this.bothHandedState = CarryState.None;
this.simulationState = simulationState;
//Create a list which contains the hands which should be considered for carrying
List <HandContainer> toPlan = new List <HandContainer>();
if (instruction.Properties == null)
{
throw new Exception($"{this.Name}: No properties defined!");
}
//Extract the hand information
if (instruction.Properties.ContainsKey("Hand"))
{
switch (instruction.Properties["Hand"])
{
case "Left":
toPlan.Add(this.SetupHand(HandType.Left, instruction));
this.bothHandedCarry = false;
break;
case "Right":
toPlan.Add(this.SetupHand(HandType.Right, instruction));
this.bothHandedCarry = false;
break;
case "Both":
//Set flag for both handed carry
this.bothHandedCarry = true;
toPlan.Add(this.SetupHand(HandType.Left, instruction));
toPlan.Add(this.SetupHand(HandType.Right, instruction));
break;
}
}
else
{
toPlan.Add(this.SetupHand(HandType.Right, instruction));
}
//Use the carry target if defined
if (!instruction.Properties.GetValue(out this.CarryTargetName, "CarryTarget"))
{
this.CarryTargetName = null;
}
//Use the carry target if defined
if (!instruction.Properties.GetValue(out this.UseCarryIK, "UseCarryIK"))
{
UseCarryIK = false;
}
//Use carry distance if defined
if (!instruction.Properties.GetValue(out this.carryDistanceBothHanded, "CarryDistance"))
{
carryDistanceBothHanded = 0.65f;
}
//Use carry distance if defined
if (!instruction.Properties.GetValue(out this.carryHeightBothHanded, "CarryHeight"))
{
carryHeightBothHanded = 0.2f;
}
//Use carry distance if defined
if (!instruction.Properties.GetValue(out this.positionObjectVelocity, "Velocity"))
{
this.positionObjectVelocity = 1.0f;
}
//Compute and plan the relevant aspects of each hand
foreach (HandContainer hand in toPlan)
{
//Get the (initial) hand transform
MTransform handTransform = this.GetTransform(simulationState.Initial, hand.Type);
//Get the current transform of the scene object
MTransform sceneObjectTransform = this.SceneAccess.GetTransformByID(hand.Instruction.Properties["TargetID"]);
//Get the hand pose
try
{
hand.HandPose = GetTransform(simulationState.Initial, hand.Type);
}
catch (Exception e)
{
Console.WriteLine("Problem estimating hand pose: " + e.Message + e.StackTrace);
}
//Compute the relative transform of the hand (hand relative to object)
hand.HandOffset = new MTransform("", sceneObjectTransform.InverseTransformPoint(handTransform.Position), sceneObjectTransform.InverseTransformRotation(handTransform.Rotation));
//Compute the inverse offset (object relative to hand)
hand.ObjectOffset = new MTransform("", handTransform.InverseTransformPoint(sceneObjectTransform.Position), handTransform.InverseTransformRotation(sceneObjectTransform.Rotation));
//Set state to positioning
hand.State = CarryState.Positioning;
}
//Do additional computations for both handed carry
if (bothHandedCarry)
{
//Set the state to positioning
this.bothHandedState = CarryState.Positioning;
//Assign the instruction
this.instruction = instruction;
//Get the current object transorm
MTransform currentObjectTransform = this.SceneAccess.GetTransformByID(this.instruction.Properties["TargetID"]);
//Get the current root transform -> to do
MTransform rootTransform = GetTransform(this.simulationState.Initial, MJointType.PelvisCentre);
//Compute the relative object transform
this.relativeObjectRotation = rootTransform.InverseTransformRotation(currentObjectTransform.Rotation);
this.relativeObjectPosition = rootTransform.InverseTransformPoint(currentObjectTransform.Position);
//Manually specify a carry target
if (this.CarryTargetName == null || this.CarryTargetName.Length == 0)
{
MTransform refTransform = GetTransform(this.simulationState.Initial, bothHandedCarryReferenceJoint);
MVector3 forward = GetRootForwad(this.simulationState.Initial);
//Determine the ref transform rotation just consider the y axis rotation
refTransform.Rotation = MQuaternionExtensions.FromEuler(new MVector3(0, Extensions.SignedAngle(new MVector3(0, 0, 1), forward, new MVector3(0, 1, 0)), 0));
//Compute the delta
//MVector3 delta = currentObjectTransform.Position.Subtract(refTransform.Position);
//MVector3 direction = new MVector3(delta.X, 0, delta.Z).Normalize();
//The carry position i
MVector3 carryPosition = refTransform.Position.Add(forward.Multiply(this.carryDistanceBothHanded)).Add(new MVector3(0, carryHeightBothHanded, 0f));
//Forwad + offset
this.internalCarryTransform = new MTransform("CarryTarget", refTransform.InverseTransformPoint(carryPosition), refTransform.InverseTransformRotation(currentObjectTransform.Rotation));
}
}
return(new MBoolResponse(true));
}
/// <summary>
/// Method is responsible for modeling the positiong the object and hands which is the first part of the carry for both handed objects
/// </summary>
/// <param name="result"></param>
/// <param name="time"></param>
private void PositionObjectBothHanded(ref MSimulationResult result, double time)
{
double rootVelocity = this.ComputeRootVelocity(time);
MAvatarPostureValues avatarPose = this.simulationState.Initial;
MTransform currentObjectTransform = this.SceneAccess.GetTransformByID(this.instruction.Properties["TargetID"]);
//Move the object to a central spot in front of the avatar
//Create a new transform for the target object transform
MTransform targetObjectTransform = new MTransform();
if (this.CarryTargetName != null && this.CarryTargetName.Length > 0)
{
MTransform targetTransform = SceneAccess.GetTransformByID(this.CarryTargetName);
targetObjectTransform.Position = targetTransform.Position;
targetObjectTransform.Rotation = targetTransform.Rotation;
}
else
{
MTransform refTransform = GetTransform(this.simulationState.Initial, bothHandedCarryReferenceJoint);
MVector3 forward = GetRootForwad(this.simulationState.Initial);
//Determine the ref transform rotation
refTransform.Rotation = MQuaternionExtensions.FromEuler(new MVector3(0, Extensions.SignedAngle(new MVector3(0, 0, 1), forward, new MVector3(0, 1, 0)), 0));
targetObjectTransform.Position = refTransform.TransformPoint(this.internalCarryTransform.Position);
targetObjectTransform.Rotation = refTransform.TransformRotation(this.internalCarryTransform.Rotation);
}
MTransform nextObjectPose = this.DoLocalMotionPlanning(rootVelocity + positionObjectVelocity, TimeSpan.FromSeconds(time), currentObjectTransform.Position, currentObjectTransform.Rotation, targetObjectTransform.Position, targetObjectTransform.Rotation);
MTransform nextObjectTransform = new MTransform("", nextObjectPose.Position, nextObjectPose.Rotation);
//Update the position of the object
result.SceneManipulations.Add(new MSceneManipulation()
{
Transforms = new List <MTransformManipulation>()
{
new MTransformManipulation()
{
Target = instruction.Properties["TargetID"],
Position = nextObjectPose.Position,
Rotation = nextObjectPose.Rotation
}
}
});
//Update the hands
foreach (HandContainer hand in this.ActiveHands)
{
//Update the hands
MTransform nextHandPose = new MTransform("", nextObjectTransform.TransformPoint(hand.HandOffset.Position), nextObjectTransform.TransformRotation(hand.HandOffset.Rotation));
//Set a new endeffector constraint
this.constraintManager.SetEndeffectorConstraint(hand.JointType, nextHandPose.Position, nextHandPose.Rotation, hand.ConstraintID);
//Assign the hand pose to preserve finger rotations
result.Posture = AssignHandPose(result.Posture, hand.HandPose, hand.Type);
}
//Check if position is finished
if ((targetObjectTransform.Position.Subtract(nextObjectPose.Position)).Magnitude() < 0.01f && MQuaternionExtensions.Angle(targetObjectTransform.Rotation, nextObjectPose.Rotation) < 0.1f)
{
result.Events.Add(new MSimulationEvent("PositioningFinished", "PositioningFinished", instruction.ID));
//Only consider the rotation around y axis
double yRotation = this.GetRootRotation(this.simulationState.Initial).ToEuler().Y;
MTransform rootTransform = new MTransform("", this.GetRootPosition(this.simulationState.Initial), MQuaternionExtensions.FromEuler(new MVector3(0, yRotation, 0)));
//Update the new relative coordinates
this.relativeObjectRotation = rootTransform.InverseTransformRotation(nextObjectTransform.Rotation);
this.relativeObjectPosition = rootTransform.InverseTransformPoint(nextObjectTransform.Position);
this.bothHandedState = CarryState.Carry;
//Get the joint constraints
List <MConstraint> jointConstraints = this.constraintManager.GetJointConstraints();
//Solve using ik if constraints are defined
if (jointConstraints.Count > 0)
{
MIKServiceResult ikResult = this.ServiceAccess.IKService.CalculateIKPosture(result.Posture, jointConstraints, new Dictionary <string, string>());
result.Posture = ikResult.Posture;
}
}
}
/// <summary>
/// Performs a local motion planning to estimate the next pose
/// </summary>
/// <param name="velocity"></param>
/// <param name="time"></param>
/// <param name="currentPosition"></param>
/// <param name="currentRotation"></param>
/// <param name="targetPosition"></param>
/// <param name="targetRotation"></param>
/// <returns></returns>
private MTransform DoLocalMotionPlanning(double velocity, TimeSpan time, MVector3 currentPosition, MQuaternion currentRotation, MVector3 targetPosition, MQuaternion targetRotation)
{
//Create a resulting transform
MTransform result = new MTransform();
//Estimate the delta
MVector3 deltaPosition = targetPosition.Subtract(currentPosition);
//Estimate the meximum allowed delta
double maxTranslationDelta = velocity * time.TotalSeconds;
//Limit the maximum
if (deltaPosition.Magnitude() >= maxTranslationDelta)
{
deltaPosition = deltaPosition.Normalize();
deltaPosition = deltaPosition.Multiply(maxTranslationDelta);
}
float angularVelocityReach = 100f;
double angle = Math.Abs(MQuaternionExtensions.Angle(currentRotation, targetRotation));
double maxAngle = angularVelocityReach * time.TotalSeconds;
//Estimate the blendweihgt for the oreitnation blending
double weight = Math.Min(1, maxAngle / angle);
result.Position = currentPosition.Add(deltaPosition);
result.Rotation = MQuaternionExtensions.Slerp(currentRotation, targetRotation, (float)weight);
//result.Time = time;
return(result);
}
/// <summary>
/// Performs local motion planning to reach the defined point
/// </summary>
/// <param name="velocity"></param>
/// <param name="time"></param>
/// <param name="currentPosition"></param>
/// <param name="currentRotation"></param>
/// <param name="targetPosition"></param>
/// <param name="targetRotation"></param>
/// <returns></returns>
private MTransform DoLocalMotionPlanning(double velocity, double angularVelocity, TimeSpan time, MVector3 currentPosition, MQuaternion currentRotation, MVector3 targetPosition, MQuaternion targetRotation)
{
//Create a new transform representing the result
MTransform result = new MTransform();
//Estimate the vector to reach the goal
MVector3 delta = targetPosition.Subtract(currentPosition);
float distance = delta.Magnitude();
//Determine the angular distance
double angle = Math.Abs(MQuaternionExtensions.Angle(currentRotation, targetRotation));
//Determine the max translation delta and max angle
double maxTranslationDelta = velocity * time.TotalSeconds;
double maxAngle = angularVelocity * time.TotalSeconds;
//Compute the translation weight
float translationWeight = (float)Math.Min(1, maxTranslationDelta / delta.Magnitude());
//Compute the rotation weight
float rotationWeight = (float)Math.Min(1, maxAngle / angle);
//Limit the translation
if (delta.Magnitude() >= maxTranslationDelta)
{
delta = delta.Normalize();
delta = delta.Multiply(maxTranslationDelta);
}
//Compute the new position
result.Position = currentPosition.Add(delta);
if (angularVelocity == 0)
{
result.Rotation = MQuaternionExtensions.Slerp(currentRotation, targetRotation, translationWeight);
}
else
{
result.Rotation = MQuaternionExtensions.Slerp(currentRotation, targetRotation, rotationWeight);
}
return(result);
}
/// <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>(),
Posture = simulationState.Current
};
//Compute the target transform at the beginning of each frame
this.targetTransform = this.ComputeTargetTransform();
//The presently active constraints
List <MConstraint> globalConstraints = result.Constraints;
//The local constraints defined within the MMU
List <MConstraint> localConstraints = new List <MConstraint>();
//Use the constraint manager to manage the local constraints
constraintManager.SetConstraints(ref localConstraints);
//Set the channel data to the approved state of the last frame (all MMUs were executed including the low prio grasp/positioning)
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);
//The next pose
MTransform nextPose = null;
//The current velocity used for path planning
float currentVelocity = this.velocity;// + this.ComputeRootVelocity(time, simulationState);
//Use the trajectory if defined
if (this.trajectory != null)
{
//If a trajectory is used -> The target transform is the last point of the trajectory
this.targetTransform = this.trajectory.Last();
//Compute the next pose
nextPose = this.DoLocalMotionPlanning(currentVelocity, this.angularVelocity, TimeSpan.FromSeconds(time), currentHandPosition, currentHandRotation, this.trajectory[trajectoryIndex].Position, this.trajectory[trajectoryIndex].Rotation);
//Check if close to current target -> move to next target -> To do consider rotation
if ((nextPose.Position.Subtract(trajectory[trajectoryIndex].Position)).Magnitude() < this.translationThreshold && MQuaternionExtensions.Angle(nextPose.Rotation, trajectory[trajectoryIndex].Rotation) < this.rotationThreshold && trajectoryIndex < trajectory.Count - 1)
{
trajectoryIndex++;
}
}
else
{
//Compute the next pose
nextPose = this.DoLocalMotionPlanning(currentVelocity, this.angularVelocity, TimeSpan.FromSeconds(time), currentHandPosition, currentHandRotation, this.targetTransform.Position, this.targetTransform.Rotation);
}
//Get the current distance
float currentDistance = (nextPose.Position.Subtract(targetTransform.Position)).Magnitude();
float currentAngularDistance = (float)MQuaternionExtensions.Angle(nextPose.Rotation, targetTransform.Rotation);
//Check if the ik is only computed once and blending is performed subsequently
if (this.singleShotIK)
{
//Estimate the weight for blending
float weight = (float)Math.Min(1, (currentVelocity * time) / currentDistance);
//To check -> Why is a deep copy required?
result.Posture = Blending.PerformBlend((IntermediateSkeleton)this.SkeletonAccess, simulationState.Initial, this.singleShotIKTargetPosture.Copy(), weight, false);
if (weight >= 1 - 1e-3)
{
result.Events.Add(new MSimulationEvent(this.instruction.Name, mmiConstants.MSimulationEvent_End, this.instruction.ID));
constraintManager.SetEndeffectorConstraint(new MJointConstraint(this.handJoint)
{
GeometryConstraint = new MGeometryConstraint()
{
ParentObjectID = "",
ParentToConstraint = new MTransform(System.Guid.NewGuid().ToString(), targetTransform.Position, targetTransform.Rotation)
}
});
}
}
//Default scenario -> IK is computed for each frame
else
{
if (currentDistance <= this.translationThreshold && currentAngularDistance <= this.rotationThreshold)
{
//Set the target
nextPose.Position = targetTransform.Position;
nextPose.Rotation = targetTransform.Rotation;
MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_INFO, "Reach finished");
result.Events.Add(new MSimulationEvent(this.instruction.Name, mmiConstants.MSimulationEvent_End, this.instruction.ID));
}
//Set the desired endeffector constraints
constraintManager.SetEndeffectorConstraint(this.handJoint, nextPose.Position, nextPose.Rotation);
}
//Create a list with the specific constraints for the reach MMU -> Only get the specific ones that must be solved (local constraints)
List <MConstraint> ikConstraints = constraintManager.GetJointConstraints();
//Only solve if at least one constraint is defined
if (ikConstraints.Count > 0)
{
int ikIterations = 1;
MIKServiceResult ikResult = null;
//Use the ik to compute a posture fulfilling the requested constraints
//To do -> Try with different initial postures / compute suitability of the generated posture
for (int i = 0; i < ikIterations; i++)
{
//Compute twice
ikResult = this.ServiceAccess.IKService.CalculateIKPosture(result.Posture, ikConstraints, new Dictionary <string, string>());
result.Posture = ikResult.Posture;
}
}
//Update the constraint manager to operate on the global constraints
constraintManager.SetConstraints(ref globalConstraints);
//Integrate the newly defined constraints in the global ones
constraintManager.Combine(localConstraints);
//Just for better understanding -> Assign the previous constraints + integrated ones to the result (this is not neccessary since the constraint manager is operating on the reference)
result.Constraints = globalConstraints;
//Return the result
return(result);
}
public override MBoolResponse AssignInstruction(MInstruction instruction, MSimulationState simulationState)
{
//Assign the instruction
this.instruction = instruction;
MBoolResponse response = new MBoolResponse(true);
if (instruction.Constraints.Count > 0 && instruction.Constraints[0].GeometryConstraint != null)
{
MSceneObject parent = this.SceneAccess.GetSceneObjectByID(instruction.Constraints[0].GeometryConstraint.ParentObjectID);
if (instruction.Constraints[0].GeometryConstraint.ParentToConstraint != null)
{
MTransform ptc = instruction.Constraints[0].GeometryConstraint.ParentToConstraint;
String gtp = ptc.Parent;
if (gtp != null && this.SceneAccess.GetSceneObjectByID(gtp) != null)
{
// transform parent takes precedent.
this.targetTransform = ptc.LocalToGlobal(this.SceneAccess);
}
else if (parent != null)
{
// parent to constraint has not valid parent, thus the geometry constraint parent is
this.targetTransform = ptc.Multiply(parent.Transform.LocalToGlobal(this.SceneAccess));
}
else
{
this.targetTransform = ptc;
}
}
else
{
MVector3 pos = new MVector3(0, 0, 0);
if (instruction.Constraints[0].GeometryConstraint.TranslationConstraint != null)
{
MTranslationConstraint trlCstr = instruction.Constraints[0].GeometryConstraint.TranslationConstraint;
if (parent != null)
{
pos = parent.Transform.Position.Add(trlCstr.GetVector3());
}
else
{
pos = trlCstr.GetVector3();
}
}
MQuaternion rot = new MQuaternion(0, 0, 0, 1);
if (instruction.Constraints[0].GeometryConstraint.RotationConstraint != null)
{
MRotationConstraint rtCstr = instruction.Constraints[0].GeometryConstraint.RotationConstraint;
if (parent != null)
{
rot = rtCstr.GetQuaternion().Multiply(parent.Transform.Rotation);
}
else
{
rot = rtCstr.GetQuaternion();
}
}
this.targetTransform = new MTransform("", pos, rot);
}
}
else
{
response = new MBoolResponse(false)
{
LogData = new List <string>()
{
"Required target constraint (MGeometryConstraint) not defined"
}
};
}
//Extract the velocity if defined
if (instruction.Properties.ContainsKey("Velocity"))
{
Console.WriteLine("vel: " + instruction.Properties["Velocity"]);
float.TryParse(instruction.Properties["Velocity"], System.Globalization.NumberStyles.Float, System.Globalization.CultureInfo.InvariantCulture, out velocity);
}
else
{
velocity = -1.0f;
}
/*
* //Get the target id
* if (instruction.Properties.ContainsKey("TargetID"))
* this.targetTransform = this.SceneAccess.GetTransformByID(instruction.Properties["TargetID"]);
* //Error id not available
* else
* {
* return new MBoolResponse(false)
* {
* };
* }*/
//Return true/success
return(response);
}
public override void doSolve()
{
MIkHandleGroup handle_group = handleGroup;
if (handle_group == null)
throw new InvalidOperationException("Invalid handle group");
MObject handle = handle_group.handle(0);
MDagPath handlePath = MDagPath.getAPathTo(handle);
MFnIkHandle handleFn = new MFnIkHandle(handlePath);
//Effector
//
MDagPath effectorPath = new MDagPath();
handleFn.getEffector(effectorPath);
MFnIkEffector effectorFn = new MFnIkEffector(effectorPath);
effectorPath.pop();
MFnIkJoint midJoinFn = new MFnIkJoint(effectorPath);
// Start Joint
//
MDagPath startJointPath = new MDagPath();
handleFn.getStartJoint(startJointPath);
MFnIkJoint startJointFn = new MFnIkJoint(startJointPath);
// Preferred angles
//
double [] startJointPrefAngle = new double[3];
double[] midJointPrefAngle = new double[3];
startJointFn.getPreferedAngle(startJointPrefAngle);
midJoinFn.getPreferedAngle(midJointPrefAngle);
// Set to preferred angles
//
startJointFn.setRotation(startJointPrefAngle, startJointFn.rotationOrder);
midJoinFn.setRotation(midJointPrefAngle, midJoinFn.rotationOrder);
MPoint handlePos = handleFn.rotatePivot(MSpace.Space.kWorld);
AwPoint awHandlePos = new AwPoint(handlePos.x, handlePos.y, handlePos.z, handlePos.w);
MPoint effectorPos = effectorFn.rotatePivot(MSpace.Space.kWorld);
AwPoint awEffectorPos = new AwPoint(effectorPos.x, effectorPos.y, effectorPos.z, effectorPos.w);
MPoint midJoinPos = midJoinFn.rotatePivot(MSpace.Space.kWorld);
AwPoint awMidJoinPos = new AwPoint(midJoinPos.x, midJoinPos.y, midJoinPos.z, midJoinPos.w);
MPoint startJointPos = startJointFn.rotatePivot(MSpace.Space.kWorld);
AwPoint awStartJointPos = new AwPoint(startJointPos.x, startJointPos.y, startJointPos.z, startJointPos.w);
AwVector poleVector = poleVectorFromHandle(handlePath);
MMatrix m = handlePath.exclusiveMatrix;
AwMatrix awM = new AwMatrix();
awM.setMatrix(m);
poleVector = poleVector.mulMatrix(awM);
double twistValue = twistFromHandle(handlePath);
AwQuaternion qStart = new AwQuaternion();
AwQuaternion qMid = new AwQuaternion();
solveIK(awStartJointPos, awMidJoinPos, awEffectorPos, awHandlePos,
poleVector, twistValue, qStart, qMid);
MQuaternion mid = new MQuaternion(qMid.x, qMid.y, qMid.z, qMid.w);
MQuaternion start = new MQuaternion(qStart.x, qStart.y, qStart.z, qStart.w);
midJoinFn.rotateBy(mid, MSpace.Space.kWorld);
startJointFn.rotateBy(start, MSpace.Space.kWorld);
return;
}