1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.apache.commons.geometry.euclidean.twod;
18
19 import java.util.Arrays;
20 import java.util.Comparator;
21 import java.util.Iterator;
22 import java.util.function.UnaryOperator;
23
24 import org.apache.commons.geometry.core.internal.DoubleFunction2N;
25 import org.apache.commons.geometry.core.internal.SimpleTupleFormat;
26 import org.apache.commons.geometry.euclidean.EuclideanVectorSum;
27 import org.apache.commons.geometry.euclidean.MultiDimensionalEuclideanVector;
28 import org.apache.commons.geometry.euclidean.internal.Vectors;
29 import org.apache.commons.numbers.core.Precision;
30
31
32
33
34 public class Vector2D extends MultiDimensionalEuclideanVector<Vector2D> {
35
36
37 public static final Vector2D ZERO = new Vector2D(0, 0);
38
39
40 public static final Vector2D NaN = new Vector2D(Double.NaN, Double.NaN);
41
42
43 public static final Vector2D POSITIVE_INFINITY =
44 new Vector2D(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY);
45
46
47 public static final Vector2D NEGATIVE_INFINITY =
48 new Vector2D(Double.NEGATIVE_INFINITY, Double.NEGATIVE_INFINITY);
49
50
51
52
53
54 public static final Comparator<Vector2D> COORDINATE_ASCENDING_ORDER = (a, b) -> {
55 int cmp = 0;
56
57 if (a != null && b != null) {
58 cmp = Double.compare(a.getX(), b.getX());
59 if (cmp == 0) {
60 cmp = Double.compare(a.getY(), b.getY());
61 }
62 } else if (a != null) {
63 cmp = -1;
64 } else if (b != null) {
65 cmp = 1;
66 }
67
68 return cmp;
69 };
70
71
72 private final double x;
73
74
75 private final double y;
76
77
78
79
80
81 private Vector2D(final double x, final double y) {
82 this.x = x;
83 this.y = y;
84 }
85
86
87
88
89 public double getX() {
90 return x;
91 }
92
93
94
95
96 public double getY() {
97 return y;
98 }
99
100
101
102
103 public double[] toArray() {
104 return new double[]{x, y};
105 }
106
107
108 @Override
109 public int getDimension() {
110 return 2;
111 }
112
113
114 @Override
115 public boolean isNaN() {
116 return Double.isNaN(x) || Double.isNaN(y);
117 }
118
119
120 @Override
121 public boolean isInfinite() {
122 return !isNaN() && (Double.isInfinite(x) || Double.isInfinite(y));
123 }
124
125
126 @Override
127 public boolean isFinite() {
128 return Double.isFinite(x) && Double.isFinite(y);
129 }
130
131
132 @Override
133 public Vector2D vectorTo(final Vector2D v) {
134 return v.subtract(this);
135 }
136
137
138 @Override
139 public Unit directionTo(final Vector2D v) {
140 return vectorTo(v).normalize();
141 }
142
143
144 @Override
145 public Vector2D lerp(final Vector2D p, final double t) {
146 return Sum.create()
147 .addScaled(1.0 - t, this)
148 .addScaled(t, p).get();
149 }
150
151
152 @Override
153 public Vector2D getZero() {
154 return ZERO;
155 }
156
157
158 @Override
159 public double norm() {
160 return Vectors.norm(x, y);
161 }
162
163
164 @Override
165 public double normSq() {
166 return Vectors.normSq(x, y);
167 }
168
169
170 @Override
171 public Vector2D withNorm(final double magnitude) {
172 final double invNorm = 1.0 / getCheckedNorm();
173
174 return new Vector2D(
175 magnitude * x * invNorm,
176 magnitude * y * invNorm
177 );
178 }
179
180
181 @Override
182 public Vector2D add(final Vector2D v) {
183 return new Vector2D(x + v.x, y + v.y);
184 }
185
186
187 @Override
188 public Vector2D add(final double factor, final Vector2D v) {
189 return new Vector2D(x + (factor * v.x), y + (factor * v.y));
190 }
191
192
193 @Override
194 public Vector2D subtract(final Vector2D v) {
195 return new Vector2D(x - v.x, y - v.y);
196 }
197
198
199 @Override
200 public Vector2D subtract(final double factor, final Vector2D v) {
201 return new Vector2D(x - (factor * v.x), y - (factor * v.y));
202 }
203
204
205 @Override
206 public Vector2D negate() {
207 return new Vector2D(-x, -y);
208 }
209
210
211 @Override
212 public Unit normalize() {
213 return Unit.from(x, y);
214 }
215
216
217 @Override
218 public Unit normalizeOrNull() {
219 return Unit.tryCreateNormalized(x, y, false);
220 }
221
222
223 @Override
224 public Vector2D multiply(final double a) {
225 return new Vector2D(a * x, a * y);
226 }
227
228
229 @Override
230 public double distance(final Vector2D v) {
231 return Vectors.norm(x - v.x, y - v.y);
232 }
233
234
235 @Override
236 public double distanceSq(final Vector2D v) {
237 return Vectors.normSq(x - v.x, y - v.y);
238 }
239
240
241 @Override
242 public double dot(final Vector2D v) {
243 return Vectors.linearCombination(x, v.x, y, v.y);
244 }
245
246
247
248
249
250
251
252
253 @Override
254 public double angle(final Vector2D v) {
255 final double normProduct = getCheckedNorm() * v.getCheckedNorm();
256
257 final double dot = dot(v);
258 final double threshold = normProduct * 0.9999;
259 if ((dot < -threshold) || (dot > threshold)) {
260
261 final double n = Math.abs(Vectors.linearCombination(x, v.y, -y, v.x));
262 if (dot >= 0) {
263 return Math.asin(n / normProduct);
264 }
265 return Math.PI - Math.asin(n / normProduct);
266 }
267
268
269 return Math.acos(dot / normProduct);
270 }
271
272
273 @Override
274 public Vector2D project(final Vector2D base) {
275 return getComponent(base, false, Vector2D::new);
276 }
277
278
279 @Override
280 public Vector2D reject(final Vector2D base) {
281 return getComponent(base, true, Vector2D::new);
282 }
283
284
285
286
287
288
289
290
291
292 @Override
293 public Vector2D.Unit orthogonal() {
294 return Unit.from(-y, x);
295 }
296
297
298 @Override
299 public Vector2D.Unit orthogonal(final Vector2D dir) {
300 return dir.getComponent(this, true, Vector2D.Unit::from);
301 }
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317 public double signedArea(final Vector2D v) {
318 return Vectors.linearCombination(
319 x, v.y,
320 -y, v.x);
321 }
322
323
324
325
326
327
328 public Vector2D transform(final UnaryOperator<Vector2D> fn) {
329 return fn.apply(this);
330 }
331
332
333 @Override
334 public boolean eq(final Vector2D vec, final Precision.DoubleEquivalence precision) {
335 return precision.eq(x, vec.x) &&
336 precision.eq(y, vec.y);
337 }
338
339
340
341
342
343
344
345
346 @Override
347 public int hashCode() {
348 if (isNaN()) {
349 return 542;
350 }
351 return 122 * (76 * Double.hashCode(x) + Double.hashCode(y));
352 }
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373 @Override
374 public boolean equals(final Object other) {
375 if (this == other) {
376 return true;
377 }
378 if (other instanceof Vector2D) {
379 final Vector2D rhs = (Vector2D) other;
380 if (rhs.isNaN()) {
381 return this.isNaN();
382 }
383
384 return Double.compare(x, rhs.x) == 0 &&
385 Double.compare(y, rhs.y) == 0;
386 }
387 return false;
388 }
389
390
391 @Override
392 public String toString() {
393 return SimpleTupleFormat.getDefault().format(x, y);
394 }
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409 private <T extends Vector2D> T getComponent(final Vector2D base, final boolean reject,
410 final DoubleFunction2N<T> factory) {
411 final double aDotB = dot(base);
412
413
414
415
416
417
418
419 final double baseMagSq = Vectors.checkedNorm(base.normSq());
420
421 final double scale = aDotB / baseMagSq;
422
423 final double projX = scale * base.x;
424 final double projY = scale * base.y;
425
426 if (reject) {
427 return factory.apply(x - projX, y - projY);
428 }
429
430 return factory.apply(projX, projY);
431 }
432
433
434
435
436
437
438 public static Vector2D of(final double x, final double y) {
439 return new Vector2D(x, y);
440 }
441
442
443
444
445
446
447 public static Vector2D of(final double[] v) {
448 if (v.length != 2) {
449 throw new IllegalArgumentException("Dimension mismatch: " + v.length + " != 2");
450 }
451 return new Vector2D(v[0], v[1]);
452 }
453
454
455
456
457
458
459
460 public static Vector2D parse(final String str) {
461 return SimpleTupleFormat.getDefault().parse(str, Vector2D::new);
462 }
463
464
465
466
467
468
469 public static Vector2D max(final Vector2D first, final Vector2D... more) {
470 return computeMax(first, Arrays.asList(more).iterator());
471 }
472
473
474
475
476
477
478 public static Vector2D max(final Iterable<Vector2D> vecs) {
479 final Iterator<Vector2D> it = vecs.iterator();
480 if (!it.hasNext()) {
481 throw new IllegalArgumentException("Cannot compute vector max: no vectors given");
482 }
483
484 return computeMax(it.next(), it);
485 }
486
487
488
489
490
491
492 private static Vector2D computeMax(final Vector2D first, final Iterator<? extends Vector2D> more) {
493 double x = first.getX();
494 double y = first.getY();
495
496 Vector2D vec;
497 while (more.hasNext()) {
498 vec = more.next();
499
500 x = Math.max(x, vec.getX());
501 y = Math.max(y, vec.getY());
502 }
503
504 return Vector2D.of(x, y);
505 }
506
507
508
509
510
511
512 public static Vector2D min(final Vector2D first, final Vector2D... more) {
513 return computeMin(first, Arrays.asList(more).iterator());
514 }
515
516
517
518
519
520
521 public static Vector2D min(final Iterable<Vector2D> vecs) {
522 final Iterator<Vector2D> it = vecs.iterator();
523 if (!it.hasNext()) {
524 throw new IllegalArgumentException("Cannot compute vector min: no vectors given");
525 }
526
527 return computeMin(it.next(), it);
528 }
529
530
531
532
533
534
535 private static Vector2D computeMin(final Vector2D first, final Iterator<? extends Vector2D> more) {
536 double x = first.getX();
537 double y = first.getY();
538
539 Vector2D vec;
540 while (more.hasNext()) {
541 vec = more.next();
542
543 x = Math.min(x, vec.getX());
544 y = Math.min(y, vec.getY());
545 }
546
547 return Vector2D.of(x, y);
548 }
549
550
551
552
553
554
555
556 public static Vector2D centroid(final Vector2D first, final Vector2D... more) {
557 return computeCentroid(first, Arrays.asList(more).iterator());
558 }
559
560
561
562
563
564
565
566 public static Vector2D centroid(final Iterable<Vector2D> pts) {
567 final Iterator<Vector2D> it = pts.iterator();
568 if (!it.hasNext()) {
569 throw new IllegalArgumentException("Cannot compute centroid: no points given");
570 }
571
572 return computeCentroid(it.next(), it);
573 }
574
575
576
577
578
579
580 private static Vector2D computeCentroid(final Vector2D first, final Iterator<? extends Vector2D> more) {
581 final Sum sum = Sum.of(first);
582 int count = 1;
583
584 while (more.hasNext()) {
585 sum.add(more.next());
586 ++count;
587 }
588
589 return sum.get().multiply(1.0 / count);
590 }
591
592
593
594
595
596 public static final class Unit extends Vector2D {
597
598 public static final Unit PLUS_X = new Unit(1d, 0d);
599
600 public static final Unit MINUS_X = new Unit(-1d, 0d);
601
602 public static final Unit PLUS_Y = new Unit(0d, 1d);
603
604 public static final Unit MINUS_Y = new Unit(0d, -1d);
605
606
607
608
609 private static final double UNSCALED_MAX = 0x1.0p+500;
610
611
612
613
614 private static final double SCALE_UP_FACTOR = 0x1.0p+600;
615
616
617
618
619 private static final double SCALE_DOWN_FACTOR = 0x1.0p-600;
620
621
622
623
624
625
626 private Unit(final double x, final double y) {
627 super(x, y);
628 }
629
630
631 @Override
632 public double norm() {
633 return 1;
634 }
635
636
637 @Override
638 public double normSq() {
639 return 1;
640 }
641
642
643 @Override
644 public Unit normalize() {
645 return this;
646 }
647
648
649 @Override
650 public Unit normalizeOrNull() {
651 return this;
652 }
653
654
655 @Override
656 public Vector2D.Unit orthogonal() {
657 return new Unit(-getY(), getX());
658 }
659
660
661 @Override
662 public Vector2D withNorm(final double mag) {
663 return multiply(mag);
664 }
665
666
667 @Override
668 public Unit negate() {
669 return new Unit(-getX(), -getY());
670 }
671
672
673
674
675
676
677
678
679 public static Unit from(final double x, final double y) {
680 return tryCreateNormalized(x, y, true);
681 }
682
683
684
685
686
687
688
689 public static Unit from(final Vector2D v) {
690 return v instanceof Unit ?
691 (Unit) v :
692 from(v.getX(), v.getY());
693 }
694
695
696
697
698
699
700
701
702
703
704
705 private static Unit tryCreateNormalized(final double x, final double y, final boolean throwOnFailure) {
706
707
708
709
710 final double norm = Vectors.norm(x, y);
711 final double normInv = 1.0 / norm;
712 if (Vectors.isRealNonZero(normInv)) {
713 return new Unit(
714 x * normInv,
715 y * normInv);
716 }
717
718
719
720 final double scaledX;
721 final double scaledY;
722
723 final double maxCoord = Math.max(Math.abs(x), Math.abs(y));
724 if (maxCoord > UNSCALED_MAX) {
725 scaledX = x * SCALE_DOWN_FACTOR;
726 scaledY = y * SCALE_DOWN_FACTOR;
727 } else {
728 scaledX = x * SCALE_UP_FACTOR;
729 scaledY = y * SCALE_UP_FACTOR;
730 }
731
732 final double scaledNormInv = 1.0 / Vectors.norm(scaledX, scaledY);
733
734 if (Vectors.isRealNonZero(scaledNormInv)) {
735 return new Unit(
736 scaledX * scaledNormInv,
737 scaledY * scaledNormInv);
738 } else if (throwOnFailure) {
739 throw Vectors.illegalNorm(norm);
740 }
741 return null;
742 }
743 }
744
745
746
747
748
749
750
751 public static final class Sum extends EuclideanVectorSum<Vector2D> {
752
753 private final org.apache.commons.numbers.core.Sum xsum;
754
755 private final org.apache.commons.numbers.core.Sum ysum;
756
757
758
759
760 Sum(final Vector2D initial) {
761 this.xsum = org.apache.commons.numbers.core.Sum.of(initial.x);
762 this.ysum = org.apache.commons.numbers.core.Sum.of(initial.y);
763 }
764
765
766 @Override
767 public Sum add(final Vector2D vec) {
768 xsum.add(vec.x);
769 ysum.add(vec.y);
770 return this;
771 }
772
773
774 @Override
775 public Sum addScaled(final double scale, final Vector2D vec) {
776 xsum.addProduct(scale, vec.x);
777 ysum.addProduct(scale, vec.y);
778 return this;
779 }
780
781
782 @Override
783 public Vector2D get() {
784 return Vector2D.of(
785 xsum.getAsDouble(),
786 ysum.getAsDouble());
787 }
788
789
790
791
792 public static Sum create() {
793 return new Sum(Vector2D.ZERO);
794 }
795
796
797
798
799
800 public static Sum of(final Vector2D initial) {
801 return new Sum(initial);
802 }
803
804
805
806
807
808
809 public static Sum of(final Vector2D first, final Vector2D... more) {
810 final Sum s = new Sum(first);
811 for (final Vector2D v : more) {
812 s.add(v);
813 }
814 return s;
815 }
816 }
817 }