diff --git a/Engine/source/math/mAngAxis.cpp b/Engine/source/math/mAngAxis.cpp index d7196cdd4..83a5e673f 100644 --- a/Engine/source/math/mAngAxis.cpp +++ b/Engine/source/math/mAngAxis.cpp @@ -34,6 +34,42 @@ AngAxisF & AngAxisF::set( const QuatF & q ) axis.set(1.0f,0.0f,0.0f); return *this; } +AngAxisF& AngAxisF::set(const EulerF& eul) +{ + F32 c1 = mCos(eul.y / 2); + F32 s1 = mSin(eul.y / 2); + F32 c2 = mCos(eul.z / 2); + F32 s2 = mSin(eul.z / 2); + F32 c3 = mCos(eul.x / 2); + F32 s3 = mSin(eul.x / 2); + + F32 c1c2 = c1 * c2; + F32 s1s2 = s1 * s2; + + F32 w = c1c2 * c3 - s1s2 * s3; + F32 x = c1c2 * s3 + s1s2 * c3; + F32 y = s1 * c2 * c3 + c1 * s2 * s3; + F32 z = c1 * s2 * c3 - s1 * c2 * s3; + + angle = 2.0f * mAcos(w); + + F32 norm = x * x + y * y + z * z; + if (norm < POINT_EPSILON) + { + axis.set(1.0f, 0.0f, 0.0f); + } + else + { + norm = mSqrt(norm); + x /= norm; + y /= norm; + z /= norm; + } + + axis.set(x, y, z); + + return *this; +} AngAxisF & AngAxisF::set( const MatrixF & mat ) { @@ -93,3 +129,34 @@ void AngAxisF::RotateZ(F32 angle, const Point3F & from, Point3F * to) mat.mulV(from,to); } +EulerF AngAxisF::toEuler() const +{ + EulerF r; + + F32 s = mSin(angle); + F32 c = mCos(angle); + F32 invc = 1 - c; + + if ((axis.x * axis.y * invc + axis.z * s) > (1 - POINT_EPSILON)) + { + r.y = 2.0f * mAtan2(axis.x * mSin(angle / 2), mCos(angle / 2)); + r.z = -M_HALFPI_F; + r.x = 0.f; + return r; + } + + if ((axis.x * axis.y * invc + axis.z * s) < -(1 - POINT_EPSILON)) + { + r.y = -2.0f * mAtan2(axis.x * mSin(angle / 2), mCos(angle / 2)); + r.z = -M_HALFPI_F; + r.x = 0.f; + return r; + } + + r.x = mAtan2(axis.x * s - axis.y * axis.z * invc, 1.0f - (axis.x * axis.x + axis.z * axis.z) * invc); + r.y = mAtan2(axis.y * s - axis.x * axis.z * invc, 1.0f - (axis.y * axis.y + axis.z * axis.z) * invc); + r.z = mAsin(axis.x * axis.y * invc + axis.z * s); + + return r; + +} diff --git a/Engine/source/math/mAngAxis.h b/Engine/source/math/mAngAxis.h index fd02ad8ec..a5adca3a7 100644 --- a/Engine/source/math/mAngAxis.h +++ b/Engine/source/math/mAngAxis.h @@ -45,6 +45,7 @@ class AngAxisF explicit AngAxisF( const QuatF &q ); AngAxisF& set( const Point3F & _axis, F32 _angle ); + AngAxisF& set( const EulerF & _axis); AngAxisF& set( const MatrixF & m ); AngAxisF& set( const QuatF & q ); @@ -60,6 +61,7 @@ class AngAxisF static void RotateX(F32 angle, const Point3F & from, Point3F * to); static void RotateY(F32 angle, const Point3F & from, Point3F * to); static void RotateZ(F32 angle, const Point3F & from, Point3F * to); + EulerF toEuler() const; }; //---------------------------------------------------------------------------- diff --git a/Engine/source/math/mMath.h b/Engine/source/math/mMath.h index eb816bffe..4ddb78ac4 100644 --- a/Engine/source/math/mMath.h +++ b/Engine/source/math/mMath.h @@ -54,16 +54,14 @@ inline AngAxisF mEulDegToAng(EulerF euler) { - MatrixF tempMat = MatrixF(euler * M_PI_F / 180.0f, Point3F::Zero); - AngAxisF angAx = AngAxisF(tempMat); + AngAxisF angAx; + angAx.set(euler * M_PI_F / 180.0f); return angAx; } inline EulerF mAngToEul(AngAxisF angAx) { - MatrixF tempMat; - angAx.setMatrix(&tempMat); - EulerF euler = tempMat.toEuler(); + EulerF euler = angAx.toEuler(); euler *= 180.0f / M_PI_F; return euler; }