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 #ifndef __cplusplus
00043 #error Must use C++ for the type Robot
00044 #endif
00045 #ifndef UTILS_H
00046 #define UTILS_H
00047
00048
00049
00050
00051
00052
00053 #ifndef __UNUSED__
00054
00055 #define __UNUSED__ __attribute__((unused))
00056 #endif
00057
00058 #ifdef _MSC_VER // Microsoft
00059 #pragma warning (disable:4786)
00060 #endif
00061 #include <stdio.h>
00062 #include <vector>
00063 #include <stdexcept>
00064 #define WANT_STRING
00065 #define WANT_STREAM
00066 #define WANT_FSTREAM
00067 #define WANT_MATH
00068
00069
00070 #include "newmatap.h"
00071
00072 #include "newmatio.h"
00073 #include "config.h"
00074
00075 #ifdef use_namespace
00076 namespace ROBOOP {
00077 using namespace NEWMAT;
00078 #endif
00079
00080 static const char header_utils_rcsid[] __UNUSED__ = "$Id: utils.h,v 1.5 2005/07/26 03:22:09 ejt Exp $";
00081
00082 #ifndef __WATCOMC__
00083 using namespace std;
00084 #endif
00085
00086 #ifndef M_PI
00087 #define M_PI 3.14159265358979
00088 #endif
00089
00090
00091
00092 extern Real fourbyfourident[];
00093 extern Real threebythreeident[];
00094
00095
00096
00097 ReturnMatrix x_prod_matrix(const ColumnVector & x);
00098
00099
00100
00101 ReturnMatrix Integ_Trap(const ColumnVector & present, ColumnVector & past, const Real dt);
00102
00103 void Runge_Kutta4(ReturnMatrix (*xdot)(Real time, const Matrix & xin),
00104 const Matrix & xo, Real to, Real tf, int nsteps,
00105 RowVector & tout, Matrix & xout);
00106
00107 void Runge_Kutta4_Real_time(ReturnMatrix (*xdot)(Real time, const Matrix & xin),
00108 const Matrix & xo, Real to, Real tf, int nsteps);
00109
00110 void Runge_Kutta4_Real_time(ReturnMatrix (*xdot)(Real time, const Matrix & xin,
00111 bool & exit, bool & init),
00112 const Matrix & xo, Real to, Real tf, int nsteps);
00113
00114 void odeint(ReturnMatrix (*xdot)(Real time, const Matrix & xin),
00115 Matrix & xo, Real to, Real tf, Real eps, Real h1, Real hmin,
00116 int & nok, int & nbad,
00117 RowVector & tout, Matrix & xout, Real dtsav);
00118
00119 ReturnMatrix sign(const Matrix & x);
00120
00121 short sign(const Real x);
00122
00123
00124 ReturnMatrix trans(const ColumnVector & a);
00125
00126
00127 ReturnMatrix rotx(const Real alpha);
00128 ReturnMatrix roty(const Real beta);
00129 ReturnMatrix rotz(const Real gamma);
00130 ReturnMatrix rotk(const Real theta, const ColumnVector & k);
00131
00132 ReturnMatrix rpy(const ColumnVector & a);
00133 ReturnMatrix eulzxz(const ColumnVector & a);
00134 ReturnMatrix rotd(const Real theta, const ColumnVector & k1, const ColumnVector & k2);
00135
00136
00137
00138 ReturnMatrix irotk(const Matrix & R);
00139 ReturnMatrix irpy(const Matrix & R);
00140 ReturnMatrix ieulzxz(const Matrix & R);
00141
00142 #ifdef use_namespace
00143 }
00144 #endif
00145
00146 #endif
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162