Unverified Commit ea5785ff authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #1145 from jcarpent/devel

Fix potential numerical issue in log3
parents 9c03aa82 ea873e7a
......@@ -45,7 +45,7 @@ ENDIF(WIN32)
# --- OPTIONS ----------------------------------------
OPTION(BUILD_BENCHMARK "Build the benchmarks" OFF)
OPTION(BUILD_UTILS "Build the utils" OFF)
OPTION(BUILD_PYTHON_INTERFACE "Build the python binding" ON)
OPTION(BUILD_PYTHON_INTERFACE "Build the Python binding" ON)
OPTION(BUILD_WITH_COMMIT_VERSION "Build libraries by setting specific commit version" OFF)
IF(DEFINED BUILD_UNIT_TESTS)
......@@ -59,10 +59,10 @@ OPTION(BUILD_ADVANCED_TESTING "Build the advanced tests (multiprecision, etc.) o
# --- OPTIONAL DEPENDENCIES -------------------------
OPTION(BUILD_WITH_URDF_SUPPORT "Build the library with the URDF format support" ON)
OPTION(BUILD_WITH_COLLISION_SUPPORT "Build the library with the Collision support (required HPP-FCL)" OFF)
OPTION(BUILD_WITH_AUTODIFF_SUPPORT "Build the library with the Automatic Differentiation support" OFF)
OPTION(BUILD_WITH_CASADI_SUPPORT "Build the library with the CASADI support" OFF)
OPTION(BUILD_WITH_CODEGEN_SUPPORT "Build the library with the Code Generation support" OFF)
OPTION(BUILD_WITH_COLLISION_SUPPORT "Build the library with the collision support (required HPP-FCL)" OFF)
OPTION(BUILD_WITH_AUTODIFF_SUPPORT "Build the library with the automatic differentiation support (via CppAD)" OFF)
OPTION(BUILD_WITH_CASADI_SUPPORT "Build the library with the support of CASADI" OFF)
OPTION(BUILD_WITH_CODEGEN_SUPPORT "Build the library with the support of code generation (via CppADCodeGen)" OFF)
OPTION(INITIALIZE_WITH_NAN "Initialize Eigen entries with NaN" OFF)
......@@ -159,9 +159,9 @@ IF(BUILD_WITH_HPP_FCL_SUPPORT)
ERROR_QUIET)
IF(_hpp_fcl_python_bindings_not_found EQUAL 0)
SET(BUILD_WITH_HPP_FCL_PYTHON_BINDINGS TRUE)
MESSAGE(STATUS "Found hpp-fcl python bindings.")
MESSAGE(STATUS "Found hpp-fcl Python bindings.")
ELSE()
MESSAGE(STATUS "Did not find hpp-fcl python bindings.")
MESSAGE(STATUS "hpp-fcl Python bindings NOT found.")
ENDIF()
UNSET(_hpp_fcl_python_bindings_not_found)
ENDIF(BUILD_PYTHON_INTERFACE)
......
......@@ -25,10 +25,19 @@ namespace pinocchio
static const Scalar PI_value = PI<Scalar>();
const Scalar tr = R.trace();
if(tr >= Scalar(3)) theta = Scalar(0); // acos((3-1)/2)
else if(tr <= Scalar(-1)) theta = PI_value; // acos((-1-1)/2)
else theta = math::acos((tr - Scalar(1))/Scalar(2));
Scalar tr = R.trace();
if(tr >= Scalar(3))
{
tr = Scalar(3); // clip value
theta = Scalar(0); // acos((3-1)/2)
}
else if(tr <= Scalar(-1))
{
tr = Scalar(-1); // clip value
theta = PI_value; // acos((-1-1)/2)
}
else
theta = math::acos((tr - Scalar(1))/Scalar(2));
assert(theta == theta && "theta contains some NaN"); // theta != NaN
Vector3Out & res_ = PINOCCHIO_EIGEN_CONST_CAST(Vector3Out,res);
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment