32 #include <mt/exception.h>
34 #ifdef MT_USE_BASIC_SCALAR
101 typedef double value_t;
105 typedef float value_t;
112 #ifdef MT_USE_BASIC_SCALAR
114 typedef BasicScalarTraits<value_t> ScalarTraits;
115 typedef BasicScalar<value_t> Scalar;
119 typedef value_t Scalar;
126 const Scalar PI(std::atan2(Scalar(0.0), Scalar(-1.0)));
127 const Scalar HALF_PI(PI / Scalar(2.0));
128 const Scalar TWO_PI (PI * Scalar(2.0));
134 Scalar degToRad(
const Scalar& s);
137 Scalar radToDeg(
const Scalar& s);
140 Scalar sq(
const Scalar& s);
145 Scalar sgn(
const Scalar& s);
168 T normalize(
const T& s,
175 T normalize(
const T& s,
182 T saturate(
const T& s,
193 const T& tol = T(1.0));
201 T asinMt(
const T& sin_ang,
210 T acosMt(
const T& cos_ang,
215 T getValue(
const T& s);
219 inline Scalar degToRad(
const Scalar& s)
221 static const Scalar rads_per_deg(PI / Scalar(180.0));
222 return Scalar(s * rads_per_deg);
226 inline Scalar radToDeg(
const Scalar& s)
228 static const Scalar degs_per_rad(Scalar(180.0) / PI);
229 return Scalar(s * degs_per_rad);
233 inline Scalar sq(
const Scalar& s)
238 inline Scalar sgn(
const Scalar& s)
240 if (s >= Scalar(0.0))
251 template<
class T>
inline
252 T normalize(
const T& s,
257 util::Assert(lower < upper,
258 Exception(
"Normalizing interval is ill defined"));
261 if (s >= lower && s < upper)
267 const T diff(upper - lower);
268 cycles = (s < lower) ?
269 ceil((s - upper) / diff) : floor((s - lower) / diff);
270 s_norm -= (diff * cycles);
276 template<
class T>
inline
277 T normalize(
const T& s,
282 return normalize(s, lower, upper, cycles);
286 template<
class T>
inline
287 T saturate(
const T& s,
291 util::Assert(lower < upper,
292 Exception(
"Saturation interval is ill defined"));
307 template<
class T>
inline
311 const T abs_tol = abs(tol);
312 const T x = s / abs_tol;
313 return T(floor(x + 0.5) * abs_tol);
317 template<
class T>
inline
318 T asinMt(
const T& sin_ang,
322 T val1(asin(sin_ang));
326 val1 = normalize(val1, T(0.0), TWO_PI);
327 Scalar ref_norm(normalize(ref_ang, T(0.0), TWO_PI));
330 T diff1(abs(ref_norm - val1));
331 T diff2(abs(ref_norm - val2));
332 return (diff1 < diff2) ? val1 : val2;
336 template<
class T>
inline
337 T acosMt(
const T& cos_ang,
341 const T val1(acos(cos_ang));
342 const T val2(TWO_PI - val1);
345 Scalar ref_norm(normalize(ref_ang, T(0.0), TWO_PI));
348 T diff1(abs(ref_norm - val1));
349 T diff2(abs(ref_norm - val2));
350 return (diff1 < diff2) ? val1 : val2;
354 template<
class T>
inline
355 T getValue(
const T& s)
362 #endif // MT_SCALAR_H