View Javadoc

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   * Location within the world.
22   * 
23   * Location is represented as a point within the world's coordinates.
24   * 
25   * @author Juraj 'Loque' Simlovic
26   * @author Radek 'Black_Hand' Pibil
27   */
28  public class Location implements ILocated, Serializable, Cloneable {
29  
30  	/**
31  	 * Location representing NONE.
32  	 */
33  	public static final Location NONE = new Location(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
34  	
35  	/**
36  	 * Location(0,0,0).
37  	 */
38  	public static final Location ZERO = new Location();
39  
40  	/**
41  	 * This here is for StoryFactory compatibility reasons. Can be removed in
42  	 * 2012.
43  	 */
44  	static final long serialVersionUID = -7001866845605943889L;
45  
46  	/**
47  	 * We have to register PropertyEditor for Location, without it, Location
48  	 * won't be introspectable.
49  	 */
50  	static {
51  		PropertyEditorManager.registerEditor(Location.class, Location.PropertyEditor.class);
52  	}
53  
54  	/**
55  	 * PropertyEditor for class Location.
56  	 */
57  	public static class PropertyEditor extends PropertyEditorSupport {
58  
59  		@Override
60  		public String getAsText() {
61  			if (getValue() != null) {
62  				return getValue().toString();
63  			} else {
64  				return "null";
65  			}
66  		}
67  
68  		@Override
69  		public void setAsText(String s) {
70  			if ("null".equals(s.trim())) {
71  				setValue(null);
72  			} else {
73  				double[] d = Location.PropertyEditor.parseNumberArray(s);
74  				if (d.length != 3) {
75  					throw new IllegalArgumentException();
76  				}
77  				setValue(new Location(d));
78  			}
79  		}
80  
81  		public static double[] parseNumberArray(String s) {
82  			s = s.trim();
83  			// if string has brackets, remove them
84  			if ((s.startsWith("[") && s.endsWith("]")) || (s.startsWith("(") && s.endsWith(")"))) {
85  				s = s.substring(1, s.length() - 1);
86  			}
87  			// now we expect num , num, num
88  			StringTokenizer st = new StringTokenizer(s, ";");
89  
90  			// try to parse numbers and set the new value
91  			try {
92  				double[] d = new double[st.countTokens()];
93  				for (int i = 0; i < d.length; ++i) {
94  					d[i] = Double.parseDouble(st.nextToken());
95  				}
96  				return d;
97  			} catch (NumberFormatException ex) {
98  				throw new IllegalArgumentException(ex);
99  			}
100 		}
101 
102 		@Override
103 		public boolean supportsCustomEditor() {
104 			return false;
105 		}
106 	}
107 
108 	@Override
109 	public Location clone() {
110 		return new Location(this);
111 	}
112 
113 	public Vector3d asVector3d() {
114 		return new Vector3d(x, y, z);
115 	}
116 
117 	public Point3d asPoint3d() {
118 		return new Point3d(x, y, z);
119 	}
120 
121 	public Point3D asPoint3D() {
122 		return new Point3D(x, y, z);
123 	}
124 
125 	/** X coordinate. */
126 	public final double x;
127 	/** Y coordinate. */
128 	public final double y;
129 	/** Z coordinate. */
130 	public final double z;
131 	
132 	private Integer hashCode = null;
133 
134 	/* ********************************************************************** */
135 
136 	/**
137 	 * X coordinate.
138 	 * 
139 	 * @return X coordinate.
140 	 */
141 	public double getX() {
142 		return x;
143 	}
144 
145 	/**
146 	 * Y coordinate.
147 	 * 
148 	 * @return Y coordinate.
149 	 */
150 	public double getY() {
151 		return y;
152 	}
153 
154 	/**
155 	 * Z coordinate.
156 	 * 
157 	 * @return Z coordinate.
158 	 */
159 	public double getZ() {
160 		return z;
161 	}
162 
163 	/* ********************************************************************** */
164 
165 	/**
166 	 * Retreives sum of this location and given location.
167 	 * 
168 	 * @param l
169 	 *            Location to be added to this location.
170 	 * @return Sum of the two locations.
171 	 */
172 	public Location add(Location l) {
173 		// create sum of the locations
174 		return new Location(x + l.x, y + l.y, z + l.z);
175 	}
176 
177 	/**
178 	 * Retreives sum of two given locations.
179 	 * 
180 	 * @param l1
181 	 *            First location to be summed.
182 	 * @param l2
183 	 *            Second location to be summed.
184 	 * @return Sum of the two locations.
185 	 */
186 	public static Location add(Location l1, Location l2) {
187 		// create sum of the locations
188 		return new Location(l1.x + l2.x, l1.y + l2.y, l1.z + l2.z);
189 	}
190 
191 	/**
192 	 * Retreives subtraction of given location from this location.
193 	 * 
194 	 * @param l
195 	 *            Location to be subtracted.
196 	 * @return Subtraction of the two locations.
197 	 */
198 	public Location sub(Location l) {
199 		// create substraction of the locations
200 		return new Location(x - l.x, y - l.y, z - l.z);
201 	}
202 
203 	/**
204 	 * Retreives subtraction of two given locations.
205 	 * 
206 	 * @param l1
207 	 *            Location to be subtracted from.
208 	 * @param l2
209 	 *            Location to be subtracted.
210 	 * @return Subtraction of the two locations.
211 	 */
212 	public static Location sub(Location l1, Location l2) {
213 		// create substraction of the locations
214 		return new Location(l1.x - l2.x, l1.y - l2.y, l1.z - l2.z);
215 	}
216 
217 	/* ********************************************************************* */
218 
219 	/**
220 	 * Adds given velocity to this location.
221 	 * 
222 	 * @param v
223 	 *            Velocity to be added to this location.
224 	 * @return Sum of the location and velocity.
225 	 */
226 	public Location add(Velocity v) {
227 		// create sum of the locations
228 		return new Location(x + v.x, y + v.y, z + v.z);
229 	}
230 
231 	/**
232 	 * Adds given velocity to given location.
233 	 * 
234 	 * @param l
235 	 *            Location to be summed.
236 	 * @param v
237 	 *            Velocity to be summed.
238 	 * @return Sum of the location and velocity.
239 	 */
240 	public static Location add(Location l, Velocity v) {
241 		// create sum of the locations
242 		return new Location(l.x + v.x, l.y + v.y, l.z + v.z);
243 	}
244 
245 	/**
246 	 * Subtracts given velocity from this location.
247 	 * 
248 	 * @param v
249 	 *            Velocity to be subtracted.
250 	 * @return Subtraction of the velocity from the location.
251 	 */
252 	public Location sub(Velocity v) {
253 		// create substraction of the locations
254 		return new Location(x - v.x, y - v.y, z - v.z);
255 	}
256 
257 	/**
258 	 * Subtracts given velocity from given location.
259 	 * 
260 	 * @param l
261 	 *            Location to be subtracted from.
262 	 * @param v
263 	 *            Velocity to be subtracted.
264 	 * @return Subtraction of the velocity from the location.
265 	 */
266 	public static Location sub(Location l, Velocity v) {
267 		// create substraction of the locations
268 		return new Location(l.x - v.x, l.y - v.y, l.z - v.z);
269 	}
270 
271 	/* ********************************************************************* */
272 
273 	/**
274 	 * Scales values of all three coordinates by given multiplier.
275 	 * 
276 	 * @param d
277 	 *            Scaling multiplier.
278 	 * @return Location with all three coordinates negated.
279 	 */
280 	public Location scale(double d) {
281 		// create location with scaled values
282 		return new Location(x * d, y * d, z * d);
283 	}
284 
285 	/* ********************************************************************* */
286 
287 	/**
288 	 * Lineary interpolates between this location and given location.
289 	 * 
290 	 * @param l
291 	 *            Location to be interpolated to.
292 	 * @param d
293 	 *            Interpolation parameter.
294 	 * @return Linear interpolation between the two locations.
295 	 */
296 	public Location interpolate(Location l, double d) {
297 		// from the other side
298 		double d1 = 1.0D - d;
299 		// create interpolation of the locations
300 		return new Location(d1 * x + d * l.x, d1 * y + d * l.y, d1 * z + d * l.z);
301 	}
302 
303 	/**
304 	 * Linearly interpolates between two given locations.
305 	 * 
306 	 * @param l1
307 	 *            Location to be interpolated from.
308 	 * @param l2
309 	 *            Location to be interpolated to.
310 	 * @param d
311 	 *            Interpolation parameter.
312 	 * @return Linear interpolation between the two locations.
313 	 */
314 	public static Location interpolate(Location l1, Location l2, double d) {
315 		// from the other side
316 		double d1 = 1.0D - d;
317 		// create interpolation of the locations
318 		return new Location(d1 * l1.x + d * l2.x, d1 * l1.y + d * l2.y, d1 * l1.z + d * l2.z);
319 	}
320 
321 	/* ********************************************************************** */
322 
323 	/**
324 	 * Generates a hashCode for this Location.
325 	 * 
326 	 * @return the hashcode for this Location.
327 	 */
328 
329 	@Override
330 	public int hashCode() {		
331 		if (hashCode == null) hashCode = computeHashCode();
332 		return hashCode;
333 	}
334 
335 	/**
336 	 * Tells, whether this location equals to given object.
337 	 * 
338 	 * @param obj
339 	 *            Object to be compared with.
340 	 * @return True, if the object is a Location and has has the same values of
341 	 *         all three corresponding coordinates.
342 	 */
343 	@Override
344 	public boolean equals(Object obj) {
345 		if (this == obj)
346 			return true;
347 		if (obj == null)
348 			return false;
349 		if (getClass() != obj.getClass())
350 			return false;
351 		Location other = (Location) obj;
352 		if (hashCode != other.hashCode) return false;
353 		if (Double.doubleToLongBits(x) != Double.doubleToLongBits(other.x))
354 			return false;
355 		if (Double.doubleToLongBits(y) != Double.doubleToLongBits(other.y))
356 			return false;
357 		if (Double.doubleToLongBits(z) != Double.doubleToLongBits(other.z))
358 			return false;
359 		return true;
360 	}
361 
362 	/**
363 	 * Tells, whether two given locations equal.
364 	 * 
365 	 * @param l1
366 	 *            First location to compare.
367 	 * @param l2
368 	 *            Second location to compare.
369 	 * @return True, if the locations has the same values of all three
370 	 *         corresponding coordinates.
371 	 */
372 	public static boolean equal(Location l1, Location l2) {
373 		if (l1 == null && l2 == null)
374 			return true;
375 		if (l1 == null || l2 == null)
376 			return false;
377 
378 		return l1.equals(l2);
379 	}
380 
381 	/**
382 	 * Tells, whether the distance between coordinates of this location and
383 	 * given location is less than or equal to the given epsilon.
384 	 * 
385 	 * @param l
386 	 *            Location to compare with.
387 	 * @param epsilon
388 	 *            Epsilon to compare with.
389 	 * @return True, if the distance between the locations is less than the
390 	 *         epsilon, false otherwise.
391 	 */
392 	public boolean equals(Location l, double epsilon) {
393 		if (l == null)
394 			return false;
395 
396 		double d;
397 
398 		// x axes distance
399 		d = x - l.x;
400 		if ((d >= 0 ? d : -d) > epsilon)
401 			return false;
402 
403 		// y axes distance
404 		d = y - l.y;
405 		if ((d >= 0.0D ? d : -d) > epsilon)
406 			return false;
407 
408 		// z axes distance
409 		d = z - l.z;
410 		if ((d >= 0.0D ? d : -d) > epsilon)
411 			return false;
412 
413 		// aye, aye, sir..
414 		return true;
415 	}
416 
417 	/**
418 	 * Tells, whether the distance between coordinates of two given locations is
419 	 * less than or equal to the given epsilon.
420 	 * 
421 	 * @param l1
422 	 *            First location to compare.
423 	 * @param l2
424 	 *            Second location to compare.
425 	 * @param epsilon
426 	 *            Epsilon to compare with.
427 	 * @return True, if the distance between the locations is less than the
428 	 *         epsilon, false otherwise.
429 	 */
430 	public static boolean equal(Location l1, Location l2, double epsilon) {
431 		double d;
432 
433 		// x axes distance
434 		d = l1.x - l2.x;
435 		if ((d >= 0 ? d : -d) > epsilon)
436 			return false;
437 
438 		// y axes distance
439 		d = l1.y - l2.y;
440 		if ((d >= 0.0D ? d : -d) > epsilon)
441 			return false;
442 
443 		// z axes distance
444 		d = l1.z - l2.z;
445 		if ((d >= 0.0D ? d : -d) > epsilon)
446 			return false;
447 
448 		// aye, aye, sir..
449 		return true;
450 	}
451 
452 	/* ********************************************************************** */
453 
454 	/**
455 	 * Calculates average of all 'locations'. If locations.size() == 0, returns
456 	 * null.
457 	 * 
458 	 * @param locations
459 	 * @return average location
460 	 */
461 	public static Location getAverage(Collection<Location> locations) {
462 		if (locations.size() == 0)
463 			return null;
464 		Iterator<Location> iter = locations.iterator();
465 		Location result = new Location(iter.next());
466 		while (iter.hasNext()) {
467 			result.add(iter.next());
468 		}
469 		return result.scale(1 / locations.size());
470 	}
471 
472 	/* ********************************************************************** */
473 
474 	/**
475 	 * Calculates the distance between this and given location.
476 	 * 
477 	 * @param l
478 	 *            Location to be calculated the distance to.
479 	 * @return Euclidean distance between the two locations.
480 	 */
481 	public double getDistance(Location l) {
482 		double dx = l.x - x;
483 		double dy = l.y - y;
484 		double dz = l.z - z;
485 		return Math.sqrt(dx * dx + dy * dy + dz * dz);
486 	}
487 
488 	/**
489 	 * Calculates the distance between this and given location (ignoring 'z'
490 	 * coordinate).
491 	 * 
492 	 * @param l
493 	 *            Location to be calculated the distance to.
494 	 * @return Euclidean distance between the two locations.
495 	 */
496 	public double getDistance2D(Location l) {
497 		double dx = l.x - x;
498 		double dy = l.y - y;
499 		return Math.sqrt(dx * dx + dy * dy);
500 	}
501 
502 	/**
503 	 * Calculates the distance between two given locations.
504 	 * 
505 	 * @param l1
506 	 *            Location to be calculated the distance from.
507 	 * @param l2
508 	 *            Location to be calculated the distance to.
509 	 * @return Euclidean distance between the two locations.
510 	 */
511 	public static double getDistance(Location l1, Location l2) {
512 		double dx = l2.x - l1.x;
513 		double dy = l2.y - l1.y;
514 		double dz = l2.z - l1.z;
515 		return Math.sqrt(dx * dx + dy * dy + dz * dz);
516 	}
517 
518 	/**
519 	 * Returns difference between z-coords (this.z - location.z).
520 	 * 
521 	 * @param location
522 	 * @return z-difference
523 	 */
524 	public double getDistanceZ(Location location) {
525 		return z - location.z;
526 	}
527 
528 	/**
529 	 * Calculates the distance between two given locations (ignoring 'z'
530 	 * coordinate).
531 	 * 
532 	 * @param l1
533 	 *            Location to be calculated the distance from.
534 	 * @param l2
535 	 *            Location to be calculated the distance to.
536 	 * @return Euclidean distance between the two locations.
537 	 */
538 	public static double getDistance2D(Location l1, Location l2) {
539 		double dx = l2.x - l1.x;
540 		double dy = l2.y - l1.y;
541 		return Math.sqrt(dx * dx + dy * dy);
542 	}
543 
544 	/**
545 	 * Calculates the square of the distance between this and given location.
546 	 * 
547 	 * @param l
548 	 *            Location to be calculated the distance to.
549 	 * @return Square of the euclidean distance between the two locations.
550 	 */
551 	public double getDistanceSquare(Location l) {
552 		double dx = l.x - x;
553 		double dy = l.y - y;
554 		double dz = l.z - z;
555 		return dx * dx + dy * dy + dz * dz;
556 	}
557 
558 	/**
559 	 * Calculates the square of the distance between two given locations.
560 	 * 
561 	 * @param l1
562 	 *            Location to be calculated the distance from.
563 	 * @param l2
564 	 *            Location to be calculated the distance to.
565 	 * @return Square of the euclidean distance between the two locations.
566 	 */
567 	public static double getDistanceSquare(Location l1, Location l2) {
568 		double dx = l2.x - l1.x;
569 		double dy = l2.y - l1.y;
570 		double dz = l2.z - l1.z;
571 		return dx * dx + dy * dy + dz * dz;
572 	}
573 
574 	/**
575 	 * Calculates the Manhattan distance between this and given location.
576 	 * 
577 	 * @param l
578 	 *            Location to be calculated the distance to.
579 	 * @return Manhattan (i.e. 1-norm) distance between the two locations.
580 	 */
581 	public double getDistanceL1(Location l) {
582 		double dx = Math.abs(l.x - x);
583 		double dy = Math.abs(l.y - y);
584 		double dz = Math.abs(l.z - z);
585 		return dx + dy + dz;
586 	}
587 
588 	/**
589 	 * Calculates the Manhattan distance between two given locations.
590 	 * 
591 	 * @param l1
592 	 *            Location to be calculated the distance from.
593 	 * @param l2
594 	 *            Location to be calculated the distance to.
595 	 * @return Manhattan (i.e. 1-norm) distance between the two locations.
596 	 */
597 	public static double getDistanceL1(Location l1, Location l2) {
598 		double dx = Math.abs(l2.x - l1.x);
599 		double dy = Math.abs(l2.y - l1.y);
600 		double dz = Math.abs(l2.z - l1.z);
601 		return dx + dy + dz;
602 	}
603 
604 	/**
605 	 * Calculates the Chebyshev distance between this and given location.
606 	 * 
607 	 * @param l
608 	 *            Location to be calculated the distance to.
609 	 * @return Chebyshev (i.e. infinity-norm) distance between the two
610 	 *         locations.
611 	 */
612 	public double getDistanceLinf(Location l) {
613 		double dx = Math.abs(l.x - x);
614 		double dy = Math.abs(l.y - y);
615 		double dz = Math.abs(l.z - z);
616 		return Math.max(Math.max(dx, dy), dz);
617 	}
618 
619 	/**
620 	 * Calculates the Chebyshev distance between two given locations.
621 	 * 
622 	 * @param l1
623 	 *            Location to be calculated the distance from.
624 	 * @param l2
625 	 *            Location to be calculated the distance to.
626 	 * @return Chebyshev (i.e. infinity-norm) distance between the two
627 	 *         locations.
628 	 */
629 	public static double getDistanceLinf(Location l1, Location l2) {
630 		double dx = Math.abs(l2.x - l1.x);
631 		double dy = Math.abs(l2.y - l1.y);
632 		double dz = Math.abs(l2.z - l1.z);
633 		return Math.max(Math.max(dx, dy), dz);
634 	}
635 
636 	/**
637 	 * Calculates the distance between this and given location after being
638 	 * projected to the (x,y) plane.
639 	 * 
640 	 * @param l
641 	 *            Location to be calculated the distance to.
642 	 * @return Plane-projected distance between the two locations.
643 	 */
644 	public double getDistancePlane(Location l) {
645 		double dx = l.x - x;
646 		double dy = l.y - y;
647 		return Math.sqrt(dx * dx + dy * dy);
648 	}
649 
650 	/**
651 	 * Calculates the distance between two given locations after being projected
652 	 * to the (x,y) plane.
653 	 * 
654 	 * @param l1
655 	 *            Location to be calculated the distance from.
656 	 * @param l2
657 	 *            Location to be calculated the distance to.
658 	 * @return Plane-projected distance between the two locations.
659 	 */
660 	public static double getDistancePlane(Location l1, Location l2) {
661 		double dx = l2.x - l1.x;
662 		double dy = l2.y - l1.y;
663 		return Math.sqrt(dx * dx + dy * dy);
664 	}
665 
666 	/* ********************************************************************** */
667 
668 	/**
669 	 * Retreives the location itself to implement {@link ILocated}.
670 	 * 
671 	 * @return The location itself (note: does not create a copy).
672 	 */
673 	@Override
674 	public Location getLocation() {
675 		return this;
676 	}
677 
678 	/**
679 	 * Retreives javax.vecmath.Point3d representation of the location.
680 	 * 
681 	 * @return javax.vecmath.Point3d representation with x, y and z values set.
682 	 */
683 	public Point3d getPoint3d() {
684 		return new Point3d(x, y, z);
685 	}
686 
687 	/* ********************************************************************** */
688 
689 	/**
690 	 * Creates location with all values set to zeroes.
691 	 */
692 	private Location() {
693 		this(0,0,0);
694 	}
695 
696 	/**
697 	 * Creates location with specified coordinates.
698 	 * 
699 	 * @param x
700 	 *            X coordinate.
701 	 * @param y
702 	 *            Y coordinate.
703 	 * @param z
704 	 *            Z coordinate.
705 	 */
706 	public Location(double x, double y, double z) {
707 		this.x = x;
708 		this.y = y;
709 		this.z = z;
710 	}
711 
712 	private int computeHashCode() {
713 		final int prime = 31;
714 		int result = 1;
715 		long temp;
716 		temp = Double.doubleToLongBits(x);
717 		result = prime * result + (int) (temp ^ (temp >>> 32));
718 		temp = Double.doubleToLongBits(y);
719 		result = prime * result + (int) (temp ^ (temp >>> 32));
720 		temp = Double.doubleToLongBits(z);
721 		result = prime * result + (int) (temp ^ (temp >>> 32));
722 		return result;
723 	}
724 
725 	/**
726 	 * Creates location with specified planar coordinates. Sets z to zero.
727 	 * 
728 	 * @param x
729 	 *            X coordinate.
730 	 * @param y
731 	 *            Y coordinate.
732 	 */
733 	public Location(double x, double y) {
734 		this(x,y,0);		
735 	}
736 
737 	/**
738 	 * Copy constructor
739 	 * 
740 	 * @param source
741 	 *            Location to copy
742 	 */
743 	public Location(Location source) {
744 		this(source.getX(), source.getY(), source.getZ());
745 	}
746 
747 	/**
748 	 * Pattern used to parse {@link Location#toString()} in
749 	 * {@link Location#Location(String)}.
750 	 */
751 	public static final Pattern locationPattern = Pattern
752 			.compile("\\[([-+]?[0-9]+(\\.[0-9]+){0,1})\\; ([-+]?[0-9]+(\\.[0-9]+){0,1})\\; ([-+]?[0-9]+(\\.[0-9]+){0,1})\\]");
753 
754 	/**
755 	 * Zero delta for {@link Location#equals(Location)}.
756 	 */
757 	public static final double DISTANCE_ZERO = 0.000000001;
758 
759 	/**
760 	 * Parses the location from the "string" generated by
761 	 * {@link Location#toString()}. If it fails, it throws RuntimeException.
762 	 * 
763 	 * @param string
764 	 */
765 	public Location(String string) {
766 		Matcher m = locationPattern.matcher(string);
767 		if (m.find()) {
768 			String strX = m.group(1);
769 			String strY = m.group(3);
770 			String strZ = m.group(5);
771 			try {
772 				this.x = Double.parseDouble(strX);
773 			} catch (Exception e) {
774 				throw new RuntimeException("String '" + string
775 						+ "', was not matched as Location, because X-coordinate '" + strX + "' is not a number.");
776 			}
777 			try {
778 				this.y = Double.parseDouble(strY);
779 			} catch (Exception e) {
780 				throw new RuntimeException("String '" + string
781 						+ "', was not matched as Location, because Y-coordinate '" + strY + "' is not a number.");
782 			}
783 			try {
784 				this.z = Double.parseDouble(strZ);
785 			} catch (Exception e) {
786 				throw new RuntimeException("String '" + string
787 						+ "', was not matched as Location, because Z-coordinate '" + strZ + "' is not a number.");
788 			}
789 		} else {
790 			throw new RuntimeException("String '" + string + "' was not matched as Location.");
791 		}
792 	}
793 
794 	/**
795 	 * Creates location from array of three doubles. Sets x = d[0], y = d[1] and
796 	 * z = d[2].
797 	 * 
798 	 * @param d
799 	 *            Array of doubles to be used for creation.
800 	 */
801 	public Location(double d[]) {
802 		if (d.length >= 1)
803 			this.x = d[0];
804 		else 
805 			this.x = 0;
806 		if (d.length >= 2)
807 			this.y = d[1];
808 		else 
809 			this.y = 0;
810 		if (d.length >= 3)
811 			this.z = d[2];
812 		else
813 			this.z = 0;
814 	}
815 
816 	/**
817 	 * Creates location from array of three doubles. Sets x = f[0], y = f[1] and
818 	 * z = f[2].
819 	 * 
820 	 * @param f
821 	 *            Array of to be used for creation.
822 	 */
823 	public Location(float f[]) {
824 		if (f.length >= 1)
825 			this.x = f[0];
826 		else
827 			this.x = 0;
828 		if (f.length >= 2)
829 			this.y = f[1];
830 		else
831 			this.y = 0;
832 		if (f.length >= 3)
833 			this.z = f[2];
834 		else
835 			this.z = 0;
836 	}
837 
838 	/**
839 	 * Creates location from specified 3D point.
840 	 * 
841 	 * @param p
842 	 *            Point in space to be used for creation.
843 	 */
844 	public Location(Tuple3d p) {
845 		this(p.x, p.y, p.z);
846 	}
847 
848 	/**
849 	 * Creates location from specified 3D point.
850 	 * 
851 	 * @param p
852 	 *            Point in space to be used for creation.
853 	 */
854 	public Location(Point3D p) {
855 		this(p.getX(), p.getY(), p.getZ());
856 	}
857 
858 	/**
859 	 * Calculates dot product of this Location and Location b
860 	 * 
861 	 * @param b
862 	 *            Location to dot with
863 	 * @return dot product of this and b
864 	 */
865 	public double dot(Location b) {
866 		return x * b.getX() + y * b.getY() + z * b.getZ();
867 	}
868 
869 	/**
870 	 * Calculates dot product of this Location and Location b in 2D (x,y coord
871 	 * only)
872 	 * 
873 	 * @param b
874 	 *            Location to dot with
875 	 * @return dot product of this and b
876 	 */
877 	public double dot2D(Location b) {
878 		return x * b.getX() + y * b.getY();
879 	}
880 
881 	/**
882 	 * Calculates cross product of this Location and Lcoations b
883 	 * 
884 	 * @param b
885 	 *            Location to cross with
886 	 * @return cross product of this and b
887 	 */
888 	public Location cross(Location b) {
889 		return new Location(y * b.getZ() - z * b.getY(), z * b.getX() - x * b.getZ(), x * b.getY() - y * b.getX());
890 	}
891 
892 	/**
893 	 * Converts Location into Rotation. Since location is only a single vector,
894 	 * roll is ommited.
895 	 * 
896 	 * @param order
897 	 *            tells resulting rotation, which rotation order should it
898 	 *            represent
899 	 * @return resulting rotation
900 	 */
901 	public Rotation getRotation(Rotation.Order order) {
902 		Location this_normalized = getNormalized();
903 		double yaw = 0d, pitch = 0d;
904 		switch (order) {
905 		case YAW_PITCH_ROLL:
906 		case ROLL_YAW_PITCH:
907 		case YAW_ROLL_PITCH:
908 			yaw = Math.atan2(this_normalized.getY(), Math.sqrt(1 - this_normalized.getY() * this_normalized.getY()));
909 
910 			pitch = Math.atan2(this_normalized.getZ(), this_normalized.getX());
911 			break;
912 		case PITCH_YAW_ROLL:
913 		case PITCH_ROLL_YAW:
914 		case ROLL_PITCH_YAW:
915 			pitch = Math.atan2(Math.sqrt(1 - this_normalized.getZ() * this_normalized.getZ()), this_normalized.getZ());
916 			yaw = Math.atan2(this_normalized.getX(), this_normalized.getY());
917 			break;
918 		}
919 		return new Rotation(pitch / Math.PI * 32768 - 1, yaw / Math.PI * 32768 - 1, 0);
920 	}
921 
922 	/**
923 	 * WIP not completed yet. Use only in case roll yaw pitch order is used.
924 	 * Converts this location into rotation required to be applied on (1,0,0) in
925 	 * order achieve this location. Difference from getRotation is that in this
926 	 * case, quaternion like rotation chaining is achieved. That means that if
927 	 * you take rotation (x, y, z) then result would be same as applying
928 	 * rotation (x, 0, 0) on (1, 0, 0) - identity - then (0, y, 0) on the
929 	 * result, but like it was identity. Then again (0, 0, z) on the result like
930 	 * it was identity. Thus it works in similar vein as quaternion
931 	 * multiplication.
932 	 * 
933 	 * @param order
934 	 * @return
935 	 */
936 	public Rotation getQuatLikeRotationSeq(Rotation.Order order) {
937 		Location projected = new Location(1, 0, 0);
938 
939 		double yaw = 0d, pitch = 0d;
940 		switch (order) {
941 		case ROLL_YAW_PITCH:
942 			yaw = Math.atan2(getY(), getX());
943 			projected = projected.mul(Rotation.constructXYRot(yaw));
944 
945 			pitch = Math.atan2(getZ(), new Location(getX(), getY(), 0).dot(projected));
946 
947 			return new Rotation(pitch / Math.PI * 32768 - 1, yaw / Math.PI * 32768 - 1, 0);
948 		}
949 		return Rotation.ZERO;
950 	}
951 
952 	/**
953 	 * Normalizes this Location
954 	 * 
955 	 * @return normalized
956 	 */
957 	public Location getNormalized() {
958 		return scale(1 / getLength());
959 	}
960 
961 	/**
962 	 * Calculates length of Location
963 	 * 
964 	 * @return length
965 	 */
966 	public double getLength() {
967 		return Math.sqrt(x * x + y * y + z * z);
968 	}
969 
970 	/**
971 	 * Projects this Location (vector) using matrix from parameter
972 	 * 
973 	 * @param matrix
974 	 *            projection matrix
975 	 * @return resulting Location
976 	 */
977 	public Location mul(Matrix3d matrix) {
978 		Location res = new Location(
979 							matrix.getM00() * x + matrix.getM10() * y + matrix.getM20() * z,
980 							matrix.getM01() * x + matrix.getM11() * y + matrix.getM21() * z,
981 							matrix.getM02() * x + matrix.getM12() * y + matrix.getM22() * z
982 				       );
983 
984 		return res;
985 	}
986 
987 	/**
988 	 * Calculates inverse Location
989 	 * 
990 	 * @return new inverted Location
991 	 */
992 	public Location invert() {
993 		return new Location(-x, -y, -z);
994 	}
995 
996 	/**
997 	 * Sets the X coordinate.
998 	 * 
999 	 * @return new location object
1000 	 */
1001 	public Location setX(double x) {
1002 		return new Location(x, this.y, this.z);
1003 	}
1004 		
1005 	/**
1006 	 * Sets the Y coordinate.
1007 	 * 
1008 	 * @return new location object
1009 	 */
1010 	public Location setY(double y) {
1011 		return new Location(this.x, y, this.z);
1012 	}
1013 	
1014 	/**
1015 	 * Sets the Z coordinate.
1016 	 * 
1017 	 * @return new location object
1018 	 */
1019 	public Location setZ(double z) {
1020 		return new Location(this.x, this.y, z);
1021 	}
1022 	
1023 	/**
1024 	 * Adds to the X coordinate.
1025 	 * 
1026 	 * @return new location object
1027 	 */
1028 	public Location addX(double x) {
1029 		return new Location(this.x + x, this.y, this.z);
1030 	}
1031 		
1032 	/**
1033 	 * Adds to the Y coordinate.
1034 	 * 
1035 	 * @return new location object
1036 	 */
1037 	public Location addY(double y) {
1038 		return new Location(this.x, this.y + y, this.z);
1039 	}
1040 	
1041 	/**
1042 	 * Adds to the Z coordinate.
1043 	 * 
1044 	 * @return new location object
1045 	 */
1046 	public Location addZ(double z) {
1047 		return new Location(this.x, this.y, this.z + z);
1048 	}
1049 	
1050 	/**
1051 	 * Scales the X coordinate.
1052 	 * 
1053 	 * @return new location object
1054 	 */
1055 	public Location scaleX(double x) {
1056 		return new Location(this.x * x, this.y, this.z);
1057 	}
1058 		
1059 	/**
1060 	 * Scales the Y coordinate.
1061 	 * 
1062 	 * @return new location object
1063 	 */
1064 	public Location scaleY(double y) {
1065 		return new Location(this.x, this.y * y, this.z);
1066 	}
1067 	
1068 	/**
1069 	 * Scales the Z coordinate.
1070 	 * 
1071 	 * @return new location object
1072 	 */
1073 	public Location scaleZ(double z) {
1074 		return new Location(this.x, this.y, this.z * z);
1075 	}
1076 	
1077 //	/**
1078 //	 * Set content of this location from passed tocation.
1079 //	 * 
1080 //	 * @return this with newly set value
1081 //	 */
1082 //	public Location setTo(Location l) {
1083 //		this.x = l.x;
1084 //		this.y = l.y;
1085 //		this.z = l.z;
1086 //
1087 //		return this;
1088 //	}
1089 //
1090 //	/**
1091 //	 * Set content of this location from passed data.
1092 //	 * 
1093 //	 * @return this with newly set value
1094 //	 */
1095 //	public Location setTo(double x, double y, double z) {
1096 //		this.x = x;
1097 //		this.y = y;
1098 //		this.z = z;
1099 //
1100 //		return this;
1101 //	}
1102 
1103 	/* ********************************************************************** */
1104 
1105 	@Override
1106 	public String toString() {
1107 		return String.format(Locale.ENGLISH, "[%.2f; %.2f; %.2f]", x, y, z);
1108 	}
1109 
1110 }