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

Last change on this file since 137 was 15, checked in by Bayard Gildas, 6 years ago

sources reformatted with flair-format-dir script

File size: 4.9 KB
Line 
1// %flair:license{
2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
4// %flair:license}
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
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;
39
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);
51
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);
59
60 output->SetValue(0, 0, 0);
61 output->SetValue(0, 1, 0);
62 output->SetValue(1, 0, 0);
63 output->SetValue(1, 1, 0);
64}
65
66TrajectoryGenerator2DCircle_impl::~TrajectoryGenerator2DCircle_impl() {
67 delete output;
68}
69
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;
76
77 // configure trajectory
78 angle_off = atan2(start_pos.y - pos_off.y, start_pos.x - pos_off.x);
79 CurrentTime = 0;
80}
81
82void TrajectoryGenerator2DCircle_impl::FinishTraj(void) {
83 if (!is_finishing) {
84 is_finishing = true;
85 FinishTime = CurrentTime;
86 }
87}
88
89void TrajectoryGenerator2DCircle_impl::Update(Time time) {
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;
96
97 if (V < 0)
98 A = -A;
99
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);
138 } else {
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);
148 }
149 }
150 }
151
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 }
161
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);
171}
Note: See TracBrowser for help on using the repository browser.