GetRotationBetween() public static method

Returns a quaternion representing the rotation from vector a to b Robust, does not return NaN
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
Ejemplo n.º 1
0
        /// <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);
        }
Ejemplo n.º 2
0
        // 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];
            }
        }
Ejemplo n.º 4
0
        /// <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);
        }
Ejemplo n.º 5
0
 public void RotateTowards(Vector3 v, float stiffness = 1f)
 {
     Rotate(QuaternionHelper2.GetRotationBetween(GetYAxis(), v, stiffness = this.stiffness));
 }