public MQuaternion RetargetRotationToIS(MQuaternion globalRot)
{
MQuaternion rot = globalRot.Multiply(this.invTargetGBR).Multiply(this.isGBR);
this.globalRot = rot;
return(rot);
}
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);
}
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);
}
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);
}
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();
}
}
/// <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);
}
public static List <MJoint> GetDefaultJointList()
{
List <MJoint> defaultJoints = new List <MJoint>()
{
// 7 joints along the spine (6 animated, 39 channels)
NewMJoint("Root", MJointType.Root, new MVector3(0, 0, 0), identityQ, null, defaultRootChannels),
NewMJoint("PelvisCenter", MJointType.PelvisCentre, new MVector3(0, 0, 0), qRotY90, "Root", defaultRootChannels),
NewMJoint("S1L5Joint", MJointType.S1L5Joint, new MVector3(0, 0.18, 0), identityQ, "PelvisCenter", defaultRootChannels),
NewMJoint("T12L1Joint", MJointType.T12L1Joint, new MVector3(0, 0.15, 0), identityQ, "S1L5Joint", defaultRootChannels), // 0.33 - 0.18
NewMJoint("T1T2Joint", MJointType.T1T2Joint, new MVector3(0, 0.43, 0), identityQ, "T12L1Joint", defaultRootChannels), // 0.76 - 0.33
NewMJoint("C4C5Joint", MJointType.C4C5Joint, new MVector3(0, 0.11, 0), identityQ, "T1T2Joint", defaultRootChannels), // 0.87 - 0.76
NewMJoint("HeadJoint", MJointType.HeadJoint, new MVector3(0, 0.13, 0), qRotYM180, "C4C5Joint", defaultJointChannels), // 1.0 - 0.87
NewMJoint("HeadTip", MJointType.HeadTip, new MVector3(0, 0.16, 0), identityQ, "HeadJoint", zeroChannels),
// Left Arm: 3 joints (3 animated, 15 channels)
NewMJoint("LeftShoulder", MJointType.LeftShoulder, new MVector3(0, 0, -1), qRotXM90.Multiply(qRotYM90), "T1T2Joint", defaultRootChannels),
NewMJoint("LeftElbow", MJointType.LeftElbow, new MVector3(0, 1, 0), qRotY90, "LeftShoulder", defaultJointChannels),
NewMJoint("LeftWrist", MJointType.LeftWrist, new MVector3(0, 1, 0), qRotY90, "LeftElbow", defaultJointChannels),
//left hand: 20 joints (16 animated, 75 channels)
NewMJoint("LeftMiddleProximal", MJointType.LeftMiddleProximal, new MVector3(0, 1, 0), identityQ, "LeftWrist", defaultRootChannels),
NewMJoint("LeftMiddleMeta", MJointType.LeftMiddleMeta, new MVector3(0, 1, 0), identityQ, "LeftMiddleProximal", defaultJointChannels),
NewMJoint("LeftMiddleDistal", MJointType.LeftMiddleDistal, new MVector3(0, 1, 0), identityQ, "LeftMiddleMeta", defaultJointChannels),
NewMJoint("LeftMiddleTip", MJointType.LeftMiddleTip, new MVector3(0, 0.03, 0), identityQ, "LeftMiddleDistal", zeroChannels),
NewMJoint("LeftIndexProximal", MJointType.LeftIndexProximal, new MVector3(0, 1, 0), identityQ, "LeftWrist", defaultRootChannels),
NewMJoint("LeftIndexMeta", MJointType.LeftIndexMeta, new MVector3(0, 1, 0), identityQ, "LeftIndexProximal", defaultJointChannels),
NewMJoint("LeftIndexDistal", MJointType.LeftIndexDistal, new MVector3(0, 1, 0), identityQ, "LeftIndexMeta", defaultJointChannels),
NewMJoint("LeftIndexTip", MJointType.LeftIndexTip, new MVector3(0, 0.03, 0), identityQ, "LeftIndexDistal", zeroChannels),
NewMJoint("LeftRingProximal", MJointType.LeftRingProximal, new MVector3(0, 1, 0), identityQ, "LeftWrist", defaultRootChannels),
NewMJoint("LeftRingMeta", MJointType.LeftRingMeta, new MVector3(0, 1, 0), identityQ, "LeftRingProximal", defaultJointChannels),
NewMJoint("LeftRingDistal", MJointType.LeftRingDistal, new MVector3(0, 1, 0), identityQ, "LeftRingMeta", defaultJointChannels),
NewMJoint("LeftRingTip", MJointType.LeftRingTip, new MVector3(0, 0.03, 0), identityQ, "LeftRingDistal", zeroChannels),
NewMJoint("LeftLittleProximal", MJointType.LeftLittleProximal, new MVector3(0, 1, 0), identityQ, "LeftWrist", defaultRootChannels),
NewMJoint("LeftLittleMeta", MJointType.LeftLittleMeta, new MVector3(0, 1, 0), identityQ, "LeftLittleProximal", defaultJointChannels),
NewMJoint("LeftLittleDistal", MJointType.LeftLittleDistal, new MVector3(0, 1, 0), identityQ, "LeftLittleMeta", defaultJointChannels),
NewMJoint("LeftLittleTip", MJointType.LeftLittleTip, new MVector3(0, 0.03, 0), identityQ, "LeftLittleDistal", zeroChannels),
NewMJoint("LeftThumbMid", MJointType.LeftThumbMid, new MVector3(0, 1, 0), new MQuaternion(-0.3826834559440613, 0, 0, 0.9238795042037964), "LeftWrist", defaultRootChannels),//0.53730, 0, 0, 0.84339
NewMJoint("LeftThumbMeta", MJointType.LeftThumbMeta, new MVector3(0, 1, 0), identityQ, "LeftThumbMid", defaultJointChannels),
NewMJoint("LeftThumbCarpal", MJointType.LeftThumbCarpal, new MVector3(0, 1, 0), identityQ, "LeftThumbMeta", defaultJointChannels),
NewMJoint("LeftThumbTip", MJointType.LeftThumbTip, new MVector3(0, 0.03, 0), identityQ, "LeftThumbCarpal", zeroChannels),
// Right Arm: 3 joints (3 animated, 15 channels)
NewMJoint("RightShoulder", MJointType.RightShoulder, new MVector3(0, 0, 1), qRotX90.Multiply(qRotY90), "T1T2Joint", defaultRootChannels),
NewMJoint("RightElbow", MJointType.RightElbow, new MVector3(0, 1, 0), qRotYM90, "RightShoulder", defaultJointChannels),
NewMJoint("RightWrist", MJointType.RightWrist, new MVector3(0, 1, 0), qRotYM90, "RightElbow", defaultJointChannels),
//left hand: 20 joints (16 animated, 75 channels)
NewMJoint("RightMiddleProximal", MJointType.RightMiddleProximal, new MVector3(0, 1, 0), identityQ, "RightWrist", defaultRootChannels),
NewMJoint("RightMiddleMeta", MJointType.RightMiddleMeta, new MVector3(0, 1, 0), identityQ, "RightMiddleProximal", defaultJointChannels),
NewMJoint("RightMiddleDistal", MJointType.RightMiddleDistal, new MVector3(0, 1, 0), identityQ, "RightMiddleMeta", defaultJointChannels),
NewMJoint("RightMiddleTip", MJointType.RightMiddleTip, new MVector3(0, 0.03, 0), identityQ, "RightMiddleDistal", zeroChannels),
NewMJoint("RightIndexProximal", MJointType.RightIndexProximal, new MVector3(0, 1, 0), identityQ, "RightWrist", defaultRootChannels),
NewMJoint("RightIndexMeta", MJointType.RightIndexMeta, new MVector3(0, 1, 0), identityQ, "RightIndexProximal", defaultJointChannels),
NewMJoint("RightIndexDistal", MJointType.RightIndexDistal, new MVector3(0, 1, 0), identityQ, "RightIndexMeta", defaultJointChannels),
NewMJoint("RightIndexTip", MJointType.RightIndexTip, new MVector3(0, 0.03, 0), identityQ, "RightIndexDistal", zeroChannels),
NewMJoint("RightRingProximal", MJointType.RightRingProximal, new MVector3(0, 1, 0), identityQ, "RightWrist", defaultRootChannels),
NewMJoint("RightRingMeta", MJointType.RightRingMeta, new MVector3(0, 1, 0), identityQ, "RightRingProximal", defaultJointChannels),
NewMJoint("RightRingDistal", MJointType.RightRingDistal, new MVector3(0, 1, 0), identityQ, "RightRingMeta", defaultJointChannels),
NewMJoint("RightRingTip", MJointType.RightRingTip, new MVector3(0, 0.03, 0), identityQ, "RightRingDistal", zeroChannels),
NewMJoint("RightLittleProximal", MJointType.RightLittleProximal, new MVector3(0, 1, 0), identityQ, "RightWrist", defaultRootChannels),
NewMJoint("RightLittleMeta", MJointType.RightLittleMeta, new MVector3(0, 1, 0), identityQ, "RightLittleProximal", defaultJointChannels),
NewMJoint("RightLittleDistal", MJointType.RightLittleDistal, new MVector3(0, 1, 0), identityQ, "RightLittleMeta", defaultJointChannels),
NewMJoint("RightLittleTip", MJointType.RightLittleTip, new MVector3(0, 0.03, 0), identityQ, "RightLittleDistal", zeroChannels),
NewMJoint("RightThumbMid", MJointType.RightThumbMid, new MVector3(0, 1, 0), new MQuaternion(0.3826834559440613, 0, 0, 0.9238795042037964), "RightWrist", defaultRootChannels),//0.53730, 0, 0, 0.84339
NewMJoint("RightThumbMeta", MJointType.RightThumbMeta, new MVector3(0, 1, 0), identityQ, "RightThumbMid", defaultJointChannels),
NewMJoint("RightThumbCarpal", MJointType.RightThumbCarpal, new MVector3(0, 1, 0), identityQ, "RightThumbMeta", defaultJointChannels),
NewMJoint("RightThumbTip", MJointType.RightThumbTip, new MVector3(0, 0.03, 0), identityQ, "RightThumbCarpal", zeroChannels),
// Left leg: 5 joints (4 animated, 16 channels)
NewMJoint("LeftHip", MJointType.LeftHip, new MVector3(0, 0, -1), qRotZ180, "PelvisCenter", defaultJointChannels),
NewMJoint("LeftKnee", MJointType.LeftKnee, new MVector3(0, 1, 0), identityQ, "LeftHip", defaultJointChannels),
NewMJoint("LeftAnkle", MJointType.LeftAnkle, new MVector3(0, 1, 0), new MQuaternion(0, 0, -0.53730, 0.84339), "LeftKnee", defaultJointChannels),
NewMJoint("LeftBall", MJointType.LeftBall, new MVector3(0, 1, 0), new MQuaternion(0, 0, -0.2164396196603775, 0.9762960076332092), "LeftAnkle", defaultJointChannels),
NewMJoint("LeftBallTip", MJointType.LeftBallTip, new MVector3(0, 0.08, 0), identityQ, "LeftBall", zeroChannels),
// Right leg: 5 joints (4 animated, 16 channels)
NewMJoint("RightHip", MJointType.RightHip, new MVector3(0, 0, 1), qRotZ180, "PelvisCenter", defaultJointChannels),
NewMJoint("RightKnee", MJointType.RightKnee, new MVector3(0, 1, 0), identityQ, "RightHip", defaultJointChannels),
NewMJoint("RightAnkle", MJointType.RightAnkle, new MVector3(0, 1, 0), new MQuaternion(0, 0, -0.53730, 0.84339), "RightKnee", defaultJointChannels),
NewMJoint("RightBall", MJointType.RightBall, new MVector3(0, 1, 0), new MQuaternion(0, 0, -0.2164396196603775, 0.9762960076332092), "RightAnkle", defaultJointChannels),
NewMJoint("RightBallTip", MJointType.RightBallTip, new MVector3(0, 0.08, 0), identityQ, "RightBall", zeroChannels)
};
return(defaultJoints);
}
private int SetAvatarPostureValues(MAvatarPostureValues values, int id, List <MJointType> animatedJoints)
{
if (animatedJoints == null || animatedJoints.Contains(this.joint.Type))
{
translation = new MVector3(0, 0, 0);
this.currentRotationValues = new MQuaternion(0, 0, 0, 1);
if (values != null)
{
MVector3 translation = new MVector3(0, 0, 0);
for (int i = 0; i < this.joint.Channels.Count; i++)
{
switch (this.joint.Channels[i])
{
case MChannel.WRotation:
this.currentRotationValues.W = values.PostureData[id];
break;
case MChannel.XRotation:
this.currentRotationValues.X = values.PostureData[id];
break;
case MChannel.YRotation:
this.currentRotationValues.Y = values.PostureData[id];
break;
case MChannel.ZRotation:
this.currentRotationValues.Z = values.PostureData[id];
break;
case MChannel.XOffset:
translation.X = (float)values.PostureData[id];
break;
case MChannel.YOffset:
translation.Y = (float)values.PostureData[id];
break;
case MChannel.ZOffset:
translation.Z = (float)values.PostureData[id];
break;
}
id += 1;
}
this.translation = translation;
}
}
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);
this.globalRot = parentRotation.Multiply(this.joint.Rotation).Multiply(this.currentRotationValues);
foreach (Joint c in this.children)
{
id = c.SetAvatarPostureValues(values, id, animatedJoints);
}
return(id);
}
/// <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);
}
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);
}