Math3D.GetUpVector C# (CSharp) Méthode

    public static Vector3 GetUpVector(Quaternion q)
    {
        return q * Vector3.up;
    }

        protected internal override void OnApplyConstraint(UserJointBase joint)
        {
            Matrix3D m = Joint.BodyToWorldMatrix(joint.ParentBody, _parentDirection);
            Matrix3D c = Joint.BodyToWorldMatrix(joint.ChildBody, this.Direction);

            double sa = Vector3D.DotProduct(Vector3D.CrossProduct(Math3D.GetUpVector(ref c), Math3D.GetUpVector(ref m)), Math3D.GetFrontVector(ref c));
            double ca = Vector3D.DotProduct(Math3D.GetUpVector(ref c), Math3D.GetUpVector(ref m));
            double a  = Math.Atan2(sa, ca);

            joint.NewtonJoint.UserBilateralAddAngularRow((float)a, Math3D.GetFrontVector(ref m));
            ApplyConstraintProperties(joint);

            sa = Vector3D.DotProduct(Vector3D.CrossProduct(Math3D.GetRightVector(ref c), Math3D.GetRightVector(ref m)), Math3D.GetUpVector(ref c));
            ca = Vector3D.DotProduct(Math3D.GetRightVector(ref c), Math3D.GetRightVector(ref m));
            a  = Math.Atan2(sa, ca);

            joint.NewtonJoint.UserBilateralAddAngularRow((float)a, Math3D.GetUpVector(ref m));
            ApplyConstraintProperties(joint);

            sa = Vector3D.DotProduct(Vector3D.CrossProduct(Math3D.GetFrontVector(ref c), Math3D.GetFrontVector(ref m)), Math3D.GetRightVector(ref c));
            ca = Vector3D.DotProduct(Math3D.GetFrontVector(ref c), Math3D.GetFrontVector(ref m));
            a  = Math.Atan2(sa, ca);

            joint.NewtonJoint.UserBilateralAddAngularRow((float)a, Math3D.GetRightVector(ref m));
            ApplyConstraintProperties(joint);
        }