/// <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>
/// 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>
/// 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>
/// 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 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;
}
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);
}
}
public Vector2d Rotated(double phi)
{
double rads = Angle() + phi;
return(new Vector2d(Mathd.Cos(rads), Mathd.Sin(rads)) * Length());
}
