Advertisement
Guest User

Untitled

a guest
Oct 23rd, 2014
141
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 8.73 KB | None | 0 0
  1. for position in sref_
  2. ... // get position data
  3. // start parallel region
  4. for time in vtwin->trefloc_ // time will be offsetted
  5. for start_velocity in vtwin->vrefloc_ // start_velocity will be offsetted
  6. ... // check constraint
  7. for stop_velocity in vtwin->vrefloc_ // stop_velocity will be offsetted
  8. ... // check many constraints
  9. ... // temporarily save 6 values if better than of other stop_velocity
  10. ... // save the 6 values which proofed to be best to output
  11. // end parallel region
  12.  
  13. // RECURSION ROUTINE
  14. static void rmss(const FunMapsVel *fmapsvel, const FunMapsPos *fmapspos,
  15. const Window *vtwin, double const * const sref_, const size_t nv,
  16. const size_t nt, const size_t ns, const double max_decel,
  17. const double total_dynmass, ArgOut *matout)
  18. {
  19. const double Inf = (const double)mxGetInf();
  20. double prodiv = 0.;
  21. size_t k = ns - 1;
  22. // POSITIONAL LOOP
  23. while(k--){ // indices from ns - 2 to zero
  24. clock_t tloop = clock();
  25. int h;
  26. // get position dependent values
  27. const double sk = sref_[k]; // position s
  28. const double sstep = sref_[k+1] - sk; // current step width
  29. const double toff_sk = vtwin->toffset_[k]; // time offset at current position
  30. const double toff_skp1 = vtwin->toffset_[k+1]; // time offset at next position
  31. const double voff_sk = vtwin->voffset_[k]; // velocity offset at current position
  32. const double voff_skp1 = vtwin->voffset_[k+1]; // velocity offset at next position
  33. const double vmax_sk = fmapspos->velolim_at_s_[k]; // velocity limit at current position
  34. const double vmax_skp1 = fmapspos->velolim_at_s_[k+1]; // velocity limit at next position
  35. // window time bound at next position
  36. const double tmax_skp1 = (vtwin->twidth-1/vtwin->tstep_r) + toff_skp1;
  37. // drag at current position
  38. const double route_drag_sk = fmapspos->route_drag_at_s_[k]; /
  39. // check for validity of matrix slice k+1 of costToEnd
  40. check_previous_mat(matout->costToEnd, k, nv, nt);
  41. // Check for user interruption
  42. if (utIsInterruptPending()) {
  43. mexErrMsgIdAndTxt("MATLAB:dprm:userinterrupt",
  44. "User interrupt detected.");
  45. }
  46. #if PARALLEL
  47. #pragma omp parallel for private(h) num_threads(8) schedule(dynamic)
  48. #endif
  49. for(h = 0; h < (int)nt; h++){
  50. size_t mk;
  51. double tk;
  52. // current time
  53. tk = vtwin->trefloc_[h] + toff_sk;
  54. // INITIAL VELOCITY LOOP
  55. for(mk = 0; mk < nv-1; mk++){
  56. size_t mkp1 = nv-1;
  57. size_t tmp_iv, tmp_it;
  58. double tmp_f, tmp_e, tmp_t, tmp_c = Inf;
  59. // current velocity
  60. const double vk = vtwin->vrefloc_[mk] + voff_sk;
  61. // check for velocity limit exceedance
  62. if(vk > vmax_sk)
  63. break;
  64. // TARGET VELOCITY LOOP
  65. while(mkp1--){ // 100 % executions
  66. size_t t_skp1_idx, vmap_vq_idx, vmap_vk_idx, fmap_freq_idx;
  67. double dv, vq, dt, aq, tkp1, F_DRAG, F_REQ,
  68. F_TRACTION_TRAIN, dE, cost, new_cost;
  69. // next velocity
  70. const double vkp1 = vtwin->vrefloc_[mkp1] + voff_skp1;
  71. // check for velocity limit exceedance
  72. if(vkp1 > vmax_skp1)
  73. continue;
  74. // --- DISCRETE SOLUTION OF EQUATION OF MOTION ---
  75. // >>> mp * a = - Drag(sk, vk) + F_trac
  76. // Difference equations
  77. dv = vkp1 - vk; // velocity stepwidth of current step
  78. vq = (vkp1 + vk) * 0.5; // average velocity on transition
  79. dt = sstep / vq; // time of transition
  80. aq = dv / dt; // average acceleration on transition
  81. // Check celeration constraints
  82. if(aq < -max_decel){
  83. break; // 2.4 % of inner loop executions
  84. }
  85. tkp1 = dt + tk;
  86. // Check for being within time window
  87. if(tkp1 < toff_skp1){
  88. continue;
  89. }
  90. if(tkp1 > tmax_skp1){
  91. break;
  92. }
  93. // Compute time index
  94. t_skp1_idx = (size_t)(((tkp1-toff_skp1) * vtwin->tstep_r) + 0.5);
  95. // CHECK BOUNDS - t_skp1_idx
  96. // (to prevent segfaults)
  97. if(t_skp1_idx >= nt){
  98. break;
  99. }
  100. // Check for valid follow up state
  101. if(matout->costToEnd[IND3(mkp1, t_skp1_idx, k+1, nv, nt)] == Inf){
  102. continue; // 24.4 % of inner loop executions
  103. }
  104. // TRACTION FORCE COMPUTATION
  105. // Get index for average velocity
  106. vmap_vq_idx = (size_t)(vq*fmapsvel->vmapstep_r + 0.5);
  107. // Compute total drag force: F(s, v)
  108. F_DRAG = fmapsvel->train_drag_at_vq_[vmap_vq_idx] + route_drag_sk;
  109. // ----- Equation of motion -----
  110. // Compute required transition traction force
  111. // F = m*p*a + Drag(s, v)
  112. F_REQ = total_dynmass * aq + F_DRAG;
  113. // Differentiate + / - traction force
  114. if(F_REQ > 0){
  115. // stop iteration if maximum transmittable force is exceeded
  116. if(F_REQ > fmapsvel->ftransmax){
  117. continue; // 8.3 % of inner loop executions
  118. }
  119. // Get index for step divisible velocity
  120. vmap_vk_idx = (size_t)(MAX(vkp1, vk) * fmapsvel->vmapstep_r + 0.5);
  121. // Get max available traction force for max transition velocity
  122. F_TRACTION_TRAIN = fmapsvel->trac_force_at_vq_[vmap_vk_idx];
  123.  
  124. if(F_REQ > F_TRACTION_TRAIN){
  125. continue; // 10 % of inner loop executions
  126. }
  127. }
  128. // Check for maximum negativ transmittable force, so that
  129. // fmap_freq_idx will be within bounds
  130. if(F_REQ < -fmapsvel->ftransmax){
  131. break;
  132. }
  133. // --- all fine, acquire transition cost and save ---
  134. // Get index for discretized traction force
  135. fmap_freq_idx = (size_t)((F_REQ + fmapsvel->ftransmax) * fmapsvel->fmapstep_r + 0.5);
  136. // CHECK BOUNDS for fmap_freq_idx
  137. if(fmap_freq_idx >= fmapsvel->len_costfunc){
  138. continue;
  139. }
  140. // 54.5 % of inner loop executions
  141. cost = fmapsvel->costFunction[fmap_freq_idx + (vmap_vq_idx * fmapsvel->len_costfunc)];
  142. dE = cost * dt; // our transition cost value (dE)
  143. // Compute cost for current state
  144. new_cost = dE + matout->costToEnd[IND3(mkp1, t_skp1_idx, k + 1, nv, nt)];
  145. // Replace temporary transition is current transition is better
  146. if(new_cost < tmp_c){
  147. tmp_c = new_cost;
  148. tmp_it = t_skp1_idx;
  149. tmp_iv = mkp1;
  150. tmp_f = F_REQ;
  151. tmp_e = dE;
  152. tmp_t = dt;
  153. }
  154. } // END TARGET VELOCITY LOOP
  155.  
  156. // Store transition to output
  157. if(tmp_c < Inf){
  158. size_t idx = IND3(mk, h, k, nv, nt);
  159. matout->costToEnd[idx] = tmp_c;
  160. matout->optV_ind[idx] = (uint8_T )tmp_iv + 1; // MATLAB index conversion
  161. matout->optT_ind[idx] = (uint16_T)tmp_it + 1; // by increment
  162. matout->transTimes[idx] = tmp_t;
  163.  
  164. if(matout->save_all){
  165. matout->transForces[idx] = tmp_f;
  166. matout->transEnergies[idx] = tmp_e;
  167. }
  168. tmp_c = Inf;
  169. }
  170. } // END INIT VELOCITY LOOP
  171. } // END TIME LOOP
  172. // print loop state
  173. print_state(tloop, k+1, ns, &prodiv);
  174. } // END POSITIONAL LOOP
  175. // END OF RECURSION
  176. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement