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