/ru/navigation/trajectoryplanner.cpp
C++ | 213 lines | 155 code | 30 blank | 28 comment | 35 complexity | ef98214e91554f52ed35544d28b0b1f7 MD5 | raw file
1#include "trajectoryplanner.h"
2
3bool isAngleBetweenTwoAngles(float angle, float start_angle, float end_angle)
4{
5 bool angle_match = false;
6
7 // ?? ???????
8 if ((angle>=start_angle)&&(angle<=end_angle)) {
9 angle_match = true;
10 } else {
11 angle_match = false;
12 }
13
14 return angle_match;
15}
16
17float TrajectoryElement::getDistanceToPoint(const RobotPosition &element_pos, const QPointF &point)
18{
19 float result = 10000000.0f;
20 if (type==ElementType_Line) {
21 // ?????????? ?? ???????
22 QPointF end_pos = getEndpointPosition(element_pos);
23 QVector2D v1(point-element_pos);
24 QVector2D v2(end_pos-element_pos);
25 float product = QVector2D::dotProduct(v1,v2);
26
27 QPointF p = QPointF(element_pos) + (end_pos-element_pos)*(product/v2.lengthSquared());
28
29 float line_sign = sign(float(v1.x()*v2.y()-v1.y()*v2.x()));
30
31 if ((product>=0.0)&&((product/v2.lengthSquared())<=1.0)) {
32 // ????? ????? ??????
33 result = line_sign*QVector2D(p-point).length();
34 } else {
35 // ????? ???????
36 if (product<0) // ?????
37 result = line_sign*QVector2D(element_pos-point).length();
38 else // ??????
39 result = line_sign*QVector2D(end_pos-point).length();
40 }
41 } else if (type==ElementType_Arc) {
42 // ?????????? ?? ????
43 // ?????????? ????? ??????????
44 QPointF rot = getArcCenter(element_pos);
45 // ??????? ????? ? ?????? ????????? ? rot
46 QPointF pos = (point-rot);
47 QPointF start_pos = (element_pos-rot);
48 // ?????????? ????????? ???? ????
49 float start_angle_offset = Angle(atan2(start_pos.y(), start_pos.x()));
50 float start_angle = Angle(atan2(start_pos.y(), start_pos.x())-start_angle_offset).to2PIFormat();
51 start_angle = 0;
52 // ???????? ????
53 float angle_wise = sign(angle)*sign(R);
54 float end_angle = Angle(fabs(angle)).to2PIFormat();
55
56
57 // ?????????? ???? ????? ?????? ? ???????
58 float point_angle1 = 0;
59 float point_angle2 = 0;
60 if (angle_wise>0) {
61 point_angle1 = PIx2-Angle(atan2(pos.y(), pos.x()) - start_angle_offset).to2PIFormat(); // ???? ?? ?????? ? ?????
62 point_angle2 = PIx2-Angle(atan2(pos.y(), pos.x())+PI - start_angle_offset).to2PIFormat(); // ??????????????? ????
63 } else {
64 point_angle1 = Angle(atan2(pos.y(), pos.x()) - start_angle_offset).to2PIFormat(); // ???? ?? ?????? ? ?????
65 point_angle2 = Angle(atan2(pos.y(), pos.x())+PI - start_angle_offset).to2PIFormat(); // ??????????????? ????
66 }
67 //qDebug()<<point_angle1;
68
69
70 // ??????? ???????? ?? ??? ???? ? ????????
71
72 bool angle_match_short = false;
73 bool angle_match_long = false;
74 if (fabs(angle)<PIx2) {
75 //
76 angle_match_long = isAngleBetweenTwoAngles(point_angle1, start_angle, end_angle);
77 angle_match_short = isAngleBetweenTwoAngles(point_angle2, start_angle, end_angle);
78 } else { // ???? ?????? ??? PI*2 ?? 100% ???????????
79 angle_match_short = true;
80 }
81
82 //qDebug()<<angle_match_long;
83
84 // ?????????? ??????????
85 if (angle_match_short) {
86 // ?????????? ?? ?????? - ??????
87 result = R+angle_wise*QVector2D(pos).length();
88 } else if (angle_match_long) { // ?????????? ?? ?????? + ??????
89 result = R-angle_wise*QVector2D(pos).length();
90 } else {
91 // ????????? ?????????? ?? ????? ?? ???? ?????
92 // ??? ?? ????????????
93 }
94 }
95 return result;
96}
97
98RobotPosition TrajectoryElement::getEndpointPosition(const RobotPosition &start_pos)
99{
100 RobotPosition pos;
101 if (type==ElementType_Line) {
102 pos.a = start_pos.a;
103 pos.x = start_pos.x + line_length*cos(pos.a);
104 pos.y = start_pos.y + line_length*sin(pos.a);
105
106 } else if (type==ElementType_Arc) {
107 pos.a = start_pos.a - angle*sign(R);
108 // ????? ?????? ??????? ???????
109 QPointF rot = getArcCenter(start_pos);
110 //qDebug()<<rot;
111
112 float an = -angle*sign(R);
113 //qDebug()<<an;
114 // ???????
115 float tx = rot.x()+(start_pos.x-rot.x())*cos(an)-(start_pos.y-rot.y())*sin(an);
116 float ty = rot.y()+(start_pos.x-rot.x())*sin(an)+(start_pos.y-rot.y())*cos(an);
117
118 pos.x = tx;
119 pos.y = ty;
120 } else if (type==ElementType_Rotation) {
121 pos.a = start_pos.a+angle;
122 pos.x = start_pos.x;
123 pos.y = start_pos.y;
124 }
125 pos.speed = endpoint_speed;
126 return pos;
127}
128
129RobotPosition TrajectoryElement::interpolatePosition(const RobotPosition &start_pos, float value)
130{
131 RobotPosition pos;
132 if (type==ElementType_Line) {
133 pos.a = start_pos.a;
134 pos.x = start_pos.x + line_length*cos(pos.a)*value;
135 pos.y = start_pos.y + line_length*sin(pos.a)*value;
136
137 } else if (type==ElementType_Arc) {
138 float an = -angle*sign(R)*value;
139 pos.a = start_pos.a - an;
140 // ????? ?????? ??????? ???????
141 QPointF rot = getArcCenter(start_pos);
142 //qDebug()<<rot;
143
144 //qDebug()<<an;
145 // ???????
146 float tx = rot.x()+(start_pos.x-rot.x())*cos(an)-(start_pos.y-rot.y())*sin(an);
147 float ty = rot.y()+(start_pos.x-rot.x())*sin(an)+(start_pos.y-rot.y())*cos(an);
148
149 pos.x = tx;
150 pos.y = ty;
151 } else if (type==ElementType_Rotation) {
152 pos.a = start_pos.a+angle*value;
153 pos.x = start_pos.x;
154 pos.y = start_pos.y;
155 }
156 pos.speed = startpoint_speed*(1.0-value)+endpoint_speed*value;
157 return pos;
158}
159
160QPointF TrajectoryElement::getArcCenter(const RobotPosition &start_pos)
161{
162 return QPointF(start_pos.x-R*cos(start_pos.a+PId2), start_pos.y-R*sin(start_pos.a+PId2));
163}
164
165float ITrajectoryPlanner::getTrajectoryLength(const Trajectory &trajectory)
166{
167 float res = 0;
168 for (int i=0; i<trajectory.size(); i++) {
169 if (trajectory.elements[i].type==ElementType_Arc) {
170 res += sqr((trajectory.elements[i].R * trajectory.elements[i].angle * 10.0f)); // ???? ????? ????????????????, ??? ??????, ??????? ??? ??????
171 //res += abs(trajectory[i].R * trajectory[i].angle );
172 } else
173 if (trajectory.elements[i].type==ElementType_Line) {
174 res += fabs(trajectory.elements[i].line_length);
175 }
176 }
177
178 return res;
179}
180
181float TrajectoryElement::getElementLength()
182{
183 if (type==ElementType_Line)
184 return fabs(line_length);
185 if (type==ElementType_Arc)
186 return fabs(R*angle);
187 return 0.0f;
188}
189
190float Trajectory::getDistanceToPoint(const RobotPosition &start_pos, const QPointF &point)
191{
192 RobotPosition element_position = start_pos;
193 int size = elements.size();
194 float min_size = 10000000.0;
195
196 for (int i=0; i<size; i++) {
197 float dist = elements[i].getDistanceToPoint(element_position, point);
198 if (fabs(dist)<fabs(min_size)) {
199 min_size = dist;
200 }
201 element_position = elements[i].getEndpointPosition(element_position);
202 //qDebug()<<QString("%1 %2;%3").arg(i).arg(element_position.x).arg(element_position.y);
203 }
204 return min_size;
205}
206
207
208void Trajectory::appendTrajectory(const Trajectory &trajectory)
209{
210 for (int i=0; i<trajectory.size(); i++) {
211 elements.push_back(trajectory.elements[i]);
212 }
213}