Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048 #ifndef __cplusplus
00049 #error Must use C++ for the type Robot
00050 #endif
00051 #ifndef UTILS_H
00052 #define UTILS_H
00053
00059
00060 static const char header_utils_rcsid[] = "$Id: utils.h,v 1.10 2006/05/16 16:11:15 gourdeau Exp $";
00061
00062 #ifdef _MSC_VER // Microsoft
00063 #pragma warning (disable:4786)
00064 #endif
00065 #include <stdio.h>
00066 #include <limits>
00067 #define WANT_STRING
00068 #define WANT_STREAM
00069 #define WANT_FSTREAM
00070 #define WANT_MATH
00071
00072
00073 #include "newmatap.h"
00074
00075 #include "newmatio.h"
00076
00077 #ifdef use_namespace
00078 namespace ROBOOP {
00079 using namespace NEWMAT;
00080 #endif
00081
00082 #ifndef M_PI
00083 #define M_PI 3.14159265358979
00084 #endif
00085
00086 #define GRAVITY 9.81
00087
00088
00089 extern Real fourbyfourident[];
00090 extern Real threebythreeident[];
00091
00092
00093 inline double deg2rad(const double angle_deg){ return angle_deg*M_PI/180; }
00094 inline double rad2deg(const double angle_rad){ return angle_rad*180/M_PI; }
00095
00096
00097
00098 ReturnMatrix x_prod_matrix(const ColumnVector & x);
00099
00100 ReturnMatrix pinv(const Matrix & M);
00101
00102
00103
00104 ReturnMatrix Integ_Trap(const ColumnVector & present, ColumnVector & past, const Real dt);
00105
00106 void Runge_Kutta4(ReturnMatrix (*xdot)(Real time, const Matrix & xin),
00107 const Matrix & xo, Real to, Real tf, int nsteps,
00108 RowVector & tout, Matrix & xout);
00109
00110 void Runge_Kutta4_Real_time(ReturnMatrix (*xdot)(Real time, const Matrix & xin),
00111 const Matrix & xo, Real to, Real tf, int nsteps);
00112
00113 void Runge_Kutta4_Real_time(ReturnMatrix (*xdot)(Real time, const Matrix & xin,
00114 bool & exit, bool & init),
00115 const Matrix & xo, Real to, Real tf, int nsteps);
00116
00117 void odeint(ReturnMatrix (*xdot)(Real time, const Matrix & xin),
00118 Matrix & xo, Real to, Real tf, Real eps, Real h1, Real hmin,
00119 int & nok, int & nbad,
00120 RowVector & tout, Matrix & xout, Real dtsav);
00121
00122 ReturnMatrix sign(const Matrix & x);
00123
00124 short sign(const Real x);
00125
00126 const double epsilon = 0.0000001;
00127
00128 inline bool isZero(const double x)
00129 {
00130 if ( fabs(x) < epsilon)
00131 {
00132 return true;
00133 }
00134 return false;
00135 }
00136
00137
00138
00139 ReturnMatrix trans(const ColumnVector & a);
00140
00141
00142 ReturnMatrix rotx(const Real alpha);
00143 ReturnMatrix roty(const Real beta);
00144 ReturnMatrix rotz(const Real gamma);
00145 ReturnMatrix rotk(const Real theta, const ColumnVector & k);
00146
00147 ReturnMatrix rpy(const ColumnVector & a);
00148 ReturnMatrix eulzxz(const ColumnVector & a);
00149 ReturnMatrix rotd(const Real theta, const ColumnVector & k1, const ColumnVector & k2);
00150
00151
00152
00153 ReturnMatrix irotk(const Matrix & R);
00154 ReturnMatrix irpy(const Matrix & R);
00155 ReturnMatrix ieulzxz(const Matrix & R);
00156
00157 #ifdef use_namespace
00158 }
00159 #endif
00160
00161 #endif
00162