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

Start to add some constraint stuff. I now have point-coincident, 

and point-in-plane. These work, but the equation is still stupid,
solving everything at once and not substituting trivial equations.

[git-p4: depot-paths = "//depot/solvespace/": change = 1677]
solver
Jonathan Westhues 2008-04-21 00:16:38 -08:00
parent 7220f998fc
commit 2926fa95d9
15 changed files with 283 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
constraint.cpp
View File

@ -33,6 +33,22 @@ void Constraint::MenuConstrain(int id) {
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 {
Error("Bad selection for on point / curve / plane constraint.");
return;
}
AddConstraint(&c);
break;
case GraphicsWindow::MNU_SOLVE_NOW:
SS.Solve();
return;
@ -88,6 +104,28 @@ void Constraint::Generate(IdList<Equation,hEquation> *l) {
AddEq(l, Distance(ptA, ptB)->Minus(exprA), 0);
break;
case POINTS_COINCIDENT: {
Expr *ax, *ay, *az;
Expr *bx, *by, *bz;
SS.GetEntity(ptA)->PointGetExprs(&ax, &ay, &az);
SS.GetEntity(ptB)->PointGetExprs(&bx, &by, &bz);
AddEq(l, ax->Minus(bx), 0);
AddEq(l, ay->Minus(by), 1);
AddEq(l, az->Minus(bz), 2);
break;
}
case PT_IN_PLANE: {
Expr *px, *py, *pz;
Expr *nx, *ny, *nz, *d;
SS.GetEntity(ptA)->PointGetExprs(&px, &py, &pz);
SS.GetEntity(entityA)->PlaneGetExprs(&nx, &ny, &nz, &d);
AddEq(l,
((px->Times(nx))->Plus((py->Times(ny)->Plus(pz->Times(nz)))))
->Minus(d), 0);
break;
}
default: oops();
}
}

75
drawconstraint.cpp
View File

@ -10,7 +10,24 @@ bool Constraint::HasLabel(void) {
}
}
void Constraint::LineDrawOrGetDistance(Vector a, Vector b) {
if(dogd.drawing) {
glBegin(GL_LINE_STRIP);
glxVertex3v(a);
glxVertex3v(b);
glEnd();
} else {
Point2d ap = SS.GW.ProjectPoint(a);
Point2d bp = SS.GW.ProjectPoint(b);
double d = dogd.mp.DistanceToLine(ap, bp.Minus(ap), true);
dogd.dmin = min(dogd.dmin, d);
}
}
void Constraint::DrawOrGetDistance(void) {
if(!SS.GW.showConstraints) return;
// Unit vectors that describe our current view of the scene.
Vector gr = SS.GW.projRight;
Vector gu = SS.GW.projUp;
@ -24,21 +41,17 @@ void Constraint::DrawOrGetDistance(void) {
Vector ref = ((ap.Plus(bp)).ScaledBy(0.5)).Plus(disp.offset);
Vector ab = ap.Minus(bp);
Vector ar = ap.Minus(ref);
// Normal to a plan containing the line and the label origin.
Vector n = ab.Cross(ar);
Vector out = ab.Cross(n).WithMagnitude(1);
out = out.ScaledBy(-out.Dot(ar));
LineDrawOrGetDistance(ap, ap.Plus(out));
LineDrawOrGetDistance(bp, bp.Plus(out));
if(dogd.drawing) {
Vector ab = ap.Minus(bp);
Vector ar = ap.Minus(ref);
// Normal to a plan containing the line and the label origin.
Vector n = ab.Cross(ar);
Vector out = ab.Cross(n).WithMagnitude(1);
out = out.ScaledBy(-out.Dot(ar));
glBegin(GL_LINES);
glxVertex3v(ap);
glxVertex3v(ap.Plus(out));
glxVertex3v(bp);
glxVertex3v(bp.Plus(out));
glEnd();
glPushMatrix();
glxTranslatev(ref);
glxOntoCsys(gr, gu);
@ -46,12 +59,44 @@ void Constraint::DrawOrGetDistance(void) {
glPopMatrix();
} else {
Point2d o = SS.GW.ProjectPoint(ref);
dogd.dmin = o.DistanceTo(dogd.mp) - 10;
dogd.dmin = min(dogd.dmin, o.DistanceTo(dogd.mp) - 10);
}
break;
}
case POINTS_COINCIDENT: {
// It's impossible to select this constraint on the drawing;
// have to do it from the text window.
if(!dogd.drawing) break;
double s = 2;
Vector r = SS.GW.projRight.ScaledBy(s/SS.GW.scale);
Vector d = SS.GW.projUp.ScaledBy(s/SS.GW.scale);
for(int i = 0; i < 2; i++) {
Vector p = SS.GetEntity(i == 0 ? ptA : ptB)->PointGetCoords();
glxColor(0.4, 0, 0.4);
glBegin(GL_QUADS);
glxVertex3v(p.Plus (r).Plus (d));
glxVertex3v(p.Plus (r).Minus(d));
glxVertex3v(p.Minus(r).Minus(d));
glxVertex3v(p.Minus(r).Plus (d));
glEnd();
}
break;
}
case PT_IN_PLANE: {
double s = 6;
Vector r = SS.GW.projRight.ScaledBy(s/SS.GW.scale);
Vector d = SS.GW.projUp.ScaledBy(s/SS.GW.scale);
Vector p = SS.GetEntity(ptA)->PointGetCoords();
LineDrawOrGetDistance(p.Plus (r).Plus (d), p.Plus (r).Minus(d));
LineDrawOrGetDistance(p.Plus (r).Minus(d), p.Minus(r).Minus(d));
LineDrawOrGetDistance(p.Minus(r).Minus(d), p.Minus(r).Plus (d));
LineDrawOrGetDistance(p.Minus(r).Plus (d), p.Plus (r).Plus (d));
break;
}
default: oops();
}
}

7
dsc.h
View File

@ -146,6 +146,13 @@ public:
elem = NULL;
}
void DeepCopyInto(IdList<T,H> *l) {
l->elem = (T *)MemAlloc(elemsAllocated * sizeof(elem[0]));
memcpy(l->elem, elem, elemsAllocated * sizeof(elem[0]));
l->elemsAllocated = elemsAllocated;
l->n = n;
}
void Clear(void) {
elemsAllocated = n = 0;
if(elem) free(elem);

68
entity.cpp
View File

@ -43,6 +43,43 @@ void Entity::Csys2dGetBasisExprs(Expr **u, Expr **v) {
v[2] = (v[2])->Plus(two->Times(c->Times(d)));
}
bool Entity::HasPlane(void) {
switch(type) {
case CSYS_2D:
return true;
default:
return false;
}
}
void Entity::PlaneGetExprs(Expr **x, Expr **y, Expr **z, Expr **dn) {
Expr *a = Expr::FromParam(param.h[0]);
Expr *b = Expr::FromParam(param.h[1]);
Expr *c = Expr::FromParam(param.h[2]);
Expr *d = Expr::FromParam(param.h[3]);
Expr *two = Expr::FromConstant(2);
// Convert the quaternion to our plane's normal vector.
*x = two->Times(a->Times(c));
*x = (*x)->Plus (two->Times(b->Times(d)));
*y = two->Times(c->Times(d));
*y = (*y)->Minus(two->Times(a->Times(b)));
*z = a->Square();
*z = (*z)->Minus(b->Square());
*z = (*z)->Minus(c->Square());
*z = (*z)->Plus (d->Square());
Expr *x0, *y0, *z0;
SS.GetEntity(assoc[0])->PointGetExprs(&x0, &y0, &z0);
// The plane is n dot (p - p0) = 0, or
// n dot p - n dot p0 = 0
// so dn = n dot p0
*dn = x0->Times(*x);
*dn = (*dn)->Plus(y0->Times(*y));
*dn = (*dn)->Plus(z0->Times(*z));
}
bool Entity::IsPoint(void) {
switch(type) {
case POINT_IN_3D:
@ -53,28 +90,37 @@ bool Entity::IsPoint(void) {
}
}
bool Entity::PointIsKnown(void) {
switch(type) {
case POINT_IN_3D:
return SS.GetParam(param.h[0])->known &&
SS.GetParam(param.h[1])->known &&
SS.GetParam(param.h[2])->known;
case POINT_IN_2D:
return SS.GetParam(param.h[0])->known &&
SS.GetParam(param.h[1])->known;
default: oops();
}
}
bool Entity::PointIsFromReferences(void) {
hRequest hr = h.request();
if(hr.v == Request::HREQUEST_REFERENCE_XY.v) return true;
if(hr.v == Request::HREQUEST_REFERENCE_YZ.v) return true;
if(hr.v == Request::HREQUEST_REFERENCE_ZX.v) return true;
return false;
return h.request().IsFromReferences();
}
void Entity::PointForceTo(Vector p) {
switch(type) {
case POINT_IN_3D:
SS.GetParam(param.h[0])->ForceTo(p.x);
SS.GetParam(param.h[1])->ForceTo(p.y);
SS.GetParam(param.h[2])->ForceTo(p.z);
SS.GetParam(param.h[0])->val = p.x;
SS.GetParam(param.h[1])->val = p.y;
SS.GetParam(param.h[2])->val = p.z;
break;
case POINT_IN_2D: {
Entity *c = SS.GetEntity(csys);
Vector u, v;
c->Csys2dGetBasisVectors(&u, &v);
SS.GetParam(param.h[0])->ForceTo(p.Dot(u));
SS.GetParam(param.h[1])->ForceTo(p.Dot(v));
SS.GetParam(param.h[0])->val = p.Dot(u);
SS.GetParam(param.h[1])->val = p.Dot(v);
break;
}
default: oops();
@ -166,6 +212,8 @@ void Entity::DrawOrGetDistance(void) {
switch(type) {
case POINT_IN_3D:
case POINT_IN_2D: {
if(!SS.GW.showPoints) break;
Entity *isfor = SS.GetEntity(h.request().entity(0));
if(!SS.GW.show2dCsyss && isfor->type == Entity::CSYS_2D) break;

21
expr.cpp
View File

@ -65,6 +65,27 @@ Expr *Expr::DeepCopyKeep(void) {
return n;
}
Expr *Expr::DeepCopyWithParamsAsPointers(IdList<Param,hParam> *firstTry,
IdList<Param,hParam> *thenTry)
{
Expr *n = AllocExpr();
if(op == PARAM) {
// A param that is referenced by its hParam gets rewritten to go
// straight in to the parameter table with a pointer.
n->op = PARAM_PTR;
Param *p = firstTry->FindByIdNoOops(x.parh);
if(!p) p = thenTry->FindById(x.parh);
n->x.parp = p;
return n;
}
*n = *this;
int c = n->Children();
if(c > 0) n->a = a->DeepCopyWithParamsAsPointers(firstTry, thenTry);
if(c > 1) n->b = b->DeepCopyWithParamsAsPointers(firstTry, thenTry);
return n;
}
double Expr::Eval(void) {
switch(op) {
case PARAM: return SS.GetParam(x.parh)->val;

6
expr.h
View File

@ -83,6 +83,12 @@ public:
// number of child nodes: 0 (e.g. constant), 1 (sqrt), or 2 (+)
int Children(void);
// Make a copy, with the parameters (usually referenced by hParam)
// resolved to pointers to the actual value. This speeds things up
// considerably.
Expr *DeepCopyWithParamsAsPointers(IdList<Param,hParam> *firstTry,
IdList<Param,hParam> *thenTry);
static Expr *FromString(char *in);
static void Lex(char *in);
static Expr *Next(void);

2
glhelper.cpp
View File

@ -7,7 +7,7 @@ static bool ColorLocked;
void glxWriteText(char *str)
{
double scale = 0.7/SS.GW.scale;
double scale = 0.5/SS.GW.scale;
int xo = 5;
int yo = 5;

33
graphicswin.cpp
View File

@ -69,7 +69,7 @@ const GraphicsWindow::MenuEntry GraphicsWindow::menu[] = {
{ 1, "&Horizontal\tShift+H", 0, 'H'|S, NULL },
{ 1, "&Vertical\tShift+V", 0, 'V'|S, NULL },
{ 1, NULL, 0, NULL },
{ 1, "Coincident / &On Curve\tShift+O", 0, 'O'|S, NULL },
{ 1, "&On Point / Curve / Plane\tShift+O", MNU_ON_ENTITY, 'O'|S, mCon },
{ 1, "E&qual Length / Radius\tShift+Q", 0, 'Q'|S, NULL },
{ 1, "At &Midpoint\tShift+M", 0, 'M'|S, NULL },
{ 1, "S&ymmetric\tShift+Y", 0, 'Y'|S, NULL },
@ -238,16 +238,20 @@ void GraphicsWindow::MenuEdit(int id) {
case MNU_DELETE: {
int i;
SS.request.ClearTags();
SS.constraint.ClearTags();
for(i = 0; i < MAX_SELECTED; i++) {
Selection *s = &(SS.GW.selection[i]);
hRequest r;
r.v = 0;
hRequest r; r.v = 0;
if(s->entity.v) {
r = s->entity.request();
}
if(r.v) SS.request.Tag(r, 1);
if(r.v && !r.IsFromReferences()) SS.request.Tag(r, 1);
if(s->constraint.v) {
SS.constraint.Tag(s->constraint, 1);
}
}
SS.request.RemoveTagged();
SS.constraint.RemoveTagged();
SS.GenerateAll();
SS.GW.ClearSelection();
@ -433,24 +437,24 @@ void GraphicsWindow::ClearSelection(void) {
}
void GraphicsWindow::GroupSelection(void) {
gs.points = gs.entities = 0;
gs.csyss = gs.lineSegments = 0;
gs.n = 0;
memset(&gs, 0, sizeof(gs));
int i;
for(i = 0; i < MAX_SELECTED; i++) {
Selection *s = &(selection[i]);
if(s->entity.v) {
gs.entity[(gs.entities)++] = s->entity;
(gs.n)++;
Entity *e = SS.entity.FindById(s->entity);
if(e->IsPoint()) {
gs.point[(gs.points)++] = s->entity;
} else {
gs.entity[(gs.entities)++] = s->entity;
}
switch(e->type) {
case Entity::CSYS_2D: (gs.csyss)++; break;
case Entity::LINE_SEGMENT: (gs.lineSegments)++; break;
}
if(e->IsPoint()) {
gs.point[(gs.points)++] = s->entity;
}
if(e->HasPlane()) (gs.planes)++;
}
}
}
@ -620,12 +624,15 @@ void GraphicsWindow::Paint(int w, int h) {
for(i = 0; i < SS.entity.n; i++) {
SS.entity.elem[i].Draw();
}
// Want the constraints to get drawn in front, so disable depth test.
glDisable(GL_DEPTH_TEST);
// Draw the constraints
for(i = 0; i < SS.constraint.n; i++) {
SS.constraint.elem[i].Draw();
}
// Then redraw whatever the mouse is hovering over, highlighted. Have
// to disable the depth test, so that we can overdraw.
// Then redraw whatever the mouse is hovering over, highlighted.
glDisable(GL_DEPTH_TEST);
glxLockColorTo(1, 1, 0);
hover.Draw();

6
sketch.cpp
View File

@ -99,9 +99,3 @@ char *Request::DescriptionString(void) {
return ret;
}
void Param::ForceTo(double v) {
val = v;
known = true;
}

18
sketch.h
View File

@ -27,6 +27,8 @@ public:
inline hEntity entity(int i);
inline hParam param(int i);
inline bool IsFromReferences(void);
};
class hEntity {
public:
@ -147,6 +149,12 @@ public:
Vector PointGetCoords(void);
void PointForceTo(Vector v);
bool PointIsFromReferences(void);
bool PointIsKnown(void);
// Applies for anything that comes with a plane
bool HasPlane(void);
// The plane is points P such that P dot (xn, yn, zn) - d = 0
void PlaneGetExprs(Expr **xn, Expr **yn, Expr **zn, Expr **d);
// Routines to draw and hit-test the representation of the entity
// on-screen.
@ -170,11 +178,15 @@ public:
double val;
bool known;
void ForceTo(double v);
};
inline bool hRequest::IsFromReferences(void) {
if(v == Request::HREQUEST_REFERENCE_XY.v) return true;
if(v == Request::HREQUEST_REFERENCE_YZ.v) return true;
if(v == Request::HREQUEST_REFERENCE_ZX.v) return true;
return false;
}
inline hEntity hRequest::entity(int i)
{ hEntity r; r.v = (v << 16) | i; return r; }
inline hParam hRequest::param(int i)
@ -200,6 +212,7 @@ public:
static const int POINTS_COINCIDENT = 20;
static const int PT_PT_DISTANCE = 30;
static const int PT_LINE_DISTANCE = 31;
static const int PT_IN_PLANE = 40;
static const int HORIZONTAL = 40;
static const int VERTICAL = 41;
@ -232,6 +245,7 @@ public:
Point2d mp;
double dmin;
} dogd; // state for drawing or getting distance (for hit testing)
void LineDrawOrGetDistance(Vector a, Vector b);
double GetDistance(Point2d mp);
void Draw(void);
void DrawOrGetDistance(void);

32
solvespace.cpp
View File

@ -88,10 +88,10 @@ void SolveSpace::ForceReferences(void) {
Vector v = Vector::MakeFrom(0, 0, 0);
GetEntity(hr.entity(1))->PointForceTo(v);
// The quaternion that defines the rotation, from the table.
GetParam(hr.param(0))->ForceTo(Quat[i].a);
GetParam(hr.param(1))->ForceTo(Quat[i].b);
GetParam(hr.param(2))->ForceTo(Quat[i].c);
GetParam(hr.param(3))->ForceTo(Quat[i].d);
GetParam(hr.param(0))->val = Quat[i].a;
GetParam(hr.param(1))->val = Quat[i].b;
GetParam(hr.param(2))->val = Quat[i].c;
GetParam(hr.param(3))->val = Quat[i].d;
}
}
@ -132,10 +132,12 @@ bool SolveSpace::SolveGroup(hGroup hg) {
c->Generate(&(sys.eq));
}
return sys.Solve();
bool r = sys.Solve();
FreeAllExprs();
return r;
}
bool SolveSpace::SolveWorker(void) {
bool SolveSpace::SolveWorker(int order) {
bool allSolved = true;
int i;
@ -145,17 +147,25 @@ bool SolveSpace::SolveWorker(void) {
allSolved = false;
dbp("try solve group %s", g->DescriptionString());
// Save the parameter table; a failed solve attempt will mess that
// up a little bit.
IdList<Param,hParam> savedParam;
param.DeepCopyInto(&savedParam);
if(SolveGroup(g->h)) {
g->solved = true;
g->solveOrder = order;
// So this one worked; let's see if we can go any further.
if(SolveWorker()) {
if(SolveWorker(order+1)) {
// So everything worked; we're done.
return true;
} else {
// Didn't work, so undo this choice and give up
g->solved = false;
}
}
// Didn't work, so undo this choice and give up
g->solved = false;
param.Clear();
savedParam.MoveSelfInto(&param);
}
// If we got here, then either everything failed, so we're stuck, or
@ -168,7 +178,7 @@ void SolveSpace::Solve(void) {
for(i = 0; i < group.n; i++) {
group.elem[i].solved = false;
}
SolveWorker();
SolveWorker(0);
InvalidateGraphics();
}

2
solvespace.h
View File

@ -151,7 +151,7 @@ public:
void Init(void);
bool SolveGroup(hGroup hg);
bool SolveWorker(void);
bool SolveWorker(int order);
void Solve(void);
static void MenuFile(int id);

45
system.cpp
View File

@ -17,10 +17,12 @@ void System::WriteJacobian(int eqTag, int paramTag) {
Equation *e = &(eq.elem[a]);
if(e->tag != eqTag) continue;
mat.eq[i] = eq.elem[i].h;
mat.B.sym[i] = eq.elem[i].e;
mat.eq[i] = e->h;
mat.B.sym[i] = e->e->DeepCopyWithParamsAsPointers(&param, &(SS.param));
for(j = 0; j < mat.n; j++) {
mat.A.sym[i][j] = e->e->PartialWrt(mat.param[j]);
Expr *pd = e->e->PartialWrt(mat.param[j]);
mat.A.sym[i][j] =
pd->DeepCopyWithParamsAsPointers(&param, &(SS.param));
}
i++;
}
@ -165,14 +167,13 @@ bool System::NewtonSolve(int tag) {
int iter = 0;
bool converged = false;
int i;
// Evaluate the functions at our operating point.
for(i = 0; i < mat.m; i++) {
mat.B.num[i] = (mat.B.sym[i])->Eval();
}
do {
// Evaluate the functions numerically
for(i = 0; i < mat.m; i++) {
mat.B.num[i] = (mat.B.sym[i])->Eval();
dbp("mat.B.num[%d] = %.3f", i, mat.B.num[i]);
dbp("mat.B.sym[%d] = %s", i, (mat.B.sym[i])->Print());
}
// And likewise for the Jacobian
// And evaluate the Jacobian at our initial operating point.
EvalJacobian();
if(!SolveLinearSystem()) break;
@ -184,12 +185,13 @@ bool System::NewtonSolve(int tag) {
dbp("modifying param %08x, now %.3f", mat.param[i],
param.FindById(mat.param[i])->val);
(param.FindById(mat.param[i]))->val -= mat.X[i];
// XXX do this properly
SS.GetParam(mat.param[i])->val =
(param.FindById(mat.param[i]))->val;
}
// XXX re-evaluate functions before checking convergence
// Re-evalute the functions, since the params have just changed.
for(i = 0; i < mat.m; i++) {
mat.B.num[i] = (mat.B.sym[i])->Eval();
}
// Check for convergence
converged = true;
for(i = 0; i < mat.m; i++) {
if(!Tol(mat.B.num[i])) {
@ -199,7 +201,7 @@ bool System::NewtonSolve(int tag) {
}
} while(iter++ < 50 && !converged);
if(converged) {
if(converged) {
return true;
} else {
return false;
@ -238,7 +240,18 @@ bool System::Solve(void) {
param.FindById(mat.param[j])->tag = ASSUMED;
}
NewtonSolve(0);
bool ok = NewtonSolve(0);
if(ok) {
// System solved correctly, so write the new values back in to the
// main parameter table.
for(i = 0; i < param.n; i++) {
Param *p = &(param.elem[i]);
Param *pp = SS.GetParam(p->h);
pp->val = p->val;
pp->known = true;
}
}
return true;
}

2
ui.h
View File

@ -96,6 +96,7 @@ public:
MNU_LINE_SEGMENT,
// Constrain
MNU_DISTANCE_DIA,
MNU_ON_ENTITY,
MNU_SOLVE_NOW,
} MenuId;
typedef void MenuHandler(int id);
@ -167,6 +168,7 @@ public:
int points;
int entities;
int csyss;
int planes;
int lineSegments;
int n;
} gs;

4
win32/w32main.cpp
View File

@ -36,10 +36,11 @@ HFONT FixedFont, LinkFont;
void dbp(char *str, ...)
{
va_list f;
char buf[1024];
static char buf[1024*50];
va_start(f, str);
vsprintf(buf, str, f);
OutputDebugString(buf);
va_end(f);
}
void Error(char *str, ...)
@ -48,6 +49,7 @@ void Error(char *str, ...)
char buf[1024];
va_start(f, str);
vsprintf(buf, str, f);
va_end(f);
HWND h = GetForegroundWindow();
MessageBox(h, buf, "SolveSpace Error", MB_OK | MB_ICONERROR);