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

Merge pull request #563 from jcarpent/devel

Fix sincos for floats
parents f9415524 ffb5f021
//
// Copyright (c) 2015-2016 CNRS
// Copyright (c) 2015-2018 CNRS INRIA
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
// This file is part of Pinocchio
......@@ -21,12 +21,82 @@
#include <cmath>
namespace se3
{
/// Forward declaration
template<typename Scalar> struct SINCOSAlgo;
///
/// \brief Computes sin/cos values of a given input scalar.
///
/// \tparam Scalar Type of the input/output variables
///
/// \param[in] a The input scalar from which we evalute the sin and cos.
/// \param[inout] sa Variable containing the sin of a.
/// \param[inout] ca Variable containing the cos of a.
///
template<typename Scalar>
void SINCOS(const Scalar & a, Scalar * sa, Scalar * ca)
{
SINCOSAlgo<Scalar>::run(a,sa,ca);
}
/// Generic evaluation of sin/cos functions.
template<typename Scalar>
struct SINCOSAlgo
{
static void run(const Scalar & a, Scalar * sa, Scalar * ca)
{
(*sa) = std::sin(a); (*ca) = std::cos(a);
}
};
/// Specific evaluation of sin/cos for double type.
template<>
struct SINCOSAlgo<double>
{
static void run(const double & a, double * sa, double * ca)
{
#ifdef __linux__
#define SINCOS sincos
sincos(a,sa,ca);
#elif __APPLE__
#define SINCOS __sincos
__sincos(a,sa,ca);
#else // if sincos specialization does not exist
#define SINCOS(a,sa,ca) (*sa) = std::sin(a); (*ca) = std::cos(a)
(*sa) = std::sin(a); (*ca) = std::cos(a);
#endif
}
};
/// Specific evaluation of sin/cos for float type.
template<>
struct SINCOSAlgo<float>
{
static void run(const float & a, float * sa, float * ca)
{
#ifdef __linux__
sincosf(a,sa,ca);
#elif __APPLE__
__sincosf(a,sa,ca);
#else // if sincosf specialization does not exist
(*sa) = std::sin(a); (*ca) = std::cos(a);
#endif
}
};
/// Specific evaluation of sin/cos for long double.
template<>
struct SINCOSAlgo<long double>
{
static void run(const long double & a, long double * sa, long double * ca)
{
#ifdef __linux__
sincosl(a,sa,ca);
#else // if sincosl specialization does not exist
(*sa) = std::sin(a); (*ca) = std::cos(a);
#endif
}
};
}
#endif //#ifndef __math_sincos_hpp__
#
# Copyright (c) 2015-2018 CNRS
# Copyright (c) 2015-2018 CNRS INRIA
# Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
#
# This file is part of Pinocchio
......@@ -49,6 +49,10 @@ ENDMACRO(ADD_PINOCCHIO_UNIT_TEST)
# --- RULES -------------------------------------------------------------------
# --- RULES -------------------------------------------------------------------
# Math components
ADD_PINOCCHIO_UNIT_TEST(sincos eigen3)
# Pinocchio features
ADD_PINOCCHIO_UNIT_TEST(tspatial eigen3)
ADD_PINOCCHIO_UNIT_TEST(symmetric eigen3)
ADD_PINOCCHIO_UNIT_TEST(aba eigen3)
......
//
// Copyright (c) 2018 INRIA
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
#include "utils/macros.hpp"
#include "pinocchio/math/sincos.hpp"
#include <cstdlib>
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
template<typename Scalar>
void testSINCOS(int n)
{
for(int k = 0; k < n; ++k)
{
Scalar sin_value, cos_value;
Scalar alpha = (Scalar)std::rand()/(Scalar)RAND_MAX;
se3::SINCOS(alpha,&sin_value,&cos_value);
Scalar sin_value_ref = std::sin(alpha),
cos_value_ref = std::cos(alpha);
BOOST_CHECK(sin_value == sin_value_ref);
BOOST_CHECK(cos_value == cos_value_ref);
}
}
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
BOOST_AUTO_TEST_CASE(test_sincos)
{
#ifndef NDEBUG
const int n = 1e3;
#else
const int n = 1e6;
#endif
testSINCOS<float>(n);
testSINCOS<double>(n);
testSINCOS<long double>(n);
}
BOOST_AUTO_TEST_SUITE_END()
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