2 #include "Integrator.hpp"
5 void Integrator::setBody(RigidBody* body)
10 void Integrator::setEnvironment(BodyEnvironment* env)
15 void Integrator::setInterval(float dt)
20 float Integrator::getInterval()
25 void Integrator::setState(State* s)
30 State* Integrator::getState()
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)
39 Math::tmul33(_s.orient, v, out);
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,
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)
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
60 l2gVector(o2, cg, cg); // cg = l2gNEW(cg) ("cg1")
61 Math::sub3(tmp, cg, tmp); // tmp = cg0 + deltaCG - cg1
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)
69 // A straight euler integration, for reference. Don't use.
70 void Integrator::calcNewInterval()
77 orthonormalize(_s.orient);
83 _body->getAccel(s.acc);
84 l2gVector(_s.orient, s.acc, s.acc);
86 _body->getAngularAccel(s.racc);
87 l2gVector(_s.orient, s.racc, s.racc);
90 rotMatrix(s.rot, dt, rotmat);
91 Math::mmul33(_s.orient, rotmat, s.orient);
93 extrapolatePosition(s.pos, s.v, dt, _s.orient, s.orient);
95 Math::mul3(dt, s.acc, tmp);
96 Math::add3(tmp, s.v, s.v);
98 Math::mul3(dt, s.racc, tmp);
99 Math::add3(tmp, s.rot, s.rot);
108 void Integrator::calcNewInterval()
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 };
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];
120 float *currVel = _s.v;
121 float *currAcc = _s.acc;
122 float *currRot = _s.rot;
123 float *currRac = _s.racc;
125 // First off, sanify the initial orientation
126 orthonormalize(_s.orient);
129 for(i=0; i<NITER; i++) {
131 // extrapolate forward based on current values of the
132 // derivatives and the ORIGINAL values of the
133 // position/orientation.
135 float dt = _dt * TIMESTEP[i];
138 // "add" rotation to orientation (generate a rotation matrix)
140 rotMatrix(currRot, dt, rotmat);
141 Math::mmul33(_s.orient, rotmat, ori[i]);
143 // add velocity to (original!) position
145 for(j=0; j<3; j++) pos[i][j] = _s.pos[j];
146 extrapolatePosition(pos[i], currVel, dt, _s.orient, ori[i]);
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]);
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]);
159 // Tell the environment to generate new forces on the body,
160 // extract the accelerations, and convert to vectors in the
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.
170 stmp.pos[j] = pos[i][j];
171 stmp.v[j] = vel[i][j];
172 stmp.rot[j] = rot[i][j];
175 stmp.orient[j] = ori[i][j];
176 _env->calcForces(&stmp);
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]);
184 // Save the resulting derivatives for the next iteration
186 currVel = vel[i]; currAcc = acc[i];
187 currRot = rot[i]; currRac = rac[i];
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.
196 for(i=0; i<NITER; i++) {
197 float wgt = WEIGHTS[i];
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];
207 derivs.v[i] *= itot; derivs.rot[i] *= itot;
208 derivs.acc[i] *= itot; derivs.racc[i] *= itot;
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
216 // save the starting orientation
218 for(i=0; i<9; i++) orient0[i] = _s.orient[i];
221 rotMatrix(derivs.rot, _dt, rotmat);
222 Math::mmul33(orient0, rotmat, _s.orient);
224 extrapolatePosition(_s.pos, derivs.v, _dt, orient0, _s.orient);
227 Math::mul3(_dt, derivs.acc, tmp);
228 Math::add3(_s.v, tmp, _s.v);
230 Math::mul3(_dt, derivs.racc, tmp);
231 Math::add3(_s.rot, tmp, _s.rot);
234 _s.acc[i] = derivs.acc[i];
235 _s.racc[i] = derivs.racc[i];
238 // Tell the environment about our decision
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.
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)
253 // Normalize the vector, and extract the angle through which we'll
255 float mag = Math::mag3(r);
256 float angle = dt*mag;
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.
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;
272 Math::mul3(1/mag, r, runit);
274 float s = Math::sin(angle);
275 float c = Math::cos(angle);
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;
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 ;
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)
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;
305 Math::unit3(x, x); // x = x/|x|
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|
312 Math::cross3(z, x, y); // y = z cross x
315 }; // namespace yasim