| public static GetRotationBetween ( OpenTK.Vector3 a, OpenTK.Vector3 b, float stiffness = 1f ) : Quaternion | ||
| a | OpenTK.Vector3 | The first vector |
| b | OpenTK.Vector3 | The secound vector |
| stiffness | float | Stiffness value |
| return | Quaternion | |
/// <summary>
/// Draws the twist constraints accoriding in Unity using Giszmos
/// </summary>
/// <param name="b">The bone with its constraints</param>
/// <param name="refBone">The to be twisted against</param>
/// <param name="poss">The position of where it should be drawn</param>
/// <param name="scale">The scale of the constraints</param>
public static void DrawTwistConstraints(Bone b, Bone refBone, OpenTK.Vector3 poss, float scale)
{
if (b.Orientation.Xyz.IsNaN() || refBone.Orientation.Xyz.IsNaN())
{
return;
}
OpenTK.Vector3 thisY = b.GetYAxis();
OpenTK.Quaternion referenceRotation = refBone.Orientation * b.ParentPointer;
OpenTK.Vector3 parentY = OpenTK.Vector3.Transform(OpenTK.Vector3.UnitY, referenceRotation);
OpenTK.Vector3 parentZ = OpenTK.Vector3.Transform(OpenTK.Vector3.UnitZ, referenceRotation);
OpenTK.Quaternion rot = QuaternionHelper2.GetRotationBetween(parentY, thisY);
OpenTK.Vector3 reference = OpenTK.Vector3.Transform(parentZ, rot);
reference.Normalize();
Debug.DrawRay(poss.Convert(), (b.GetZAxis() * scale * 2).Convert(), Color.cyan);
float startTwistLimit = OpenTK.MathHelper.DegreesToRadians(b.StartTwistLimit);
OpenTK.Vector3 m = OpenTK.Vector3.Transform(reference, OpenTK.Quaternion.FromAxisAngle(thisY, startTwistLimit));
m.Normalize();
Debug.DrawRay(poss.Convert(), m.Convert() * scale, Color.yellow);
float endTwistLimit = OpenTK.MathHelper.DegreesToRadians(b.EndTwistLimit);
OpenTK.Vector3 m2 = OpenTK.Vector3.Transform(reference, OpenTK.Quaternion.FromAxisAngle(thisY, endTwistLimit));
m2.Normalize();
Debug.DrawRay(poss.Convert(), m2.Convert() * scale, Color.magenta);
Debug.DrawLine((poss + (m * scale)).Convert(), (poss + (m2 * scale)).Convert(), Color.cyan);
}
// An orientational constraint is the twist of the bone around its own direction vector
// with respect to its parent
// It is defined as an allowed range betwen angles [start,end]
// where start != end && 0 < start, end <= 360
// If both start and end is 0 no twist constraint exist
/// <summary>
/// Checks if the bone has a legal rotation in regards to its parent, returns true if legal, false otherwise.
/// The out rotation gives the rotation the bone should be applied to to be inside the twist constraints
/// </summary>
/// <param name="b">The bone under consideration</param>
/// <param name="refBone">The parent of the bone, to check whether the child has legal rotation</param>
/// <param name="rotation">The rotation bone b should applie to be inside the constraints</param>
/// <returns></returns>
public bool CheckOrientationalConstraint(Bone b, Bone refBone, out Quaternion rotation)
{
if (b.Orientation.Xyz.IsNaN() || refBone.Orientation.Xyz.IsNaN())
{
rotation = Quaternion.Identity;
return(false);
}
Vector3 thisY = b.GetYAxis();
Quaternion referenceRotation = refBone.Orientation * b.ParentPointer;
Vector3 reference = Vector3.Transform(
Vector3.Transform(Vector3.UnitZ, referenceRotation),
QuaternionHelper2.GetRotationBetween(
Vector3.Transform(Vector3.UnitY, referenceRotation),
thisY));
float twistAngle = MathHelper.RadiansToDegrees(Vector3.CalculateAngle(reference, b.GetZAxis()));
if (Vector3.CalculateAngle(reference, b.GetXAxis()) > Mathf.PI / 2) // b is twisted left with respect to parent
{
twistAngle = 360 - twistAngle;
}
float leftLimit = b.StartTwistLimit;
float rightLimit = b.EndTwistLimit;
float precision = 0.5f;
bool inside = (leftLimit >= rightLimit) ? // The allowed range is on both sides of the reference vector
inside = twistAngle - leftLimit >= precision || twistAngle - rightLimit <= precision :
inside = twistAngle - leftLimit >= precision && twistAngle - rightLimit <= precision;
if (!inside) // not inside constraints
{
// Create a rotation to the closest limit
float toLeft = Math.Min(360 - Math.Abs(twistAngle - leftLimit), Math.Abs(twistAngle - leftLimit));
float toRight = Math.Min(360 - Math.Abs(twistAngle - rightLimit), Math.Abs(twistAngle - rightLimit));
if (toLeft < toRight)
{
// Anti-clockwise rotation to left limit
rotation = Quaternion.FromAxisAngle(thisY, -MathHelper.DegreesToRadians(toLeft));
return(true);
}
else
{
// Clockwise to right limit
rotation = Quaternion.FromAxisAngle(thisY, MathHelper.DegreesToRadians(toRight));
return(true);
}
}
rotation = Quaternion.Identity;
return(false);
}
/// <summary>
/// If any of the hip markers are missing, we predict them using the last position
/// </summary>
/// <param name="markers">The dictionary of markers</param>
private void MissingEssentialMarkers(Dictionary <string, Vector3> markers)
{
Vector3 dirVec1, dirVec2, possiblePos1, possiblePos2,
sacrumLastFrame = lastSACRUMknown,
liasLastFrame = lastLIASknown,
riasLastFrame = lastRIASknown;
Vector3 Sacrum = markers[m.bodyBase],
RIAS = markers[m.rightHip],
LIAS = markers[m.leftHip];
bool s = !Sacrum.IsNaN(),
r = !RIAS.IsNaN(),
l = !LIAS.IsNaN();
if (s) // sacrum exists
{
if (r) // sacrum and rias exist, lias missing
{
dirVec1 = liasLastFrame - sacrumLastFrame; // vector from sacrum too lias in last frame
dirVec2 = liasLastFrame - riasLastFrame;
Quaternion between = Quaternion.Invert(
QuaternionHelper2.GetRotationBetween(
(RIAS - Sacrum), (riasLastFrame - sacrumLastFrame))
);
Vector3 transVec1 = Vector3.Transform(dirVec1, (between));
Vector3 transVec2 = Vector3.Transform(dirVec2, (between));
possiblePos1 = Sacrum + transVec1; // add vector from sacrum to lias last frame to this frames sacrum
possiblePos2 = RIAS + transVec2;
markers[m.leftHip] = DidntMovedToMuch(markersLastFrame[m.leftHip], Vector3Helper.MidPoint(possiblePos1, possiblePos2)); // get mid point of possible positions
}
else if (l) // sacrum and lias exists, rias missing
{
dirVec1 = riasLastFrame - sacrumLastFrame;
dirVec2 = riasLastFrame - liasLastFrame;
Quaternion between = Quaternion.Invert(
QuaternionHelper2.GetRotationBetween(
(LIAS - Sacrum), (liasLastFrame - sacrumLastFrame))
);
Vector3 transVec1 = Vector3.Transform(dirVec1, (between));
Vector3 transVec2 = Vector3.Transform(dirVec2, (between));
possiblePos1 = Sacrum + transVec1;
possiblePos2 = LIAS + transVec2;
markers[m.rightHip] = DidntMovedToMuch(markersLastFrame[m.rightHip], Vector3Helper.MidPoint(possiblePos1, possiblePos2));
}
else // only sacrum exists, lias and rias missing
{
markers[m.rightHip] = DidntMovedToMuch(markersLastFrame[m.rightHip], Sacrum + riasLastFrame - sacrumLastFrame);
markers[m.leftHip] = DidntMovedToMuch(markersLastFrame[m.leftHip], Sacrum + liasLastFrame - sacrumLastFrame);
}
}
else if (r) // rias exists, sacrum missing
{
if (l) // rias and ias exists, sacrum missing
{
dirVec1 = sacrumLastFrame - riasLastFrame;
dirVec2 = sacrumLastFrame - liasLastFrame;
Quaternion between = Quaternion.Invert(
QuaternionHelper2.GetRotationBetween(
(LIAS - RIAS), (liasLastFrame - riasLastFrame))
);
Vector3 transVec1 = Vector3.Transform(dirVec1, (between));
Vector3 transVec2 = Vector3.Transform(dirVec2, (between));
possiblePos1 = RIAS + transVec1;
possiblePos2 = LIAS + transVec2;
markers[m.bodyBase] = DidntMovedToMuch(markersLastFrame[m.bodyBase], Vector3Helper.MidPoint(possiblePos1, possiblePos2));
}
else // only rias exists, lias and sacrum missing
{
markers[m.bodyBase] = DidntMovedToMuch(markersLastFrame[m.bodyBase], RIAS + sacrumLastFrame - riasLastFrame);
markers[m.leftHip] = DidntMovedToMuch(markersLastFrame[m.leftHip], RIAS + liasLastFrame - riasLastFrame);
}
}
else if (l) // only lias exists, rias and sacrum missing
{
markers[m.bodyBase] = DidntMovedToMuch(markersLastFrame[m.bodyBase], LIAS + sacrumLastFrame - liasLastFrame);
markers[m.rightHip] = DidntMovedToMuch(markersLastFrame[m.rightHip], LIAS + riasLastFrame - liasLastFrame);
}
else // all markers missing
{
markers[m.bodyBase] = markersLastFrame[m.bodyBase];
markers[m.rightHip] = markersLastFrame[m.rightHip];
markers[m.leftHip] = markersLastFrame[m.leftHip];
}
}
/// <summary>
/// Given a array of bones, and a target bone, solves the chain so that the last bone in the chain is at at the same position as the target
/// </summary>
/// <param name="bones">An array of bones, the chain to be solved by IK</param>
/// <param name="target">The target for the chain</param>
/// <param name="grandparent">the parent of the first bone in the bones chain, is used to ensure constraints</param>
/// <returns>True if target was reached, false if maximum iteration was reached first </returns>
override public bool SolveBoneChain(Bone[] bones, Bone target, Bone grandparent)
{
if (!IsReachable(bones, target))
{
TargetUnreachable(bones, target.Pos, grandparent);
bones[bones.Length - 1].Orientation = new Quaternion(target.Orientation.Xyz, target.Orientation.W);
return(true);
}
int numberOfBones = bones.Length;
int iter = 0;
int degrees = degreeStep;
bool toggle = false;
bool doneOneLapAroundYAxis = false;
int maxdegrees = 120;
float lastDistanceToTarget = float.MaxValue;
float distanceToTarget = (bones[bones.Length - 1].Pos - target.Pos).Length;
// main loop
while (distanceToTarget > threshold && MaxIterations > ++iter)
{
// if CCD is stuck becouse of constraints, we twist the chain
if (distanceToTarget >= lastDistanceToTarget)
{
if (!doneOneLapAroundYAxis && degrees > maxdegrees)
{
doneOneLapAroundYAxis = true;
degrees = degreeStep;
}
else if (doneOneLapAroundYAxis && degrees > maxdegrees)
{
break;
}
Quaternion q = doneOneLapAroundYAxis ?
QuaternionHelper2.RotationX(MathHelper.DegreesToRadians(toggle ? degrees : -degrees))
:
QuaternionHelper2.RotationY(MathHelper.DegreesToRadians(toggle ? degrees : -degrees));
ForwardKinematics(ref bones, q);
if (toggle)
{
degrees += degreeStep;
}
toggle = !toggle;
}
// for each bone, starting with the one closest to the end effector
// (but not the end effector itself)
Vector3 a, b;
Quaternion rotation;
for (int i = numberOfBones - 2; i >= 0; i--)
{
// Get the vectors between the points
a = bones[numberOfBones - 1].Pos - bones[i].Pos;
b = target.Pos - bones[i].Pos;
// Make a rotation quaternion and rotate
// - first the endEffector
// - then the rest of the affected joints
rotation = (a.LengthFast == 0 || b.LengthFast == 0) ? Quaternion.Identity
: QuaternionHelper2.GetRotationBetween(a, b, bones[i].Stiffness);
if (bones[i].HasConstraints)
{
Vector3 res;
Quaternion rot;
if (constraints.CheckRotationalConstraints(
bones[i],
((i > 0) ? bones[i - 1] : grandparent).Orientation, //Reference
bones[i].Pos + Vector3.Transform(bones[i + 1].Pos - bones[i].Pos, rotation), // Target
out res, out rot))
{
rotation = rot * rotation;
}
}
// Move the chain
ForwardKinematics(ref bones, rotation, i);
// Check for twist constraints
if (bones[i].HasTwistConstraints)
{
Quaternion rotation2;
if (constraints.CheckOrientationalConstraint(bones[i], (i > 0) ? bones[i - 1] : grandparent, out rotation2))
{
ForwardKinematics(ref bones, rotation2, i);
}
}
}
lastDistanceToTarget = distanceToTarget;
distanceToTarget = (bones[bones.Length - 1].Pos - target.Pos).LengthFast;
}
// Copy the targets rotation so that rotation is consistant
bones[bones.Length - 1].Orientation = new Quaternion(target.Orientation.Xyz, target.Orientation.W);
return(distanceToTarget <= threshold);
}
public void RotateTowards(Vector3 v, float stiffness = 1f)
{
Rotate(QuaternionHelper2.GetRotationBetween(GetYAxis(), v, stiffness = this.stiffness));
}