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