Vector Vector::ProjectVectorInto(hEntity wrkpl) {
EntityBase *w = SK.GetEntity(wrkpl);
Vector u = w->Normal()->NormalU();
Vector v = w->Normal()->NormalV();
double up = this->Dot(u);
double vp = this->Dot(v);
return (u.ScaledBy(up)).Plus(v.ScaledBy(vp));
}
ExprVector ConstraintBase::PointInThreeSpace(hEntity workplane,
Expr *u, Expr *v)
{
EntityBase *w = SK.GetEntity(workplane);
ExprVector ub = w->Normal()->NormalExprsU();
ExprVector vb = w->Normal()->NormalExprsV();
ExprVector ob = w->WorkplaneGetOffsetExprs();
return (ub.ScaledBy(u)).Plus(vb.ScaledBy(v)).Plus(ob);
}
ExprVector EntityBase::PointGetExprs(void) {
ExprVector r;
switch(type) {
case POINT_IN_3D:
r = ExprVector::From(param[0], param[1], param[2]);
break;
case POINT_IN_2D: {
EntityBase *c = SK.GetEntity(workplane);
ExprVector u = c->Normal()->NormalExprsU();
ExprVector v = c->Normal()->NormalExprsV();
r = c->WorkplaneGetOffsetExprs();
r = r.Plus(u.ScaledBy(Expr::From(param[0])));
r = r.Plus(v.ScaledBy(Expr::From(param[1])));
break;
}
case POINT_N_TRANS: {
ExprVector orig = ExprVector::From(numPoint);
ExprVector trans = ExprVector::From(param[0], param[1], param[2]);
r = orig.Plus(trans.ScaledBy(Expr::From(timesApplied)));
break;
}
case POINT_N_ROT_TRANS: {
ExprVector orig = ExprVector::From(numPoint);
ExprVector trans = ExprVector::From(param[0], param[1], param[2]);
ExprQuaternion q =
ExprQuaternion::From(param[3], param[4], param[5], param[6]);
orig = q.Rotate(orig);
r = orig.Plus(trans);
break;
}
case POINT_N_ROT_AA: {
ExprVector orig = ExprVector::From(numPoint);
ExprVector trans = ExprVector::From(param[0], param[1], param[2]);
ExprQuaternion q = GetAxisAngleQuaternionExprs(3);
orig = orig.Minus(trans);
orig = q.Rotate(orig);
r = orig.Plus(trans);
break;
}
case POINT_N_COPY:
r = ExprVector::From(numPoint);
break;
default: oops();
}
return r;
}
Vector EntityBase::PointGetNum(void) {
Vector p;
switch(type) {
case POINT_IN_3D:
p = Vector::From(param[0], param[1], param[2]);
break;
case POINT_IN_2D: {
EntityBase *c = SK.GetEntity(workplane);
Vector u = c->Normal()->NormalU();
Vector v = c->Normal()->NormalV();
p = u.ScaledBy(SK.GetParam(param[0])->val);
p = p.Plus(v.ScaledBy(SK.GetParam(param[1])->val));
p = p.Plus(c->WorkplaneGetOffset());
break;
}
case POINT_N_TRANS: {
Vector trans = Vector::From(param[0], param[1], param[2]);
p = numPoint.Plus(trans.ScaledBy(timesApplied));
break;
}
case POINT_N_ROT_TRANS: {
Vector offset = Vector::From(param[0], param[1], param[2]);
Quaternion q = PointGetQuaternion();
p = q.Rotate(numPoint);
p = p.Plus(offset);
break;
}
case POINT_N_ROT_AA: {
Vector offset = Vector::From(param[0], param[1], param[2]);
Quaternion q = PointGetQuaternion();
p = numPoint.Minus(offset);
p = q.Rotate(p);
p = p.Plus(offset);
break;
}
case POINT_N_COPY:
p = numPoint;
break;
default: oops();
}
return p;
}
Vector GraphicsWindow::SnapToGrid(Vector p) {
if(!LockedInWorkplane()) return p;
EntityBase *wrkpl = SK.GetEntity(ActiveWorkplane()),
*norm = wrkpl->Normal();
Vector wo = SK.GetEntity(wrkpl->point[0])->PointGetNum(),
wu = norm->NormalU(),
wv = norm->NormalV(),
wn = norm->NormalN();
Vector pp = (p.Minus(wo)).DotInToCsys(wu, wv, wn);
pp.x = floor((pp.x / SS.gridSpacing) + 0.5)*SS.gridSpacing;
pp.y = floor((pp.y / SS.gridSpacing) + 0.5)*SS.gridSpacing;
pp.z = 0;
return pp.ScaleOutOfCsys(wu, wv, wn).Plus(wo);
}
//-----------------------------------------------------------------------------
// 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 *ConstraintBase::DirectionCosine(hEntity wrkpl,
ExprVector ae, ExprVector be)
{
if(wrkpl.v == EntityBase::FREE_IN_3D.v) {
Expr *mags = (ae.Magnitude())->Times(be.Magnitude());
return (ae.Dot(be))->Div(mags);
} else {
EntityBase *w = SK.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));
}
}
void EntityBase::PointGetExprsInWorkplane(hEntity wrkpl, Expr **u, Expr **v) {
if(type == POINT_IN_2D && workplane.v == wrkpl.v) {
// They want our coordinates in the form that we've written them,
// very nice.
*u = Expr::From(param[0]);
*v = Expr::From(param[1]);
} else {
// Get the offset and basis vectors for this weird exotic csys.
EntityBase *w = SK.GetEntity(wrkpl);
ExprVector wp = w->WorkplaneGetOffsetExprs();
ExprVector wu = w->Normal()->NormalExprsU();
ExprVector wv = w->Normal()->NormalExprsV();
// Get our coordinates in three-space, and project them into that
// coordinate system.
ExprVector ev = PointGetExprs();
ev = ev.Minus(wp);
*u = ev.Dot(wu);
*v = ev.Dot(wv);
}
}
void EntityBase::PointForceTo(Vector p) {
switch(type) {
case POINT_IN_3D:
SK.GetParam(param[0])->val = p.x;
SK.GetParam(param[1])->val = p.y;
SK.GetParam(param[2])->val = p.z;
break;
case POINT_IN_2D: {
EntityBase *c = SK.GetEntity(workplane);
p = p.Minus(c->WorkplaneGetOffset());
SK.GetParam(param[0])->val = p.Dot(c->Normal()->NormalU());
SK.GetParam(param[1])->val = p.Dot(c->Normal()->NormalV());
break;
}
case POINT_N_TRANS: {
if(timesApplied == 0) break;
Vector trans = (p.Minus(numPoint)).ScaledBy(1.0/timesApplied);
SK.GetParam(param[0])->val = trans.x;
SK.GetParam(param[1])->val = trans.y;
SK.GetParam(param[2])->val = trans.z;
break;
}
case POINT_N_ROT_TRANS: {
// Force only the translation; leave the rotation unchanged. But
// remember that we're working with respect to the rotated
// point.
Vector trans = p.Minus(PointGetQuaternion().Rotate(numPoint));
SK.GetParam(param[0])->val = trans.x;
SK.GetParam(param[1])->val = trans.y;
SK.GetParam(param[2])->val = trans.z;
break;
}
case POINT_N_ROT_AA: {
// Force only the angle; the axis and center of rotation stay
Vector offset = Vector::From(param[0], param[1], param[2]);
Vector normal = Vector::From(param[4], param[5], param[6]);
Vector u = normal.Normal(0), v = normal.Normal(1);
Vector po = p.Minus(offset), numo = numPoint.Minus(offset);
double thetap = atan2(v.Dot(po), u.Dot(po));
double thetan = atan2(v.Dot(numo), u.Dot(numo));
double thetaf = (thetap - thetan);
double thetai = (SK.GetParam(param[3])->val)*timesApplied*2;
double dtheta = thetaf - thetai;
// Take the smallest possible change in the actual step angle,
// in order to avoid jumps when you cross from +pi to -pi
while(dtheta < -PI) dtheta += 2*PI;
while(dtheta > PI) dtheta -= 2*PI;
SK.GetParam(param[3])->val = (thetai + dtheta)/(timesApplied*2);
break;
}
case POINT_N_COPY:
// Nothing to do; it's a static copy
break;
default: oops();
}
}
void ConstraintBase::GenerateReal(IdList<Equation,hEquation> *l) const {
Expr *exA = Expr::From(valA);
switch(type) {
case Type::PT_PT_DISTANCE:
AddEq(l, Distance(workplane, ptA, ptB)->Minus(exA), 0);
return;
case Type::PROJ_PT_DISTANCE: {
ExprVector pA = SK.GetEntity(ptA)->PointGetExprs(),
pB = SK.GetEntity(ptB)->PointGetExprs(),
dp = pB.Minus(pA);
ExprVector pp = SK.GetEntity(entityA)->VectorGetExprs();
pp = pp.WithMagnitude(Expr::From(1.0));
AddEq(l, (dp.Dot(pp))->Minus(exA), 0);
return;
}
case Type::PT_LINE_DISTANCE:
AddEq(l,
PointLineDistance(workplane, ptA, entityA)->Minus(exA), 0);
return;
case Type::PT_PLANE_DISTANCE: {
ExprVector pt = SK.GetEntity(ptA)->PointGetExprs();
AddEq(l, (PointPlaneDistance(pt, entityA))->Minus(exA), 0);
return;
}
case Type::PT_FACE_DISTANCE: {
ExprVector pt = SK.GetEntity(ptA)->PointGetExprs();
EntityBase *f = SK.GetEntity(entityA);
ExprVector p0 = f->FaceGetPointExprs();
ExprVector n = f->FaceGetNormalExprs();
AddEq(l, (pt.Minus(p0)).Dot(n)->Minus(exA), 0);
return;
}
case Type::EQUAL_LENGTH_LINES: {
EntityBase *a = SK.GetEntity(entityA);
EntityBase *b = SK.GetEntity(entityB);
AddEq(l, Distance(workplane, a->point[0], a->point[1])->Minus(
Distance(workplane, b->point[0], b->point[1])), 0);
return;
}
// These work on distance squared, since the pt-line distances are
// signed, and we want the absolute value.
case Type::EQ_LEN_PT_LINE_D: {
EntityBase *forLen = SK.GetEntity(entityA);
Expr *d1 = Distance(workplane, forLen->point[0], forLen->point[1]);
Expr *d2 = PointLineDistance(workplane, ptA, entityB);
AddEq(l, (d1->Square())->Minus(d2->Square()), 0);
return;
}
case Type::EQ_PT_LN_DISTANCES: {
Expr *d1 = PointLineDistance(workplane, ptA, entityA);
Expr *d2 = PointLineDistance(workplane, ptB, entityB);
AddEq(l, (d1->Square())->Minus(d2->Square()), 0);
return;
}
case Type::LENGTH_RATIO: {
EntityBase *a = SK.GetEntity(entityA);
EntityBase *b = SK.GetEntity(entityB);
Expr *la = Distance(workplane, a->point[0], a->point[1]);
Expr *lb = Distance(workplane, b->point[0], b->point[1]);
AddEq(l, (la->Div(lb))->Minus(exA), 0);
return;
}
case Type::LENGTH_DIFFERENCE: {
EntityBase *a = SK.GetEntity(entityA);
EntityBase *b = SK.GetEntity(entityB);
Expr *la = Distance(workplane, a->point[0], a->point[1]);
Expr *lb = Distance(workplane, b->point[0], b->point[1]);
AddEq(l, (la->Minus(lb))->Minus(exA), 0);
return;
}
case Type::DIAMETER: {
EntityBase *circle = SK.GetEntity(entityA);
Expr *r = circle->CircleGetRadiusExpr();
AddEq(l, (r->Times(Expr::From(2)))->Minus(exA), 0);
return;
}
case Type::EQUAL_RADIUS: {
EntityBase *c1 = SK.GetEntity(entityA);
EntityBase *c2 = SK.GetEntity(entityB);
AddEq(l, (c1->CircleGetRadiusExpr())->Minus(
c2->CircleGetRadiusExpr()), 0);
return;
}
case Type::EQUAL_LINE_ARC_LEN: {
EntityBase *line = SK.GetEntity(entityA),
*arc = SK.GetEntity(entityB);
// Get the line length
ExprVector l0 = SK.GetEntity(line->point[0])->PointGetExprs(),
l1 = SK.GetEntity(line->point[1])->PointGetExprs();
Expr *ll = (l1.Minus(l0)).Magnitude();
// And get the arc radius, and the cosine of its angle
EntityBase *ao = SK.GetEntity(arc->point[0]),
*as = SK.GetEntity(arc->point[1]),
*af = SK.GetEntity(arc->point[2]);
ExprVector aos = (as->PointGetExprs()).Minus(ao->PointGetExprs()),
aof = (af->PointGetExprs()).Minus(ao->PointGetExprs());
Expr *r = aof.Magnitude();
ExprVector n = arc->Normal()->NormalExprsN();
ExprVector u = aos.WithMagnitude(Expr::From(1.0));
ExprVector v = n.Cross(u);
// so in our new csys, we start at (1, 0, 0)
Expr *costheta = aof.Dot(u)->Div(r);
Expr *sintheta = aof.Dot(v)->Div(r);
double thetas, thetaf, dtheta;
arc->ArcGetAngles(&thetas, &thetaf, &dtheta);
Expr *theta;
if(dtheta < 3*PI/4) {
theta = costheta->ACos();
} else if(dtheta < 5*PI/4) {
// As the angle crosses pi, cos theta is not invertible;
// so use the sine to stop blowing up
theta = Expr::From(PI)->Minus(sintheta->ASin());
} else {
theta = (Expr::From(2*PI))->Minus(costheta->ACos());
}
// And write the equation; r*theta = L
AddEq(l, (r->Times(theta))->Minus(ll), 0);
return;
}
case Type::POINTS_COINCIDENT: {
EntityBase *a = SK.GetEntity(ptA);
EntityBase *b = SK.GetEntity(ptB);
if(workplane.v == EntityBase::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);
}
return;
}
case Type::PT_IN_PLANE:
// This one works the same, whether projected or not.
AddEq(l, PointPlaneDistance(
SK.GetEntity(ptA)->PointGetExprs(), entityA), 0);
return;
case Type::PT_ON_FACE: {
// a plane, n dot (p - p0) = 0
ExprVector p = SK.GetEntity(ptA)->PointGetExprs();
EntityBase *f = SK.GetEntity(entityA);
ExprVector p0 = f->FaceGetPointExprs();
ExprVector n = f->FaceGetNormalExprs();
AddEq(l, (p.Minus(p0)).Dot(n), 0);
return;
}
case Type::PT_ON_LINE:
if(workplane.v == EntityBase::FREE_IN_3D.v) {
EntityBase *ln = SK.GetEntity(entityA);
EntityBase *a = SK.GetEntity(ln->point[0]);
EntityBase *b = SK.GetEntity(ln->point[1]);
EntityBase *p = SK.GetEntity(ptA);
ExprVector ep = p->PointGetExprs();
ExprVector ea = a->PointGetExprs();
ExprVector eb = b->PointGetExprs();
ExprVector eab = ea.Minus(eb);
// Construct a vector from the point to either endpoint of
// the line segment, and choose the longer of these.
ExprVector eap = ea.Minus(ep);
ExprVector ebp = eb.Minus(ep);
ExprVector elp =
(ebp.Magnitude()->Eval() > eap.Magnitude()->Eval()) ?
ebp : eap;
if(p->group.v == group.v) {
AddEq(l, VectorsParallel(0, eab, elp), 0);
AddEq(l, VectorsParallel(1, eab, elp), 1);
} else {
AddEq(l, VectorsParallel(0, elp, eab), 0);
AddEq(l, VectorsParallel(1, elp, eab), 1);
}
} else {
AddEq(l, PointLineDistance(workplane, ptA, entityA), 0);
}
return;
case Type::PT_ON_CIRCLE: {
// This actually constrains the point to lie on the cylinder.
EntityBase *circle = SK.GetEntity(entityA);
ExprVector center = SK.GetEntity(circle->point[0])->PointGetExprs();
ExprVector pt = SK.GetEntity(ptA)->PointGetExprs();
EntityBase *normal = SK.GetEntity(circle->normal);
ExprVector u = normal->NormalExprsU(),
v = normal->NormalExprsV();
Expr *du = (center.Minus(pt)).Dot(u),
*dv = (center.Minus(pt)).Dot(v);
Expr *r = circle->CircleGetRadiusExpr();
AddEq(l,
((du->Square())->Plus(dv->Square()))->Minus(r->Square()), 0);
return;
}
case Type::AT_MIDPOINT:
if(workplane.v == EntityBase::FREE_IN_3D.v) {
EntityBase *ln = SK.GetEntity(entityA);
ExprVector a = SK.GetEntity(ln->point[0])->PointGetExprs();
ExprVector b = SK.GetEntity(ln->point[1])->PointGetExprs();
ExprVector m = (a.Plus(b)).ScaledBy(Expr::From(0.5));
if(ptA.v) {
ExprVector p = SK.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 {
EntityBase *ln = SK.GetEntity(entityA);
EntityBase *a = SK.GetEntity(ln->point[0]);
EntityBase *b = SK.GetEntity(ln->point[1]);
Expr *au, *av, *bu, *bv;
a->PointGetExprsInWorkplane(workplane, &au, &av);
b->PointGetExprsInWorkplane(workplane, &bu, &bv);
Expr *mu = Expr::From(0.5)->Times(au->Plus(bu));
Expr *mv = Expr::From(0.5)->Times(av->Plus(bv));
if(ptA.v) {
EntityBase *p = SK.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);
}
}
return;
case Type::SYMMETRIC:
if(workplane.v == EntityBase::FREE_IN_3D.v) {
EntityBase *plane = SK.GetEntity(entityA);
EntityBase *ea = SK.GetEntity(ptA);
EntityBase *eb = SK.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::From(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), 1);
AddEq(l, av->Minus(bv), 2);
} else {
EntityBase *plane = SK.GetEntity(entityA);
EntityBase *a = SK.GetEntity(ptA);
EntityBase *b = SK.GetEntity(ptB);
Expr *au, *av, *bu, *bv;
a->PointGetExprsInWorkplane(workplane, &au, &av);
b->PointGetExprsInWorkplane(workplane, &bu, &bv);
Expr *mu = Expr::From(0.5)->Times(au->Plus(bu));
Expr *mv = Expr::From(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.
EntityBase *w = SK.GetEntity(workplane);
ExprVector u = w->Normal()->NormalExprsU();
ExprVector v = w->Normal()->NormalExprsV();
ExprVector pa = a->PointGetExprs();
ExprVector pb = b->PointGetExprs();
ExprVector n;
Expr *d;
plane->WorkplaneGetPlaneExprs(&n, &d);
AddEq(l, (n.Cross(u.Cross(v))).Dot(pa.Minus(pb)), 1);
}
return;
case Type::SYMMETRIC_HORIZ:
case Type::SYMMETRIC_VERT: {
EntityBase *a = SK.GetEntity(ptA);
EntityBase *b = SK.GetEntity(ptB);
Expr *au, *av, *bu, *bv;
a->PointGetExprsInWorkplane(workplane, &au, &av);
b->PointGetExprsInWorkplane(workplane, &bu, &bv);
if(type == Type::SYMMETRIC_HORIZ) {
AddEq(l, av->Minus(bv), 0);
AddEq(l, au->Plus(bu), 1);
} else {
AddEq(l, au->Minus(bu), 0);
AddEq(l, av->Plus(bv), 1);
}
return;
}
case Type::SYMMETRIC_LINE: {
EntityBase *pa = SK.GetEntity(ptA);
EntityBase *pb = SK.GetEntity(ptB);
Expr *pau, *pav, *pbu, *pbv;
pa->PointGetExprsInWorkplane(workplane, &pau, &pav);
pb->PointGetExprsInWorkplane(workplane, &pbu, &pbv);
EntityBase *ln = SK.GetEntity(entityA);
EntityBase *la = SK.GetEntity(ln->point[0]);
EntityBase *lb = SK.GetEntity(ln->point[1]);
Expr *lau, *lav, *lbu, *lbv;
la->PointGetExprsInWorkplane(workplane, &lau, &lav);
lb->PointGetExprsInWorkplane(workplane, &lbu, &lbv);
Expr *dpu = pbu->Minus(pau), *dpv = pbv->Minus(pav);
Expr *dlu = lbu->Minus(lau), *dlv = lbv->Minus(lav);
// The line through the points is perpendicular to the line
// of symmetry.
AddEq(l, (dlu->Times(dpu))->Plus(dlv->Times(dpv)), 0);
// And the signed distances of the points to the line are
// equal in magnitude and opposite in sign, so sum to zero
Expr *dista = (dlv->Times(lau->Minus(pau)))->Minus(
(dlu->Times(lav->Minus(pav))));
Expr *distb = (dlv->Times(lau->Minus(pbu)))->Minus(
(dlu->Times(lav->Minus(pbv))));
AddEq(l, dista->Plus(distb), 1);
return;
}
case Type::HORIZONTAL:
case Type::VERTICAL: {
hEntity ha, hb;
if(entityA.v) {
EntityBase *e = SK.GetEntity(entityA);
ha = e->point[0];
hb = e->point[1];
} else {
ha = ptA;
hb = ptB;
}
EntityBase *a = SK.GetEntity(ha);
EntityBase *b = SK.GetEntity(hb);
Expr *au, *av, *bu, *bv;
a->PointGetExprsInWorkplane(workplane, &au, &av);
b->PointGetExprsInWorkplane(workplane, &bu, &bv);
AddEq(l, (type == Type::HORIZONTAL) ? av->Minus(bv) : au->Minus(bu), 0);
return;
}
case Type::SAME_ORIENTATION: {
EntityBase *a = SK.GetEntity(entityA);
EntityBase *b = SK.GetEntity(entityB);
if(b->group.v != group.v) {
swap(a, b);
}
ExprVector au = a->NormalExprsU(),
an = a->NormalExprsN();
ExprVector bu = b->NormalExprsU(),
bv = b->NormalExprsV(),
bn = b->NormalExprsN();
AddEq(l, VectorsParallel(0, an, bn), 0);
AddEq(l, VectorsParallel(1, an, bn), 1);
Expr *d1 = au.Dot(bv);
Expr *d2 = au.Dot(bu);
// Allow either orientation for the coordinate system, depending
// on how it was drawn.
if(fabs(d1->Eval()) < fabs(d2->Eval())) {
AddEq(l, d1, 2);
} else {
AddEq(l, d2, 2);
}
return;
}
case Type::PERPENDICULAR:
case Type::ANGLE: {
EntityBase *a = SK.GetEntity(entityA);
EntityBase *b = SK.GetEntity(entityB);
ExprVector ae = a->VectorGetExprs();
ExprVector be = b->VectorGetExprs();
if(other) ae = ae.ScaledBy(Expr::From(-1));
Expr *c = DirectionCosine(workplane, ae, be);
if(type == Type::ANGLE) {
// The direction cosine is equal to the cosine of the
// specified angle
Expr *rads = exA->Times(Expr::From(PI/180)),
*rc = rads->Cos();
double arc = fabs(rc->Eval());
// avoid false detection of inconsistent systems by gaining
// up as the difference in dot products gets small at small
// angles; doubles still have plenty of precision, only
// problem is that rank test
Expr *mult = Expr::From(arc > 0.99 ? 0.01/(1.00001 - arc) : 1);
AddEq(l, (c->Minus(rc))->Times(mult), 0);
} else {
// The dot product (and therefore the direction cosine)
// is equal to zero, perpendicular.
AddEq(l, c, 0);
}
return;
}
case Type::EQUAL_ANGLE: {
EntityBase *a = SK.GetEntity(entityA);
EntityBase *b = SK.GetEntity(entityB);
EntityBase *c = SK.GetEntity(entityC);
EntityBase *d = SK.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);
return;
}
case Type::ARC_LINE_TANGENT: {
EntityBase *arc = SK.GetEntity(entityA);
EntityBase *line = SK.GetEntity(entityB);
ExprVector ac = SK.GetEntity(arc->point[0])->PointGetExprs();
ExprVector ap =
SK.GetEntity(arc->point[other ? 2 : 1])->PointGetExprs();
ExprVector ld = line->VectorGetExprs();
// The line is perpendicular to the radius
AddEq(l, ld.Dot(ac.Minus(ap)), 0);
return;
}
case Type::CUBIC_LINE_TANGENT: {
EntityBase *cubic = SK.GetEntity(entityA);
EntityBase *line = SK.GetEntity(entityB);
ExprVector a;
if(other) {
a = cubic->CubicGetFinishTangentExprs();
} else {
a = cubic->CubicGetStartTangentExprs();
}
ExprVector b = line->VectorGetExprs();
if(workplane.v == EntityBase::FREE_IN_3D.v) {
AddEq(l, VectorsParallel(0, a, b), 0);
AddEq(l, VectorsParallel(1, a, b), 1);
} else {
EntityBase *w = SK.GetEntity(workplane);
ExprVector wn = w->Normal()->NormalExprsN();
AddEq(l, (a.Cross(b)).Dot(wn), 0);
}
return;
}
case Type::CURVE_CURVE_TANGENT: {
bool parallel = true;
int i;
ExprVector dir[2];
for(i = 0; i < 2; i++) {
EntityBase *e = SK.GetEntity((i == 0) ? entityA : entityB);
bool oth = (i == 0) ? other : other2;
if(e->type == Entity::Type::ARC_OF_CIRCLE) {
ExprVector center, endpoint;
center = SK.GetEntity(e->point[0])->PointGetExprs();
endpoint =
SK.GetEntity(e->point[oth ? 2 : 1])->PointGetExprs();
dir[i] = endpoint.Minus(center);
// We're using the vector from the center of the arc to
// an endpoint; so that's normal to the tangent, not
// parallel.
parallel = !parallel;
} else if(e->type == Entity::Type::CUBIC) {
if(oth) {
dir[i] = e->CubicGetFinishTangentExprs();
} else {
dir[i] = e->CubicGetStartTangentExprs();
}
} else {
ssassert(false, "Unexpected entity types for CURVE_CURVE_TANGENT");
}
}
if(parallel) {
EntityBase *w = SK.GetEntity(workplane);
ExprVector wn = w->Normal()->NormalExprsN();
AddEq(l, ((dir[0]).Cross(dir[1])).Dot(wn), 0);
} else {
AddEq(l, (dir[0]).Dot(dir[1]), 0);
}
return;
}
case Type::PARALLEL: {
EntityBase *ea = SK.GetEntity(entityA), *eb = SK.GetEntity(entityB);
if(eb->group.v != group.v) {
swap(ea, eb);
}
ExprVector a = ea->VectorGetExprs();
ExprVector b = eb->VectorGetExprs();
if(workplane.v == EntityBase::FREE_IN_3D.v) {
AddEq(l, VectorsParallel(0, a, b), 0);
AddEq(l, VectorsParallel(1, a, b), 1);
} else {
EntityBase *w = SK.GetEntity(workplane);
ExprVector wn = w->Normal()->NormalExprsN();
AddEq(l, (a.Cross(b)).Dot(wn), 0);
}
return;
}
case Type::WHERE_DRAGGED: {
EntityBase *ep = SK.GetEntity(ptA);
if(workplane.v == EntityBase::FREE_IN_3D.v) {
ExprVector ev = ep->PointGetExprs();
Vector v = ep->PointGetNum();
AddEq(l, ev.x->Minus(Expr::From(v.x)), 0);
AddEq(l, ev.y->Minus(Expr::From(v.y)), 1);
AddEq(l, ev.z->Minus(Expr::From(v.z)), 2);
} else {
Expr *u, *v;
ep->PointGetExprsInWorkplane(workplane, &u, &v);
AddEq(l, u->Minus(Expr::From(u->Eval())), 0);
AddEq(l, v->Minus(Expr::From(v->Eval())), 1);
}
return;
}
case Type::COMMENT:
return;
}
ssassert(false, "Unexpected constraint ID");
}
void GraphicsWindow::Paint(void) {
int i;
havePainted = true;
int w, h;
GetGraphicsWindowSize(&w, &h);
width = w; height = h;
glViewport(0, 0, w, h);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
glScaled(scale*2.0/w, scale*2.0/h, scale*1.0/30000);
double mat[16];
// Last thing before display is to apply the perspective
double clp = SS.CameraTangent()*scale;
MakeMatrix(mat, 1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, clp, 1);
glMultMatrixd(mat);
// Before that, we apply the rotation
Vector n = projUp.Cross(projRight);
MakeMatrix(mat, projRight.x, projRight.y, projRight.z, 0,
projUp.x, projUp.y, projUp.z, 0,
n.x, n.y, n.z, 0,
0, 0, 0, 1);
glMultMatrixd(mat);
// And before that, the translation
MakeMatrix(mat, 1, 0, 0, offset.x,
0, 1, 0, offset.y,
0, 0, 1, offset.z,
0, 0, 0, 1);
glMultMatrixd(mat);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glShadeModel(GL_SMOOTH);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glEnable(GL_BLEND);
glEnable(GL_LINE_SMOOTH);
// don't enable GL_POLYGON_SMOOTH; that looks ugly on some graphics cards,
// drawn with leaks in the mesh
glEnable(GL_POLYGON_OFFSET_LINE);
glEnable(GL_POLYGON_OFFSET_FILL);
glEnable(GL_DEPTH_TEST);
glHint(GL_LINE_SMOOTH_HINT, GL_NICEST);
glEnable(GL_NORMALIZE);
// At the same depth, we want later lines drawn over earlier.
glDepthFunc(GL_LEQUAL);
if(SS.AllGroupsOkay()) {
glClearColor(SS.backgroundColor.redF(),
SS.backgroundColor.greenF(),
SS.backgroundColor.blueF(), 1.0f);
} else {
// Draw a different background whenever we're having solve problems.
RgbColor rgb = Style::Color(Style::DRAW_ERROR);
glClearColor(0.4f*rgb.redF(), 0.4f*rgb.greenF(), 0.4f*rgb.blueF(), 1.0f);
// And show the text window, which has info to debug it
ForceTextWindowShown();
}
glClear(GL_COLOR_BUFFER_BIT);
glClearDepth(1.0);
glClear(GL_DEPTH_BUFFER_BIT);
if(SS.bgImage.fromFile) {
// If a background image is loaded, then we draw it now as a texture.
// This handles the resizing for us nicely.
glBindTexture(GL_TEXTURE_2D, TEXTURE_BACKGROUND_IMG);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP);
glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_DECAL);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB,
SS.bgImage.rw, SS.bgImage.rh,
0,
GL_RGB, GL_UNSIGNED_BYTE,
SS.bgImage.fromFile);
double tw = ((double)SS.bgImage.w) / SS.bgImage.rw,
th = ((double)SS.bgImage.h) / SS.bgImage.rh;
double mmw = SS.bgImage.w / SS.bgImage.scale,
mmh = SS.bgImage.h / SS.bgImage.scale;
Vector origin = SS.bgImage.origin;
origin = origin.DotInToCsys(projRight, projUp, n);
// Place the depth of our origin at the point that corresponds to
// w = 1, so that it's unaffected by perspective.
origin.z = (offset.ScaledBy(-1)).Dot(n);
origin = origin.ScaleOutOfCsys(projRight, projUp, n);
// Place the background at the very back of the Z order, though, by
// mucking with the depth range.
glDepthRange(1, 1);
glEnable(GL_TEXTURE_2D);
glBegin(GL_QUADS);
glTexCoord2d(0, 0);
ssglVertex3v(origin);
glTexCoord2d(0, th);
ssglVertex3v(origin.Plus(projUp.ScaledBy(mmh)));
glTexCoord2d(tw, th);
ssglVertex3v(origin.Plus(projRight.ScaledBy(mmw).Plus(
projUp. ScaledBy(mmh))));
glTexCoord2d(tw, 0);
ssglVertex3v(origin.Plus(projRight.ScaledBy(mmw)));
glEnd();
glDisable(GL_TEXTURE_2D);
}
ssglDepthRangeOffset(0);
// Nasty case when we're reloading the imported files; could be that
// we get an error, so a dialog pops up, and a message loop starts, and
// we have to get called to paint ourselves. If the sketch is screwed
// up, then we could trigger an oops trying to draw.
if(!SS.allConsistent) return;
// Let's use two lights, at the user-specified locations
GLfloat f;
glEnable(GL_LIGHT0);
f = (GLfloat)SS.lightIntensity[0];
GLfloat li0[] = { f, f, f, 1.0f };
glLightfv(GL_LIGHT0, GL_DIFFUSE, li0);
glLightfv(GL_LIGHT0, GL_SPECULAR, li0);
glEnable(GL_LIGHT1);
f = (GLfloat)SS.lightIntensity[1];
GLfloat li1[] = { f, f, f, 1.0f };
glLightfv(GL_LIGHT1, GL_DIFFUSE, li1);
glLightfv(GL_LIGHT1, GL_SPECULAR, li1);
Vector ld;
ld = VectorFromProjs(SS.lightDir[0]);
GLfloat ld0[4] = { (GLfloat)ld.x, (GLfloat)ld.y, (GLfloat)ld.z, 0 };
glLightfv(GL_LIGHT0, GL_POSITION, ld0);
ld = VectorFromProjs(SS.lightDir[1]);
GLfloat ld1[4] = { (GLfloat)ld.x, (GLfloat)ld.y, (GLfloat)ld.z, 0 };
glLightfv(GL_LIGHT1, GL_POSITION, ld1);
if(SS.drawBackFaces) {
// For debugging, draw the backs of the triangles in red, so that we
// notice when a shell is open
glLightModelf(GL_LIGHT_MODEL_TWO_SIDE, 1);
} else {
glLightModelf(GL_LIGHT_MODEL_TWO_SIDE, 0);
}
GLfloat ambient[4] = { (float)SS.ambientIntensity,
(float)SS.ambientIntensity,
(float)SS.ambientIntensity, 1 };
glLightModelfv(GL_LIGHT_MODEL_AMBIENT, ambient);
ssglUnlockColor();
if(showSnapGrid && LockedInWorkplane()) {
hEntity he = ActiveWorkplane();
EntityBase *wrkpl = SK.GetEntity(he),
*norm = wrkpl->Normal();
Vector wu, wv, wn, wp;
wp = SK.GetEntity(wrkpl->point[0])->PointGetNum();
wu = norm->NormalU();
wv = norm->NormalV();
wn = norm->NormalN();
double g = SS.gridSpacing;
double umin = VERY_POSITIVE, umax = VERY_NEGATIVE,
vmin = VERY_POSITIVE, vmax = VERY_NEGATIVE;
int a;
for(a = 0; a < 4; a++) {
// Ideally, we would just do +/- half the width and height; but
// allow some extra slop for rounding.
Vector horiz = projRight.ScaledBy((0.6*width)/scale + 2*g),
vert = projUp. ScaledBy((0.6*height)/scale + 2*g);
if(a == 2 || a == 3) horiz = horiz.ScaledBy(-1);
if(a == 1 || a == 3) vert = vert. ScaledBy(-1);
Vector tp = horiz.Plus(vert).Minus(offset);
// Project the point into our grid plane, normal to the screen
// (not to the grid plane). If the plane is on edge then this is
// impossible so don't try to draw the grid.
bool parallel;
Vector tpp = Vector::AtIntersectionOfPlaneAndLine(
wn, wn.Dot(wp),
tp, tp.Plus(n),
¶llel);
if(parallel) goto nogrid;
tpp = tpp.Minus(wp);
double uu = tpp.Dot(wu),
vv = tpp.Dot(wv);
umin = min(uu, umin);
umax = max(uu, umax);
vmin = min(vv, vmin);
vmax = max(vv, vmax);
}
int i, j, i0, i1, j0, j1;
i0 = (int)(umin / g);
i1 = (int)(umax / g);
j0 = (int)(vmin / g);
j1 = (int)(vmax / g);
if(i0 > i1 || i1 - i0 > 400) goto nogrid;
if(j0 > j1 || j1 - j0 > 400) goto nogrid;
glLineWidth(1);
ssglColorRGBa(Style::Color(Style::DATUM), 0.3);
glBegin(GL_LINES);
for(i = i0 + 1; i < i1; i++) {
ssglVertex3v(wp.Plus(wu.ScaledBy(i*g)).Plus(wv.ScaledBy(j0*g)));
ssglVertex3v(wp.Plus(wu.ScaledBy(i*g)).Plus(wv.ScaledBy(j1*g)));
}
for(j = j0 + 1; j < j1; j++) {
ssglVertex3v(wp.Plus(wu.ScaledBy(i0*g)).Plus(wv.ScaledBy(j*g)));
ssglVertex3v(wp.Plus(wu.ScaledBy(i1*g)).Plus(wv.ScaledBy(j*g)));
}
glEnd();
// Clear the depth buffer, so that the grid is at the very back of
// the Z order.
glClear(GL_DEPTH_BUFFER_BIT);
nogrid:;
}
// Draw the active group; this does stuff like the mesh and edges.
(SK.GetGroup(activeGroup))->Draw();
// Now draw the entities
if(showHdnLines) glDisable(GL_DEPTH_TEST);
Entity::DrawAll();
// Draw filled paths in all groups, when those filled paths were requested
// specially by assigning a style with a fill color, or when the filled
// paths are just being filled by default. This should go last, to make
// the transparency work.
Group *g;
for(g = SK.group.First(); g; g = SK.group.NextAfter(g)) {
if(!(g->IsVisible())) continue;
g->DrawFilledPaths();
}
glDisable(GL_DEPTH_TEST);
// Draw the constraints
for(i = 0; i < SK.constraint.n; i++) {
SK.constraint.elem[i].Draw();
}
// Draw the traced path, if one exists
glLineWidth(Style::Width(Style::ANALYZE));
ssglColorRGB(Style::Color(Style::ANALYZE));
SContour *sc = &(SS.traced.path);
glBegin(GL_LINE_STRIP);
for(i = 0; i < sc->l.n; i++) {
ssglVertex3v(sc->l.elem[i].p);
}
glEnd();
// And the naked edges, if the user did Analyze -> Show Naked Edges.
glLineWidth(Style::Width(Style::DRAW_ERROR));
ssglColorRGB(Style::Color(Style::DRAW_ERROR));
ssglDrawEdges(&(SS.nakedEdges), true);
// Then redraw whatever the mouse is hovering over, highlighted.
glDisable(GL_DEPTH_TEST);
ssglLockColorTo(Style::Color(Style::HOVERED));
hover.Draw();
// And finally draw the selection, same mechanism.
ssglLockColorTo(Style::Color(Style::SELECTED));
for(Selection *s = selection.First(); s; s = selection.NextAfter(s)) {
s->Draw();
}
ssglUnlockColor();
// If a marquee selection is in progress, then draw the selection
// rectangle, as an outline and a transparent fill.
if(pending.operation == DRAGGING_MARQUEE) {
Point2d begin = ProjectPoint(orig.marqueePoint);
double xmin = min(orig.mouse.x, begin.x),
xmax = max(orig.mouse.x, begin.x),
ymin = min(orig.mouse.y, begin.y),
ymax = max(orig.mouse.y, begin.y);
Vector tl = UnProjectPoint(Point2d::From(xmin, ymin)),
tr = UnProjectPoint(Point2d::From(xmax, ymin)),
br = UnProjectPoint(Point2d::From(xmax, ymax)),
bl = UnProjectPoint(Point2d::From(xmin, ymax));
glLineWidth((GLfloat)1.3);
ssglColorRGB(Style::Color(Style::HOVERED));
glBegin(GL_LINE_LOOP);
ssglVertex3v(tl);
ssglVertex3v(tr);
ssglVertex3v(br);
ssglVertex3v(bl);
glEnd();
ssglColorRGBa(Style::Color(Style::HOVERED), 0.10);
glBegin(GL_QUADS);
ssglVertex3v(tl);
ssglVertex3v(tr);
ssglVertex3v(br);
ssglVertex3v(bl);
glEnd();
}
// An extra line, used to indicate the origin when rotating within the
// plane of the monitor.
if(SS.extraLine.draw) {
glLineWidth(1);
ssglLockColorTo(Style::Color(Style::DATUM));
glBegin(GL_LINES);
ssglVertex3v(SS.extraLine.ptA);
ssglVertex3v(SS.extraLine.ptB);
glEnd();
}
// A note to indicate the origin in the just-exported file.
if(SS.justExportedInfo.draw) {
ssglColorRGB(Style::Color(Style::DATUM));
Vector p = SS.justExportedInfo.pt,
u = SS.justExportedInfo.u,
v = SS.justExportedInfo.v;
glLineWidth(1.5);
glBegin(GL_LINES);
ssglVertex3v(p.Plus(u.WithMagnitude(-15/scale)));
ssglVertex3v(p.Plus(u.WithMagnitude(30/scale)));
ssglVertex3v(p.Plus(v.WithMagnitude(-15/scale)));
ssglVertex3v(p.Plus(v.WithMagnitude(30/scale)));
glEnd();
ssglWriteText("(x, y) = (0, 0) for file just exported",
DEFAULT_TEXT_HEIGHT,
p.Plus(u.ScaledBy(10/scale)).Plus(v.ScaledBy(10/scale)),
u, v, NULL, NULL);
ssglWriteText("press Esc to clear this message",
DEFAULT_TEXT_HEIGHT,
p.Plus(u.ScaledBy(40/scale)).Plus(
v.ScaledBy(-(DEFAULT_TEXT_HEIGHT)/scale)),
u, v, NULL, NULL);
}
// And finally the toolbar.
if(SS.showToolbar) {
ToolbarDraw();
}
}