# Rotation constraint implementation - comments?

## Recommended Posts

sebjf    187
Hello,

I am working on constraining a skeleton and my implementation has a couple of problems and I wondered if someone could take a look?

In short here is how it works:

Constraints are stored as a set of axis with min & max rotations around them. In practice (because of where I got the constraints) they are equivalent to euler angles but the idea is any set of arbitrary axis' could be used.

1. Recieves a transform matrix that the game (e.g. animation editor) wants to set as the transform for a bone.
2. Method deconstructs matrix getting rotation as quaternion
3. Identifies a vector orthogonal to the axis of the constraint.
4. Rotates this vector with the quaternion from (2)
5. Projects the result onto the plane whose normal is the axis from the constraint. ACos of the dot of this projection and the original is the angle in this plane (around the axis)
6. If the angle is outside the min/max, the rotation quaternion is concatenated with a new rotation equivalent to the difference between the angle & the min/max, around the constraint axis.

So in effect it works by 'nudging' the rotation back into range every time it strays, however it has a couple of problems:

1. When at the extreme the limbs 'twitch' at high speed as if the method is unable to stabalise - floating point error perhaps?
2. The corrections 'leak' into other axis, meaning a correction that prevents a bone from rotating further in one axis will cause it to suddenly shoot upwards in another, or turn over entirely.

Below is my code.

I'd appreciate any commments on the constraint method or its implementation, thanks for taking a look!

        public Matrix ConstrainBoneTransform(String bone, Matrix transform)        {            List<BoneConstraint> applicable_constraints;            ///[Get applicable_constraints here]            Vector3 scale;            Vector3 translation;            Quaternion rotation;            transform.Decompose(out scale, out rotation, out translation);            rotation.Normalize();            Matrix rotationmodification = Matrix.Identity;            foreach (BoneConstraint c in applicable_constraints)            {                float angle1 = ExMath.FindQuaternionTwistSigned(rotation, c.Axis);                float delta = 0;                if (angle1 > max)                {                    delta = (max - angle1);                }                if (angle1 < min)                {                    delta = (min - angle1);                }                if (delta != 0)                {                    rotationmodification *= Matrix.CreateFromAxisAngle(c.Axis, delta);                }            }            if (rotationmodification != Matrix.Identity)            {                return (Matrix.CreateScale(scale) * Matrix.CreateFromQuaternion(rotation) * rotationmodification * Matrix.CreateTranslation(translation));            }            return transform;        }

public static float FindQuaternionTwistSigned(Quaternion q, Vector3 axis)        {            axis.Normalize();            //get the plane the axis is a normal of            Vector3 orthonormal1, orthonormal2;            ExMath.FindOrthonormals(axis, out orthonormal1, out orthonormal2);            Vector3 transformed = Vector3.Transform(orthonormal1, q);            //project transformed vector onto plane            Vector3 flattened = transformed - (Vector3.Dot(transformed, axis) * axis);            flattened.Normalize();            Vector3 sign_reference = Vector3.Transform(orthonormal1,Quaternion.CreateFromAxisAngle(axis,((float)Math.PI/2)));            dot = Vector3.Dot(sign_reference, flattened);            //get angle between original vector and projected transform to get angle around normal            float a = (float)Math.Acos((double)Vector3.Dot(orthonormal1, flattened));            if (dot < 0)            {                a = -a;            }            return a;        }