1 //-----------------------------------------------------------------------------
2 // The implementation of our entities in the symbolic algebra system, methods
3 // to return a symbolic representation of the entity (line by its endpoints,
4 // circle by center and radius, etc.).
5 //
6 // Copyright 2008-2013 Jonathan Westhues.
7 //-----------------------------------------------------------------------------
8 #include "solvespace.h"
9 
10 const hEntity  EntityBase::FREE_IN_3D = { 0 };
11 const hEntity  EntityBase::NO_ENTITY = { 0 };
12 
HasVector(void)13 bool EntityBase::HasVector(void) {
14     switch(type) {
15         case LINE_SEGMENT:
16         case NORMAL_IN_3D:
17         case NORMAL_IN_2D:
18         case NORMAL_N_COPY:
19         case NORMAL_N_ROT:
20         case NORMAL_N_ROT_AA:
21             return true;
22 
23         default:
24             return false;
25     }
26 }
27 
VectorGetExprs(void)28 ExprVector EntityBase::VectorGetExprs(void) {
29     switch(type) {
30         case LINE_SEGMENT:
31             return (SK.GetEntity(point[0])->PointGetExprs()).Minus(
32                     SK.GetEntity(point[1])->PointGetExprs());
33 
34         case NORMAL_IN_3D:
35         case NORMAL_IN_2D:
36         case NORMAL_N_COPY:
37         case NORMAL_N_ROT:
38         case NORMAL_N_ROT_AA:
39             return NormalExprsN();
40 
41         default: oops();
42     }
43 }
44 
VectorGetNum(void)45 Vector EntityBase::VectorGetNum(void) {
46     switch(type) {
47         case LINE_SEGMENT:
48             return (SK.GetEntity(point[0])->PointGetNum()).Minus(
49                     SK.GetEntity(point[1])->PointGetNum());
50 
51         case NORMAL_IN_3D:
52         case NORMAL_IN_2D:
53         case NORMAL_N_COPY:
54         case NORMAL_N_ROT:
55         case NORMAL_N_ROT_AA:
56             return NormalN();
57 
58         default: oops();
59     }
60 }
61 
VectorGetRefPoint(void)62 Vector EntityBase::VectorGetRefPoint(void) {
63     switch(type) {
64         case LINE_SEGMENT:
65             return ((SK.GetEntity(point[0])->PointGetNum()).Plus(
66                      SK.GetEntity(point[1])->PointGetNum())).ScaledBy(0.5);
67 
68         case NORMAL_IN_3D:
69         case NORMAL_IN_2D:
70         case NORMAL_N_COPY:
71         case NORMAL_N_ROT:
72         case NORMAL_N_ROT_AA:
73             return SK.GetEntity(point[0])->PointGetNum();
74 
75         default: oops();
76     }
77 }
78 
VectorGetStartPoint(void)79 Vector EntityBase::VectorGetStartPoint(void) {
80     switch(type) {
81         case LINE_SEGMENT:
82             return SK.GetEntity(point[1])->PointGetNum();
83 
84         case NORMAL_IN_3D:
85         case NORMAL_IN_2D:
86         case NORMAL_N_COPY:
87         case NORMAL_N_ROT:
88         case NORMAL_N_ROT_AA:
89             return SK.GetEntity(point[0])->PointGetNum();
90 
91         default: oops();
92     }
93 }
94 
IsCircle(void)95 bool EntityBase::IsCircle(void) {
96     return (type == CIRCLE) || (type == ARC_OF_CIRCLE);
97 }
98 
CircleGetRadiusExpr(void)99 Expr *EntityBase::CircleGetRadiusExpr(void) {
100     if(type == CIRCLE) {
101         return SK.GetEntity(distance)->DistanceGetExpr();
102     } else if(type == ARC_OF_CIRCLE) {
103         return Constraint::Distance(workplane, point[0], point[1]);
104     } else oops();
105 }
106 
CircleGetRadiusNum(void)107 double EntityBase::CircleGetRadiusNum(void) {
108     if(type == CIRCLE) {
109         return SK.GetEntity(distance)->DistanceGetNum();
110     } else if(type == ARC_OF_CIRCLE) {
111         Vector c  = SK.GetEntity(point[0])->PointGetNum();
112         Vector pa = SK.GetEntity(point[1])->PointGetNum();
113         return (pa.Minus(c)).Magnitude();
114     } else oops();
115 }
116 
ArcGetAngles(double * thetaa,double * thetab,double * dtheta)117 void EntityBase::ArcGetAngles(double *thetaa, double *thetab, double *dtheta) {
118     if(type != ARC_OF_CIRCLE) oops();
119 
120     Quaternion q = Normal()->NormalGetNum();
121     Vector u = q.RotationU(), v = q.RotationV();
122 
123     Vector c  = SK.GetEntity(point[0])->PointGetNum();
124     Vector pa = SK.GetEntity(point[1])->PointGetNum();
125     Vector pb = SK.GetEntity(point[2])->PointGetNum();
126 
127     Point2d c2  = c.Project2d(u, v);
128     Point2d pa2 = (pa.Project2d(u, v)).Minus(c2);
129     Point2d pb2 = (pb.Project2d(u, v)).Minus(c2);
130 
131     *thetaa = atan2(pa2.y, pa2.x);
132     *thetab = atan2(pb2.y, pb2.x);
133     *dtheta = *thetab - *thetaa;
134     // If the endpoints are coincident, call it a full arc, not a zero arc;
135     // useful concept to have when splitting
136     while(*dtheta < 1e-6) *dtheta += 2*PI;
137     while(*dtheta > (2*PI)) *dtheta -= 2*PI;
138 }
139 
CubicGetStartNum(void)140 Vector EntityBase::CubicGetStartNum(void) {
141     return SK.GetEntity(point[0])->PointGetNum();
142 }
CubicGetFinishNum(void)143 Vector EntityBase::CubicGetFinishNum(void) {
144     return SK.GetEntity(point[3+extraPoints])->PointGetNum();
145 }
CubicGetStartTangentExprs(void)146 ExprVector EntityBase::CubicGetStartTangentExprs(void) {
147     ExprVector pon  = SK.GetEntity(point[0])->PointGetExprs(),
148                poff = SK.GetEntity(point[1])->PointGetExprs();
149     return (pon.Minus(poff));
150 }
CubicGetFinishTangentExprs(void)151 ExprVector EntityBase::CubicGetFinishTangentExprs(void) {
152     ExprVector pon  = SK.GetEntity(point[3+extraPoints])->PointGetExprs(),
153                poff = SK.GetEntity(point[2+extraPoints])->PointGetExprs();
154     return (pon.Minus(poff));
155 }
CubicGetStartTangentNum(void)156 Vector EntityBase::CubicGetStartTangentNum(void) {
157     Vector pon  = SK.GetEntity(point[0])->PointGetNum(),
158            poff = SK.GetEntity(point[1])->PointGetNum();
159     return (pon.Minus(poff));
160 }
CubicGetFinishTangentNum(void)161 Vector EntityBase::CubicGetFinishTangentNum(void) {
162     Vector pon  = SK.GetEntity(point[3+extraPoints])->PointGetNum(),
163            poff = SK.GetEntity(point[2+extraPoints])->PointGetNum();
164     return (pon.Minus(poff));
165 }
166 
IsWorkplane(void)167 bool EntityBase::IsWorkplane(void) {
168     return (type == WORKPLANE);
169 }
170 
WorkplaneGetOffsetExprs(void)171 ExprVector EntityBase::WorkplaneGetOffsetExprs(void) {
172     return SK.GetEntity(point[0])->PointGetExprs();
173 }
174 
WorkplaneGetOffset(void)175 Vector EntityBase::WorkplaneGetOffset(void) {
176     return SK.GetEntity(point[0])->PointGetNum();
177 }
178 
WorkplaneGetPlaneExprs(ExprVector * n,Expr ** dn)179 void EntityBase::WorkplaneGetPlaneExprs(ExprVector *n, Expr **dn) {
180     if(type == WORKPLANE) {
181         *n = Normal()->NormalExprsN();
182 
183         ExprVector p0 = SK.GetEntity(point[0])->PointGetExprs();
184         // The plane is n dot (p - p0) = 0, or
185         //              n dot p - n dot p0 = 0
186         // so dn = n dot p0
187         *dn = p0.Dot(*n);
188     } else {
189         oops();
190     }
191 }
192 
IsDistance(void)193 bool EntityBase::IsDistance(void) {
194     return (type == DISTANCE) ||
195            (type == DISTANCE_N_COPY);
196 }
DistanceGetNum(void)197 double EntityBase::DistanceGetNum(void) {
198     if(type == DISTANCE) {
199         return SK.GetParam(param[0])->val;
200     } else if(type == DISTANCE_N_COPY) {
201         return numDistance;
202     } else oops();
203 }
DistanceGetExpr(void)204 Expr *EntityBase::DistanceGetExpr(void) {
205     if(type == DISTANCE) {
206         return Expr::From(param[0]);
207     } else if(type == DISTANCE_N_COPY) {
208         return Expr::From(numDistance);
209     } else oops();
210 }
DistanceForceTo(double v)211 void EntityBase::DistanceForceTo(double v) {
212     if(type == DISTANCE) {
213         (SK.GetParam(param[0]))->val = v;
214     } else if(type == DISTANCE_N_COPY) {
215         // do nothing, it's locked
216     } else oops();
217 }
218 
Normal(void)219 EntityBase *EntityBase::Normal(void) {
220     return SK.GetEntity(normal);
221 }
222 
IsPoint(void)223 bool EntityBase::IsPoint(void) {
224     switch(type) {
225         case POINT_IN_3D:
226         case POINT_IN_2D:
227         case POINT_N_COPY:
228         case POINT_N_TRANS:
229         case POINT_N_ROT_TRANS:
230         case POINT_N_ROT_AA:
231             return true;
232 
233         default:
234             return false;
235     }
236 }
237 
IsNormal(void)238 bool EntityBase::IsNormal(void) {
239     switch(type) {
240         case NORMAL_IN_3D:
241         case NORMAL_IN_2D:
242         case NORMAL_N_COPY:
243         case NORMAL_N_ROT:
244         case NORMAL_N_ROT_AA:
245             return true;
246 
247         default:           return false;
248     }
249 }
250 
NormalGetNum(void)251 Quaternion EntityBase::NormalGetNum(void) {
252     Quaternion q;
253     switch(type) {
254         case NORMAL_IN_3D:
255             q = Quaternion::From(param[0], param[1], param[2], param[3]);
256             break;
257 
258         case NORMAL_IN_2D: {
259             EntityBase *wrkpl = SK.GetEntity(workplane);
260             EntityBase *norm = SK.GetEntity(wrkpl->normal);
261             q = norm->NormalGetNum();
262             break;
263         }
264         case NORMAL_N_COPY:
265             q = numNormal;
266             break;
267 
268         case NORMAL_N_ROT:
269             q = Quaternion::From(param[0], param[1], param[2], param[3]);
270             q = q.Times(numNormal);
271             break;
272 
273         case NORMAL_N_ROT_AA: {
274             q = GetAxisAngleQuaternion(0);
275             q = q.Times(numNormal);
276             break;
277         }
278 
279         default: oops();
280     }
281     return q;
282 }
283 
NormalForceTo(Quaternion q)284 void EntityBase::NormalForceTo(Quaternion q) {
285     switch(type) {
286         case NORMAL_IN_3D:
287             SK.GetParam(param[0])->val = q.w;
288             SK.GetParam(param[1])->val = q.vx;
289             SK.GetParam(param[2])->val = q.vy;
290             SK.GetParam(param[3])->val = q.vz;
291             break;
292 
293         case NORMAL_IN_2D:
294         case NORMAL_N_COPY:
295             // There's absolutely nothing to do; these are locked.
296             break;
297         case NORMAL_N_ROT: {
298             Quaternion qp = q.Times(numNormal.Inverse());
299 
300             SK.GetParam(param[0])->val = qp.w;
301             SK.GetParam(param[1])->val = qp.vx;
302             SK.GetParam(param[2])->val = qp.vy;
303             SK.GetParam(param[3])->val = qp.vz;
304             break;
305         }
306 
307         case NORMAL_N_ROT_AA:
308             // Not sure if I'll bother implementing this one
309             break;
310 
311         default: oops();
312     }
313 }
314 
NormalU(void)315 Vector EntityBase::NormalU(void) {
316     return NormalGetNum().RotationU();
317 }
NormalV(void)318 Vector EntityBase::NormalV(void) {
319     return NormalGetNum().RotationV();
320 }
NormalN(void)321 Vector EntityBase::NormalN(void) {
322     return NormalGetNum().RotationN();
323 }
324 
NormalExprsU(void)325 ExprVector EntityBase::NormalExprsU(void) {
326     return NormalGetExprs().RotationU();
327 }
NormalExprsV(void)328 ExprVector EntityBase::NormalExprsV(void) {
329     return NormalGetExprs().RotationV();
330 }
NormalExprsN(void)331 ExprVector EntityBase::NormalExprsN(void) {
332     return NormalGetExprs().RotationN();
333 }
334 
NormalGetExprs(void)335 ExprQuaternion EntityBase::NormalGetExprs(void) {
336     ExprQuaternion q;
337     switch(type) {
338         case NORMAL_IN_3D:
339             q = ExprQuaternion::From(param[0], param[1], param[2], param[3]);
340             break;
341 
342         case NORMAL_IN_2D: {
343             EntityBase *wrkpl = SK.GetEntity(workplane);
344             EntityBase *norm = SK.GetEntity(wrkpl->normal);
345             q = norm->NormalGetExprs();
346             break;
347         }
348         case NORMAL_N_COPY:
349             q = ExprQuaternion::From(numNormal);
350             break;
351 
352         case NORMAL_N_ROT: {
353             ExprQuaternion orig = ExprQuaternion::From(numNormal);
354             q = ExprQuaternion::From(param[0], param[1], param[2], param[3]);
355 
356             q = q.Times(orig);
357             break;
358         }
359 
360         case NORMAL_N_ROT_AA: {
361             ExprQuaternion orig = ExprQuaternion::From(numNormal);
362             q = GetAxisAngleQuaternionExprs(0);
363             q = q.Times(orig);
364             break;
365         }
366 
367         default: oops();
368     }
369     return q;
370 }
371 
PointForceTo(Vector p)372 void EntityBase::PointForceTo(Vector p) {
373     switch(type) {
374         case POINT_IN_3D:
375             SK.GetParam(param[0])->val = p.x;
376             SK.GetParam(param[1])->val = p.y;
377             SK.GetParam(param[2])->val = p.z;
378             break;
379 
380         case POINT_IN_2D: {
381             EntityBase *c = SK.GetEntity(workplane);
382             p = p.Minus(c->WorkplaneGetOffset());
383             SK.GetParam(param[0])->val = p.Dot(c->Normal()->NormalU());
384             SK.GetParam(param[1])->val = p.Dot(c->Normal()->NormalV());
385             break;
386         }
387 
388         case POINT_N_TRANS: {
389             if(timesApplied == 0) break;
390             Vector trans = (p.Minus(numPoint)).ScaledBy(1.0/timesApplied);
391             SK.GetParam(param[0])->val = trans.x;
392             SK.GetParam(param[1])->val = trans.y;
393             SK.GetParam(param[2])->val = trans.z;
394             break;
395         }
396 
397         case POINT_N_ROT_TRANS: {
398             // Force only the translation; leave the rotation unchanged. But
399             // remember that we're working with respect to the rotated
400             // point.
401             Vector trans = p.Minus(PointGetQuaternion().Rotate(numPoint));
402             SK.GetParam(param[0])->val = trans.x;
403             SK.GetParam(param[1])->val = trans.y;
404             SK.GetParam(param[2])->val = trans.z;
405             break;
406         }
407 
408         case POINT_N_ROT_AA: {
409             // Force only the angle; the axis and center of rotation stay
410             Vector offset = Vector::From(param[0], param[1], param[2]);
411             Vector normal = Vector::From(param[4], param[5], param[6]);
412             Vector u = normal.Normal(0), v = normal.Normal(1);
413             Vector po = p.Minus(offset), numo = numPoint.Minus(offset);
414             double thetap = atan2(v.Dot(po), u.Dot(po));
415             double thetan = atan2(v.Dot(numo), u.Dot(numo));
416             double thetaf = (thetap - thetan);
417             double thetai = (SK.GetParam(param[3])->val)*timesApplied*2;
418             double dtheta = thetaf - thetai;
419             // Take the smallest possible change in the actual step angle,
420             // in order to avoid jumps when you cross from +pi to -pi
421             while(dtheta < -PI) dtheta += 2*PI;
422             while(dtheta > PI) dtheta -= 2*PI;
423             SK.GetParam(param[3])->val = (thetai + dtheta)/(timesApplied*2);
424             break;
425         }
426 
427         case POINT_N_COPY:
428             // Nothing to do; it's a static copy
429             break;
430 
431         default: oops();
432     }
433 }
434 
PointGetNum(void)435 Vector EntityBase::PointGetNum(void) {
436     Vector p;
437     switch(type) {
438         case POINT_IN_3D:
439             p = Vector::From(param[0], param[1], param[2]);
440             break;
441 
442         case POINT_IN_2D: {
443             EntityBase *c = SK.GetEntity(workplane);
444             Vector u = c->Normal()->NormalU();
445             Vector v = c->Normal()->NormalV();
446             p =        u.ScaledBy(SK.GetParam(param[0])->val);
447             p = p.Plus(v.ScaledBy(SK.GetParam(param[1])->val));
448             p = p.Plus(c->WorkplaneGetOffset());
449             break;
450         }
451 
452         case POINT_N_TRANS: {
453             Vector trans = Vector::From(param[0], param[1], param[2]);
454             p = numPoint.Plus(trans.ScaledBy(timesApplied));
455             break;
456         }
457 
458         case POINT_N_ROT_TRANS: {
459             Vector offset = Vector::From(param[0], param[1], param[2]);
460             Quaternion q = PointGetQuaternion();
461             p = q.Rotate(numPoint);
462             p = p.Plus(offset);
463             break;
464         }
465 
466         case POINT_N_ROT_AA: {
467             Vector offset = Vector::From(param[0], param[1], param[2]);
468             Quaternion q = PointGetQuaternion();
469             p = numPoint.Minus(offset);
470             p = q.Rotate(p);
471             p = p.Plus(offset);
472             break;
473         }
474 
475         case POINT_N_COPY:
476             p = numPoint;
477             break;
478 
479         default: oops();
480     }
481     return p;
482 }
483 
PointGetExprs(void)484 ExprVector EntityBase::PointGetExprs(void) {
485     ExprVector r;
486     switch(type) {
487         case POINT_IN_3D:
488             r = ExprVector::From(param[0], param[1], param[2]);
489             break;
490 
491         case POINT_IN_2D: {
492             EntityBase *c = SK.GetEntity(workplane);
493             ExprVector u = c->Normal()->NormalExprsU();
494             ExprVector v = c->Normal()->NormalExprsV();
495             r = c->WorkplaneGetOffsetExprs();
496             r = r.Plus(u.ScaledBy(Expr::From(param[0])));
497             r = r.Plus(v.ScaledBy(Expr::From(param[1])));
498             break;
499         }
500         case POINT_N_TRANS: {
501             ExprVector orig = ExprVector::From(numPoint);
502             ExprVector trans = ExprVector::From(param[0], param[1], param[2]);
503             r = orig.Plus(trans.ScaledBy(Expr::From(timesApplied)));
504             break;
505         }
506         case POINT_N_ROT_TRANS: {
507             ExprVector orig = ExprVector::From(numPoint);
508             ExprVector trans = ExprVector::From(param[0], param[1], param[2]);
509             ExprQuaternion q =
510                 ExprQuaternion::From(param[3], param[4], param[5], param[6]);
511             orig = q.Rotate(orig);
512             r = orig.Plus(trans);
513             break;
514         }
515         case POINT_N_ROT_AA: {
516             ExprVector orig = ExprVector::From(numPoint);
517             ExprVector trans = ExprVector::From(param[0], param[1], param[2]);
518             ExprQuaternion q = GetAxisAngleQuaternionExprs(3);
519             orig = orig.Minus(trans);
520             orig = q.Rotate(orig);
521             r = orig.Plus(trans);
522             break;
523         }
524         case POINT_N_COPY:
525             r = ExprVector::From(numPoint);
526             break;
527 
528         default: oops();
529     }
530     return r;
531 }
532 
PointGetExprsInWorkplane(hEntity wrkpl,Expr ** u,Expr ** v)533 void EntityBase::PointGetExprsInWorkplane(hEntity wrkpl, Expr **u, Expr **v) {
534     if(type == POINT_IN_2D && workplane.v == wrkpl.v) {
535         // They want our coordinates in the form that we've written them,
536         // very nice.
537         *u = Expr::From(param[0]);
538         *v = Expr::From(param[1]);
539     } else {
540         // Get the offset and basis vectors for this weird exotic csys.
541         EntityBase *w = SK.GetEntity(wrkpl);
542         ExprVector wp = w->WorkplaneGetOffsetExprs();
543         ExprVector wu = w->Normal()->NormalExprsU();
544         ExprVector wv = w->Normal()->NormalExprsV();
545 
546         // Get our coordinates in three-space, and project them into that
547         // coordinate system.
548         ExprVector ev = PointGetExprs();
549         ev = ev.Minus(wp);
550         *u = ev.Dot(wu);
551         *v = ev.Dot(wv);
552     }
553 }
554 
PointForceQuaternionTo(Quaternion q)555 void EntityBase::PointForceQuaternionTo(Quaternion q) {
556     if(type != POINT_N_ROT_TRANS) oops();
557 
558     SK.GetParam(param[3])->val = q.w;
559     SK.GetParam(param[4])->val = q.vx;
560     SK.GetParam(param[5])->val = q.vy;
561     SK.GetParam(param[6])->val = q.vz;
562 }
563 
GetAxisAngleQuaternion(int param0)564 Quaternion EntityBase::GetAxisAngleQuaternion(int param0) {
565     Quaternion q;
566     double theta = timesApplied*SK.GetParam(param[param0+0])->val;
567     double s = sin(theta), c = cos(theta);
568     q.w = c;
569     q.vx = s*SK.GetParam(param[param0+1])->val;
570     q.vy = s*SK.GetParam(param[param0+2])->val;
571     q.vz = s*SK.GetParam(param[param0+3])->val;
572     return q;
573 }
574 
GetAxisAngleQuaternionExprs(int param0)575 ExprQuaternion EntityBase::GetAxisAngleQuaternionExprs(int param0) {
576     ExprQuaternion q;
577 
578     Expr *theta = Expr::From(timesApplied)->Times(
579                   Expr::From(param[param0+0]));
580     Expr *c = theta->Cos(), *s = theta->Sin();
581     q.w = c;
582     q.vx = s->Times(Expr::From(param[param0+1]));
583     q.vy = s->Times(Expr::From(param[param0+2]));
584     q.vz = s->Times(Expr::From(param[param0+3]));
585     return q;
586 }
587 
PointGetQuaternion(void)588 Quaternion EntityBase::PointGetQuaternion(void) {
589     Quaternion q;
590 
591     if(type == POINT_N_ROT_AA) {
592         q = GetAxisAngleQuaternion(3);
593     } else if(type == POINT_N_ROT_TRANS) {
594         q = Quaternion::From(param[3], param[4], param[5], param[6]);
595     } else oops();
596 
597     return q;
598 }
599 
IsFace(void)600 bool EntityBase::IsFace(void) {
601     switch(type) {
602         case FACE_NORMAL_PT:
603         case FACE_XPROD:
604         case FACE_N_ROT_TRANS:
605         case FACE_N_TRANS:
606         case FACE_N_ROT_AA:
607             return true;
608         default:
609             return false;
610     }
611 }
612 
FaceGetNormalExprs(void)613 ExprVector EntityBase::FaceGetNormalExprs(void) {
614     ExprVector r;
615     if(type == FACE_NORMAL_PT) {
616         Vector v = Vector::From(numNormal.vx, numNormal.vy, numNormal.vz);
617         r = ExprVector::From(v.WithMagnitude(1));
618     } else if(type == FACE_XPROD) {
619         ExprVector vc = ExprVector::From(param[0], param[1], param[2]);
620         ExprVector vn =
621             ExprVector::From(numNormal.vx, numNormal.vy, numNormal.vz);
622         r = vc.Cross(vn);
623         r = r.WithMagnitude(Expr::From(1.0));
624     } else if(type == FACE_N_ROT_TRANS) {
625         // The numerical normal vector gets the rotation; the numerical
626         // normal has magnitude one, and the rotation doesn't change that,
627         // so there's no need to fix it up.
628         r = ExprVector::From(numNormal.vx, numNormal.vy, numNormal.vz);
629         ExprQuaternion q =
630             ExprQuaternion::From(param[3], param[4], param[5], param[6]);
631         r = q.Rotate(r);
632     } else if(type == FACE_N_TRANS) {
633         r = ExprVector::From(numNormal.vx, numNormal.vy, numNormal.vz);
634     } else if(type == FACE_N_ROT_AA) {
635         r = ExprVector::From(numNormal.vx, numNormal.vy, numNormal.vz);
636         ExprQuaternion q = GetAxisAngleQuaternionExprs(3);
637         r = q.Rotate(r);
638     } else oops();
639     return r;
640 }
641 
FaceGetNormalNum(void)642 Vector EntityBase::FaceGetNormalNum(void) {
643     Vector r;
644     if(type == FACE_NORMAL_PT) {
645         r = Vector::From(numNormal.vx, numNormal.vy, numNormal.vz);
646     } else if(type == FACE_XPROD) {
647         Vector vc = Vector::From(param[0], param[1], param[2]);
648         Vector vn = Vector::From(numNormal.vx, numNormal.vy, numNormal.vz);
649         r = vc.Cross(vn);
650     } else if(type == FACE_N_ROT_TRANS) {
651         // The numerical normal vector gets the rotation
652         r = Vector::From(numNormal.vx, numNormal.vy, numNormal.vz);
653         Quaternion q = Quaternion::From(param[3], param[4], param[5], param[6]);
654         r = q.Rotate(r);
655     } else if(type == FACE_N_TRANS) {
656         r = Vector::From(numNormal.vx, numNormal.vy, numNormal.vz);
657     } else if(type == FACE_N_ROT_AA) {
658         r = Vector::From(numNormal.vx, numNormal.vy, numNormal.vz);
659         Quaternion q = GetAxisAngleQuaternion(3);
660         r = q.Rotate(r);
661     } else oops();
662     return r.WithMagnitude(1);
663 }
664 
FaceGetPointExprs(void)665 ExprVector EntityBase::FaceGetPointExprs(void) {
666     ExprVector r;
667     if(type == FACE_NORMAL_PT) {
668         r = SK.GetEntity(point[0])->PointGetExprs();
669     } else if(type == FACE_XPROD) {
670         r = ExprVector::From(numPoint);
671     } else if(type == FACE_N_ROT_TRANS) {
672         // The numerical point gets the rotation and translation.
673         ExprVector trans = ExprVector::From(param[0], param[1], param[2]);
674         ExprQuaternion q =
675             ExprQuaternion::From(param[3], param[4], param[5], param[6]);
676         r = ExprVector::From(numPoint);
677         r = q.Rotate(r);
678         r = r.Plus(trans);
679     } else if(type == FACE_N_TRANS) {
680         ExprVector trans = ExprVector::From(param[0], param[1], param[2]);
681         r = ExprVector::From(numPoint);
682         r = r.Plus(trans.ScaledBy(Expr::From(timesApplied)));
683     } else if(type == FACE_N_ROT_AA) {
684         ExprVector trans = ExprVector::From(param[0], param[1], param[2]);
685         ExprQuaternion q = GetAxisAngleQuaternionExprs(3);
686         r = ExprVector::From(numPoint);
687         r = r.Minus(trans);
688         r = q.Rotate(r);
689         r = r.Plus(trans);
690     } else oops();
691     return r;
692 }
693 
FaceGetPointNum(void)694 Vector EntityBase::FaceGetPointNum(void) {
695     Vector r;
696     if(type == FACE_NORMAL_PT) {
697         r = SK.GetEntity(point[0])->PointGetNum();
698     } else if(type == FACE_XPROD) {
699         r = numPoint;
700     } else if(type == FACE_N_ROT_TRANS) {
701         // The numerical point gets the rotation and translation.
702         Vector trans = Vector::From(param[0], param[1], param[2]);
703         Quaternion q = Quaternion::From(param[3], param[4], param[5], param[6]);
704         r = q.Rotate(numPoint);
705         r = r.Plus(trans);
706     } else if(type == FACE_N_TRANS) {
707         Vector trans = Vector::From(param[0], param[1], param[2]);
708         r = numPoint.Plus(trans.ScaledBy(timesApplied));
709     } else if(type == FACE_N_ROT_AA) {
710         Vector trans = Vector::From(param[0], param[1], param[2]);
711         Quaternion q = GetAxisAngleQuaternion(3);
712         r = numPoint.Minus(trans);
713         r = q.Rotate(r);
714         r = r.Plus(trans);
715     } else oops();
716     return r;
717 }
718 
HasEndpoints(void)719 bool EntityBase::HasEndpoints(void) {
720     return (type == LINE_SEGMENT) ||
721            (type == CUBIC) ||
722            (type == ARC_OF_CIRCLE);
723 }
EndpointStart()724 Vector EntityBase::EndpointStart() {
725     if(type == LINE_SEGMENT) {
726         return SK.GetEntity(point[0])->PointGetNum();
727     } else if(type == CUBIC) {
728         return CubicGetStartNum();
729     } else if(type == ARC_OF_CIRCLE) {
730         return SK.GetEntity(point[1])->PointGetNum();
731     } else {
732         oops();
733     }
734 }
EndpointFinish()735 Vector EntityBase::EndpointFinish() {
736     if(type == LINE_SEGMENT) {
737         return SK.GetEntity(point[1])->PointGetNum();
738     } else if(type == CUBIC) {
739         return CubicGetFinishNum();
740     } else if(type == ARC_OF_CIRCLE) {
741         return SK.GetEntity(point[2])->PointGetNum();
742     } else {
743         oops();
744     }
745 }
746 
AddEq(IdList<Equation,hEquation> * l,Expr * expr,int index)747 void EntityBase::AddEq(IdList<Equation,hEquation> *l, Expr *expr, int index) {
748     Equation eq;
749     eq.e = expr;
750     eq.h = h.equation(index);
751     l->Add(&eq);
752 }
753 
GenerateEquations(IdList<Equation,hEquation> * l)754 void EntityBase::GenerateEquations(IdList<Equation,hEquation> *l) {
755     switch(type) {
756         case NORMAL_IN_3D: {
757             ExprQuaternion q = NormalGetExprs();
758             AddEq(l, (q.Magnitude())->Minus(Expr::From(1)), 0);
759             break;
760         }
761         case ARC_OF_CIRCLE: {
762             // If this is a copied entity, with its point already fixed
763             // with respect to each other, then we don't want to generate
764             // the distance constraint!
765             if(SK.GetEntity(point[0])->type != POINT_IN_2D) break;
766 
767             // If the two endpoints of the arc are constrained coincident
768             // (to make a complete circle), then our distance constraint
769             // would be redundant and therefore overconstrain things.
770             int i;
771             for(i = 0; i < SK.constraint.n; i++) {
772                 ConstraintBase *c = &(SK.constraint.elem[i]);
773                 if(c->group.v != group.v) continue;
774                 if(c->type != Constraint::POINTS_COINCIDENT) continue;
775 
776                 if((c->ptA.v == point[1].v && c->ptB.v == point[2].v) ||
777                    (c->ptA.v == point[2].v && c->ptB.v == point[1].v))
778                 {
779                     break;
780                 }
781             }
782             if(i < SK.constraint.n) break;
783 
784             Expr *ra = Constraint::Distance(workplane, point[0], point[1]);
785             Expr *rb = Constraint::Distance(workplane, point[0], point[2]);
786             AddEq(l, ra->Minus(rb), 0);
787             break;
788         }
789         default:;
790             // Most entities do not generate equations.
791     }
792 }
793 
794