Euler Angle from Matrix

 

 

 

ZYX Euler Angle을 이용한 Rotation Matrix 구하기

public Matrix getMatrix() { double sinA = Math.sin(this._alpha); double cosA = Math.cos(this._alpha); double sinB = Math.sin(this._beta); double cosB = Math.cos(this._beta); double sinC = Math.sin(this._gamma); double cosC = Math.cos(this._gamma); double d00 = cosA * cosB; double d10 = sinA * cosB; double d20 = -sinB; double d01 = cosA * sinB * sinC - sinA * cosC; double d11 = sinA * sinB * sinC + cosA * cosC; double d21 = cosB * sinC; double d02 = cosA * sinB * cosC + sinA * sinC; double d12 = sinA * sinB * cosC - cosA * sinC; double d22 = cosB * cosC; Matrix matrix = Matrix.ofColumnFirst(d00, d10, d20, d01, d11, d21, d02, d12, d22); return matrix; }

 

 

Rotation Matrix 에서 ZYX Euler Angle 구하기

public static EulerZyxRotation of(IRotation orig) { if(orig == null) { throw new IllegalArgumentException("orig is null"); } else if(orig instanceof EulerZyxRotation) { return (EulerZyxRotation)orig; } else if(orig instanceof Rotation) { Rotation epsilon1 = (Rotation)orig; return ofRad(epsilon1.getAlphaRad(), epsilon1.getBetaRad(), epsilon1.getGammaRad()); } else { double epsilon = 0.001D; Matrix matrix = orig.getMatrix(); double d00 = matrix.get(0, 0); double d10 = matrix.get(1, 0); double d20 = matrix.get(2, 0); double d01 = matrix.get(0, 1); double d11 = matrix.get(1, 1); double d21 = matrix.get(2, 1); double d22 = matrix.get(2, 2); double beta = Math.atan2(-d20, Math.sqrt(d00 * d00 + d10 * d10)); if(Double.compare(beta, -0.0D) == 0) { beta = 0.0D; } double alpha; if(!MathUtils.equal(beta, 1.5707963267948966D, 0.001D) && !MathUtils.equal(beta, -1.5707963267948966D, 0.001D)) { alpha = Math.atan2(d10, d00); } else { alpha = 0.0D; } double gamma; if(MathUtils.equal(beta, 1.5707963267948966D, 0.001D)) { gamma = Math.atan2(d01, d11); } else if(MathUtils.equal(beta, -1.5707963267948966D, 0.001D)) { gamma = -Math.atan2(d01, d11); } else { gamma = Math.atan2(d21, d22); } return new EulerZyxRotation(alpha, beta, gamma); } }