Ignore:
Timestamp:
04/08/16 15:40:57 (6 years ago)
Author:
Bayard Gildas
Message:

sources reformatted with flair-format-dir script

File:
1 edited

Legend:

Unmodified
Added
Removed
  • trunk/lib/FlairFilter/src/TrajectoryGenerator2DCircle_impl.cpp

    r10 r15  
    3131using namespace flair::filter;
    3232
    33 TrajectoryGenerator2DCircle_impl::TrajectoryGenerator2DCircle_impl(TrajectoryGenerator2DCircle* self,const LayoutPosition* position,string name) {
    34     first_update=true;
    35     is_running=false;
    36     is_finishing=false;
     33TrajectoryGenerator2DCircle_impl::TrajectoryGenerator2DCircle_impl(
     34    TrajectoryGenerator2DCircle *self, const LayoutPosition *position,
     35    string name) {
     36  first_update = true;
     37  is_running = false;
     38  is_finishing = false;
    3739
    38     //init UI
    39    GroupBox* reglages_groupbox=new GroupBox(position,name);
    40         T=new DoubleSpinBox(reglages_groupbox->NewRow(),"period, 0 for auto"," s",0,1,0.01);
    41         rayon=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"R"," m",0,1000,.1);
    42         veloctity=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"velocity"," m/s",-10,10,1);
    43         acceleration=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"acceleration (absolute)"," m/s²",0,10,.1);
     40  // init UI
     41  GroupBox *reglages_groupbox = new GroupBox(position, name);
     42  T = new DoubleSpinBox(reglages_groupbox->NewRow(), "period, 0 for auto", " s",
     43                        0, 1, 0.01);
     44  rayon = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(), "R", " m", 0,
     45                            1000, .1);
     46  veloctity = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(), "velocity",
     47                                " m/s", -10, 10, 1);
     48  acceleration =
     49      new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),
     50                        "acceleration (absolute)", " m/s²", 0, 10, .1);
    4451
    45     //init matrix
    46     cvmatrix_descriptor* desc=new cvmatrix_descriptor(2,2);
    47     desc->SetElementName(0,0,"pos.x");
    48     desc->SetElementName(0,1,"pos.y");
    49     desc->SetElementName(1,0,"vel.x");
    50     desc->SetElementName(1,1,"vel.y");
    51     output=new cvmatrix(self,desc,floatType,name);
     52  // init matrix
     53  cvmatrix_descriptor *desc = new cvmatrix_descriptor(2, 2);
     54  desc->SetElementName(0, 0, "pos.x");
     55  desc->SetElementName(0, 1, "pos.y");
     56  desc->SetElementName(1, 0, "vel.x");
     57  desc->SetElementName(1, 1, "vel.y");
     58  output = new cvmatrix(self, desc, floatType, name);
    5259
    53     output->SetValue(0,0,0);
    54     output->SetValue(0,1,0);
    55     output->SetValue(1,0,0);
    56     output->SetValue(1,1,0);
     60  output->SetValue(0, 0, 0);
     61  output->SetValue(0, 1, 0);
     62  output->SetValue(1, 0, 0);
     63  output->SetValue(1, 1, 0);
    5764}
    5865
    59 
    6066TrajectoryGenerator2DCircle_impl::~TrajectoryGenerator2DCircle_impl() {
    61     delete output;
     67  delete output;
    6268}
    6369
    64 void TrajectoryGenerator2DCircle_impl::StartTraj(const Vector2D &start_pos,float nb_lap) {
    65     is_running=true;
    66     first_update=true;
    67     is_finishing=false;
    68     this->nb_lap=nb_lap;
     70void TrajectoryGenerator2DCircle_impl::StartTraj(const Vector2D &start_pos,
     71                                                 float nb_lap) {
     72  is_running = true;
     73  first_update = true;
     74  is_finishing = false;
     75  this->nb_lap = nb_lap;
    6976
    70     //configure trajectory
    71     angle_off=atan2(start_pos.y-pos_off.y,start_pos.x-pos_off.x);
    72     CurrentTime=0;
     77  // configure trajectory
     78  angle_off = atan2(start_pos.y - pos_off.y, start_pos.x - pos_off.x);
     79  CurrentTime = 0;
    7380}
    7481
    7582void TrajectoryGenerator2DCircle_impl::FinishTraj(void) {
    76     if(!is_finishing) {
    77         is_finishing=true;
    78         FinishTime=CurrentTime;
    79     }
     83  if (!is_finishing) {
     84    is_finishing = true;
     85    FinishTime = CurrentTime;
     86  }
    8087}
    8188
    8289void TrajectoryGenerator2DCircle_impl::Update(Time time) {
    83     float delta_t;
    84     float theta;
    85     float V=veloctity->Value();
    86     float A=acceleration->Value();
    87     float R=rayon->Value();
    88     Vector2D v;
     90  float delta_t;
     91  float theta;
     92  float V = veloctity->Value();
     93  float A = acceleration->Value();
     94  float R = rayon->Value();
     95  Vector2D v;
    8996
    90     if(V<0) A=-A;
     97  if (V < 0)
     98    A = -A;
    9199
    92     if(T->Value()==0) {
    93         if(first_update) {
    94             first_update=false;
    95             previous_time=time;
    96             return;
     100  if (T->Value() == 0) {
     101    if (first_update) {
     102      first_update = false;
     103      previous_time = time;
     104      return;
     105    } else {
     106      delta_t = (float)(time - previous_time) / 1000000000.;
     107    }
     108  } else {
     109    delta_t = T->Value();
     110  }
     111  previous_time = time;
     112  CurrentTime += delta_t;
     113
     114  if (is_finishing && CurrentTime > FinishTime + V / A)
     115    is_running = false;
     116
     117  if (is_running) {
     118    if (R == 0) {
     119      pos.x = 0;
     120      pos.y = 0;
     121      v.x = 0;
     122      v.y = 0;
     123    } else {
     124      if (CurrentTime < V / A) {
     125        theta = angle_off + A / 2 * CurrentTime * CurrentTime / R;
     126        pos.x = R * cos(theta);
     127        pos.y = R * sin(theta);
     128        v.x = -A * CurrentTime * sin(theta);
     129        v.y = A * CurrentTime * cos(theta);
     130      } else {
     131        if (!is_finishing) {
     132          theta =
     133              angle_off + V * V / (2 * A * R) + (CurrentTime - V / A) * V / R;
     134          pos.x = R * cos(theta);
     135          pos.y = R * sin(theta);
     136          v.x = -V * sin(theta);
     137          v.y = V * cos(theta);
    97138        } else {
    98             delta_t=(float)(time-previous_time)/1000000000.;
     139          theta = angle_off + V * V / (2 * A * R) +
     140                  (FinishTime - V / A) * V / R -
     141                  A / 2 * (FinishTime - CurrentTime) *
     142                      (FinishTime - CurrentTime) / R +
     143                  V * (CurrentTime - FinishTime) / R;
     144          pos.x = R * cos(theta);
     145          pos.y = R * sin(theta);
     146          v.x = -(V + A * (FinishTime - CurrentTime)) * sin(theta);
     147          v.y = (V + A * (FinishTime - CurrentTime)) * cos(theta);
    99148        }
    100     } else {
    101         delta_t=T->Value();
    102     }
    103     previous_time=time;
    104     CurrentTime+=delta_t;
    105 
    106     if(is_finishing && CurrentTime>FinishTime+V/A) is_running=false;
    107 
    108     if(is_running) {
    109         if(R==0) {
    110             pos.x=0;
    111             pos.y=0;
    112             v.x=0;
    113             v.y=0;
    114         } else {
    115             if(CurrentTime<V/A) {
    116                 theta=angle_off+A/2*CurrentTime*CurrentTime/R;
    117                 pos.x=R*cos(theta);
    118                 pos.y=R*sin(theta);
    119                 v.x=-A*CurrentTime*sin(theta);
    120                 v.y=A*CurrentTime*cos(theta);
    121             } else {
    122                 if(!is_finishing) {
    123                     theta=angle_off+V*V/(2*A*R)+(CurrentTime-V/A)*V/R;
    124                     pos.x=R*cos(theta);
    125                     pos.y=R*sin(theta);
    126                     v.x=-V*sin(theta);
    127                     v.y=V*cos(theta);
    128                 } else {
    129                     theta=angle_off+V*V/(2*A*R)+(FinishTime-V/A)*V/R-A/2*(FinishTime-CurrentTime)*(FinishTime-CurrentTime)/R+V*(CurrentTime-FinishTime)/R;
    130                     pos.x=R*cos(theta);
    131                     pos.y=R*sin(theta);
    132                     v.x=-(V+A*(FinishTime-CurrentTime))*sin(theta);
    133                     v.y=(V+A*(FinishTime-CurrentTime))*cos(theta);
    134                 }
    135             }
    136         }
    137 
    138         if(theta-angle_off>=nb_lap*2*PI-(-A/2*(V/A)*(V/A)/R+V*(V/A)/R) && nb_lap>0) {
    139             FinishTraj();
    140         }
    141     } else {
    142         v.x=0;
    143         v.y=0;
     149      }
    144150    }
    145151
    146     //on prend une fois pour toute les mutex et on fait des accès directs
    147     output->GetMutex();
    148     output->SetValueNoMutex(0,0,pos.x+pos_off.x);
    149     output->SetValueNoMutex(0,1,pos.y+pos_off.y);
    150     output->SetValueNoMutex(1,0,v.x+vel_off.x);
    151     output->SetValueNoMutex(1,1,v.y+vel_off.y);
    152     output->ReleaseMutex();
     152    if (theta - angle_off >= nb_lap * 2 * PI - (-A / 2 * (V / A) * (V / A) / R +
     153                                                V * (V / A) / R) &&
     154        nb_lap > 0) {
     155      FinishTraj();
     156    }
     157  } else {
     158    v.x = 0;
     159    v.y = 0;
     160  }
    153161
    154     output->SetDataTime(time);
     162  // on prend une fois pour toute les mutex et on fait des accès directs
     163  output->GetMutex();
     164  output->SetValueNoMutex(0, 0, pos.x + pos_off.x);
     165  output->SetValueNoMutex(0, 1, pos.y + pos_off.y);
     166  output->SetValueNoMutex(1, 0, v.x + vel_off.x);
     167  output->SetValueNoMutex(1, 1, v.y + vel_off.y);
     168  output->ReleaseMutex();
     169
     170  output->SetDataTime(time);
    155171}
Note: See TracChangeset for help on using the changeset viewer.