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;
235 Obj y = -0.5 * (-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);
241 Obj x = 0.5 * (-a1 * a1 * b3 + a1 * a1 * b2 + 2 * R * r2 * b3 + b1 * a3 * a3 + b1 * b3 * b3 +
242 2 * b1 * R * r3 - 2 * R * r1 * b3 + 2 * R * r1 * b2 + b1 * r2 * r2 -
243 b2 * a3 * a3 - b2 * b3 * b3 + b2 * r3 * r3 - 2 * b2 * R * r3 - b1 * r3 * r3 -
244 r2 * r2 * b3 + a2 * a2 * b3 - r1 * r1 * b2 - b1 * b2 * b2 + b1 * b1 * b2 -
245 b1 * b1 * b3 - b1 * a2 * a2 + b2 * b2 * b3 + r1 * r1 * b3 - 2 * b1 * R * r2) /
246 (a2 * b3 + a3 * b1 - a2 * b1 - a1 * b3 + a1 * b2 - a3 * b2);
247 return tlp::Circle<Obj, OTYPE>(x, y, R);
252 result = tlp::enclosingCircle((*circles)[b1], (*circles)[b2]);
254 unsigned selectedCircle = popBack();
257 if (!(*circles)[selectedCircle].isIncludeIn(result)) {
258 result = enclosingCircle((*circles)[b1], (*circles)[b2], (*circles)[selectedCircle]);
259 pushFront(selectedCircle);
261 pushBack(selectedCircle);
268 result = (*circles)[b1];
270 unsigned selectedCircle = popBack();
273 if (!(*circles)[selectedCircle].isIncludeIn(result)) {
276 pushFront(selectedCircle);
278 pushBack(selectedCircle);
284 result = Circle<Obj, OTYPE>(0, 0, 0);
286 unsigned selectedCircle = popBack();
289 if (!(*circles)[selectedCircle].isIncludeIn(result)) {
292 pushFront(selectedCircle);
294 pushBack(selectedCircle);
298 bool isEmpty()
const {
299 return first == (last + 1) % enclosedCircles.size();
301 void pushFront(
unsigned c) {
302 first = (first + enclosedCircles.size() - 1) % enclosedCircles.size();
303 enclosedCircles[first] = c;
306 unsigned result = enclosedCircles[last];
307 last = (last + enclosedCircles.size() - 1) % enclosedCircles.size();
310 void pushBack(
unsigned c) {
311 last = (last + 1) % enclosedCircles.size();
312 enclosedCircles[last] = c;
316 OptimumCircleHull() : circles(nullptr), first(0), last(0), b1(0), b2(0) {}
318 tlp::Circle<Obj, OTYPE> operator()(
const std::vector<tlp::Circle<Obj, OTYPE>> &circlesSet) {
319 circles = &circlesSet;
320 enclosedCircles.resize(circlesSet.size() + 1);
322 last = circlesSet.size() - 1;
324 for (
unsigned i = 0; i < circlesSet.size(); ++i)
325 enclosedCircles[i] = i;
327 for (
unsigned i = circlesSet.size(); i > 0;) {
330 std::swap(enclosedCircles[idx], enclosedCircles[i]);
337 return OptimumCircleHull()(circles);
340 template <
typename Obj,
typename OTYPE>
341 std::ostream &tlp::operator<<(std::ostream &os, const tlp::Circle<Obj, OTYPE> &a) {
342 os <<
"((" << a[0] <<
"," << a[1] <<
")," << a.radius <<
")";
unsigned int randomUnsignedInteger(unsigned int max)
Returns a random unsigned integer in the range [0, max].