Я нашел лучшее решение, которое учитывает поворот на 0 или 180 градусов.Мое первое решение не будет работать в этом случае.Я нашел решение и java-версию кода на euclideanspace.com .
Вот мой код, получающий матрицу из Transform
и затем вызывающий функцию для получения оси иугол.
double[][] matrix = new double[][]
{
new double[]{ oldTransform.BasisX.X, oldTransform.BasisY.X, oldTransform.BasisZ.X },
new double[]{ oldTransform.BasisX.Y, oldTransform.BasisY.Y, oldTransform.BasisZ.Y },
new double[]{ oldTransform.BasisX.Z, oldTransform.BasisY.Z, oldTransform.BasisZ.Z }
};
GetAxisAngleFromMatrix(matrix, out double angleOfRotation, out XYZ axisOfRotation);
Line rotationLine = Line.CreateUnbound(oldTransform.Origin, axisOfRotation);
Вот математическая функция
public void GetAxisAngleFromMatrix(double[][] m, out double angleOfRotation, out XYZ axisOfRotation)
{
double angle, x, y, z; // variables for result
double epsilon = 0.01; // margin to allow for rounding errors
double epsilon2 = 0.1; // margin to distinguish between 0 and 180 degrees
// optional check that input is pure rotation, 'isRotationMatrix' is defined at:
// https://www.euclideanspace.com/maths/algebra/matrix/orthogonal/rotation/
if ((Math.Abs(m[0][1] - m[1][0]) < epsilon)
&& (Math.Abs(m[0][2] - m[2][0]) < epsilon)
&& (Math.Abs(m[1][2] - m[2][1]) < epsilon))
{
// singularity found
// first check for identity matrix which must have +1 for all terms
// in leading diagonaland zero in other terms
if ((Math.Abs(m[0][1] + m[1][0]) < epsilon2)
&& (Math.Abs(m[0][2] + m[2][0]) < epsilon2)
&& (Math.Abs(m[1][2] + m[2][1]) < epsilon2)
&& (Math.Abs(m[0][0] + m[1][1] + m[2][2] - 3) < epsilon2))
{
// this singularity is identity matrix so angle = 0
angleOfRotation = 0;
axisOfRotation = new XYZ(1, 0, 0);
return;
}
// otherwise this singularity is angle = 180
angle = Math.PI;
double xx = (m[0][0] + 1) / 2;
double yy = (m[1][1] + 1) / 2;
double zz = (m[2][2] + 1) / 2;
double xy = (m[0][1] + m[1][0]) / 4;
double xz = (m[0][2] + m[2][0]) / 4;
double yz = (m[1][2] + m[2][1]) / 4;
if ((xx > yy) && (xx > zz))
{ // m[0][0] is the largest diagonal term
if (xx < epsilon)
{
x = 0;
y = 0.7071;
z = 0.7071;
}
else
{
x = Math.Sqrt(xx);
y = xy / x;
z = xz / x;
}
}
else if (yy > zz)
{ // m[1][1] is the largest diagonal term
if (yy < epsilon)
{
x = 0.7071;
y = 0;
z = 0.7071;
}
else
{
y = Math.Sqrt(yy);
x = xy / y;
z = yz / y;
}
}
else
{ // m[2][2] is the largest diagonal term so base result on this
if (zz < epsilon)
{
x = 0.7071;
y = 0.7071;
z = 0;
}
else
{
z = Math.Sqrt(zz);
x = xz / z;
y = yz / z;
}
}
angleOfRotation = angle;
axisOfRotation = new XYZ(x, y, z); // return 180 deg rotation
return;
}
// as we have reached here there are no singularities so we can handle normally
double s = Math.Sqrt((m[2][1] - m[1][2]) * (m[2][1] - m[1][2])
+ (m[0][2] - m[2][0]) * (m[0][2] - m[2][0])
+ (m[1][0] - m[0][1]) * (m[1][0] - m[0][1])); // used to normalise
if (Math.Abs(s) < 0.001) s = 1;
// prevent divide by zero, should not happen if matrix is orthogonal and should be
// caught by singularity test above, but I've left it in just in case
angle = Math.Acos((m[0][0] + m[1][1] + m[2][2] - 1) / 2);
x = (m[2][1] - m[1][2]) / s;
y = (m[0][2] - m[2][0]) / s;
z = (m[1][0] - m[0][1]) / s;
angleOfRotation = angle;
axisOfRotation = new XYZ(x, y, z);
}