src/FDM/YASim/Integrator.cpp

   1 #include "Math.hpp"
   2 #include "Integrator.hpp"
   3 namespace yasim {
   4
   5 void Integrator::setBody(RigidBody* body)
   6 {
   7     _body = body;
   8 }
   9
  10 void Integrator::setEnvironment(BodyEnvironment* env)
  11 {
  12     _env = env;
  13 }
  14
  15 void Integrator::setInterval(float dt)
  16 {
  17     _dt = dt;
  18 }
  19
  20 float Integrator::getInterval()
  21 {
  22     return _dt;
  23 }
  24
  25 void Integrator::setState(State* s)
  26 {
  27     _s = *s;
  28 }
  29
  30 State* Integrator::getState()
  31 {
  32     return &_s;
  33 }
  34
  35 // Transforms a "local" vector to a "global" vector (not coordinate!)
  36 // using the specified orientation.
  37 void Integrator::l2gVector(float* orient, float* v, float* out)
  38 {
  39     Math::tmul33(_s.orient, v, out);
  40 }
  41
  42 // Updates a position vector for a body c.g. motion with velocity v
  43 // over time dt, from orientation o0 to o1.  Because the position
  44 // references the local coordinate origin, but the velocity is that of
  45 // the c.g., this gets a bit complicated.
  46 void Integrator::extrapolatePosition(double* pos, float* v, float dt,
  47                                      float* o1, float* o2)
  48 {
  49     // Remember that it's the c.g. that's moving, so account for
  50     // changes in orientation.  The motion of the coordinate
  51     // frame will be l2gOLD(cg) + deltaCG - l2gNEW(cg)
  52     float cg[3], tmp[3];
  53
  54     _body->getCG(cg);
  55     l2gVector(o1, cg, cg);    // cg = l2gOLD(cg) ("cg0")
  56     Math::set3(v, tmp);       // tmp = vel
  57     Math::mul3(dt, tmp, tmp); //     = vel*dt ("deltaCG")
  58     Math::add3(cg, tmp, tmp); //     = cg0 + deltaCG
  59     _body->getCG(cg);
  60     l2gVector(o2, cg, cg);    // cg = l2gNEW(cg) ("cg1")
  61     Math::sub3(tmp, cg, tmp); // tmp = cg0 + deltaCG - cg1
  62
  63     pos[0] += tmp[0];         // p1 = p0 + (cg0+deltaCG-cg1)
  64     pos[1] += tmp[1];         //  (positions are doubles, so we
  65     pos[2] += tmp[2];         //   can't use Math::add3)
  66 }
  67
  68 #if 0
  69 // A straight euler integration, for reference.  Don't use.
  70 void Integrator::calcNewInterval()
  71 {
  72     float tmp[3];
  73     State s = _s;
  74
  75     float dt = _dt / 4;
  76
  77     orthonormalize(_s.orient);
  78     int i;
  79     for(i=0; i<4; i++) {
  80         _body->reset();
  81         _env->calcForces(&s);
  82
  83         _body->getAccel(s.acc);
  84         l2gVector(_s.orient, s.acc, s.acc);
  85
  86         _body->getAngularAccel(s.racc);
  87         l2gVector(_s.orient, s.racc, s.racc);
  88
  89         float rotmat[9];
  90         rotMatrix(s.rot, dt, rotmat);
  91         Math::mmul33(_s.orient, rotmat, s.orient);
  92
  93         extrapolatePosition(s.pos, s.v, dt, _s.orient, s.orient);
  94
  95         Math::mul3(dt, s.acc, tmp);
  96         Math::add3(tmp, s.v, s.v);
  97
  98         Math::mul3(dt, s.racc, tmp);
  99         Math::add3(tmp, s.rot, s.rot);
 100
 101         _s = s;
 102     }
 103
 104     _env->newState(&_s);
 105 }
 106 #endif
 107
 108 void Integrator::calcNewInterval()
 109 {
 110     // In principle, these could be changed for something other than
 111     // a 4th order integration.  I doubt if anyone cares.
 112     const static int NITER=4;
 113     static float TIMESTEP[] = { 1.0, 0.5, 0.5, 1.0 };
 114     static float WEIGHTS[]  = { 6.0, 3.0, 3.0, 6.0 };
 115
 116     // Scratch pads:
 117     double pos[NITER][3]; float vel[NITER][3]; float acc[NITER][3];
 118     float  ori[NITER][9]; float rot[NITER][3]; float rac[NITER][3];
 119
 120     float *currVel = _s.v;
 121     float *currAcc = _s.acc;
 122     float *currRot = _s.rot;
 123     float *currRac = _s.racc;
 124
 125     // First off, sanify the initial orientation
 126     orthonormalize(_s.orient);
 127
 128     int i;
 129     for(i=0; i<NITER; i++) {
 130         //
 131         // extrapolate forward based on current values of the
 132         // derivatives and the ORIGINAL values of the
 133         // position/orientation.
 134         //
 135         float dt = _dt * TIMESTEP[i];
 136         float tmp[3];
 137
 138         // "add" rotation to orientation (generate a rotation matrix)
 139         float rotmat[9];
 140         rotMatrix(currRot, dt, rotmat);
 141         Math::mmul33(_s.orient, rotmat, ori[i]);
 142
 143         // add velocity to (original!) position
 144         int j;
 145         for(j=0; j<3; j++) pos[i][j] = _s.pos[j];
 146         extrapolatePosition(pos[i], currVel, dt, _s.orient, ori[i]);
 147
 148         // add acceleration to (original!) velocity
 149         Math::set3(currAcc, tmp);
 150         Math::mul3(dt, tmp, tmp);
 151         Math::add3(_s.v, tmp, vel[i]);
 152
 153         // add rotational acceleration to rotation
 154         Math::set3(currRac, tmp);
 155         Math::mul3(dt, tmp, tmp);
 156         Math::add3(_s.rot, tmp, rot[i]);
 157
 158         //
 159         // Tell the environment to generate new forces on the body,
 160         // extract the accelerations, and convert to vectors in the
 161         // global frame.
 162         //
 163         _body->reset();
 164
 165         // FIXME.  Copying into a state object is clumsy!  The
 166         // per-coordinate arrays should be changed into a single array
 167         // of State objects.  Ick.
 168         State stmp;
 169         for(j=0; j<3; j++) {
 170             stmp.pos[j] = pos[i][j];
 171             stmp.v[j] = vel[i][j];
 172             stmp.rot[j] = rot[i][j];
 173         }
 174         for(j=0; j<9; j++)
 175             stmp.orient[j] = ori[i][j];
 176         _env->calcForces(&stmp);
 177
 178         _body->getAccel(acc[i]);
 179         _body->getAngularAccel(rac[i]);
 180         l2gVector(_s.orient, acc[i], acc[i]);
 181         l2gVector(_s.orient, rac[i], rac[i]);
 182
 183         //
 184         // Save the resulting derivatives for the next iteration
 185         //
 186         currVel = vel[i]; currAcc = acc[i];
 187         currRot = rot[i]; currRac = rac[i];
 188     }
 189
 190     // Average the resulting derivatives together according to their
 191     // weights.  Yes, we're "averaging" rotations, which isn't
 192     // stricly correct -- rotations live in a non-cartesian space.
 193     // But the space is "locally" cartesian.
 194     State derivs;
 195     float tot = 0;
 196     for(i=0; i<NITER; i++) {
 197         float wgt = WEIGHTS[i];
 198         tot += wgt;
 199         int j;
 200         for(j=0; j<3; j++) {
 201             derivs.v[j]   += wgt*vel[i][j];  derivs.rot[j]    += wgt*rot[i][j];
 202             derivs.acc[j] += wgt*acc[i][j];  derivs.racc[j] += wgt*rac[i][j];
 203         }
 204     }
 205     float itot = 1/tot;
 206     for(i=0; i<3; i++) {
 207         derivs.v[i]   *= itot;  derivs.rot[i]    *= itot;
 208         derivs.acc[i] *= itot;  derivs.racc[i] *= itot;
 209     }
 210
 211     // And finally extrapolate once more, using the averaged
 212     // derivatives, to the final position and orientation.  This code
 213     // is essentially identical to the position extrapolation step
 214     // inside the loop.
 215
 216     // save the starting orientation
 217     float orient0[9];
 218     for(i=0; i<9; i++) orient0[i] = _s.orient[i];
 219
 220     float rotmat[9];
 221     rotMatrix(derivs.rot, _dt, rotmat);
 222     Math::mmul33(orient0, rotmat, _s.orient);
 223
 224     extrapolatePosition(_s.pos, derivs.v, _dt, orient0, _s.orient);
 225
 226     float tmp[3];
 227     Math::mul3(_dt, derivs.acc, tmp);
 228     Math::add3(_s.v, tmp, _s.v);
 229
 230     Math::mul3(_dt, derivs.racc, tmp);
 231     Math::add3(_s.rot, tmp, _s.rot);
 232
 233     for(i=0; i<3; i++) {
 234         _s.acc[i] = derivs.acc[i];
 235         _s.racc[i] = derivs.racc[i];
 236     }
 237
 238     // Tell the environment about our decision
 239     _env->newState(&_s);
 240 }
 241
 242 // Generates a matrix that rotates about axis r through an angle equal
 243 // to (|r| * dt).  That is, a rotation effected by rotating with rate
 244 // r for dt "seconds" (or whatever time unit is in use).
 245 // Implementation shamelessly cribbed from the OpenGL specification.
 246 //
 247 // NOTE: we're actually returning the _transpose_ of the rotation
 248 // matrix!  This is becuase we store orientations as global-to-local
 249 // transformations.  Thus, we want to rotate the ROWS of the old
 250 // matrix to get the new one.
 251 void Integrator::rotMatrix(float* r, float dt, float* out)
 252 {
 253     // Normalize the vector, and extract the angle through which we'll
 254     // rotate.
 255     float mag = Math::mag3(r);
 256     float angle = dt*mag;
 257
 258     // Tiny rotations result in degenerate (zero-length) rotation
 259     // vectors, so clamp to an identity matrix.  1e-06 radians
 260     // per 1/30th of a second (typical dt unit) corresponds to one
 261     // revolution per 2.4 days, or significantly less than the
 262     // coriolis rotation.  And it's still preserves half the floating
 263     // point precision of a radian-per-iteration rotation.
 264     if(angle < 1e-06) {
 265         out[0] = 1; out[1] = 0; out[2] = 0;
 266         out[3] = 0; out[4] = 1; out[5] = 0;
 267         out[6] = 0; out[7] = 0; out[8] = 1;
 268         return;
 269     }
 270
 271     float runit[3];
 272     Math::mul3(1/mag, r, runit);
 273
 274     float s    = Math::sin(angle);
 275     float c    = Math::cos(angle);
 276     float c1   = 1-c;
 277     float c1rx = c1*runit[0];
 278     float c1ry = c1*runit[1];
 279     float xx   = c1rx*runit[0];
 280     float xy   = c1rx*runit[1];
 281     float xz   = c1rx*runit[2];
 282     float yy   = c1ry*runit[1];
 283     float yz   = c1ry*runit[2];
 284     float zz   = c1*runit[2]*runit[2];
 285     float xs   = runit[0]*s;
 286     float ys   = runit[1]*s;
 287     float zs   = runit[2]*s;
 288
 289     out[0] = xx+c ; out[3] = xy-zs; out[6] = xz+ys;
 290     out[1] = xy+zs; out[4] = yy+c ; out[7] = yz-xs;
 291     out[2] = xz-ys; out[5] = yz+xs; out[8] = zz+c ;
 292 }
 293
 294 // Does a Gram-Schmidt orthonormalization on the rows of a
 295 // global-to-local orientation matrix.  The order of normalization
 296 // here is x, z, y.  This is because of the convention of "x" being
 297 // the forward vector and "z" being up in the body frame.  These two
 298 // vectors are the most important to keep correct.
 299 void Integrator::orthonormalize(float* m)
 300 {
 301     // The 1st, 2nd and 3rd rows of the matrix store the global frame
 302     // equivalents of the x, y, and z unit vectors in the local frame.
 303     float *x = m, *y = m+3, *z = m+6;
 304
 305     Math::unit3(x, x);                  // x = x/|x|
 306
 307     float v[3];
 308     Math::mul3(Math::dot3(x, z), z, v); // v = z(x dot z)
 309     Math::sub3(z, v, z);                // z = z - z*(x dot z)
 310     Math::unit3(z, z);                  // z = z/|z|
 311
 312     Math::cross3(z, x, y);              // y = z cross x
 313 }
 314
 315 }; // namespace yasim