1 // Copyright 2009, 2012 Michal Sojka <sojkam1@fel.cvut.cz>
2 // Copyright 2009 Petr Beneš
4 // This file is part of Trgen library.
6 // Trgen is free software: you can redistribute it and/or modify
7 // it under the terms of the GNU General Public License as published by
8 // the Free Software Foundation, either version 3 of the License, or
9 // (at your option) any later version.
11 // Trgen is distributed in the hope that it will be useful,
12 // but WITHOUT ANY WARRANTY; without even the implied warranty of
13 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 // GNU General Public License for more details.
16 // You should have received a copy of the GNU General Public License
17 // along with Trgen. If not, see <http://www.gnu.org/licenses/>.
26 * Segment representing turn at one place.
28 * Speed related variables @c v1, @c v2 and @c acc represents
32 Turn::Turn(Point *_center, double _startHeading, double _turnBy) :
33 center(_center), turnBy(_turnBy), startHeading(_startHeading)
37 void Turn::setMaxV(const TrajectoryConstraints &constr) {
38 maxv = constr.maxomega;
44 double Turn::getDistance(double time) const {
46 return 0.5*acc*time*time + v1*time;
50 void Turn::getPointAt(double distance, Point *p) {
54 TrajectorySegment* Turn::splitAt(double distance, Point *newEnd) {
55 if (distance <= 0 || distance >= fabs(turnBy)) {
56 dbgPrintf("splitAt: distance=%g turnBy=%g\n", distance, turnBy);
60 Turn *ns = new Turn(*this);
64 ns->startHeading+=distance;
65 ns->turnBy -= distance;
69 TrajectorySegment* Turn::splitAtByTime(double time, Point *newEnd) {
70 if (time <= t1 || time >= t2) {
71 dbgPrintf("splitAt: distance=%g turnBy=%g\n", time, turnBy);
75 Turn *ns = new Turn(*this);
76 double distance = 0.5 * acc * time * time + v1 * time;
80 ns->startHeading+=distance;
81 ns->turnBy -= distance;
85 void Turn::getRefPos(double time, Pos &rp) {
87 double fraction = t/(t2-t1);
88 double distance = (v1 + 0.5*acc*t) * t;
92 rp.phi = startHeading + distance;
94 rp.phi = startHeading - distance;
97 rp.omega = v1+fraction*(v2-v1);
98 if (turnBy < 0) rp.omega = -rp.omega;
104 } // namespace Segment