Project

General

Profile

Statistics
| Branch: | Revision:

root / src / layout / geometry.h @ 8aeaaccc

History | View | Annotate | Download (16.8 KB)

1 01873262 Georg Kunz
//=========================================================================
2
//  GEOMETRY.H - part of
3
//                  OMNeT++/OMNEST
4
//           Discrete System Simulation in C++
5
//
6
//  Author: Levente Meszaros
7
//
8
//=========================================================================
9
10
/*--------------------------------------------------------------*
11
  Copyright (C) 2006-2008 OpenSim Ltd.
12

13
  This file is distributed WITHOUT ANY WARRANTY. See the file
14
  `license' for details on this and other legal matters.
15
*--------------------------------------------------------------*/
16
17
#ifndef __GEOMETRY_H_
18
#define __GEOMETRY_H_
19
20
#include <math.h>
21
#include <vector>
22
#include "layoutdefs.h"
23
#include "commonutil.h"
24
#include "exception.h"
25
26
NAMESPACE_BEGIN
27
28
29
/**
30
 * A three dimensional point. Base plane means z = 0.
31
 */
32
class Pt
33
{
34
    public:
35
        double x;
36
37
        double y;
38
39
        double z;
40
41
    public:
42
        Pt() {
43
            assign(NaN, NaN, NaN);
44
        }
45
46
        Pt(double x, double y, double z) {
47
            assign(x, y, z);
48
        }
49
50
        Pt(const Pt& pt) {
51
            assign(pt);
52
        }
53
54
        Pt& assign(const Pt& pt) {
55
            return assign(pt.x, pt.y, pt.z);
56
        }
57
58
        Pt& assign(double x, double y, double z) {
59
            this->x = x;
60
            this->y = y;
61
            this->z = z;
62
63
            return *this;
64
        }
65
66
        Pt copy() const {
67
            return Pt(*this);
68
        }
69
70
        void setZero() {
71
            assign(0, 0, 0);
72
        }
73
74
        double getLength() const {
75
            return sqrt(x * x + y * y + z * z);
76
        }
77
78
        double getLengthSquare() const {
79
            return x * x + y * y + z * z;
80
        }
81
82
        double getBasePlaneProjectionLength() const {
83
            return sqrt(x * x + y * y);
84
        }
85
86
        double getBasePlaneProjectionLengthSquare() const {
87
            return x * x + y * y;
88
        }
89
90
        double getDistance(const Pt& other) const {
91
            return getDistance(x, y, z, other.x, other.y, other.z);
92
        }
93
94
        double getBasePlaneProjectionDistance(const Pt& other) const {
95
            return getDistance(x, y, 0, other.x, other.y, 0);
96
        }
97
98
        static double getDistance(double x1, double y1, double z1, double x2, double y2, double z2) {
99
            double dx = x1 - x2;
100
            double dy = y1 - y2;
101
            double dz = z1 - z2;
102
103
            return sqrt(dx * dx + dy * dy + dz * dz);
104
        }
105
106
        Pt getBasePlaneProjection() const {
107
            return Pt(x, y, 0);
108
        }
109
110
        double getBasePlaneProjectionAngle() const {
111
            return atan2(y, x);
112
        }
113
114
        void basePlaneRotate(double rotateAngle) {
115
            double angle = getBasePlaneProjectionAngle();
116
117
            if (!isNaN(angle)) {
118
                double length = getBasePlaneProjectionLength();
119
                angle += rotateAngle;
120
                x = cos(angle) * length;
121
                y = sin(angle) * length;
122
            }
123
        }
124
125
        Pt& basePlaneTranspose() {
126
            return assign(y, -x, z);
127
        }
128
129
        Pt& normalize() {
130
            double length = getLength();
131
            x /= length;
132
            y /= length;
133
            z /= length;
134
135
            return *this;
136
        }
137
138
        Pt& add(const Pt& pt) {
139
            return add(pt.x, pt.y, pt.z);
140
        }
141
142
        Pt& add(double x, double y, double z) {
143
            this->x += x;
144
            this->y += y;
145
            this->z += z;
146
147
            return *this;
148
        }
149
150
        Pt& subtract(const Pt& pt) {
151
            return subtract(pt.x, pt.y, pt.z);
152
        }
153
154
        Pt& subtract(double x, double y, double z) {
155
            this->x -= x;
156
            this->y -= y;
157
            this->z -= z;
158
159
            return *this;
160
        }
161
162
        Pt& multiply(double d) {
163
            x *= d;
164
            y *= d;
165
            z *= d;
166
167
            return *this;
168
        }
169
170
        Pt& multiply(const Pt& pt) {
171
            this->x *= pt.x;
172
            this->y *= pt.y;
173
            this->z *= pt.z;
174
175
            return *this;
176
        }
177
178
        Pt& reverse() {
179
            return multiply(-1);
180
        }
181
182
        Pt& divide(double d) {
183
            x /= d;
184
            y /= d;
185
            z /= d;
186
187
            return *this;
188
        }
189
190
        double dotProduct(Pt& other) {
191
            return x * other.x + y * other.y + z * other.z;
192
        }
193
194
        Pt& crossProduct(Pt& other) {
195
            assign(y * other.z - z * other.y, z * other.x - x * other.z, x * other.y - y * other.x);
196
197
            return *this;
198
        }
199
200
        void setNaNToZero() {
201
            if (isNaN(x))
202
                x = 0;
203
204
            if (isNaN(y))
205
                y = 0;
206
207
            if (isNaN(z))
208
                z = 0;
209
        }
210
211
        bool isZero() const {
212
            return x == 0 && y == 0 && z == 0;
213
        }
214
215
                bool isPositiveQuadrant() const {
216
                        return x >= 0 && y >= 0;
217
                }
218
219
        static Pt getBasePlaneRadial(double radius, double angle) {
220
            return Pt(cos(angle) * radius, sin(angle) * radius, 0);
221
        }
222
223
        static Pt getNil() {
224
            return Pt(NaN, NaN, NaN);
225
        }
226
227
        static Pt getZero() {
228
            return Pt(0, 0, 0);
229
        }
230
231
        bool isNil() const {
232
            return isNaN(x) && isNaN(y) && isNaN(z);
233
        }
234
235
        bool isFullySpecified() const {
236
            return !isNaN(x) && !isNaN(y) && !isNaN(z);
237
        }
238
239
        static double determinant(double a1, double a2, double b1, double b2) {
240
            return a1 * b2 - a2 * b1;
241
        }
242
243
        static double determinant(double a1, double a2, double a3, double b1, double b2, double b3, double c1, double c2, double c3) {
244
            return a1 * b2 * c3 - a1 * b3 * c2 - a2 * b1 * c3 + a2 * b3 * c1 + a3 * b1 * c2 - a3 * b2 * c1;
245
        }
246
};
247
248
/**
249
 * A three dimensional line or section defined by two points.
250
 */
251
class Ln {
252
    public:
253
        Pt begin;
254
255
        Pt end;
256
257
    public:
258
        Ln() {
259
            assign(Pt::getNil(), Pt::getNil());
260
        }
261
262
        Ln(double x1, double y1, double z1, double x2, double y2, double z2) {
263
            assign(Pt(x1, y1, z1), Pt(x2, y2, z2));
264
        }
265
266
        Ln(const Pt& begin, const Pt& end) {
267
            assign(begin, end);
268
        }
269
270
        Ln& assign(const Pt& begin, const Pt& end) {
271
            this->begin = begin;
272
            this->end = end;
273
274
            return *this;
275
        }
276
277
        Ln getBasePlaneProjection() const {
278
            return Ln(begin.getBasePlaneProjection(), end.getBasePlaneProjection());
279
        }
280
281
        Pt getClosestPoint(const Pt& pt) const {
282
            Pt r(pt);
283
            r.subtract(begin);
284
285
            Pt q = getDirectionVector();
286
            q.normalize();
287
            q.multiply(q.dotProduct(r));
288
            q.add(begin);
289
290
            return q;
291
        }
292
293
        Pt getDirectionVector() const {
294
            Pt v(end);
295
            v.subtract(begin);
296
297
            return v;
298
        }
299
300
        Pt basePlaneProjectionIntersect(const Ln& ln) const {
301
            double x1 = begin.x;
302
            double y1 = begin.y;
303
            double x2 = end.x;
304
            double y2 = end.y;
305
            double x3 = ln.begin.x;
306
            double y3 = ln.begin.y;
307
            double x4 = ln.end.x;
308
            double y4 = ln.end.y;
309
            double a = Pt::determinant(x1, y1, x2, y2);
310
            double b = Pt::determinant(x3, y3, x4, y4);
311
            double c = Pt::determinant(x1 - x2, y1 - y2, x3 - x4, y3 - y4);
312
            double x = Pt::determinant(a, x1 - x2, b, x3 - x4) / c;
313
            double y = Pt::determinant(a, y1 - y2, b, y3 - y4) / c;
314
315
            return Pt(x, y, 0);
316
        }
317
318
        static Ln getNil() {
319
            return Ln(Pt::getNil(), Pt::getNil());
320
        }
321
};
322
323
/**
324
 * A two dimensional rectangular size parallel to the base plane.
325
 */
326
class Rs {
327
    public:
328
        double width;
329
330
        double height;
331
332
    public:
333
        Rs() {
334
            assign(NaN, NaN);
335
        }
336
337
        Rs(double width, double height) {
338
            assign(width, height);
339
        }
340
341
        Rs& assign(double width, double height) {
342
            this->width = width;
343
            this->height = height;
344
345
            return *this;
346
        }
347
348
        static Rs getNil() {
349
            return Rs(NaN, NaN);
350
        }
351
352
        bool isNil() const {
353
            return isNaN(width) && isNaN(height);
354
        }
355
356
        bool isFullySpecified() const {
357
            return !isNaN(width) && !isNaN(height);
358
        }
359
360
        double getDiagonalLength() const {
361
            return sqrt(width * width + height * height);
362
        }
363
364
        double getArea() const {
365
            return width * height;
366
        }
367
};
368
369
/**
370
 * A two dimensional rectangle positioned in three dimensions and parallel to the base plane.
371
 */
372
class Rc {
373
    public:
374
        Pt pt;
375
376
        Rs rs;
377
378
    public:
379
        Rc() {
380
            assign(Pt::getNil(), Rs::getNil());
381
        }
382
383
        Rc(double x, double y, double z, double width, double height) {
384
            assign(Pt(x, y, z), Rs(width, height));
385
        }
386
387
        Rc(const Pt& pt, const Rs& rs) {
388
            assign(pt, rs);
389
        }
390
391
        Rc& assign(const Pt& pt, const Rs& rs) {
392
            this->pt = pt;
393
            this->rs = rs;
394
395
            return *this;
396
        }
397
398
        bool basePlaneProjectionIntersects(Rc rc2, bool strictly = false) const {
399
            return
400
                rc2.basePlaneProjectionContains(getLeftTop(), strictly) ||
401
                rc2.basePlaneProjectionContains(getRightTop(), strictly) ||
402
                rc2.basePlaneProjectionContains(getLeftBottom(), strictly) ||
403
                rc2.basePlaneProjectionContains(getRightBottom(), strictly);
404
        }
405
406
        bool basePlaneProjectionContains(const Pt& p, bool strictly = false) const {
407
            if (strictly)
408
                return pt.x < p.x && p.x < pt.x + rs.width && pt.y < p.y && p.y < pt.y + rs.height;
409
            else
410
                return pt.x <= p.x && p.x <= pt.x + rs.width && pt.y <= p.y && p.y <= pt.y + rs.height;
411
        }
412
413
        double getLeft() const {
414
            return pt.x;
415
        }
416
417
        double getRight() const {
418
            return pt.x + rs.width;
419
        }
420
421
        double getTop() const {
422
            return pt.y;
423
        }
424
425
        double getBottom() const {
426
            return pt.y + rs.height;
427
        }
428
429
        Pt getLeftTop() const {
430
            return Pt(pt.x, pt.y, pt.z);
431
        }
432
433
        Pt getCenterTop() const {
434
            return Pt(pt.x + rs.width / 2, pt.y, pt.z);
435
        }
436
437
        Pt getRightTop() const {
438
            return Pt(pt.x + rs.width, pt.y, pt.z);
439
        }
440
441
        Pt getLeftCenter() const {
442
            return Pt(pt.x, pt.y + rs.height / 2, pt.z);
443
        }
444
445
        Pt getCenterCenter() const {
446
            return Pt(pt.x + rs.width / 2, pt.y + rs.height / 2, pt.z);
447
        }
448
449
        Pt getRightCenter() const {
450
            return Pt(pt.x + rs.width, pt.y + rs.height / 2, pt.z);
451
        }
452
453
        Pt getLeftBottom() const {
454
            return Pt(pt.x, pt.y + rs.height, pt.z);
455
        }
456
457
        Pt getCenterBottom() const {
458
            return Pt(pt.x + rs.width / 2, pt.y + rs.height, pt.z);
459
        }
460
461
        Pt getRightBottom() const {
462
            return Pt(pt.x + rs.width, pt.y + rs.height, pt.z);
463
        }
464
465
        Ln getBasePlaneProjectionDistance(const Rc &other, double &distance) const {
466
            double x1 = pt.x;
467
            double y1 = pt.y;
468
            double z = pt.z;
469
            double x2 = pt.x + rs.width;
470
            double y2 = pt.y + rs.height;
471
            double x3 = other.pt.x;
472
            double y3 = other.pt.y;
473
            double zOther = other.pt.z;
474
            double x4 = other.pt.x + other.rs.width;
475
            double y4 = other.pt.y + other.rs.height;
476
477
            int bx;
478
            if (x2 <= x3)
479
                bx = 0;
480
            else if (x4 <= x1)
481
                bx = 2;
482
            else
483
                bx = 1;
484
485
            int by;
486
            if (y2 <= y3)
487
                by = 0;
488
            else if (y4 <= y1)
489
                by = 2;
490
            else
491
                by = 1;
492
493
            int b = by * 3 + bx;
494
            switch (b) {
495
                case 0:
496
                    distance = Pt::getDistance(x2, y2, 0, x3, y3, 0);
497
                    return Ln(x2, y2, z, x3, y3, zOther);
498
                case 1:
499
                    distance = y3 - y2;
500
                    return Ln(NaN, y2, z, NaN, y3, zOther);
501
                case 2:
502
                    distance = Pt::getDistance(x1, y2, 0, x4, y3, 0);
503
                    return Ln(x1, y2, z, x4, y3, zOther);
504
                case 3:
505
                    distance = x3 - x2;
506
                    return Ln(x2, NaN, z, x3, NaN, zOther);
507
                case 4:
508
                    distance = 0;
509
                    return Ln::getNil();
510
                case 5:
511
                    distance = x1 - x4;
512
                    return Ln(x1, NaN, z, x4, NaN, zOther);
513
                case 6:
514
                    distance = Pt::getDistance(x2, y1, 0, x3, y4, 0);
515
                    return Ln(x2, y1, z, x3, y4, zOther);
516
                case 7:
517
                    distance = y1 - y4;
518
                    return Ln(NaN, y1, z, NaN, y4, zOther);
519
                case 8:
520
                    distance = Pt::getDistance(x1, y1, 0, x4, y4, 0);
521
                    return Ln(x1, y1, z, x4, y4, zOther);
522
                default:
523
                    distance = NaN;
524
                    return Ln::getNil();
525
            }
526
        }
527
528
        static Rc getRcFromCenterSize(const Pt& center, const Rs& size) {
529
            return Rc(center.x - size.width / 2, center.y - size.height / 2, size.width, size.height, center.z);
530
        }
531
532
        static Rc getNil() {
533
            return Rc(Pt::getNil(), Rs::getNil());
534
        }
535
536
        bool isNil() const {
537
            return pt.isNil() && rs.isNil();
538
        }
539
};
540
541
/**
542
 * A two dimensional circle positioned in three dimensions and parallel to the base plane.
543
 */
544
class Cc {
545
    public:
546
        Pt origin;
547
548
        /**
549
         * Radius is always interpreted parallel to the base plane.
550
         */
551
        double radius;
552
553
    public:
554
        Cc() {
555
            assign(Pt::getNil(), NaN);
556
        }
557
558
        Cc(double x, double y, double z, double radius) {
559
            assign(Pt(x, y, z), radius);
560
        }
561
562
        Cc(const Pt& origin, double radius) {
563
            assign(origin, radius);
564
        }
565
566
        Cc& assign(const Pt& origin, double radius) {
567
            this->origin = origin;
568
            this->radius = radius;
569
570
            return *this;
571
        }
572
573
        Cc getBasePlaneProjection() const {
574
            return Cc(Pt(origin.x, origin.y, 0), radius);
575
        }
576
577
        bool basePlaneProjectionContains(Pt& pt, bool strictly = false) const {
578
            double distance = origin.getBasePlaneProjectionDistance(pt);
579
            return distance < radius || (!strictly && distance == radius);
580
        }
581
582
        std::vector<Pt> basePlaneProjectionIntersect(const Cc& other) const {
583
            double R2 = radius * radius;
584
            double r2 = other.radius * other.radius;
585
            double d = origin.getBasePlaneProjectionDistance(other.origin);
586
            double d2 = d * d;
587
            double a = (d2 - r2 + R2);
588
589
            if (d2 == 0)
590
                return std::vector<Pt>();
591
592
            double y2 = (4 * d2 * R2 - a * a) / (4 * d2);
593
594
            if (y2 < 0)
595
                return std::vector<Pt>();
596
597
            double y = sqrt(y2);
598
            double x = (d2 - r2 + R2) / (2 * d);
599
            Pt pt1 = Pt(x, y, 0);
600
            Pt pt2 = Pt(x, -y, 0);
601
            double angle = other.origin.copy().subtract(origin).getBasePlaneProjectionAngle();
602
            pt1.basePlaneRotate(angle);
603
            pt2.basePlaneRotate(angle);
604
            pt1.add(origin);
605
            pt2.add(origin);
606
607
            std::vector<Pt> pts;
608
            pts.push_back(pt1);
609
            pts.push_back(pt2);
610
611
            return pts;
612
        }
613
614
        static Cc getBasePlaneProjectionEnclosingCircle(const std::vector<Cc>& circles) {
615
            Cc cc = circles[0];
616
617
            for (std::vector<Cc>::const_iterator it = circles.begin(); it != circles.end(); it++)
618
                cc = cc.getBasePlaneProjectionEnclosingCircle(*it);
619
620
            return cc;
621
        }
622
623
        Cc getBasePlaneProjectionEnclosingCircle(const Cc& other) const {
624
            double distance = origin.getDistance(other.origin);
625
            double d = distance + std::max(radius, other.radius - distance) + std::max(other.radius, radius - distance);
626
            Pt pt(d / 2 - std::max(radius, other.radius - distance), 0, 0);
627
            double angle = other.origin.copy().subtract(origin).getBasePlaneProjectionAngle();
628
            pt.basePlaneRotate(angle);
629
            pt.add(origin);
630
631
            return Cc(pt, d / 2);
632
        }
633
634
        Pt getCenterTop() const {
635
            return Pt(origin.x, origin.y - radius, origin.z);
636
        }
637
638
        Pt getCenterBottom() const {
639
            return Pt(origin.x, origin.y + radius, origin.z);
640
        }
641
642
        Pt getLeftCenter() const {
643
            return Pt(origin.x - radius, origin.y, origin.z);
644
        }
645
646
        Pt getRightCenter() const {
647
            return Pt(origin.x + radius, origin.y, origin.z);
648
        }
649
};
650
651
NAMESPACE_END
652
653
654
#endif