/// <summary>
/// Creates quaternion representing a rotation around
/// an axis by an angle.
/// </summary>
// todo ISSUE 20090420 andi : sm, rft: What about adding an AxisAngle struct?.
public __rot3t__(__v3t__ axis, __ft__ angleInRadians)
{
var halfAngle = angleInRadians / 2;
W = halfAngle.Cos();
V = axis.Normalized * halfAngle.Sin();
}
/// <summary>
/// Creates a quaternion representing a rotation from one
/// vector into another.
/// </summary>
public __rot3t__(__v3t__ from, __v3t__ into)
{
var a = from.Normalized;
var b = into.Normalized;
var angle = Fun.Clamp(__v3t__.Dot(a, b), -1, 1).Acos();
var angleAbs = angle.Abs();
__v3t__ axis;
// some vectors do not normalize to 1.0 -> Vec.Dot = -0.99999999999999989 || -0.99999994f
// acos => 3.1415926386886319 or 3.14124632f -> delta of 1e-7 or 1e-3
if (angle < __rotIntoEps__)
{
axis = a;
angle = 0;
}
else if (__pi__ - angleAbs < __rotIntoEps__)
{
axis = a.AxisAlignedNormal();
angle = __pi__;
}
else
{
axis = __v3t__.Cross(a, b).Normalized;
}
this = new __rot3t__(axis, angle);
}
/// <summary>
/// Transforms direction vector v (v.w is presumed 0.0) by quaternion q.
/// </summary>
public static __v3t__ TransformDir(__rot3t__ q, __v3t__ v)
{
// first transforming quaternion to __m33t__ is approximately equal in terms of operations ...
return(((__m33t__)q).Transform(v));
// ... than direct multiplication ...
//QuaternionF r = q.Conjugated() * new QuaternionF(0, v) * q;
//return new __v3t__(r.X, r.Y, r.Z);
}
public static __v3t__ Transform(this __type__ rot, __v3t__ v)
{
__ftype__ a = Fun.Cos(rot.Angle);
__ftype__ b = Fun.Sin(rot.Angle);
return(new __v3t__(a * v.X + -b * v.Y,
b * v.X + a * v.Y,
v.Z));
}
public static __v3t__ Multiply(__rot2t__ rot, __v3t__ vec)
{
__ft__ ca = (__ft__)System.Math.Cos(rot.Angle);
__ft__ sa = (__ft__)System.Math.Sin(rot.Angle);
return(new __v3t__(ca * vec.X + sa * vec.Y,
-sa * vec.X + ca * vec.Y,
vec.Z));
}
public static __v3t__ InvTransform(this __type__ r, __v3t__ v)
{
var w = r.X * v.X + r.Y * v.Y + r.Z * v.Z;
var x = r.W * v.X - r.Y * v.Z + r.Z * v.Y;
var y = r.W * v.Y - r.Z * v.X + r.X * v.Z;
var z = r.W * v.Z - r.X * v.Y + r.Y * v.X;
return(new __v3t__(
w * r.X + x * r.W + y * r.Z - z * r.Y,
w * r.Y + y * r.W + z * r.X - x * r.Z,
w * r.Z + z * r.W + x * r.Y - y * r.X
));
}
// https://stackoverflow.com/questions/1171849/finding-quaternion-representing-the-rotation-from-one-vector-to-another
public static __rot3t__ HalfWayVec(__v3t__ from, __v3t__ into)
{
if (from.Dot(into).ApproximateEquals(-1))
{
return(new __rot3t__(0, from.AxisAlignedNormal()));
}
else
{
__v3t__ half = Vec.Normalized(from + into);
__quatt__ q = new __quatt__(Vec.Dot(from, half), Vec.Cross(from, half));
return(new __rot3t__(q.Normalized));
}
}
/// <summary>
/// Inverts this quaternion (multiplicative inverse).
/// Returns this.
/// </summary>
public void Invert()
{
var norm = NormSquared;
if (norm == 0)
{
throw new ArithmeticException("quaternion is not invertible");
}
var scale = 1 / norm;
W *= scale;
V *= -scale;
}
public static __rot3t__ HalfWayQuat(__v3t__ from, __v3t__ into)
{
var d = Vec.Dot(from, into);
if (d.ApproximateEquals(-1))
{
return(new __rot3t__(0, from.AxisAlignedNormal()));
}
else
{
__quatt__ q = new __quatt__(d + 1, Vec.Cross(from, into));
return(new __rot3t__(q.Normalized));
}
}
/// <summary>
/// Normalizes this quaternion.
/// </summary>
/// <returns>This.</returns>
public void Normalize()
{
var norm = Norm;
if (norm == 0)
{
this = __rot3t__.Identity;
}
else
{
var scale = 1 / norm;
W *= scale;
V *= scale;
}
}
/// <summary>
/// Create from Rodrigues axis-angle vactor
/// </summary>
public static __rot3t__ FromAngleAxis(__v3t__ angleAxis)
{
__ft__ theta2 = angleAxis.LengthSquared;
if (theta2 > Constant <__ft__> .PositiveTinyValue)
{
var theta = Fun.Sqrt(theta2);
var thetaHalf = theta / 2;
var k = Fun.Sin(thetaHalf) / theta;
return(new __rot3t__(Fun.Cos(thetaHalf), k * angleAxis));
}
else
{
return(new __rot3t__(1, 0, 0, 0));
}
}
public static void ToAxisAngle(this __type__ r, ref __v3t__ axis, ref __ftype__ angleInRadians)
{
angleInRadians = 2 * Fun.Acos(r.W);
var s = Fun.Sqrt(1 - r.W * r.W); // assuming quaternion normalised then w is less than 1, so term always positive.
if (s < 0.001)
{ // test to avoid divide by zero, s is always positive due to sqrt
// if s close to zero then direction of axis not important
axis.X = r.X; // if it is important that axis is normalised then replace with x=1; y=z=0;
axis.Y = r.Y;
axis.Z = r.Z;
}
else
{
axis.X = r.X / s; // normalise axis
axis.Y = r.Y / s;
axis.Z = r.Z / s;
}
}
public static __rot3t__ Original(__v3t__ from, __v3t__ into)
{
var angle = from.AngleBetween(into);
//# var rotIntoEps = isDouble ? "1e-15" : "1e-6f";
if (angle < __rotIntoEps__)
{
return(__rot3t__.Identity);
}
else if (__pi__ - angle < __rotIntoEps__)
{
return(new __rot3t__(0, from.AxisAlignedNormal()));
}
else
{
__v3t__ axis = Vec.Cross(from, into).Normalized;
return(__rot3t__.Rotation(axis, angle));
}
}
/// <summary>
/// Converts this Rotation to the axis angle representation.
/// </summary>
/// <param name="axis">Output of normalized axis of rotation.</param>
/// <param name="angleInRadians">Output of angle of rotation about axis (Right Hand Rule).</param>
public void ToAxisAngle(ref __v3t__ axis, ref __ft__ angleInRadians)
{
if (!Fun.ApproximateEquals(NormSquared, 1, 0.001))
{
throw new ArgumentException("Quaternion needs to be normalized to represent a rotation.");
}
angleInRadians = 2 * (__ft__)System.Math.Acos(W);
var s = (__ft__)System.Math.Sqrt(1 - W * W); // assuming quaternion normalised then w is less than 1, so term always positive.
if (s < 0.001)
{ // test to avoid divide by zero, s is always positive due to sqrt
// if s close to zero then direction of axis not important
axis.X = X; // if it is important that axis is normalised then replace with x=1; y=z=0;
axis.Y = Y;
axis.Z = Z;
}
else
{
axis.X = X / s; // normalise axis
axis.Y = Y / s;
axis.Z = Z / s;
}
}
/// <summary>
/// Transforms direction vector v (v.w is presumed 0.0) by the inverse of this rigid transformation.
/// Actually, only the rotation is used.
/// </summary>
public __v3t__ InvTransformDir(__v3t__ v)
{
return(InvTransformDir(this, v));
}
/// <summary>
/// Transforms direction vector v (v.w is presumed 0.0) by this rigid transformation.
/// Actually, only the rotation is used.
/// </summary>
public __v3t__ TransformDir(__v3t__ v)
{
return(TransformDir(this, v));
}
/// <summary>
/// Transforms point p (p.w is presumed 1.0) by the inverse of the rigid transformation r.
/// </summary>
public static __v3t__ InvTransformPos(__e3t__ r, __v3t__ p)
{
return(r.Rot.InvTransformPos(p - r.Trans));
}
/// <summary>
/// Transforms direction vector v (v.w is presumed 0.0) by the inverse of the rigid transformation r.
/// Actually, only the rotation is used.
/// </summary>
public static __v3t__ InvTransformDir(__e3t__ r, __v3t__ v)
{
return(r.Rot.InvTransformDir(v));
}
/// <summary>
/// Transforms point p (p.w is presumed 1.0) by rigid transformation r.
/// </summary>
public static __v3t__ TransformPos(__e3t__ r, __v3t__ p)
{
return(r.Rot.TransformPos(p) + r.Trans);
}
/// <summary>
/// Creates quaternion (w, (x, y, z)).
/// </summary>
// todo ISSUE 20090420 andi : sm, rft: Add asserts for unit-length constraint
public __rot3t__(__ft__ w, __ft__ x, __ft__ y, __ft__ z)
{
W = w;
V = new __v3t__(x, y, z);
}
/// <summary>
/// Transforms point p (p.w is presumed 1.0) by this quaternion.
/// For quaternions, this method is equivalent to TransformDir, and
/// is made available only to provide a consistent set of operations
/// for all transforms.
/// </summary>
public __v3t__ TransformPos(__v3t__ p)
{
return(TransformDir(this, p));
}
/// <summary>
/// WARNING: UNTESTED!!!
/// </summary>
public static __rot3t__ FromFrame(__v3t__ x, __v3t__ y, __v3t__ z)
{
return(From__m33t__(__m33t__.FromCols(x, y, z)));
}
/// <summary>
/// Creates quaternion from array starting at specified index.
/// (w = a[start], (x = a[start+1], y = a[start+2], z = a[start+3])).
/// </summary>
public __rot3t__(__ft__[] a, int start)
{
W = a[start];
V = new __v3t__(a[start + 1], a[start + 2], a[start + 3]);
}
/// <summary>
/// Creates quaternion (w, (v.x, v.y, v.z)).
/// </summary>
public __rot3t__(__ft__ w, __v3t__ v)
{
W = w;
V = v;
}
/// <summary>
/// Creates a rigid transformation from a rotation <paramref name="rot"/> and a (subsequent) translation <paramref name="trans"/>.
/// </summary>
public __e3t__(__r3t__ rot, __v3t__ trans)
{
Rot = rot;
Trans = trans;
}
/// <summary>
/// Transforms point p (p.w is presumed 1.0) by the inverse of this rigid transformation.
/// </summary>
public __v3t__ InvTransformPos(__v3t__ p)
{
return(InvTransformPos(this, p));
}
/// <summary>
/// Inverts this rigid transformation (multiplicative inverse).
/// this = [Rot^T,-Rot^T Trans]
/// </summary>
public void Invert()
{
Rot.Invert();
Trans = -Rot.TransformDir(Trans);
}
/// <summary>
/// Conjugates this quaternion. Returns this.
/// For normalized rotation-quaternions this is the same as Invert().
/// </summary>
public void Conjugate()
{
V = -V;
}
/// <summary>
/// Creates a rigid transformation from a rotation matrix <paramref name="rot"/> and a (subsequent) translation <paramref name="trans"/>.
/// </summary>
public __e3t__(M3__s3f__ rot, __v3t__ trans, __ft__ epsilon = __eps__)
{
Rot = __r3t__.FromM3__s3f__(rot, epsilon);
Trans = trans;
}
/// <summary>
/// Creates quaternion from array.
/// (w = a[0], (x = a[1], y = a[2], z = a[3])).
/// </summary>
public __rot3t__(__ft__[] a)
{
W = a[0];
V = new __v3t__(a[1], a[2], a[3]);
}
