sincos.hpp

//
// Copyright (c) 2015-2018 CNRS INRIA
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

#ifndef __math_sincos_hpp__
#define __math_sincos_hpp__

#include "pinocchio/fwd.hpp"
#include <cmath>

namespace pinocchio
{
  template<typename Scalar> struct SINCOSAlgo;
  
  
  template<typename Scalar>
  void SINCOS(const Scalar & a, Scalar * sa, Scalar * ca)
  {
    SINCOSAlgo<Scalar>::run(a,sa,ca);
  }
  
  template<typename Scalar>
  struct SINCOSAlgo
  {
    static void run(const Scalar & a, Scalar * sa, Scalar * ca) 
    {   
      (*sa) = std::sin(a); (*ca) = std::cos(a);
    }   
  };  

  template<>
  struct SINCOSAlgo<double>
  {
    static void run(const double & a, double * sa, double * ca) 
    {   
#ifdef __linux__
      sincos(a,sa,ca);
#elif __APPLE__
      __sincos(a,sa,ca);
#else // if sincos specialization does not exist
      (*sa) = std::sin(a); (*ca) = std::cos(a);
#endif
    }   
  };
  
  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
    }
  };
  
  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
    }
  };

#ifdef PINOCCHIO_WITH_CPPAD_SUPPORT
  
  template<typename Scalar>
  struct SINCOSAlgo< CppAD::AD<Scalar> >
  {
    static void run(const CppAD::AD<Scalar> & a, CppAD::AD<Scalar> * sa, CppAD::AD<Scalar> * ca)
    {
      (*sa) = CppAD::sin(a); (*ca) = CppAD::cos(a);
    }
  };
  
#endif
 
}

#endif //#ifndef __math_sincos_hpp__