Пример #1
0
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);
}
Пример #2
0
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;
	}
}
Пример #3
0
// 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
}
Пример #4
0
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;
}