/// <summary>
/// Rotate ellipse around point 'p' as a rotation center.
/// </summary>
public Ellipse Rotate(Rotation r, Point3d p)
{
return(new Ellipse(this.Center.Rotate(r, p), _v1.Rotate(r), _v2.Rotate(r)));
}
/// <summary>
/// Rotate sphere around point 'p' as a rotation center
/// </summary>
public Sphere Rotate(Rotation r, Point3d p)
{
return(new Sphere(this.Center.Rotate(r, p), this.R));
}
/// <summary>
/// Rotate line around point 'p' as a rotation center.
/// </summary>
public virtual Line3d Rotate(Rotation r, Point3d p)
{
return(new Line3d(this.Point.Rotate(r, p), this.Direction.Rotate(r)));
}
/// <summary>
/// Combine two rotations.
/// </summary>
public Rotation Mult(Rotation r)
{
Matrix3d m = this.ToRotationMatrix * r.ConvertTo(this.Coord).ToRotationMatrix;
return(new Rotation(m, this.Coord));
}
/// <summary>
/// Rotate plane around point 'p' as a rotation center
/// </summary>
public Plane3d Rotate(Rotation r, Point3d p)
{
return(new Plane3d(this.Point.Rotate(r, p), this.Normal.Rotate(r)));
}
/// <summary>
/// Spherical linear interpolation of two rotations.
/// </summary>
/// <param name="r1">Initial rotation</param>
/// <param name="r2">Final rotation</param>
/// <param name="t">Interpolation parameter within range [0, 1]</param>
public static Rotation SLERP(Rotation r1, Rotation r2, double t)
{
return(new Rotation(Quaternion.SLERP(r1.ToQuaternion, r2.ToQuaternion, t)));
}
/// <summary>
/// Factor a rotation matrix as product of three elemental rotations, i.e. rotations about the axes of a coordinate system.
/// <para>Both proper Euler angles ("xyx", "zxz", etc.) or Tait–Bryan angles ("xyz", "yzx") are allowed.</para>
/// Extrinsic rotations (rotations in fixed frame) should be written in lower case ("xyz", zxz", etc.).
/// <para>Intrinsic rotations (rotations in moving frame) should be written in upper case ("XYZ", "ZXZ", etc.).</para>
/// Note that such factorization generally is not unique!
/// </summary>
/// <param name="RotationOrder">String, representing rotation axes in the form "xyz" (extrinsic rotations, fixed frame) or "XYZ" (intrinsic rotations, moving frame).</param>
/// <param name="coord">Reference coordinate system, default - Coord3d.GlobalCS.</param>
/// <returns>Double array with first, second and third rotation angles</returns>
public double[] ToEulerAngles(string RotationOrder, Coord3d coord = null)
{
if (string.IsNullOrEmpty(RotationOrder) || RotationOrder.Length < 3)
{
throw new ArgumentException("Invalid parameter: RotationOrder");
}
coord = (coord == null) ? Coord3d.GlobalCS : coord;
Rotation r = this.ConvertTo(coord);
// Portions of the code were derived from article
// https://www.geometrictools.com/Documentation/EulerAngles.pdf
// published under Boost license
//============================================================================
// Boost Software License - Version 1.0 - August 17th, 2003
// Permission is hereby granted, free of charge, to any person or organization
//obtaining a copy of the software and accompanying documentation covered by
//this license(the "Software") to use, reproduce, display, distribute,
//execute, and transmit the Software, and to prepare derivative works of the
//Software, and to permit third - parties to whom the Software is furnished to
//do so, all subject to the following:
// The copyright notices in the Software and this entire statement, including
//the above license grant, this restriction and the following disclaimer,
//must be included in all copies of the Software, in whole or in part, and
//all derivative works of the Software, unless such copies or derivative
//works are solely in the form of machine-executable object code generated by
//a source language processor.
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON - INFRINGEMENT.IN NO EVENT
//SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
//FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
//ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
//DEALINGS IN THE SOFTWARE.
//============================================================================
double ax, ay, az;
if (RotationOrder == "XYZ" || RotationOrder == "zyx")
{
if (GeometRi3D.Smaller(r[0, 2], 1))
{
if (GeometRi3D.Greater(r[0, 2], -1))
{
ay = Asin(r[0, 2]);
ax = Atan2(-r[1, 2], r[2, 2]);
az = Atan2(-r[0, 1], r[0, 0]);
}
else
{
ay = -PI / 2.0;
ax = -Atan2(r[1, 0], r[1, 1]);
az = 0;
}
}
else
{
ay = PI / 2.0;
ax = Atan2(r[1, 0], r[1, 1]);
az = 0;
}
if (RotationOrder == "XYZ")
{
return(new[] { ax, ay, az });
}
else
{
return(new[] { az, ay, ax });
}
}
if (RotationOrder == "XZY" || RotationOrder == "yzx")
{
if (GeometRi3D.Smaller(r[0, 1], 1))
{
if (GeometRi3D.Greater(r[0, 1], -1))
{
az = Asin(-r[0, 1]);
ax = Atan2(r[2, 1], r[1, 1]);
ay = Atan2(r[0, 2], r[0, 0]);
}
else
{
az = PI / 2.0;
ax = -Atan2(-r[2, 0], r[2, 2]);
ay = 0;
}
}
else
{
az = -PI / 2.0;
ax = Atan2(-r[2, 0], r[2, 2]);
ay = 0;
}
if (RotationOrder == "XZY")
{
return(new[] { ax, az, ay });
}
else
{
return(new[] { ay, az, ax });
}
}
if (RotationOrder == "YXZ" || RotationOrder == "zxy")
{
if (GeometRi3D.Smaller(r[1, 2], 1))
{
if (GeometRi3D.Greater(r[1, 2], -1))
{
ax = Asin(-r[1, 2]);
ay = Atan2(r[0, 2], r[2, 2]);
az = Atan2(r[1, 0], r[1, 1]);
}
else
{
ax = PI / 2.0;
ay = -Atan2(-r[0, 2], r[0, 0]);
az = 0;
}
}
else
{
ax = -PI / 2.0;
ay = Atan2(-r[0, 1], r[0, 0]);
az = 0;
}
if (RotationOrder == "YXZ")
{
return(new[] { ay, ax, az });
}
else
{
return(new[] { az, ax, ay });
}
}
if (RotationOrder == "YZX" || RotationOrder == "xzy")
{
if (GeometRi3D.Smaller(r[1, 0], 1))
{
if (GeometRi3D.Greater(r[1, 0], -1))
{
az = Asin(r[1, 0]);
ay = Atan2(-r[2, 0], r[0, 0]);
ax = Atan2(-r[1, 2], r[1, 1]);
}
else
{
az = -PI / 2.0;
ay = -Atan2(r[2, 1], r[2, 2]);
ax = 0;
}
}
else
{
az = PI / 2.0;
ay = Atan2(r[2, 1], r[2, 2]);
ax = 0;
}
if (RotationOrder == "YZX")
{
return(new[] { ay, az, ax });
}
else
{
return(new[] { ax, az, ay });
}
}
if (RotationOrder == "ZXY" || RotationOrder == "yxz")
{
if (GeometRi3D.Smaller(r[2, 1], 1))
{
if (GeometRi3D.Greater(r[2, 1], -1))
{
ax = Asin(r[2, 1]);
az = Atan2(-r[0, 1], r[1, 1]);
ay = Atan2(-r[2, 0], r[2, 2]);
}
else
{
ax = -PI / 2.0;
az = -Atan2(r[0, 2], r[0, 0]);
ay = 0;
}
}
else
{
ax = PI / 2.0;
az = Atan2(r[0, 2], r[0, 0]);
ay = 0;
}
if (RotationOrder == "ZXY")
{
return(new[] { az, ax, ay });
}
else
{
return(new[] { ay, ax, az });
}
}
if (RotationOrder == "ZYX" || RotationOrder == "xyz")
{
if (GeometRi3D.Smaller(r[2, 0], 1))
{
if (GeometRi3D.Greater(r[2, 0], -1))
{
ay = Asin(-r[2, 0]);
az = Atan2(r[1, 0], r[0, 0]);
ax = Atan2(r[2, 1], r[2, 2]);
}
else
{
ay = PI / 2.0;
az = -Atan2(-r[1, 2], r[1, 1]);
ax = 0;
}
}
else
{
ay = -PI / 2.0;
az = Atan2(-r[1, 2], r[1, 1]);
ax = 0;
}
if (RotationOrder == "ZYX")
{
return(new[] { az, ay, ax });
}
else
{
return(new[] { ax, ay, az });
}
}
double a1, a2, a3;
if (RotationOrder == "XYX" || RotationOrder == "xyx")
{
if (GeometRi3D.Smaller(r[0, 0], 1))
{
if (GeometRi3D.Greater(r[0, 0], -1))
{
a2 = Acos(r[0, 0]);
a1 = Atan2(r[1, 0], -r[2, 0]);
a3 = Atan2(r[0, 1], r[0, 2]);
}
else
{
a2 = PI;
a1 = -Atan2(-r[1, 2], r[1, 1]);
a3 = 0;
}
}
else
{
a2 = 0;
a1 = Atan2(-r[1, 2], r[1, 1]);
a3 = 0;
}
if (RotationOrder == "XYX")
{
return(new[] { a1, a2, a3 });
}
else
{
return(new[] { a3, a2, a1 });
}
}
if (RotationOrder == "XZX" || RotationOrder == "xzx")
{
if (GeometRi3D.Smaller(r[0, 0], 1))
{
if (GeometRi3D.Greater(r[0, 0], -1))
{
a2 = Acos(r[0, 0]);
a1 = Atan2(r[2, 0], r[1, 0]);
a3 = Atan2(r[0, 2], -r[0, 1]);
}
else
{
a2 = PI;
a1 = -Atan2(-r[2, 1], r[2, 2]);
a3 = 0;
}
}
else
{
a2 = 0;
a1 = Atan2(r[2, 1], r[2, 2]);
a3 = 0;
}
if (RotationOrder == "XZX")
{
return(new[] { a1, a2, a3 });
}
else
{
return(new[] { a3, a2, a1 });
}
}
if (RotationOrder == "YXY" || RotationOrder == "yxy")
{
if (GeometRi3D.Smaller(r[1, 1], 1))
{
if (GeometRi3D.Greater(r[1, 1], -1))
{
a2 = Acos(r[1, 1]);
a1 = Atan2(r[0, 1], r[2, 1]);
a3 = Atan2(r[1, 0], -r[1, 2]);
}
else
{
a2 = PI;
a1 = -Atan2(r[0, 2], r[0, 0]);
a3 = 0;
}
}
else
{
a2 = 0;
a1 = Atan2(r[0, 2], r[0, 0]);
a3 = 0;
}
if (RotationOrder == "YXY")
{
return(new[] { a1, a2, a3 });
}
else
{
return(new[] { a3, a2, a1 });
}
}
if (RotationOrder == "YZY" || RotationOrder == "yzy")
{
if (GeometRi3D.Smaller(r[1, 1], 1))
{
if (GeometRi3D.Greater(r[1, 1], -1))
{
a2 = Acos(r[1, 1]);
a1 = Atan2(r[2, 1], -r[0, 1]);
a3 = Atan2(r[1, 2], r[1, 0]);
}
else
{
a2 = PI;
a1 = -Atan2(-r[2, 0], r[2, 2]);
a3 = 0;
}
}
else
{
a2 = 0;
a1 = Atan2(-r[2, 0], r[2, 2]);
a3 = 0;
}
if (RotationOrder == "YZY")
{
return(new[] { a1, a2, a3 });
}
else
{
return(new[] { a3, a2, a1 });
}
}
if (RotationOrder == "ZXZ" || RotationOrder == "zxz")
{
if (GeometRi3D.Smaller(r[2, 2], 1))
{
if (GeometRi3D.Greater(r[2, 2], -1))
{
a2 = Acos(r[2, 2]);
a1 = Atan2(r[0, 2], -r[1, 2]);
a3 = Atan2(r[2, 0], r[2, 1]);
}
else
{
a2 = PI;
a1 = -Atan2(-r[0, 1], r[0, 0]);
a3 = 0;
}
}
else
{
a2 = 0;
a1 = Atan2(-r[0, 1], r[0, 0]);
a3 = 0;
}
if (RotationOrder == "ZXZ")
{
return(new[] { a1, a2, a3 });
}
else
{
return(new[] { a3, a2, a1 });
}
}
if (RotationOrder == "ZYZ" || RotationOrder == "zyz")
{
if (GeometRi3D.Smaller(r[2, 2], 1))
{
if (GeometRi3D.Greater(r[2, 2], -1))
{
a2 = Acos(r[2, 2]);
a1 = Atan2(r[1, 2], r[0, 2]);
a3 = Atan2(r[2, 1], -r[2, 0]);
}
else
{
a2 = PI;
a1 = -Atan2(r[1, 0], r[1, 1]);
a3 = 0;
}
}
else
{
a2 = 0;
a1 = Atan2(r[1, 0], r[1, 1]);
a3 = 0;
}
if (RotationOrder == "ZYZ")
{
return(new[] { a1, a2, a3 });
}
else
{
return(new[] { a3, a2, a1 });
}
}
throw new ArgumentException("Invalid parameter: RotationOrder");
}
/// <summary>
/// Rotate triangle around point 'p' as a rotation center
/// </summary>
public Triangle Rotate(Rotation r, Point3d p)
{
return(new Triangle(_a.Rotate(r, p), _b.Rotate(r, p), _c.Rotate(r, p)));
}
/// <summary>
/// Rotate ray around point 'p' as a rotation center
/// </summary>
public Ray3d Rotate(Rotation r, Point3d p)
{
return(new Ray3d(this.Point.Rotate(r, p), this.Direction.Rotate(r)));
}
/// <summary>
/// Rotate circle around point 'p' as a rotation center
/// </summary>
public Circle3d Rotate(Rotation r, Point3d p)
{
return new Circle3d(this.Center.Rotate(r, p), this.R, this.Normal.Rotate(r));
}
