@@ -70,89 +70,81 @@ void DriveWidget::paintEvent( QPaintEvent* event )
70
70
{
71
71
int w = width ();
72
72
int h = height ();
73
-
74
- float lin = h/2 * linear_velocity_ / linear_max_;
75
- float ang = angular_velocity_;
73
+ int size = ( w > h ) ? h : w;
76
74
77
75
QPen arrow;
78
- int line_width = 2 + abs ( int ( lin / 5.0 ));
79
- arrow.setWidth ( h/20 );
76
+ arrow.setWidth ( size/20 );
80
77
arrow.setColor ( Qt::green );
81
78
arrow.setCapStyle ( Qt::RoundCap );
79
+ arrow.setJoinStyle ( Qt::RoundJoin );
82
80
painter.setPen ( arrow );
83
81
84
- int arrowhead_x, arrowhead_y;
85
- float arrowhead_ang;
82
+ int step_count = 100 ;
83
+ QPointF left_track[ step_count ];
84
+ QPointF right_track[ step_count ];
85
+
86
+ float half_track_width = size/4.0 ;
87
+
88
+ float cx = w/2 ;
89
+ float cy = h/2 ;
90
+ left_track[ 0 ].setX ( cx - half_track_width );
91
+ left_track[ 0 ].setY ( cy );
92
+ right_track[ 0 ].setX ( cx + half_track_width );
93
+ right_track[ 0 ].setY ( cy );
94
+ float angle = M_PI/2 ;
95
+ float delta_angle = angular_velocity_ / step_count;
96
+ float step_dist = linear_velocity_ * size/2 / linear_max_ / step_count;
97
+ for ( int step = 1 ; step < step_count; step++ )
98
+ {
99
+ angle += delta_angle / 2 ;
100
+ float next_cx = cx + step_dist * cosf ( angle );
101
+ float next_cy = cy - step_dist * sinf ( angle );
102
+ angle += delta_angle / 2 ;
103
+
104
+ left_track[ step ].setX ( next_cx + half_track_width * cosf ( angle + M_PI/2 ));
105
+ left_track[ step ].setY ( next_cy - half_track_width * sinf ( angle + M_PI/2 ));
106
+ right_track[ step ].setX ( next_cx + half_track_width * cosf ( angle - M_PI/2 ));
107
+ right_track[ step ].setY ( next_cy - half_track_width * sinf ( angle - M_PI/2 ));
108
+
109
+ cx = next_cx;
110
+ cy = next_cy;
111
+ }
112
+ painter.drawPolyline ( left_track, step_count );
113
+ painter.drawPolyline ( right_track, step_count );
114
+
115
+ int left_arrow_dir = (-step_dist + half_track_width * delta_angle > 0 );
116
+ int right_arrow_dir = (-step_dist - half_track_width * delta_angle > 0 );
117
+
118
+ arrow.setJoinStyle ( Qt::MiterJoin );
119
+ painter.setPen ( arrow );
86
120
87
- if ( angular_velocity_ == 0 )
121
+ float head_len = size / 8.0 ;
122
+ QPointF arrow_head[ 3 ];
123
+ float x, y;
124
+ if ( fabsf ( -step_dist + half_track_width * delta_angle ) > .01 )
88
125
{
89
- painter.drawLine ( w/2 , h/2 , w/2 , h/2 - lin );
90
- arrowhead_x = w/2 ;
91
- arrowhead_y = h/2 - lin;
92
- if ( lin < 0 )
93
- {
94
- arrowhead_ang = M_PI;
95
- }
96
- else
97
- {
98
- arrowhead_ang = 0 ;
99
- }
126
+ x = left_track[ step_count - 1 ].x ();
127
+ y = left_track[ step_count - 1 ].y ();
128
+ arrow_head[ 0 ].setX ( x + head_len * cosf ( angle + 3 *M_PI/4 + left_arrow_dir * M_PI ));
129
+ arrow_head[ 0 ].setY ( y - head_len * sinf ( angle + 3 *M_PI/4 + left_arrow_dir * M_PI ));
130
+ arrow_head[ 1 ].setX ( x );
131
+ arrow_head[ 1 ].setY ( y );
132
+ arrow_head[ 2 ].setX ( x + head_len * cosf ( angle - 3 *M_PI/4 + left_arrow_dir * M_PI ));
133
+ arrow_head[ 2 ].setY ( y - head_len * sinf ( angle - 3 *M_PI/4 + left_arrow_dir * M_PI ));
134
+ painter.drawPolyline ( arrow_head, 3 );
100
135
}
101
- else
136
+ if ( fabsf ( -step_dist - half_track_width * delta_angle ) > . 01 )
102
137
{
103
- int radius = abs ( int ( lin / ang ));
104
- int arc_pi = 180 * 16 ;
105
- int start_ang;
106
- int span_ang = abs ( int ( ang*180.0 /M_PI*16 ));
107
- int x;
108
- if ( ang > 0 )
109
- {
110
- if ( lin > 0 )
111
- {
112
- x = w/2 - 2 *radius;
113
- start_ang = 0 ;
114
- arrowhead_ang = ang;
115
- arrowhead_x = x + radius + radius * cosf ( ang );
116
- arrowhead_y = h/2 - radius * sinf ( ang );
117
- }
118
- else
119
- {
120
- x = w/2 ;
121
- start_ang = arc_pi;
122
- arrowhead_ang = M_PI + ang;
123
- arrowhead_x = x + radius - radius * cosf ( ang );
124
- arrowhead_y = h/2 + radius * sinf ( ang );
125
- }
126
- }
127
- else
128
- {
129
- if ( lin > 0 )
130
- {
131
- x = w/2 ;
132
- start_ang = arc_pi - span_ang;
133
- arrowhead_ang = ang;
134
- arrowhead_x = x + radius - radius * cosf ( -ang );
135
- arrowhead_y = h/2 - radius * sinf ( -ang );
136
- }
137
- else
138
- {
139
- x = w/2 - 2 *radius;
140
- start_ang = -span_ang;
141
- arrowhead_ang = M_PI + ang;
142
- arrowhead_x = x + radius + radius * cosf ( -ang );
143
- arrowhead_y = h/2 + radius * sinf ( -ang );
144
- }
145
- }
146
- painter.drawArc ( x, h/2 - radius, 2 *radius, 2 *radius, start_ang, span_ang );
138
+ x = right_track[ step_count - 1 ].x ();
139
+ y = right_track[ step_count - 1 ].y ();
140
+ arrow_head[ 0 ].setX ( x + head_len * cosf ( angle + 3 *M_PI/4 + right_arrow_dir * M_PI ));
141
+ arrow_head[ 0 ].setY ( y - head_len * sinf ( angle + 3 *M_PI/4 + right_arrow_dir * M_PI ));
142
+ arrow_head[ 1 ].setX ( x );
143
+ arrow_head[ 1 ].setY ( y );
144
+ arrow_head[ 2 ].setX ( x + head_len * cosf ( angle - 3 *M_PI/4 + right_arrow_dir * M_PI ));
145
+ arrow_head[ 2 ].setY ( y - head_len * sinf ( angle - 3 *M_PI/4 + right_arrow_dir * M_PI ));
146
+ painter.drawPolyline ( arrow_head, 3 );
147
147
}
148
- float tip_spread = M_PI/3 ;
149
- float head_l_ang = arrowhead_ang + M_PI * 1.5 - tip_spread / 2 ;
150
- float head_r_ang = arrowhead_ang + M_PI * 1.5 + tip_spread / 2 ;
151
- float head_size = line_width * 2 ;
152
- painter.drawLine ( arrowhead_x + head_size * cosf ( head_l_ang ), arrowhead_y - head_size * sinf ( head_l_ang ),
153
- arrowhead_x, arrowhead_y );
154
- painter.drawLine ( arrowhead_x, arrowhead_y,
155
- arrowhead_x + head_size * cosf ( head_r_ang ), arrowhead_y - head_size * sinf ( head_r_ang ));
156
148
}
157
149
}
158
150
0 commit comments