Changeset 15 in flair-src for trunk/lib/FlairFilter/src/TrajectoryGenerator2DCircle_impl.cpp
- Timestamp:
- Apr 8, 2016, 3:40:57 PM (9 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/lib/FlairFilter/src/TrajectoryGenerator2DCircle_impl.cpp
r10 r15 31 31 using namespace flair::filter; 32 32 33 TrajectoryGenerator2DCircle_impl::TrajectoryGenerator2DCircle_impl(TrajectoryGenerator2DCircle* self,const LayoutPosition* position,string name) { 34 first_update=true; 35 is_running=false; 36 is_finishing=false; 33 TrajectoryGenerator2DCircle_impl::TrajectoryGenerator2DCircle_impl( 34 TrajectoryGenerator2DCircle *self, const LayoutPosition *position, 35 string name) { 36 first_update = true; 37 is_running = false; 38 is_finishing = false; 37 39 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); 44 51 45 //init matrix46 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); 52 59 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); 57 64 } 58 65 59 60 66 TrajectoryGenerator2DCircle_impl::~TrajectoryGenerator2DCircle_impl() { 61 67 delete output; 62 68 } 63 69 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; 70 void 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; 69 76 70 //configure trajectory71 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; 73 80 } 74 81 75 82 void 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 } 80 87 } 81 88 82 89 void TrajectoryGenerator2DCircle_impl::Update(Time time) { 83 84 85 float V=veloctity->Value();86 float A=acceleration->Value();87 float R=rayon->Value();88 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; 89 96 90 if(V<0) A=-A; 97 if (V < 0) 98 A = -A; 91 99 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); 97 138 } 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); 99 148 } 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 } 144 150 } 145 151 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 } 153 161 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); 155 171 }
Note:
See TracChangeset
for help on using the changeset viewer.