1 /*
2 * Copyright (C) 2012 Alberto Irurueta Carro (alberto@irurueta.com)
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16 package com.irurueta.geometry;
17
18 import com.irurueta.algebra.AlgebraException;
19 import com.irurueta.algebra.Matrix;
20 import com.irurueta.algebra.SingularValueDecomposer;
21
22 import java.io.Serializable;
23 import java.util.Objects;
24
25 /**
26 * Line2D in R2. Lines can be expressed using the following expression:
27 * A * x + B * y + C = 0
28 * where A and B are different from zero. Changing this expression to a
29 * y = m * x + b format leads to the following definitions:
30 * m: slope of the line. Defined as m = -A/B
31 * b: interception point of this line. Defined as b = -c/B
32 * angle of the line with respect to the x-axis = Math.atan(slope).
33 */
34 public class Line2D implements Serializable {
35
36 /**
37 * Number of line parameters.
38 */
39 public static final int LINE_NUMBER_PARAMS = 3;
40
41 /**
42 * Positive threshold determine whether points lay inside (is locus) of a
43 * given line or not.
44 */
45 public static final double DEFAULT_LOCUS_THRESHOLD = 1e-12;
46
47 /**
48 * Minimum allowed threshold.
49 */
50 public static final double MIN_THRESHOLD = 0.0;
51
52 /**
53 * Defines the threshold used when comparing two values.
54 */
55 public static final double DEFAULT_COMPARISON_THRESHOLD = 1e-10;
56
57 /**
58 * Constant defining the size of vector that define the direction of a line.
59 */
60 private static final int INHOM_VECTOR_SIZE = 2;
61
62 /**
63 * Machine precision.
64 */
65 private static final double PRECISION = 1e-12;
66
67 /**
68 * Parameter A of a line.
69 */
70 private double a;
71
72 /**
73 * Parameter B of a line.
74 */
75 private double b;
76
77 /**
78 * Parameter C of a line.
79 */
80 private double c;
81
82 /**
83 * Indicates if line is normalized or not.
84 */
85 private boolean normalized;
86
87 /**
88 * Constructor.
89 */
90 public Line2D() {
91 a = b = c = 0.0;
92 normalized = false;
93 }
94
95 /**
96 * Constructor with parameters. Parameters of a line are provided in the
97 * following homogeneous format:
98 * A * x + B * y + C = 0
99 *
100 * @param a Parameter A of a line.
101 * @param b Parameter B of a line.
102 * @param c Parameter C of a line.
103 */
104 public Line2D(final double a, final double b, final double c) {
105 setParameters(a, b, c);
106 }
107
108 /**
109 * Constructor. This constructor takes two 2D points to build a line passing
110 * through both of them.
111 *
112 * @param pointA First point used to compute the line.
113 * @param pointB Second point used to compute the line.
114 * @param noThrow If true no exception is thrown even if points are
115 * coincident.
116 * @throws CoincidentPointsException Raised if points are equal.
117 */
118 public Line2D(final Point2D pointA, final Point2D pointB, final boolean noThrow) throws CoincidentPointsException {
119 setParametersFromPairOfPoints(pointA, pointB, noThrow);
120 }
121
122 /**
123 * Constructor. This constructor takes two 2D points to build a line passing
124 * through both of them.
125 *
126 * @param pointA First point used to compute the line.
127 * @param pointB Second point used to compute the line.
128 */
129 public Line2D(final Point2D pointA, final Point2D pointB) {
130 setParametersFromPairOfPoints(pointA, pointB);
131 }
132
133 /**
134 * Constructor.
135 *
136 * @param array Array containing the three parameters of a line (A, B, C).
137 * @throws IllegalArgumentException Raised if length of provided array is
138 * not three.
139 */
140 public Line2D(final double[] array) {
141 setParameters(array);
142 }
143
144 /**
145 * Constructor of a line from one point and its director vector.
146 *
147 * @param point point passing through the line.
148 * @param vector director vector.
149 * @throws IllegalArgumentException raised if vector length is not 2.
150 */
151 public Line2D(final Point2D point, final double[] vector) {
152 setParametersFromPointAndDirectorVector(point, vector);
153 }
154
155 /**
156 * Returns parameter A of this line.
157 *
158 * @return Parameter A of this line.
159 */
160 public double getA() {
161 return a;
162 }
163
164 /**
165 * Returns parameter B of this line.
166 *
167 * @return Parameter B of this line.
168 */
169 public double getB() {
170 return b;
171 }
172
173 /**
174 * Returns parameter C of this line.
175 *
176 * @return Parameter C of this line.
177 */
178 public double getC() {
179 return c;
180 }
181
182 /**
183 * Sets parameters of this line.
184 *
185 * @param a Parameter A of this line.
186 * @param b Parameter B of this line.
187 * @param c Parameter C of this line.
188 */
189 public final void setParameters(final double a, final double b, final double c) {
190 this.a = a;
191 this.b = b;
192 this.c = c;
193 normalized = false;
194 }
195
196 /**
197 * Sets parameters of this line.
198 *
199 * @param array Array containing parameters of this line.
200 * @throws IllegalArgumentException Raised if provided array does not
201 * have length equal to 3.
202 */
203 public final void setParameters(final double[] array) {
204 if (array.length != LINE_NUMBER_PARAMS) {
205 throw new IllegalArgumentException();
206 }
207
208 a = array[0];
209 b = array[1];
210 c = array[2];
211 normalized = false;
212 }
213
214 /**
215 * Sets parameter A of this line.
216 *
217 * @param a Parameter A of this line.
218 */
219 public void setA(final double a) {
220 this.a = a;
221 normalized = false;
222 }
223
224 /**
225 * Sets parameter B of this line.
226 *
227 * @param b Parameter B of this line.
228 */
229 public void setB(final double b) {
230 this.b = b;
231 normalized = false;
232 }
233
234 /**
235 * Sets parameter C of this line.
236 *
237 * @param c Parameter C of this line.
238 */
239 public void setC(final double c) {
240 this.c = c;
241 normalized = false;
242 }
243
244 /**
245 * Returns the slope of this line.
246 *
247 * @return Slope of this line.
248 */
249 public double getSlope() {
250 return (-a / b);
251 }
252
253 /**
254 * Sets the slope of this line.
255 *
256 * @param slope Slope of this line.
257 */
258 public void setSlope(final double slope) {
259 normalize();
260 if (Math.abs(slope) > 1.0) {
261 a = 1.0;
262 b = -a / slope;
263 } else {
264 b = 1.0;
265 a = -b * slope;
266 }
267 normalized = false;
268 }
269
270 /**
271 * Returns the angle of this line in radians.
272 *
273 * @return Angle of this line in radians.
274 */
275 public double getAngle() {
276 var alpha = Math.atan2(-a, b);
277 // if alpha is not between -90 and 90 degrees, then fix angle
278 if (alpha > Math.PI / 2.0) {
279 alpha = alpha - Math.PI;
280 } else if (alpha < -Math.PI / 2.0) {
281 alpha = alpha + Math.PI;
282 }
283
284 return alpha;
285 }
286
287 /**
288 * Sets angle of this line in radians.
289 *
290 * @param angle Angle of this line in radians.
291 */
292 public void setAngle(final double angle) {
293 final var slope = Math.tan(angle);
294 setSlope(slope);
295 }
296
297 /**
298 * Returns the y-coordinate intercept point of this line.
299 * For a line following the expression A * X + B * y + C = 0 this method
300 * evaluates y for x = 0.
301 *
302 * @return Vertical coordinate where the line intercepts with the Y axis.
303 */
304 public double getYIntercept() {
305 return -(c / b);
306 }
307
308 /**
309 * Sets the y-coordinate intercept point of this line.
310 * For a line following the expression A * x + B * y + C = 0 this method
311 * recalculates the parameters that define the line.
312 *
313 * @param yIntercept Vertical coordinate where the line intercepts with the
314 * Y axis.
315 */
316 public void setYIntercept(final double yIntercept) {
317 c = -b * yIntercept;
318 normalized = false;
319 }
320
321 /**
322 * Sets parameters of this line from a pair of 2D points. The line will pass
323 * through both points.
324 *
325 * @param pointA First point laying on this line.
326 * @param pointB Second point laying on this line.
327 * @param noThrow Enables/disables throwing exceptions.
328 * @throws CoincidentPointsException Raised if provided points are equal.
329 */
330 public final void setParametersFromPairOfPoints(
331 final Point2D pointA, final Point2D pointB, final boolean noThrow) throws CoincidentPointsException {
332
333 try {
334 pointA.normalize();
335 pointB.normalize();
336
337 final var m = new Matrix(2, Point2D.POINT2D_HOMOGENEOUS_COORDINATES_LENGTH);
338
339 m.setElementAt(0, 0, pointA.getHomX());
340 m.setElementAt(0, 1, pointA.getHomY());
341 m.setElementAt(0, 2, pointA.getHomW());
342
343 m.setElementAt(1, 0, pointB.getHomX());
344 m.setElementAt(1, 1, pointB.getHomY());
345 m.setElementAt(1, 2, pointB.getHomW());
346
347 // m = U * S * V', where m is 2x3, U is 2x3, S is 3x3 and V is 3x3
348 final var decomposer = new SingularValueDecomposer(m);
349 decomposer.decompose();
350
351 // because we have only 2 points, matrix will be rank 2 only if
352 // points are not linearly dependent, if they are, then they are
353 // coincident because homogeneous points are defined up to scale
354 if (decomposer.getRank() < 2 && !noThrow) {
355 throw new CoincidentPointsException();
356 }
357
358 // Because m is rank 2, the last column of V, from now on l, will be
359 // the right null-space of m. Hence, m*l = 0, which is the equation of
360 // a line.
361 // Hence, l is the null space for both pointA and pointB, or in other
362 // words, pointA and pointB are the locus of l, or l is the line
363 // passing through both pointA and pointB
364 final var v = decomposer.getV();
365
366 a = v.getElementAt(0, 2);
367 b = v.getElementAt(1, 2);
368 c = v.getElementAt(2, 2);
369 normalized = false;
370 } catch (final AlgebraException ignore) {
371 // never happens
372 }
373 }
374
375 /**
376 * Sets parameters of this line from a pair of 2D points. The line will pass
377 * through both points.
378 *
379 * @param pointA First point laying on this line.
380 * @param pointB Second point laying on this line.
381 */
382 public final void setParametersFromPairOfPoints(final Point2D pointA, final Point2D pointB) {
383 try {
384 setParametersFromPairOfPoints(pointA, pointB, true);
385 } catch (final CoincidentPointsException ignore) {
386 // exception never raised
387 // because of last true parameter
388 }
389 }
390
391 /**
392 * Sets parameters of a 2D line from one point and its director vector.
393 *
394 * @param point point passing through the line.
395 * @param vector director vector.
396 * @throws IllegalArgumentException raised if vector length is not 2.
397 */
398 public final void setParametersFromPointAndDirectorVector(final Point2D point, final double[] vector) {
399 if (vector.length != INHOM_VECTOR_SIZE) {
400 throw new IllegalArgumentException();
401 }
402
403 // normalize point to increase accuracy
404 point.normalize();
405
406 a = vector[0];
407 b = vector[1];
408
409 c = -(a * point.getHomX() + b * point.getHomY()) / point.getHomW();
410
411 normalized = false;
412 }
413
414 /**
415 * Returns boolean indicating whether provided point lies within this line
416 * (at a maximum distance of provided threshold).
417 *
418 * @param point Point to be checked.
419 * @param threshold Threshold to determine whether the point lies inside the
420 * line.
421 * @return True if point lies inside the line (is locus), false otherwise.
422 * @throws IllegalArgumentException Raised if provided threshold is negative.
423 */
424 public boolean isLocus(final Point2D point, final double threshold) {
425 if (threshold < MIN_THRESHOLD) {
426 throw new IllegalArgumentException();
427 }
428
429 // make dot product of homogeneous coordinates with line
430 // m = [x, y, w], l = [a, b, c], then
431 // x * a + y * b + w * c must be very small to be locus
432
433 point.normalize();
434 normalize();
435
436 final var dotProd = point.getHomX() * a + point.getHomY() * b + point.getHomW() * c;
437
438 return Math.abs(dotProd) < threshold;
439 }
440
441 /**
442 * Returns boolean indicating whether provided point lies within this line
443 * (at a maximum distance of DEFAULT_LOCUS_THRESHOLD).
444 *
445 * @param point Point to be checked.
446 * @return True if point lies inside the line (is locus), false otherwise.
447 */
448 public boolean isLocus(final Point2D point) {
449 return isLocus(point, DEFAULT_LOCUS_THRESHOLD);
450 }
451
452 /**
453 * Distance between a line and a point. Returned distance equals to the
454 * Euclidean distance between this line and provided point but having sign.
455 * Sign indicates whether point is at one side or the other of the line.
456 *
457 * @param point Point whose distance to this line will be computed.
458 * @return Distance between this line and provided point.
459 */
460 public double signedDistance(final Point2D point) {
461 point.normalize();
462 normalize();
463
464 // numerator is the dot product of point and line
465 final var num = point.getHomX() * a + point.getHomY() * b + point.getHomW() * c;
466
467 final var den = Math.sqrt(a * a + b * b) * point.getHomW();
468
469 return num / den;
470 }
471
472 /**
473 * Returns the point belonging to this line closest to provided point, which
474 * will be located at signedDistance(Point2D) from this line.
475 * If provided point belong to this line, then the same point will be
476 * returned as a result.
477 *
478 * @param point Point to be checked.
479 * @return Closest point.
480 */
481 public Point2D getClosestPoint(final Point2D point) {
482 return getClosestPoint(point, DEFAULT_LOCUS_THRESHOLD);
483 }
484
485 /**
486 * Returns the point belonging to this line closest to provided point, which
487 * will be located at signedDistance(Point2D) from this line.
488 * If provided point belong to this line, then the same point will be
489 * returned as a result.
490 *
491 * @param point Point to be checked.
492 * @param threshold Threshold to determine whether point is locus of line or
493 * not.
494 * @return Closest point.
495 * @throws IllegalArgumentException Raised if threshold is negative.
496 */
497 public Point2D getClosestPoint(final Point2D point, final double threshold) {
498 final var result = Point2D.create();
499 closestPoint(point, result, threshold);
500 return result;
501 }
502
503 /**
504 * Computes the point belonging to this line closest to provided point,
505 * which will be located at signedDistance(Point2D) from this line.
506 * If provided point belongs to this line, then the same point will be
507 * returned as a result.
508 *
509 * @param point Point to be checked.
510 * @param result Instance where the closest point will be stored.
511 */
512 public void closestPoint(final Point2D point, final Point2D result) {
513 closestPoint(point, result, DEFAULT_LOCUS_THRESHOLD);
514 }
515
516 /**
517 * Computes the point belonging to this line closest to provided point,
518 * which will be located at signedDistance(Point2D) from this line.
519 * If provided point belongs to this line, then the same point will be
520 * returned as a result.
521 *
522 * @param point Point to be checked.
523 * @param result Instance where the closest point will be stored.
524 * @param threshold threshold to determine whether a point belongs to the
525 * line's locus.
526 * @throws IllegalArgumentException Raised if threshold is negative.
527 */
528 public void closestPoint(final Point2D point, final Point2D result, final double threshold) {
529 if (threshold < MIN_THRESHOLD) {
530 throw new IllegalArgumentException();
531 }
532
533 // normalize point to increase accuracy
534 point.normalize();
535
536 if (isLocus(point, threshold)) {
537 // if point belongs to line, then it is returned as result
538 result.setCoordinates(point);
539 return;
540 }
541
542 // move point in director vector direction until it belong to this line
543 // (point.getInhomX() + mA * amount) * mA + (point.getInhomY() +
544 // mB * amount) * mB + mC = 0
545
546 final var amount = -(point.getHomX() * a + point.getHomY() * b + point.getHomW() * c)
547 / (point.getHomW() * (a * a + b * b));
548 result.setHomogeneousCoordinates(point.getHomX() + a * amount * point.getHomW(),
549 point.getHomY() + b * amount * point.getHomW(), point.getHomW());
550 result.normalize();
551 }
552
553 /**
554 * Returns parameters of this line as an array containing [a, b, c].
555 *
556 * @return Array containing all the parameters that describe this line.
557 */
558 public double[] asArray() {
559 final var array = new double[LINE_NUMBER_PARAMS];
560 asArray(array);
561 return array;
562 }
563
564 /**
565 * Stores the parameters of this line in provided array as [a, b, c].
566 *
567 * @param array Array where parameters of this line will be stored.
568 * @throws IllegalArgumentException Raised if provided array doesn't have
569 * length 3.
570 */
571 public void asArray(final double[] array) {
572 if (array.length != LINE_NUMBER_PARAMS) {
573 throw new IllegalArgumentException();
574 }
575
576 array[0] = a;
577 array[1] = b;
578 array[2] = c;
579 }
580
581 /**
582 * Normalizes the parameters of this line to increase the accuracy of some
583 * computations.
584 */
585 @SuppressWarnings("DuplicatedCode")
586 public void normalize() {
587 if (!normalized) {
588 var norm = Math.sqrt(a * a + b * b + c * c);
589
590 if (norm > PRECISION) {
591 a /= norm;
592 b /= norm;
593 c /= norm;
594
595 normalized = true;
596 }
597 }
598 }
599
600 /**
601 * Returns boolean indicating whether this line has already been normalized.
602 *
603 * @return True if this line is normalized, false otherwise.
604 */
605 public boolean isNormalized() {
606 return normalized;
607 }
608
609 /**
610 * Returns director vector of this line.
611 *
612 * @return Director vector of this line.
613 */
614 public double[] getDirectorVector() {
615 final var out = new double[INHOM_VECTOR_SIZE];
616 directorVector(out);
617 return out;
618 }
619
620 /**
621 * Computes director vector of this line and stores the result in provided
622 * array.
623 *
624 * @param directorVector Array containing director vector.
625 * @throws IllegalArgumentException Raised if provided array does not have
626 * length 2.
627 */
628 public void directorVector(final double[] directorVector) {
629 if (directorVector.length != INHOM_VECTOR_SIZE) {
630 throw new IllegalArgumentException();
631 }
632
633 directorVector[0] = a;
634 directorVector[1] = b;
635 }
636
637 /**
638 * Computes the intersection of this line with provided line.
639 * Notice that parallel lines intersect at infinity.
640 *
641 * @param otherLine other line to be intersected with.
642 * @return A 2D point indicating containing the intersection.
643 * @throws NoIntersectionException if for numerical instabilities the
644 * intersection cannot be computed.
645 */
646 public Point2D getIntersection(final Line2D otherLine) throws NoIntersectionException {
647 final var result = Point2D.create();
648 intersection(otherLine, result);
649 return result;
650 }
651
652 /**
653 * Computes the intersection of this line with provided line.
654 * Notice that parallel lines intersect at infinity.
655 *
656 * @param otherLine other line to be intersected with.
657 * @param result 2D point where intersection will be stored. For greater
658 * accuracy it is recommended to use an HomogeneousPoint2D instance.
659 * @throws NoIntersectionException if for numerical instabilities the
660 * intersection cannot be computed.
661 */
662 public void intersection(final Line2D otherLine, final Point2D result) throws NoIntersectionException {
663
664 // normalize lines to increase accuracy
665 normalize();
666 otherLine.normalize();
667
668 // set matrix where each row contains the parameters of the line
669 try {
670 final var m = new Matrix(2, 3);
671 m.setElementAt(0, 0, a);
672 m.setElementAt(0, 1, b);
673 m.setElementAt(0, 2, c);
674
675 m.setElementAt(1, 0, otherLine.getA());
676 m.setElementAt(1, 1, otherLine.getB());
677 m.setElementAt(1, 2, otherLine.getC());
678
679 // If lines are not parallel, then matrix has rank 2, and its right
680 // null-space is equal to their intersection.
681 final var decomposer = new SingularValueDecomposer(m);
682 decomposer.decompose();
683
684 // lines are parallel
685 if (decomposer.getRank() < 2) {
686 throw new NoIntersectionException();
687 }
688
689 final var v = decomposer.getV();
690
691 // last column of V contains the right null-space of m, which is the
692 // intersection of lines expressed in homogeneous coordinates.
693 // because column is already normalized by SVD decomposition, point
694 // will also be normalized
695 result.setHomogeneousCoordinates(v.getElementAt(0, 2), v.getElementAt(1, 2),
696 v.getElementAt(2, 2));
697 } catch (final AlgebraException e) {
698 // lines are numerically unstable
699 throw new NoIntersectionException(e);
700 }
701 }
702
703 /**
704 * Computes the dot product between the parameters A, B,C of this line and
705 * the ones of provided line.
706 * This method normalizes both lines to compute dot product.
707 *
708 * @param line line to compute dot product with.
709 * @return dot product value.
710 */
711 public double dotProduct(final Line2D line) {
712 normalize();
713 line.normalize();
714 return a * line.a + b * line.b + c * line.c;
715 }
716
717 /**
718 * Checks if the line described by this instance equals provided line up to
719 * provided threshold.
720 *
721 * @param line line to be compared to.
722 * @param threshold threshold grade of tolerance to determine whether the
723 * lines are equal or not. It is used because due to machine precision,
724 * the values might not be exactly equal (if not provided
725 * DEFAULT_COMPARISON_THRESHOLD is used).
726 * @return true if current line and provided one are the same, false
727 * otherwise.
728 * @throws IllegalArgumentException if threshold is negative.
729 */
730 public boolean equals(final Line2D line, final double threshold) {
731 if (threshold < MIN_THRESHOLD) {
732 throw new IllegalArgumentException();
733 }
734
735 normalize();
736 line.normalize();
737
738 return (1.0 - Math.abs(dotProduct(line))) <= threshold;
739 }
740
741 /**
742 * Checks if the line described by this instance equals provided line up to
743 * default comparison threshold.
744 *
745 * @param line line to be compared to.
746 * @return true if current line and provided one are the same, false
747 * otherwise.
748 */
749 public boolean equals(final Line2D line) {
750 return equals(line, DEFAULT_COMPARISON_THRESHOLD);
751 }
752
753 /**
754 * Checks if provided object equals current line.
755 *
756 * @param obj object to compare.
757 * @return true if both objects are considered to be equal, false otherwise.
758 */
759 @Override
760 public boolean equals(final Object obj) {
761 if (!(obj instanceof Line2D line)) {
762 return false;
763 }
764 if (obj == this) {
765 return true;
766 }
767
768 return equals(line);
769 }
770
771 /**
772 * Returns hash code value. This is only defined to keep the compiler happy.
773 * This method must be overridden in subclasses of this class.
774 *
775 * @return Hash code
776 */
777 @Override
778 public int hashCode() {
779 return Objects.hash(a, b, c);
780 }
781
782 /**
783 * Creates a new instance of a 2D line located at the canonical infinity.
784 * The canonical infinity corresponds to all 2D points located at infinity
785 * (i.e. m = (x,y,w = 0), hence l = (A = 0, B = 0, C = 1))
786 *
787 * @return a new instance of a 2D line located at the canonical infinity.
788 */
789 public static Line2D createCanonicalLineAtInfinity() {
790 final var l = new Line2D();
791 setAsCanonicalLineAtInfinity(l);
792 return l;
793 }
794
795 /**
796 * Sets provided 2D line into the canonical infinity.
797 * The canonical infinity corresponds to all 2D points located at infinity
798 * (i.e. m = (x,y,w = 0), hence l = (A = 0, B = 0, C = 1)).
799 *
800 * @param line 2D line to be set at infinity.
801 */
802 public static void setAsCanonicalLineAtInfinity(final Line2D line) {
803 line.a = line.b = 0.0;
804 line.c = 1.0;
805 line.normalized = true;
806 }
807 }