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 import com.irurueta.algebra.WrongSizeException;
22
23 import java.io.Serializable;
24
25
26 /**
27 * This class contains the implementation of a conic.
28 */
29 @SuppressWarnings("DuplicatedCode")
30 public class Conic extends BaseConic implements Serializable {
31
32 /**
33 * Constructor.
34 */
35 public Conic() {
36 super();
37 }
38
39 /**
40 * Constructor of this class. This constructor accepts every parameter
41 * describing a conic (parameters a, b, c, d, e, f).
42 *
43 * @param a Parameter A of the conic.
44 * @param b Parameter B of the conic.
45 * @param c Parameter C of the conic.
46 * @param d Parameter D of the conic.
47 * @param e Parameter E of the conic.
48 * @param f Parameter F of the conic.
49 */
50 public Conic(final double a, final double b, final double c, final double d, final double e, final double f) {
51 super(a, b, c, d, e, f);
52 }
53
54 /**
55 * This method sets the matrix used to describe a conic.
56 * This matrix must be 3x3 and symmetric.
57 *
58 * @param m 3x3 Matrix describing the conic.
59 * @throws IllegalArgumentException Raised when the size of the matrix is
60 * not 3x3.
61 * @throws NonSymmetricMatrixException Raised when the conic matrix is not
62 * symmetric.
63 */
64 public Conic(final Matrix m) throws NonSymmetricMatrixException {
65 super(m);
66 }
67
68 /**
69 * Creates conic where provided points are contained (are locus).
70 *
71 * @param point1 1st point.
72 * @param point2 2nd point.
73 * @param point3 3rd point.
74 * @param point4 4th point.
75 * @param point5 5th point.
76 * @throws CoincidentPointsException Raised if points are coincident or
77 * produce a degenerated configuration.
78 */
79 public Conic(final Point2D point1, final Point2D point2, final Point2D point3, final Point2D point4,
80 final Point2D point5) throws CoincidentPointsException {
81 setParametersFromPoints(point1, point2, point3, point4, point5);
82 }
83
84 /**
85 * Checks if the given point is locus (lies within) this conic.
86 *
87 * @param point Point to be checked.
88 * @param threshold Threshold of distance to determine whether the
89 * point is locus of the conic or not. Threshold might be needed because of
90 * machine precision issues. If not provided DEFAULT_LOCUS_THRESHOLD will be
91 * used instead.
92 * @return True if the point lies within this conic, false otherwise.
93 * @throws IllegalArgumentException Raised if threshold is negative.
94 */
95 public boolean isLocus(final Point2D point, final double threshold) {
96 if (threshold < MIN_THRESHOLD) {
97 throw new IllegalArgumentException();
98 }
99
100 try {
101 normalize();
102 final var c = asMatrix();
103 final var homPoint = new Matrix(Point2D.POINT2D_HOMOGENEOUS_COORDINATES_LENGTH, 1);
104 point.normalize();
105 homPoint.setElementAt(0, 0, point.getHomX());
106 homPoint.setElementAt(1, 0, point.getHomY());
107 homPoint.setElementAt(2, 0, point.getHomW());
108 final var locusMatrix = homPoint.transposeAndReturnNew();
109 locusMatrix.multiply(c);
110 locusMatrix.multiply(homPoint);
111
112 return Math.abs(locusMatrix.getElementAt(0, 0)) < threshold;
113 } catch (final WrongSizeException ignore) {
114 return false;
115 }
116 }
117
118 /**
119 * Checks if the given point is locus (lies within) this conic.
120 *
121 * @param point Point to be checked.
122 * @return True if the point lies within this conic, false otherwise.
123 * @see #isLocus(Point2D, double)
124 */
125 public boolean isLocus(final Point2D point) {
126 return isLocus(point, DEFAULT_LOCUS_THRESHOLD);
127 }
128
129 /**
130 * Computes the angle between two 2D points using this conic as a geometry
131 * base.
132 *
133 * @param pointA First point.
134 * @param pointB Second point.
135 * @return Angle between provided points given in radians..
136 */
137 public double angleBetweenPoints(final Point2D pointA, final Point2D pointB) {
138 try {
139 // retrieve conic as matrix
140 normalize();
141 final var c = asMatrix();
142 final var transHomPointA = new Matrix(1, Point2D.POINT2D_HOMOGENEOUS_COORDINATES_LENGTH);
143 pointA.normalize();
144 transHomPointA.setElementAt(0, 0, pointA.getHomX());
145 transHomPointA.setElementAt(0, 1, pointA.getHomY());
146 transHomPointA.setElementAt(0, 2, pointA.getHomW());
147
148 final var tmp = transHomPointA.multiplyAndReturnNew(c);
149 tmp.multiply(transHomPointA.transposeAndReturnNew()); //This is
150 // homPointA' * C * homPointA
151
152 final var normA = tmp.getElementAt(0, 0);
153
154 final var homPointB = new Matrix(Point2D.POINT2D_HOMOGENEOUS_COORDINATES_LENGTH, 1);
155 pointB.normalize();
156 homPointB.setElementAt(0, 0, pointB.getHomX());
157 homPointB.setElementAt(1, 0, pointB.getHomY());
158 homPointB.setElementAt(2, 0, pointB.getHomW());
159
160 homPointB.transpose(tmp);
161 tmp.multiply(c);
162 tmp.multiply(homPointB);
163
164 final var normB = tmp.getElementAt(0, 0);
165
166 transHomPointA.multiply(c);
167 transHomPointA.multiply(homPointB);
168 // This is homPointA' * C * homPointB
169
170 final var angleNumerator = transHomPointA.getElementAt(0, 0);
171
172 final var cosTheta = angleNumerator / Math.sqrt(normA * normB);
173 return Math.acos(cosTheta);
174 } catch (final WrongSizeException ignore) {
175 // This will never happen
176 return 0.0;
177 }
178 }
179
180 /**
181 * Checks if two points are perpendicular in the geometry base generated by
182 * this conic.
183 *
184 * @param pointA First point.
185 * @param pointB Second point.
186 * @param threshold Threshold to determine whether the points are
187 * perpendicular or not. If the dot product between provided points and this
188 * conic is greater than provided threshold, then points won't be assumed to
189 * be perpendicular. Threshold is provided because of machine precision
190 * limits, if not provided DEFAULT_PERPENDICULAR_THRESHOLD will be used
191 * instead.
192 * @return True if points are perpendicular, false otherwise.
193 * @throws IllegalArgumentException Raised if threshold is negative.
194 */
195 public boolean arePerpendicularPoints(final Point2D pointA, final Point2D pointB, final double threshold) {
196 try {
197 // retrieve conic as matrix
198 final var transHomPointA = new Matrix(1, Point2D.POINT2D_HOMOGENEOUS_COORDINATES_LENGTH);
199 pointA.normalize();
200 transHomPointA.setElementAt(0, 0, pointA.getHomX());
201 transHomPointA.setElementAt(0, 1, pointA.getHomY());
202 transHomPointA.setElementAt(0, 2, pointA.getHomW());
203
204 final var homPointB = new Matrix(Point2D.POINT2D_HOMOGENEOUS_COORDINATES_LENGTH, 1);
205 pointB.normalize();
206 homPointB.setElementAt(0, 0, pointB.getHomX());
207 homPointB.setElementAt(1, 0, pointB.getHomY());
208 homPointB.setElementAt(2, 0, pointB.getHomW());
209
210 normalize();
211 final var c = asMatrix();
212 transHomPointA.multiply(c);
213 transHomPointA.multiply(homPointB);
214 // This is homPointA' * C * homPointB
215
216 final var perpend = transHomPointA.getElementAt(0, 0);
217
218 return Math.abs(perpend) < threshold;
219 } catch (final WrongSizeException ignore) {
220 // This will never happen
221 return false;
222 }
223 }
224
225 /**
226 * Sets the values of the dual conic corresponding to this conic instance
227 * into provided dualConic instance.
228 * The dual conic is equal to the inverse of the conic matrix.
229 *
230 * @param dualConic Dual conic instance where the values of the dual conic
231 * of this conic instance will be stored.
232 * @throws DualConicNotAvailableException Raised if the dual conic does not
233 * exist because this conic instance is degenerate (its inverse cannot be
234 * computed).
235 */
236 public void dualConic(final DualConic dualConic) throws DualConicNotAvailableException {
237
238 final var conicMatrix = asMatrix();
239 try {
240 final var invMatrix = com.irurueta.algebra.Utils.inverse(conicMatrix);
241
242 // ensure that resulting matrix after inversion is symmetric
243 // by computing the mean of off-diagonal elements
244 final var a = invMatrix.getElementAt(0, 0);
245 final var b = 0.5 * (invMatrix.getElementAt(0, 1) + invMatrix.getElementAt(1, 0));
246 final var c = invMatrix.getElementAt(1, 1);
247 final var d = 0.5 * (invMatrix.getElementAt(0, 2) + invMatrix.getElementAt(2, 0));
248 final var e = 0.5 * (invMatrix.getElementAt(1, 2) + invMatrix.getElementAt(2, 1));
249 final var f = invMatrix.getElementAt(2, 2);
250 dualConic.setParameters(a, b, c, d, e, f);
251 } catch (final AlgebraException e) {
252 throw new DualConicNotAvailableException(e);
253 }
254 }
255
256 /**
257 * Computes the dual conic of this conic.
258 * The dual conic is equal to the inverse of the conic matrix.
259 *
260 * @return A new DualConic corresponding to the dual conic of this instance.
261 * @throws DualConicNotAvailableException Raised if the dual conic does not
262 * exist because this conic instance is degenerate (its inverse cannot be
263 * computed).
264 */
265 public DualConic getDualConic() throws DualConicNotAvailableException {
266 final var dualConic = new DualConic();
267 dualConic(dualConic);
268 return dualConic;
269 }
270
271 /**
272 * Returns the ConicType of this conic.
273 *
274 * @return A ConicType describing the type of this conic. It can be
275 * one of the following: ELLIPSE_CONIC_TYPE, CIRCLE_CONIC_TYPE,
276 * PARABOLA_CONIC_TYPE, HYPERBOLA_CONIC_TYPE and
277 * RECTANGULAR_HYPERBOLA_CONIC_TYPE.
278 */
279 public ConicType getConicType() {
280 // computes and evaluates the following expression: b^2 - 4ac
281 final var expression = (b * b) - (a * c);
282
283 if (expression < 0) {
284 if (a == c && b == 0) {
285 return ConicType.CIRCLE_CONIC_TYPE;
286 } else {
287 return ConicType.ELLIPSE_CONIC_TYPE;
288 }
289 } else if (expression == 0) {
290 return ConicType.PARABOLA_CONIC_TYPE;
291 } else {
292 // expression > 0
293 if ((a + c) == 0) {
294 return ConicType.RECTANGULAR_HYPERBOLA_CONIC_TYPE;
295 } else {
296 return ConicType.HYPERBOLA_CONIC_TYPE;
297 }
298 }
299 }
300
301 /**
302 * Sets parameters of this conic so that provided points lie within it (are
303 * locus).
304 *
305 * @param point1 1st point.
306 * @param point2 2nd point.
307 * @param point3 3rd point.
308 * @param point4 4th point.
309 * @param point5 5th point.
310 * @throws CoincidentPointsException Raised if points are coincident or
311 * produce a degenerated configuration.
312 */
313 public final void setParametersFromPoints(
314 final Point2D point1, final Point2D point2, final Point2D point3, final Point2D point4,
315 final Point2D point5) throws CoincidentPointsException {
316
317 // normalize points to increase accuracy
318 point1.normalize();
319 point2.normalize();
320 point3.normalize();
321 point4.normalize();
322 point5.normalize();
323
324 try {
325 // each point belonging to a conic follows equation:
326 // p' * C * p = 0 ==>
327 // x^2 + y^2 + w^2 + 2*x*y + 2*x*w + 2*y*w = 0
328 final var m = new Matrix(5, 6);
329 var x = point1.getHomX();
330 var y = point1.getHomY();
331 var w = point1.getHomW();
332 m.setElementAt(0, 0, x * x);
333 m.setElementAt(0, 1, 2.0 * x * y);
334 m.setElementAt(0, 2, y * y);
335 m.setElementAt(0, 3, 2.0 * x * w);
336 m.setElementAt(0, 4, 2.0 * y * w);
337 m.setElementAt(0, 5, w * w);
338 x = point2.getHomX();
339 y = point2.getHomY();
340 w = point2.getHomW();
341 m.setElementAt(1, 0, x * x);
342 m.setElementAt(1, 1, 2.0 * x * y);
343 m.setElementAt(1, 2, y * y);
344 m.setElementAt(1, 3, 2.0 * x * w);
345 m.setElementAt(1, 4, 2.0 * y * w);
346 m.setElementAt(1, 5, w * w);
347 x = point3.getHomX();
348 y = point3.getHomY();
349 w = point3.getHomW();
350 m.setElementAt(2, 0, x * x);
351 m.setElementAt(2, 1, 2.0 * x * y);
352 m.setElementAt(2, 2, y * y);
353 m.setElementAt(2, 3, 2.0 * x * w);
354 m.setElementAt(2, 4, 2.0 * y * w);
355 m.setElementAt(2, 5, w * w);
356 x = point4.getHomX();
357 y = point4.getHomY();
358 w = point4.getHomW();
359 m.setElementAt(3, 0, x * x);
360 m.setElementAt(3, 1, 2.0 * x * y);
361 m.setElementAt(3, 2, y * y);
362 m.setElementAt(3, 3, 2.0 * x * w);
363 m.setElementAt(3, 4, 2.0 * y * w);
364 m.setElementAt(3, 5, w * w);
365 x = point5.getHomX();
366 y = point5.getHomY();
367 w = point5.getHomW();
368 m.setElementAt(4, 0, x * x);
369 m.setElementAt(4, 1, 2.0 * x * y);
370 m.setElementAt(4, 2, y * y);
371 m.setElementAt(4, 3, 2.0 * x * w);
372 m.setElementAt(4, 4, 2.0 * y * w);
373 m.setElementAt(4, 5, w * w);
374
375 // normalize each row to increase accuracy
376 final var row = new double[6];
377 double rowNorm;
378
379 for (var j = 0; j < 5; j++) {
380 m.getSubmatrixAsArray(j, 0, j, 5, row);
381 rowNorm = com.irurueta.algebra.Utils.normF(row);
382 for (var i = 0; i < 6; i++) {
383 m.setElementAt(j, i, m.getElementAt(j, i) / rowNorm);
384 }
385 }
386
387 final var decomposer = new SingularValueDecomposer(m);
388 decomposer.decompose();
389
390 if (decomposer.getRank() < 5) {
391 throw new CoincidentPointsException();
392 }
393
394 // the right null-space of m contains the parameters a, b, c, d, e ,f
395 // of the conic
396 final var v = decomposer.getV();
397
398 final var a = v.getElementAt(0, 5);
399 final var b = v.getElementAt(1, 5);
400 final var c = v.getElementAt(2, 5);
401 final var d = v.getElementAt(3, 5);
402 final var e = v.getElementAt(4, 5);
403 final var f = v.getElementAt(5, 5);
404
405 setParameters(a, b, c, d, e, f);
406 } catch (final AlgebraException ex) {
407 throw new CoincidentPointsException(ex);
408 }
409 }
410
411 /**
412 * Returns a line tangent to this conic at provided point. Provided point
413 * must be locus of this conic, otherwise a NotLocusException will be thrown.
414 *
415 * @param point a locus point of this conic.
416 * @return A 2D line tangent to this conic at provided point.
417 * @throws NotLocusException if provided point is not locus of this conic up
418 * to DEFAULT_LOCUS_THRESHOLD.
419 */
420 public Line2D getTangentLineAt(final Point2D point) throws NotLocusException {
421 final var line = new Line2D();
422 tangentLineAt(point, line, DEFAULT_LOCUS_THRESHOLD);
423 return line;
424 }
425
426 /**
427 * Computes a line tangent to this conic at provided point. Provided point
428 * must be locus of this conic, otherwise a NotLocusException will be thrown.
429 *
430 * @param point a locus point of this conic.
431 * @param line instance of a 2D line where result will be stored.
432 * @param threshold threshold to determine if provided point is locus.
433 * @throws NotLocusException if provided point is not locus of this conic up
434 * to provided threshold.
435 * @throws IllegalArgumentException if provided threshold is negative.
436 */
437 public void tangentLineAt(final Point2D point, final Line2D line, final double threshold) throws NotLocusException {
438 if (!isLocus(point, threshold)) {
439 throw new NotLocusException();
440 }
441
442 point.normalize();
443 normalize();
444
445 final var c = asMatrix();
446
447 try {
448 final var p = new Matrix(Point2D.POINT2D_HOMOGENEOUS_COORDINATES_LENGTH, 1);
449 p.setElementAt(0, 0, point.getHomX());
450 p.setElementAt(1, 0, point.getHomY());
451 p.setElementAt(2, 0, point.getHomW());
452
453 c.multiply(p);
454 } catch (final WrongSizeException ignore) {
455 // never happens
456 }
457
458 line.setParameters(c.getElementAt(0, 0), c.getElementAt(1, 0),
459 c.getElementAt(2, 0));
460 }
461
462 /**
463 * Creates a canonical instance of the absolute conic in the metric stratum.
464 * The absolute conic in the metric stratum is the intersection of the
465 * absolute quadric with the plane at the infinity.
466 * Both the absolute conic and the dual absolute conic define orthogonality
467 * in the metric stratum, and in a purely metric stratum (i.e. when camera
468 * is correctly calibrated), their canonical value is equal to the identity.
469 *
470 * @return a canonical instance of the absolute conic.
471 */
472 public static Conic createCanonicalAbsoluteConic() {
473 return new Conic(1.0, 0.0, 1.0, 0.0, 0.0, 1.0);
474 }
475
476 //TODO: shortest distance of point to conic
477 //TODO: closest point to conic
478 //TODO: intersection of Line2D with Conic results in two points (page 9 PHD report.pdf)
479 }