Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- for position in sref_
- ... // get position data
- // start parallel region
- for time in vtwin->trefloc_ // time will be offsetted
- for start_velocity in vtwin->vrefloc_ // start_velocity will be offsetted
- ... // check constraint
- for stop_velocity in vtwin->vrefloc_ // stop_velocity will be offsetted
- ... // check many constraints
- ... // temporarily save 6 values if better than of other stop_velocity
- ... // save the 6 values which proofed to be best to output
- // end parallel region
- // RECURSION ROUTINE
- static void rmss(const FunMapsVel *fmapsvel, const FunMapsPos *fmapspos,
- const Window *vtwin, double const * const sref_, const size_t nv,
- const size_t nt, const size_t ns, const double max_decel,
- const double total_dynmass, ArgOut *matout)
- {
- const double Inf = (const double)mxGetInf();
- double prodiv = 0.;
- size_t k = ns - 1;
- // POSITIONAL LOOP
- while(k--){ // indices from ns - 2 to zero
- clock_t tloop = clock();
- int h;
- // get position dependent values
- const double sk = sref_[k]; // position s
- const double sstep = sref_[k+1] - sk; // current step width
- const double toff_sk = vtwin->toffset_[k]; // time offset at current position
- const double toff_skp1 = vtwin->toffset_[k+1]; // time offset at next position
- const double voff_sk = vtwin->voffset_[k]; // velocity offset at current position
- const double voff_skp1 = vtwin->voffset_[k+1]; // velocity offset at next position
- const double vmax_sk = fmapspos->velolim_at_s_[k]; // velocity limit at current position
- const double vmax_skp1 = fmapspos->velolim_at_s_[k+1]; // velocity limit at next position
- // window time bound at next position
- const double tmax_skp1 = (vtwin->twidth-1/vtwin->tstep_r) + toff_skp1;
- // drag at current position
- const double route_drag_sk = fmapspos->route_drag_at_s_[k]; /
- // check for validity of matrix slice k+1 of costToEnd
- check_previous_mat(matout->costToEnd, k, nv, nt);
- // Check for user interruption
- if (utIsInterruptPending()) {
- mexErrMsgIdAndTxt("MATLAB:dprm:userinterrupt",
- "User interrupt detected.");
- }
- #if PARALLEL
- #pragma omp parallel for private(h) num_threads(8) schedule(dynamic)
- #endif
- for(h = 0; h < (int)nt; h++){
- size_t mk;
- double tk;
- // current time
- tk = vtwin->trefloc_[h] + toff_sk;
- // INITIAL VELOCITY LOOP
- for(mk = 0; mk < nv-1; mk++){
- size_t mkp1 = nv-1;
- size_t tmp_iv, tmp_it;
- double tmp_f, tmp_e, tmp_t, tmp_c = Inf;
- // current velocity
- const double vk = vtwin->vrefloc_[mk] + voff_sk;
- // check for velocity limit exceedance
- if(vk > vmax_sk)
- break;
- // TARGET VELOCITY LOOP
- while(mkp1--){ // 100 % executions
- size_t t_skp1_idx, vmap_vq_idx, vmap_vk_idx, fmap_freq_idx;
- double dv, vq, dt, aq, tkp1, F_DRAG, F_REQ,
- F_TRACTION_TRAIN, dE, cost, new_cost;
- // next velocity
- const double vkp1 = vtwin->vrefloc_[mkp1] + voff_skp1;
- // check for velocity limit exceedance
- if(vkp1 > vmax_skp1)
- continue;
- // --- DISCRETE SOLUTION OF EQUATION OF MOTION ---
- // >>> mp * a = - Drag(sk, vk) + F_trac
- // Difference equations
- dv = vkp1 - vk; // velocity stepwidth of current step
- vq = (vkp1 + vk) * 0.5; // average velocity on transition
- dt = sstep / vq; // time of transition
- aq = dv / dt; // average acceleration on transition
- // Check celeration constraints
- if(aq < -max_decel){
- break; // 2.4 % of inner loop executions
- }
- tkp1 = dt + tk;
- // Check for being within time window
- if(tkp1 < toff_skp1){
- continue;
- }
- if(tkp1 > tmax_skp1){
- break;
- }
- // Compute time index
- t_skp1_idx = (size_t)(((tkp1-toff_skp1) * vtwin->tstep_r) + 0.5);
- // CHECK BOUNDS - t_skp1_idx
- // (to prevent segfaults)
- if(t_skp1_idx >= nt){
- break;
- }
- // Check for valid follow up state
- if(matout->costToEnd[IND3(mkp1, t_skp1_idx, k+1, nv, nt)] == Inf){
- continue; // 24.4 % of inner loop executions
- }
- // TRACTION FORCE COMPUTATION
- // Get index for average velocity
- vmap_vq_idx = (size_t)(vq*fmapsvel->vmapstep_r + 0.5);
- // Compute total drag force: F(s, v)
- F_DRAG = fmapsvel->train_drag_at_vq_[vmap_vq_idx] + route_drag_sk;
- // ----- Equation of motion -----
- // Compute required transition traction force
- // F = m*p*a + Drag(s, v)
- F_REQ = total_dynmass * aq + F_DRAG;
- // Differentiate + / - traction force
- if(F_REQ > 0){
- // stop iteration if maximum transmittable force is exceeded
- if(F_REQ > fmapsvel->ftransmax){
- continue; // 8.3 % of inner loop executions
- }
- // Get index for step divisible velocity
- vmap_vk_idx = (size_t)(MAX(vkp1, vk) * fmapsvel->vmapstep_r + 0.5);
- // Get max available traction force for max transition velocity
- F_TRACTION_TRAIN = fmapsvel->trac_force_at_vq_[vmap_vk_idx];
- if(F_REQ > F_TRACTION_TRAIN){
- continue; // 10 % of inner loop executions
- }
- }
- // Check for maximum negativ transmittable force, so that
- // fmap_freq_idx will be within bounds
- if(F_REQ < -fmapsvel->ftransmax){
- break;
- }
- // --- all fine, acquire transition cost and save ---
- // Get index for discretized traction force
- fmap_freq_idx = (size_t)((F_REQ + fmapsvel->ftransmax) * fmapsvel->fmapstep_r + 0.5);
- // CHECK BOUNDS for fmap_freq_idx
- if(fmap_freq_idx >= fmapsvel->len_costfunc){
- continue;
- }
- // 54.5 % of inner loop executions
- cost = fmapsvel->costFunction[fmap_freq_idx + (vmap_vq_idx * fmapsvel->len_costfunc)];
- dE = cost * dt; // our transition cost value (dE)
- // Compute cost for current state
- new_cost = dE + matout->costToEnd[IND3(mkp1, t_skp1_idx, k + 1, nv, nt)];
- // Replace temporary transition is current transition is better
- if(new_cost < tmp_c){
- tmp_c = new_cost;
- tmp_it = t_skp1_idx;
- tmp_iv = mkp1;
- tmp_f = F_REQ;
- tmp_e = dE;
- tmp_t = dt;
- }
- } // END TARGET VELOCITY LOOP
- // Store transition to output
- if(tmp_c < Inf){
- size_t idx = IND3(mk, h, k, nv, nt);
- matout->costToEnd[idx] = tmp_c;
- matout->optV_ind[idx] = (uint8_T )tmp_iv + 1; // MATLAB index conversion
- matout->optT_ind[idx] = (uint16_T)tmp_it + 1; // by increment
- matout->transTimes[idx] = tmp_t;
- if(matout->save_all){
- matout->transForces[idx] = tmp_f;
- matout->transEnergies[idx] = tmp_e;
- }
- tmp_c = Inf;
- }
- } // END INIT VELOCITY LOOP
- } // END TIME LOOP
- // print loop state
- print_state(tloop, k+1, ns, &prodiv);
- } // END POSITIONAL LOOP
- // END OF RECURSION
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement