zcy
/
solvespace
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
solvespace/polygon.cpp

862 lines
25 KiB
C++
Raw Blame History

#include "solvespace.h"
Vector STriangle::Normal(void) {
Vector ab = b.Minus(a), bc = c.Minus(b);
return ab.Cross(bc);
}
double STriangle::MinAltitude(void) {
double altA = a.DistanceToLine(b, c.Minus(b)),
altB = b.DistanceToLine(c, a.Minus(c)),
altC = c.DistanceToLine(a, b.Minus(a));
return min(altA, min(altB, altC));
}
bool STriangle::ContainsPoint(Vector p) {
Vector n = Normal();
if(MinAltitude() < LENGTH_EPS) {
// shouldn't happen; zero-area triangle
return false;
}
return ContainsPointProjd(n.WithMagnitude(1), p);
}
bool STriangle::ContainsPointProjd(Vector n, Vector p) {
Vector ab = b.Minus(a), bc = c.Minus(b), ca = a.Minus(c);
Vector no_ab = n.Cross(ab);
if(no_ab.Dot(p) < no_ab.Dot(a) - LENGTH_EPS) return false;
Vector no_bc = n.Cross(bc);
if(no_bc.Dot(p) < no_bc.Dot(b) - LENGTH_EPS) return false;
Vector no_ca = n.Cross(ca);
if(no_ca.Dot(p) < no_ca.Dot(c) - LENGTH_EPS) return false;
return true;
}
void STriangle::FlipNormal(void) {
SWAP(Vector, a, b);
SWAP(Vector, an, bn);
}
STriangle STriangle::From(STriMeta meta, Vector a, Vector b, Vector c) {
STriangle tr;
ZERO(&tr);
tr.meta = meta;
tr.a = a;
tr.b = b;
tr.c = c;
return tr;
}
SEdge SEdge::From(Vector a, Vector b) {
SEdge se = { 0, 0, 0, a, b };
return se;
}
bool SEdge::EdgeCrosses(Vector ea, Vector eb, Vector *ppi, SPointList *spl) {
Vector d = eb.Minus(ea);
double t_eps = LENGTH_EPS/d.Magnitude();
double dist_a, dist_b;
double t, tthis;
bool skew;
Vector pi;
bool inOrEdge0, inOrEdge1;
Vector dthis = b.Minus(a);
double tthis_eps = LENGTH_EPS/dthis.Magnitude();
if((ea.Equals(a) && eb.Equals(b)) ||
(eb.Equals(a) && ea.Equals(b)))
{
if(ppi) *ppi = a;
if(spl) spl->Add(a);
return true;
}
dist_a = a.DistanceToLine(ea, d),
dist_b = b.DistanceToLine(ea, d);
// Can't just test if dist_a equals dist_b; they could be on opposite
// sides, since it's unsigned.
double m = sqrt(d.Magnitude()*dthis.Magnitude());
if(sqrt(fabs(d.Dot(dthis))) > (m - LENGTH_EPS)) {
// The edges are parallel.
if(fabs(dist_a) > LENGTH_EPS) {
// and not coincident, so can't be interesecting
return false;
}
// The edges are coincident. Make sure that neither endpoint lies
// on the other
bool inters = false;
double t;
t = a.Minus(ea).DivPivoting(d);
if(t > t_eps && t < (1 - t_eps)) inters = true;
t = b.Minus(ea).DivPivoting(d);
if(t > t_eps && t < (1 - t_eps)) inters = true;
t = ea.Minus(a).DivPivoting(dthis);
if(t > tthis_eps && t < (1 - tthis_eps)) inters = true;
t = eb.Minus(a).DivPivoting(dthis);
if(t > tthis_eps && t < (1 - tthis_eps)) inters = true;
if(inters) {
if(ppi) *ppi = a;
if(spl) spl->Add(a);
return true;
} else {
// So coincident but disjoint, okay.
return false;
}
}
// Lines are not parallel, so look for an intersection.
pi = Vector::AtIntersectionOfLines(ea, eb, a, b,
&skew,
&t, &tthis);
if(skew) return false;
inOrEdge0 = (t > -t_eps) && (t < (1 + t_eps));
inOrEdge1 = (tthis > -tthis_eps) && (tthis < (1 + tthis_eps));
if(inOrEdge0 && inOrEdge1) {
if(a.Equals(ea) || b.Equals(ea) ||
a.Equals(eb) || b.Equals(eb))
{
// Not an intersection if we share an endpoint with an edge
return false;
}
// But it's an intersection if a vertex of one edge lies on the
// inside of the other (or if they cross away from either's
// vertex).
if(ppi) *ppi = pi;
if(spl) spl->Add(pi);
return true;
}
return false;
}
void SEdgeList::Clear(void) {
l.Clear();
}
void SEdgeList::AddEdge(Vector a, Vector b, int auxA, int auxB) {
SEdge e; ZERO(&e);
e.a = a;
e.b = b;
e.auxA = auxA;
e.auxB = auxB;
l.Add(&e);
}
bool SEdgeList::AssembleContour(Vector first, Vector last, SContour *dest,
SEdge *errorAt, bool keepDir)
{
int i;
dest->AddPoint(first);
dest->AddPoint(last);
do {
for(i = 0; i < l.n; i++) {
SEdge *se = &(l.elem[i]);
if(se->tag) continue;
if(se->a.Equals(last)) {
dest->AddPoint(se->b);
last = se->b;
se->tag = 1;
break;
}
// Don't allow backwards edges if keepDir is true.
if(!keepDir && se->b.Equals(last)) {
dest->AddPoint(se->a);
last = se->a;
se->tag = 1;
break;
}
}
if(i >= l.n) {
// Couldn't assemble a closed contour; mark where.
if(errorAt) {
errorAt->a = first;
errorAt->b = last;
}
return false;
}
} while(!last.Equals(first));
return true;
}
bool SEdgeList::AssemblePolygon(SPolygon *dest, SEdge *errorAt, bool keepDir) {
dest->Clear();
bool allClosed = true;
for(;;) {
Vector first, last;
int i;
for(i = 0; i < l.n; i++) {
if(!l.elem[i].tag) {
first = l.elem[i].a;
last = l.elem[i].b;
l.elem[i].tag = 1;
break;
}
}
if(i >= l.n) {
return allClosed;
}
// Create a new empty contour in our polygon, and finish assembling
// into that contour.
dest->AddEmptyContour();
if(!AssembleContour(first, last, &(dest->l.elem[dest->l.n-1]),
errorAt, keepDir))
{
allClosed = false;
}
// But continue assembling, even if some of the contours are open
}
}
//-----------------------------------------------------------------------------
// Test if the specified edge crosses any of the edges in our list. Two edges
// are not considered to cross if they share an endpoint (within LENGTH_EPS),
// but they are considered to cross if they are coincident and overlapping.
// If pi is not NULL, then a crossing is returned in that.
//-----------------------------------------------------------------------------
int SEdgeList::AnyEdgeCrossings(Vector a, Vector b,
Vector *ppi, SPointList *spl)
{
int cnt = 0;
SEdge *se;
for(se = l.First(); se; se = l.NextAfter(se)) {
if(se->EdgeCrosses(a, b, ppi, spl)) {
cnt++;
}
}
return cnt;
}
//-----------------------------------------------------------------------------
// Returns true if the intersecting edge list contains an edge that shares
// an endpoint with one of our edges.
//-----------------------------------------------------------------------------
bool SEdgeList::ContainsEdgeFrom(SEdgeList *sel) {
SEdge *se;
for(se = l.First(); se; se = l.NextAfter(se)) {
if(sel->ContainsEdge(se)) return true;
}
return false;
}
bool SEdgeList::ContainsEdge(SEdge *set) {
SEdge *se;
for(se = l.First(); se; se = l.NextAfter(se)) {
if((se->a).Equals(set->a) && (se->b).Equals(set->b)) return true;
if((se->b).Equals(set->a) && (se->a).Equals(set->b)) return true;
}
return false;
}
//-----------------------------------------------------------------------------
// Remove unnecessary edges: if two are anti-parallel then remove both, and if
// two are parallel then remove one.
//-----------------------------------------------------------------------------
void SEdgeList::CullExtraneousEdges(void) {
l.ClearTags();
int i, j;
for(i = 0; i < l.n; i++) {
SEdge *se = &(l.elem[i]);
for(j = i+1; j < l.n; j++) {
SEdge *set = &(l.elem[j]);
if((set->a).Equals(se->a) && (set->b).Equals(se->b)) {
// Two parallel edges exist; so keep only the first one.
set->tag = 1;
}
if((set->a).Equals(se->b) && (set->b).Equals(se->a)) {
// Two anti-parallel edges exist; so keep neither.
se->tag = 1;
set->tag = 1;
}
}
}
l.RemoveTagged();
}
//-----------------------------------------------------------------------------
// Make a kd-tree of edges. This is used for O(log(n)) implementations of stuff
// that would naively be O(n).
//-----------------------------------------------------------------------------
SKdNodeEdges *SKdNodeEdges::Alloc(void) {
return (SKdNodeEdges *)AllocTemporary(sizeof(SKdNodeEdges));
}
SEdgeLl *SEdgeLl::Alloc(void) {
return (SEdgeLl *)AllocTemporary(sizeof(SEdgeLl));
}
SKdNodeEdges *SKdNodeEdges::From(SEdgeList *sel) {
SEdgeLl *sell = NULL;
SEdge *se;
for(se = sel->l.First(); se; se = sel->l.NextAfter(se)) {
SEdgeLl *n = SEdgeLl::Alloc();
n->se = se;
n->next = sell;
sell = n;
}
return SKdNodeEdges::From(sell);
}
SKdNodeEdges *SKdNodeEdges::From(SEdgeLl *sell) {
SKdNodeEdges *n = SKdNodeEdges::Alloc();
// Compute the midpoints (just mean, though median would be better) of
// each component.
Vector ptAve = Vector::From(0, 0, 0);
SEdgeLl *flip;
int totaln = 0;
for(flip = sell; flip; flip = flip->next) {
ptAve = ptAve.Plus(flip->se->a);
ptAve = ptAve.Plus(flip->se->b);
totaln++;
}
ptAve = ptAve.ScaledBy(1.0 / (2*totaln));
// For each component, see how it splits.
int ltln[3] = { 0, 0, 0 }, gtln[3] = { 0, 0, 0 };
double badness[3];
for(flip = sell; flip; flip = flip->next) {
for(int i = 0; i < 3; i++) {
if(flip->se->a.Element(i) < ptAve.Element(i) + KDTREE_EPS ||
flip->se->b.Element(i) < ptAve.Element(i) + KDTREE_EPS)
{
ltln[i]++;
}
if(flip->se->a.Element(i) > ptAve.Element(i) - KDTREE_EPS ||
flip->se->b.Element(i) > ptAve.Element(i) - KDTREE_EPS)
{
gtln[i]++;
}
}
}
for(int i = 0; i < 3; i++) {
badness[i] = pow((double)ltln[i], 4) + pow((double)gtln[i], 4);
}
// Choose the least bad coordinate to split along.
if(badness[0] < badness[1] && badness[0] < badness[2]) {
n->which = 0;
} else if(badness[1] < badness[2]) {
n->which = 1;
} else {
n->which = 2;
}
n->c = ptAve.Element(n->which);
if(totaln < 3 || totaln == gtln[n->which] || totaln == ltln[n->which]) {
n->edges = sell;
// and we're a leaf node
return n;
}
// Sort the edges according to which side(s) of the split plane they're on.
SEdgeLl *gtl = NULL, *ltl = NULL;
for(flip = sell; flip; flip = flip->next) {
if(flip->se->a.Element(n->which) < n->c + KDTREE_EPS ||
flip->se->b.Element(n->which) < n->c + KDTREE_EPS)
{
SEdgeLl *selln = SEdgeLl::Alloc();
selln->se = flip->se;
selln->next = ltl;
ltl = selln;
}
if(flip->se->a.Element(n->which) > n->c - KDTREE_EPS ||
flip->se->b.Element(n->which) > n->c - KDTREE_EPS)
{
SEdgeLl *selln = SEdgeLl::Alloc();
selln->se = flip->se;
selln->next = gtl;
gtl = selln;
}
}
n->lt = SKdNodeEdges::From(ltl);
n->gt = SKdNodeEdges::From(gtl);
return n;
}
int SKdNodeEdges::AnyEdgeCrossings(Vector a, Vector b, int cnt,
Vector *pi, SPointList *spl)
{
int inters = 0;
if(gt && lt) {
if(a.Element(which) < c + KDTREE_EPS ||
b.Element(which) < c + KDTREE_EPS)
{
inters += lt->AnyEdgeCrossings(a, b, cnt, pi, spl);
}
if(a.Element(which) > c - KDTREE_EPS ||
b.Element(which) > c - KDTREE_EPS)
{
inters += gt->AnyEdgeCrossings(a, b, cnt, pi, spl);
}
} else {
SEdgeLl *sell;
for(sell = edges; sell; sell = sell->next) {
SEdge *se = sell->se;
if(se->tag == cnt) continue;
if(se->EdgeCrosses(a, b, pi, spl)) {
inters++;
}
se->tag = cnt;
}
}
return inters;
}
//-----------------------------------------------------------------------------
// We have an edge list that contains only collinear edges, maybe with more
// splits than necessary. Merge any collinear segments that join.
//-----------------------------------------------------------------------------
static Vector LineStart, LineDirection;
static int ByTAlongLine(const void *av, const void *bv)
{
SEdge *a = (SEdge *)av,
*b = (SEdge *)bv;
double ta = (a->a.Minus(LineStart)).DivPivoting(LineDirection),
tb = (b->a.Minus(LineStart)).DivPivoting(LineDirection);
return (ta > tb) ? 1 : -1;
}
void SEdgeList::MergeCollinearSegments(Vector a, Vector b) {
LineStart = a;
LineDirection = b.Minus(a);
qsort(l.elem, l.n, sizeof(l.elem[0]), ByTAlongLine);
l.ClearTags();
int i;
for(i = 1; i < l.n; i++) {
SEdge *prev = &(l.elem[i-1]),
*now = &(l.elem[i]);
if((prev->b).Equals(now->a)) {
// The previous segment joins up to us; so merge it into us.
prev->tag = 1;
now->a = prev->a;
}
}
l.RemoveTagged();
}
void SPointList::Clear(void) {
l.Clear();
}
bool SPointList::ContainsPoint(Vector pt) {
return (IndexForPoint(pt) >= 0);
}
int SPointList::IndexForPoint(Vector pt) {
int i;
for(i = 0; i < l.n; i++) {
SPoint *p = &(l.elem[i]);
if(pt.Equals(p->p)) {
return i;
}
}
// Not found, so return negative to indicate that.
return -1;
}
void SPointList::IncrementTagFor(Vector pt) {
SPoint *p;
for(p = l.First(); p; p = l.NextAfter(p)) {
if(pt.Equals(p->p)) {
(p->tag)++;
return;
}
}
SPoint pa;
pa.p = pt;
pa.tag = 1;
l.Add(&pa);
}
void SPointList::Add(Vector pt) {
SPoint p;
ZERO(&p);
p.p = pt;
l.Add(&p);
}
void SContour::AddPoint(Vector p) {
SPoint sp;
sp.tag = 0;
sp.p = p;
l.Add(&sp);
}
void SContour::MakeEdgesInto(SEdgeList *el) {
int i;
for(i = 0; i < (l.n - 1); i++) {
el->AddEdge(l.elem[i].p, l.elem[i+1].p);
}
}
void SContour::CopyInto(SContour *dest) {
SPoint *sp;
for(sp = l.First(); sp; sp = l.NextAfter(sp)) {
dest->AddPoint(sp->p);
}
}
void SContour::FindPointWithMinX(void) {
SPoint *sp;
xminPt = Vector::From(1e10, 1e10, 1e10);
for(sp = l.First(); sp; sp = l.NextAfter(sp)) {
if(sp->p.x < xminPt.x) {
xminPt = sp->p;
}
}
}
Vector SContour::ComputeNormal(void) {
Vector n = Vector::From(0, 0, 0);
for(int i = 0; i < l.n - 2; i++) {
Vector u = (l.elem[i+1].p).Minus(l.elem[i+0].p).WithMagnitude(1);
Vector v = (l.elem[i+2].p).Minus(l.elem[i+1].p).WithMagnitude(1);
Vector nt = u.Cross(v);
if(nt.Magnitude() > n.Magnitude()) {
n = nt;
}
}
return n.WithMagnitude(1);
}
Vector SContour::AnyEdgeMidpoint(void) {
if(l.n < 2) oops();
return ((l.elem[0].p).Plus(l.elem[1].p)).ScaledBy(0.5);
}
bool SContour::IsClockwiseProjdToNormal(Vector n) {
// Degenerate things might happen as we draw; doesn't really matter
// what we do then.
if(n.Magnitude() < 0.01) return true;
return (SignedAreaProjdToNormal(n) < 0);
}
double SContour::SignedAreaProjdToNormal(Vector n) {
// An arbitrary 2d coordinate system that has n as its normal
Vector u = n.Normal(0);
Vector v = n.Normal(1);
double area = 0;
for(int i = 0; i < (l.n - 1); i++) {
double u0 = (l.elem[i ].p).Dot(u);
double v0 = (l.elem[i ].p).Dot(v);
double u1 = (l.elem[i+1].p).Dot(u);
double v1 = (l.elem[i+1].p).Dot(v);
area += ((v0 + v1)/2)*(u1 - u0);
}
return area;
}
bool SContour::ContainsPointProjdToNormal(Vector n, Vector p) {
Vector u = n.Normal(0);
Vector v = n.Normal(1);
double up = p.Dot(u);
double vp = p.Dot(v);
bool inside = false;
for(int i = 0; i < (l.n - 1); i++) {
double ua = (l.elem[i ].p).Dot(u);
double va = (l.elem[i ].p).Dot(v);
// The curve needs to be exactly closed; approximation is death.
double ub = (l.elem[(i+1)%(l.n-1)].p).Dot(u);
double vb = (l.elem[(i+1)%(l.n-1)].p).Dot(v);
if ((((va <= vp) && (vp < vb)) ||
((vb <= vp) && (vp < va))) &&
(up < (ub - ua) * (vp - va) / (vb - va) + ua))
{
inside = !inside;
}
}
return inside;
}
void SContour::Reverse(void) {
l.Reverse();
}
void SPolygon::Clear(void) {
int i;
for(i = 0; i < l.n; i++) {
(l.elem[i]).l.Clear();
}
l.Clear();
}
void SPolygon::AddEmptyContour(void) {
SContour c;
memset(&c, 0, sizeof(c));
l.Add(&c);
}
void SPolygon::MakeEdgesInto(SEdgeList *el) {
int i;
for(i = 0; i < l.n; i++) {
(l.elem[i]).MakeEdgesInto(el);
}
}
Vector SPolygon::ComputeNormal(void) {
if(l.n < 1) return Vector::From(0, 0, 0);
return (l.elem[0]).ComputeNormal();
}
double SPolygon::SignedArea(void) {
SContour *sc;
double area = 0;
// This returns the true area only if the contours are all oriented
// correctly, with the holes backwards from the outer contours.
for(sc = l.First(); sc; sc = l.NextAfter(sc)) {
area += sc->SignedAreaProjdToNormal(normal);
}
return area;
}
bool SPolygon::ContainsPoint(Vector p) {
return (WindingNumberForPoint(p) % 2) == 1;
}
int SPolygon::WindingNumberForPoint(Vector p) {
int winding = 0;
int i;
for(i = 0; i < l.n; i++) {
SContour *sc = &(l.elem[i]);
if(sc->ContainsPointProjdToNormal(normal, p)) {
winding++;
}
}
return winding;
}
void SPolygon::FixContourDirections(void) {
// At output, the contour's tag will be 1 if we reversed it, else 0.
l.ClearTags();
// Outside curve looks counterclockwise, projected against our normal.
int i, j;
for(i = 0; i < l.n; i++) {
SContour *sc = &(l.elem[i]);
if(sc->l.n < 2) continue;
// The contours may not intersect, but they may share vertices; so
// testing a vertex for point-in-polygon may fail, but the midpoint
// of an edge is okay.
Vector pt = (((sc->l.elem[0]).p).Plus(sc->l.elem[1].p)).ScaledBy(0.5);
sc->timesEnclosed = 0;
bool outer = true;
for(j = 0; j < l.n; j++) {
if(i == j) continue;
SContour *sct = &(l.elem[j]);
if(sct->ContainsPointProjdToNormal(normal, pt)) {
outer = !outer;
(sc->timesEnclosed)++;
}
}
bool clockwise = sc->IsClockwiseProjdToNormal(normal);
if(clockwise && outer || (!clockwise && !outer)) {
sc->Reverse();
sc->tag = 1;
}
}
}
bool SPolygon::IsEmpty(void) {
if(l.n == 0 || l.elem[0].l.n == 0) return true;
return false;
}
Vector SPolygon::AnyPoint(void) {
if(IsEmpty()) oops();
return l.elem[0].l.elem[0].p;
}
bool SPolygon::SelfIntersecting(Vector *intersectsAt) {
SEdgeList el;
ZERO(&el);
MakeEdgesInto(&el);
SKdNodeEdges *kdtree = SKdNodeEdges::From(&el);
int cnt = 1;
el.l.ClearTags();
bool ret = false;
SEdge *se;
for(se = el.l.First(); se; se = el.l.NextAfter(se)) {
int inters = kdtree->AnyEdgeCrossings(se->a, se->b, cnt, intersectsAt);
if(inters != 1) {
ret = true;
break;
}
cnt++;
}
el.Clear();
return ret;
}
//-----------------------------------------------------------------------------
// Low-quality routines to cutter radius compensate a polygon. Assumes the
// polygon is in the xy plane, and the contours all go in the right direction
// with respect to normal (0, 0, -1).
//-----------------------------------------------------------------------------
void SPolygon::OffsetInto(SPolygon *dest, double r) {
int i;
dest->Clear();
for(i = 0; i < l.n; i++) {
SContour *sc = &(l.elem[i]);
dest->AddEmptyContour();
sc->OffsetInto(&(dest->l.elem[dest->l.n-1]), r);
}
}
//-----------------------------------------------------------------------------
// Calculate the intersection point of two lines. Returns true for success,
// false if they're parallel.
//-----------------------------------------------------------------------------
static bool IntersectionOfLines(double x0A, double y0A, double dxA, double dyA,
double x0B, double y0B, double dxB, double dyB,
double *xi, double *yi)
{
double A[2][2];
double b[2];
// The line is given to us in the form:
// (x(t), y(t)) = (x0, y0) + t*(dx, dy)
// so first rewrite it as
// (x - x0, y - y0) dot (dy, -dx) = 0
// x*dy - x0*dy - y*dx + y0*dx = 0
// x*dy - y*dx = (x0*dy - y0*dx)
// So write the matrix, pre-pivoted.
if(fabs(dyA) > fabs(dyB)) {
A[0][0] = dyA; A[0][1] = -dxA; b[0] = x0A*dyA - y0A*dxA;
A[1][0] = dyB; A[1][1] = -dxB; b[1] = x0B*dyB - y0B*dxB;
} else {
A[1][0] = dyA; A[1][1] = -dxA; b[1] = x0A*dyA - y0A*dxA;
A[0][0] = dyB; A[0][1] = -dxB; b[0] = x0B*dyB - y0B*dxB;
}
// Check the determinant; if it's zero then no solution.
if(fabs(A[0][0]*A[1][1] - A[0][1]*A[1][0]) < LENGTH_EPS) {
return false;
}
// Solve
double v = A[1][0] / A[0][0];
A[1][0] -= A[0][0]*v;
A[1][1] -= A[0][1]*v;
b[1] -= b[0]*v;
// Back-substitute.
*yi = b[1] / A[1][1];
*xi = (b[0] - A[0][1]*(*yi)) / A[0][0];
return true;
}
void SContour::OffsetInto(SContour *dest, double r) {
int i;
for(i = 0; i < l.n; i++) {
Vector a, b, c;
Vector dp, dn;
double thetan, thetap;
a = l.elem[WRAP(i-1, (l.n-1))].p;
b = l.elem[WRAP(i, (l.n-1))].p;
c = l.elem[WRAP(i+1, (l.n-1))].p;
dp = a.Minus(b);
thetap = atan2(dp.y, dp.x);
dn = b.Minus(c);
thetan = atan2(dn.y, dn.x);
// A short line segment in a badly-generated polygon might look
// okay but screw up our sense of direction.
if(dp.Magnitude() < LENGTH_EPS || dn.Magnitude() < LENGTH_EPS) {
continue;
}
if(thetan > thetap && (thetan - thetap) > PI) {
thetap += 2*PI;
}
if(thetan < thetap && (thetap - thetan) > PI) {
thetan += 2*PI;
}
if(fabs(thetan - thetap) < (1*PI)/180) {
Vector p = { b.x - r*sin(thetap), b.y + r*cos(thetap) };
dest->AddPoint(p);
} else if(thetan < thetap) {
// This is an inside corner. We have two edges, Ep and En. Move
// out from their intersection by radius, normal to En, and
// then draw a line parallel to En. Move out from their
// intersection by radius, normal to Ep, and then draw a second
// line parallel to Ep. The point that we want to generate is
// the intersection of these two lines--it removes as much
// material as we can without removing any that we shouldn't.
double px0, py0, pdx, pdy;
double nx0, ny0, ndx, ndy;
double x, y;
px0 = b.x - r*sin(thetap);
py0 = b.y + r*cos(thetap);
pdx = cos(thetap);
pdy = sin(thetap);
nx0 = b.x - r*sin(thetan);
ny0 = b.y + r*cos(thetan);
ndx = cos(thetan);
ndy = sin(thetan);
IntersectionOfLines(px0, py0, pdx, pdy,
nx0, ny0, ndx, ndy,
&x, &y);
dest->AddPoint(Vector::From(x, y, 0));
} else {
if(fabs(thetap - thetan) < (6*PI)/180) {
Vector pp = { b.x - r*sin(thetap),
b.y + r*cos(thetap), 0 };
dest->AddPoint(pp);
Vector pn = { b.x - r*sin(thetan),
b.y + r*cos(thetan), 0 };
dest->AddPoint(pn);
} else {
double theta;
for(theta = thetap; theta <= thetan; theta += (6*PI)/180) {
Vector p = { b.x - r*sin(theta),
b.y + r*cos(theta), 0 };
dest->AddPoint(p);
}
}
}
}
}
Reference in New Issue View Git Blame Copy Permalink