BEPUphysics.Constraints.TwoEntity.Joints.TwistJoint.Initialize C# (CSharp) Method

        private void Initialize()
        {
            //Compute a vector which is perpendicular to the axis.  It'll be added in local space to both connections.
            Vector3 yAxis;
            Vector3.Cross(ref worldAxisA, ref Toolbox.UpVector, out yAxis);
            float length = yAxis.LengthSquared();
            if (length < Toolbox.Epsilon)
            {
                Vector3.Cross(ref worldAxisA, ref Toolbox.RightVector, out yAxis);
            }
            yAxis.Normalize();

            //Put the axis into the local space of A.
            Quaternion conjugate;
            Quaternion.Conjugate(ref connectionA.orientation, out conjugate);
            Vector3.Transform(ref yAxis, ref conjugate, out aLocalAxisY);

            //Complete A's basis.
            Vector3.Cross(ref localAxisA, ref aLocalAxisY, out aLocalAxisZ);

            //Rotate the axis to B since it could be arbitrarily rotated.
            Quaternion rotation;
            Toolbox.GetQuaternionBetweenNormalizedVectors(ref worldAxisA, ref worldAxisB, out rotation);
            Vector3.Transform(ref yAxis, ref rotation, out yAxis);

            //Put it into local space.
            Quaternion.Conjugate(ref connectionB.orientation, out conjugate);
            Vector3.Transform(ref yAxis, ref conjugate, out bLocalAxisY);
        }
    }