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.Locale;
7 import java.util.regex.Matcher;
8 import java.util.regex.Pattern;
9
10 import javax.vecmath.Matrix3d;
11 import javax.vecmath.Point3d;
12
13
14
15
16
17
18
19
20
21
22
23 public class Rotation implements IRotable, Serializable, Cloneable {
24
25
26
27
28 public static final Rotation NONE = new Rotation(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
29
30
31
32
33 public static final Rotation ZERO = new Rotation();
34
35
36
37
38
39 static final long serialVersionUID = -1964427510333336912L;
40
41 static {
42
43
44 PropertyEditorManager.registerEditor(Rotation.class, Rotation.PropertyEditor.class);
45 }
46
47
48
49
50 public static class PropertyEditor extends PropertyEditorSupport {
51
52 @Override
53 public String getAsText() {
54 if (getValue() != null) {
55 return getValue().toString();
56 } else {
57 return "null";
58 }
59 }
60
61 @Override
62 public void setAsText(String s) {
63 if ("null".equals(s.trim())) {
64 setValue(null);
65 return;
66 } else {
67 double[] d = Location.PropertyEditor.parseNumberArray(s);
68 if (d.length != 3) {
69 throw new IllegalArgumentException();
70 }
71 setValue(new Rotation(d[0], d[1], d[2]));
72 }
73 }
74 }
75
76 @Override
77 public Rotation clone() {
78 return new Rotation(this);
79 }
80
81
82
83
84
85 public final double yaw;
86
87
88
89
90 public final double roll;
91
92
93
94
95 public final double pitch;
96
97 private Integer hashCode;
98
99
100
101
102
103
104
105
106 public double getYaw() {
107 return yaw;
108 }
109
110
111
112
113
114
115 public double getPitch() {
116 return pitch;
117 }
118
119
120
121
122
123
124 public double getRoll() {
125 return roll;
126 }
127
128
129
130
131
132
133
134
135 @Override
136 public Rotation getRotation() {
137 return this;
138 }
139
140
141
142
143
144
145 public Point3d getPoint3d() {
146 return new Point3d(pitch, yaw, roll);
147 }
148
149
150
151
152
153
154 private Rotation() {
155 this(0,0,0);
156 }
157
158
159
160
161
162
163
164
165
166
167
168
169 public Rotation(double pitch, double yaw, double roll) {
170 this.pitch = pitch;
171 this.yaw = yaw;
172 this.roll = roll;
173 }
174
175
176
177
178
179
180
181 public Rotation(Rotation rotation) {
182 this(rotation.getPitch(), rotation.getYaw(), rotation.getRoll());
183 }
184
185 private int computeHashCode() {
186 final int prime = 31;
187 int result = 1;
188 long temp;
189 temp = Double.doubleToLongBits(pitch);
190 result = prime * result + (int) (temp ^ (temp >>> 32));
191 temp = Double.doubleToLongBits(roll);
192 result = prime * result + (int) (temp ^ (temp >>> 32));
193 temp = Double.doubleToLongBits(yaw);
194 result = prime * result + (int) (temp ^ (temp >>> 32));
195 return result;
196 }
197
198
199
200
201
202 public static final Pattern rotationPattern = Pattern
203 .compile("\\[([-+]?[0-9]+(\\.[0-9]+){0,1})\\; ([-+]?[0-9]+(\\.[0-9]+){0,1})\\; ([-+]?[0-9]+(\\.[0-9]+){0,1})\\]");
204
205
206
207
208
209
210
211 public Rotation(String string) {
212 Matcher m = rotationPattern.matcher(string);
213 if (m.find()) {
214 String strPitch = m.group(1);
215 String strYaw = m.group(3);
216 String strRoll = m.group(5);
217 try {
218 this.pitch = Double.parseDouble(strPitch);
219 } catch (Exception e) {
220 throw new RuntimeException("String '" + string
221 + "', was not matched as Rotation, because pitch-value '" + strPitch + "' is not a number.");
222 }
223 try {
224 this.yaw = Double.parseDouble(strYaw);
225 } catch (Exception e) {
226 throw new RuntimeException("String '" + string + "', was not matched as Rotation, because yaw-value '"
227 + strYaw + "' is not a number.");
228 }
229 try {
230 this.roll = Double.parseDouble(strRoll);
231 } catch (Exception e) {
232 throw new RuntimeException("String '" + string + "', was not matched as Rotation, because roll-value '"
233 + strRoll + "' is not a number.");
234 }
235 } else {
236 throw new RuntimeException("String '" + string + "' was not matched as Rotation.");
237 }
238 }
239
240
241
242
243
244
245
246
247
248
249
250
251
252 public static final double LinearInterp(double a, double b, double alpha) {
253 return a + (b - a) * alpha;
254 }
255
256
257
258
259
260
261
262
263
264
265
266
267
268 public static final double LogInterp(double a, double b, double alpha) {
269 return a + (b - a) * Math.log(1 + alpha * (Math.E - 1));
270 }
271
272
273
274
275
276
277
278
279
280
281
282
283
284 public static final double ExpInterp(double a, double b, double alpha) {
285
286 return a + (b - a) * (Math.exp(alpha) - 1) / (Math.E - 1);
287 }
288
289
290
291
292
293
294
295
296
297
298
299
300
301 public static final Rotation RotationLinearInterp(Rotation a, Rotation b, double alpha) {
302 return new Rotation(LinearInterp(a.pitch, b.pitch, alpha), LinearInterp(a.yaw, b.yaw, alpha), LinearInterp(
303 a.roll, b.roll, alpha));
304 }
305
306
307
308
309
310
311
312
313
314
315
316
317
318 public final Rotation RotationLinearInterp(Rotation b, double alpha) {
319 return RotationLinearInterp(this, b, alpha);
320 }
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335 public static final Rotation RotationLogInterp(Rotation a, Rotation b, double alpha) {
336 return new Rotation(LogInterp(a.pitch, b.pitch, alpha), LogInterp(a.yaw, b.yaw, alpha), LogInterp(a.roll,
337 b.roll, alpha));
338 }
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353 public final Rotation RotationLogInterp(Rotation b, double alpha) {
354 return RotationLogInterp(this, b, alpha);
355 }
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370 public static final Rotation RotationExpInterp(Rotation a, Rotation b, double alpha) {
371 return new Rotation(ExpInterp(a.pitch, b.pitch, alpha), ExpInterp(a.yaw, b.yaw, alpha), ExpInterp(a.roll,
372 b.roll, alpha));
373 }
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388 public final Rotation RotationExpInterp(Rotation b, double alpha) {
389 return RotationExpInterp(this, b, alpha);
390 }
391
392
393
394
395 public enum Order {
396 YAW_PITCH_ROLL, ROLL_PITCH_YAW, PITCH_YAW_ROLL, PITCH_ROLL_YAW, YAW_ROLL_PITCH, ROLL_YAW_PITCH;
397 }
398
399
400
401
402
403
404
405 @Override
406 public int hashCode() {
407 if (hashCode == null) hashCode = computeHashCode();
408 return hashCode;
409 }
410
411
412
413
414
415
416
417
418
419 @Override
420 public boolean equals(Object obj) {
421 if (this == obj)
422 return true;
423 if (obj == null)
424 return false;
425 if (!(obj instanceof Rotation))
426 return false;
427 Rotation other = (Rotation) obj;
428 if (hashCode != other.hashCode) return false;
429 if (Double.doubleToLongBits(pitch) != Double.doubleToLongBits(other.pitch))
430 return false;
431 if (Double.doubleToLongBits(roll) != Double.doubleToLongBits(other.roll))
432 return false;
433 if (Double.doubleToLongBits(yaw) != Double.doubleToLongBits(other.yaw))
434 return false;
435 return true;
436 }
437
438
439
440
441
442
443
444
445
446
447
448 public static boolean equal(Rotation r1, Rotation r2) {
449 if (r1 == null && r2 == null)
450 return true;
451 if (r1 == null || r2 == null)
452 return false;
453
454 return r1.equals(r2);
455 }
456
457 @Override
458 public String toString() {
459 return String.format(Locale.ENGLISH, "[%.2f; %.2f; %.2f]", pitch, yaw, roll);
460 }
461
462
463
464
465
466
467 public Location toLocation() {
468 return toLocation(Order.PITCH_YAW_ROLL);
469 }
470
471
472
473
474
475
476
477
478 public Location toLocation(Order order) {
479 Matrix3d yaw = constructXYRot(getYaw() / 32767 * Math.PI);
480 Matrix3d roll = constructYZRot(getRoll() / 32767 * Math.PI);
481 Matrix3d pitch = constructXZRot(getPitch() / 32767 * Math.PI);
482
483 Location res = new Location(1, 0, 0);
484
485 switch (order) {
486 case YAW_PITCH_ROLL:
487 return res.mul(yaw).mul(pitch).mul(roll);
488 case ROLL_PITCH_YAW:
489 return res.mul(roll).mul(pitch).mul(yaw);
490 case PITCH_YAW_ROLL:
491 return res.mul(pitch).mul(yaw).mul(roll);
492 case PITCH_ROLL_YAW:
493 return res.mul(pitch).mul(roll).mul(yaw);
494 case YAW_ROLL_PITCH:
495 return res.mul(yaw).mul(roll).mul(pitch);
496 case ROLL_YAW_PITCH:
497 return res.mul(roll).mul(yaw).mul(pitch);
498 }
499
500 return null;
501 }
502
503
504
505
506
507
508
509
510 public static Matrix3d constructYZRot(double angle) {
511 Matrix3d res = new Matrix3d();
512 double cos = Math.cos(angle);
513 double sin = Math.sin(angle);
514 res.setM00(1);
515 res.setM11(cos);
516 res.setM21(-sin);
517 res.setM12(sin);
518 res.setM22(cos);
519
520 return res;
521 }
522
523
524
525
526
527
528
529
530 public static Matrix3d constructXZRot(double angle) {
531 Matrix3d res = new Matrix3d();
532 double cos = Math.cos(angle);
533 double sin = Math.sin(angle);
534 res.setM00(cos);
535 res.setM20(sin);
536 res.setM11(1);
537 res.setM02(-sin);
538 res.setM22(cos);
539
540 return res;
541 }
542
543
544
545
546
547
548
549
550 public static Matrix3d constructXYRot(double angle) {
551 Matrix3d res = new Matrix3d();
552 double cos = Math.cos(angle);
553 double sin = Math.sin(angle);
554 res.setM00(cos);
555 res.setM10(-sin);
556 res.setM01(sin);
557 res.setM11(cos);
558 res.setM22(1);
559
560 return res;
561 }
562
563
564
565
566
567
568 public Rotation setYaw(double yaw) {
569 return new Rotation(this.pitch, yaw, this.roll);
570 }
571
572
573
574
575
576
577 public Rotation setPitch(double pitch) {
578 return new Rotation(pitch, this.yaw, this.roll);
579 }
580
581
582
583
584
585
586 public Rotation setRoll(double roll) {
587 return new Rotation(this.pitch, this.yaw, roll);
588 }
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624 }