예제 #1
0
        /// <summary>
        ///
        /// </summary>
        /// <param name="from"></param>
        /// <param name="to"></param>
        /// <returns></returns>
        public static Quaterniond CreateFromTo(Vec3d from, Vec3d to)
        {
            // impl refs
            // https://stackoverflow.com/questions/1171849/finding-quaternion-representing-the-rotation-from-one-vector-to-another
            // http://lolengine.net/blog/2013/09/18/beautiful-maths-quaternion-from-vectors

            if (!from.Unitize() || !to.Unitize())
            {
                return(Identity);
            }

            var ca = Vec3d.Dot(from, to);

            // parallel check
            if (zMath.ApproxEquals(Math.Abs(ca), 1.0))
            {
                //opposite check
                if (ca < 0.0)
                {
                    var perp = from.X < 1.0 ? from.CrossX() : from.CrossY();
                    var t    = 1.0 / perp.Length;
                    return(new Quaterniond(perp.X * t, perp.Y * t, perp.Z * t, 0.0));
                }

                return(Identity);
            }

            // can assume axis is valid
            var axis = Vec3d.Cross(from, to);
            var q    = new Quaterniond(axis.X, axis.Y, axis.Z, ca + 1);

            q.Unitize();

            return(q);
        }
예제 #2
0
        /// <summary>
        ///
        /// </summary>
        /// <param name="axis"></param>
        /// <param name="angle"></param>
        public bool Set(Vec3d axis, double angle)
        {
            if (!axis.Unitize())
            {
                return(false);
            }

            SetImpl(axis, angle);
            return(true);
        }
예제 #3
0
        /// <summary>
        /// Creates the rotation between v0 and v1
        /// </summary>
        /// <param name="from"></param>
        /// <param name="to"></param>
        /// <returns></returns>
        public static OrthoBasis3d CreateFromTo(Vec3d from, Vec3d to)
        {
            // impl ref
            // https://math.stackexchange.com/questions/180418/calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d

            if (!from.Unitize() || !to.Unitize())
            {
                return(Identity);
            }

            var v = Vec3d.Cross(from, to);
            var c = Vec3d.Dot(from, to);
            var k = 1.0 / (1.0 + c);

            return(new OrthoBasis3d {
                _x = new Vec3d(v.X * v.X * k + c, v.X * v.Y * k + v.Z, v.X * v.Z * k - v.Y),
                _y = new Vec3d(v.Y * v.X * k - v.Z, v.Y * v.Y * k + c, v.Y * v.Z * k + v.X),
                _z = new Vec3d(v.Z * v.X * k + v.Y, v.Z * v.Y * k - v.X, v.Z * v.Z * k + c)
            });
        }
예제 #4
0
        /// <summary>
        ///
        /// </summary>
        /// <param name="from"></param>
        /// <param name="to"></param>
        /// <returns></returns>
        public static AxisAngle3d CreateFromTo(Vec3d from, Vec3d to)
        {
            if (!from.Unitize() || !to.Unitize())
            {
                return(Identity);
            }

            var ct = Vec3d.Dot(from, to);

            // parallel check
            if (1.0 - Math.Abs(ct) < zMath.ZeroTolerance)
            {
                // opposite check
                if (ct < 0.0)
                {
                    var perp = from.X < 1.0 ? from.CrossX() : from.CrossY();
                    return(new AxisAngle3d()
                    {
                        _axis = perp / perp.Length,
                        _angle = Math.PI,
                        _cosAngle = -1.0,
                        _sinAngle = 0.0
                    });
                }

                return(Identity);
            }

            // can assume axis is valid
            var axis = Vec3d.Cross(from, to);
            var st   = axis.Length;

            return(new AxisAngle3d()
            {
                _axis = axis / st,
                _angle = Math.Acos(ct),
                _sinAngle = st,
                _cosAngle = ct
            });
        }