21 #define M_PI 3.14159265358979323846 31 #ifndef KNI_MATH_HELPER_FUNCTIONS 32 #define KNI_MATH_HELPER_FUNCTIONS 40 template<
typename _T>
inline short sign(_T x) {
return ( (x<0) ? -1 : 1 ); }
65 template<
typename _T>
inline _T
atan1(_T in1, _T in2) {
67 if(in1==0.0 && in2 != 0.0)
72 if(in1==0.0 && in2 == 0.0)
76 return atan(in2/in1)+
M_PI;
78 if( (in1>0.0) && (in2<0.0) )
79 return atan(in2/in1)+2.0*
M_PI;
85 template<
typename _T>
inline _T
acotan(
const _T in) {
93 template<
typename _T>
inline _T
atan0(
const _T in1,
const _T in2) {
100 template<
typename _T>
inline _T
pow2(
const _T in) {
108 template<
typename _T>
inline _T
rad2deg(
const _T
a) {
109 return a*(180.0/
M_PI);
115 template<
typename _T>
struct unary_rad2deg :
public std::unary_function<_T, _T> {
122 template<
typename _T>
inline _T
deg2rad(
const _T
a) {
123 return a*(
M_PI/180.0);
129 template<
typename _T>
struct unary_deg2rad :
public std::unary_function<_T, _T> {
135 return a - floor( a/(2*
M_PI) )*2*
M_PI;
142 template<
typename _angleT,
typename _encT>
inline _encT
rad2enc(_angleT
const&
angle, _angleT
const& angleOffset, _encT
const& epc, _encT
const& encOffset, _encT
const& rotDir) {
144 _angleT _epc = epc, _rotDir = rotDir, _angleOffset = angleOffset, _encOffset = encOffset;
146 double temp = _encOffset + (_angleOffset-
angle)*_epc*_rotDir/(2*
M_PI);
147 return static_cast<_encT
>( (temp >= 0) ? floor(temp + 0.5) : floor(temp - 0.5) );
149 return static_cast<_encT
>( round( _encOffset + (_angleOffset-angle)*_epc*_rotDir/(2*
M_PI) ) );
156 template<
typename _angleT,
typename _encT>
inline _angleT
enc2rad(_encT
const&
enc, _angleT
const& angleOffset, _encT
const& epc, _encT
const& encOffset, _encT
const& rotDir) {
158 _angleT _epc = epc, _rotDir = rotDir, _angleOffset = angleOffset, _encOffset = encOffset, _enc =
enc;
159 return _angleOffset - (_enc - _encOffset)*2.0*
M_PI/(_epc*_rotDir);
168 v1[0] = acos(cosValue);
170 v2[0] = asin(sinValue);
171 v2[1] =
M_PI - v2[0];
173 for(
int i=0;i<2;++i) {
174 for(
int j=0;j<2;++j) {
178 assert(!
"precondition for findFirstEqualAngle failed -> no equal angles found");
_encT rad2enc(_angleT const &angle, _angleT const &angleOffset, _encT const &epc, _encT const &encOffset, _encT const &rotDir)
_T anglereduce(const _T a)
FloatVector FloatVector * a
_angleT enc2rad(_encT const &enc, _angleT const &angleOffset, _encT const &epc, _encT const &encOffset, _encT const &rotDir)
double findFirstEqualAngle(double cosValue, double sinValue, double tolerance)
_T atan0(const _T in1, const _T in2)