3
\$\begingroup\$

Here's a function doing some trajectory optimization in the following manner:

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

The function tries to find optimal transitions according to the transition properties and some cost function which is evaluated in the innermost loop (by reading from array). The percentage numbers in the code provide information about how often (it's not about time) a line is executed with respect to the total number of executions of the innermost loop.

The function has changed since I asked for it being reviewed here. I tried to give it more structure (by using structures) but that's it already. Somehow I don't see parts that could be split up. If I ...

  • ...put both velocity loops in a separate function, I'd have to pass such a big bunch of variables to the new function.
  • ...put only the inner most loop in a seperate function, I'd have to pass a big bunch of variables and create an array for returning 6 return values, which are stored by the outer velocity loop (see pseudo code).
  • ...put some tiny parts of the inner most loop in subfunctions it does not help that much. Many values are used several times. Constraint checking cannot be sourced out, can it? And even I'm not sure, if everything will be inlined, cause a tried already and I see a slight impact on execution time, even though not much.

Maybe it'll be a bit easier to review now, even thought it's not well-structured.

// 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
}
\$\endgroup\$
1
\$\begingroup\$

First thing you can do is more descriptive variable names. I know naming things is hard but please don't go so cryptic.

Second putting your tmp_* in "INITIAL VELOCITY LOOP" in a struct will be much clearer and allows you to put the "TARGET VELOCITY LOOP" in a separate function that only returns that struct (without needing to remember the order of the return values).

\$\endgroup\$
  • \$\begingroup\$ yeah, the naming. Probably I fear verbosity so that clearity suffers a lot from it. Good to be told. \$\endgroup\$ – embert Oct 23 '14 at 11:19
  • \$\begingroup\$ By now, after re-factoring I'm certain: reasonable naming does make a big difference. For long var names, if in doubt, I force myself to take the long version. In some cases, parts of the name can be shortened as the context makes it clear. It's also helpful to be consistent with naming conventions, e.g. using camel case for this and lower case with underscores for that. The "modularization" you suggested has been not only applied to tmp_* but I also sourced out a number of things to functions. As long as all performance critical parts belong to the same compilation unit it seems just fine \$\endgroup\$ – embert Feb 1 '15 at 16:17

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.