void Integrator::doRK2Step( tBodyVec& bodies ) {
saveState( bodies );
double halfStep = 0.5*_timeStep;
doEulerStep( bodies, halfStep);
computeAccel( bodies );
incAccState( bodies, 1.0 );
restoreState( bodies );
restoreAccState( bodies );
doEulerStep( bodies, _timeStep);
}
void Integrator::doStep( tBodyVec& bodies ) {
computeAccel( bodies );
switch ( _integratorType ) {
case Euler: {
doEulerStep( bodies, _timeStep );
return;
}
case RK2: {
doRK2Step( bodies );
return;
}
case RK4: {
doRK4Step( bodies );
return;
}
default:
return;
}
}
// This is the lookahead algorithm. From a high level, we simply
// limit our end speed and the next start speed to appropriate limits,
// then further limit them to a speed where reaching a stop is possible.
void Motion::gcode_optimize(GCode& gcode, GCode& nextg)
{
#ifdef LOOKAHEAD
// THIS NEEDS SERIOUS REFACTORIN
if(gcode.optimized)
return;
gcode.optimized = true;
// If this isn't a move, we have nothing to do.
if(gcode[G].isUnused() || gcode[G].getInt() != 1 || gcode.movesteps == 0)
return;
// If the NEXT gcode isn't a move, then we will just recomute our accels
// (assuming the previous move changed them during it's optimization)
if(nextg[G].isUnused() || nextg[G].getInt() != 1 || nextg.movesteps == 0)
{
computeAccel(gcode);
return;
}
// Calculate the requested speed of each axis at its peak during this move,
// including a direction sign. TODO: We calculated this once already, maybe we can store it.
int32_t axisspeeds[NUM_AXES];
int32_t endspeeds[NUM_AXES];
int32_t nextspeeds[NUM_AXES];
int32_t startspeeds[NUM_AXES];
float mult1 = (float)gcode.maxfeed / gcode.actualmm;
float mult2 = (float)nextg.maxfeed / nextg.actualmm;
for(int ax=0;ax<NUM_AXES;ax++)
{
axisspeeds[ax] = (float)((float)gcode.axismovesteps[ax] / AXES[ax].getStepsPerMM()) * mult1;
axisspeeds[ax] *= gcode.axisdirs[ax] ? 1 : -1;
endspeeds[ax] = axisspeeds[ax];
nextspeeds[ax] = (float)((float)nextg.axismovesteps[ax] / AXES[ax].getStepsPerMM()) * mult2;
nextspeeds[ax] *= nextg.axisdirs[ax] ? 1 : -1;
startspeeds[ax] = nextspeeds[ax];
}
int32_t *ends, *starts, *temp;
ends = endspeeds;
starts = startspeeds;
// TODO this is stupid shit.
int MAX_OPTS = (float)AXES[0].getMaxFeed() / (float)AXES[0].getStartFeed();
if(MAX_OPTS < 4)
MAX_OPTS = 4;
int c;
// this algorithm is crap.
for(c=0;c<MAX_OPTS;c++)
{
// Calculate the end speeds (of this move) we can use to meet the start speeds (of the next move).
if(!join_moves(ends,starts) && c >= 1)
break;
temp = ends;
ends = starts;
starts = temp;
}
for(int ax=0;ax<NUM_AXES;ax++)
{
#ifdef DEBUG_OPT
HOST.labelnum("OPTS:",c);
HOST.labelnum("ASP[", ax, false);
HOST.labelnum("]:", axisspeeds[ax],false);
HOST.labelnum("->", endspeeds[ax],false);
HOST.labelnum(", ", startspeeds[ax],false);
HOST.labelnum("->", nextspeeds[ax], false);
HOST.labelnum(" @jump:", AXES[ax].getStartFeed());
#endif
if(labs(endspeeds[ax] - startspeeds[ax]) > AXES[ax].getStartFeed())
{
if(labs(endspeeds[ax] - startspeeds[ax]) - AXES[ax].getStartFeed() > (float)AXES[ax].getStartFeed())
{
#ifdef DEBUG_JUMP
HOST.labelnum("JUMP EXCEED LINE ", gcode.linenum);
#endif
endspeeds[gcode.leading_axis] = AXES[gcode.leading_axis].getStartFeed() / 2;
startspeeds[nextg.leading_axis] = AXES[nextg.leading_axis].getStartFeed() / 2;
}
}
}
// Speed at which the next move's leading axis can reach 0 during the next move.
float speedto0 = AXES[nextg.leading_axis].
getSpeedAtEnd(AXES[nextg.leading_axis].getStartFeed()/2,
nextg.accel,
nextg.movesteps);
// NOT TODO: startspeeds/endspeeds is signed previous to this operation but unsigned
// after. Not important, though.
// If the next move cannot reach 0, limit it so it can.
if(speedto0 < labs(startspeeds[nextg.leading_axis]))
{
#ifdef DEBUG_LAME
HOST.labelnum("st0beg:",speedto0);
HOST.labelnum("beg:",startspeeds[nextg.leading_axis]);
#endif
startspeeds[nextg.leading_axis] = speedto0;
}
// now speedto0 is the maximum speed the primary in the next move can be going. We need the equivalent speed of the primary in this move.
speedto0 = (speedto0 * gcode.axisratio[nextg.leading_axis]);
// If this move isn't moving the primary of the next move, then speedto0 will come out to 0.
// This is obviously incorrect...
if(speedto0 < labs(endspeeds[gcode.leading_axis]))
{
#ifdef DEBUG_LAME
HOST.labelnum("st0end:",speedto0);
HOST.labelnum("end:",endspeeds[gcode.leading_axis]);
#endif
endspeeds[gcode.leading_axis] = speedto0;
}
#ifdef DEBUG_LAME
HOST.labelnum("ef: ", gcode.endfeed, false);
HOST.labelnum("sf: ", gcode.startfeed, false);
#endif
gcode.endfeed = max(gcode.endfeed,labs(endspeeds[gcode.leading_axis]));
nextg.startfeed = max(nextg.startfeed,labs(startspeeds[nextg.leading_axis]));
// because we only lookahead one move, our maximum exit speed has to be either the desired
// exit speed or the speed that the next move can reach to 0 (0+jerk, actually) during.
computeAccel(gcode, &nextg);
#endif // LOOKAHEAD
}
void Leapfrog::run(){
if(!reset && !_forceResetChecked){
shared_ptr<ForceResetter> resetter;
for(const auto& e: scene->engines){ resetter=dynamic_pointer_cast<ForceResetter>(e); if(resetter) break; }
if(!resetter) LOG_WARN("Leapfrog.reset==False and no ForceResetter in Scene.engines! Are you sure this is ok? (proceeding)");
_forceResetChecked=true;
}
#ifdef DUMP_INTEGRATOR
cerr<<std::setprecision(17);
#endif
homoDeform=(scene->isPeriodic ? scene->cell->homoDeform : -1); // -1 for aperiodic simulations
dGradV=scene->cell->nextGradV-scene->cell->gradV;
midGradV=.5*(scene->cell->gradV+scene->cell->nextGradV);
//cerr<<"gradV\n"<<scene->cell->gradV<<"\nnextGradV\n"<<scene->cell->nextGradV<<endl;
dt=scene->dt;
if(homoDeform==Cell::HOMO_GRADV2){
const Matrix3r& pprevL(scene->cell->gradV); const Matrix3r& nnextL(scene->cell->nextGradV);
ImLL4hInv=(Matrix3r::Identity()-dt*(nnextL+pprevL)/4.).inverse();
IpLL4h = Matrix3r::Identity()+dt*(nnextL+pprevL)/4.;
LmL=(nnextL-pprevL);
// https://answers.launchpad.net/yade/+question/247806 : am I really sure about the .5? no, I am not!!
// difference of "spin" (angVel) vectors, which are duals of spin tensor
deltaSpinVec=-.5*leviCivita((.5*(pprevL-pprevL.transpose())).eval())+.5*leviCivita((.5*(nnextL-nnextL.transpose())).eval());
}
const bool isPeriodic(scene->isPeriodic);
/* don't evaluate energy in the first step with non-zero velocity gradient, since kinetic energy will be way off
(meanfield velocity has not yet been applied) */
const bool reallyTrackEnergy=(scene->trackEnergy&&(!isPeriodic || scene->step>0 || scene->cell->gradV==Matrix3r::Identity()));
DemField* dem=dynamic_cast<DemField*>(field.get());
assert(dem);
bool hasGravity(dem->gravity!=Vector3r::Zero());
if(dem->nodes.empty()){
Master::instance().checkApi(/*minApi*/10101,"DemField.nodes is empty; woo.dem.Leapfrog no longer calls DemField.collectNodes() automatically.",/*pyWarn*/false); // can happen in bg thread?
}
size_t size=dem->nodes.size();
const auto& nodes=dem->nodes;
#ifdef WOO_OPENMP
#pragma omp parallel for schedule(guided)
#endif
for(size_t i=0; i<size; i++){
const shared_ptr<Node>& node=nodes[i];
assert(node->hasData<DemData>()); // checked in DemField::selfTest
DemData& dyn(node->getData<DemData>());
// handle clumps
if(dyn.isClumped()) continue; // those particles are integrated via the clump's master node
bool isClump=dyn.isClump();
bool damp=(damping!=0. && !dyn.isDampingSkip());
// useless to compute node force if the value will not be used at all
if(isClump && (!dyn.isBlockedAll() || (dyn.impose && (dyn.impose->what & Impose::READ_FORCE)))){
// accumulates to existing values of dyn.force, dy.torque (normally zero)
ClumpData::forceTorqueFromMembers(node,dyn.force,dyn.torque);
}
Vector3r& f=dyn.force;
Vector3r& t=dyn.torque;
if(unlikely(reallyTrackEnergy)){
if(damp) doDampingDissipation(node);
if(hasGravity) doGravityWork(dyn,*dem);
}
// fluctuation velocity does not contain meanfield velocity in periodic boundaries
// in aperiodic boundaries, it is equal to absolute velocity
// it is only computed later, since acceleration is needed to be known already
Vector3r pprevFluctVel, pprevFluctAngVel;
// whether to use aspherical rotation integration for this body; for no accelerations, spherical integrator is "exact" (and faster)
bool useAspherical=dyn.useAsphericalLeapfrog(); // shorthand for (dyn.isAspherical() && !dyn.isBlockedAllRot())
Vector3r linAccel(Vector3r::Zero()), angAccel(Vector3r::Zero());
// for particles not totally blocked, compute accelerations; otherwise, the computations would be useless
if (!dyn.isBlockedAll()) {
linAccel=computeAccel(f,dyn.mass,dyn);
// fluctuation velocities
if(isPeriodic){
pprevFluctVel=scene->cell->pprevFluctVel(node->pos,dyn.vel,dt);
pprevFluctAngVel=scene->cell->pprevFluctAngVel(dyn.angVel);
} else { pprevFluctVel=dyn.vel; pprevFluctAngVel=dyn.angVel; }
// linear damping
if(damp) nonviscDamp2nd(dt,f,pprevFluctVel,linAccel);
// compute v(t+dt/2)
if(homoDeform==Cell::HOMO_GRADV2) dyn.vel=ImLL4hInv*(LmL*node->pos+IpLL4h*dyn.vel+linAccel*dt);
else dyn.vel+=dt*linAccel; // correction for this case is below
// angular acceleration
if(dyn.inertia!=Vector3r::Zero()){
if(!useAspherical){ // spherical integrator, uses angular velocity
angAccel=computeAngAccel(t,dyn.inertia,dyn);
if(damp) nonviscDamp2nd(dt,t,pprevFluctAngVel,angAccel);
dyn.angVel+=dt*angAccel;
if(homoDeform==Cell::HOMO_GRADV2) dyn.angVel-=deltaSpinVec;
} else { // uses torque
for(int i=0; i<3; i++) if(dyn.isBlockedAxisDOF(i,true)) t[i]=0; // block DOFs here
if(damp) nonviscDamp1st(t,pprevFluctAngVel);
}
}
// in case velocity is imposed, it must be re-applied to cancel effects of forces
// this is not necessary if all DOFs are blocked, since then velocity is not modified
if(dyn.impose && (dyn.impose->what & Impose::VELOCITY)) dyn.impose->velocity(scene,node);
}
else{
// fixed particle, with gradV2: velocity correction, without acceleration
if(homoDeform==Cell::HOMO_GRADV2) dyn.vel=ImLL4hInv*(LmL*node->pos+IpLL4h*dyn.vel);
}
/* adapt node velocity/position in (t+dt/2) to space gradV; must be done before position update, so that particle follows */
// this is for both fixed and free particles, without gradV2
if(homoDeform>=0 && homoDeform!=Cell::HOMO_GRADV2) applyPeriodicCorrections(node,linAccel);
// kinetic energy
// accelerations are needed, therefore not evaluated earlier;
if(unlikely(reallyTrackEnergy)) doKineticEnergy(node,pprevFluctVel,pprevFluctAngVel,linAccel,angAccel);
// update positions from velocities
leapfrogTranslate(node);
#ifdef DUMP_INTEGRATOR
if(!dyn.isBlockedAll()) cerr<<", posNew="<<node->pos<<endl;
#endif
// update orientation from angular velocity (or torque, for aspherical integrator)
if(!useAspherical) leapfrogSphericalRotate(node);
else {
if(dyn.inertia==Vector3r::Zero()) throw std::runtime_error("Leapfrog::run: DemField.nodes["+to_string(i)+"].den.inertia==(0,0,0), but the node wants to use aspherical integrator. Aspherical integrator is selected for non-spherical particles which have at least one rotational DOF free.");
if(!isPeriodic) leapfrogAsphericalRotate(node,t);
else{
// FIXME: add fake torque from rotating space or modify angMom or angVel
leapfrogAsphericalRotate(node,t); //-dyn.inertia.asDiagonal()*node->ori.conjugate()*deltaSpinVec/dt*2);
}
}
// read back forces from the node (before they are reset)
if(dyn.impose && (dyn.impose->what & Impose::READ_FORCE)) dyn.impose->readForce(scene,node);
if(reset){
// apply gravity only to the clump itself (not to the nodes later, in CLumpData::applyToMembers)
dyn.force=(hasGravity && !dyn.isGravitySkip())?(dyn.mass*dem->gravity).eval():Vector3r::Zero();
dyn.torque=Vector3r::Zero();
if(dyn.impose && (dyn.impose->what & Impose::FORCE)) dyn.impose->force(scene,node);
}
// if something is imposed, apply it here
if(dyn.impose && (dyn.impose->what & Impose::VELOCITY)) dyn.impose->velocity(scene,node);
// for clumps, update positions/orientations of members as well
// (gravity already applied to the clump node itself, pass zero here! */
if(isClump) ClumpData::applyToMembers(node,/*resetForceTorque*/reset);
}
// if(isPeriodic) prevVelGrad=scene->cell->velGrad;
}