diff options
Diffstat (limited to 'source/blender/ikplugin/intern/itasc_plugin.cpp')
| -rw-r--r-- | source/blender/ikplugin/intern/itasc_plugin.cpp | 38 |
1 files changed, 19 insertions, 19 deletions
diff --git a/source/blender/ikplugin/intern/itasc_plugin.cpp b/source/blender/ikplugin/intern/itasc_plugin.cpp index 72370158723..0f178366b39 100644 --- a/source/blender/ikplugin/intern/itasc_plugin.cpp +++ b/source/blender/ikplugin/intern/itasc_plugin.cpp @@ -428,14 +428,14 @@ static double EulerAngleFromMatrix(const KDL::Rotation& R, int axis) if (t > 16.0*KDL::epsilon) { if (axis == 0) return -KDL::atan2(R(1,2), R(2,2)); - else if(axis == 1) return KDL::atan2(-R(0,2), t); - else return -KDL::atan2(R(0,1), R(0,0)); + else if(axis == 1) return KDL::atan2(-R(0,2), t); + else return -KDL::atan2(R(0,1), R(0,0)); } else { if (axis == 0) return -KDL::atan2(-R(2,1), R(1,1)); - else if(axis == 1) return KDL::atan2(-R(0,2), t); - else return 0.0f; - } + else if(axis == 1) return KDL::atan2(-R(0,2), t); + else return 0.0f; + } } static double ComputeTwist(const KDL::Rotation& R) @@ -474,29 +474,29 @@ static void RemoveEulerAngleFromMatrix(KDL::Rotation& R, double angle, int axis) static void GetEulerXZY(const KDL::Rotation& R, double& X,double& Z,double& Y) { if (fabs(R(0,1)) > 1.0 - KDL::epsilon ) { - X = -KDL::sign(R(0,1)) * KDL::atan2(R(1,2), R(1,0)); - Z = -KDL::sign(R(0,1)) * KDL::PI / 2; - Y = 0.0; + X = -KDL::sign(R(0,1)) * KDL::atan2(R(1,2), R(1,0)); + Z = -KDL::sign(R(0,1)) * KDL::PI / 2; + Y = 0.0; } else { - X = KDL::atan2(R(2,1), R(1,1)); - Z = KDL::atan2(-R(0,1), KDL::sqrt( KDL::sqr(R(0,0)) + KDL::sqr(R(0,2)))); - Y = KDL::atan2(R(0,2), R(0,0)); - } + X = KDL::atan2(R(2,1), R(1,1)); + Z = KDL::atan2(-R(0,1), KDL::sqrt( KDL::sqr(R(0,0)) + KDL::sqr(R(0,2)))); + Y = KDL::atan2(R(0,2), R(0,0)); + } } static void GetEulerXYZ(const KDL::Rotation& R, double& X,double& Y,double& Z) { if (fabs(R(0,2)) > 1.0 - KDL::epsilon ) { - X = KDL::sign(R(0,2)) * KDL::atan2(-R(1,0), R(1,1)); - Y = KDL::sign(R(0,2)) * KDL::PI / 2; - Z = 0.0; + X = KDL::sign(R(0,2)) * KDL::atan2(-R(1,0), R(1,1)); + Y = KDL::sign(R(0,2)) * KDL::PI / 2; + Z = 0.0; } else { - X = KDL::atan2(-R(1,2), R(2,2)); - Y = KDL::atan2(R(0,2), KDL::sqrt( KDL::sqr(R(0,0)) + KDL::sqr(R(0,1)))); - Z = KDL::atan2(-R(0,1), R(0,0)); - } + X = KDL::atan2(-R(1,2), R(2,2)); + Y = KDL::atan2(R(0,2), KDL::sqrt( KDL::sqr(R(0,0)) + KDL::sqr(R(0,1)))); + Z = KDL::atan2(-R(0,1), R(0,0)); + } } #endif |