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.Utils;
22
23 import java.io.Serializable;
24
25 /**
26 * This class defines a lines in 3D space.
27 * A line in 3D space is defined as the intersection of two non-parallel planes.
28 */
29 public class Line3D implements Serializable {
30
31 /**
32 * Positive threshold determine whether points lay inside (is locus) of a
33 * given line or not.
34 */
35 public static final double DEFAULT_LOCUS_THRESHOLD = 1e-12;
36
37 /**
38 * Minimum allowed threshold
39 */
40 public static final double MIN_THRESHOLD = 0.0;
41
42 /**
43 * Constant defining the size of vector that define the direction of a line
44 */
45 private static final int INHOM_VECTOR_SIZE = 3;
46
47 /**
48 * 1st plane forming this 3D line.
49 */
50 private Plane plane1;
51
52 /**
53 * 2nd plane forming this 3D line.
54 */
55 private Plane plane2;
56
57 /**
58 * Constructor.
59 * Sets planes intersecting into this 3D line.
60 *
61 * @param plane1 1st plane.
62 * @param plane2 2nd plane.
63 * @throws CoincidentPlanesException Raised if provided planes are
64 * coincident and hence never intersect. Notice that parallel planes are
65 * not coincident and they intersect at infinity.
66 */
67 public Line3D(final Plane plane1, final Plane plane2) throws CoincidentPlanesException {
68 setPlanes(plane1, plane2);
69 }
70
71 /**
72 * Constructor.
73 * Builds a 3D line passing through provided 2 points.
74 *
75 * @param point1 1st point.
76 * @param point2 2nd point.
77 * @throws CoincidentPointsException Raised if provided points are
78 * considered to be equal.
79 */
80 public Line3D(final Point3D point1, final Point3D point2) throws CoincidentPointsException {
81 setPlanesFromPoints(point1, point2);
82 }
83
84 /**
85 * Determines whether provided planes are coincident.
86 * Two planes are considered coincident if they are equal up to scale.
87 *
88 * @param plane1 1st plane.
89 * @param plane2 2nd plane.
90 * @return True if planes are coincident, false otherwise.
91 */
92 public static boolean areCoincidentPlanes(final Plane plane1, final Plane plane2) {
93 // normalize planes to increase accuracy
94 plane1.normalize();
95 plane2.normalize();
96
97 try {
98 final var m = new Matrix(2, Plane.PLANE_NUMBER_PARAMS);
99 m.setElementAt(0, 0, plane1.getA());
100 m.setElementAt(0, 1, plane1.getB());
101 m.setElementAt(0, 2, plane1.getC());
102 m.setElementAt(0, 3, plane1.getD());
103
104 m.setElementAt(1, 0, plane2.getA());
105 m.setElementAt(1, 1, plane2.getB());
106 m.setElementAt(1, 2, plane2.getC());
107 m.setElementAt(1, 3, plane2.getD());
108
109 // check that matrix rank is 2 (and its nullity is also 2), in that
110 // case its right null-space has dimension two, which is linear
111 // combination of two 3D points (a line in 3D), and line is defined.
112 // Note that rank cannot be greater than 2 because matrix m has only
113 // 2 rows.
114 // If rank is smaller than 2, then the 2 planes are parallel, and the
115 // null-space has at least dimension 3, which is a perpendicular
116 // plane or the whole space (Depending whether nullity is 3 or 4)
117 return Utils.rank(m) < 2;
118 } catch (final AlgebraException e) {
119 // if for numerical reasons it cannot be determined whether planes
120 // are parallel, it will be assumed the worst case, which is that
121 // planes are parallel, which is something common for very large
122 // values or planes close at infinity
123 return true;
124 }
125 }
126
127 /**
128 * Sets intersecting planes for this 3D line.
129 * The intersection of provided planes will determine this 3D line.
130 *
131 * @param plane1 1st plane.
132 * @param plane2 2nd plane.
133 * @throws CoincidentPlanesException Raised if provided planes are
134 * coincident. Notice that parallel planes are not coincident and they
135 * intersect at infinity.
136 */
137 public final void setPlanes(final Plane plane1, final Plane plane2) throws CoincidentPlanesException {
138 if (areCoincidentPlanes(plane1, plane2)) {
139 throw new CoincidentPlanesException();
140 }
141
142 this.plane1 = plane1;
143 this.plane2 = plane2;
144 }
145
146 /**
147 * Sets planes of this 3D line so that it passes through provided points.
148 *
149 * @param point1 1st point.
150 * @param point2 2nd point.
151 * @throws CoincidentPointsException Raised if provided points are equal
152 * and hence a line cannot be determined.
153 */
154 public final void setPlanesFromPoints(final Point3D point1, final Point3D point2) throws CoincidentPointsException {
155 // build matrix containing director vector on a row
156 try {
157 final var m = new Matrix(1, INHOM_VECTOR_SIZE);
158 m.setElementAt(0, 0, point2.getInhomX() - point1.getInhomX());
159 m.setElementAt(0, 1, point2.getInhomY() - point1.getInhomY());
160 m.setElementAt(0, 2, point2.getInhomZ() - point1.getInhomZ());
161
162 // m matrix will have rank 1 and nullity 2. The null-space will be
163 // formed by two vectors perpendicular to the director vector
164 final var decomposer = new SingularValueDecomposer(m);
165 decomposer.decompose();
166
167 // check that points are not coincident, and hence all the values of
168 // matrix m are not zero
169 if (decomposer.getRank() < 1) {
170 throw new CoincidentPointsException();
171 }
172
173 // last two columns of V contains director vectors of plane1 and
174 // plane2
175 final var v = decomposer.getV();
176
177 final var directorVector1 = v.getSubmatrixAsArray(0, 1, 2, 1);
178 final var directorVector2 = v.getSubmatrixAsArray(0, 2, 2, 2);
179
180 plane1 = new Plane(point1, directorVector1);
181 plane2 = new Plane(point1, directorVector2);
182 } catch (final AlgebraException e) {
183 throw new CoincidentPointsException(e);
184 }
185 }
186
187 /**
188 * Returns 1st plane determining this 3D line.
189 * The intersection of plane1 and plane2 determines this 3D line.
190 *
191 * @return 1st plane.
192 */
193 public Plane getPlane1() {
194 return plane1;
195 }
196
197 /**
198 * Returns 2nd plane determining this 3D line.
199 * The intersection of plane1 and plane2 determines this 3D line.
200 *
201 * @return 2nd plane.
202 */
203 public Plane getPlane2() {
204 return plane2;
205 }
206
207 /**
208 * Determines if provided point is locus of this 3D line up to provided
209 * threshold.
210 *
211 * @param point Point to be checked.
212 * @param threshold Threshold to determine if provided point is locus or
213 * not. This should usually be a small value.
214 * @return True if provided point belongs to this 3D line, false otherwise.
215 * @throws IllegalArgumentException Raised if provided threshold is negative.
216 */
217 public boolean isLocus(final Point3D point, final double threshold) {
218 return plane1.isLocus(point, threshold) && plane2.isLocus(point, threshold);
219 }
220
221 /**
222 * Raised if provided point is locus of this 3D line.
223 *
224 * @param point Point to be checked.
225 * @return True if provided point belongs to this 3D line, false otherwise.
226 */
227 public boolean isLocus(final Point3D point) {
228 return isLocus(point, DEFAULT_LOCUS_THRESHOLD);
229 }
230
231 /**
232 * Returns shortest distance of provided point to this 3D line.
233 * The shortest distance is obtained in perpendicular direction of this
234 * line.
235 *
236 * @param point Point to be checked.
237 * @return Shortest distance of provided point to this 3D line.
238 */
239 public double getDistance(final Point3D point) {
240 final var closestPoint = getClosestPoint(point);
241 return point.distanceTo(closestPoint);
242 }
243
244 /**
245 * Returns closest point belonging to this 3D line respect provided point.
246 *
247 * @param point Point to be checked.
248 * @return Closest point belonging to this 3D line respect provided point.
249 */
250 public Point3D getClosestPoint(final Point3D point) {
251 return getClosestPoint(point, DEFAULT_LOCUS_THRESHOLD);
252 }
253
254 /**
255 * Returns closest point belonging to this 3D line respect provided point
256 * up to provided threshold.
257 *
258 * @param point Point to be checked.
259 * @param threshold Threshold to determine the closest point.
260 * @return Closest point belonging to this 3D line respect provided point.
261 * @throws IllegalArgumentException Raised if provided threshold is negative.
262 */
263 public Point3D getClosestPoint(final Point3D point, final double threshold) {
264 final var result = Point3D.create();
265 closestPoint(point, result, threshold);
266 return result;
267 }
268
269 /**
270 * Computes closest point belonging to this 3D line respect provided point
271 * and stores the result in provided instance.
272 *
273 * @param point Point to be checked.
274 * @param result Instance where computed point will be stored.
275 */
276 public void closestPoint(final Point3D point, final Point3D result) {
277 closestPoint(point, result, DEFAULT_LOCUS_THRESHOLD);
278 }
279
280 /**
281 * Computes closest point belonging to this 3D line respect provided point
282 * up to provided threshold and stores the result in provided instance.
283 *
284 * @param point Point to be checked.
285 * @param result Instance where computed point will be stored.
286 * @param threshold Threshold to determine the closest point.
287 * @throws IllegalArgumentException Raised if provided threshold is negative.
288 */
289 public void closestPoint(final Point3D point, final Point3D result, final double threshold) {
290 if (threshold < MIN_THRESHOLD) {
291 throw new IllegalArgumentException();
292 }
293
294 // normalize to increase accuracy
295 point.normalize();
296
297 // compute director vector perpendicular to director vectors of plane1
298 // and plane2, and use it along provided point to set a 3rd plane.
299 // Using plane1, plane2 and the 3rd plane, we can get their intersection
300 // to obtain a point which will be locus of this line3 and will be
301 // located at shortest distance of provided point to this line3.
302
303 // This is a plane having as director vector this line 3D and passing
304 // through provided point
305 final var p = new Plane(point, getDirection());
306 try {
307 intersection(p, result);
308 } catch (final NoIntersectionException ignore) {
309 // never happens
310 }
311 }
312
313 /**
314 * Normalize the planes forming this 3D line.
315 */
316 public void normalize() {
317 plane1.normalize();
318 plane2.normalize();
319 }
320
321 /**
322 * Determines whether the planes forming this 3D line are normalized or not.
323 *
324 * @return True if planes forming this 3D line are normalized, false
325 * otherwise.
326 */
327 public boolean isNormalized() {
328 return plane1.isNormalized() && plane2.isNormalized();
329 }
330
331 /**
332 * Returns array containing vector that indicates the direction of this 3D
333 * line.
334 *
335 * @return Returns vector indicating the direction of this 3D line.
336 */
337 public double[] getDirection() {
338 try {
339 return Utils.crossProduct(plane1.getDirectorVector(), plane2.getDirectorVector());
340 } catch (final AlgebraException ignore) {
341 return null;
342 }
343 }
344
345 /**
346 * Returns point where provided point intersects this 3D line.
347 *
348 * @param plane Plane to intersect this 3D line.
349 * @return Point where provided point intersects this 3D line.
350 * @throws NoIntersectionException Raised if provided plane does not
351 * intersect this 3D line.
352 */
353 public Point3D getIntersection(final Plane plane) throws NoIntersectionException {
354 final var result = Point3D.create();
355 intersection(plane, result);
356 return result;
357 }
358
359 /**
360 * Computes point where provided point intersects this 3D line and stores
361 * the result in provided instance.
362 *
363 * @param plane Plane to intersect this 3D line.
364 * @param result Instance where computes point will be stored.
365 * @throws NoIntersectionException Raised if provided plane does not
366 * intersect this 3D line.
367 */
368 public void intersection(final Plane plane, final Point3D result) throws NoIntersectionException {
369 // use plane1, plane2 and provided plane to find an intersection
370 plane.intersection(plane1, plane2, result);
371 }
372 }