/// <summary>
/// Constructs a transformation matrix from a rotation value and origin vector.
/// </summary>
/// <param name="rot">The rotation of the new transform, in radians.</param>
/// <param name="pos">The origin vector, or column index 2.</param>
public Transform2Dd(double rot, Vector2d pos)
{
x.x = y.y = Mathd.Cos(rot);
x.y = y.x = Mathd.Sin(rot);
y.x *= -1;
origin = pos;
}
/// <summary>
/// Constructs a quaternion that will rotate around the given axis
/// by the specified angle. The axis must be a normalized vector.
/// </summary>
/// <param name="axis">The axis to rotate around. Must be normalized.</param>
/// <param name="angle">The angle to rotate, in radians.</param>
public Quatd(Vector3d axis, double angle)
{
#if DEBUG
if (!axis.IsNormalized())
{
throw new ArgumentException("Argument is not normalized", nameof(axis));
}
#endif
double d = axis.Length();
if (d == 0f)
{
x = 0f;
y = 0f;
z = 0f;
w = 0f;
}
else
{
double sinAngle = Mathd.Sin(angle * 0.5f);
double cosAngle = Mathd.Cos(angle * 0.5f);
double s = sinAngle / d;
x = axis.x * s;
y = axis.y * s;
z = axis.z * s;
w = cosAngle;
}
}
/// <summary>
/// Creates a Dimetric Basis25Dd from the angle between the Y axis and the others.
/// Dimetric(Tau/3) or Dimetric(2.09439510239) is the same as Isometric.
/// Try to keep this number away from a multiple of Tau/4 (or Pi/2) radians.
/// </summary>
/// <param name="angle">The angle, in radians, between the Y axis and the X/Z axes.</param>
public static Basis25Dd Dimetric(double angle)
{
double sin = Mathd.Sin(angle);
double cos = Mathd.Cos(angle);
return(new Basis25Dd(sin, -cos, 0, -1, -sin, -cos));
}
/// <summary>
/// Rotates this vector by `phi` radians.
/// </summary>
/// <param name="phi">The angle to rotate by, in radians.</param>
/// <returns>The rotated vector.</returns>
public Vector2d Rotated(double phi)
{
double sine = Mathd.Sin(phi);
double cosi = Mathd.Cos(phi);
return(new Vector2d(
x * cosi - y * sine,
x * sine + y * cosi));
}
public Quatd Slerp(Quatd b, double t)
{
// Calculate cosine
double cosom = x * b.x + y * b.y + z * b.z + w * b.w;
var to1 = new Quatd();
// Adjust signs if necessary
if (cosom < 0.0)
{
cosom = -cosom;
to1.x = -b.x;
to1.y = -b.y;
to1.z = -b.z;
to1.w = -b.w;
}
else
{
to1.x = b.x;
to1.y = b.y;
to1.z = b.z;
to1.w = b.w;
}
double sinom, scale0, scale1;
// Calculate coefficients
if (1.0 - cosom > Mathd.Epsilon)
{
// Standard case (Slerp)
double omega = Mathd.Acos(cosom);
sinom = Mathd.Sin(omega);
scale0 = Mathd.Sin((1.0f - t) * omega) / sinom;
scale1 = Mathd.Sin(t * omega) / sinom;
}
else
{
// Quatdernions are very close so we can do a linear interpolation
scale0 = 1.0f - t;
scale1 = t;
}
// Calculate final values
return(new Quatd
(
scale0 * x + scale1 * to1.x,
scale0 * y + scale1 * to1.y,
scale0 * z + scale1 * to1.z,
scale0 * w + scale1 * to1.w
));
}
public Transform2Dd InterpolateWith(Transform2Dd m, double c)
{
double r1 = Rotation;
double r2 = m.Rotation;
Vector2d s1 = Scale;
Vector2d s2 = m.Scale;
// Slerp rotation
var v1 = new Vector2d(Mathd.Cos(r1), Mathd.Sin(r1));
var v2 = new Vector2d(Mathd.Cos(r2), Mathd.Sin(r2));
double dot = v1.Dot(v2);
// Clamp dot to [-1, 1]
dot = dot < -1.0f ? -1.0f : (dot > 1.0f ? 1.0f : dot);
Vector2d v;
if (dot > 0.9995f)
{
// Linearly interpolate to avoid numerical precision issues
v = v1.LinearInterpolate(v2, c).Normalized();
}
else
{
double angle = c * Mathd.Acos(dot);
Vector2d v3 = (v2 - v1 * dot).Normalized();
v = v1 * Mathd.Cos(angle) + v3 * Mathd.Sin(angle);
}
// Extract parameters
Vector2d p1 = origin;
Vector2d p2 = m.origin;
// Construct matrix
var res = new Transform2Dd(Mathd.Atan2(v.y, v.x), p1.LinearInterpolate(p2, c));
Vector2d scale = s1.LinearInterpolate(s2, c);
res.x *= scale;
res.y *= scale;
return(res);
}
/// <summary>
/// Constructs a quaternion that will perform a rotation specified by
/// Euler angles (in the YXZ convention: when decomposing,
/// first Z, then X, and Y last),
/// given in the vector format as (X angle, Y angle, Z angle).
/// </summary>
/// <param name="eulerYXZ"></param>
public Quatd(Vector3d eulerYXZ)
{
double half_a1 = eulerYXZ.y * 0.5f;
double half_a2 = eulerYXZ.x * 0.5f;
double half_a3 = eulerYXZ.z * 0.5f;
// R = Y(a1).X(a2).Z(a3) convention for Euler angles.
// Conversion to quaternion as listed in https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19770024290.pdf (page A-6)
// a3 is the angle of the first rotation, following the notation in this reference.
double cos_a1 = Mathd.Cos(half_a1);
double sin_a1 = Mathd.Sin(half_a1);
double cos_a2 = Mathd.Cos(half_a2);
double sin_a2 = Mathd.Sin(half_a2);
double cos_a3 = Mathd.Cos(half_a3);
double sin_a3 = Mathd.Sin(half_a3);
x = sin_a1 * cos_a2 * sin_a3 + cos_a1 * sin_a2 * cos_a3;
y = sin_a1 * cos_a2 * cos_a3 - cos_a1 * sin_a2 * sin_a3;
z = cos_a1 * cos_a2 * sin_a3 - sin_a1 * sin_a2 * cos_a3;
w = sin_a1 * sin_a2 * sin_a3 + cos_a1 * cos_a2 * cos_a3;
}
/// <summary>
/// Returns the result of the spherical linear interpolation between
/// this quaternion and `to` by amount `weight`, but without
/// checking if the rotation path is not bigger than 90 degrees.
/// </summary>
/// <param name="to">The destination quaternion for interpolation. Must be normalized.</param>
/// <param name="weight">A value on the range of 0.0 to 1.0, representing the amount of interpolation.</param>
/// <returns>The resulting quaternion of the interpolation.</returns>
public Quatd Slerpni(Quatd to, double weight)
{
double dot = Dot(to);
if (Mathd.Abs(dot) > 0.9999f)
{
return(this);
}
double theta = Mathd.Acos(dot);
double sinT = 1.0f / Mathd.Sin(theta);
double newFactor = Mathd.Sin(weight * theta) * sinT;
double invFactor = Mathd.Sin((1.0f - weight) * theta) * sinT;
return(new Quatd
(
invFactor * x + newFactor * to.x,
invFactor * y + newFactor * to.y,
invFactor * z + newFactor * to.z,
invFactor * w + newFactor * to.w
));
}
public Quatd Slerpni(Quatd b, double t)
{
double dot = Dot(b);
if (Mathd.Abs(dot) > 0.9999f)
{
return(this);
}
double theta = Mathd.Acos(dot);
double sinT = 1.0f / Mathd.Sin(theta);
double newFactor = Mathd.Sin(t * theta) * sinT;
double invFactor = Mathd.Sin((1.0f - t) * theta) * sinT;
return(new Quatd
(
invFactor * x + newFactor * b.x,
invFactor * y + newFactor * b.y,
invFactor * z + newFactor * b.z,
invFactor * w + newFactor * b.w
));
}
public Quatd(Vector3d axis, double angle)
{
double d = axis.Length();
double angle_t = angle;
if (d == 0f)
{
x = 0f;
y = 0f;
z = 0f;
w = 0f;
}
else
{
double s = Mathd.Sin(angle_t * 0.5f) / d;
x = axis.x * s;
y = axis.y * s;
z = axis.z * s;
w = Mathd.Cos(angle_t * 0.5f);
}
}
/// <summary>
/// Returns the result of the spherical linear interpolation between
/// this quaternion and `to` by amount `weight`.
///
/// Note: Both quaternions must be normalized.
/// </summary>
/// <param name="to">The destination quaternion for interpolation. Must be normalized.</param>
/// <param name="weight">A value on the range of 0.0 to 1.0, representing the amount of interpolation.</param>
/// <returns>The resulting quaternion of the interpolation.</returns>
public Quatd Slerp(Quatd to, double weight)
{
#if DEBUG
if (!IsNormalized())
{
throw new InvalidOperationException("Quatd is not normalized");
}
if (!to.IsNormalized())
{
throw new ArgumentException("Argument is not normalized", nameof(to));
}
#endif
// Calculate cosine.
double cosom = x * to.x + y * to.y + z * to.z + w * to.w;
var to1 = new Quatd();
// Adjust signs if necessary.
if (cosom < 0.0)
{
cosom = -cosom;
to1.x = -to.x;
to1.y = -to.y;
to1.z = -to.z;
to1.w = -to.w;
}
else
{
to1.x = to.x;
to1.y = to.y;
to1.z = to.z;
to1.w = to.w;
}
double sinom, scale0, scale1;
// Calculate coefficients.
if (1.0 - cosom > Mathd.Epsilon)
{
// Standard case (Slerp).
double omega = Mathd.Acos(cosom);
sinom = Mathd.Sin(omega);
scale0 = Mathd.Sin((1.0f - weight) * omega) / sinom;
scale1 = Mathd.Sin(weight * omega) / sinom;
}
else
{
// Quaternions are very close so we can do a linear interpolation.
scale0 = 1.0f - weight;
scale1 = weight;
}
// Calculate final values.
return(new Quatd
(
scale0 * x + scale1 * to1.x,
scale0 * y + scale1 * to1.y,
scale0 * z + scale1 * to1.z,
scale0 * w + scale1 * to1.w
));
}
public Vector2d Rotated(double phi)
{
double rads = Angle() + phi;
return(new Vector2d(Mathd.Cos(rads), Mathd.Sin(rads)) * Length());
}
