1 package cz.cuni.amis.pogamut.base3d.worldview.object;
2
3 import java.beans.PropertyEditorManager;
4 import java.beans.PropertyEditorSupport;
5 import java.io.Serializable;
6 import java.util.Collection;
7 import java.util.Iterator;
8 import java.util.Locale;
9 import java.util.StringTokenizer;
10 import java.util.regex.Matcher;
11 import java.util.regex.Pattern;
12
13 import javax.vecmath.Matrix3d;
14 import javax.vecmath.Point3d;
15 import javax.vecmath.Tuple3d;
16 import javax.vecmath.Vector3d;
17
18 import math.geom3d.Point3D;
19
20
21
22
23
24
25
26
27
28 public class Location implements ILocated, Serializable, Cloneable {
29
30
31
32
33 public static final Location NONE = new Location(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
34
35
36
37
38
39 static final long serialVersionUID = -7001866845605943889L;
40
41
42
43
44
45 static {
46 PropertyEditorManager.registerEditor(Location.class,
47 Location.PropertyEditor.class);
48 }
49
50
51
52
53 public static class PropertyEditor extends PropertyEditorSupport {
54
55 @Override
56 public String getAsText() {
57 if (getValue() != null) {
58 return getValue().toString();
59 } else {
60 return "null";
61 }
62 }
63
64 @Override
65 public void setAsText(String s) {
66 if ("null".equals(s.trim())) {
67 setValue(null);
68 } else {
69 double[] d = Location.PropertyEditor.parseNumberArray(s);
70 if (d.length != 3) {
71 throw new IllegalArgumentException();
72 }
73 setValue(new Location(d));
74 }
75 }
76
77 public static double[] parseNumberArray(String s) {
78 s = s.trim();
79
80 if ((s.startsWith("[") && s.endsWith("]"))
81 || (s.startsWith("(") && s.endsWith(")"))) {
82 s = s.substring(1, s.length() - 1);
83 }
84
85 StringTokenizer st = new StringTokenizer(s, ";");
86
87
88 try {
89 double[] d = new double[st.countTokens()];
90 for (int i = 0; i < d.length; ++i) {
91 d[i] = Double.parseDouble(st.nextToken());
92 }
93 return d;
94 } catch (NumberFormatException ex) {
95 throw new IllegalArgumentException(ex);
96 }
97 }
98
99 @Override
100 public boolean supportsCustomEditor() {
101 return false;
102 }
103 }
104
105 @Override
106 public Location clone() {
107 return new Location(this);
108 }
109
110 public Vector3d asVector3d() {
111 return new Vector3d(x, y, z);
112 }
113
114 public Point3d asPoint3d() {
115 return new Point3d(x, y, z);
116 }
117
118 public Point3D asPoint3D() {
119 return new Point3D(x, y, z);
120 }
121
122
123 public double x = 0;
124
125 public double y = 0;
126
127 public double z = 0;
128
129
130
131
132
133
134
135
136 public double getX() {
137 return x;
138 }
139
140
141
142
143
144
145 public double getY() {
146 return y;
147 }
148
149
150
151
152
153
154 public double getZ() {
155 return z;
156 }
157
158
159
160
161
162
163
164
165
166
167 public Location add(Location l) {
168
169 return new Location(x + l.x, y + l.y, z + l.z);
170 }
171
172
173
174
175
176
177
178
179
180
181 public static Location add(Location l1, Location l2) {
182
183 return new Location(l1.x + l2.x, l1.y + l2.y, l1.z + l2.z);
184 }
185
186
187
188
189
190
191
192
193 public Location sub(Location l) {
194
195 return new Location(x - l.x, y - l.y, z - l.z);
196 }
197
198
199
200
201
202
203
204
205
206
207 public static Location sub(Location l1, Location l2) {
208
209 return new Location(l1.x - l2.x, l1.y - l2.y, l1.z - l2.z);
210 }
211
212
213
214
215
216
217
218
219
220
221 public Location add(Velocity v) {
222
223 return new Location(x + v.x, y + v.y, z + v.z);
224 }
225
226
227
228
229
230
231
232
233
234
235 public static Location add(Location l, Velocity v) {
236
237 return new Location(l.x + v.x, l.y + v.y, l.z + v.z);
238 }
239
240
241
242
243
244
245
246
247 public Location sub(Velocity v) {
248
249 return new Location(x - v.x, y - v.y, z - v.z);
250 }
251
252
253
254
255
256
257
258
259
260
261 public static Location sub(Location l, Velocity v) {
262
263 return new Location(l.x - v.x, l.y - v.y, l.z - v.z);
264 }
265
266
267
268
269
270
271
272
273
274
275 public Location scale(double d) {
276
277 return new Location(x * d, y * d, z * d);
278 }
279
280
281
282
283
284
285
286
287
288
289
290
291 public Location interpolate(Location l, double d) {
292
293 double d1 = 1.0D - d;
294
295 return new Location(d1 * x + d * l.x, d1 * y + d * l.y, d1 * z + d
296 * l.z);
297 }
298
299
300
301
302
303
304
305
306
307
308
309
310 public static Location interpolate(Location l1, Location l2, double d) {
311
312 double d1 = 1.0D - d;
313
314 return new Location(d1 * l1.x + d * l2.x, d1 * l1.y + d * l2.y, d1
315 * l1.z + d * l2.z);
316 }
317
318
319
320
321
322
323
324
325
326
327
328 public boolean equals(Location v) {
329 if (v == null)
330 return false;
331
332 return (x == v.x) && (y == v.y) && (z == v.z);
333 }
334
335
336
337
338
339
340
341
342
343
344
345 public static boolean equal(Location l1, Location l2) {
346
347 return (l1.x == l2.x) && (l1.y == l2.y) && (l1.z == l2.z);
348 }
349
350
351
352
353
354
355
356
357
358
359
360
361 public boolean equals(Location l, double epsilon) {
362 double d;
363
364
365 d = x - l.x;
366 if ((d >= 0 ? d : -d) > epsilon)
367 return false;
368
369
370 d = y - l.y;
371 if ((d >= 0.0D ? d : -d) > epsilon)
372 return false;
373
374
375 d = z - l.z;
376 if ((d >= 0.0D ? d : -d) > epsilon)
377 return false;
378
379
380 return true;
381 }
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396 public static boolean equal(Location l1, Location l2, double epsilon) {
397 double d;
398
399
400 d = l1.x - l2.x;
401 if ((d >= 0 ? d : -d) > epsilon)
402 return false;
403
404
405 d = l1.y - l2.y;
406 if ((d >= 0.0D ? d : -d) > epsilon)
407 return false;
408
409
410 d = l1.z - l2.z;
411 if ((d >= 0.0D ? d : -d) > epsilon)
412 return false;
413
414
415 return true;
416 }
417
418
419
420
421
422
423
424
425
426
427 public static Location getAverage(Collection<Location> locations) {
428 if (locations.size() == 0)
429 return null;
430 Iterator<Location> iter = locations.iterator();
431 Location result = new Location(iter.next());
432 while (iter.hasNext()) {
433 result.add(iter.next());
434 }
435 return result.scale(1 / locations.size());
436 }
437
438
439
440
441
442
443
444
445
446
447 public double getDistance(Location l) {
448 double dx = l.x - x;
449 double dy = l.y - y;
450 double dz = l.z - z;
451 return Math.sqrt(dx * dx + dy * dy + dz * dz);
452 }
453
454
455
456
457
458
459
460
461
462 public double getDistance2D(Location l) {
463 double dx = l.x - x;
464 double dy = l.y - y;
465 return Math.sqrt(dx * dx + dy * dy);
466 }
467
468
469
470
471
472
473
474
475
476
477 public static double getDistance(Location l1, Location l2) {
478 double dx = l2.x - l1.x;
479 double dy = l2.y - l1.y;
480 double dz = l2.z - l1.z;
481 return Math.sqrt(dx * dx + dy * dy + dz * dz);
482 }
483
484
485
486
487
488
489
490 public double getDistanceZ(Location location) {
491 return z - location.z;
492 }
493
494
495
496
497
498
499
500
501
502
503
504 public static double getDistance2D(Location l1, Location l2) {
505 double dx = l2.x - l1.x;
506 double dy = l2.y - l1.y;
507 return Math.sqrt(dx * dx + dy * dy);
508 }
509
510
511
512
513
514
515
516
517 public double getDistanceSquare(Location l) {
518 double dx = l.x - x;
519 double dy = l.y - y;
520 double dz = l.z - z;
521 return dx * dx + dy * dy + dz * dz;
522 }
523
524
525
526
527
528
529
530
531
532
533 public static double getDistanceSquare(Location l1, Location l2) {
534 double dx = l2.x - l1.x;
535 double dy = l2.y - l1.y;
536 double dz = l2.z - l1.z;
537 return dx * dx + dy * dy + dz * dz;
538 }
539
540
541
542
543
544
545
546
547 public double getDistanceL1(Location l) {
548 double dx = Math.abs(l.x - x);
549 double dy = Math.abs(l.y - y);
550 double dz = Math.abs(l.z - z);
551 return dx + dy + dz;
552 }
553
554
555
556
557
558
559
560
561
562
563 public static double getDistanceL1(Location l1, Location l2) {
564 double dx = Math.abs(l2.x - l1.x);
565 double dy = Math.abs(l2.y - l1.y);
566 double dz = Math.abs(l2.z - l1.z);
567 return dx + dy + dz;
568 }
569
570
571
572
573
574
575
576
577
578 public double getDistanceLinf(Location l) {
579 double dx = Math.abs(l.x - x);
580 double dy = Math.abs(l.y - y);
581 double dz = Math.abs(l.z - z);
582 return Math.max(Math.max(dx, dy), dz);
583 }
584
585
586
587
588
589
590
591
592
593
594
595 public static double getDistanceLinf(Location l1, Location l2) {
596 double dx = Math.abs(l2.x - l1.x);
597 double dy = Math.abs(l2.y - l1.y);
598 double dz = Math.abs(l2.z - l1.z);
599 return Math.max(Math.max(dx, dy), dz);
600 }
601
602
603
604
605
606
607
608
609
610 public double getDistancePlane(Location l) {
611 double dx = l.x - x;
612 double dy = l.y - y;
613 return Math.sqrt(dx * dx + dy * dy);
614 }
615
616
617
618
619
620
621
622
623
624
625
626 public static double getDistancePlane(Location l1, Location l2) {
627 double dx = l2.x - l1.x;
628 double dy = l2.y - l1.y;
629 return Math.sqrt(dx * dx + dy * dy);
630 }
631
632
633
634
635
636
637
638
639 @Override
640 public Location getLocation() {
641 return this;
642 }
643
644
645
646
647
648
649 public Point3d getPoint3d() {
650 return new Point3d(x, y, z);
651 }
652
653
654
655
656
657
658 public Location() {
659 }
660
661
662
663
664
665
666
667
668
669
670
671 public Location(double x, double y, double z) {
672 this.x = x;
673 this.y = y;
674 this.z = z;
675 }
676
677
678
679
680
681
682
683
684
685 public Location(double x, double y) {
686 this.x = x;
687 this.y = y;
688 }
689
690
691
692
693
694
695
696 public Location(Location source) {
697 this.x = source.getX();
698 this.y = source.getY();
699 this.z = source.getZ();
700 }
701
702
703
704
705
706 public static final Pattern locationPattern = Pattern
707 .compile("\\[([-+]?[0-9]+(\\.[0-9]+){0,1})\\; ([-+]?[0-9]+(\\.[0-9]+){0,1})\\; ([-+]?[0-9]+(\\.[0-9]+){0,1})\\]");
708
709
710
711
712 public static final double DISTANCE_ZERO = 0.000000001;
713
714
715
716
717
718
719
720 public Location(String string) {
721 Matcher m = locationPattern.matcher(string);
722 if (m.find()) {
723 String strX = m.group(1);
724 String strY = m.group(3);
725 String strZ = m.group(5);
726 try {
727 this.x = Double.parseDouble(strX);
728 } catch (Exception e) {
729 throw new RuntimeException(
730 "String '"
731 + string
732 + "', was not matched as Location, because X-coordinate '"
733 + strX + "' is not a number.");
734 }
735 try {
736 this.y = Double.parseDouble(strY);
737 } catch (Exception e) {
738 throw new RuntimeException(
739 "String '"
740 + string
741 + "', was not matched as Location, because Y-coordinate '"
742 + strY + "' is not a number.");
743 }
744 try {
745 this.z = Double.parseDouble(strZ);
746 } catch (Exception e) {
747 throw new RuntimeException(
748 "String '"
749 + string
750 + "', was not matched as Location, because Z-coordinate '"
751 + strZ + "' is not a number.");
752 }
753 } else {
754 throw new RuntimeException("String '" + string
755 + "' was not matched as Location.");
756 }
757 }
758
759
760
761
762
763
764
765
766 public Location(double d[]) {
767 if (d.length >= 1)
768 this.x = d[0];
769 if (d.length >= 2)
770 this.y = d[1];
771 if (d.length >= 3)
772 this.z = d[2];
773 }
774
775
776
777
778
779
780
781
782 public Location(float f[]) {
783 if (f.length >= 1)
784 this.x = f[0];
785 if (f.length >= 2)
786 this.y = f[1];
787 if (f.length >= 3)
788 this.z = f[2];
789 }
790
791
792
793
794
795
796
797 public Location(Tuple3d p) {
798 this.x = p.x;
799 this.y = p.y;
800 this.z = p.z;
801 }
802
803
804
805
806
807
808
809
810 public double dot(Location b) {
811 return x * b.getX() + y * b.getY() + z * b.getZ();
812 }
813
814
815
816
817
818
819
820
821
822 public double dot2D(Location b) {
823 return x * b.getX() + y * b.getY();
824 }
825
826
827
828
829
830
831
832
833 public Location cross(Location b) {
834 return new Location(y * b.getZ() - z * b.getY(), z * b.getX() - x
835 * b.getZ(), x * b.getY() - y * b.getX());
836 }
837
838
839
840
841
842
843
844
845
846
847 public Rotation getRotation(Rotation.Order order) {
848 Location this_normalized = getNormalized();
849 double yaw = 0d, pitch = 0d;
850 switch (order) {
851 case YAW_PITCH_ROLL:
852 case ROLL_YAW_PITCH:
853 case YAW_ROLL_PITCH:
854 yaw = Math.atan2(this_normalized.getY(), Math.sqrt(1
855 - this_normalized.getY() * this_normalized.getY()));
856
857 pitch = Math.atan2(this_normalized.getZ(), this_normalized.getX());
858 break;
859 case PITCH_YAW_ROLL:
860 case PITCH_ROLL_YAW:
861 case ROLL_PITCH_YAW:
862 pitch = Math.atan2(Math.sqrt(1 - this_normalized.getZ()
863 * this_normalized.getZ()), this_normalized.getZ());
864 yaw = Math.atan2(this_normalized.getX(), this_normalized.getY());
865 break;
866 }
867 return new Rotation(pitch / Math.PI * 32768 - 1, yaw / Math.PI * 32768
868 - 1, 0);
869 }
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885 public Rotation getQuatLikeRotationSeq(Rotation.Order order) {
886 Location projected = new Location(1, 0, 0);
887
888 double yaw = 0d, pitch = 0d;
889 switch (order) {
890 case ROLL_YAW_PITCH:
891 yaw = Math.atan2(getY(), getX());
892 projected = projected.mul(Rotation.constructXYRot(yaw));
893
894 pitch = Math.atan2(getZ(), new Location(getX(), getY(), 0)
895 .dot(projected));
896
897 return new Rotation(pitch / Math.PI * 32768 - 1, yaw / Math.PI
898 * 32768 - 1, 0);
899 }
900 return new Rotation();
901 }
902
903
904
905
906
907
908 public Location getNormalized() {
909 return scale(1 / getLength());
910 }
911
912
913
914
915
916
917 public double getLength() {
918 return Math.sqrt(x * x + y * y + z * z);
919 }
920
921
922
923
924
925
926
927
928 public Location mul(Matrix3d matrix) {
929 Location res = new Location();
930 res.x = matrix.getM00() * x + matrix.getM10() * y + matrix.getM20() * z;
931 res.y = matrix.getM01() * x + matrix.getM11() * y + matrix.getM21() * z;
932 res.z = matrix.getM02() * x + matrix.getM12() * y + matrix.getM22() * z;
933
934 return res;
935 }
936
937
938
939
940
941
942 public Location invert() {
943 return new Location(-x, -y, -z);
944 }
945
946
947
948
949
950
951 public Location setTo(Location l) {
952 this.x = l.x;
953 this.y = l.y;
954 this.z = l.z;
955
956 return this;
957 }
958
959
960
961
962
963
964 public Location setTo(double x, double y, double z) {
965 this.x = x;
966 this.y = y;
967 this.z = z;
968
969 return this;
970 }
971
972
973
974 public boolean equals(Object o) {
975 if (o == null) return false;
976 if (!(o instanceof Location)) return false;
977 Location l = (Location)o;
978 return getDistance(l) < DISTANCE_ZERO;
979 }
980
981 @Override
982 public String toString() {
983 return String.format(Locale.ENGLISH, "[%.2f; %.2f; %.2f]", x, y, z);
984 }
985
986 }