Solvespace

1
//-----------------------------------------------------------------------------
2
// Operations on polygons (planar, of line segment edges).
3
//
4
// Copyright 2008-2013 Jonathan Westhues.
5
//-----------------------------------------------------------------------------
6
#include "solvespace.h"
7

8
Vector STriangle::Normal() const {
9
    Vector ab = b.Minus(a), bc = c.Minus(b);
10
    return ab.Cross(bc);
11
}
12

13
double STriangle::MinAltitude() const {
14
    double altA = a.DistanceToLine(b, c.Minus(b)),
15
           altB = b.DistanceToLine(c, a.Minus(c)),
16
           altC = c.DistanceToLine(a, b.Minus(a));
17

18
    return min(altA, min(altB, altC));
19
}
20

21
bool STriangle::ContainsPoint(Vector p) const {
22
    Vector n = Normal();
23
    if(MinAltitude() < LENGTH_EPS) {
24
        // shouldn't happen; zero-area triangle
25
        return false;
26
    }
27
    return ContainsPointProjd(n.WithMagnitude(1), p);
28
}
29

30
bool STriangle::ContainsPointProjd(Vector n, Vector p) const {
31
    Vector ab = b.Minus(a), bc = c.Minus(b), ca = a.Minus(c);
32

33
    Vector no_ab = n.Cross(ab);
34
    if(no_ab.Dot(p) < no_ab.Dot(a) - LENGTH_EPS) return false;
35

36
    Vector no_bc = n.Cross(bc);
37
    if(no_bc.Dot(p) < no_bc.Dot(b) - LENGTH_EPS) return false;
38

39
    Vector no_ca = n.Cross(ca);
40
    if(no_ca.Dot(p) < no_ca.Dot(c) - LENGTH_EPS) return false;
41

42
    return true;
43
}
44

45
bool STriangle::Raytrace(const Vector &rayPoint, const Vector &rayDir,
46
                         double *t, Vector *inters) const {
47
    // Algorithm from: "Fast, Minimum Storage Ray/Triangle Intersection" by
48
    // Tomas Moeller and Ben Trumbore.
49

50
    // Find vectors for two edges sharing vertex A.
51
    Vector edge1 = b.Minus(a);
52
    Vector edge2 = c.Minus(a);
53

54
    // Begin calculating determinant - also used to calculate U parameter.
55
    Vector pvec = rayDir.Cross(edge2);
56

57
    // If determinant is near zero, ray lies in plane of triangle.
58
    // Also, cull back facing triangles here.
59
    double det = edge1.Dot(pvec);
60
    if(-det < LENGTH_EPS) return false;
61
    double inv_det = 1.0f / det;
62

63
    // Calculate distance from vertex A to ray origin.
64
    Vector tvec = rayPoint.Minus(a);
65

66
    // Calculate U parameter and test bounds.
67
    double u = tvec.Dot(pvec) * inv_det;
68
    if (u < 0.0f || u > 1.0f) return false;
69

70
    // Prepare to test V parameter.
71
    Vector qvec = tvec.Cross(edge1);
72

73
    // Calculate V parameter and test bounds.
74
    double v = rayDir.Dot(qvec) * inv_det;
75
    if (v < 0.0f || u + v > 1.0f) return false;
76

77
    // Calculate t, ray intersects triangle.
78
    *t = edge2.Dot(qvec) * inv_det;
79

80
    // Calculate intersection point.
81
    if(inters != NULL) *inters = rayPoint.Plus(rayDir.ScaledBy(*t));
82

83
    return true;
84
}
85

86
double STriangle::SignedVolume() const {
87
    return a.Dot(b.Cross(c)) / 6.0;
88
}
89

90
double STriangle::Area() const {
91
    Vector ab = a.Minus(b);
92
    Vector cb = c.Minus(b);
93
    return ab.Cross(cb).Magnitude() / 2.0;
94
}
95

96
bool STriangle::IsDegenerate() const {
97
    return a.OnLineSegment(b, c) ||
98
           b.OnLineSegment(a, c) ||
99
           c.OnLineSegment(a, b);
100
}
101

102
void STriangle::FlipNormal() {
103
    swap(a, b);
104
    swap(an, bn);
105
}
106

107
STriangle STriangle::Transform(Vector u, Vector v, Vector n) const {
108
    STriangle tr = *this;
109
    tr.a  = tr.a.ScaleOutOfCsys(u, v, n);
110
    tr.an = tr.an.ScaleOutOfCsys(u, v, n);
111
    tr.b  = tr.b.ScaleOutOfCsys(u, v, n);
112
    tr.bn = tr.bn.ScaleOutOfCsys(u, v, n);
113
    tr.c  = tr.c.ScaleOutOfCsys(u, v, n);
114
    tr.cn = tr.cn.ScaleOutOfCsys(u, v, n);
115
    return tr;
116
}
117

118
STriangle STriangle::From(STriMeta meta, Vector a, Vector b, Vector c) {
119
    STriangle tr = {};
120
    tr.meta = meta;
121
    tr.a = a;
122
    tr.b = b;
123
    tr.c = c;
124
    return tr;
125
}
126

127
SEdge SEdge::From(Vector a, Vector b) {
128
    SEdge se = {};
129
    se.a = a;
130
    se.b = b;
131
    return se;
132
}
133

134
bool SEdge::EdgeCrosses(Vector ea, Vector eb, Vector *ppi, SPointList *spl) const {
135
    Vector d = eb.Minus(ea);
136
    double t_eps = LENGTH_EPS/d.Magnitude();
137

138
    double t, tthis;
139
    bool skew;
140
    Vector pi;
141
    bool inOrEdge0, inOrEdge1;
142

143
    Vector dthis = b.Minus(a);
144
    double tthis_eps = LENGTH_EPS/dthis.Magnitude();
145

146
    if((ea.Equals(a) && eb.Equals(b)) ||
147
       (eb.Equals(a) && ea.Equals(b)))
148
    {
149
        if(ppi) *ppi = a;
150
        if(spl) spl->Add(a);
151
        return true;
152
    }
153

154
    // Can't just test if distance between d and a equals distance between d and b;
155
    // they could be on opposite sides, since we don't have the sign.
156
    double m = sqrt(d.Magnitude()*dthis.Magnitude());
157
    if(sqrt(fabs(d.Dot(dthis))) > (m - LENGTH_EPS)) {
158
        // The edges are parallel.
159
        if(fabs(a.DistanceToLine(ea, d)) > LENGTH_EPS) {
160
            // and not coincident, so can't be intersecting
161
            return false;
162
        }
163
        // The edges are coincident. Make sure that neither endpoint lies
164
        // on the other
165
        bool inters = false;
166
        double t;
167
        t = a.Minus(ea).DivProjected(d);
168
        if(t > t_eps && t < (1 - t_eps)) inters = true;
169
        t = b.Minus(ea).DivProjected(d);
170
        if(t > t_eps && t < (1 - t_eps)) inters = true;
171
        t = ea.Minus(a).DivProjected(dthis);
172
        if(t > tthis_eps && t < (1 - tthis_eps)) inters = true;
173
        t = eb.Minus(a).DivProjected(dthis);
174
        if(t > tthis_eps && t < (1 - tthis_eps)) inters = true;
175

176
        if(inters) {
177
            if(ppi) *ppi = a;
178
            if(spl) spl->Add(a);
179
            return true;
180
        } else {
181
            // So coincident but disjoint, okay.
182
            return false;
183
        }
184
    }
185

186
    // Lines are not parallel, so look for an intersection.
187
    pi = Vector::AtIntersectionOfLines(ea, eb, a, b,
188
                                       &skew,
189
                                       &t, &tthis);
190
    if(skew) return false;
191

192
    inOrEdge0 = (t     > -t_eps)     && (t     < (1 + t_eps));
193
    inOrEdge1 = (tthis > -tthis_eps) && (tthis < (1 + tthis_eps));
194

195
    if(inOrEdge0 && inOrEdge1) {
196
        if(a.Equals(ea) || b.Equals(ea) ||
197
           a.Equals(eb) || b.Equals(eb))
198
        {
199
            // Not an intersection if we share an endpoint with an edge
200
            return false;
201
        }
202
        // But it's an intersection if a vertex of one edge lies on the
203
        // inside of the other (or if they cross away from either's
204
        // vertex).
205
        if(ppi) *ppi = pi;
206
        if(spl) spl->Add(pi);
207
        return true;
208
    }
209
    return false;
210
}
211

212
void SEdgeList::Clear() {
213
    l.Clear();
214
}
215

216
void SEdgeList::AddEdge(Vector a, Vector b, int auxA, int auxB, int tag) {
217
    SEdge e = {};
218
    e.tag = tag;
219
    e.a = a;
220
    e.b = b;
221
    e.auxA = auxA;
222
    e.auxB = auxB;
223
    l.Add(&e);
224
}
225

226
bool SEdgeList::AssembleContour(Vector first, Vector last, SContour *dest,
227
                                SEdge *errorAt, bool keepDir, int start) const
228
{
229
    int i;
230

231
    dest->AddPoint(first);
232
    dest->AddPoint(last);
233

234
    do {
235
        for(i = start; i < l.n; i++) {
236
            /// @todo fix const!
237
            SEdge *se = const_cast<SEdge*>(&(l[i]));
238
            if(se->tag) continue;
239

240
            if(se->a.Equals(last)) {
241
                dest->AddPoint(se->b);
242
                last = se->b;
243
                se->tag = 1;
244
                break;
245
            }
246
            // Don't allow backwards edges if keepDir is true.
247
            if(!keepDir && se->b.Equals(last)) {
248
                dest->AddPoint(se->a);
249
                last = se->a;
250
                se->tag = 1;
251
                break;
252
            }
253
        }
254
        if(i >= l.n) {
255
            // Couldn't assemble a closed contour; mark where.
256
            if(errorAt) {
257
                errorAt->a = first;
258
                errorAt->b = last;
259
            }
260
            return false;
261
        }
262

263
    } while(!last.Equals(first));
264

265
    return true;
266
}
267

268
bool SEdgeList::AssemblePolygon(SPolygon *dest, SEdge *errorAt, bool keepDir) const {
269
    dest->Clear();
270

271
    bool allClosed = true;
272
    Vector first = Vector::From(0, 0, 0);
273
    Vector last  = Vector::From(0, 0, 0);
274
    int i;
275
    for(i = 0; i < l.n; i++) {
276
        if(!l[i].tag) {
277
            first = l[i].a;
278
            last = l[i].b;
279
            /// @todo fix const!
280
            const_cast<SEdge*>(&(l[i]))->tag = 1;
281
            // Create a new empty contour in our polygon, and finish assembling
282
            // into that contour.
283
            dest->AddEmptyContour();
284
            if(!AssembleContour(first, last, dest->l.Last(), errorAt, keepDir, i+1)) {
285
                allClosed = false;
286
            }
287
            // But continue assembling, even if some of the contours are open
288
        }
289
    }
290
    return allClosed;
291
}
292

293
//-----------------------------------------------------------------------------
294
// Test if the specified edge crosses any of the edges in our list. Two edges
295
// are not considered to cross if they share an endpoint (within LENGTH_EPS),
296
// but they are considered to cross if they are coincident and overlapping.
297
// If pi is not NULL, then a crossing is returned in that.
298
//-----------------------------------------------------------------------------
299
int SEdgeList::AnyEdgeCrossings(Vector a, Vector b, Vector *ppi, SPointList *spl) const {
300
    auto cnt = std::count_if(l.begin(), l.end(),
301
                             [&](SEdge const &se) { return se.EdgeCrosses(a, b, ppi, spl); });
302
    return static_cast<int>(cnt);
303
}
304

305
//-----------------------------------------------------------------------------
306
// Returns true if the intersecting edge list contains an edge that shares
307
// an endpoint with one of our edges.
308
//-----------------------------------------------------------------------------
309
bool SEdgeList::ContainsEdgeFrom(const SEdgeList *sel) const {
310
    for(const SEdge *se = l.First(); se; se = l.NextAfter(se)) {
311
        if(sel->ContainsEdge(se)) return true;
312
    }
313
    return false;
314
}
315
bool SEdgeList::ContainsEdge(const SEdge *set) const {
316
    for(const SEdge *se = l.First(); se; se = l.NextAfter(se)) {
317
        if((se->a).Equals(set->a) && (se->b).Equals(set->b)) return true;
318
        if((se->b).Equals(set->a) && (se->a).Equals(set->b)) return true;
319
    }
320
    return false;
321
}
322

323
//-----------------------------------------------------------------------------
324
// Remove unnecessary edges:
325
// - if two are anti-parallel then
326
//     if both=true, remove both
327
//     else remove only one.
328
// - if two are parallel then remove one.
329
//-----------------------------------------------------------------------------
330
void SEdgeList::CullExtraneousEdges(bool both) {
331
    l.ClearTags();
332
    for(int i = 0; i < l.n; i++) {
333
        SEdge *se = &(l[i]);
334
        for(int j = i + 1; j < l.n; j++) {
335
            SEdge *set = &(l[j]);
336
            if((set->a).Equals(se->a) && (set->b).Equals(se->b)) {
337
                // Two parallel edges exist; so keep only the first one.
338
                set->tag = 1;
339
            }
340
            if((set->a).Equals(se->b) && (set->b).Equals(se->a)) {
341
                // Two anti-parallel edges exist; if both=true, keep neither,
342
                // otherwise keep only one.
343
                if (both) se->tag = 1;
344
                set->tag = 1;
345
            }
346
        }
347
    }
348
    l.RemoveTagged();
349
}
350

351
//-----------------------------------------------------------------------------
352
// Make a kd-tree of edges. This is used for O(log(n)) implementations of stuff
353
// that would naively be O(n).
354
//-----------------------------------------------------------------------------
355
SKdNodeEdges *SKdNodeEdges::Alloc() {
356
    SKdNodeEdges *ne = (SKdNodeEdges *)AllocTemporary(sizeof(SKdNodeEdges));
357
    *ne = {};
358
    return ne;
359
}
360
SEdgeLl *SEdgeLl::Alloc() {
361
    SEdgeLl *sell = (SEdgeLl *)AllocTemporary(sizeof(SEdgeLl));
362
    *sell = {};
363
    return sell;
364
}
365
SKdNodeEdges *SKdNodeEdges::From(SEdgeList *sel) {
366
    SEdgeLl *sell = NULL;
367
    SEdge *se;
368
    for(se = sel->l.First(); se; se = sel->l.NextAfter(se)) {
369
        SEdgeLl *n = SEdgeLl::Alloc();
370
        n->se = se;
371
        n->next = sell;
372
        sell = n;
373
    }
374
    return SKdNodeEdges::From(sell);
375
}
376
SKdNodeEdges *SKdNodeEdges::From(SEdgeLl *sell) {
377
    SKdNodeEdges *n = SKdNodeEdges::Alloc();
378

379
    // Compute the midpoints (just mean, though median would be better) of
380
    // each component.
381
    Vector ptAve = Vector::From(0, 0, 0);
382
    SEdgeLl *flip;
383
    int totaln = 0;
384
    for(flip = sell; flip; flip = flip->next) {
385
        ptAve = ptAve.Plus(flip->se->a);
386
        ptAve = ptAve.Plus(flip->se->b);
387
        totaln++;
388
    }
389
    ptAve = ptAve.ScaledBy(1.0 / (2*totaln));
390

391
    // For each component, see how it splits.
392
    int ltln[3] = { 0, 0, 0 }, gtln[3] = { 0, 0, 0 };
393
    double badness[3];
394
    for(flip = sell; flip; flip = flip->next) {
395
        for(int i = 0; i < 3; i++) {
396
            if(flip->se->a.Element(i) < ptAve.Element(i) + KDTREE_EPS ||
397
               flip->se->b.Element(i) < ptAve.Element(i) + KDTREE_EPS)
398
            {
399
                ltln[i]++;
400
            }
401
            if(flip->se->a.Element(i) > ptAve.Element(i) - KDTREE_EPS ||
402
               flip->se->b.Element(i) > ptAve.Element(i) - KDTREE_EPS)
403
            {
404
                gtln[i]++;
405
            }
406
        }
407
    }
408
    for(int i = 0; i < 3; i++) {
409
        badness[i] = pow((double)ltln[i], 4) + pow((double)gtln[i], 4);
410
    }
411

412
    // Choose the least bad coordinate to split along.
413
    if(badness[0] < badness[1] && badness[0] < badness[2]) {
414
        n->which = 0;
415
    } else if(badness[1] < badness[2]) {
416
        n->which = 1;
417
    } else {
418
        n->which = 2;
419
    }
420
    n->c = ptAve.Element(n->which);
421

422
    if(totaln < 3 || totaln == gtln[n->which] || totaln == ltln[n->which]) {
423
        n->edges = sell;
424
        // and we're a leaf node
425
        return n;
426
    }
427

428
    // Sort the edges according to which side(s) of the split plane they're on.
429
    SEdgeLl *gtl = NULL, *ltl = NULL;
430
    for(flip = sell; flip; flip = flip->next) {
431
        if(flip->se->a.Element(n->which) < n->c + KDTREE_EPS ||
432
           flip->se->b.Element(n->which) < n->c + KDTREE_EPS)
433
        {
434
            SEdgeLl *selln = SEdgeLl::Alloc();
435
            selln->se = flip->se;
436
            selln->next = ltl;
437
            ltl = selln;
438
        }
439
        if(flip->se->a.Element(n->which) > n->c - KDTREE_EPS ||
440
           flip->se->b.Element(n->which) > n->c - KDTREE_EPS)
441
        {
442
            SEdgeLl *selln = SEdgeLl::Alloc();
443
            selln->se = flip->se;
444
            selln->next = gtl;
445
            gtl = selln;
446
        }
447
    }
448

449
    n->lt = SKdNodeEdges::From(ltl);
450
    n->gt = SKdNodeEdges::From(gtl);
451
    return n;
452
}
453

454
int SKdNodeEdges::AnyEdgeCrossings(Vector a, Vector b, int cnt,
455
        Vector *pi, SPointList *spl) const
456
{
457
    int inters = 0;
458
    if(gt && lt) {
459
        if(a.Element(which) < c + KDTREE_EPS ||
460
           b.Element(which) < c + KDTREE_EPS)
461
        {
462
            inters += lt->AnyEdgeCrossings(a, b, cnt, pi, spl);
463
        }
464
        if(a.Element(which) > c - KDTREE_EPS ||
465
           b.Element(which) > c - KDTREE_EPS)
466
        {
467
            inters += gt->AnyEdgeCrossings(a, b, cnt, pi, spl);
468
        }
469
    } else {
470
        SEdgeLl *sell;
471
        for(sell = edges; sell; sell = sell->next) {
472
            SEdge *se = sell->se;
473
            if(se->tag == cnt) continue;
474
            if(se->EdgeCrosses(a, b, pi, spl)) {
475
                inters++;
476
            }
477
            se->tag = cnt;
478
        }
479
    }
480
    return inters;
481
}
482

483
//-----------------------------------------------------------------------------
484
// We have an edge list that contains only collinear edges, maybe with more
485
// splits than necessary. Merge any collinear segments that join.
486
//-----------------------------------------------------------------------------
487
void SEdgeList::MergeCollinearSegments(Vector a, Vector b) {
488
    const Vector lineStart = a;
489
    const Vector lineDirection = b.Minus(a);
490
    std::sort(l.begin(), l.end(), [&](const SEdge &a, const SEdge &b) {
491
        double ta = (a.a.Minus(lineStart)).DivProjected(lineDirection);
492
        double tb = (b.a.Minus(lineStart)).DivProjected(lineDirection);
493

494
        return (ta < tb);
495
    });
496

497
    l.ClearTags();
498
    SEdge *prev = nullptr;
499
    for(auto &now : l) {
500
        if(prev != nullptr) {
501
            if((prev->b).Equals(now.a) && prev->auxA == now.auxA) {
502
                // The previous segment joins up to us; so merge it into us.
503
                prev->tag = 1;
504
                now.a     = prev->a;
505
            }
506
        }
507
        prev = &now;
508
    }
509
    l.RemoveTagged();
510
}
511

512
void SPointList::Clear() {
513
    l.Clear();
514
}
515

516
bool SPointList::ContainsPoint(Vector pt) const {
517
    return (IndexForPoint(pt) >= 0);
518
}
519

520
int SPointList::IndexForPoint(Vector pt) const {
521
    int i;
522
    for(i = 0; i < l.n; i++) {
523
        const SPoint *p = &(l[i]);
524
        if(pt.Equals(p->p)) {
525
            return i;
526
        }
527
    }
528
    // Not found, so return negative to indicate that.
529
    return -1;
530
}
531

532
void SPointList::IncrementTagFor(Vector pt) {
533
    SPoint *p;
534
    for(p = l.First(); p; p = l.NextAfter(p)) {
535
        if(pt.Equals(p->p)) {
536
            (p->tag)++;
537
            return;
538
        }
539
    }
540
    SPoint pa;
541
    pa.p = pt;
542
    pa.tag = 1;
543
    l.Add(&pa);
544
}
545

546
void SPointList::Add(Vector pt) {
547
    SPoint p = {};
548
    p.p = pt;
549
    l.Add(&p);
550
}
551

552
void SContour::AddPoint(Vector p) {
553
    SPoint sp;
554
    sp.tag = 0;
555
    sp.p = p;
556

557
    l.Add(&sp);
558
}
559

560
void SContour::MakeEdgesInto(SEdgeList *el) const {
561
    int i;
562
    for(i = 0; i < (l.n - 1); i++) {
563
        el->AddEdge(l[i].p, l[i+1].p);
564
    }
565
}
566

567
void SContour::CopyInto(SContour *dest) const {
568
    for(const SPoint *sp = l.First(); sp; sp = l.NextAfter(sp)) {
569
        dest->AddPoint(sp->p);
570
    }
571
}
572

573
void SContour::FindPointWithMinX() {
574
    xminPt = Vector::From(1e10, 1e10, 1e10);
575
    for(const SPoint *sp = l.First(); sp; sp = l.NextAfter(sp)) {
576
        if(sp->p.x < xminPt.x) {
577
            xminPt = sp->p;
578
        }
579
    }
580
}
581

582
Vector SContour::ComputeNormal() const {
583
    Vector n = Vector::From(0, 0, 0);
584

585
    for(int i = 0; i < l.n - 2; i++) {
586
        Vector u = (l[i+1].p).Minus(l[i+0].p).WithMagnitude(1);
587
        Vector v = (l[i+2].p).Minus(l[i+1].p).WithMagnitude(1);
588
        Vector nt = u.Cross(v);
589
        if(nt.Magnitude() > n.Magnitude()) {
590
            n = nt;
591
        }
592
    }
593
    return n.WithMagnitude(1);
594
}
595

596
Vector SContour::AnyEdgeMidpoint() const {
597
    ssassert(l.n >= 2, "Need two points to find a midpoint");
598
    return ((l[0].p).Plus(l[1].p)).ScaledBy(0.5);
599
}
600

601
bool SContour::IsClockwiseProjdToNormal(Vector n) const {
602
    // Degenerate things might happen as we draw; doesn't really matter
603
    // what we do then.
604
    if(n.Magnitude() < 0.01) return true;
605

606
    return (SignedAreaProjdToNormal(n) < 0);
607
}
608

609
double SContour::SignedAreaProjdToNormal(Vector n) const {
610
    // An arbitrary 2d coordinate system that has n as its normal
611
    Vector u = n.Normal(0);
612
    Vector v = n.Normal(1);
613

614
    double area = 0;
615
    for(int i = 0; i < (l.n - 1); i++) {
616
        double u0 = (l[i  ].p).Dot(u);
617
        double v0 = (l[i  ].p).Dot(v);
618
        double u1 = (l[i+1].p).Dot(u);
619
        double v1 = (l[i+1].p).Dot(v);
620

621
        area += ((v0 + v1)/2)*(u1 - u0);
622
    }
623
    return area;
624
}
625

626
bool SContour::ContainsPointProjdToNormal(Vector n, Vector p) const {
627
    Vector u = n.Normal(0);
628
    Vector v = n.Normal(1);
629

630
    double up = p.Dot(u);
631
    double vp = p.Dot(v);
632

633
    bool inside = false;
634
    for(int i = 0; i < (l.n - 1); i++) {
635
        double ua = (l[i  ].p).Dot(u);
636
        double va = (l[i  ].p).Dot(v);
637
        // The curve needs to be exactly closed; approximation is death.
638
        double ub = (l[(i+1)%(l.n-1)].p).Dot(u);
639
        double vb = (l[(i+1)%(l.n-1)].p).Dot(v);
640

641
        if ((((va <= vp) && (vp < vb)) ||
642
             ((vb <= vp) && (vp < va))) &&
643
            (up < (ub - ua) * (vp - va) / (vb - va) + ua))
644
        {
645
          inside = !inside;
646
        }
647
    }
648

649
    return inside;
650
}
651

652
void SContour::Reverse() {
653
    l.Reverse();
654
}
655

656

657
void SPolygon::Clear() {
658
    int i;
659
    for(i = 0; i < l.n; i++) {
660
        (l[i]).l.Clear();
661
    }
662
    l.Clear();
663
}
664

665
void SPolygon::AddEmptyContour() {
666
    SContour c = {};
667
    l.Add(&c);
668
}
669

670
void SPolygon::MakeEdgesInto(SEdgeList *el) const {
671
    int i;
672
    for(i = 0; i < l.n; i++) {
673
        (l[i]).MakeEdgesInto(el);
674
    }
675
}
676

677
Vector SPolygon::ComputeNormal() const {
678
    if(l.IsEmpty())
679
        return Vector::From(0, 0, 0);
680
    return (l[0]).ComputeNormal();
681
}
682

683
double SPolygon::SignedArea() const {
684
    double area = 0;
685
    // This returns the true area only if the contours are all oriented
686
    // correctly, with the holes backwards from the outer contours.
687
    for(const SContour *sc = l.First(); sc; sc = l.NextAfter(sc)) {
688
        area += sc->SignedAreaProjdToNormal(normal);
689
    }
690
    return area;
691
}
692

693
bool SPolygon::ContainsPoint(Vector p) const {
694
    return (WindingNumberForPoint(p) % 2) == 1;
695
}
696

697
size_t SPolygon::WindingNumberForPoint(Vector p) const {
698
    auto winding = std::count_if(l.begin(), l.end(), [&](const SContour &sc) {
699
        return sc.ContainsPointProjdToNormal(normal, p);
700
    });
701
    return winding;
702
}
703

704
void SPolygon::FixContourDirections() {
705
    // At output, the contour's tag will be 1 if we reversed it, else 0.
706
    l.ClearTags();
707

708
    // Outside curve looks counterclockwise, projected against our normal.
709
    int i, j;
710
    for(i = 0; i < l.n; i++) {
711
        SContour *sc = &(l[i]);
712
        if(sc->l.n < 2) continue;
713
        // The contours may not intersect, but they may share vertices; so
714
        // testing a vertex for point-in-polygon may fail, but the midpoint
715
        // of an edge is okay.
716
        Vector pt = (((sc->l[0]).p).Plus(sc->l[1].p)).ScaledBy(0.5);
717

718
        sc->timesEnclosed = 0;
719
        bool outer = true;
720
        for(j = 0; j < l.n; j++) {
721
            if(i == j) continue;
722
            SContour *sct = &(l[j]);
723
            if(sct->ContainsPointProjdToNormal(normal, pt)) {
724
                outer = !outer;
725
                (sc->timesEnclosed)++;
726
            }
727
        }
728

729
        bool clockwise = sc->IsClockwiseProjdToNormal(normal);
730
        if((clockwise && outer) || (!clockwise && !outer)) {
731
            sc->Reverse();
732
            sc->tag = 1;
733
        }
734
    }
735
}
736

737
bool SPolygon::IsEmpty() const {
738
    if(l.IsEmpty() || l[0].l.IsEmpty())
739
        return true;
740
    return false;
741
}
742

743
Vector SPolygon::AnyPoint() const {
744
    ssassert(!IsEmpty(), "Need at least one point");
745
    return l[0].l[0].p;
746
}
747

748
bool SPolygon::SelfIntersecting(Vector *intersectsAt) const {
749
    SEdgeList el = {};
750
    MakeEdgesInto(&el);
751
    SKdNodeEdges *kdtree = SKdNodeEdges::From(&el);
752

753
    int cnt = 1;
754
    el.l.ClearTags();
755

756
    bool ret = false;
757
    SEdge *se;
758
    for(se = el.l.First(); se; se = el.l.NextAfter(se)) {
759
        int inters = kdtree->AnyEdgeCrossings(se->a, se->b, cnt, intersectsAt);
760
        if(inters != 1) {
761
            ret = true;
762
            break;
763
        }
764
        cnt++;
765
    }
766

767
    el.Clear();
768
    return ret;
769
}
770

771
void SPolygon::InverseTransformInto(SPolygon *sp, Vector u, Vector v, Vector n) const {
772
    for(const SContour &sc : l) {
773
        SContour tsc = {};
774
        tsc.timesEnclosed = sc.timesEnclosed;
775
        for(const SPoint &sp : sc.l) {
776
            tsc.AddPoint(sp.p.DotInToCsys(u, v, n));
777
        }
778
        sp->l.Add(&tsc);
779
    }
780
}
781

782
//-----------------------------------------------------------------------------
783
// Low-quality routines to cutter radius compensate a polygon. Assumes the
784
// polygon is in the xy plane, and the contours all go in the right direction
785
// with respect to normal (0, 0, -1).
786
//-----------------------------------------------------------------------------
787
void SPolygon::OffsetInto(SPolygon *dest, double r) const {
788
    int i;
789
    dest->Clear();
790
    for(i = 0; i < l.n; i++) {
791
        const SContour *sc = &(l[i]);
792
        dest->AddEmptyContour();
793
        sc->OffsetInto(&(dest->l[dest->l.n-1]), r);
794
    }
795
}
796
//-----------------------------------------------------------------------------
797
// Calculate the intersection point of two lines. Returns true for success,
798
// false if they're parallel.
799
//-----------------------------------------------------------------------------
800
static bool IntersectionOfLines(double x0A, double y0A, double dxA, double dyA,
801
                                double x0B, double y0B, double dxB, double dyB,
802
                                double *xi, double *yi)
803
{
804
    double A[2][2];
805
    double b[2];
806

807
    // The line is given to us in the form:
808
    //    (x(t), y(t)) = (x0, y0) + t*(dx, dy)
809
    // so first rewrite it as
810
    //    (x - x0, y - y0) dot (dy, -dx) = 0
811
    //    x*dy - x0*dy - y*dx + y0*dx = 0
812
    //    x*dy - y*dx = (x0*dy - y0*dx)
813

814
    // So write the matrix, pre-pivoted.
815
    if(fabs(dyA) > fabs(dyB)) {
816
        A[0][0] = dyA;  A[0][1] = -dxA;  b[0] = x0A*dyA - y0A*dxA;
817
        A[1][0] = dyB;  A[1][1] = -dxB;  b[1] = x0B*dyB - y0B*dxB;
818
    } else {
819
        A[1][0] = dyA;  A[1][1] = -dxA;  b[1] = x0A*dyA - y0A*dxA;
820
        A[0][0] = dyB;  A[0][1] = -dxB;  b[0] = x0B*dyB - y0B*dxB;
821
    }
822

823
    // Check the determinant; if it's zero then no solution.
824
    if(fabs(A[0][0]*A[1][1] - A[0][1]*A[1][0]) < LENGTH_EPS) {
825
        return false;
826
    }
827

828
    // Solve
829
    double v = A[1][0] / A[0][0];
830
    A[1][0] -= A[0][0]*v;
831
    A[1][1] -= A[0][1]*v;
832
    b[1] -= b[0]*v;
833

834
    // Back-substitute.
835
    *yi = b[1] / A[1][1];
836
    *xi = (b[0] - A[0][1]*(*yi)) / A[0][0];
837

838
    return true;
839
}
840
void SContour::OffsetInto(SContour *dest, double r) const {
841
    int i;
842

843
    for(i = 0; i < l.n; i++) {
844
        Vector a, b, c;
845
        Vector dp, dn;
846
        double thetan, thetap;
847

848
        a = l[WRAP(i-1, (l.n-1))].p;
849
        b = l[WRAP(i,   (l.n-1))].p;
850
        c = l[WRAP(i+1, (l.n-1))].p;
851

852
        dp = a.Minus(b);
853
        thetap = atan2(dp.y, dp.x);
854

855
        dn = b.Minus(c);
856
        thetan = atan2(dn.y, dn.x);
857

858
        // A short line segment in a badly-generated polygon might look
859
        // okay but screw up our sense of direction.
860
        if(dp.Magnitude() < LENGTH_EPS || dn.Magnitude() < LENGTH_EPS) {
861
            continue;
862
        }
863

864
        if(thetan > thetap && (thetan - thetap) > PI) {
865
            thetap += 2*PI;
866
        }
867
        if(thetan < thetap && (thetap - thetan) > PI) {
868
            thetan += 2*PI;
869
        }
870

871
        if(fabs(thetan - thetap) < (1*PI)/180) {
872
            Vector p = { b.x - r*sin(thetap), b.y + r*cos(thetap), 0 };
873
            dest->AddPoint(p);
874
        } else if(thetan < thetap) {
875
            // This is an inside corner. We have two edges, Ep and En. Move
876
            // out from their intersection by radius, normal to En, and
877
            // then draw a line parallel to En. Move out from their
878
            // intersection by radius, normal to Ep, and then draw a second
879
            // line parallel to Ep. The point that we want to generate is
880
            // the intersection of these two lines--it removes as much
881
            // material as we can without removing any that we shouldn't.
882
            double px0, py0, pdx, pdy;
883
            double nx0, ny0, ndx, ndy;
884
            double x = 0.0, y = 0.0;
885

886
            px0 = b.x - r*sin(thetap);
887
            py0 = b.y + r*cos(thetap);
888
            pdx = cos(thetap);
889
            pdy = sin(thetap);
890

891
            nx0 = b.x - r*sin(thetan);
892
            ny0 = b.y + r*cos(thetan);
893
            ndx = cos(thetan);
894
            ndy = sin(thetan);
895

896
            IntersectionOfLines(px0, py0, pdx, pdy,
897
                                nx0, ny0, ndx, ndy,
898
                                &x, &y);
899

900
            dest->AddPoint(Vector::From(x, y, 0));
901
        } else {
902
            if(fabs(thetap - thetan) < (6*PI)/180) {
903
                Vector pp = { b.x - r*sin(thetap),
904
                              b.y + r*cos(thetap), 0 };
905
                dest->AddPoint(pp);
906

907
                Vector pn = { b.x - r*sin(thetan),
908
                              b.y + r*cos(thetan), 0 };
909
                dest->AddPoint(pn);
910
            } else {
911
                double theta;
912
                for(theta = thetap; theta <= thetan; theta += (6*PI)/180) {
913
                    Vector p = { b.x - r*sin(theta),
914
                                 b.y + r*cos(theta), 0 };
915
                    dest->AddPoint(p);
916
                }
917
            }
918
        }
919
    }
920
}
921

922