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 Arc::Arc(Point *_p1, Point *_p2, double _radius) :
27 p1(_p1), p2(_p2), radius(fabs(_radius)) {
29 // Find the center of the arc
30 double angp1p2 = p1->angleTo(*p2);
31 double m = p1->distanceTo(*p2)/2.0;
34 dbgPrintf("EEEERRRRRRRRROOOOOOOOORRRRRRRRR!!!!!!!!!!\n");
36 double angcen = acos(m/radius);
37 if (_radius < 0) angcen = -angcen;
38 center = Point(p1->x + radius * cos(angp1p2+angcen),
39 p1->y + radius * sin(angp1p2+angcen));
41 startAngle = center.angleTo(*p1);
42 angle = center.angleTo(*p2) - startAngle;
43 if (angle < -M_PI) angle += 2.0*M_PI;
44 if (angle > +M_PI) angle -= 2.0*M_PI;
46 length = fabs(angle*radius);
49 void Arc::setMaxV(const TrajectoryConstraints &constr) {
51 maxv = fmin(constr.maxv, constr.maxomega * r);
52 maxv = fmin(maxv, sqrt(constr.maxcenacc * r));
56 * not implemented, because this class is not used any more
58 double Arc::getDistance(double time) const {
62 void Arc::getPointAt(double distance, Point *p) {
65 ratio = distance/length;
68 ratio = (length-distance)/length;
70 a = startAngle + ratio*angle;
71 p->x = center.x + radius*cos(a);
72 p->y = center.y + radius*sin(a);
74 void Arc::shortenBy(double distance, Point *newEnd) {
75 getPointAt(-distance, newEnd);
77 angle *= (length-distance)/length;
81 startAngle = startAngle + distance/length*angle;
82 angle *= (length-distance)/length;
88 TrajectorySegment* Arc::splitAt(double distance, Point *newEnd) {
89 if (distance <= 0 || distance >= length)
92 getPointAt(distance, newEnd);
93 Arc *ns = new Arc(*this);
95 double a = distance/length*angle;
102 ns->length -= distance;
107 void Arc::getRefPos(double time, Pos &rp) {
109 double fraction = t/(t2-t1);
110 double distance = (v1 + 0.5*acc*t) * t;
111 double a = startAngle + distance/length*angle;
112 rp.x = center.x + radius*cos(a);
113 rp.y = center.y + radius*sin(a);
115 rp.phi = a + M_PI/2.0;
117 rp.phi = a - M_PI/2.0;
119 rp.v = v1+fraction*(v2-v1);
120 rp.omega =rp.v/radius;
121 if (angle < 0) rp.omega = -rp.omega;
124 #ifdef MATLAB_MEX_FILE
125 void Arc::plot(const char *style) {
130 mxArray *x = mxCreateDoubleMatrix(1, len, mxREAL);
131 mxArray *y = mxCreateDoubleMatrix(1, len, mxREAL);
132 mxArray *s = mxCreateCharMatrixFromStrings(1, &style);
133 mxArray *rhs[] = {x,y,s};
135 for (i=0; i < len; i++) {
136 getRefPos(t1+(t2-t1)*i/(len-1), rp);
137 mxGetPr(x)[i] = rp.x;
138 mxGetPr(y)[i] = rp.y;
140 mexCallMATLAB(0, NULL, 3, rhs, "plot");
141 sprintf(cmd, "plot([%g %g], [%g %g], 'mo')",
142 p1->x, p2->x, p1->y, p2->y);
150 } // namespace Segment