source: flair-src/trunk/lib/FlairFilter/src/TrajectoryGenerator2DCircle_impl.cpp @ 167

Last change on this file since 167 was 167, checked in by Sanahuja Guillaume, 5 years ago

modifs pour template vectors

File size: 4.8 KB
RevLine 
[10]1// %flair:license{
[15]2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
[10]4// %flair:license}
[7]5//  created:    2013/04/08
6//  filename:   TrajectoryGenerator2DCircle_impl.cpp
7//
8//  author:     Guillaume Sanahuja
9//              Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11//  version:    $Id: $
12//
13//  purpose:    objet permettant la generation d'une trajectoire cercle
14//
15//
16/*********************************************************************/
17
18#include "TrajectoryGenerator2DCircle_impl.h"
19#include "TrajectoryGenerator2DCircle.h"
20#include <cvmatrix.h>
21#include <Layout.h>
22#include <GroupBox.h>
23#include <DoubleSpinBox.h>
24#include <cmath>
25
26#define PI ((float)3.14159265358979323846)
27
28using std::string;
29using namespace flair::core;
30using namespace flair::gui;
31using namespace flair::filter;
32
[15]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;
[7]39
[15]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);
[7]51
[15]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);
[148]59  delete desc;
[7]60}
61
62TrajectoryGenerator2DCircle_impl::~TrajectoryGenerator2DCircle_impl() {
[15]63  delete output;
[7]64}
65
[167]66void TrajectoryGenerator2DCircle_impl::StartTraj(const Vector2Df &start_pos,
[15]67                                                 float nb_lap) {
68  is_running = true;
69  first_update = true;
70  is_finishing = false;
71  this->nb_lap = nb_lap;
[7]72
[15]73  // configure trajectory
74  angle_off = atan2(start_pos.y - pos_off.y, start_pos.x - pos_off.x);
75  CurrentTime = 0;
[7]76}
77
78void TrajectoryGenerator2DCircle_impl::FinishTraj(void) {
[15]79  if (!is_finishing) {
80    is_finishing = true;
81    FinishTime = CurrentTime;
82  }
[7]83}
84
85void TrajectoryGenerator2DCircle_impl::Update(Time time) {
[15]86  float delta_t;
87  float theta;
88  float V = veloctity->Value();
89  float A = acceleration->Value();
90  float R = rayon->Value();
[167]91  Vector2Df v;
[7]92
[15]93  if (V < 0)
94    A = -A;
[7]95
[15]96  if (T->Value() == 0) {
97    if (first_update) {
98      first_update = false;
99      previous_time = time;
100      return;
[7]101    } else {
[15]102      delta_t = (float)(time - previous_time) / 1000000000.;
[7]103    }
[15]104  } else {
105    delta_t = T->Value();
106  }
107  previous_time = time;
108  CurrentTime += delta_t;
[7]109
[15]110  if (is_finishing && CurrentTime > FinishTime + V / A)
111    is_running = false;
[7]112
[15]113  if (is_running) {
114    if (R == 0) {
115      pos.x = 0;
116      pos.y = 0;
117      v.x = 0;
118      v.y = 0;
119    } else {
120      if (CurrentTime < V / A) {
121        theta = angle_off + A / 2 * CurrentTime * CurrentTime / R;
122        pos.x = R * cos(theta);
123        pos.y = R * sin(theta);
124        v.x = -A * CurrentTime * sin(theta);
125        v.y = A * CurrentTime * cos(theta);
126      } else {
127        if (!is_finishing) {
128          theta =
129              angle_off + V * V / (2 * A * R) + (CurrentTime - V / A) * V / R;
130          pos.x = R * cos(theta);
131          pos.y = R * sin(theta);
132          v.x = -V * sin(theta);
133          v.y = V * cos(theta);
[7]134        } else {
[15]135          theta = angle_off + V * V / (2 * A * R) +
136                  (FinishTime - V / A) * V / R -
137                  A / 2 * (FinishTime - CurrentTime) *
138                      (FinishTime - CurrentTime) / R +
139                  V * (CurrentTime - FinishTime) / R;
140          pos.x = R * cos(theta);
141          pos.y = R * sin(theta);
142          v.x = -(V + A * (FinishTime - CurrentTime)) * sin(theta);
143          v.y = (V + A * (FinishTime - CurrentTime)) * cos(theta);
[7]144        }
[15]145      }
146    }
[7]147
[15]148    if (theta - angle_off >= nb_lap * 2 * PI - (-A / 2 * (V / A) * (V / A) / R +
149                                                V * (V / A) / R) &&
150        nb_lap > 0) {
151      FinishTraj();
[7]152    }
[15]153  } else {
154    v.x = 0;
155    v.y = 0;
156  }
[7]157
[15]158  // on prend une fois pour toute les mutex et on fait des accès directs
159  output->GetMutex();
160  output->SetValueNoMutex(0, 0, pos.x + pos_off.x);
161  output->SetValueNoMutex(0, 1, pos.y + pos_off.y);
162  output->SetValueNoMutex(1, 0, v.x + vel_off.x);
163  output->SetValueNoMutex(1, 1, v.y + vel_off.y);
164  output->ReleaseMutex();
[7]165
[15]166  output->SetDataTime(time);
[7]167}
Note: See TracBrowser for help on using the repository browser.