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.6 2007/11/11 23:57:25 ejt Exp $";
00081
00082 #ifndef M_PI
00083 #define M_PI 3.14159265358979
00084 #endif
00085
00086
00087
00088 extern Real fourbyfourident[];
00089 extern Real threebythreeident[];
00090
00091
00092
00093 ReturnMatrix x_prod_matrix(const ColumnVector & x);
00094
00095
00096
00097 ReturnMatrix Integ_Trap(const ColumnVector & present, ColumnVector & past, const Real dt);
00098
00099 void Runge_Kutta4(ReturnMatrix (*xdot)(Real time, const Matrix & xin),
00100 const Matrix & xo, Real to, Real tf, int nsteps,
00101 RowVector & tout, Matrix & xout);
00102
00103 void Runge_Kutta4_Real_time(ReturnMatrix (*xdot)(Real time, const Matrix & xin),
00104 const Matrix & xo, Real to, Real tf, int nsteps);
00105
00106 void Runge_Kutta4_Real_time(ReturnMatrix (*xdot)(Real time, const Matrix & xin,
00107 bool & exit, bool & init),
00108 const Matrix & xo, Real to, Real tf, int nsteps);
00109
00110 void odeint(ReturnMatrix (*xdot)(Real time, const Matrix & xin),
00111 Matrix & xo, Real to, Real tf, Real eps, Real h1, Real hmin,
00112 int & nok, int & nbad,
00113 RowVector & tout, Matrix & xout, Real dtsav);
00114
00115 ReturnMatrix sign(const Matrix & x);
00116
00117 short sign(const Real x);
00118
00119
00120 ReturnMatrix trans(const ColumnVector & a);
00121
00122
00123 ReturnMatrix rotx(const Real alpha);
00124 ReturnMatrix roty(const Real beta);
00125 ReturnMatrix rotz(const Real gamma);
00126 ReturnMatrix rotk(const Real theta, const ColumnVector & k);
00127
00128 ReturnMatrix rpy(const ColumnVector & a);
00129 ReturnMatrix eulzxz(const ColumnVector & a);
00130 ReturnMatrix rotd(const Real theta, const ColumnVector & k1, const ColumnVector & k2);
00131
00132
00133
00134 ReturnMatrix irotk(const Matrix & R);
00135 ReturnMatrix irpy(const Matrix & R);
00136 ReturnMatrix ieulzxz(const Matrix & R);
00137
00138 #ifdef use_namespace
00139 }
00140 #endif
00141
00142 #endif
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158