/// <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);
}
// [todo ISSUE 20090225 andi : andi] Wir sollten auch folgendes beruecksichtigen -q == q, weil es die selbe rotation definiert.
// [todo ISSUE 20090427 andi : andi] use an angle-tolerance
// [todo ISSUE 20090427 andi : andi] add __rot3t__.ApproxEqual(__rot3t__ other);
public static bool ApproxEqual(__rot3t__ r0, __rot3t__ r1, __ft__ tolerance)
{
return((r0.W - r1.W).Abs() <= tolerance &&
(r0.X - r1.X).Abs() <= tolerance &&
(r0.Y - r1.Y).Abs() <= tolerance &&
(r0.Z - r1.Z).Abs() <= tolerance);
}
// todo andi {
public static __rot3t__ Divide(__ft__ s, __rot3t__ q)
{
return(new __rot3t__(
s / q.W,
s / q.X, s / q.Y, s / q.Z
));
}
/// <summary>
/// Creates quaternion from euler angles [yaw, pitch, roll].
/// </summary>
/// <param name="yawInRadians">Rotation around X</param>
/// <param name="pitchInRadians">Rotation around Y</param>
/// <param name="rollInRadians">Rotation around Z</param>
public __rot3t__(__ft__ yawInRadians, __ft__ pitchInRadians, __ft__ rollInRadians)
{
var qx = new __rot3t__(__v3t__.XAxis, yawInRadians);
var qy = new __rot3t__(__v3t__.YAxis, pitchInRadians);
var qz = new __rot3t__(__v3t__.ZAxis, rollInRadians);
this = qz * qy * qx;
}
/// <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);
}
// todo andi }
/// <summary>
/// Multiplies 2 quaternions.
/// This concatenates the two rotations into a single one.
/// Attention: Multiplication is NOT commutative!
/// </summary>
public static __rot3t__ Multiply(__rot3t__ a, __rot3t__ b)
{
return(new __rot3t__(
a.W * b.W - a.X * b.X - a.Y * b.Y - a.Z * b.Z,
a.W * b.X + a.X * b.W + a.Y * b.Z - a.Z * b.Y,
a.W * b.Y + a.Y * b.W + a.Z * b.X - a.X * b.Z,
a.W * b.Z + a.Z * b.W + a.X * b.Y - a.Y * b.X
));
}
// [todo ISSUE 20090225 andi : andi>
// Investigate if we can use power to "scale" the rotation (=enlarging the angle of rotation by a factor.)
// If so, reenable the Log and Exp methods if they are useful then.
// <]
#if false
/// <summary>
/// Calculates the logarithm of the quaternion.
/// </summary>
public static __rot3t__ Log(__rot3t__ a)
{
var result = __rot3t__.Zero;
if (a.W.Abs() < 1)
{
var angle = a.W.Acos();
var sin = angle.Sin();
result.V = (sin.Abs() >= 0) ? (a.V * (angle / sin)) : a.V;
}
return(result);
}
/// <summary>
/// Calculates exponent of the quaternion.
/// </summary>
public __rot3t__ Exp(__rot3t__ a)
{
var result = __rot3t__.Zero;
var angle = (a.X * a.X + a.Y * a.Y + a.Z * a.Z).Sqrt();
var sin = angle.Sin();
if (sin.Abs() > 0)
{
var coeff = angle / sin;
result.V = coeff * a.V;
}
else
{
result.V = a.V;
}
return(result);
}
// todo andi }
/// <summary>
/// Returns the dot-product of 2 quaternions.
/// </summary>
public static __ft__ Dot(__rot3t__ a, __rot3t__ b)
{
return(a.W * b.W + a.X * b.X + a.Y * b.Y + a.Z * b.Z);
}
// todo andi {
/// <summary>
/// Returns the component-wise reciprocal (1/q.w, 1/q.x, 1/q.y, 1/q.z)
/// of quaternion q.
/// </summary>
public static __rot3t__ Reciprocal(__rot3t__ q)
{
return(new __rot3t__(1 / q.W, 1 / q.X, 1 / q.Y, 1 / q.Z));
}
// todo andi }
/// <summary>
/// Returns the component-wise negation (-q.w, -q.v) of quaternion q.
/// This represents the same rotation.
/// </summary>
public static __rot3t__ Negated(__rot3t__ q)
{
return(new __rot3t__(-q.W, -q.X, -q.Y, -q.Z));
}
/// <summary>
/// Divides 2 quaternions.
/// </summary>
public static __rot3t__ Divide(__rot3t__ a, __rot3t__ b)
{
return(Multiply(a, Reciprocal(b)));
}
/// <summary>
/// Returns (q.w / s, q.v * (1/s)).
/// </summary>
public static __rot3t__ Divide(__rot3t__ q, __ft__ s)
{
return(Multiply(q, 1 / s));
}
// [todo ISSUE 20090421 andi : andi>
// Operations like Add, Subtract and Multiplication with scalar, Divide, Reciprocal
// should not be defined in a Rot3*.
// These are perfectly valid for a quaternion, but a rotation is defined on a
// NORMALIZED quaternion. This Norm-Constraint would be violated with above operations.
// <]
// todo andi {
/// <summary>
/// Returns the sum of 2 quaternions (a.w + b.w, a.v + b.v).
/// </summary>
public static __rot3t__ Add(__rot3t__ a, __rot3t__ b)
{
return(new __rot3t__(a.W + b.W, a.X + b.X, a.Y + b.Y, a.Z + b.Z));
}
/// <summary>
/// Returns (q.w + s, (q.x + s, q.y + s, q.z + s)).
/// </summary>
public static __rot3t__ Add(__rot3t__ q, __ft__ s)
{
return(new __rot3t__(q.W + s, q.X + s, q.Y + s, q.Z + s));
}
/// <summary>
/// Returns (q.w - s, (q.x - s, q.y - s, q.z - s)).
/// </summary>
public static __rot3t__ Subtract(__rot3t__ q, __ft__ s)
{
return(Add(q, -s));
}
/// <summary>
/// Returns (q.w * s, q.v * s).
/// </summary>
public static __rot3t__ Multiply(__rot3t__ q, __ft__ s)
{
return(new __rot3t__(q.W * s, q.X * s, q.Y * s, q.Z * s));
}
/// <summary>
/// Returns (a.w - b.w, a.v - b.v).
/// </summary>
public static __rot3t__ Subtract(__rot3t__ a, __rot3t__ b)
{
return(Add(a, -b));
}
/// <summary>
/// Returns (s - q.w, (s - q.x, s- q.y, s- q.z)).
/// </summary>
public static __rot3t__ Subtract(__ft__ s, __rot3t__ q)
{
return(Add(-q, s));
}
public static bool ApproxEqual(__rot3t__ r0, __rot3t__ r1)
{
return(ApproxEqual(r0, r1, Constant <__ft__> .PositiveTinyValue));
}
/// <summary>
/// Transforms direction vector v (v.w is presumed 0.0) by the inverse of quaternion q.
/// </summary>
public static __v3t__ InvTransformDir(__rot3t__ q, __v3t__ v)
{
//for Rotation Matrices R^-1 == R^T:
return(((__m33t__)q).TransposedTransform(v));
}
/// <summary>
/// Transforms point p (p.w is presumed 1.0) by the incerse of quaternion q.
/// For quaternions, this method is equivalent to InvTransformDir, and
/// is made available only to provide a consistent set of operations
/// for all transforms.
/// </summary>
public static __v3t__ InvTransformPos(__rot3t__ q, __v3t__ p)
{
return(InvTransformDir(q, p));
}
public static __m33t__ Multiply(__rot2t__ rot2, __rot3t__ rot3)
{
return(__rot2t__.Multiply(rot2, (__m33t__)rot3));
}
