38 #define IMETHOD __forceinline 41 #define IMETHOD inline 51 #ifdef KDL_INDEX_CHECK 52 #define FRAMES_CHECKI(a) assert(a) 54 #define FRAMES_CHECKI(a) 83 #pragma warning (disable:4786) 85 inline double sin(
double a) {
89 inline double cos(
double a) {
92 inline double exp(
double a) {
95 inline double log(
double a) {
98 inline double tan(
double a) {
122 inline double pow(
double a,
double b) {
125 inline double atan2(
double a,
double b) {
130 #if (__cplusplus > 199711L) 180 extern const double PI;
183 extern const double PI_2;
186 extern const double PI_4;
204 inline double max(
double a,
double b) {
212 inline double min(
double a,
double b) {
224 #define INLINE __forceinline 227 #define INLINE inline 232 double beta,
double b ) {
233 return alfa*a+beta*b;
237 double beta,
double b,
double& result ) {
238 result=alfa*a+beta*b;
251 inline double sign(
double arg) {
252 return (arg<0)?(-1):(1);
255 inline double sqr(
double arg) {
return arg*arg;}
256 inline double Norm(
double arg) {
257 return fabs( (
double)arg );
260 #if defined __WIN32__ && !defined __GNUC__ 261 inline double hypot(
double y,
double x) { return ::_hypot(y,x);}
262 inline double abs(
double x) { return ::fabs(x);}
268 inline bool Equal(
double a,
double b,
double eps=epsilon)
271 return ((eps>tmp)&& (tmp>-eps) );
275 a = 1.98*rand()/(double)RAND_MAX -0.99;
279 a = 0.001+0.99*rand()/(double)RAND_MAX;
282 inline double diff(
double a,
double b,
double dt) {
288 inline double addDelta(
double a,
double da,
double dt) {
const double rad2deg
the value 180/pi
INLINE S Norm(const Rall1d< T, V, S > &value)
const double PI_2
the value of pi/2
double atan2(double a, double b)
INLINE void LinCombR(S alfa, const Rall1d< T, V, S > &a, const T &beta, const Rall1d< T, V, S > &b, Rall1d< T, V, S > &result)
INLINE Rall1d< T, V, S > log(const Rall1d< T, V, S > &arg)
const double PI
the value of pi
INLINE Rall1d< T, V, S > cosh(const Rall1d< T, V, S > &arg)
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
INLINE Rall1d< T, V, S > hypot(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
int VSIZE
the number of derivatives used in the RN-... objects.
INLINE Rall1d< T, V, S > sqr(const Rall1d< T, V, S > &arg)
const T & Arg
Arg is used for passing the element to a function.
INLINE Rall1d< T, V, S > sinh(const Rall1d< T, V, S > &arg)
int MAXLENFILENAME
maximal length of a file name
INLINE Rall1d< T, V, S > tanh(const Rall1d< T, V, S > &arg)
IMETHOD void SetToZero(Vector &v)
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
INLINE Rall1d< T, V, S > atan(const Rall1d< T, V, S > &x)
double pow(double a, double b)
INLINE Rall1d< T, V, S > asin(const Rall1d< T, V, S > &x)
double epsilon
default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
double min(double a, double b)
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
IMETHOD void posrandom(Vector &a)
INLINE Rall1d< T, V, S > exp(const Rall1d< T, V, S > &arg)
INLINE void SetToIdentity(Rall1d< T, V, S > &value)
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
IMETHOD Vector addDelta(const Vector &p_w_a, const Vector &p_w_da, double dt=1)
adds vector da to vector a. see also the corresponding diff() routine.
const double PI_4
the value of pi/4
const double deg2rad
the value pi/180
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
INLINE Rall1d< T, V, S > tan(const Rall1d< T, V, S > &arg)
double max(double a, double b)
IMETHOD void random(Vector &a)
addDelta operator for displacement rotational velocity.
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
INLINE Rall1d< T, V, S > LinComb(S alfa, const Rall1d< T, V, S > &a, const T &beta, const Rall1d< T, V, S > &b)