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 (it = circles.begin(); it != circles.end(); ++it)
114 template <
typename Obj,
typename OTYPE>
115 tlp::Circle<Obj, OTYPE> tlp::enclosingCircle(
const std::vector<tlp::Circle<Obj, OTYPE>> &circles) {
116 class OptimumCircleHull {
117 const std::vector<tlp::Circle<Obj, OTYPE>> *circles;
118 std::vector<unsigned> enclosedCircles;
119 unsigned first, last;
121 tlp::Circle<Obj, OTYPE> result;
123 static tlp::Circle<Obj, OTYPE> enclosingCircle(
const tlp::Circle<Obj, OTYPE> &c1,
124 const tlp::Circle<Obj, OTYPE> &c2,
125 const tlp::Circle<Obj, OTYPE> &c3) {
135 Obj b = -b1 * r2 * a1 * a1 * b3 + a3 * a3 * r2 * r2 * r2 + b2 * b2 * r3 * r3 * r3 +
136 b1 * b1 * r3 * r3 * r3 + b1 * b1 * r2 * r2 * r2 + r1 * r1 * r1 * b3 * b3 +
137 r1 * r1 * r1 * b2 * b2 + r2 * r2 * r2 * b3 * b3 + a1 * a1 * r3 * r3 * r3 +
138 a2 * a2 * r1 * r1 * r1 + a2 * a2 * r3 * r3 * r3 + a1 * a1 * r2 * r2 * r2 +
139 a3 * a3 * r1 * r1 * r1 - b2 * b2 * r3 * a3 * a3 - b2 * b2 * r3 * b3 * b3 -
140 2 * b2 * r3 * r3 * r3 * b1 - b1 * b1 * r3 * a3 * a3 - b1 * b1 * r3 * b3 * b3 -
141 b1 * b1 * r2 * a2 * a2 + 2 * b1 * b1 * r2 * b3 * b3 - b1 * b1 * r2 * r3 * r3 -
142 r1 * b3 * b3 * b3 * b2 + r1 * b3 * b3 * b3 * b1 + 2 * r1 * b2 * b2 * b3 * b3 -
143 r1 * b2 * b2 * r3 * r3 + r2 * b3 * b3 * b3 * b2 - r2 * b3 * b3 * b3 * b1 -
144 b2 * b2 * b2 * r3 * b1 + 2 * b2 * b2 * r3 * b1 * b1 + b2 * b2 * b2 * r3 * b3 -
145 b2 * b2 * r3 * r1 * r1 + b1 * b1 * b1 * r3 * b3 - b1 * b1 * b1 * r3 * b2 -
146 b1 * b1 * r3 * r2 * r2 - b1 * b1 * r2 * b2 * b2 - b1 * b1 * b1 * r2 * b3 +
147 b1 * b1 * b1 * r2 * b2 - 2 * b1 * r2 * r2 * r2 * b3 - r1 * b3 * b3 * b1 * b1 -
148 2 * r1 * r1 * r1 * b3 * b2 - r1 * b3 * b3 * r2 * r2 - r1 * b3 * b3 * a1 * a1 +
149 r1 * b2 * b2 * b2 * b1 - r1 * b2 * b2 * b1 * b1 - r1 * b2 * b2 * b2 * b3 -
150 r1 * b2 * b2 * a1 * a1 - r2 * b3 * b3 * a2 * a2 - r2 * b3 * b3 * b2 * b2 -
151 r2 * b3 * b3 * r1 * r1 - b2 * r3 * b1 * b1 * b3 + b2 * r3 * a2 * a2 * b3 +
152 b2 * r3 * r1 * r1 * b3 - b2 * r3 * r2 * r2 * b3 + b2 * r3 * a1 * a1 * b3 -
153 2 * b2 * r3 * a1 * a2 * b3 + a1 * a3 * b2 * b2 * r3 - 2 * a1 * a3 * b2 * b1 * r3 -
154 2 * a1 * a3 * b2 * b1 * r2 - 2 * a1 * a3 * b2 * r1 * b3 + a1 * a3 * b2 * b2 * r1 -
155 2 * a1 * a3 * b2 * r2 * b3 - b2 * r3 * b1 * a2 * a2 + 2 * b2 * r3 * b1 * a3 * a3 +
156 2 * b2 * r3 * b1 * b3 * b3 + b1 * r2 * b2 * a3 * a3 - b1 * r2 * b2 * b3 * b3 +
157 b1 * r2 * b2 * r3 * r3 + r1 * b3 * b1 * a2 * a2 - r1 * b3 * b2 * a3 * a3 +
158 r1 * b3 * b2 * r3 * r3 + r1 * b3 * b1 * a3 * a3 - r1 * b3 * b1 * r3 * r3 +
159 r1 * b2 * b1 * a2 * a2 + r1 * b2 * b1 * a3 * a3 - r1 * b2 * b1 * b3 * b3 +
160 r1 * b2 * b1 * r3 * r3 + 2 * r2 * b3 * b1 * a2 * a2 + r2 * b3 * b2 * a3 * a3 -
161 r2 * b3 * b2 * r3 * r3 - r2 * b3 * b1 * a3 * a3 + r2 * b3 * b1 * r3 * r3 +
162 b2 * r3 * b1 * r2 * r2 + 4 * b2 * r3 * a1 * a2 * b1 + b1 * r3 * a2 * a2 * b3 -
163 b1 * r3 * b2 * b2 * b3 - b1 * r3 * r1 * r1 * b3 + b1 * r3 * r1 * r1 * b2 +
164 b1 * r3 * r2 * r2 * b3 + b1 * r3 * a1 * a1 * b3 - b1 * r3 * a1 * a1 * b2 -
165 2 * b1 * r3 * a1 * a2 * b3 - b1 * b1 * r3 * a1 * a2 + b1 * b1 * r3 * a1 * a3 +
166 2 * b1 * r2 * b2 * b2 * b3 + b1 * r2 * r1 * r1 * b3 - b1 * r2 * r1 * r1 * b2 +
167 b1 * r2 * a1 * a1 * b2 - 2 * b1 * r2 * a1 * a2 * b3 + b1 * b1 * r2 * a1 * a2 -
168 b1 * b1 * r2 * a1 * a3 - r1 * b3 * b1 * b2 * b2 + 2 * r1 * b3 * b1 * b1 * b2 +
169 2 * r1 * b3 * a1 * a1 * b2 + r1 * b3 * b1 * r2 * r2 + r1 * b3 * b3 * a1 * a2 -
170 r1 * b2 * a2 * a2 * b3 + r1 * b2 * r2 * r2 * b3 - r1 * b2 * b1 * r2 * r2 -
171 2 * r1 * b2 * a1 * a2 * b3 - r2 * b3 * b1 * b1 * b2 + r2 * b3 * r1 * r1 * b2 +
172 r2 * b3 * a1 * a1 * b2 + r2 * b3 * b3 * a1 * a2 + 4 * r2 * b3 * a1 * a3 * b1 -
173 a1 * a1 * a3 * a3 * r3 + 2 * a1 * a1 * a3 * a3 * r2 + a1 * a3 * a3 * a3 * r1 -
174 a1 * a1 * b3 * b3 * r3 - a1 * b3 * b3 * a3 * r2 + a1 * r3 * r3 * a3 * r2 -
175 a2 * a1 * a1 * a3 * r2 + a2 * b1 * b1 * a3 * r2 + a2 * b3 * b3 * a3 * r2 -
176 a1 * a3 * a3 * a2 * r1 + 2 * a1 * a3 * a3 * a2 * r3 + 2 * a1 * b3 * b3 * a2 * r3 +
177 a1 * b3 * b3 * a3 * r1 - 2 * a1 * r3 * r3 * r3 * a2 - a1 * a1 * r3 * r3 * r2 -
178 a2 * a1 * a1 * a1 * r3 - a2 * a2 * a1 * a1 * r1 + 2 * a2 * a2 * a1 * a1 * r3 +
179 a2 * a1 * a1 * a1 * r2 - a2 * a2 * b1 * b1 * r1 + 2 * a2 * a2 * a3 * a3 * r1 -
180 a2 * a2 * a3 * a3 * r3 - a2 * a3 * a3 * a3 * r1 - a2 * a2 * b3 * b3 * r3 -
181 a2 * a2 * r1 * r1 * r3 - 2 * a2 * r1 * r1 * r1 * a3 + a1 * r3 * r3 * a2 * r1 -
182 a1 * r3 * r3 * a3 * r1 + 2 * a2 * a1 * a1 * a3 * r1 + 2 * a2 * b1 * b1 * a3 * r1 -
183 a2 * a3 * a3 * a1 * r2 - a2 * b3 * b3 * a3 * r1 + a2 * r1 * r1 * a1 * r3 -
184 a2 * r1 * r1 * a1 * r2 - a2 * a2 * r3 * r3 * r1 - a1 * a3 * a3 * a3 * r2 +
185 a2 * a3 * a3 * a3 * r2 - a1 * a1 * r3 * r2 * r2 + a1 * a1 * a1 * r3 * a3 +
186 a2 * a2 * a2 * r1 * a1 - a2 * a2 * a2 * r1 * a3 - a2 * a2 * a2 * r3 * a1 +
187 a2 * a2 * a2 * r3 * a3 - a1 * a1 * r2 * a2 * a2 - a1 * a1 * r2 * b2 * b2 -
188 a1 * a1 * a1 * r2 * a3 - 2 * a1 * r2 * r2 * r2 * a3 - a3 * a3 * r1 * a1 * a1 -
189 a3 * a3 * r1 * b1 * b1 - a3 * a3 * r1 * r2 * r2 - a3 * a3 * r2 * a2 * a2 -
190 a3 * a3 * r2 * b2 * b2 - a3 * a3 * r2 * r1 * r1 + a2 * r3 * r3 * a1 * r2 +
191 a2 * r3 * r3 * a3 * r1 - 2 * a3 * r2 * b1 * a2 * b3 + a2 * r1 * r1 * a3 * r2 -
192 a2 * r3 * r3 * a3 * r2 - a1 * r3 * a3 * a2 * a2 - a1 * r3 * a3 * r1 * r1 +
193 a1 * r3 * a3 * r2 * r2 + a2 * r1 * a1 * b2 * b2 - a2 * r1 * a1 * r2 * r2 -
194 a2 * r1 * a3 * b2 * b2 + a2 * r1 * a3 * r2 * r2 - 2 * a2 * r1 * b1 * a3 * b2 -
195 a2 * r3 * a1 * b2 * b2 + a2 * r3 * a1 * r2 * r2 - a2 * r3 * a3 * a1 * a1 +
196 a2 * r3 * a3 * b1 * b1 + a2 * r3 * a3 * b2 * b2 + a2 * r3 * a3 * r1 * r1 -
197 a2 * r3 * a3 * r2 * r2 - 2 * a2 * r3 * b1 * a3 * b2 + 2 * a1 * r2 * a3 * a2 * a2 +
198 2 * a1 * r2 * a3 * b2 * b2 + a1 * r2 * a3 * r1 * r1 - a3 * r1 * a1 * a2 * a2 +
199 a3 * r1 * a1 * r2 * r2 - 2 * a3 * r1 * b1 * a2 * b3 + 4 * r1 * a3 * b2 * a2 * b3;
200 Obj tmp = a2 * b3 - a3 * b2 - a2 * b1 - a1 * b3 + a1 * b2 + a3 * b1;
201 Obj d = sqrt((a2 * a2 + b2 * b2 - r2 * r2 - 2 * a2 * a3 + a3 * a3 - r3 * r3 - 2 * b3 * b2 +
202 b3 * b3 + 2 * r3 * r2) *
203 (b3 * b3 + b1 * b1 - r1 * r1 - r3 * r3 - 2 * b1 * b3 + 2 * r3 * r1 + a3 * a3 -
204 2 * a1 * a3 + a1 * a1) *
205 (a1 * a1 - 2 * a1 * a2 - r1 * r1 + b1 * b1 + b2 * b2 - r2 * r2 - 2 * b2 * b1 +
206 2 * r2 * r1 + a2 * a2) *
211 return tlp::Circle<Obj, OTYPE>(0, 0, 0);
213 Obj aa = -2 * a3 * b2 * b2 * a1 - 2 * a2 * b3 * b3 * a1 - 2 * a1 * a1 * b3 * b2 +
214 a3 * a3 * b2 * b2 + a2 * a2 * b3 * b3 - r1 * r1 * b3 * b3 - r1 * r1 * b2 * b2 +
215 a2 * a2 * b1 * b1 + a3 * a3 * b1 * b1 - a2 * a2 * r1 * r1 - a2 * a2 * r3 * r3 -
216 a3 * a3 * r2 * r2 - 2 * a3 * b2 * a2 * b3 + 2 * a3 * b2 * a2 * b1 +
217 2 * a2 * b3 * a3 * b1 - 2 * b1 * a2 * a2 * b3 - a3 * a3 * r1 * r1 -
218 2 * a3 * a3 * b2 * b1 + 2 * b1 * r3 * r3 * b2 + 2 * r1 * r1 * b3 * b2 +
219 2 * b3 * b1 * r2 * r2 - 2 * a2 * b1 * b1 * a3 + 2 * a2 * a3 * r1 * r1 -
220 b2 * b2 * r3 * r3 - b1 * b1 * r3 * r3 - b1 * b1 * r2 * r2 - r2 * r2 * b3 * b3 -
221 a1 * a1 * r3 * r3 - a1 * a1 * r2 * r2 + a1 * a1 * b3 * b3 + a1 * a1 * b2 * b2 +
222 2 * b2 * b2 * r3 * r1 + 2 * b1 * b1 * r3 * r2 + 2 * r1 * b3 * b3 * r2 -
223 2 * b2 * r3 * b1 * r2 - 2 * b2 * r3 * r1 * b3 + 2 * b2 * r3 * r2 * b3 +
224 2 * b1 * r3 * r1 * b3 - 2 * b1 * r3 * r1 * b2 - 2 * b1 * r3 * r2 * b3 -
225 2 * b1 * r2 * r1 * b3 + 2 * b1 * r2 * r1 * b2 - 2 * r1 * b2 * r2 * b3 +
226 2 * a1 * r3 * r3 * a2 + 2 * a1 * a1 * r3 * r2 + 2 * a2 * a2 * r1 * r3 +
227 2 * a1 * r2 * r2 * a3 + 2 * a3 * a3 * r1 * r2 - 2 * a1 * r3 * a2 * r1 +
228 2 * a1 * r3 * a3 * r1 + 2 * a2 * r1 * a1 * r2 - 2 * a2 * r3 * a1 * r2 -
229 2 * a2 * r3 * a3 * r1 - 2 * a1 * r2 * a3 * r1 - 2 * a1 * r3 * a3 * r2 -
230 2 * a2 * r1 * a3 * r2 + 2 * a2 * r3 * a3 * r2 + 2 * a2 * b3 * a1 * b2 +
231 2 * a3 * b2 * a1 * b3 + 2 * a2 * b1 * a1 * b3 - 2 * a2 * b1 * a1 * b2 -
232 2 * a1 * b3 * a3 * b1 + 2 * a1 * b2 * a3 * b1;
233 Obj R = 0.5 * v / aa;
235 (-2 * a1 * R * r2 - 2 * a3 * R * r1 + 2 * a2 * R * r1 + 2 * a3 * R * r2 -
236 2 * a2 * R * r3 + a1 * a3 * a3 + a1 * b3 * b3 - a1 * r3 * r3 + a2 * a1 * a1 +
237 a2 * b1 * b1 + a1 * r2 * r2 + 2 * a1 * R * r3 - a3 * b1 * b1 + a3 * a2 * a2 +
238 a3 * b2 * b2 + a3 * r1 * r1 - a3 * r2 * r2 - a2 * a3 * a3 - a2 * b3 * b3 -
239 a2 * r1 * r1 + a2 * r3 * r3 - a1 * a2 * a2 - a1 * b2 * b2 - a3 * a1 * a1) /
240 (a2 * b3 + a3 * b1 - a2 * b1 - a1 * b3 + a1 * b2 - a3 * b2);
242 (-a1 * a1 * b3 + a1 * a1 * b2 + 2 * R * r2 * b3 + b1 * a3 * a3 + b1 * b3 * b3 +
243 2 * b1 * R * r3 - 2 * R * r1 * b3 + 2 * R * r1 * b2 + b1 * r2 * r2 - b2 * a3 * a3 -
244 b2 * b3 * b3 + b2 * r3 * r3 - 2 * b2 * R * r3 - b1 * r3 * r3 - r2 * r2 * b3 +
245 a2 * a2 * b3 - r1 * r1 * b2 - b1 * b2 * b2 + b1 * b1 * b2 - b1 * b1 * b3 -
246 b1 * a2 * a2 + b2 * b2 * b3 + r1 * r1 * b3 - 2 * b1 * R * r2) /
247 (a2 * b3 + a3 * b1 - a2 * b1 - a1 * b3 + a1 * b2 - a3 * b2);
248 return tlp::Circle<Obj, OTYPE>(x, y, R);
253 result = tlp::enclosingCircle((*circles)[sc1], (*circles)[sc2]);
255 unsigned selectedCircle = popBack();
258 if (!(*circles)[selectedCircle].isIncludeIn(result)) {
259 result = enclosingCircle((*circles)[sc1], (*circles)[sc2], (*circles)[selectedCircle]);
260 pushFront(selectedCircle);
262 pushBack(selectedCircle);
269 result = (*circles)[sc1];
271 unsigned selectedCircle = popBack();
274 if (!(*circles)[selectedCircle].isIncludeIn(result)) {
275 sc2 = selectedCircle;
277 pushFront(selectedCircle);
279 pushBack(selectedCircle);
285 result = Circle<Obj, OTYPE>(0, 0, 0);
287 unsigned selectedCircle = popBack();
290 if (!(*circles)[selectedCircle].isIncludeIn(result)) {
291 sc1 = selectedCircle;
293 pushFront(selectedCircle);
295 pushBack(selectedCircle);
299 bool isEmpty()
const {
300 return first == (last + 1) % enclosedCircles.size();
302 void pushFront(
unsigned c) {
303 first = (first + enclosedCircles.size() - 1) % enclosedCircles.size();
304 enclosedCircles[first] = c;
307 unsigned tmp = enclosedCircles[last];
308 last = (last + enclosedCircles.size() - 1) % enclosedCircles.size();
311 void pushBack(
unsigned c) {
312 last = (last + 1) % enclosedCircles.size();
313 enclosedCircles[last] = c;
317 OptimumCircleHull() : circles(nullptr), first(0), last(0), sc1(0), sc2(0) {}
319 tlp::Circle<Obj, OTYPE> operator()(
const std::vector<tlp::Circle<Obj, OTYPE>> &circlesSet) {
320 circles = &circlesSet;
321 enclosedCircles.resize(circlesSet.size() + 1);
323 last = circlesSet.size() - 1;
325 for (
unsigned i = 0; i < circlesSet.size(); ++i)
326 enclosedCircles[i] = i;
328 for (
unsigned i = circlesSet.size(); i > 0;) {
331 std::swap(enclosedCircles[idx], enclosedCircles[i]);
338 return OptimumCircleHull()(circles);
341 template <
typename Obj,
typename OTYPE>
342 std::ostream &
tlp::operator<<(std::ostream &os,
const tlp::Circle<Obj, OTYPE> &a) {
343 os <<
"((" << a[0] <<
"," << a[1] <<
")," << a.radius <<
")";
unsigned int randomUnsignedInteger(unsigned int max)
Returns a random unsigned integer in the range [0, max].
std::ostream & operator<<(std::ostream &os, const Array< T, N > &array)
operator << stream operator to easily print an array, or save it to a file.