GetUpVector() public static method

public static GetUpVector ( Quaternion q ) : Vector3
q Quaternion
return Vector3
コード例 #1
0
        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);
        }
コード例 #2
0
        protected internal override void OnApplyConstraint(UserJointBase joint)
        {
            Matrix3D m = Joint.BodyToWorldMatrix(joint.ChildBody, this.PivotPoint, this.Direction);
            Vector3D childPivotPoint = Math3D.GetOffset(ref m);

            Vector3D parentPivotPoint = (Vector3D)Joint.BodyToWorld(joint.ParentBody, _parentPivotPoint);

            joint.NewtonJoint.UserBilateralAddLinearRow(
                childPivotPoint, parentPivotPoint,
                Math3D.GetFrontVector(ref m)
                );
            ApplyConstraintProperties(joint);

            joint.NewtonJoint.UserBilateralAddLinearRow(
                childPivotPoint, parentPivotPoint,
                Math3D.GetUpVector(ref m)
                );
            ApplyConstraintProperties(joint);

            joint.NewtonJoint.UserBilateralAddLinearRow(
                childPivotPoint, parentPivotPoint,
                Math3D.GetRightVector(ref m)
                );
            ApplyConstraintProperties(joint);
        }