// using the specified orientation.
void Integrator::l2gVector(float* orient, float* v, float* out)
{
// using the specified orientation.
void Integrator::l2gVector(float* orient, float* v, float* out)
{
// First off, sanify the initial orientation
orthonormalize(_s.orient);
// First off, sanify the initial orientation
orthonormalize(_s.orient);
Math::mmul33(_s.orient, rotmat, ori[i]);
// add velocity to (original!) position
Math::mmul33(_s.orient, rotmat, ori[i]);
// add velocity to (original!) position
extrapolatePosition(pos[i], currVel, dt, _s.orient, ori[i]);
// add acceleration to (original!) velocity
extrapolatePosition(pos[i], currVel, dt, _s.orient, ori[i]);
// add acceleration to (original!) velocity
derivs.v[j] += wgt*vel[i][j]; derivs.rot[j] += wgt*rot[i][j];
derivs.acc[j] += wgt*acc[i][j]; derivs.racc[j] += wgt*rac[i][j];
}
}
float itot = 1/tot;
derivs.v[j] += wgt*vel[i][j]; derivs.rot[j] += wgt*rot[i][j];
derivs.acc[j] += wgt*acc[i][j]; derivs.racc[j] += wgt*rac[i][j];
}
}
float itot = 1/tot;
Math::mul3(_dt, derivs.racc, tmp);
Math::add3(_s.rot, tmp, _s.rot);
Math::mul3(_dt, derivs.racc, tmp);
Math::add3(_s.rot, tmp, _s.rot);