From 304c8f8be9a8f7aacbbf37a97272c163610560a9 Mon Sep 17 00:00:00 2001 From: Jonathan Westhues Date: Sun, 20 Jul 2008 04:24:43 -0800 Subject: [PATCH] Add equal-angle constraints. These are implemented by brute force, as the difference between the cosines of the two angles. All of the angle stuff generates huge expressions (Expr *), but doesn't seem noticeably slow. [git-p4: depot-paths = "//depot/solvespace/": change = 1847] --- constraint.cpp | 86 +++++++++++++++++++----- drawconstraint.cpp | 158 +++++++++++++++++++++++++++------------------ file.cpp | 3 +- generate.cpp | 5 +- sketch.h | 7 +- 5 files changed, 176 insertions(+), 83 deletions(-) diff --git a/constraint.cpp b/constraint.cpp index 6e91c30..aa73311 100644 --- a/constraint.cpp +++ b/constraint.cpp @@ -35,6 +35,7 @@ char *Constraint::DescriptionString(void) { case CUBIC_LINE_TANGENT:s = "cubic-line-tangent"; break; case PERPENDICULAR: s = "perpendicular"; break; case EQUAL_RADIUS: s = "eq-radius"; break; + case EQUAL_ANGLE: s = "eq-angle"; break; case COMMENT: s = "comment"; break; default: s = "???"; break; } @@ -199,6 +200,18 @@ void Constraint::MenuConstrain(int id) { c.entityA = gs.entity[0]; c.entityB = gs.entity[1]; c.ptA = gs.point[0]; + } else if(gs.vectors == 4 && gs.n == 4) { + c.type = EQUAL_ANGLE; + c.entityA = gs.vector[0]; + c.entityB = gs.vector[1]; + c.entityC = gs.vector[2]; + c.entityD = gs.vector[3]; + } else if(gs.vectors == 3 && gs.n == 3) { + c.type = EQUAL_ANGLE; + c.entityA = gs.vector[0]; + c.entityB = gs.vector[1]; + c.entityC = gs.vector[1]; + c.entityD = gs.vector[2]; } else if(gs.circlesOrArcs == 2 && gs.n == 2) { c.type = EQUAL_RADIUS; c.entityA = gs.entity[0]; @@ -213,6 +226,10 @@ void Constraint::MenuConstrain(int id) { "(equal point-line distances)\r\n" " * a line segment, and a point and line segment " "(point-line distance equals length)\r\n" + " * four line segments or normals " + "(equal angle between A,B and C,D)\r\n" + " * three line segments or normals " + "(equal angle between A,B and B,C)\r\n" " * two circles or arcs (equal radius)\r\n"); return; } @@ -428,10 +445,18 @@ void Constraint::MenuConstrain(int id) { if(gs.constraints == 1 && gs.n == 0) { Constraint *c = SS.GetConstraint(gs.constraint[0]); if(c->type == ANGLE) { + SS.UndoRemember(); c->other = !(c->other); c->ModifyToSatisfy(); break; } + if(c->type == EQUAL_ANGLE) { + SS.UndoRemember(); + c->other = !(c->other); + SS.MarkGroupDirty(c->group); + SS.later.generateAll = true; + break; + } } Error("Must select an angle constraint."); return; @@ -663,6 +688,29 @@ Expr *Constraint::Distance(hEntity wrkpl, hEntity hpa, hEntity hpb) { } } +//----------------------------------------------------------------------------- +// Return the cosine of the angle between two vectors. If a workplane is +// specified, then it's the cosine of their projections into that workplane. +//----------------------------------------------------------------------------- +Expr *Constraint::DirectionCosine(hEntity wrkpl, ExprVector ae, ExprVector be) { + if(wrkpl.v == Entity::FREE_IN_3D.v) { + Expr *mags = (ae.Magnitude())->Times(be.Magnitude()); + return (ae.Dot(be))->Div(mags); + } else { + Entity *w = SS.GetEntity(wrkpl); + ExprVector u = w->Normal()->NormalExprsU(); + ExprVector v = w->Normal()->NormalExprsV(); + Expr *ua = u.Dot(ae); + Expr *va = v.Dot(ae); + Expr *ub = u.Dot(be); + Expr *vb = v.Dot(be); + Expr *maga = (ua->Square()->Plus(va->Square()))->Sqrt(); + Expr *magb = (ub->Square()->Plus(vb->Square()))->Sqrt(); + Expr *dot = (ua->Times(ub))->Plus(va->Times(vb)); + return dot->Div(maga->Times(magb)); + } +} + ExprVector Constraint::PointInThreeSpace(hEntity workplane, Expr *u, Expr *v) { Entity *w = SS.GetEntity(workplane); @@ -1076,23 +1124,8 @@ void Constraint::GenerateReal(IdList *l) { ExprVector ae = a->VectorGetExprs(); ExprVector be = b->VectorGetExprs(); if(other) ae = ae.ScaledBy(Expr::From(-1)); - Expr *c; - if(workplane.v == Entity::FREE_IN_3D.v) { - Expr *mags = (ae.Magnitude())->Times(be.Magnitude()); - c = (ae.Dot(be))->Div(mags); - } else { - Entity *w = SS.GetEntity(workplane); - ExprVector u = w->Normal()->NormalExprsU(); - ExprVector v = w->Normal()->NormalExprsV(); - Expr *ua = u.Dot(ae); - Expr *va = v.Dot(ae); - Expr *ub = u.Dot(be); - Expr *vb = v.Dot(be); - Expr *maga = (ua->Square()->Plus(va->Square()))->Sqrt(); - Expr *magb = (ub->Square()->Plus(vb->Square()))->Sqrt(); - Expr *dot = (ua->Times(ub))->Plus(va->Times(vb)); - c = dot->Div(maga->Times(magb)); - } + Expr *c = DirectionCosine(workplane, ae, be); + if(type == ANGLE) { // The direction cosine is equal to the cosine of the // specified angle @@ -1106,6 +1139,25 @@ void Constraint::GenerateReal(IdList *l) { break; } + case EQUAL_ANGLE: { + Entity *a = SS.GetEntity(entityA); + Entity *b = SS.GetEntity(entityB); + Entity *c = SS.GetEntity(entityC); + Entity *d = SS.GetEntity(entityD); + ExprVector ae = a->VectorGetExprs(); + ExprVector be = b->VectorGetExprs(); + ExprVector ce = c->VectorGetExprs(); + ExprVector de = d->VectorGetExprs(); + + if(other) ae = ae.ScaledBy(Expr::From(-1)); + + Expr *cab = DirectionCosine(workplane, ae, be); + Expr *ccd = DirectionCosine(workplane, ce, de); + + AddEq(l, cab->Minus(ccd), 0); + break; + } + case ARC_LINE_TANGENT: { Entity *arc = SS.GetEntity(entityA); Entity *line = SS.GetEntity(entityB); diff --git a/drawconstraint.cpp b/drawconstraint.cpp index 96da52b..54782f6 100644 --- a/drawconstraint.cpp +++ b/drawconstraint.cpp @@ -104,6 +104,75 @@ void Constraint::DoEqualLenTicks(Vector a, Vector b, Vector gn) { LineDrawOrGetDistance(m.Minus(n), m.Plus(n)); } +void Constraint::DoArcForAngle(Vector a0, Vector da, Vector b0, Vector db, + Vector offset, Vector *ref) +{ + Vector gr = SS.GW.projRight.ScaledBy(1/SS.GW.scale); + Vector gu = SS.GW.projUp.ScaledBy(1/SS.GW.scale); + + if(workplane.v != Entity::FREE_IN_3D.v) { + a0 = a0.ProjectInto(workplane); + b0 = b0.ProjectInto(workplane); + da = da.ProjectVectorInto(workplane); + db = db.ProjectVectorInto(workplane); + } + + // Make an orthogonal coordinate system from those directions + Vector dn = da.Cross(db); // normal to both + Vector dna = dn.Cross(da); // normal to da + Vector dnb = dn.Cross(db); // normal to db + // At the intersection of the lines + // a0 + pa*da = b0 + pb*db (where pa, pb are scalar params) + // So dot this equation against dna and dnb to get two equations + // to solve for da and db + double pb = ((a0.Minus(b0)).Dot(dna))/(db.Dot(dna)); + double pa = -((a0.Minus(b0)).Dot(dnb))/(da.Dot(dnb)); + + Vector pi = a0.Plus(da.ScaledBy(pa)); + if(pi.Equals(b0.Plus(db.ScaledBy(pb)))) { + *ref = pi.Plus(offset); + // We draw in a coordinate system centered at pi, with + // basis vectors da and dna. + da = da.WithMagnitude(1); dna = dna.WithMagnitude(1); + Vector rm = (*ref).Minus(pi); + double rda = rm.Dot(da), rdna = rm.Dot(dna); + double r = sqrt(rda*rda + rdna*rdna); + double c = (da.Dot(db))/(da.Magnitude()*db.Magnitude()); + double thetaf = acos(c); + + Vector m = da.ScaledBy(cos(thetaf/2)).Plus( + dna.ScaledBy(sin(thetaf/2))); + if(m.Dot(rm) < 0) { + da = da.ScaledBy(-1); dna = dna.ScaledBy(-1); + } + + Vector prev = da.ScaledBy(r).Plus(pi); + int i, n = 30; + for(i = 0; i <= n; i++) { + double theta = (i*thetaf)/n; + Vector p = da. ScaledBy(r*cos(theta)).Plus( + dna.ScaledBy(r*sin(theta))).Plus(pi); + LineDrawOrGetDistance(prev, p); + prev = p; + } + + double tl = atan2(rm.Dot(gu), rm.Dot(gr)); + double adj = EllipticalInterpolation( + glxStrWidth(Label())/2, glxStrHeight()/2, tl); + *ref = (*ref).Plus(rm.WithMagnitude(adj + 3/SS.GW.scale)); + } else { + // The lines are skew; no wonderful way to illustrate that. + *ref = a0.Plus(b0); + *ref = (*ref).ScaledBy(0.5).Plus(disp.offset); + glPushMatrix(); + gu = gu.WithMagnitude(1); + glxTranslatev((*ref).Plus(gu.ScaledBy(-1.5*glxStrHeight()))); + glxOntoWorkplane(gr, gu); + glxWriteTextRefCenter("angle between skew lines"); + glPopMatrix(); + } +} + void Constraint::DrawOrGetDistance(Vector *labelPos) { if(!SS.GW.showConstraints) return; Group *g = SS.GetGroup(group); @@ -297,6 +366,32 @@ void Constraint::DrawOrGetDistance(Vector *labelPos) { break; } + case EQUAL_ANGLE: { + Vector ref; + Entity *a = SS.GetEntity(entityA); + Entity *b = SS.GetEntity(entityB); + Entity *c = SS.GetEntity(entityC); + Entity *d = SS.GetEntity(entityD); + + Vector a0 = a->VectorGetRefPoint(); + Vector b0 = b->VectorGetRefPoint(); + Vector c0 = c->VectorGetRefPoint(); + Vector d0 = d->VectorGetRefPoint(); + Vector da = a->VectorGetNum(); + Vector db = b->VectorGetNum(); + Vector dc = c->VectorGetNum(); + Vector dd = d->VectorGetNum(); + + if(other) da = da.ScaledBy(-1); + + DoArcForAngle(a0, da, b0, db, + da.WithMagnitude(40/SS.GW.scale), &ref); + DoArcForAngle(c0, dc, d0, dd, + dc.WithMagnitude(40/SS.GW.scale), &ref); + + break; + } + case ANGLE: { Entity *a = SS.GetEntity(entityA); Entity *b = SS.GetEntity(entityB); @@ -307,69 +402,8 @@ void Constraint::DrawOrGetDistance(Vector *labelPos) { Vector db = b->VectorGetNum(); if(other) da = da.ScaledBy(-1); - if(workplane.v != Entity::FREE_IN_3D.v) { - a0 = a0.ProjectInto(workplane); - b0 = b0.ProjectInto(workplane); - da = da.ProjectVectorInto(workplane); - db = db.ProjectVectorInto(workplane); - } - - // Make an orthogonal coordinate system from those directions - Vector dn = da.Cross(db); // normal to both - Vector dna = dn.Cross(da); // normal to da - Vector dnb = dn.Cross(db); // normal to db - // At the intersection of the lines - // a0 + pa*da = b0 + pb*db (where pa, pb are scalar params) - // So dot this equation against dna and dnb to get two equations - // to solve for da and db - double pb = ((a0.Minus(b0)).Dot(dna))/(db.Dot(dna)); - double pa = -((a0.Minus(b0)).Dot(dnb))/(da.Dot(dnb)); - - Vector pi = a0.Plus(da.ScaledBy(pa)); Vector ref; - if(pi.Equals(b0.Plus(db.ScaledBy(pb)))) { - ref = pi.Plus(disp.offset); - // We draw in a coordinate system centered at pi, with - // basis vectors da and dna. - da = da.WithMagnitude(1); dna = dna.WithMagnitude(1); - Vector rm = ref.Minus(pi); - double rda = rm.Dot(da), rdna = rm.Dot(dna); - double r = sqrt(rda*rda + rdna*rdna); - double c = (da.Dot(db))/(da.Magnitude()*db.Magnitude()); - double thetaf = acos(c); - - Vector m = da.ScaledBy(cos(thetaf/2)).Plus( - dna.ScaledBy(sin(thetaf/2))); - if(m.Dot(rm) < 0) { - da = da.ScaledBy(-1); dna = dna.ScaledBy(-1); - } - - Vector prev = da.ScaledBy(r).Plus(pi); - int i, n = 30; - for(i = 0; i <= n; i++) { - double theta = (i*thetaf)/n; - Vector p = da. ScaledBy(r*cos(theta)).Plus( - dna.ScaledBy(r*sin(theta))).Plus(pi); - LineDrawOrGetDistance(prev, p); - prev = p; - } - - double tl = atan2(rm.Dot(gu), rm.Dot(gr)); - double adj = EllipticalInterpolation( - glxStrWidth(Label())/2, glxStrHeight()/2, tl); - ref = ref.Plus(rm.WithMagnitude(adj + 3/SS.GW.scale)); - } else { - // The lines are skew; no wonderful way to illustrate that. - ref = a->VectorGetRefPoint().Plus(b->VectorGetRefPoint()); - ref = ref.ScaledBy(0.5).Plus(disp.offset); - glPushMatrix(); - gu = gu.WithMagnitude(1); - glxTranslatev(ref.Plus(gu.ScaledBy(-1.5*glxStrHeight()))); - glxOntoWorkplane(gr, gu); - glxWriteTextRefCenter("angle between skew lines"); - glPopMatrix(); - } - + DoArcForAngle(a0, da, b0, db, disp.offset, &ref); DoLabel(ref, labelPos, gr, gu); break; } diff --git a/file.cpp b/file.cpp index f27a645..e23d392 100644 --- a/file.cpp +++ b/file.cpp @@ -129,9 +129,10 @@ const SolveSpace::SaveTable SolveSpace::SAVED[] = { { 'c', "Constraint.valA", 'f', &(SS.sv.c.valA) }, { 'c', "Constraint.ptA.v", 'x', &(SS.sv.c.ptA.v) }, { 'c', "Constraint.ptB.v", 'x', &(SS.sv.c.ptB.v) }, - { 'c', "Constraint.ptC.v", 'x', &(SS.sv.c.ptC.v) }, { 'c', "Constraint.entityA.v", 'x', &(SS.sv.c.entityA.v) }, { 'c', "Constraint.entityB.v", 'x', &(SS.sv.c.entityB.v) }, + { 'c', "Constraint.entityC.v", 'x', &(SS.sv.c.entityC.v) }, + { 'c', "Constraint.entityD.v", 'x', &(SS.sv.c.entityD.v) }, { 'c', "Constraint.other", 'b', &(SS.sv.c.other) }, { 'c', "Constraint.reference", 'b', &(SS.sv.c.reference) }, { 'c', "Constraint.comment", 'N', &(SS.sv.c.comment) }, diff --git a/generate.cpp b/generate.cpp index 589904e..1a77aac 100644 --- a/generate.cpp +++ b/generate.cpp @@ -109,9 +109,10 @@ bool SolveSpace::PruneConstraints(hGroup hg) { if(EntityExists(c->workplane) && EntityExists(c->ptA) && EntityExists(c->ptB) && - EntityExists(c->ptC) && EntityExists(c->entityA) && - EntityExists(c->entityB)) + EntityExists(c->entityB) && + EntityExists(c->entityC) && + EntityExists(c->entityD)) { continue; } diff --git a/sketch.h b/sketch.h index b049d1c..e568182 100644 --- a/sketch.h +++ b/sketch.h @@ -450,6 +450,7 @@ public: static const int LENGTH_RATIO = 51; static const int EQ_LEN_PT_LINE_D = 52; static const int EQ_PT_LN_DISTANCES = 53; + static const int EQUAL_ANGLE = 54; static const int SYMMETRIC = 60; static const int SYMMETRIC_HORIZ = 61; static const int SYMMETRIC_VERT = 62; @@ -481,9 +482,10 @@ public: double valA; hEntity ptA; hEntity ptB; - hEntity ptC; hEntity entityA; hEntity entityB; + hEntity entityC; + hEntity entityD; bool other; bool reference; // a ref dimension, that generates no eqs @@ -511,6 +513,8 @@ public: void DrawOrGetDistance(Vector *labelPos); double EllipticalInterpolation(double rx, double ry, double theta); char *Label(void); + void DoArcForAngle(Vector a0, Vector da, Vector b0, Vector db, + Vector offset, Vector *ref); void DoLabel(Vector ref, Vector *labelPos, Vector gr, Vector gu); void DoProjectedPoint(Vector *p); void DoEqualLenTicks(Vector a, Vector b, Vector gn); @@ -527,6 +531,7 @@ public: // Some helpers when generating symbolic constraint equations void ModifyToSatisfy(void); void AddEq(IdList *l, Expr *expr, int index); + static Expr *DirectionCosine(hEntity wrkpl, ExprVector ae, ExprVector be); static Expr *Distance(hEntity workplane, hEntity pa, hEntity pb); static Expr *PointLineDistance(hEntity workplane, hEntity pt, hEntity ln); static Expr *PointPlaneDistance(ExprVector p, hEntity plane);