853c6cb59c
vectors", represented by unit quaternions. This permits me to add circles, where the normal defines the plane of the circle. Still many things painful. The interface for editing normals is not so intuitive, and it's not yet clear how I would e.g. export a circle entity and recreate it properly, since that entity has a param not associated with a normal or point. And the transformed points/normals do not yet support rotations. That will be necessary soon. [git-p4: depot-paths = "//depot/solvespace/": change = 1705]
482 lines
17 KiB
C++
482 lines
17 KiB
C++
#include "solvespace.h"
|
|
|
|
char *Constraint::DescriptionString(void) {
|
|
static char ret[1024];
|
|
sprintf(ret, "c%03x", h.v);
|
|
return ret;
|
|
}
|
|
|
|
hConstraint Constraint::AddConstraint(Constraint *c) {
|
|
SS.constraint.AddAndAssignId(c);
|
|
|
|
SS.GenerateAll(SS.GW.solving == GraphicsWindow::SOLVE_ALWAYS);
|
|
return c->h;
|
|
}
|
|
|
|
void Constraint::ConstrainCoincident(hEntity ptA, hEntity ptB) {
|
|
Constraint c;
|
|
memset(&c, 0, sizeof(c));
|
|
c.group = SS.GW.activeGroup;
|
|
c.workplane = SS.GW.activeWorkplane;
|
|
c.type = Constraint::POINTS_COINCIDENT;
|
|
c.ptA = ptA;
|
|
c.ptB = ptB;
|
|
AddConstraint(&c);
|
|
}
|
|
|
|
void Constraint::MenuConstrain(int id) {
|
|
Constraint c;
|
|
memset(&c, 0, sizeof(c));
|
|
c.group = SS.GW.activeGroup;
|
|
c.workplane = SS.GW.activeWorkplane;
|
|
|
|
SS.GW.GroupSelection();
|
|
#define gs (SS.GW.gs)
|
|
|
|
switch(id) {
|
|
case GraphicsWindow::MNU_DISTANCE_DIA: {
|
|
if(gs.points == 2 && gs.n == 2) {
|
|
c.type = PT_PT_DISTANCE;
|
|
c.ptA = gs.point[0];
|
|
c.ptB = gs.point[1];
|
|
} else if(gs.lineSegments == 1 && gs.n == 1) {
|
|
c.type = PT_PT_DISTANCE;
|
|
Entity *e = SS.GetEntity(gs.entity[0]);
|
|
c.ptA = e->point[0];
|
|
c.ptB = e->point[1];
|
|
} else {
|
|
Error("Bad selection for distance / diameter constraint.");
|
|
return;
|
|
}
|
|
Vector n = SS.GW.projRight.Cross(SS.GW.projUp);
|
|
Vector a = SS.GetEntity(c.ptA)->PointGetNum();
|
|
Vector b = SS.GetEntity(c.ptB)->PointGetNum();
|
|
|
|
c.disp.offset = n.Cross(a.Minus(b)).WithMagnitude(50);
|
|
c.exprA = Expr::FromString("0")->DeepCopyKeep();
|
|
c.ModifyToSatisfy();
|
|
AddConstraint(&c);
|
|
break;
|
|
}
|
|
|
|
case GraphicsWindow::MNU_ON_ENTITY:
|
|
if(gs.points == 2 && gs.n == 2) {
|
|
c.type = POINTS_COINCIDENT;
|
|
c.ptA = gs.point[0];
|
|
c.ptB = gs.point[1];
|
|
} else if(gs.points == 1 && gs.planes == 1 && gs.n == 2) {
|
|
c.type = PT_IN_PLANE;
|
|
c.ptA = gs.point[0];
|
|
c.entityA = gs.entity[0];
|
|
} else if(gs.points == 1 && gs.lineSegments == 1 && gs.n == 2) {
|
|
c.type = PT_ON_LINE;
|
|
c.ptA = gs.point[0];
|
|
c.entityA = gs.entity[0];
|
|
} else {
|
|
Error("Bad selection for on point / curve / plane constraint.");
|
|
return;
|
|
}
|
|
AddConstraint(&c);
|
|
break;
|
|
|
|
case GraphicsWindow::MNU_EQUAL:
|
|
if(gs.lineSegments == 2 && gs.n == 2) {
|
|
c.type = EQUAL_LENGTH_LINES;
|
|
c.entityA = gs.entity[0];
|
|
c.entityB = gs.entity[1];
|
|
} else {
|
|
Error("Bad selection for equal length / radius constraint.");
|
|
return;
|
|
}
|
|
AddConstraint(&c);
|
|
break;
|
|
|
|
case GraphicsWindow::MNU_AT_MIDPOINT:
|
|
if(gs.lineSegments == 1 && gs.points == 1 && gs.n == 2) {
|
|
c.type = AT_MIDPOINT;
|
|
c.entityA = gs.entity[0];
|
|
c.ptA = gs.point[0];
|
|
} else if(gs.lineSegments == 1 && gs.planes == 1 && gs.n == 2) {
|
|
c.type = AT_MIDPOINT;
|
|
int i = SS.GetEntity(gs.entity[0])->HasPlane() ? 1 : 0;
|
|
c.entityA = gs.entity[i];
|
|
c.entityB = gs.entity[1-i];
|
|
} else {
|
|
Error("Bad selection for at midpoint constraint.");
|
|
return;
|
|
}
|
|
AddConstraint(&c);
|
|
break;
|
|
|
|
case GraphicsWindow::MNU_SYMMETRIC:
|
|
if(gs.points == 2 && gs.planes == 1 && gs.n == 3) {
|
|
c.type = SYMMETRIC;
|
|
c.entityA = gs.entity[0];
|
|
c.ptA = gs.point[0];
|
|
c.ptB = gs.point[1];
|
|
} else if(gs.lineSegments == 1 && gs.planes == 1 && gs.n == 2) {
|
|
c.type = SYMMETRIC;
|
|
int i = SS.GetEntity(gs.entity[0])->HasPlane() ? 1 : 0;
|
|
Entity *line = SS.GetEntity(gs.entity[i]);
|
|
c.entityA = gs.entity[1-i];
|
|
c.ptA = line->point[0];
|
|
c.ptB = line->point[1];
|
|
} else {
|
|
Error("Bad selection for symmetric constraint.");
|
|
return;
|
|
}
|
|
AddConstraint(&c);
|
|
break;
|
|
|
|
case GraphicsWindow::MNU_VERTICAL:
|
|
case GraphicsWindow::MNU_HORIZONTAL: {
|
|
hEntity ha, hb;
|
|
if(c.workplane.v == Entity::FREE_IN_3D.v) {
|
|
Error("Select workplane before constraining horiz/vert.");
|
|
return;
|
|
}
|
|
if(gs.lineSegments == 1 && gs.n == 1) {
|
|
c.entityA = gs.entity[0];
|
|
Entity *e = SS.GetEntity(c.entityA);
|
|
ha = e->point[0];
|
|
hb = e->point[1];
|
|
} else if(gs.points == 2 && gs.n == 2) {
|
|
ha = c.ptA = gs.point[0];
|
|
hb = c.ptB = gs.point[1];
|
|
} else {
|
|
Error("Bad selection for horizontal / vertical constraint.");
|
|
return;
|
|
}
|
|
if(id == GraphicsWindow::MNU_HORIZONTAL) {
|
|
c.type = HORIZONTAL;
|
|
} else {
|
|
c.type = VERTICAL;
|
|
}
|
|
AddConstraint(&c);
|
|
break;
|
|
}
|
|
|
|
case GraphicsWindow::MNU_SOLVE_NOW:
|
|
SS.GenerateAll(true);
|
|
return;
|
|
|
|
case GraphicsWindow::MNU_SOLVE_AUTO:
|
|
if(SS.GW.solving == GraphicsWindow::SOLVE_ALWAYS) {
|
|
SS.GW.solving = GraphicsWindow::DONT_SOLVE;
|
|
} else {
|
|
SS.GW.solving = GraphicsWindow::SOLVE_ALWAYS;
|
|
}
|
|
SS.GW.EnsureValidActives();
|
|
break;
|
|
|
|
default: oops();
|
|
}
|
|
|
|
SS.GW.ClearSelection();
|
|
InvalidateGraphics();
|
|
}
|
|
|
|
Expr *Constraint::VectorsParallel(int eq, ExprVector a, ExprVector b) {
|
|
ExprVector r = a.Cross(b);
|
|
// Hairy ball theorem screws me here. There's no clean solution that I
|
|
// know, so let's pivot on the initial numerical guess.
|
|
double mx = fabs((a.x)->Eval()) + fabs((b.x)->Eval());
|
|
double my = fabs((a.y)->Eval()) + fabs((b.y)->Eval());
|
|
double mz = fabs((a.z)->Eval()) + fabs((b.z)->Eval());
|
|
// The basis vector in which the vectors have the LEAST energy is the
|
|
// one that we should look at most (e.g. if both vectors lie in the xy
|
|
// plane, then the z component of the cross product is most important).
|
|
// So find the strongest component of a and b, and that's the component
|
|
// of the cross product to ignore.
|
|
double m = max(mx, max(my, mz));
|
|
Expr *e0, *e1;
|
|
if(m == mx) { e0 = r.y; e1 = r.z; }
|
|
else if(m == my) { e0 = r.z; e1 = r.x; }
|
|
else if(m == mz) { e0 = r.x; e1 = r.y; }
|
|
else oops();
|
|
|
|
if(eq == 0) return e0;
|
|
if(eq == 1) return e1;
|
|
oops();
|
|
}
|
|
|
|
Expr *Constraint::PointLineDistance(hEntity wrkpl, hEntity hpt, hEntity hln) {
|
|
Entity *ln = SS.GetEntity(hln);
|
|
Entity *a = SS.GetEntity(ln->point[0]);
|
|
Entity *b = SS.GetEntity(ln->point[1]);
|
|
|
|
Entity *p = SS.GetEntity(hpt);
|
|
|
|
if(wrkpl.v == Entity::FREE_IN_3D.v) {
|
|
ExprVector ep = p->PointGetExprs();
|
|
|
|
ExprVector ea = a->PointGetExprs();
|
|
ExprVector eb = b->PointGetExprs();
|
|
ExprVector eab = ea.Minus(eb);
|
|
Expr *m = eab.Magnitude();
|
|
|
|
return ((eab.Cross(ea.Minus(ep))).Magnitude())->Div(m);
|
|
} else {
|
|
Expr *ua, *va, *ub, *vb;
|
|
a->PointGetExprsInWorkplane(wrkpl, &ua, &va);
|
|
b->PointGetExprsInWorkplane(wrkpl, &ub, &vb);
|
|
|
|
Expr *du = ua->Minus(ub);
|
|
Expr *dv = va->Minus(vb);
|
|
|
|
Expr *u, *v;
|
|
p->PointGetExprsInWorkplane(wrkpl, &u, &v);
|
|
|
|
Expr *m = ((du->Square())->Plus(dv->Square()))->Sqrt();
|
|
|
|
Expr *proj = (dv->Times(ua->Minus(u)))->Minus(
|
|
(du->Times(va->Minus(v))));
|
|
|
|
return proj->Div(m);
|
|
}
|
|
}
|
|
|
|
Expr *Constraint::PointPlaneDistance(ExprVector p, hEntity hpl) {
|
|
ExprVector n;
|
|
Expr *d;
|
|
SS.GetEntity(hpl)->PlaneGetExprs(&n, &d);
|
|
return (p.Dot(n))->Minus(d);
|
|
}
|
|
|
|
Expr *Constraint::Distance(hEntity wrkpl, hEntity hpa, hEntity hpb) {
|
|
Entity *pa = SS.GetEntity(hpa);
|
|
Entity *pb = SS.GetEntity(hpb);
|
|
if(!(pa->IsPoint() && pb->IsPoint())) oops();
|
|
|
|
if(wrkpl.v == Entity::FREE_IN_3D.v) {
|
|
// This is true distance
|
|
ExprVector ea, eb, eab;
|
|
ea = pa->PointGetExprs();
|
|
eb = pb->PointGetExprs();
|
|
eab = ea.Minus(eb);
|
|
|
|
return eab.Magnitude();
|
|
} else {
|
|
// This is projected distance, in the given workplane.
|
|
Expr *au, *av, *bu, *bv;
|
|
|
|
pa->PointGetExprsInWorkplane(wrkpl, &au, &av);
|
|
pb->PointGetExprsInWorkplane(wrkpl, &bu, &bv);
|
|
|
|
Expr *du = au->Minus(bu);
|
|
Expr *dv = av->Minus(bv);
|
|
|
|
return ((du->Square())->Plus(dv->Square()))->Sqrt();
|
|
}
|
|
}
|
|
|
|
ExprVector Constraint::PointInThreeSpace(hEntity workplane, Expr *u, Expr *v) {
|
|
ExprVector ub, vb, ob;
|
|
Entity *w = SS.GetEntity(workplane);
|
|
w->WorkplaneGetBasisExprs(&ub, &vb);
|
|
ob = w->WorkplaneGetOffsetExprs();
|
|
|
|
return (ub.ScaledBy(u)).Plus(vb.ScaledBy(v)).Plus(ob);
|
|
}
|
|
|
|
void Constraint::ModifyToSatisfy(void) {
|
|
IdList<Equation,hEquation> l;
|
|
// An uninit IdList could lead us to free some random address, bad.
|
|
memset(&l, 0, sizeof(l));
|
|
|
|
Generate(&l);
|
|
if(l.n != 1) oops();
|
|
|
|
// These equations are written in the form f(...) - d = 0, where
|
|
// d is the value of the exprA.
|
|
double v = (l.elem[0].e)->Eval();
|
|
double nd = exprA->Eval() + v;
|
|
Expr::FreeKeep(&exprA);
|
|
exprA = Expr::FromConstant(nd)->DeepCopyKeep();
|
|
|
|
l.Clear();
|
|
}
|
|
|
|
void Constraint::AddEq(IdList<Equation,hEquation> *l, Expr *expr, int index) {
|
|
Equation eq;
|
|
eq.e = expr;
|
|
eq.h = h.equation(index);
|
|
l->Add(&eq);
|
|
}
|
|
|
|
void Constraint::Generate(IdList<Equation,hEquation> *l) {
|
|
switch(type) {
|
|
case PT_PT_DISTANCE:
|
|
AddEq(l, Distance(workplane, ptA, ptB)->Minus(exprA), 0);
|
|
break;
|
|
|
|
case EQUAL_LENGTH_LINES: {
|
|
Entity *a = SS.GetEntity(entityA);
|
|
Entity *b = SS.GetEntity(entityB);
|
|
AddEq(l, Distance(workplane, a->point[0], a->point[1])->Minus(
|
|
Distance(workplane, b->point[0], b->point[1])), 0);
|
|
break;
|
|
}
|
|
|
|
case POINTS_COINCIDENT: {
|
|
Entity *a = SS.GetEntity(ptA);
|
|
Entity *b = SS.GetEntity(ptB);
|
|
if(workplane.v == Entity::FREE_IN_3D.v) {
|
|
ExprVector pa = a->PointGetExprs();
|
|
ExprVector pb = b->PointGetExprs();
|
|
AddEq(l, pa.x->Minus(pb.x), 0);
|
|
AddEq(l, pa.y->Minus(pb.y), 1);
|
|
AddEq(l, pa.z->Minus(pb.z), 2);
|
|
} else {
|
|
Expr *au, *av;
|
|
Expr *bu, *bv;
|
|
a->PointGetExprsInWorkplane(workplane, &au, &av);
|
|
b->PointGetExprsInWorkplane(workplane, &bu, &bv);
|
|
AddEq(l, au->Minus(bu), 0);
|
|
AddEq(l, av->Minus(bv), 1);
|
|
}
|
|
break;
|
|
}
|
|
|
|
case PT_IN_PLANE:
|
|
// This one works the same, whether projected or not.
|
|
AddEq(l, PointPlaneDistance(
|
|
SS.GetEntity(ptA)->PointGetExprs(), entityA), 0);
|
|
break;
|
|
|
|
case PT_ON_LINE:
|
|
if(workplane.v == Entity::FREE_IN_3D.v) {
|
|
Entity *ln = SS.GetEntity(entityA);
|
|
Entity *a = SS.GetEntity(ln->point[0]);
|
|
Entity *b = SS.GetEntity(ln->point[1]);
|
|
Entity *p = SS.GetEntity(ptA);
|
|
|
|
ExprVector ep = p->PointGetExprs();
|
|
ExprVector ea = a->PointGetExprs();
|
|
ExprVector eb = b->PointGetExprs();
|
|
ExprVector eab = ea.Minus(eb);
|
|
ExprVector eap = ea.Minus(ep);
|
|
|
|
AddEq(l, VectorsParallel(0, eab, eap), 0);
|
|
AddEq(l, VectorsParallel(1, eab, eap), 1);
|
|
} else {
|
|
AddEq(l, PointLineDistance(workplane, ptA, entityA), 0);
|
|
}
|
|
break;
|
|
|
|
case AT_MIDPOINT:
|
|
if(workplane.v == Entity::FREE_IN_3D.v) {
|
|
Entity *ln = SS.GetEntity(entityA);
|
|
ExprVector a = SS.GetEntity(ln->point[0])->PointGetExprs();
|
|
ExprVector b = SS.GetEntity(ln->point[1])->PointGetExprs();
|
|
ExprVector m = (a.Plus(b)).ScaledBy(Expr::FromConstant(0.5));
|
|
|
|
if(ptA.v) {
|
|
ExprVector p = SS.GetEntity(ptA)->PointGetExprs();
|
|
AddEq(l, (m.x)->Minus(p.x), 0);
|
|
AddEq(l, (m.y)->Minus(p.y), 1);
|
|
AddEq(l, (m.z)->Minus(p.z), 2);
|
|
} else {
|
|
AddEq(l, PointPlaneDistance(m, entityB), 0);
|
|
}
|
|
} else {
|
|
Entity *ln = SS.GetEntity(entityA);
|
|
Entity *a = SS.GetEntity(ln->point[0]);
|
|
Entity *b = SS.GetEntity(ln->point[1]);
|
|
|
|
Expr *au, *av, *bu, *bv;
|
|
a->PointGetExprsInWorkplane(workplane, &au, &av);
|
|
b->PointGetExprsInWorkplane(workplane, &bu, &bv);
|
|
Expr *mu = Expr::FromConstant(0.5)->Times(au->Plus(bu));
|
|
Expr *mv = Expr::FromConstant(0.5)->Times(av->Plus(bv));
|
|
|
|
if(ptA.v) {
|
|
Entity *p = SS.GetEntity(ptA);
|
|
Expr *pu, *pv;
|
|
p->PointGetExprsInWorkplane(workplane, &pu, &pv);
|
|
AddEq(l, pu->Minus(mu), 0);
|
|
AddEq(l, pv->Minus(mv), 1);
|
|
} else {
|
|
ExprVector m = PointInThreeSpace(workplane, mu, mv);
|
|
AddEq(l, PointPlaneDistance(m, entityB), 0);
|
|
}
|
|
}
|
|
break;
|
|
|
|
case SYMMETRIC:
|
|
if(workplane.v == Entity::FREE_IN_3D.v) {
|
|
Entity *plane = SS.GetEntity(entityA);
|
|
Entity *ea = SS.GetEntity(ptA);
|
|
Entity *eb = SS.GetEntity(ptB);
|
|
ExprVector a = ea->PointGetExprs();
|
|
ExprVector b = eb->PointGetExprs();
|
|
|
|
// The midpoint of the line connecting the symmetric points
|
|
// lies on the plane of the symmetry.
|
|
ExprVector m = (a.Plus(b)).ScaledBy(Expr::FromConstant(0.5));
|
|
AddEq(l, PointPlaneDistance(m, plane->h), 0);
|
|
|
|
// And projected into the plane of symmetry, the points are
|
|
// coincident.
|
|
Expr *au, *av, *bu, *bv;
|
|
ea->PointGetExprsInWorkplane(plane->h, &au, &av);
|
|
eb->PointGetExprsInWorkplane(plane->h, &bu, &bv);
|
|
AddEq(l, au->Minus(bu), 0);
|
|
AddEq(l, av->Minus(bv), 1);
|
|
} else {
|
|
Entity *plane = SS.GetEntity(entityA);
|
|
Entity *a = SS.GetEntity(ptA);
|
|
Entity *b = SS.GetEntity(ptB);
|
|
|
|
Expr *au, *av, *bu, *bv;
|
|
a->PointGetExprsInWorkplane(workplane, &au, &av);
|
|
b->PointGetExprsInWorkplane(workplane, &bu, &bv);
|
|
Expr *mu = Expr::FromConstant(0.5)->Times(au->Plus(bu));
|
|
Expr *mv = Expr::FromConstant(0.5)->Times(av->Plus(bv));
|
|
|
|
ExprVector m = PointInThreeSpace(workplane, mu, mv);
|
|
AddEq(l, PointPlaneDistance(m, plane->h), 0);
|
|
|
|
// Construct a vector within the workplane that is normal
|
|
// to the symmetry pane's normal (i.e., that lies in the
|
|
// plane of symmetry). The line connecting the points is
|
|
// perpendicular to that constructed vector.
|
|
ExprVector u, v;
|
|
Entity *w = SS.GetEntity(workplane);
|
|
w->WorkplaneGetBasisExprs(&u, &v);
|
|
|
|
ExprVector pa = a->PointGetExprs();
|
|
ExprVector pb = b->PointGetExprs();
|
|
ExprVector n;
|
|
Expr *d;
|
|
plane->PlaneGetExprs(&n, &d);
|
|
AddEq(l, (n.Cross(u.Cross(v))).Dot(pa.Minus(pb)), 1);
|
|
}
|
|
break;
|
|
|
|
case HORIZONTAL:
|
|
case VERTICAL: {
|
|
hEntity ha, hb;
|
|
if(entityA.v) {
|
|
Entity *e = SS.GetEntity(entityA);
|
|
ha = e->point[0];
|
|
hb = e->point[1];
|
|
} else {
|
|
ha = ptA;
|
|
hb = ptB;
|
|
}
|
|
Entity *a = SS.GetEntity(ha);
|
|
Entity *b = SS.GetEntity(hb);
|
|
|
|
Expr *au, *av, *bu, *bv;
|
|
a->PointGetExprsInWorkplane(workplane, &au, &av);
|
|
b->PointGetExprsInWorkplane(workplane, &bu, &bv);
|
|
|
|
AddEq(l, (type == HORIZONTAL) ? av->Minus(bv) : au->Minus(bu), 0);
|
|
break;
|
|
}
|
|
|
|
default: oops();
|
|
}
|
|
}
|