20 #include <tulip/TlpTools.h> 22 template <
typename Obj,
typename OTYPE>
23 tlp::Circle<Obj, OTYPE> &tlp::Circle<Obj, OTYPE>::merge(
const tlp::Circle<Obj, OTYPE> &c) {
24 Vector<Obj, 2, OTYPE> p1(*
this);
25 Vector<Obj, 2, OTYPE> p2(c);
26 Vector<Obj, 2, OTYPE> c12 = p2 - p1;
27 double norm = c12.norm();
29 if (norm < 0.0000001) {
30 if (radius < c.radius)
36 double dx = c12[0] / norm;
37 double dy = c12[1] / norm;
40 p2[0] += dx * c.radius;
41 p2[1] += dy * c.radius;
42 Obj tmpDist = p1.dist(p2) / 2.;
44 if ((tmpDist < radius) || (tmpDist < c.radius)) {
45 if (radius > c.radius)
59 template <
typename Obj,
typename OTYPE>
60 bool tlp::Circle<Obj, OTYPE>::isIncludeIn(
const tlp::Circle<Obj, OTYPE> &c)
const {
61 Vector<Obj, 2, OTYPE> dir = c - *
this;
62 return (dir.norm() + radius) <= c.radius;
65 template <
typename Obj,
typename OTYPE>
66 tlp::Circle<Obj, OTYPE> tlp::enclosingCircle(
const tlp::Circle<Obj, OTYPE> &c1,
67 const tlp::Circle<Obj, OTYPE> &c2) {
68 Vector<Obj, 2, OTYPE> dir = c2 - c1;
72 return Circle<Obj, OTYPE>(c1, std::max(c1.radius, c2.radius));
75 Vector<Obj, 2, OTYPE> ext1 = c1 - dir * c1.radius;
76 Vector<Obj, 2, OTYPE> ext2 = c2 + dir * c2.radius;
77 return Circle<Obj, OTYPE>((ext1 + ext2) / Obj(2), (ext2 - ext1).norm() / Obj(2));
80 template <
typename Obj,
typename OTYPE>
81 tlp::Circle<Obj, OTYPE>
82 tlp::lazyEnclosingCircle(
const std::vector<tlp::Circle<Obj, OTYPE>> &circles) {
84 tlp::Vector<Obj, 4, OTYPE> boundingBox;
86 typename std::vector<tlp::Circle<Obj, OTYPE>>::const_iterator it = circles.begin();
87 boundingBox[0] = (*it)[0] - (*it).radius;
88 boundingBox[1] = (*it)[1] - (*it).radius;
89 boundingBox[2] = (*it)[0] + (*it).radius;
90 boundingBox[3] = (*it)[1] + (*it).radius;
93 for (; it != circles.end(); ++it) {
94 boundingBox[0] = std::min(boundingBox[0], ((*it)[0] - (*it).radius));
95 boundingBox[1] = std::min(boundingBox[1], ((*it)[1] - (*it).radius));
96 boundingBox[2] = std::max(boundingBox[2], ((*it)[0] + (*it).radius));
97 boundingBox[3] = std::max(boundingBox[3], ((*it)[1] + (*it).radius));
100 tlp::Vector<Obj, 2, OTYPE> center;
101 center[0] = (boundingBox[0] + boundingBox[2]) / 2.;
102 center[1] = (boundingBox[1] + boundingBox[3]) / 2.;
104 std::max((boundingBox[2] - boundingBox[0]) / 2., (boundingBox[3] - boundingBox[1]) / 2.);
105 tlp::Circle<Obj, OTYPE> result(center, radius);
108 for (
typename std::vector<tlp::Circle<Obj, OTYPE>>::const_iterator it = circles.begin();
109 it != circles.end(); ++it)
115 template <
typename Obj,
typename OTYPE>
116 tlp::Circle<Obj, OTYPE> tlp::enclosingCircle(
const std::vector<tlp::Circle<Obj, OTYPE>> &circles) {
117 class OptimumCircleHull {
118 const std::vector<tlp::Circle<Obj, OTYPE>> *circles;
119 std::vector<unsigned> enclosedCircles;
120 unsigned first, last;
122 tlp::Circle<Obj, OTYPE> result;
124 static tlp::Circle<Obj, OTYPE> enclosingCircle(
const tlp::Circle<Obj, OTYPE> &c1,
125 const tlp::Circle<Obj, OTYPE> &c2,
126 const tlp::Circle<Obj, OTYPE> &c3) {
136 Obj b = -b1 * r2 * a1 * a1 * b3 + a3 * a3 * r2 * r2 * r2 + b2 * b2 * r3 * r3 * r3 +
137 b1 * b1 * r3 * r3 * r3 + b1 * b1 * r2 * r2 * r2 + r1 * r1 * r1 * b3 * b3 +
138 r1 * r1 * r1 * b2 * b2 + r2 * r2 * r2 * b3 * b3 + a1 * a1 * r3 * r3 * r3 +
139 a2 * a2 * r1 * r1 * r1 + a2 * a2 * r3 * r3 * r3 + a1 * a1 * r2 * r2 * r2 +
140 a3 * a3 * r1 * r1 * r1 - b2 * b2 * r3 * a3 * a3 - b2 * b2 * r3 * b3 * b3 -
141 2 * b2 * r3 * r3 * r3 * b1 - b1 * b1 * r3 * a3 * a3 - b1 * b1 * r3 * b3 * b3 -
142 b1 * b1 * r2 * a2 * a2 + 2 * b1 * b1 * r2 * b3 * b3 - b1 * b1 * r2 * r3 * r3 -
143 r1 * b3 * b3 * b3 * b2 + r1 * b3 * b3 * b3 * b1 + 2 * r1 * b2 * b2 * b3 * b3 -
144 r1 * b2 * b2 * r3 * r3 + r2 * b3 * b3 * b3 * b2 - r2 * b3 * b3 * b3 * b1 -
145 b2 * b2 * b2 * r3 * b1 + 2 * b2 * b2 * r3 * b1 * b1 + b2 * b2 * b2 * r3 * b3 -
146 b2 * b2 * r3 * r1 * r1 + b1 * b1 * b1 * r3 * b3 - b1 * b1 * b1 * r3 * b2 -
147 b1 * b1 * r3 * r2 * r2 - b1 * b1 * r2 * b2 * b2 - b1 * b1 * b1 * r2 * b3 +
148 b1 * b1 * b1 * r2 * b2 - 2 * b1 * r2 * r2 * r2 * b3 - r1 * b3 * b3 * b1 * b1 -
149 2 * r1 * r1 * r1 * b3 * b2 - r1 * b3 * b3 * r2 * r2 - r1 * b3 * b3 * a1 * a1 +
150 r1 * b2 * b2 * b2 * b1 - r1 * b2 * b2 * b1 * b1 - r1 * b2 * b2 * b2 * b3 -
151 r1 * b2 * b2 * a1 * a1 - r2 * b3 * b3 * a2 * a2 - r2 * b3 * b3 * b2 * b2 -
152 r2 * b3 * b3 * r1 * r1 - b2 * r3 * b1 * b1 * b3 + b2 * r3 * a2 * a2 * b3 +
153 b2 * r3 * r1 * r1 * b3 - b2 * r3 * r2 * r2 * b3 + b2 * r3 * a1 * a1 * b3 -
154 2 * b2 * r3 * a1 * a2 * b3 + a1 * a3 * b2 * b2 * r3 - 2 * a1 * a3 * b2 * b1 * r3 -
155 2 * a1 * a3 * b2 * b1 * r2 - 2 * a1 * a3 * b2 * r1 * b3 + a1 * a3 * b2 * b2 * r1 -
156 2 * a1 * a3 * b2 * r2 * b3 - b2 * r3 * b1 * a2 * a2 + 2 * b2 * r3 * b1 * a3 * a3 +
157 2 * b2 * r3 * b1 * b3 * b3 + b1 * r2 * b2 * a3 * a3 - b1 * r2 * b2 * b3 * b3 +
158 b1 * r2 * b2 * r3 * r3 + r1 * b3 * b1 * a2 * a2 - r1 * b3 * b2 * a3 * a3 +
159 r1 * b3 * b2 * r3 * r3 + r1 * b3 * b1 * a3 * a3 - r1 * b3 * b1 * r3 * r3 +
160 r1 * b2 * b1 * a2 * a2 + r1 * b2 * b1 * a3 * a3 - r1 * b2 * b1 * b3 * b3 +
161 r1 * b2 * b1 * r3 * r3 + 2 * r2 * b3 * b1 * a2 * a2 + r2 * b3 * b2 * a3 * a3 -
162 r2 * b3 * b2 * r3 * r3 - r2 * b3 * b1 * a3 * a3 + r2 * b3 * b1 * r3 * r3 +
163 b2 * r3 * b1 * r2 * r2 + 4 * b2 * r3 * a1 * a2 * b1 + b1 * r3 * a2 * a2 * b3 -
164 b1 * r3 * b2 * b2 * b3 - b1 * r3 * r1 * r1 * b3 + b1 * r3 * r1 * r1 * b2 +
165 b1 * r3 * r2 * r2 * b3 + b1 * r3 * a1 * a1 * b3 - b1 * r3 * a1 * a1 * b2 -
166 2 * b1 * r3 * a1 * a2 * b3 - b1 * b1 * r3 * a1 * a2 + b1 * b1 * r3 * a1 * a3 +
167 2 * b1 * r2 * b2 * b2 * b3 + b1 * r2 * r1 * r1 * b3 - b1 * r2 * r1 * r1 * b2 +
168 b1 * r2 * a1 * a1 * b2 - 2 * b1 * r2 * a1 * a2 * b3 + b1 * b1 * r2 * a1 * a2 -
169 b1 * b1 * r2 * a1 * a3 - r1 * b3 * b1 * b2 * b2 + 2 * r1 * b3 * b1 * b1 * b2 +
170 2 * r1 * b3 * a1 * a1 * b2 + r1 * b3 * b1 * r2 * r2 + r1 * b3 * b3 * a1 * a2 -
171 r1 * b2 * a2 * a2 * b3 + r1 * b2 * r2 * r2 * b3 - r1 * b2 * b1 * r2 * r2 -
172 2 * r1 * b2 * a1 * a2 * b3 - r2 * b3 * b1 * b1 * b2 + r2 * b3 * r1 * r1 * b2 +
173 r2 * b3 * a1 * a1 * b2 + r2 * b3 * b3 * a1 * a2 + 4 * r2 * b3 * a1 * a3 * b1 -
174 a1 * a1 * a3 * a3 * r3 + 2 * a1 * a1 * a3 * a3 * r2 + a1 * a3 * a3 * a3 * r1 -
175 a1 * a1 * b3 * b3 * r3 - a1 * b3 * b3 * a3 * r2 + a1 * r3 * r3 * a3 * r2 -
176 a2 * a1 * a1 * a3 * r2 + a2 * b1 * b1 * a3 * r2 + a2 * b3 * b3 * a3 * r2 -
177 a1 * a3 * a3 * a2 * r1 + 2 * a1 * a3 * a3 * a2 * r3 + 2 * a1 * b3 * b3 * a2 * r3 +
178 a1 * b3 * b3 * a3 * r1 - 2 * a1 * r3 * r3 * r3 * a2 - a1 * a1 * r3 * r3 * r2 -
179 a2 * a1 * a1 * a1 * r3 - a2 * a2 * a1 * a1 * r1 + 2 * a2 * a2 * a1 * a1 * r3 +
180 a2 * a1 * a1 * a1 * r2 - a2 * a2 * b1 * b1 * r1 + 2 * a2 * a2 * a3 * a3 * r1 -
181 a2 * a2 * a3 * a3 * r3 - a2 * a3 * a3 * a3 * r1 - a2 * a2 * b3 * b3 * r3 -
182 a2 * a2 * r1 * r1 * r3 - 2 * a2 * r1 * r1 * r1 * a3 + a1 * r3 * r3 * a2 * r1 -
183 a1 * r3 * r3 * a3 * r1 + 2 * a2 * a1 * a1 * a3 * r1 + 2 * a2 * b1 * b1 * a3 * r1 -
184 a2 * a3 * a3 * a1 * r2 - a2 * b3 * b3 * a3 * r1 + a2 * r1 * r1 * a1 * r3 -
185 a2 * r1 * r1 * a1 * r2 - a2 * a2 * r3 * r3 * r1 - a1 * a3 * a3 * a3 * r2 +
186 a2 * a3 * a3 * a3 * r2 - a1 * a1 * r3 * r2 * r2 + a1 * a1 * a1 * r3 * a3 +
187 a2 * a2 * a2 * r1 * a1 - a2 * a2 * a2 * r1 * a3 - a2 * a2 * a2 * r3 * a1 +
188 a2 * a2 * a2 * r3 * a3 - a1 * a1 * r2 * a2 * a2 - a1 * a1 * r2 * b2 * b2 -
189 a1 * a1 * a1 * r2 * a3 - 2 * a1 * r2 * r2 * r2 * a3 - a3 * a3 * r1 * a1 * a1 -
190 a3 * a3 * r1 * b1 * b1 - a3 * a3 * r1 * r2 * r2 - a3 * a3 * r2 * a2 * a2 -
191 a3 * a3 * r2 * b2 * b2 - a3 * a3 * r2 * r1 * r1 + a2 * r3 * r3 * a1 * r2 +
192 a2 * r3 * r3 * a3 * r1 - 2 * a3 * r2 * b1 * a2 * b3 + a2 * r1 * r1 * a3 * r2 -
193 a2 * r3 * r3 * a3 * r2 - a1 * r3 * a3 * a2 * a2 - a1 * r3 * a3 * r1 * r1 +
194 a1 * r3 * a3 * r2 * r2 + a2 * r1 * a1 * b2 * b2 - a2 * r1 * a1 * r2 * r2 -
195 a2 * r1 * a3 * b2 * b2 + a2 * r1 * a3 * r2 * r2 - 2 * a2 * r1 * b1 * a3 * b2 -
196 a2 * r3 * a1 * b2 * b2 + a2 * r3 * a1 * r2 * r2 - a2 * r3 * a3 * a1 * a1 +
197 a2 * r3 * a3 * b1 * b1 + a2 * r3 * a3 * b2 * b2 + a2 * r3 * a3 * r1 * r1 -
198 a2 * r3 * a3 * r2 * r2 - 2 * a2 * r3 * b1 * a3 * b2 + 2 * a1 * r2 * a3 * a2 * a2 +
199 2 * a1 * r2 * a3 * b2 * b2 + a1 * r2 * a3 * r1 * r1 - a3 * r1 * a1 * a2 * a2 +
200 a3 * r1 * a1 * r2 * r2 - 2 * a3 * r1 * b1 * a2 * b3 + 4 * r1 * a3 * b2 * a2 * b3;
201 Obj tmp = a2 * b3 - a3 * b2 - a2 * b1 - a1 * b3 + a1 * b2 + a3 * b1;
202 Obj d = sqrt((a2 * a2 + b2 * b2 - r2 * r2 - 2 * a2 * a3 + a3 * a3 - r3 * r3 - 2 * b3 * b2 +
203 b3 * b3 + 2 * r3 * r2) *
204 (b3 * b3 + b1 * b1 - r1 * r1 - r3 * r3 - 2 * b1 * b3 + 2 * r3 * r1 + a3 * a3 -
205 2 * a1 * a3 + a1 * a1) *
206 (a1 * a1 - 2 * a1 * a2 - r1 * r1 + b1 * b1 + b2 * b2 - r2 * r2 - 2 * b2 * b1 +
207 2 * r2 * r1 + a2 * a2) *
212 return tlp::Circle<Obj, OTYPE>(0, 0, 0);
214 Obj aa = -2 * a3 * b2 * b2 * a1 - 2 * a2 * b3 * b3 * a1 - 2 * a1 * a1 * b3 * b2 +
215 a3 * a3 * b2 * b2 + a2 * a2 * b3 * b3 - r1 * r1 * b3 * b3 - r1 * r1 * b2 * b2 +
216 a2 * a2 * b1 * b1 + a3 * a3 * b1 * b1 - a2 * a2 * r1 * r1 - a2 * a2 * r3 * r3 -
217 a3 * a3 * r2 * r2 - 2 * a3 * b2 * a2 * b3 + 2 * a3 * b2 * a2 * b1 +
218 2 * a2 * b3 * a3 * b1 - 2 * b1 * a2 * a2 * b3 - a3 * a3 * r1 * r1 -
219 2 * a3 * a3 * b2 * b1 + 2 * b1 * r3 * r3 * b2 + 2 * r1 * r1 * b3 * b2 +
220 2 * b3 * b1 * r2 * r2 - 2 * a2 * b1 * b1 * a3 + 2 * a2 * a3 * r1 * r1 -
221 b2 * b2 * r3 * r3 - b1 * b1 * r3 * r3 - b1 * b1 * r2 * r2 - r2 * r2 * b3 * b3 -
222 a1 * a1 * r3 * r3 - a1 * a1 * r2 * r2 + a1 * a1 * b3 * b3 + a1 * a1 * b2 * b2 +
223 2 * b2 * b2 * r3 * r1 + 2 * b1 * b1 * r3 * r2 + 2 * r1 * b3 * b3 * r2 -
224 2 * b2 * r3 * b1 * r2 - 2 * b2 * r3 * r1 * b3 + 2 * b2 * r3 * r2 * b3 +
225 2 * b1 * r3 * r1 * b3 - 2 * b1 * r3 * r1 * b2 - 2 * b1 * r3 * r2 * b3 -
226 2 * b1 * r2 * r1 * b3 + 2 * b1 * r2 * r1 * b2 - 2 * r1 * b2 * r2 * b3 +
227 2 * a1 * r3 * r3 * a2 + 2 * a1 * a1 * r3 * r2 + 2 * a2 * a2 * r1 * r3 +
228 2 * a1 * r2 * r2 * a3 + 2 * a3 * a3 * r1 * r2 - 2 * a1 * r3 * a2 * r1 +
229 2 * a1 * r3 * a3 * r1 + 2 * a2 * r1 * a1 * r2 - 2 * a2 * r3 * a1 * r2 -
230 2 * a2 * r3 * a3 * r1 - 2 * a1 * r2 * a3 * r1 - 2 * a1 * r3 * a3 * r2 -
231 2 * a2 * r1 * a3 * r2 + 2 * a2 * r3 * a3 * r2 + 2 * a2 * b3 * a1 * b2 +
232 2 * a3 * b2 * a1 * b3 + 2 * a2 * b1 * a1 * b3 - 2 * a2 * b1 * a1 * b2 -
233 2 * a1 * b3 * a3 * b1 + 2 * a1 * b2 * a3 * b1;
234 Obj R = 0.5 * v / aa;
236 (-2 * a1 * R * r2 - 2 * a3 * R * r1 + 2 * a2 * R * r1 + 2 * a3 * R * r2 -
237 2 * a2 * R * r3 + a1 * a3 * a3 + a1 * b3 * b3 - a1 * r3 * r3 + a2 * a1 * a1 +
238 a2 * b1 * b1 + a1 * r2 * r2 + 2 * a1 * R * r3 - a3 * b1 * b1 + a3 * a2 * a2 +
239 a3 * b2 * b2 + a3 * r1 * r1 - a3 * r2 * r2 - a2 * a3 * a3 - a2 * b3 * b3 -
240 a2 * r1 * r1 + a2 * r3 * r3 - a1 * a2 * a2 - a1 * b2 * b2 - a3 * a1 * a1) /
241 (a2 * b3 + a3 * b1 - a2 * b1 - a1 * b3 + a1 * b2 - a3 * b2);
243 (-a1 * a1 * b3 + a1 * a1 * b2 + 2 * R * r2 * b3 + b1 * a3 * a3 + b1 * b3 * b3 +
244 2 * b1 * R * r3 - 2 * R * r1 * b3 + 2 * R * r1 * b2 + b1 * r2 * r2 - b2 * a3 * a3 -
245 b2 * b3 * b3 + b2 * r3 * r3 - 2 * b2 * R * r3 - b1 * r3 * r3 - r2 * r2 * b3 +
246 a2 * a2 * b3 - r1 * r1 * b2 - b1 * b2 * b2 + b1 * b1 * b2 - b1 * b1 * b3 -
247 b1 * a2 * a2 + b2 * b2 * b3 + r1 * r1 * b3 - 2 * b1 * R * r2) /
248 (a2 * b3 + a3 * b1 - a2 * b1 - a1 * b3 + a1 * b2 - a3 * b2);
249 return tlp::Circle<Obj, OTYPE>(x, y, R);
254 result = tlp::enclosingCircle((*circles)[b1], (*circles)[b2]);
256 unsigned selectedCircle = popBack();
259 if (!(*circles)[selectedCircle].isIncludeIn(result)) {
260 result = enclosingCircle((*circles)[b1], (*circles)[b2], (*circles)[selectedCircle]);
261 pushFront(selectedCircle);
263 pushBack(selectedCircle);
270 result = (*circles)[b1];
272 unsigned selectedCircle = popBack();
275 if (!(*circles)[selectedCircle].isIncludeIn(result)) {
278 pushFront(selectedCircle);
280 pushBack(selectedCircle);
286 result = Circle<Obj, OTYPE>(0, 0, 0);
288 unsigned selectedCircle = popBack();
291 if (!(*circles)[selectedCircle].isIncludeIn(result)) {
294 pushFront(selectedCircle);
296 pushBack(selectedCircle);
300 bool isEmpty()
const {
301 return first == (last + 1) % enclosedCircles.size();
303 void pushFront(
unsigned c) {
304 first = (first + enclosedCircles.size() - 1) % enclosedCircles.size();
305 enclosedCircles[first] = c;
308 unsigned result = enclosedCircles[last];
309 last = (last + enclosedCircles.size() - 1) % enclosedCircles.size();
312 void pushBack(
unsigned c) {
313 last = (last + 1) % enclosedCircles.size();
314 enclosedCircles[last] = c;
318 OptimumCircleHull() : circles(nullptr), first(0), last(0), b1(0), b2(0) {}
320 tlp::Circle<Obj, OTYPE> operator()(
const std::vector<tlp::Circle<Obj, OTYPE>> &circlesSet) {
321 circles = &circlesSet;
322 enclosedCircles.resize(circlesSet.size() + 1);
324 last = circlesSet.size() - 1;
326 for (
unsigned i = 0; i < circlesSet.size(); ++i)
327 enclosedCircles[i] = i;
329 for (
unsigned i = circlesSet.size(); i > 0;) {
332 std::swap(enclosedCircles[idx], enclosedCircles[i]);
339 return OptimumCircleHull()(circles);
342 template <
typename Obj,
typename OTYPE>
343 std::ostream &tlp::operator<<(std::ostream &os, const tlp::Circle<Obj, OTYPE> &a) {
344 os <<
"((" << a[0] <<
"," << a[1] <<
")," << a.radius <<
")";
unsigned int randomUnsignedInteger(unsigned int max)
Returns a random unsigned integer in the range [0, max].