Solvespace

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

13
bool EntityBase::HasVector() const {
14
    switch(type) {
15
        case Type::LINE_SEGMENT:
16
        case Type::NORMAL_IN_3D:
17
        case Type::NORMAL_IN_2D:
18
        case Type::NORMAL_N_COPY:
19
        case Type::NORMAL_N_ROT:
20
        case Type::NORMAL_N_ROT_AA:
21
            return true;
22

23
        default:
24
            return false;
25
    }
26
}
27

28
ExprVector EntityBase::VectorGetExprsInWorkplane(hEntity wrkpl) const {
29
    if(IsFace()) {
30
        return FaceGetNormalExprs();
31
    }
32
    switch(type) {
33
        case Type::LINE_SEGMENT:
34
            return (SK.GetEntity(point[0])->PointGetExprsInWorkplane(wrkpl)).Minus(
35
                    SK.GetEntity(point[1])->PointGetExprsInWorkplane(wrkpl));
36

37
        case Type::NORMAL_IN_3D:
38
        case Type::NORMAL_IN_2D:
39
        case Type::NORMAL_N_COPY:
40
        case Type::NORMAL_N_ROT:
41
        case Type::NORMAL_N_ROT_AA: {
42
            ExprVector ev = NormalExprsN();
43
            if(wrkpl == EntityBase::FREE_IN_3D) {
44
                return ev;
45
            }
46
            // Get the offset and basis vectors for this weird exotic csys.
47
            EntityBase *w = SK.GetEntity(wrkpl);
48
            ExprVector wu = w->Normal()->NormalExprsU();
49
            ExprVector wv = w->Normal()->NormalExprsV();
50

51
            // Get our coordinates in three-space, and project them into that
52
            // coordinate system.
53
            ExprVector result;
54
            result.x = ev.Dot(wu);
55
            result.y = ev.Dot(wv);
56
            result.z = Expr::From(0.0);
57
            return result;
58
        }
59
        default: ssassert(false, "Unexpected entity type");
60
    }
61
}
62

63
ExprVector EntityBase::VectorGetExprs() const {
64
    return VectorGetExprsInWorkplane(EntityBase::FREE_IN_3D);
65
}
66

67
Vector EntityBase::VectorGetNum() const {
68
    if(IsFace()) {
69
        return FaceGetNormalNum();
70
    }
71
    switch(type) {
72
        case Type::LINE_SEGMENT:
73
            return (SK.GetEntity(point[0])->PointGetNum()).Minus(
74
                    SK.GetEntity(point[1])->PointGetNum());
75

76
        case Type::NORMAL_IN_3D:
77
        case Type::NORMAL_IN_2D:
78
        case Type::NORMAL_N_COPY:
79
        case Type::NORMAL_N_ROT:
80
        case Type::NORMAL_N_ROT_AA:
81
            return NormalN();
82

83
        default: ssassert(false, "Unexpected entity type");
84
    }
85
}
86

87
Vector EntityBase::VectorGetRefPoint() const {
88
    if(IsFace()) {
89
        return FaceGetPointNum();
90
    }
91
    switch(type) {
92
        case Type::LINE_SEGMENT:
93
            return ((SK.GetEntity(point[0])->PointGetNum()).Plus(
94
                     SK.GetEntity(point[1])->PointGetNum())).ScaledBy(0.5);
95

96
        case Type::NORMAL_IN_3D:
97
        case Type::NORMAL_IN_2D:
98
        case Type::NORMAL_N_COPY:
99
        case Type::NORMAL_N_ROT:
100
        case Type::NORMAL_N_ROT_AA:
101
            return SK.GetEntity(point[0])->PointGetNum();
102

103
        default: ssassert(false, "Unexpected entity type");
104
    }
105
}
106

107
Vector EntityBase::VectorGetStartPoint() const {
108
    switch(type) {
109
        case Type::LINE_SEGMENT:
110
            return SK.GetEntity(point[1])->PointGetNum();
111

112
        case Type::NORMAL_IN_3D:
113
        case Type::NORMAL_IN_2D:
114
        case Type::NORMAL_N_COPY:
115
        case Type::NORMAL_N_ROT:
116
        case Type::NORMAL_N_ROT_AA:
117
            return SK.GetEntity(point[0])->PointGetNum();
118

119
        default: ssassert(false, "Unexpected entity type");
120
    }
121
}
122

123
bool EntityBase::IsCircle() const {
124
    return (type == Type::CIRCLE) || (type == Type::ARC_OF_CIRCLE);
125
}
126

127
Expr *EntityBase::CircleGetRadiusExpr() const {
128
    if(type == Type::CIRCLE) {
129
        return SK.GetEntity(distance)->DistanceGetExpr();
130
    } else if(type == Type::ARC_OF_CIRCLE) {
131
        return Constraint::Distance(workplane, point[0], point[1]);
132
    } else ssassert(false, "Unexpected entity type");
133
}
134

135
double EntityBase::CircleGetRadiusNum() const {
136
    if(type == Type::CIRCLE) {
137
        return SK.GetEntity(distance)->DistanceGetNum();
138
    } else if(type == Type::ARC_OF_CIRCLE) {
139
        Vector c  = SK.GetEntity(point[0])->PointGetNum();
140
        Vector pa = SK.GetEntity(point[1])->PointGetNum();
141
        return (pa.Minus(c)).Magnitude();
142
    } else ssassert(false, "Unexpected entity type");
143
}
144

145
void EntityBase::ArcGetAngles(double *thetaa, double *thetab, double *dtheta) const {
146
    ssassert(type == Type::ARC_OF_CIRCLE, "Unexpected entity type");
147

148
    Quaternion q = Normal()->NormalGetNum();
149
    Vector u = q.RotationU(), v = q.RotationV();
150

151
    Vector c  = SK.GetEntity(point[0])->PointGetNum();
152
    Vector pa = SK.GetEntity(point[1])->PointGetNum();
153
    Vector pb = SK.GetEntity(point[2])->PointGetNum();
154

155
    Point2d c2  = c.Project2d(u, v);
156
    Point2d pa2 = (pa.Project2d(u, v)).Minus(c2);
157
    Point2d pb2 = (pb.Project2d(u, v)).Minus(c2);
158

159
    *thetaa = atan2(pa2.y, pa2.x);
160
    *thetab = atan2(pb2.y, pb2.x);
161
    *dtheta = *thetab - *thetaa;
162
    // If the endpoints are coincident, call it a full arc, not a zero arc;
163
    // useful concept to have when splitting
164
    while(*dtheta < 1e-6) *dtheta += 2*PI;
165
    while(*dtheta > (2*PI)) *dtheta -= 2*PI;
166
}
167

168
Vector EntityBase::CubicGetStartNum() const {
169
    return SK.GetEntity(point[0])->PointGetNum();
170
}
171
Vector EntityBase::CubicGetFinishNum() const {
172
    return SK.GetEntity(point[3+extraPoints])->PointGetNum();
173
}
174
ExprVector EntityBase::CubicGetStartTangentExprs() const {
175
    ExprVector pon  = SK.GetEntity(point[0])->PointGetExprs(),
176
               poff = SK.GetEntity(point[1])->PointGetExprs();
177
    return (pon.Minus(poff));
178
}
179
ExprVector EntityBase::CubicGetFinishTangentExprs() const {
180
    ExprVector pon  = SK.GetEntity(point[3+extraPoints])->PointGetExprs(),
181
               poff = SK.GetEntity(point[2+extraPoints])->PointGetExprs();
182
    return (pon.Minus(poff));
183
}
184
Vector EntityBase::CubicGetStartTangentNum() const {
185
    Vector pon  = SK.GetEntity(point[0])->PointGetNum(),
186
           poff = SK.GetEntity(point[1])->PointGetNum();
187
    return (pon.Minus(poff));
188
}
189
Vector EntityBase::CubicGetFinishTangentNum() const {
190
    Vector pon  = SK.GetEntity(point[3+extraPoints])->PointGetNum(),
191
           poff = SK.GetEntity(point[2+extraPoints])->PointGetNum();
192
    return (pon.Minus(poff));
193
}
194

195
bool EntityBase::IsWorkplane() const {
196
    return (type == Type::WORKPLANE);
197
}
198

199
ExprVector EntityBase::WorkplaneGetOffsetExprs() const {
200
    return SK.GetEntity(point[0])->PointGetExprs();
201
}
202

203
Vector EntityBase::WorkplaneGetOffset() const {
204
    return SK.GetEntity(point[0])->PointGetNum();
205
}
206

207
void EntityBase::WorkplaneGetPlaneExprs(ExprVector *n, Expr **dn) const {
208
    if(type == Type::WORKPLANE) {
209
        *n = Normal()->NormalExprsN();
210

211
        ExprVector p0 = SK.GetEntity(point[0])->PointGetExprs();
212
        // The plane is n dot (p - p0) = 0, or
213
        //              n dot p - n dot p0 = 0
214
        // so dn = n dot p0
215
        *dn = p0.Dot(*n);
216
    } else ssassert(false, "Unexpected entity type");
217
}
218

219
bool EntityBase::IsDistance() const {
220
    return (type == Type::DISTANCE) ||
221
           (type == Type::DISTANCE_N_COPY);
222
}
223
double EntityBase::DistanceGetNum() const {
224
    if(type == Type::DISTANCE) {
225
        return SK.GetParam(param[0])->val;
226
    } else if(type == Type::DISTANCE_N_COPY) {
227
        return numDistance;
228
    } else ssassert(false, "Unexpected entity type");
229
}
230
Expr *EntityBase::DistanceGetExpr() const {
231
    if(type == Type::DISTANCE) {
232
        return Expr::From(param[0]);
233
    } else if(type == Type::DISTANCE_N_COPY) {
234
        return Expr::From(numDistance);
235
    } else ssassert(false, "Unexpected entity type");
236
}
237
void EntityBase::DistanceForceTo(double v) {
238
    if(type == Type::DISTANCE) {
239
        (SK.GetParam(param[0]))->val = v;
240
    } else if(type == Type::DISTANCE_N_COPY) {
241
        // do nothing, it's locked
242
    } else ssassert(false, "Unexpected entity type");
243
}
244

245
EntityBase *EntityBase::Normal() const {
246
    return SK.GetEntity(normal);
247
}
248

249
bool EntityBase::IsPoint() const {
250
    switch(type) {
251
        case Type::POINT_IN_3D:
252
        case Type::POINT_IN_2D:
253
        case Type::POINT_N_COPY:
254
        case Type::POINT_N_TRANS:
255
        case Type::POINT_N_ROT_TRANS:
256
        case Type::POINT_N_ROT_AA:
257
        case Type::POINT_N_ROT_AXIS_TRANS:
258
            return true;
259

260
        default:
261
            return false;
262
    }
263
}
264

265
bool EntityBase::IsNormal() const {
266
    switch(type) {
267
        case Type::NORMAL_IN_3D:
268
        case Type::NORMAL_IN_2D:
269
        case Type::NORMAL_N_COPY:
270
        case Type::NORMAL_N_ROT:
271
        case Type::NORMAL_N_ROT_AA:
272
            return true;
273

274
        default:           return false;
275
    }
276
}
277

278
Quaternion EntityBase::NormalGetNum() const {
279
    Quaternion q;
280
    switch(type) {
281
        case Type::NORMAL_IN_3D:
282
            q = Quaternion::From(param[0], param[1], param[2], param[3]);
283
            break;
284

285
        case Type::NORMAL_IN_2D: {
286
            EntityBase *wrkpl = SK.GetEntity(workplane);
287
            EntityBase *norm = SK.GetEntity(wrkpl->normal);
288
            q = norm->NormalGetNum();
289
            break;
290
        }
291
        case Type::NORMAL_N_COPY:
292
            q = numNormal;
293
            break;
294

295
        case Type::NORMAL_N_ROT:
296
            q = Quaternion::From(param[0], param[1], param[2], param[3]);
297
            q = q.Times(numNormal);
298
            break;
299

300
        case Type::NORMAL_N_ROT_AA: {
301
            q = GetAxisAngleQuaternion(0);
302
            q = q.Times(numNormal);
303
            break;
304
        }
305

306
        default: ssassert(false, "Unexpected entity type");
307
    }
308
    return q;
309
}
310

311
void EntityBase::NormalForceTo(Quaternion q) {
312
    switch(type) {
313
        case Type::NORMAL_IN_3D:
314
            SK.GetParam(param[0])->val = q.w;
315
            SK.GetParam(param[1])->val = q.vx;
316
            SK.GetParam(param[2])->val = q.vy;
317
            SK.GetParam(param[3])->val = q.vz;
318
            break;
319

320
        case Type::NORMAL_IN_2D:
321
        case Type::NORMAL_N_COPY:
322
            // There's absolutely nothing to do; these are locked.
323
            break;
324
        case Type::NORMAL_N_ROT: {
325
            Quaternion qp = q.Times(numNormal.Inverse());
326

327
            SK.GetParam(param[0])->val = qp.w;
328
            SK.GetParam(param[1])->val = qp.vx;
329
            SK.GetParam(param[2])->val = qp.vy;
330
            SK.GetParam(param[3])->val = qp.vz;
331
            break;
332
        }
333

334
        case Type::NORMAL_N_ROT_AA:
335
            // Not sure if I'll bother implementing this one
336
            break;
337

338
        default: ssassert(false, "Unexpected entity type");
339
    }
340
}
341

342
Vector EntityBase::NormalU() const {
343
    return NormalGetNum().RotationU();
344
}
345
Vector EntityBase::NormalV() const {
346
    return NormalGetNum().RotationV();
347
}
348
Vector EntityBase::NormalN() const {
349
    return NormalGetNum().RotationN();
350
}
351

352
ExprVector EntityBase::NormalExprsU() const {
353
    return NormalGetExprs().RotationU();
354
}
355
ExprVector EntityBase::NormalExprsV() const {
356
    return NormalGetExprs().RotationV();
357
}
358
ExprVector EntityBase::NormalExprsN() const {
359
    return NormalGetExprs().RotationN();
360
}
361

362
ExprQuaternion EntityBase::NormalGetExprs() const {
363
    ExprQuaternion q;
364
    switch(type) {
365
        case Type::NORMAL_IN_3D:
366
            q = ExprQuaternion::From(param[0], param[1], param[2], param[3]);
367
            break;
368

369
        case Type::NORMAL_IN_2D: {
370
            EntityBase *wrkpl = SK.GetEntity(workplane);
371
            EntityBase *norm = SK.GetEntity(wrkpl->normal);
372
            q = norm->NormalGetExprs();
373
            break;
374
        }
375
        case Type::NORMAL_N_COPY:
376
            q = ExprQuaternion::From(numNormal);
377
            break;
378

379
        case Type::NORMAL_N_ROT: {
380
            ExprQuaternion orig = ExprQuaternion::From(numNormal);
381
            q = ExprQuaternion::From(param[0], param[1], param[2], param[3]);
382

383
            q = q.Times(orig);
384
            break;
385
        }
386

387
        case Type::NORMAL_N_ROT_AA: {
388
            ExprQuaternion orig = ExprQuaternion::From(numNormal);
389
            q = GetAxisAngleQuaternionExprs(0);
390
            q = q.Times(orig);
391
            break;
392
        }
393

394
        default: ssassert(false, "Unexpected entity type");
395
    }
396
    return q;
397
}
398

399
void EntityBase::PointForceParamTo(Vector p) {
400
    switch(type) {
401
        case Type::POINT_IN_3D:
402
            SK.GetParam(param[0])->val = p.x;
403
            SK.GetParam(param[1])->val = p.y;
404
            SK.GetParam(param[2])->val = p.z;
405
            break;
406

407
        case Type::POINT_IN_2D:
408
            SK.GetParam(param[0])->val = p.x;
409
            SK.GetParam(param[1])->val = p.y;
410
            break;
411

412
        default: ssassert(false, "Unexpected entity type");
413
    }
414
}
415

416
void EntityBase::PointForceTo(Vector p) {
417
    switch(type) {
418
        case Type::POINT_IN_3D:
419
            SK.GetParam(param[0])->val = p.x;
420
            SK.GetParam(param[1])->val = p.y;
421
            SK.GetParam(param[2])->val = p.z;
422
            break;
423

424
        case Type::POINT_IN_2D: {
425
            EntityBase *c = SK.GetEntity(workplane);
426
            p = p.Minus(c->WorkplaneGetOffset());
427
            SK.GetParam(param[0])->val = p.Dot(c->Normal()->NormalU());
428
            SK.GetParam(param[1])->val = p.Dot(c->Normal()->NormalV());
429
            break;
430
        }
431

432
        case Type::POINT_N_TRANS: {
433
            if(timesApplied == 0) break;
434
            Vector trans = (p.Minus(numPoint)).ScaledBy(1.0/timesApplied);
435
            SK.GetParam(param[0])->val = trans.x;
436
            SK.GetParam(param[1])->val = trans.y;
437
            SK.GetParam(param[2])->val = trans.z;
438
            break;
439
        }
440

441
        case Type::POINT_N_ROT_TRANS: {
442
            // Force only the translation; leave the rotation unchanged. But
443
            // remember that we're working with respect to the rotated
444
            // point.
445
            Vector trans = p.Minus(PointGetQuaternion().Rotate(numPoint));
446
            SK.GetParam(param[0])->val = trans.x;
447
            SK.GetParam(param[1])->val = trans.y;
448
            SK.GetParam(param[2])->val = trans.z;
449
            break;
450
        }
451

452
        case Type::POINT_N_ROT_AA: {
453
            // Force only the angle; the axis and center of rotation stay
454
            Vector offset = Vector::From(param[0], param[1], param[2]);
455
            Vector normal = Vector::From(param[4], param[5], param[6]);
456
            Vector u = normal.Normal(0), v = normal.Normal(1);
457
            Vector po = p.Minus(offset), numo = numPoint.Minus(offset);
458
            double thetap = atan2(v.Dot(po), u.Dot(po));
459
            double thetan = atan2(v.Dot(numo), u.Dot(numo));
460
            double thetaf = (thetap - thetan);
461
            double thetai = (SK.GetParam(param[3])->val)*timesApplied*2;
462
            double dtheta = thetaf - thetai;
463
            // Take the smallest possible change in the actual step angle,
464
            // in order to avoid jumps when you cross from +pi to -pi
465
            while(dtheta < -PI) dtheta += 2*PI;
466
            while(dtheta > PI) dtheta -= 2*PI;
467
            // this extra *2 explains the mystery *4
468
            SK.GetParam(param[3])->val = (thetai + dtheta)/(timesApplied*2);
469
            break;
470
        }
471

472
        case Type::POINT_N_ROT_AXIS_TRANS: {
473
            if(timesApplied == 0) break;
474
            // is the point on the rotation axis?
475
            Vector offset = Vector::From(param[0], param[1], param[2]);
476
            Vector normal = Vector::From(param[4], param[5], param[6]).WithMagnitude(1.0);
477
            Vector check = numPoint.Minus(offset).Cross(normal);
478
            if (check.Dot(check) < LENGTH_EPS) { // if so, do extrusion style drag
479
                Vector trans = (p.Minus(numPoint));
480
                SK.GetParam(param[7])->val = trans.Dot(normal)/timesApplied;
481
            } else { // otherwise do rotation style
482
                Vector u = normal.Normal(0), v = normal.Normal(1);
483
                Vector po = p.Minus(offset), numo = numPoint.Minus(offset);
484
                double thetap = atan2(v.Dot(po), u.Dot(po));
485
                double thetan = atan2(v.Dot(numo), u.Dot(numo));
486
                double thetaf = (thetap - thetan);
487
                double thetai = (SK.GetParam(param[3])->val)*timesApplied*2;
488
                double dtheta = thetaf - thetai;
489
                // Take the smallest possible change in the actual step angle,
490
                // in order to avoid jumps when you cross from +pi to -pi
491
                while(dtheta < -PI) dtheta += 2*PI;
492
                while(dtheta > PI) dtheta -= 2*PI;
493
                // this extra *2 explains the mystery *4
494
                SK.GetParam(param[3])->val = (thetai + dtheta)/(timesApplied*2);
495
            }
496
            break;
497
        }
498

499
        case Type::POINT_N_COPY:
500
            // Nothing to do; it's a static copy
501
            break;
502

503
        default: ssassert(false, "Unexpected entity type");
504
    }
505
}
506

507
Vector EntityBase::PointGetNum() const {
508
    Vector p;
509
    switch(type) {
510
        case Type::POINT_IN_3D:
511
            p = Vector::From(param[0], param[1], param[2]);
512
            break;
513

514
        case Type::POINT_IN_2D: {
515
            EntityBase *c = SK.GetEntity(workplane);
516
            Vector u = c->Normal()->NormalU();
517
            Vector v = c->Normal()->NormalV();
518
            p =        u.ScaledBy(SK.GetParam(param[0])->val);
519
            p = p.Plus(v.ScaledBy(SK.GetParam(param[1])->val));
520
            p = p.Plus(c->WorkplaneGetOffset());
521
            break;
522
        }
523

524
        case Type::POINT_N_TRANS: {
525
            Vector trans = Vector::From(param[0], param[1], param[2]);
526
            p = numPoint.Plus(trans.ScaledBy(timesApplied));
527
            break;
528
        }
529

530
        case Type::POINT_N_ROT_TRANS: {
531
            Vector offset = Vector::From(param[0], param[1], param[2]);
532
            Quaternion q = PointGetQuaternion();
533
            p = q.Rotate(numPoint);
534
            p = p.Plus(offset);
535
            break;
536
        }
537

538
        case Type::POINT_N_ROT_AA: {
539
            Vector offset = Vector::From(param[0], param[1], param[2]);
540
            Quaternion q = PointGetQuaternion();
541
            p = numPoint.Minus(offset);
542
            p = q.Rotate(p);
543
            p = p.Plus(offset);
544
            break;
545
        }
546

547
        case Type::POINT_N_ROT_AXIS_TRANS: {
548
            Vector offset = Vector::From(param[0], param[1], param[2]);
549
            Vector displace = Vector::From(param[4], param[5], param[6])
550
               .WithMagnitude(SK.GetParam(param[7])->val).ScaledBy(timesApplied);
551
            Quaternion q = PointGetQuaternion();
552
            p = numPoint.Minus(offset);
553
            p = q.Rotate(p);
554
            p = p.Plus(offset).Plus(displace);
555
            break;
556
        }
557

558
        case Type::POINT_N_COPY:
559
            p = numPoint;
560
            break;
561

562
        default: ssassert(false, "Unexpected entity type");
563
    }
564
    return p;
565
}
566

567
ExprVector EntityBase::PointGetExprs() const {
568
    ExprVector r;
569
    switch(type) {
570
        case Type::POINT_IN_3D:
571
            r = ExprVector::From(param[0], param[1], param[2]);
572
            break;
573

574
        case Type::POINT_IN_2D: {
575
            EntityBase *c = SK.GetEntity(workplane);
576
            ExprVector u = c->Normal()->NormalExprsU();
577
            ExprVector v = c->Normal()->NormalExprsV();
578
            r = c->WorkplaneGetOffsetExprs();
579
            r = r.Plus(u.ScaledBy(Expr::From(param[0])));
580
            r = r.Plus(v.ScaledBy(Expr::From(param[1])));
581
            break;
582
        }
583
        case Type::POINT_N_TRANS: {
584
            ExprVector orig = ExprVector::From(numPoint);
585
            ExprVector trans = ExprVector::From(param[0], param[1], param[2]);
586
            r = orig.Plus(trans.ScaledBy(Expr::From(timesApplied)));
587
            break;
588
        }
589
        case Type::POINT_N_ROT_TRANS: {
590
            ExprVector orig = ExprVector::From(numPoint);
591
            ExprVector trans = ExprVector::From(param[0], param[1], param[2]);
592
            ExprQuaternion q =
593
                ExprQuaternion::From(param[3], param[4], param[5], param[6]);
594
            orig = q.Rotate(orig);
595
            r = orig.Plus(trans);
596
            break;
597
        }
598
        case Type::POINT_N_ROT_AA: {
599
            ExprVector orig = ExprVector::From(numPoint);
600
            ExprVector trans = ExprVector::From(param[0], param[1], param[2]);
601
            ExprQuaternion q = GetAxisAngleQuaternionExprs(3);
602
            orig = orig.Minus(trans);
603
            orig = q.Rotate(orig);
604
            r = orig.Plus(trans);
605
            break;
606
        }
607
        case Type::POINT_N_ROT_AXIS_TRANS: {
608
            ExprVector orig = ExprVector::From(numPoint);
609
            ExprVector trans = ExprVector::From(param[0], param[1], param[2]);
610
            ExprVector displace = ExprVector::From(param[4], param[5], param[6])
611
               .WithMagnitude(Expr::From(1.0)).ScaledBy(Expr::From(timesApplied)).ScaledBy(Expr::From(param[7]));
612

613
            ExprQuaternion q = GetAxisAngleQuaternionExprs(3);
614
            orig = orig.Minus(trans);
615
            orig = q.Rotate(orig);
616
            r = orig.Plus(trans).Plus(displace);
617
            break;
618
        }
619
        case Type::POINT_N_COPY:
620
            r = ExprVector::From(numPoint);
621
            break;
622

623
        default: ssassert(false, "Unexpected entity type");
624
    }
625
    return r;
626
}
627

628
void EntityBase::PointGetExprsInWorkplane(hEntity wrkpl, Expr **u, Expr **v) const {
629
    if(type == Type::POINT_IN_2D && workplane == wrkpl) {
630
        // They want our coordinates in the form that we've written them,
631
        // very nice.
632
        *u = Expr::From(param[0]);
633
        *v = Expr::From(param[1]);
634
    } else {
635
        // Get the offset and basis vectors for this weird exotic csys.
636
        EntityBase *w = SK.GetEntity(wrkpl);
637
        ExprVector wp = w->WorkplaneGetOffsetExprs();
638
        ExprVector wu = w->Normal()->NormalExprsU();
639
        ExprVector wv = w->Normal()->NormalExprsV();
640

641
        // Get our coordinates in three-space, and project them into that
642
        // coordinate system.
643
        ExprVector ev = PointGetExprs();
644
        ev = ev.Minus(wp);
645
        *u = ev.Dot(wu);
646
        *v = ev.Dot(wv);
647
    }
648
}
649

650
ExprVector EntityBase::PointGetExprsInWorkplane(hEntity wrkpl) const {
651
    if(wrkpl == Entity::FREE_IN_3D) {
652
        return PointGetExprs();
653
    }
654

655
    ExprVector r;
656
    PointGetExprsInWorkplane(wrkpl, &r.x, &r.y);
657
    r.z = Expr::From(0.0);
658
    return r;
659
}
660

661
void EntityBase::PointForceQuaternionTo(Quaternion q) {
662
    ssassert(type == Type::POINT_N_ROT_TRANS, "Unexpected entity type");
663

664
    SK.GetParam(param[3])->val = q.w;
665
    SK.GetParam(param[4])->val = q.vx;
666
    SK.GetParam(param[5])->val = q.vy;
667
    SK.GetParam(param[6])->val = q.vz;
668
}
669

670
Quaternion EntityBase::GetAxisAngleQuaternion(int param0) const {
671
    Quaternion q;
672
    double theta = timesApplied*SK.GetParam(param[param0+0])->val;
673
    double s = sin(theta), c = cos(theta);
674
    q.w = c;
675
    q.vx = s*SK.GetParam(param[param0+1])->val;
676
    q.vy = s*SK.GetParam(param[param0+2])->val;
677
    q.vz = s*SK.GetParam(param[param0+3])->val;
678
    return q;
679
}
680

681
ExprQuaternion EntityBase::GetAxisAngleQuaternionExprs(int param0) const {
682
    ExprQuaternion q;
683

684
    Expr *theta = Expr::From(timesApplied)->Times(
685
                  Expr::From(param[param0+0]));
686
    Expr *c = theta->Cos(), *s = theta->Sin();
687
    q.w = c;
688
    q.vx = s->Times(Expr::From(param[param0+1]));
689
    q.vy = s->Times(Expr::From(param[param0+2]));
690
    q.vz = s->Times(Expr::From(param[param0+3]));
691
    return q;
692
}
693

694
Quaternion EntityBase::PointGetQuaternion() const {
695
    Quaternion q;
696

697
    if(type == Type::POINT_N_ROT_AA || type == Type::POINT_N_ROT_AXIS_TRANS) {
698
        q = GetAxisAngleQuaternion(3);
699
    } else if(type == Type::POINT_N_ROT_TRANS) {
700
        q = Quaternion::From(param[3], param[4], param[5], param[6]);
701
    } else ssassert(false, "Unexpected entity type");
702

703
    return q;
704
}
705

706
bool EntityBase::IsFace() const {
707
    switch(type) {
708
        case Type::FACE_NORMAL_PT:
709
        case Type::FACE_XPROD:
710
        case Type::FACE_N_ROT_TRANS:
711
        case Type::FACE_N_TRANS:
712
        case Type::FACE_N_ROT_AA:
713
        case Type::FACE_ROT_NORMAL_PT:
714
        case Type::FACE_N_ROT_AXIS_TRANS:
715
            return true;
716
        default:
717
            return false;
718
    }
719
}
720

721
ExprVector EntityBase::FaceGetNormalExprs() const {
722
    ExprVector r;
723
    if(type == Type::FACE_NORMAL_PT) {
724
        Vector v = Vector::From(numNormal.vx, numNormal.vy, numNormal.vz);
725
        r = ExprVector::From(v.WithMagnitude(1));
726
    } else if(type == Type::FACE_XPROD) {
727
        ExprVector vc = ExprVector::From(param[0], param[1], param[2]);
728
        ExprVector vn =
729
            ExprVector::From(numNormal.vx, numNormal.vy, numNormal.vz);
730
        r = vc.Cross(vn);
731
        r = r.WithMagnitude(Expr::From(1.0));
732
    } else if(type == Type::FACE_N_ROT_TRANS) {
733
        // The numerical normal vector gets the rotation; the numerical
734
        // normal has magnitude one, and the rotation doesn't change that,
735
        // so there's no need to fix it up.
736
        r = ExprVector::From(numNormal.vx, numNormal.vy, numNormal.vz);
737
        ExprQuaternion q =
738
            ExprQuaternion::From(param[3], param[4], param[5], param[6]);
739
        r = q.Rotate(r);
740
    } else if(type == Type::FACE_N_TRANS) {
741
        r = ExprVector::From(numNormal.vx, numNormal.vy, numNormal.vz);
742
    } else if((type == Type::FACE_N_ROT_AA) || (type == Type::FACE_ROT_NORMAL_PT)) {
743
        r = ExprVector::From(numNormal.vx, numNormal.vy, numNormal.vz);
744
        ExprQuaternion q = GetAxisAngleQuaternionExprs(3);
745
        r = q.Rotate(r);
746
    } else ssassert(false, "Unexpected entity type");
747
    return r;
748
}
749

750
Vector EntityBase::FaceGetNormalNum() const {
751
    Vector r;
752
    if(type == Type::FACE_NORMAL_PT) {
753
        r = Vector::From(numNormal.vx, numNormal.vy, numNormal.vz);
754
    } else if(type == Type::FACE_XPROD) {
755
        Vector vc = Vector::From(param[0], param[1], param[2]);
756
        Vector vn = Vector::From(numNormal.vx, numNormal.vy, numNormal.vz);
757
        r = vc.Cross(vn);
758
    } else if(type == Type::FACE_N_ROT_TRANS) {
759
        // The numerical normal vector gets the rotation
760
        r = Vector::From(numNormal.vx, numNormal.vy, numNormal.vz);
761
        Quaternion q = Quaternion::From(param[3], param[4], param[5], param[6]);
762
        r = q.Rotate(r);
763
    } else if(type == Type::FACE_N_TRANS) {
764
        r = Vector::From(numNormal.vx, numNormal.vy, numNormal.vz);
765
    } else if((type == Type::FACE_N_ROT_AA) || (type == Type::FACE_ROT_NORMAL_PT)) {
766
        r = Vector::From(numNormal.vx, numNormal.vy, numNormal.vz);
767
        Quaternion q = GetAxisAngleQuaternion(3);
768
        r = q.Rotate(r);
769
    } else ssassert(false, "Unexpected entity type");
770
    return r.WithMagnitude(1);
771
}
772

773
ExprVector EntityBase::FaceGetPointExprs() const {
774
    ExprVector r;
775
    if((type == Type::FACE_NORMAL_PT) || (type==Type::FACE_ROT_NORMAL_PT)) {
776
        r = SK.GetEntity(point[0])->PointGetExprs();
777
    } else if(type == Type::FACE_XPROD) {
778
        r = ExprVector::From(numPoint);
779
    } else if(type == Type::FACE_N_ROT_TRANS) {
780
        // The numerical point gets the rotation and translation.
781
        ExprVector trans = ExprVector::From(param[0], param[1], param[2]);
782
        ExprQuaternion q =
783
            ExprQuaternion::From(param[3], param[4], param[5], param[6]);
784
        r = ExprVector::From(numPoint);
785
        r = q.Rotate(r);
786
        r = r.Plus(trans);
787
    } else if(type == Type::FACE_N_ROT_AXIS_TRANS) {
788
            ExprVector orig = ExprVector::From(numPoint);
789
            ExprVector trans = ExprVector::From(param[0], param[1], param[2]);
790
            ExprVector displace = ExprVector::From(param[4], param[5], param[6])
791
               .WithMagnitude(Expr::From(param[7])).ScaledBy(Expr::From(timesApplied));
792
            ExprQuaternion q = GetAxisAngleQuaternionExprs(3);
793
            orig = orig.Minus(trans);
794
            orig = q.Rotate(orig);
795
            r = orig.Plus(trans).Plus(displace);
796
    } else if(type == Type::FACE_N_TRANS) {
797
        ExprVector trans = ExprVector::From(param[0], param[1], param[2]);
798
        r = ExprVector::From(numPoint);
799
        r = r.Plus(trans.ScaledBy(Expr::From(timesApplied)));
800
    } else if(type == Type::FACE_N_ROT_AA) {
801
        ExprVector trans = ExprVector::From(param[0], param[1], param[2]);
802
        ExprQuaternion q = GetAxisAngleQuaternionExprs(3);
803
        r = ExprVector::From(numPoint);
804
        r = r.Minus(trans);
805
        r = q.Rotate(r);
806
        r = r.Plus(trans);
807
    } else ssassert(false, "Unexpected entity type");
808
    return r;
809
}
810

811
Vector EntityBase::FaceGetPointNum() const {
812
    Vector r;
813
    if((type == Type::FACE_NORMAL_PT) || (type==Type::FACE_ROT_NORMAL_PT)) {
814
        r = SK.GetEntity(point[0])->PointGetNum();
815
    } else if(type == Type::FACE_XPROD) {
816
        r = numPoint;
817
    } else if(type == Type::FACE_N_ROT_TRANS) {
818
        // The numerical point gets the rotation and translation.
819
        Vector trans = Vector::From(param[0], param[1], param[2]);
820
        Quaternion q = Quaternion::From(param[3], param[4], param[5], param[6]);
821
        r = q.Rotate(numPoint);
822
        r = r.Plus(trans);
823
    } else if(type == Type::FACE_N_ROT_AXIS_TRANS) {
824
            Vector offset = Vector::From(param[0], param[1], param[2]);
825
            Vector displace = Vector::From(param[4], param[5], param[6])
826
               .WithMagnitude(SK.GetParam(param[7])->val).ScaledBy(timesApplied);
827
            Quaternion q = PointGetQuaternion();
828
            r = numPoint.Minus(offset);
829
            r = q.Rotate(r);
830
            r = r.Plus(offset).Plus(displace);
831
    } else if(type == Type::FACE_N_TRANS) {
832
        Vector trans = Vector::From(param[0], param[1], param[2]);
833
        r = numPoint.Plus(trans.ScaledBy(timesApplied));
834
    } else if(type == Type::FACE_N_ROT_AA) {
835
        Vector trans = Vector::From(param[0], param[1], param[2]);
836
        Quaternion q = GetAxisAngleQuaternion(3);
837
        r = numPoint.Minus(trans);
838
        r = q.Rotate(r);
839
        r = r.Plus(trans);
840
    } else ssassert(false, "Unexpected entity type");
841
    return r;
842
}
843

844
bool EntityBase::HasEndpoints() const {
845
    return (type == Type::LINE_SEGMENT) ||
846
           (type == Type::CUBIC) ||
847
           (type == Type::ARC_OF_CIRCLE);
848
}
849
Vector EntityBase::EndpointStart() const {
850
    if(type == Type::LINE_SEGMENT) {
851
        return SK.GetEntity(point[0])->PointGetNum();
852
    } else if(type == Type::CUBIC) {
853
        return CubicGetStartNum();
854
    } else if(type == Type::ARC_OF_CIRCLE) {
855
        return SK.GetEntity(point[1])->PointGetNum();
856
    } else ssassert(false, "Unexpected entity type");
857
}
858
Vector EntityBase::EndpointFinish() const {
859
    if(type == Type::LINE_SEGMENT) {
860
        return SK.GetEntity(point[1])->PointGetNum();
861
    } else if(type == Type::CUBIC) {
862
        return CubicGetFinishNum();
863
    } else if(type == Type::ARC_OF_CIRCLE) {
864
        return SK.GetEntity(point[2])->PointGetNum();
865
    } else ssassert(false, "Unexpected entity type");
866
}
867
static bool PointInPlane(hEntity h, Vector norm, double distance) {
868
    Vector p = SK.GetEntity(h)->PointGetNum();
869
    return (fabs(norm.Dot(p) - distance) < LENGTH_EPS);
870
}
871
bool EntityBase::IsInPlane(Vector norm, double distance) const {
872
    switch(type) {
873
        case Type::LINE_SEGMENT: {
874
            return PointInPlane(point[0], norm, distance)
875
                && PointInPlane(point[1], norm, distance);
876
        }
877
        case Type::CUBIC:
878
        case Type::CUBIC_PERIODIC: {
879
            bool periodic = type == Type::CUBIC_PERIODIC;
880
            int n = periodic ? 3 + extraPoints : extraPoints;
881
            int i;
882
            for (i=0; i<n; i++) {
883
                if (!PointInPlane(point[i], norm, distance)) return false;
884
            }
885
            return true;
886
        }
887

888
        case Type::CIRCLE:
889
        case Type::ARC_OF_CIRCLE: {
890
            // If it is an (arc of) a circle, check whether the normals
891
            // are parallel and the mid point is in the plane.
892
            Vector n = Normal()->NormalN();
893
            if (!norm.Equals(n) && !norm.Equals(n.Negated())) return false;
894
            return PointInPlane(point[0], norm, distance);
895
        }
896

897
        case Type::TTF_TEXT: {
898
            Vector n = Normal()->NormalN();
899
            if (!norm.Equals(n) && !norm.Equals(n.Negated())) return false;
900
            return PointInPlane(point[0], norm, distance)
901
                && PointInPlane(point[1], norm, distance);
902
        }
903

904
        default:
905
            return false;
906
    }
907
}
908

909
void EntityBase::RectGetPointsExprs(ExprVector *eb, ExprVector *ec) const {
910
    ssassert(type == Type::TTF_TEXT || type == Type::IMAGE,
911
             "Unexpected entity type");
912

913
    EntityBase *a = SK.GetEntity(point[0]);
914
    EntityBase *o = SK.GetEntity(point[1]);
915

916
    // Write equations for each point in the current workplane.
917
    // This reduces the complexity of resulting equations.
918
    ExprVector ea = a->PointGetExprsInWorkplane(workplane);
919
    ExprVector eo = o->PointGetExprsInWorkplane(workplane);
920

921
    // Take perpendicular vector and scale it by aspect ratio.
922
    ExprVector eu = ea.Minus(eo);
923
    ExprVector ev = ExprVector::From(eu.y, eu.x->Negate(), eu.z).ScaledBy(Expr::From(aspectRatio));
924

925
    *eb = eo.Plus(ev);
926
    *ec = eo.Plus(eu).Plus(ev);
927
}
928

929
void EntityBase::AddEq(IdList<Equation,hEquation> *l, Expr *expr, int index) const {
930
    Equation eq;
931
    eq.e = expr;
932
    eq.h = h.equation(index);
933
    l->Add(&eq);
934
}
935

936
void EntityBase::GenerateEquations(IdList<Equation,hEquation> *l) const {
937
    switch(type) {
938
        case Type::NORMAL_IN_3D: {
939
            ExprQuaternion q = NormalGetExprs();
940
            AddEq(l, (q.Magnitude())->Minus(Expr::From(1)), 0);
941
            break;
942
        }
943

944
        case Type::ARC_OF_CIRCLE: {
945
            // If this is a copied entity, with its point already fixed
946
            // with respect to each other, then we don't want to generate
947
            // the distance constraint!
948
            if(SK.GetEntity(point[0])->type != Type::POINT_IN_2D) break;
949

950
            // If the two endpoints of the arc are constrained coincident
951
            // (to make a complete circle), then our distance constraint
952
            // would be redundant and therefore overconstrain things.
953
            auto it = std::find_if(SK.constraint.begin(), SK.constraint.end(),
954
                                   [&](ConstraintBase const &con) {
955
                                       return (con.group == group) &&
956
                                              (con.type == Constraint::Type::POINTS_COINCIDENT) &&
957
                                              ((con.ptA == point[1] && con.ptB == point[2]) ||
958
                                               (con.ptA == point[2] && con.ptB == point[1]));
959
                                   });
960
            if(it != SK.constraint.end()) {
961
                break;
962
            }
963

964
            Expr *ra = Constraint::Distance(workplane, point[0], point[1]);
965
            Expr *rb = Constraint::Distance(workplane, point[0], point[2]);
966
            AddEq(l, ra->Minus(rb), 0);
967
            break;
968
        }
969

970
        case Type::IMAGE:
971
        case Type::TTF_TEXT: {
972
            if(SK.GetEntity(point[0])->type != Type::POINT_IN_2D) break;
973
            EntityBase *b = SK.GetEntity(point[2]);
974
            EntityBase *c = SK.GetEntity(point[3]);
975
            ExprVector eb = b->PointGetExprsInWorkplane(workplane);
976
            ExprVector ec = c->PointGetExprsInWorkplane(workplane);
977

978
            ExprVector ebp, ecp;
979
            RectGetPointsExprs(&ebp, &ecp);
980

981
            ExprVector beq = eb.Minus(ebp);
982
            AddEq(l, beq.x, 0);
983
            AddEq(l, beq.y, 1);
984
            ExprVector ceq = ec.Minus(ecp);
985
            AddEq(l, ceq.x, 2);
986
            AddEq(l, ceq.y, 3);
987
            break;
988
        }
989

990
        default: // Most entities do not generate equations.
991
            break;
992
    }
993
}
994