/// <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); }
/// <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); }
/// <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) }); }
/// <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 }); }