/
/
/
1#include "q_pen_drawer/bezierline.h"
2
3#include <algorithm>
4#include <cmath>
5#include <QtMath>
6#include <iostream>
7#include <QTransform>
8#include <Eigen/Core>
9#include <Eigen/LU>
10
11BezierLine::BezierLine(){
12}
13
14QRectF BezierLine::boundingRect() const {
15 if(points_.begin() == points_.end()){
16 return QRectF(0.0, 0.0, 0.0, 0.0);
17 }
18 else{
19 auto x_min = std::min_element(points_.begin(), points_.end(), [](const BezierPoint &a, const BezierPoint &b){
20 return a.position.x() < b.position.x();
21 })->position.x();
22 auto x_max = std::max_element(points_.begin(), points_.end(), [](const BezierPoint &a, const BezierPoint &b){
23 return a.position.x() > b.position.x();
24 })->position.x();
25 auto y_min = std::min_element(points_.begin(), points_.end(), [](const BezierPoint &a, const BezierPoint &b){
26 return a.position.y() < b.position.y();
27 })->position.y();
28 auto y_max = std::max_element(points_.begin(), points_.end(), [](const BezierPoint &a, const BezierPoint &b){
29 return a.position.y() > b.position.y();
30 })->position.y();
31 return QRectF(x_min-5, y_min-5, x_max - x_min + 5, y_max - y_min + 5);
32 }
33}
34
35bool BezierLine::selectClosest(QPointF point){
36 for(int i=0 ; i<points_.size(); i++){
37 if(std::hypot((points_[i].position - point).x(), (points_[i].position - point).y()) < 5){
38 active_point_ = qMakePair(i, UpdateMode::Position);
39 return true;
40 }
41 else if(std::hypot((points_[i].control_before - point).x(), (points_[i].control_before - point).y()) < 5){
42 active_point_ = qMakePair(i, UpdateMode::ControlBefore);
43 return true;
44 }
45 else if(std::hypot((points_[i].control_after - point).x(), (points_[i].control_after - point).y()) < 5){
46 active_point_ = qMakePair(i, UpdateMode::ControlAfter);
47 return true;
48 }
49 }
50 active_point_ = qMakePair(-1, UpdateMode::ControlAfter);
51 return false;
52}
53
54void BezierLine::addPoint(QPointF point){
55 points_.push_back({point, point, point});
56 active_point_ = qMakePair(-1, UpdateMode::ControlAfter);
57}
58
59void BezierLine::updateControlPoint(QPointF point){
60 if(active_point_.first == -1){
61 auto current_point_iterator = points_.end()-1;
62 current_point_iterator->control_after = point;
63 current_point_iterator->control_before = 2 * current_point_iterator->position - point;
64 }
65 else{
66 switch (active_point_.second) {
67 case UpdateMode::Position: {
68 QPointF delta = point - points_[active_point_.first].position;
69 points_[active_point_.first].position += delta;
70 points_[active_point_.first].control_after += delta;
71 points_[active_point_.first].control_before += delta;
72 break;
73 }
74 case UpdateMode::ControlAfter: {
75 QPointF w = point - points_[active_point_.first].position;
76 QPointF v = points_[active_point_.first].control_after - points_[active_point_.first].position;
77
78 points_[active_point_.first].control_after = point;
79 qreal delta = qAtan2(w.y() * v.x() - w.x() * v.y(), w.x() * v.x() + w.y() * v.y());
80
81 qreal cosa = std::cos(delta);
82 qreal sina = std::sin(delta);
83
84 QTransform translate(1, 0, 0, 1, -points_[active_point_.first].position.x(), -points_[active_point_.first].position.y());
85 QTransform rotate(cosa, sina, -sina, cosa, 0, 0);
86 QTransform back_translate(1, 0, 0, 1, points_[active_point_.first].position.x(), points_[active_point_.first].position.y());
87
88 QTransform transform = translate * rotate * back_translate;
89
90 points_[active_point_.first].control_before = transform.map(points_[active_point_.first].control_before);
91
92 break;
93 }
94 case UpdateMode::ControlBefore: {
95 QPointF w = point - points_[active_point_.first].position;
96 QPointF v = points_[active_point_.first].control_before - points_[active_point_.first].position;
97
98 points_[active_point_.first].control_before = point;
99 qreal delta = qAtan2(w.y() * v.x() - w.x() * v.y(), w.x() * v.x() + w.y() * v.y());
100
101 qreal cosa = std::cos(delta);
102 qreal sina = std::sin(delta);
103
104 QTransform translate(1, 0, 0, 1, -points_[active_point_.first].position.x(), -points_[active_point_.first].position.y());
105 QTransform rotate(cosa, sina, -sina, cosa, 0, 0);
106 QTransform back_translate(1, 0, 0, 1, points_[active_point_.first].position.x(), points_[active_point_.first].position.y());
107
108 QTransform transform = translate * rotate * back_translate;
109
110 points_[active_point_.first].control_after = transform.map(points_[active_point_.first].control_after);
111
112 break;
113 }
114 }
115
116 }
117}
118
119QList<QPointF> BezierLine::getPoints(){
120 return QList<QPointF>();
121}
122
123QPointF BezierLine::calculateBezierPoint(const QPointF &begin, const QPointF &begin_control, const QPointF &end_control, const QPointF &end, qreal t){
124 QPointF p_b = qPow(1.0-t, 3) * begin +
125 3 * qPow(1.0 - t, 2) * t * begin_control +
126 3 * qPow(t, 2) * (1.0 - t) * end_control +
127 qPow(t, 3) * end;
128 return p_b;
129}
130
131QPointF BezierLine::calculateBezierDerivative(const QPointF &begin, const QPointF &begin_control, const QPointF &end_control, const QPointF &end, qreal t){
132 QPointF p_b = 3 * qPow(1.0-t, 2) * (begin_control - begin) +
133 6 * (1.0 - t) * t * (end_control - begin_control) +
134 3 * qPow(t, 2) * (end - end_control);
135 return p_b;
136}
137
138QPointF BezierLine::calculateBezierSecondDerivative(const QPointF &begin, const QPointF &begin_control, const QPointF &end_control, const QPointF &end, qreal t){
139 QPointF p_b = 6 * (1.0-t) * (end_control - 2 * begin_control + begin) +
140 6 * t * (end - 2 * end_control + begin_control);
141 return p_b;
142}
143
144void BezierLine::paint(QPainter* painter, const QStyleOptionGraphicsItem* /*option*/, QWidget* /*widget*/){
145 QPen position_pen, control_pen, line_pen;
146
147 position_pen.setWidth(2);
148 position_pen.setStyle(Qt::SolidLine);
149 control_pen = position_pen;
150
151 line_pen.setWidth(1);
152 line_pen.setStyle(Qt::DotLine);
153 line_pen.setColor(Qt::blue);
154
155 position_pen.setColor(Qt::green);
156 control_pen.setColor(Qt::blue);
157
158
159 for(const auto &point : points_){
160 painter->setPen(position_pen);
161 painter->drawEllipse(point.position.x() - point_size_ / 2, point.position.y() - point_size_ / 2, point_size_, point_size_);
162 painter->setPen(control_pen);
163 painter->drawEllipse(point.control_before.x() - point_size_ / 2, point.control_before.y() - point_size_ / 2, point_size_, point_size_);
164 painter->drawEllipse(point.control_after.x() - point_size_ / 2, point.control_after.y() - point_size_ / 2, point_size_, point_size_);
165 painter->setPen(line_pen);
166 painter->drawLine(point.control_before, point.control_after);
167 }
168
169 if(points_.begin() == points_.end()) return;
170
171 if(points_.begin() + 1 == points_.end()) return;
172
173 for(auto it = points_.begin() ; it != points_.end(); it++){
174 if (it == points_.begin()) continue;
175
176 auto arc_length_approx = (std::hypot(((it-1)->position - it->position).x(), ((it-1)->position - it->position).y() ) +
177 std::hypot(((it-1)->position - (it-1)->control_after).x(), ((it-1)->position - (it-1)->control_after).y() ) +
178 std::hypot((it->control_before - (it-1)->control_after).x(), (it->control_before - (it-1)->control_after).y() ) +
179 std::hypot((it->position - it->control_before).x(), (it->position - it->control_before).y() )) / 2.0;
180
181 qreal delta = average_step_length_ / arc_length_approx;
182
183 qreal t = delta;
184
185 QPointF last = (it-1)->position;
186
187 while(t < 1.0){
188
189 auto p_b = calculateBezierPoint((it-1)->position, (it-1)->control_after, it->control_before, it->position, t);
190 auto p_b_1 = calculateBezierDerivative((it-1)->position, (it-1)->control_after, it->control_before, it->position, t);
191 auto p_b_2 = calculateBezierSecondDerivative((it-1)->position, (it-1)->control_after, it->control_before, it->position, t);
192
193 Eigen::Matrix2d p_b_concat = (Eigen::Matrix2d() << p_b_1.x(), p_b_2.x(),
194 p_b_1.y(), p_b_2.y()).finished();
195
196 Eigen::Vector2d p_b_1_eig = (Eigen::Vector2d() << p_b_1.x(), p_b_1.y()).finished();
197
198 auto nom = p_b_concat.determinant();
199 auto denom = qPow(p_b_1_eig.norm(), 3);
200
201 auto curvature = abs(nom / denom);
202
203 painter->setPen(control_pen);
204 painter->drawEllipse(p_b.x() - 1, p_b.y() - 1, 2, 2);
205
206 QPen curvature_pen;
207 curvature = std::clamp(curvature, 0.0001, 1.0);
208
209 int hue = 250 + (std::log10(curvature) + 4) / 4 * 110;
210 QColor curvature_color;
211
212 curvature_color.setHsl(hue, 255, 127, 255);
213
214 curvature_pen.setWidth(2);
215 curvature_pen.setStyle(Qt::SolidLine);
216 curvature_pen.setColor(curvature_color);
217 painter->setPen(curvature_pen);
218 painter->drawLine(last, p_b);
219 last = p_b;
220 t += delta;
221 }
222
223 painter->drawLine(last, it->position);
224
225 }
226}
227