Solvespace

1
//-----------------------------------------------------------------------------
2
// Utility functions, mostly various kinds of vector math (working on real
3
// numbers, not working on quantities in the symbolic algebra system).
4
//
5
// Copyright 2008-2013 Jonathan Westhues.
6
//-----------------------------------------------------------------------------
7
#include "solvespace.h"
8

9
void SolveSpace::AssertFailure(const char *file, unsigned line, const char *function,
10
                               const char *condition, const char *message) {
11
    std::string formattedMsg;
12
    formattedMsg += ssprintf("File %s, line %u, function %s:\n", file, line, function);
13
    formattedMsg += ssprintf("Assertion failed: %s.\n", condition);
14
    formattedMsg += ssprintf("Message: %s.\n", message);
15
    SolveSpace::Platform::FatalError(formattedMsg);
16
}
17

18
std::string SolveSpace::ssprintf(const char *fmt, ...)
19
{
20
    va_list va;
21

22
    va_start(va, fmt);
23
    int size = vsnprintf(NULL, 0, fmt, va);
24
    ssassert(size >= 0, "vsnprintf could not encode string");
25
    va_end(va);
26

27
    std::string result;
28
    result.resize(size + 1);
29

30
    va_start(va, fmt);
31
    vsnprintf(&result[0], size + 1, fmt, va);
32
    va_end(va);
33

34
    result.resize(size);
35
    return result;
36
}
37

38
char32_t utf8_iterator::operator*()
39
{
40
    const uint8_t *it = (const uint8_t*) this->p;
41
    char32_t result = *it;
42

43
    if((result & 0x80) != 0) {
44
      unsigned int mask = 0x40;
45

46
      do {
47
        result <<= 6;
48
        unsigned int c = (*++it);
49
        mask   <<= 5;
50
        result  += c - 0x80;
51
      } while((result & mask) != 0);
52

53
      result &= mask - 1;
54
    }
55

56
    this->n = (const char*) (it + 1);
57
    return result;
58
}
59

60
int64_t SolveSpace::GetMilliseconds()
61
{
62
    auto timestamp = std::chrono::steady_clock::now().time_since_epoch();
63
    return std::chrono::duration_cast<std::chrono::milliseconds>(timestamp).count();
64
}
65

66
void SolveSpace::MakeMatrix(double *mat,
67
                            double a11, double a12, double a13, double a14,
68
                            double a21, double a22, double a23, double a24,
69
                            double a31, double a32, double a33, double a34,
70
                            double a41, double a42, double a43, double a44)
71
{
72
    mat[ 0] = a11;
73
    mat[ 1] = a21;
74
    mat[ 2] = a31;
75
    mat[ 3] = a41;
76
    mat[ 4] = a12;
77
    mat[ 5] = a22;
78
    mat[ 6] = a32;
79
    mat[ 7] = a42;
80
    mat[ 8] = a13;
81
    mat[ 9] = a23;
82
    mat[10] = a33;
83
    mat[11] = a43;
84
    mat[12] = a14;
85
    mat[13] = a24;
86
    mat[14] = a34;
87
    mat[15] = a44;
88
}
89

90
void SolveSpace::MultMatrix(double *mata, double *matb, double *matr) {
91
    for(int i = 0; i < 4; i++) {
92
        for(int j = 0; j < 4; j++) {
93
            double s = 0.0;
94
            for(int k = 0; k < 4; k++) {
95
                s += mata[k * 4 + j] * matb[i * 4 + k];
96
            }
97
           matr[i * 4 + j] = s;
98
        }
99
    }
100
}
101

102
//-----------------------------------------------------------------------------
103
// Format the string for our message box appropriately, and then display
104
// that string.
105
//-----------------------------------------------------------------------------
106
static void MessageBox(const char *fmt, va_list va, bool error,
107
                       std::function<void()> onDismiss = std::function<void()>())
108
{
109
#ifndef LIBRARY
110
    va_list va_size;
111
    va_copy(va_size, va);
112
    int size = vsnprintf(NULL, 0, fmt, va_size);
113
    ssassert(size >= 0, "vsnprintf could not encode string");
114
    va_end(va_size);
115

116
    std::string text;
117
    text.resize(size);
118

119
    vsnprintf(&text[0], size + 1, fmt, va);
120

121
    // Split message text using a heuristic for better presentation.
122
    size_t separatorAt = 0;
123
    while(separatorAt != std::string::npos) {
124
        size_t dotAt = text.find('.', separatorAt + 1),
125
               colonAt = text.find(':', separatorAt + 1);
126
        separatorAt = min(dotAt, colonAt);
127
        if(separatorAt == std::string::npos ||
128
                (separatorAt + 1 < text.size() && isspace(text[separatorAt + 1]))) {
129
            break;
130
        }
131
    }
132
    std::string message = text;
133
    std::string description;
134
    if(separatorAt != std::string::npos) {
135
        message = text.substr(0, separatorAt + 1);
136
        if(separatorAt + 1 < text.size()) {
137
            description = text.substr(separatorAt + 1);
138
        }
139
    }
140

141
    if(description.length() > 0) {
142
        std::string::iterator it = description.begin();
143
        while(isspace(*it)) it++;
144
        description = description.substr(it - description.begin());
145
    }
146

147
    Platform::MessageDialogRef dialog = CreateMessageDialog(SS.GW.window);
148
    if (!dialog) {
149
        if (error) {
150
            fprintf(stderr, "Error: %s\n", message.c_str());
151
        } else {
152
            fprintf(stderr, "Message: %s\n", message.c_str());
153
        }
154
        if(onDismiss) {
155
            onDismiss();
156
        }
157
        return;
158
    }
159
    using Platform::MessageDialog;
160
    if(error) {
161
        dialog->SetType(MessageDialog::Type::ERROR);
162
    } else {
163
        dialog->SetType(MessageDialog::Type::INFORMATION);
164
    }
165
    dialog->SetTitle(error ? C_("title", "Error") : C_("title", "Message"));
166
    dialog->SetMessage(message);
167
    if(!description.empty()) {
168
        dialog->SetDescription(description);
169
    }
170
    dialog->AddButton(C_("button", "&OK"), MessageDialog::Response::OK,
171
                      /*isDefault=*/true);
172

173
    dialog->onResponse = [=](MessageDialog::Response _response) {
174
        if(onDismiss) {
175
            onDismiss();
176
        }
177
    };
178
    dialog->ShowModal();
179
#endif
180
}
181
void SolveSpace::Error(const char *fmt, ...)
182
{
183
    va_list f;
184
    va_start(f, fmt);
185
    MessageBox(fmt, f, /*error=*/true);
186
    va_end(f);
187
}
188
void SolveSpace::Message(const char *fmt, ...)
189
{
190
    va_list f;
191
    va_start(f, fmt);
192
    MessageBox(fmt, f, /*error=*/false);
193
    va_end(f);
194
}
195
void SolveSpace::MessageAndRun(std::function<void()> onDismiss, const char *fmt, ...)
196
{
197
    va_list f;
198
    va_start(f, fmt);
199
    MessageBox(fmt, f, /*error=*/false, onDismiss);
200
    va_end(f);
201
}
202

203
//-----------------------------------------------------------------------------
204
// Solve a mostly banded matrix. In a given row, there are LEFT_OF_DIAG
205
// elements to the left of the diagonal element, and RIGHT_OF_DIAG elements to
206
// the right (so that the total band width is LEFT_OF_DIAG + RIGHT_OF_DIAG + 1).
207
// There also may be elements in the last two columns of any row. We solve
208
// without pivoting.
209
//-----------------------------------------------------------------------------
210
void BandedMatrix::Solve() {
211
    int i, ip, j, jp;
212
    double temp;
213

214
    // Reduce the matrix to upper triangular form.
215
    for(i = 0; i < n; i++) {
216
        for(ip = i+1; ip < n && ip <= (i + LEFT_OF_DIAG); ip++) {
217
            temp = A[ip][i]/A[i][i];
218

219
            for(jp = i; jp < (n - 2) && jp <= (i + RIGHT_OF_DIAG); jp++) {
220
                A[ip][jp] -= temp*(A[i][jp]);
221
            }
222
            A[ip][n-2] -= temp*(A[i][n-2]);
223
            A[ip][n-1] -= temp*(A[i][n-1]);
224

225
            B[ip] -= temp*B[i];
226
        }
227
    }
228

229
    // And back-substitute.
230
    for(i = n - 1; i >= 0; i--) {
231
        temp = B[i];
232

233
        if(i < n-1) temp -= X[n-1]*A[i][n-1];
234
        if(i < n-2) temp -= X[n-2]*A[i][n-2];
235

236
        for(j = min(n - 3, i + RIGHT_OF_DIAG); j > i; j--) {
237
            temp -= X[j]*A[i][j];
238
        }
239
        X[i] = temp / A[i][i];
240
    }
241
}
242

243
const Quaternion Quaternion::IDENTITY = { 1, 0, 0, 0 };
244

245
Quaternion Quaternion::From(double w, double vx, double vy, double vz) {
246
    Quaternion q;
247
    q.w  = w;
248
    q.vx = vx;
249
    q.vy = vy;
250
    q.vz = vz;
251
    return q;
252
}
253

254
Quaternion Quaternion::From(hParam w, hParam vx, hParam vy, hParam vz) {
255
    Quaternion q;
256
    q.w  = SK.GetParam(w )->val;
257
    q.vx = SK.GetParam(vx)->val;
258
    q.vy = SK.GetParam(vy)->val;
259
    q.vz = SK.GetParam(vz)->val;
260
    return q;
261
}
262

263
Quaternion Quaternion::From(Vector axis, double dtheta) {
264
    Quaternion q;
265
    double c = cos(dtheta / 2), s = sin(dtheta / 2);
266
    axis = axis.WithMagnitude(s);
267
    q.w  = c;
268
    q.vx = axis.x;
269
    q.vy = axis.y;
270
    q.vz = axis.z;
271
    return q;
272
}
273

274
Quaternion Quaternion::From(Vector u, Vector v)
275
{
276
    Vector n = u.Cross(v);
277

278
    Quaternion q;
279
    double s, tr = 1 + u.x + v.y + n.z;
280
    if(tr > 1e-4) {
281
        s = 2*sqrt(tr);
282
        q.w  = s/4;
283
        q.vx = (v.z - n.y)/s;
284
        q.vy = (n.x - u.z)/s;
285
        q.vz = (u.y - v.x)/s;
286
    } else {
287
        if(u.x > v.y && u.x > n.z) {
288
            s = 2*sqrt(1 + u.x - v.y - n.z);
289
            q.w  = (v.z - n.y)/s;
290
            q.vx = s/4;
291
            q.vy = (u.y + v.x)/s;
292
            q.vz = (n.x + u.z)/s;
293
        } else if(v.y > n.z) {
294
            s = 2*sqrt(1 - u.x + v.y - n.z);
295
            q.w  = (n.x - u.z)/s;
296
            q.vx = (u.y + v.x)/s;
297
            q.vy = s/4;
298
            q.vz = (v.z + n.y)/s;
299
        } else {
300
            s = 2*sqrt(1 - u.x - v.y + n.z);
301
            q.w  = (u.y - v.x)/s;
302
            q.vx = (n.x + u.z)/s;
303
            q.vy = (v.z + n.y)/s;
304
            q.vz = s/4;
305
        }
306
    }
307

308
    return q.WithMagnitude(1);
309
}
310

311
Quaternion Quaternion::Plus(Quaternion b) const {
312
    Quaternion q;
313
    q.w  = w  + b.w;
314
    q.vx = vx + b.vx;
315
    q.vy = vy + b.vy;
316
    q.vz = vz + b.vz;
317
    return q;
318
}
319

320
Quaternion Quaternion::Minus(Quaternion b) const {
321
    Quaternion q;
322
    q.w  = w  - b.w;
323
    q.vx = vx - b.vx;
324
    q.vy = vy - b.vy;
325
    q.vz = vz - b.vz;
326
    return q;
327
}
328

329
Quaternion Quaternion::ScaledBy(double s) const {
330
    Quaternion q;
331
    q.w  = w*s;
332
    q.vx = vx*s;
333
    q.vy = vy*s;
334
    q.vz = vz*s;
335
    return q;
336
}
337

338
double Quaternion::Magnitude() const {
339
    return sqrt(w*w + vx*vx + vy*vy + vz*vz);
340
}
341

342
Quaternion Quaternion::WithMagnitude(double s) const {
343
    return ScaledBy(s/Magnitude());
344
}
345

346
Vector Quaternion::RotationU() const {
347
    Vector v;
348
    v.x = w*w + vx*vx - vy*vy - vz*vz;
349
    v.y = 2*w *vz + 2*vx*vy;
350
    v.z = 2*vx*vz - 2*w *vy;
351
    return v;
352
}
353

354
Vector Quaternion::RotationV() const {
355
    Vector v;
356
    v.x = 2*vx*vy - 2*w*vz;
357
    v.y = w*w - vx*vx + vy*vy - vz*vz;
358
    v.z = 2*w*vx + 2*vy*vz;
359
    return v;
360
}
361

362
Vector Quaternion::RotationN() const {
363
    Vector v;
364
    v.x = 2*w*vy + 2*vx*vz;
365
    v.y = 2*vy*vz - 2*w*vx;
366
    v.z = w*w - vx*vx - vy*vy + vz*vz;
367
    return v;
368
}
369

370
Vector Quaternion::Rotate(Vector p) const {
371
    // Express the point in the new basis
372
    return (RotationU().ScaledBy(p.x)).Plus(
373
            RotationV().ScaledBy(p.y)).Plus(
374
            RotationN().ScaledBy(p.z));
375
}
376

377
Quaternion Quaternion::Inverse() const {
378
    Quaternion r;
379
    r.w = w;
380
    r.vx = -vx;
381
    r.vy = -vy;
382
    r.vz = -vz;
383
    return r.WithMagnitude(1); // not that the normalize should be reqd
384
}
385

386
Quaternion Quaternion::ToThe(double p) const {
387
    // Avoid division by zero, or arccos of something not in its domain
388
    if(w >= (1 - 1e-6)) {
389
        return From(1, 0, 0, 0);
390
    } else if(w <= (-1 + 1e-6)) {
391
        return From(-1, 0, 0, 0);
392
    }
393

394
    Quaternion r;
395
    Vector axis = Vector::From(vx, vy, vz);
396
    double theta = acos(w); // okay, since magnitude is 1, so -1 <= w <= 1
397
    theta *= p;
398
    r.w = cos(theta);
399
    axis = axis.WithMagnitude(sin(theta));
400
    r.vx = axis.x;
401
    r.vy = axis.y;
402
    r.vz = axis.z;
403
    return r;
404
}
405

406
Quaternion Quaternion::Times(Quaternion b) const {
407
    double sa = w, sb = b.w;
408
    Vector va = { vx, vy, vz };
409
    Vector vb = { b.vx, b.vy, b.vz };
410

411
    Quaternion r;
412
    r.w = sa*sb - va.Dot(vb);
413
    Vector vr = vb.ScaledBy(sa).Plus(
414
                va.ScaledBy(sb).Plus(
415
                va.Cross(vb)));
416
    r.vx = vr.x;
417
    r.vy = vr.y;
418
    r.vz = vr.z;
419
    return r;
420
}
421

422
Quaternion Quaternion::Mirror() const {
423
    Vector u = RotationU(),
424
           v = RotationV();
425
    u = u.ScaledBy(-1);
426
    v = v.ScaledBy(-1);
427
    return Quaternion::From(u, v);
428
}
429

430

431
Vector Vector::From(hParam x, hParam y, hParam z) {
432
    Vector v;
433
    v.x = SK.GetParam(x)->val;
434
    v.y = SK.GetParam(y)->val;
435
    v.z = SK.GetParam(z)->val;
436
    return v;
437
}
438

439
bool Vector::EqualsExactly(Vector v) const {
440
    return EXACT(x == v.x &&
441
                 y == v.y &&
442
                 z == v.z);
443
}
444

445
double Vector::DirectionCosineWith(Vector b) const {
446
    Vector a = this->WithMagnitude(1);
447
    b = b.WithMagnitude(1);
448
    return a.Dot(b);
449
}
450

451
Vector Vector::Normal(int which) const {
452
    Vector n;
453

454
    // Arbitrarily choose one vector that's normal to us, pivoting
455
    // appropriately.
456
    double xa = fabs(x), ya = fabs(y), za = fabs(z);
457
    if(this->Equals(Vector::From(0, 0, 1))) {
458
        // Make DXFs exported in the XY plane work nicely...
459
        n = Vector::From(1, 0, 0);
460
    } else if(xa < ya && xa < za) {
461
        n.x = 0;
462
        n.y = z;
463
        n.z = -y;
464
    } else if(ya < za) {
465
        n.x = -z;
466
        n.y = 0;
467
        n.z = x;
468
    } else {
469
        n.x = y;
470
        n.y = -x;
471
        n.z = 0;
472
    }
473

474
    if(which == 0) {
475
        // That's the vector we return.
476
    } else if(which == 1) {
477
        n = this->Cross(n);
478
    } else ssassert(false, "Unexpected vector normal index");
479

480
    n = n.WithMagnitude(1);
481

482
    return n;
483
}
484

485
Vector Vector::RotatedAbout(Vector orig, Vector axis, double theta) const {
486
    Vector r = this->Minus(orig);
487
    r = r.RotatedAbout(axis, theta);
488
    return r.Plus(orig);
489
}
490

491
Vector Vector::RotatedAbout(Vector axis, double theta) const {
492
    double c = cos(theta);
493
    double s = sin(theta);
494

495
    axis = axis.WithMagnitude(1);
496

497
    Vector r;
498

499
    r.x =   (x)*(c + (1 - c)*(axis.x)*(axis.x)) +
500
            (y)*((1 - c)*(axis.x)*(axis.y) - s*(axis.z)) +
501
            (z)*((1 - c)*(axis.x)*(axis.z) + s*(axis.y));
502

503
    r.y =   (x)*((1 - c)*(axis.y)*(axis.x) + s*(axis.z)) +
504
            (y)*(c + (1 - c)*(axis.y)*(axis.y)) +
505
            (z)*((1 - c)*(axis.y)*(axis.z) - s*(axis.x));
506

507
    r.z =   (x)*((1 - c)*(axis.z)*(axis.x) - s*(axis.y)) +
508
            (y)*((1 - c)*(axis.z)*(axis.y) + s*(axis.x)) +
509
            (z)*(c + (1 - c)*(axis.z)*(axis.z));
510

511
    return r;
512
}
513

514
Vector Vector::DotInToCsys(Vector u, Vector v, Vector n) const {
515
    Vector r = {
516
        this->Dot(u),
517
        this->Dot(v),
518
        this->Dot(n)
519
    };
520
    return r;
521
}
522

523
Vector Vector::ScaleOutOfCsys(Vector u, Vector v, Vector n) const {
524
    Vector r = u.ScaledBy(x).Plus(
525
               v.ScaledBy(y).Plus(
526
               n.ScaledBy(z)));
527
    return r;
528
}
529

530
Vector Vector::InPerspective(Vector u, Vector v, Vector n,
531
                             Vector origin, double cameraTan) const
532
{
533
    Vector r = this->Minus(origin);
534
    r = r.DotInToCsys(u, v, n);
535
    // yes, minus; we are assuming a csys where u cross v equals n, backwards
536
    // from the display stuff
537
    double w = (1 - r.z*cameraTan);
538
    r = r.ScaledBy(1/w);
539

540
    return r;
541
}
542

543
double Vector::DistanceToLine(Vector p0, Vector dp) const {
544
    double m = dp.Magnitude();
545
    return ((this->Minus(p0)).Cross(dp)).Magnitude() / m;
546
}
547

548
double Vector::DistanceToPlane(Vector normal, Vector origin) const {
549
    return this->Dot(normal) - origin.Dot(normal);
550
}
551

552
bool Vector::OnLineSegment(Vector a, Vector b, double tol) const {
553
    if(this->Equals(a, tol) || this->Equals(b, tol)) return true;
554

555
    Vector d = b.Minus(a);
556

557
    double m = d.MagSquared();
558
    double distsq = ((this->Minus(a)).Cross(d)).MagSquared() / m;
559

560
    if(distsq >= tol*tol) return false;
561

562
    double t = (this->Minus(a)).DivProjected(d);
563
    // On-endpoint already tested
564
    if(t < 0 || t > 1) return false;
565
    return true;
566
}
567

568
Vector Vector::ClosestPointOnLine(Vector p0, Vector dp) const {
569
    dp = dp.WithMagnitude(1);
570
    // this, p0, and (p0+dp) define a plane; the min distance is in
571
    // that plane, so calculate its normal
572
    Vector pn = (this->Minus(p0)).Cross(dp);
573
    // The minimum distance line is in that plane, perpendicular
574
    // to the line
575
    Vector n = pn.Cross(dp);
576

577
    // Calculate the actual distance
578
    double d = (dp.Cross(p0.Minus(*this))).Magnitude();
579
    return this->Plus(n.WithMagnitude(d));
580
}
581

582
Vector Vector::WithMagnitude(double v) const {
583
    double m = Magnitude();
584
    if(EXACT(m == 0)) {
585
        // We can do a zero vector with zero magnitude, but not any other cases.
586
        if(fabs(v) > 1e-100) {
587
            dbp("Vector::WithMagnitude(%g) of zero vector!", v);
588
        }
589
        return From(0, 0, 0);
590
    } else {
591
        return ScaledBy(v/m);
592
    }
593
}
594

595
Vector Vector::ProjectVectorInto(hEntity wrkpl) const {
596
    EntityBase *w = SK.GetEntity(wrkpl);
597
    Vector u = w->Normal()->NormalU();
598
    Vector v = w->Normal()->NormalV();
599

600
    double up = this->Dot(u);
601
    double vp = this->Dot(v);
602

603
    return (u.ScaledBy(up)).Plus(v.ScaledBy(vp));
604
}
605

606
Vector Vector::ProjectInto(hEntity wrkpl) const {
607
    EntityBase *w = SK.GetEntity(wrkpl);
608
    Vector p0 = w->WorkplaneGetOffset();
609

610
    Vector f = this->Minus(p0);
611

612
    return p0.Plus(f.ProjectVectorInto(wrkpl));
613
}
614

615
Point2d Vector::Project2d(Vector u, Vector v) const {
616
    Point2d p;
617
    p.x = this->Dot(u);
618
    p.y = this->Dot(v);
619
    return p;
620
}
621

622
Point2d Vector::ProjectXy() const {
623
    Point2d p;
624
    p.x = x;
625
    p.y = y;
626
    return p;
627
}
628

629
Vector4 Vector::Project4d() const {
630
    return Vector4::From(1, x, y, z);
631
}
632

633
double Vector::DivProjected(Vector delta) const {
634
    return (x*delta.x + y*delta.y + z*delta.z)
635
         / (delta.x*delta.x + delta.y*delta.y + delta.z*delta.z);
636
}
637

638
Vector Vector::ClosestOrtho() const {
639
    double mx = fabs(x), my = fabs(y), mz = fabs(z);
640

641
    if(mx > my && mx > mz) {
642
        return From((x > 0) ? 1 : -1, 0, 0);
643
    } else if(my > mz) {
644
        return From(0, (y > 0) ? 1 : -1, 0);
645
    } else {
646
        return From(0, 0, (z > 0) ? 1 : -1);
647
    }
648
}
649

650
Vector Vector::ClampWithin(double minv, double maxv) const {
651
    Vector ret = *this;
652

653
    if(ret.x < minv) ret.x = minv;
654
    if(ret.y < minv) ret.y = minv;
655
    if(ret.z < minv) ret.z = minv;
656

657
    if(ret.x > maxv) ret.x = maxv;
658
    if(ret.y > maxv) ret.y = maxv;
659
    if(ret.z > maxv) ret.z = maxv;
660

661
    return ret;
662
}
663

664
bool Vector::OutsideAndNotOn(Vector maxv, Vector minv) const {
665
    return (x > maxv.x + LENGTH_EPS) || (x < minv.x - LENGTH_EPS) ||
666
           (y > maxv.y + LENGTH_EPS) || (y < minv.y - LENGTH_EPS) ||
667
           (z > maxv.z + LENGTH_EPS) || (z < minv.z - LENGTH_EPS);
668
}
669

670
bool Vector::BoundingBoxesDisjoint(Vector amax, Vector amin,
671
                                   Vector bmax, Vector bmin)
672
{
673
    int i;
674
    for(i = 0; i < 3; i++) {
675
        if(amax.Element(i) < bmin.Element(i) - LENGTH_EPS) return true;
676
        if(amin.Element(i) > bmax.Element(i) + LENGTH_EPS) return true;
677
    }
678
    return false;
679
}
680

681
bool Vector::BoundingBoxIntersectsLine(Vector amax, Vector amin,
682
                                       Vector p0, Vector p1, bool asSegment)
683
{
684
    Vector dp = p1.Minus(p0);
685
    double lp = dp.Magnitude();
686
    dp = dp.ScaledBy(1.0/lp);
687

688
    int i, a;
689
    for(i = 0; i < 3; i++) {
690
        int j = WRAP(i+1, 3), k = WRAP(i+2, 3);
691
        if(lp*fabs(dp.Element(i)) < LENGTH_EPS) continue; // parallel to plane
692

693
        for(a = 0; a < 2; a++) {
694
            double d = (a == 0) ? amax.Element(i) : amin.Element(i);
695
            // n dot (p0 + t*dp) = d
696
            // (n dot p0) + t * (n dot dp) = d
697
            double t = (d - p0.Element(i)) / dp.Element(i);
698
            Vector p = p0.Plus(dp.ScaledBy(t));
699

700
            if(asSegment && (t < -LENGTH_EPS || t > (lp+LENGTH_EPS))) continue;
701

702
            if(p.Element(j) > amax.Element(j) + LENGTH_EPS) continue;
703
            if(p.Element(k) > amax.Element(k) + LENGTH_EPS) continue;
704

705
            if(p.Element(j) < amin.Element(j) - LENGTH_EPS) continue;
706
            if(p.Element(k) < amin.Element(k) - LENGTH_EPS) continue;
707

708
            return true;
709
        }
710
    }
711

712
    return false;
713
}
714

715
Vector Vector::AtIntersectionOfPlanes(Vector n1, double d1,
716
                                      Vector n2, double d2)
717
{
718
    double det = (n1.Dot(n1))*(n2.Dot(n2)) -
719
                 (n1.Dot(n2))*(n1.Dot(n2));
720
    double c1 = (d1*n2.Dot(n2) - d2*n1.Dot(n2))/det;
721
    double c2 = (d2*n1.Dot(n1) - d1*n1.Dot(n2))/det;
722

723
    return (n1.ScaledBy(c1)).Plus(n2.ScaledBy(c2));
724
}
725

726
void Vector::ClosestPointBetweenLines(Vector a0, Vector da,
727
                                      Vector b0, Vector db,
728
                                      double *ta, double *tb)
729
{
730
    // Make a semi-orthogonal coordinate system from those directions;
731
    // note that dna and dnb need not be perpendicular.
732
    Vector dn = da.Cross(db); // normal to both
733
    Vector dna = dn.Cross(da); // normal to da
734
    Vector dnb = dn.Cross(db); // normal to db
735

736
    // At the intersection of the lines
737
    //    a0 + pa*da = b0 + pb*db (where pa, pb are scalar params)
738
    // So dot this equation against dna and dnb to get two equations
739
    // to solve for da and db
740
    *tb =  ((a0.Minus(b0)).Dot(dna))/(db.Dot(dna));
741
    *ta = -((a0.Minus(b0)).Dot(dnb))/(da.Dot(dnb));
742
}
743

744
Vector Vector::AtIntersectionOfLines(Vector a0, Vector a1,
745
                                     Vector b0, Vector b1,
746
                                     bool *skew,
747
                                     double *parama, double *paramb)
748
{
749
    Vector da = a1.Minus(a0), db = b1.Minus(b0);
750

751
    double pa, pb;
752
    Vector::ClosestPointBetweenLines(a0, da, b0, db, &pa, &pb);
753

754
    if(parama) *parama = pa;
755
    if(paramb) *paramb = pb;
756

757
    // And from either of those, we get the intersection point.
758
    Vector pi = a0.Plus(da.ScaledBy(pa));
759

760
    if(skew) {
761
        // Check if the intersection points on each line are actually
762
        // coincident...
763
        if(pi.Equals(b0.Plus(db.ScaledBy(pb)))) {
764
            *skew = false;
765
        } else {
766
            *skew = true;
767
        }
768
    }
769
    return pi;
770
}
771

772
Vector Vector::AtIntersectionOfPlaneAndLine(Vector n, double d,
773
                                            Vector p0, Vector p1,
774
                                            bool *parallel)
775
{
776
    Vector dp = p1.Minus(p0);
777

778
    if(fabs(n.Dot(dp)) < LENGTH_EPS) {
779
        if(parallel) *parallel = true;
780
        return Vector::From(0, 0, 0);
781
    }
782

783
    if(parallel) *parallel = false;
784

785
    // n dot (p0 + t*dp) = d
786
    // (n dot p0) + t * (n dot dp) = d
787
    double t = (d - n.Dot(p0)) / (n.Dot(dp));
788

789
    return p0.Plus(dp.ScaledBy(t));
790
}
791

792
static double det2(double a1, double b1,
793
                   double a2, double b2)
794
{
795
    return (a1*b2) - (b1*a2);
796
}
797
static double det3(double a1, double b1, double c1,
798
                   double a2, double b2, double c2,
799
                   double a3, double b3, double c3)
800
{
801
    return a1*det2(b2, c2, b3, c3) -
802
           b1*det2(a2, c2, a3, c3) +
803
           c1*det2(a2, b2, a3, b3);
804
}
805
Vector Vector::AtIntersectionOfPlanes(Vector na, double da,
806
                                      Vector nb, double db,
807
                                      Vector nc, double dc,
808
                                      bool *parallel)
809
{
810
    double det  = det3(na.x, na.y, na.z,
811
                       nb.x, nb.y, nb.z,
812
                       nc.x, nc.y, nc.z);
813
    if(fabs(det) < 1e-10) { // arbitrary tolerance, not so good
814
        *parallel = true;
815
        return Vector::From(0, 0, 0);
816
    }
817
    *parallel = false;
818

819
    double detx = det3(da,   na.y, na.z,
820
                       db,   nb.y, nb.z,
821
                       dc,   nc.y, nc.z);
822

823
    double dety = det3(na.x, da,   na.z,
824
                       nb.x, db,   nb.z,
825
                       nc.x, dc,   nc.z);
826

827
    double detz = det3(na.x, na.y, da,
828
                       nb.x, nb.y, db,
829
                       nc.x, nc.y, dc  );
830

831
    return Vector::From(detx/det, dety/det, detz/det);
832
}
833

834
size_t VectorHash::operator()(const Vector &v) const {
835
    const size_t size = (size_t)pow(std::numeric_limits<size_t>::max(), 1.0 / 3.0) - 1;
836
    const double eps = 4.0 * LENGTH_EPS;
837

838
    double x = fabs(v.x) / eps;
839
    double y = fabs(v.y) / eps;
840
    double z = fabs(v.y) / eps;
841

842
    size_t xs = size_t(fmod(x, (double)size));
843
    size_t ys = size_t(fmod(y, (double)size));
844
    size_t zs = size_t(fmod(z, (double)size));
845

846
    return (zs * size + ys) * size + xs;
847
}
848

849
bool VectorPred::operator()(Vector a, Vector b) const {
850
    return a.Equals(b, LENGTH_EPS);
851
}
852

853
Vector4 Vector4::From(double w, double x, double y, double z) {
854
    Vector4 ret;
855
    ret.w = w;
856
    ret.x = x;
857
    ret.y = y;
858
    ret.z = z;
859
    return ret;
860
}
861

862
Vector4 Vector4::From(double w, Vector v) {
863
    return Vector4::From(w, w*v.x, w*v.y, w*v.z);
864
}
865

866
Vector4 Vector4::Blend(Vector4 a, Vector4 b, double t) {
867
    return (a.ScaledBy(1 - t)).Plus(b.ScaledBy(t));
868
}
869

870
Vector4 Vector4::Plus(Vector4 b) const {
871
    return Vector4::From(w + b.w, x + b.x, y + b.y, z + b.z);
872
}
873

874
Vector4 Vector4::Minus(Vector4 b) const {
875
    return Vector4::From(w - b.w, x - b.x, y - b.y, z - b.z);
876
}
877

878
Vector4 Vector4::ScaledBy(double s) const {
879
    return Vector4::From(w*s, x*s, y*s, z*s);
880
}
881

882
Vector Vector4::PerspectiveProject() const {
883
    return Vector::From(x / w, y / w, z / w);
884
}
885

886
Point2d Point2d::From(double x, double y) {
887
    return { x, y };
888
}
889

890
Point2d Point2d::FromPolar(double r, double a) {
891
    return { r * cos(a), r * sin(a) };
892
}
893

894
double Point2d::Angle() const {
895
    double a = atan2(y, x);
896
    return M_PI + remainder(a - M_PI, 2 * M_PI);
897
}
898

899
double Point2d::AngleTo(const Point2d &p) const {
900
    return p.Minus(*this).Angle();
901
}
902

903
Point2d Point2d::Plus(const Point2d &b) const {
904
    return { x + b.x, y + b.y };
905
}
906

907
Point2d Point2d::Minus(const Point2d &b) const {
908
    return { x - b.x, y - b.y };
909
}
910

911
Point2d Point2d::ScaledBy(double s) const {
912
    return { x * s, y * s };
913
}
914

915
double Point2d::DivProjected(Point2d delta) const {
916
    return (x*delta.x + y*delta.y) / (delta.x*delta.x + delta.y*delta.y);
917
}
918

919
double Point2d::MagSquared() const {
920
    return x*x + y*y;
921
}
922

923
double Point2d::Magnitude() const {
924
    return sqrt(x*x + y*y);
925
}
926

927
Point2d Point2d::WithMagnitude(double v) const {
928
    double m = Magnitude();
929
    if(m < 1e-20) {
930
        dbp("!!! WithMagnitude() of zero vector");
931
        return { v, 0 };
932
    }
933
    return { x * v / m, y * v / m };
934
}
935

936
double Point2d::DistanceTo(const Point2d &p) const {
937
    double dx = x - p.x;
938
    double dy = y - p.y;
939
    return sqrt(dx*dx + dy*dy);
940
}
941

942
double Point2d::Dot(Point2d p) const {
943
    return x*p.x + y*p.y;
944
}
945

946
double Point2d::DistanceToLine(const Point2d &p0, const Point2d &dp, bool asSegment) const {
947
    double m = dp.x*dp.x + dp.y*dp.y;
948
    if(m < LENGTH_EPS*LENGTH_EPS) return VERY_POSITIVE;
949

950
    // Let our line be p = p0 + t*dp, for a scalar t from 0 to 1
951
    double t = (dp.x*(x - p0.x) + dp.y*(y - p0.y))/m;
952

953
    if(asSegment) {
954
        if(t < 0.0) return DistanceTo(p0);
955
        if(t > 1.0) return DistanceTo(p0.Plus(dp));
956
    }
957
    Point2d closest = p0.Plus(dp.ScaledBy(t));
958
    return DistanceTo(closest);
959
}
960

961
double Point2d::DistanceToLineSigned(const Point2d &p0, const Point2d &dp, bool asSegment) const {
962
    double m = dp.x*dp.x + dp.y*dp.y;
963
    if(m < LENGTH_EPS*LENGTH_EPS) return VERY_POSITIVE;
964

965
    Point2d n = dp.Normal().WithMagnitude(1.0);
966
    double dist = n.Dot(*this) - n.Dot(p0);
967
    if(asSegment) {
968
        // Let our line be p = p0 + t*dp, for a scalar t from 0 to 1
969
        double t = (dp.x*(x - p0.x) + dp.y*(y - p0.y))/m;
970
        double sign = (dist > 0.0) ? 1.0 : -1.0;
971
        if(t < 0.0) return DistanceTo(p0) * sign;
972
        if(t > 1.0) return DistanceTo(p0.Plus(dp)) * sign;
973
    }
974

975
    return dist;
976
}
977

978
Point2d Point2d::Normal() const {
979
    return { y, -x };
980
}
981

982
bool Point2d::Equals(Point2d v, double tol) const {
983
    double dx = v.x - x; if(dx < -tol || dx > tol) return false;
984
    double dy = v.y - y; if(dy < -tol || dy > tol) return false;
985

986
    return (this->Minus(v)).MagSquared() < tol*tol;
987
}
988

989
BBox BBox::From(const Vector &p0, const Vector &p1) {
990
    BBox bbox;
991
    bbox.minp.x = min(p0.x, p1.x);
992
    bbox.minp.y = min(p0.y, p1.y);
993
    bbox.minp.z = min(p0.z, p1.z);
994

995
    bbox.maxp.x = max(p0.x, p1.x);
996
    bbox.maxp.y = max(p0.y, p1.y);
997
    bbox.maxp.z = max(p0.z, p1.z);
998
    return bbox;
999
}
1000

1001
Vector BBox::GetOrigin() const { return minp.Plus(maxp.Minus(minp).ScaledBy(0.5)); }
1002
Vector BBox::GetExtents() const { return maxp.Minus(minp).ScaledBy(0.5); }
1003

1004
void BBox::Include(const Vector &v, double r) {
1005
    minp.x = min(minp.x, v.x - r);
1006
    minp.y = min(minp.y, v.y - r);
1007
    minp.z = min(minp.z, v.z - r);
1008

1009
    maxp.x = max(maxp.x, v.x + r);
1010
    maxp.y = max(maxp.y, v.y + r);
1011
    maxp.z = max(maxp.z, v.z + r);
1012
}
1013

1014
bool BBox::Overlaps(const BBox &b1) const {
1015
    Vector t = b1.GetOrigin().Minus(GetOrigin());
1016
    Vector e = b1.GetExtents().Plus(GetExtents());
1017

1018
    return fabs(t.x) < e.x && fabs(t.y) < e.y && fabs(t.z) < e.z;
1019
}
1020

1021
bool BBox::Contains(const Point2d &p, double r) const {
1022
    return p.x >= (minp.x - r) &&
1023
           p.y >= (minp.y - r) &&
1024
           p.x <= (maxp.x + r) &&
1025
           p.y <= (maxp.y + r);
1026
}
1027

1028
const std::vector<double>& SolveSpace::StipplePatternDashes(StipplePattern pattern) {
1029
    static bool initialized;
1030
    static std::vector<double> dashes[(size_t)StipplePattern::LAST + 1];
1031
    if(!initialized) {
1032
        // Inkscape ignores all elements that are exactly zero instead of drawing
1033
        // them as dots, so set those to 1e-6.
1034
        dashes[(size_t)StipplePattern::CONTINUOUS] =
1035
            {};
1036
        dashes[(size_t)StipplePattern::SHORT_DASH] =
1037
            { 1.0, 2.0 };
1038
        dashes[(size_t)StipplePattern::DASH] =
1039
            { 1.0, 1.0 };
1040
        dashes[(size_t)StipplePattern::DASH_DOT] =
1041
            { 1.0, 0.5, 1e-6, 0.5 };
1042
        dashes[(size_t)StipplePattern::DASH_DOT_DOT] =
1043
            { 1.0, 0.5, 1e-6, 0.5, 1e-6, 0.5 };
1044
        dashes[(size_t)StipplePattern::DOT] =
1045
            { 1e-6, 0.5 };
1046
        dashes[(size_t)StipplePattern::LONG_DASH] =
1047
            { 2.0, 0.5 };
1048
        dashes[(size_t)StipplePattern::FREEHAND] =
1049
            { 1.0, 2.0 };
1050
        dashes[(size_t)StipplePattern::ZIGZAG] =
1051
            { 1.0, 2.0 };
1052
    }
1053

1054
    return dashes[(size_t)pattern];
1055
}
1056

1057
double SolveSpace::StipplePatternLength(StipplePattern pattern) {
1058
    static bool initialized;
1059
    static double lengths[(size_t)StipplePattern::LAST + 1];
1060
    if(!initialized) {
1061
        for(size_t i = 0; i < (size_t)StipplePattern::LAST; i++) {
1062
            const std::vector<double> &dashes = StipplePatternDashes((StipplePattern)i);
1063
            double length = 0.0;
1064
            for(double dash : dashes) {
1065
                length += dash;
1066
            }
1067
            lengths[i] = length;
1068
        }
1069
    }
1070

1071
    return lengths[(size_t)pattern];
1072
}
1073