| 197 | 197 |
* convex as well in this case.
|
| 198 | 198 |
*
|
| 199 | 199 |
*/
|
| 200 | |
template<class RawShape>
|
|
200 |
template<class RawShape, class Ratio = double>
|
| 201 | 201 |
inline NfpResult<RawShape> nfpConvexOnly(const RawShape& sh,
|
| 202 | 202 |
const RawShape& other)
|
| 203 | 203 |
{
|
|
| 235 | 235 |
}
|
| 236 | 236 |
|
| 237 | 237 |
auto anglcmpfn = [](const Edge& e1, const Edge& e2) {
|
| 238 | |
// the X axis:
|
| 239 | |
Vertex ax(1, 0);
|
|
238 |
Vertex ax(1, 0); // Unit vector for the X axis
|
| 240 | 239 |
|
|
240 |
// get cectors from the edges
|
| 241 | 241 |
Vertex p1 = e1.second() - e1.first();
|
| 242 | 242 |
Vertex p2 = e2.second() - e2.first();
|
| 243 | |
|
| 244 | |
std::array<TCompute<Vertex>, 2> lcos {
|
| 245 | |
pl::dot(p1, ax), pl::dot(p2, ax)
|
| 246 | |
};
|
| 247 | |
std::array<TCompute<Vertex>, 2> lsin {
|
| 248 | |
-pl::dotperp(p1, ax), -pl::dotperp(p2, ax)
|
| 249 | |
};
|
| 250 | |
|
| 251 | |
std::array<int, 2> q {0, 0};
|
|
243 |
|
|
244 |
// Quadrant mapping array. The quadrant of a vector can be determined
|
|
245 |
// from the dot product of the vector and its perpendicular pair
|
|
246 |
// with the unit vector X axis. The products will carry the values
|
|
247 |
// lcos = dot(p, ax) = l * cos(phi) and
|
|
248 |
// lsin = -dotperp(p, ax) = l * sin(phi) where
|
|
249 |
// l is the length of vector p. From the signs of these values we can
|
|
250 |
// construct an index which has the sign of lcos as MSB and the
|
|
251 |
// sign of lsin as LSB. This index can be used to retrieve the actual
|
|
252 |
// quadrant where vector p resides using the following map:
|
|
253 |
// (+ is 0, - is 1)
|
|
254 |
// cos | sin | decimal | quadrant
|
|
255 |
// + | + | 0 | 0
|
|
256 |
// + | - | 1 | 3
|
|
257 |
// - | + | 2 | 1
|
|
258 |
// - | - | 3 | 2
|
| 252 | 259 |
std::array<int, 4> quadrants {0, 3, 1, 2 };
|
| 253 | |
|
|
260 |
|
|
261 |
std::array<int, 2> q {0, 0}; // Quadrant indices for p1 and p2
|
|
262 |
|
|
263 |
using TDots = std::array<TCompute<Vertex>, 2>;
|
|
264 |
TDots lcos { pl::dot(p1, ax), pl::dot(p2, ax) };
|
|
265 |
TDots lsin { -pl::dotperp(p1, ax), -pl::dotperp(p2, ax) };
|
|
266 |
|
|
267 |
// Construct the quadrant indices for p1 and p2
|
| 254 | 268 |
for(size_t i = 0; i < 2; ++i)
|
| 255 | 269 |
if(lcos[i] == 0) q[i] = lsin[i] > 0 ? 1 : 3;
|
| 256 | 270 |
else if(lsin[i] == 0) q[i] = lcos[i] > 0 ? 0 : 2;
|
| 257 | 271 |
else q[i] = quadrants[((lcos[i] < 0) << 1) + (lsin[i] < 0)];
|
| 258 | 272 |
|
| 259 | |
if(q[0] == q[1]) {
|
| 260 | |
auto lsq1 = pl::magnsq(p1);
|
| 261 | |
auto lsq2 = pl::magnsq(p2);
|
|
273 |
if(q[0] == q[1]) { // only bother if p1 and p2 are in the same quadrant
|
|
274 |
auto lsq1 = pl::magnsq(p1); // squared magnitudes, avoid sqrt
|
|
275 |
auto lsq2 = pl::magnsq(p2); // squared magnitudes, avoid sqrt
|
|
276 |
|
|
277 |
// We will actually compare l^2 * cos^2(phi) which saturates the
|
|
278 |
// cos function. But with the quadrant info we can get the sign back
|
| 262 | 279 |
int sign = q[0] == 1 || q[0] == 2 ? -1 : 1;
|
| 263 | 280 |
|
| 264 | |
// TODO: use rational arithmetic
|
| 265 | |
double pcos1 = sign * double(lcos[0]) * lcos[0] / lsq1;
|
| 266 | |
double pcos2 = sign * double(lcos[1]) * lcos[1] / lsq2;
|
|
281 |
// If Ratio is an actual rational type, there is no precision loss
|
|
282 |
auto pcos1 = Ratio(lcos[0]) / lsq1 * sign * lcos[0];
|
|
283 |
auto pcos2 = Ratio(lcos[1]) / lsq2 * sign * lcos[1];
|
| 267 | 284 |
|
| 268 | 285 |
return q[0] < 2 ? pcos1 < pcos2 : pcos1 > pcos2;
|
| 269 | 286 |
}
|
| 270 | 287 |
|
|
288 |
// If in different quadrants, compare the quadrant indices only.
|
| 271 | 289 |
return q[0] > q[1];
|
| 272 | |
|
| 273 | |
// auto lcos1 = pl::dot(p1, ax);
|
| 274 | |
// auto lsin1 = -pl::dotperp(p1, ax);
|
| 275 | |
|
| 276 | |
|
| 277 | |
// auto lcos2 = pl::dot(p2, ax);
|
| 278 | |
// auto lsin2 = -pl::dotperp(p2, ax);
|
| 279 | |
|
| 280 | |
// std::array<int, 4> quadrants = {0, 3, 1, 2 };
|
| 281 | |
|
| 282 | |
// int quadr1 = 0;
|
| 283 | |
// int quadr2 = 0;
|
| 284 | |
|
| 285 | |
// if(lcos1 == 0) quadr1 = lsin1 > 0 ? 1 : 3;
|
| 286 | |
// else if(lsin1 == 0) quadr1 = lcos1 > 0 ? 0 : 2;
|
| 287 | |
// else quadr1 = quadrants[((lcos1 < 0) << 1) + (lsin1 < 0)];
|
| 288 | |
|
| 289 | |
// if(lcos2 == 0) quadr2 = lsin2 > 0 ? 1 : 3;
|
| 290 | |
// else if(lsin2 == 0) quadr2 = lcos2 > 0 ? 0 : 2;
|
| 291 | |
// else quadr2 = quadrants[((lcos2 < 0) << 1) + (lsin2 < 0)];
|
| 292 | |
|
| 293 | |
// switch(quadr1) {
|
| 294 | |
// case 0: assert(e1.angleToXaxis() < Pi/2); break;
|
| 295 | |
// case 1: assert(e1.angleToXaxis() < Pi); break;
|
| 296 | |
// case 2: assert(e1.angleToXaxis() < 3*Pi/2); break;
|
| 297 | |
// case 3: assert(e1.angleToXaxis() < 2*Pi); break;
|
| 298 | |
// }
|
| 299 | |
|
| 300 | |
// switch(quadr2) {
|
| 301 | |
// case 0: assert(e2.angleToXaxis() < Pi/2); break;
|
| 302 | |
// case 1: assert(e2.angleToXaxis() < Pi); break;
|
| 303 | |
// case 2: assert(e2.angleToXaxis() < 3*Pi/2); break;
|
| 304 | |
// case 3: assert(e2.angleToXaxis() < 2*Pi); break;
|
| 305 | |
// }
|
| 306 | |
|
| 307 | |
// if(quadr1 == quadr2) {
|
| 308 | |
// auto lsq1 = pl::magnsq(p1);
|
| 309 | |
// auto lsq2 = pl::magnsq(p2);
|
| 310 | |
// int sign = quadr1 == 1 || quadr1 == 2 ? -1 : 1;
|
| 311 | |
|
| 312 | |
// double pcos1 = sign * double(lcos1) * lcos1 / lsq1;
|
| 313 | |
// double pcos2 = sign * double(lcos2) * lcos2 / lsq2;
|
| 314 | |
|
| 315 | |
// return quadr1 < 2 ? pcos1 < pcos2 : pcos1 > pcos2;
|
| 316 | |
// }
|
| 317 | |
|
| 318 | |
// return quadr1 > quadr2;
|
| 319 | 290 |
};
|
| 320 | |
|
| 321 | |
// auto edgelist2 = edgelist;
|
| 322 | |
|
| 323 | |
// // Sort the edges by angle to X axis.
|
| 324 | |
// std::sort(edgelist.begin(), edgelist.end(),
|
| 325 | |
// [](const Edge& e1, const Edge& e2)
|
| 326 | |
// {
|
| 327 | |
// return e1.angleToXaxis() > e2.angleToXaxis();
|
| 328 | |
// });
|
| 329 | |
|
| 330 | |
// std::sort(edgelist2.begin(), edgelist2.end(), anglcmpfn);
|
|
291 |
|
| 331 | 292 |
std::sort(edgelist.begin(), edgelist.end(), anglcmpfn);
|
| 332 | 293 |
|
| 333 | 294 |
__nfp::buildPolygon(edgelist, rsh, top_nfp);
|