1 /* Copyright 2002-2026 CS GROUP
2 * Licensed to CS GROUP (CS) under one or more
3 * contributor license agreements. See the NOTICE file distributed with
4 * this work for additional information regarding copyright ownership.
5 * CS licenses this file to You under the Apache License, Version 2.0
6 * (the "License"); you may not use this file except in compliance with
7 * the License. You may obtain a copy of the License at
8 *
9 * http://www.apache.org/licenses/LICENSE-2.0
10 *
11 * Unless required by applicable law or agreed to in writing, software
12 * distributed under the License is distributed on an "AS IS" BASIS,
13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 * See the License for the specific language governing permissions and
15 * limitations under the License.
16 */
17 package org.orekit.ssa.collision.shorttermencounter.probability.twod;
18
19 import org.hipparchus.geometry.euclidean.threed.Vector3D;
20 import org.hipparchus.geometry.euclidean.twod.Vector2D;
21 import org.hipparchus.linear.Array2DRowRealMatrix;
22 import org.hipparchus.linear.EigenDecompositionSymmetric;
23 import org.hipparchus.linear.LUDecomposition;
24 import org.hipparchus.linear.RealMatrix;
25 import org.hipparchus.util.FastMath;
26 import org.hipparchus.util.MathUtils;
27 import org.orekit.errors.OrekitException;
28 import org.orekit.errors.OrekitMessages;
29 import org.orekit.frames.Frame;
30 import org.orekit.frames.LOF;
31 import org.orekit.frames.LOFType;
32 import org.orekit.frames.KinematicTransform;
33 import org.orekit.frames.StaticTransform;
34 import org.orekit.frames.Transform;
35 import org.orekit.frames.encounter.EncounterLOF;
36 import org.orekit.frames.encounter.EncounterLOFType;
37 import org.orekit.orbits.Orbit;
38 import org.orekit.orbits.OrbitParamsType;
39 import org.orekit.orbits.PositionAngleType;
40 import org.orekit.propagation.covariance.StateCovariance;
41 import org.orekit.time.AbsoluteDate;
42 import org.orekit.utils.PVCoordinates;
43
44 /**
45 * Defines the encounter between two collision object at time of closest approach assuming a short-term encounter model . It
46 * uses the given {@link EncounterLOFType encounter frame type} to define the encounter.
47 * <p>
48 * Both the primary and secondary collision object can be at the reference of the encounter frame, it is up to the user to
49 * choose.
50 * <p>
51 * The "reference" object is the object considered at the reference of the given encounter frame while the "other" object is
52 * the one <b>not placed</b> at the reference.
53 * <p>
54 * For example, if the user wants the primary to be at the reference of the default encounter frame, they will have to input
55 * data in the following manner:
56 * <pre>{@code
57 * final ShortTermEncounter2DDefinition encounter = new ShortTermEncounter2DDefinition(primaryOrbitAtTCA, primaryCovariance, primaryRadius, secondaryOrbitAtTCA, secondaryCovariance, secondaryRadius);
58 * }
59 * </pre>
60 * However, if the user wants to put the secondary at the reference and use the
61 * {@link org.orekit.frames.encounter.ValsecchiEncounterFrame Valsecchi encounter frame}, they will have to type :
62 * <pre>{@code
63 * final ShortTermEncounter2DDefinition encounter = new ShortTermEncounter2DDefinition(secondaryOrbitAtTCA, secondaryCovariance, secondaryRadius, primaryOrbitAtTCA, primaryCovariance, primaryRadius, EncounterLOFType.VALSECCHI_2003);
64 * }
65 * </pre>
66 * Note that in the current implementation, the shape of the collision objects is assumed to be a sphere.
67 *
68 * @author Vincent Cucchietti
69 * @since 12.0
70 */
71 public class ShortTermEncounter2DDefinition {
72
73 /** Default threshold below which values are considered equal to zero. */
74 private static final double DEFAULT_ZERO_THRESHOLD = 1e-15;
75
76 /** Default epsilon when checking covariance matrix symmetry. */
77 private static final double DEFAULT_SYMMETRY_EPSILON = 1e-8;
78
79 /**
80 * Time of closest approach.
81 * <p>
82 * Commonly called TCA.
83 */
84 private final AbsoluteDate tca;
85
86 /** Reference collision object at time of closest approach. */
87 private final Orbit referenceAtTCA;
88
89 /** Reference collision object covariance matrix in its respective RTN frame. */
90 private final StateCovariance referenceCovariance;
91
92 /** Other collision object at time of closest approach. */
93 private final Orbit otherAtTCA;
94
95 /** Other collision object covariance matrix in its respective RTN frame. */
96 private final StateCovariance otherCovariance;
97
98 /** Combined radius (m). */
99 private final double combinedRadius;
100
101 /** Encounter local orbital frame to use. */
102 private final EncounterLOF encounterFrame;
103
104 /**
105 * Constructor.
106 *
107 * @param referenceAtTCA reference collision object orbit at time of closest approach
108 * @param referenceCovariance reference collision object covariance matrix in its respective RTN frame
109 * @param referenceRadius reference collision's equivalent sphere radius
110 * @param otherAtTCA other collision object orbit at time of closest approach
111 * @param otherCovariance other collision object covariance matrix in its respective RTN frame
112 * @param otherRadius other collision's equivalent sphere radius
113 *
114 * @throws OrekitException If both collision object spacecraft state don't have the same definition date.
115 */
116 public ShortTermEncounter2DDefinition(final Orbit referenceAtTCA, final StateCovariance referenceCovariance,
117 final double referenceRadius, final Orbit otherAtTCA,
118 final StateCovariance otherCovariance, final double otherRadius) {
119 this(referenceAtTCA, referenceCovariance, otherAtTCA, otherCovariance, referenceRadius + otherRadius);
120 }
121
122 /**
123 * Constructor.
124 *
125 * @param referenceAtTCA reference collision object orbit at time of closest approach
126 * @param referenceCovariance reference collision object covariance matrix in its respective RTN frame
127 * @param otherAtTCA other collision object orbit at time of closest approach
128 * @param otherCovariance other collision object covariance matrix in its respective RTN frame
129 * @param combinedRadius combined radius (m)
130 *
131 * @throws OrekitException If both collision object spacecraft state don't have the same definition date.
132 */
133 public ShortTermEncounter2DDefinition(final Orbit referenceAtTCA, final StateCovariance referenceCovariance,
134 final Orbit otherAtTCA, final StateCovariance otherCovariance,
135 final double combinedRadius) {
136 this(referenceAtTCA, referenceCovariance, otherAtTCA, otherCovariance, combinedRadius, EncounterLOFType.DEFAULT,
137 1e-6);
138 }
139
140 /**
141 * Constructor.
142 *
143 * @param referenceAtTCA reference collision object orbit at time of closest approach
144 * @param referenceCovariance reference collision object covariance matrix in its respective RTN frame
145 * @param referenceRadius reference collision's equivalent sphere radius
146 * @param otherAtTCA other collision object orbit at time of closest approach
147 * @param otherCovariance other collision object covariance matrix in its respective RTN frame
148 * @param otherRadius other collision's equivalent sphere radius
149 * @param encounterFrameType type of encounter frame to use
150 * @param tcaTolerance tolerance on reference and other times of closest approach difference
151 *
152 * @throws OrekitException If both collision object spacecraft state don't have the same definition date.
153 */
154 public ShortTermEncounter2DDefinition(final Orbit referenceAtTCA, final StateCovariance referenceCovariance,
155 final double referenceRadius, final Orbit otherAtTCA,
156 final StateCovariance otherCovariance, final double otherRadius,
157 final EncounterLOFType encounterFrameType, final double tcaTolerance) {
158 this(referenceAtTCA, referenceCovariance, otherAtTCA, otherCovariance, referenceRadius + otherRadius,
159 encounterFrameType, tcaTolerance);
160 }
161
162 /**
163 * Constructor.
164 *
165 * @param referenceAtTCA reference collision object orbit at time of closest approach
166 * @param referenceCovariance reference collision object covariance matrix in its respective RTN frame
167 * @param otherAtTCA other collision object orbit at time of closest approach
168 * @param otherCovariance other collision object covariance matrix in its respective RTN frame
169 * @param combinedRadius combined radius (m)
170 * @param encounterFrameType type of encounter frame to use
171 * @param tcaTolerance tolerance on reference and other times of closest approach difference
172 *
173 * @throws OrekitException If both collision object spacecraft state don't have the same definition date.
174 */
175 public ShortTermEncounter2DDefinition(final Orbit referenceAtTCA, final StateCovariance referenceCovariance,
176 final Orbit otherAtTCA, final StateCovariance otherCovariance,
177 final double combinedRadius, final EncounterLOFType encounterFrameType,
178 final double tcaTolerance) {
179
180 if (referenceAtTCA.getDate().isCloseTo(otherAtTCA.getDate(), tcaTolerance)) {
181
182 this.tca = referenceAtTCA.getDate();
183
184 this.referenceAtTCA = referenceAtTCA;
185 this.referenceCovariance = referenceCovariance;
186
187 this.otherAtTCA = otherAtTCA;
188 this.otherCovariance = otherCovariance;
189
190 this.combinedRadius = combinedRadius;
191
192 this.encounterFrame = encounterFrameType.getFrame(otherAtTCA.getPVCoordinates());
193 } else {
194 throw new OrekitException(OrekitMessages.DIFFERENT_TIME_OF_CLOSEST_APPROACH);
195 }
196
197 }
198
199 /**
200 * Compute the squared Mahalanobis distance.
201 *
202 * @param xm other collision object projected xm position onto the collision plane in the rotated encounter frame
203 * @param ym other collision object projected ym position onto the collision plane in the rotated encounter frame
204 * @param sigmaX square root of the x-axis eigen value of the diagonalized combined covariance matrix projected onto the
205 * collision plane
206 * @param sigmaY square root of the y-axis eigen value of the diagonalized combined covariance matrix projected onto the
207 * collision plane
208 *
209 * @return squared Mahalanobis distance
210 */
211 public static double computeSquaredMahalanobisDistance(final double xm, final double ym,
212 final double sigmaX, final double sigmaY) {
213 final Vector2D position = new Vector2D(xm, ym);
214
215 final RealMatrix covariance = new Array2DRowRealMatrix(new double[][] {
216 { sigmaX * sigmaX, 0 },
217 { 0, sigmaY * sigmaY } });
218
219 return computeSquaredMahalanobisDistance(position, covariance);
220 }
221
222 /**
223 * Compute the squared Mahalanobis distance.
224 *
225 * @param otherPosition other collision object projected position onto the collision plane in the rotated encounter
226 * frame
227 * @param covarianceMatrix combined covariance matrix projected onto the collision plane and diagonalized
228 *
229 * @return squared Mahalanobis distance
230 */
231 public static double computeSquaredMahalanobisDistance(final Vector2D otherPosition, final RealMatrix covarianceMatrix) {
232
233 final RealMatrix covarianceMatrixInverse = new LUDecomposition(covarianceMatrix).getSolver().getInverse();
234
235 final RealMatrix otherPositionOnCollisionPlaneMatrix = new Array2DRowRealMatrix(otherPosition.toArray());
236
237 return otherPositionOnCollisionPlaneMatrix.transposeMultiply(
238 covarianceMatrixInverse.multiply(otherPositionOnCollisionPlaneMatrix)).getEntry(0, 0);
239 }
240
241 /**
242 * Compute the other collision position and velocity relative to the reference collision object. Expressed in the
243 * reference collision object inertial frame.
244 *
245 * @return other collision position and velocity relative to the reference collision object, expressed in the reference
246 * collision object inertial frame.
247 */
248 public PVCoordinates computeOtherRelativeToReferencePVInReferenceInertial() {
249
250 // Extract reference inertial frame
251 final Frame referenceInertial = referenceAtTCA.getFrame();
252
253 // Get PVCoordinates in the same frame
254 final PVCoordinates referencePV = referenceAtTCA.getPVCoordinates();
255 final KinematicTransform kinematicTransform = otherAtTCA.getFrame().getKinematicTransformTo(referenceInertial,
256 otherAtTCA.getDate());
257 final PVCoordinates otherPVInReferenceInertial = kinematicTransform.transformOnlyPV(otherAtTCA.getPVCoordinates());
258
259 // Create relative pv expressed in the reference inertial frame
260 final Vector3D relativePosition = otherPVInReferenceInertial.getPosition().subtract(referencePV.getPosition());
261 final Vector3D relativeVelocity = otherPVInReferenceInertial.getVelocity().subtract(referencePV.getVelocity());
262
263 return new PVCoordinates(relativePosition, relativeVelocity);
264 }
265
266 /**
267 * Compute the projection matrix from the reference collision object inertial frame to the collision plane.
268 * <p>
269 * Note that this matrix will only rotate from the reference collision object inertial frame to the encounter frame and
270 * project onto the collision plane, this is only a rotation.
271 * </p>
272 *
273 * @return projection matrix from the reference collision object inertial frame to the collision plane
274 */
275 public RealMatrix computeReferenceInertialToCollisionPlaneProjectionMatrix() {
276
277 // Create transform from reference inertial frame to encounter local orbital frame
278 final StaticTransform referenceInertialToEncounterFrameTransform =
279 StaticTransform.compose(tca, computeReferenceInertialToReferenceTNWTransform(),
280 computeReferenceTNWToEncounterFrameTransform());
281
282 // Create rotation matrix from reference inertial frame to encounter local orbital frame
283 final RealMatrix referenceInertialToEncounterFrameRotationMatrix = new Array2DRowRealMatrix(
284 referenceInertialToEncounterFrameTransform.getRotation().getMatrix());
285
286 // Create projection matrix from encounter frame to collision plane
287 final RealMatrix encounterFrameToCollisionPlaneProjectionMatrix = encounterFrame.computeProjectionMatrix();
288
289 // Create projection matrix from reference inertial frame to collision plane
290 return encounterFrameToCollisionPlaneProjectionMatrix.multiply(referenceInertialToEncounterFrameRotationMatrix);
291 }
292
293 /**
294 * Compute the combined covariance matrix diagonalized and projected onto the collision plane.
295 * <p>
296 * Diagonalize projected positional covariance matrix in a specific manner to have
297 * <var>σ<sub>xx</sub><sup>2</sup> ≤ σ<sub>yy</sub><sup>2</sup></var>.
298 *
299 * @return combined covariance matrix diagonalized and projected onto the collision plane
300 */
301 public RealMatrix computeProjectedAndDiagonalizedCombinedPositionalCovarianceMatrix() {
302 final RealMatrix covariance = computeProjectedCombinedPositionalCovarianceMatrix();
303 final EigenDecompositionSymmetric ed = new EigenDecompositionSymmetric(covariance, DEFAULT_SYMMETRY_EPSILON, false);
304 return ed.getD();
305 }
306
307 /**
308 * Compute the projected combined covariance matrix onto the collision plane.
309 *
310 * @return projected combined covariance matrix onto the collision plane
311 */
312 public RealMatrix computeProjectedCombinedPositionalCovarianceMatrix() {
313
314 // Compute the positional covariance in the encounter local orbital frame
315 final RealMatrix combinedPositionalCovarianceMatrixInEncounterFrame =
316 computeCombinedCovarianceInEncounterFrame().getMatrix().getSubMatrix(0, 2, 0, 2);
317
318 // Project it onto the collision plane
319 return encounterFrame.projectOntoCollisionPlane(combinedPositionalCovarianceMatrixInEncounterFrame);
320 }
321
322 /**
323 * Compute the combined covariance expressed in the encounter frame.
324 *
325 * @return combined covariance expressed in the encounter frame
326 */
327 public StateCovariance computeCombinedCovarianceInEncounterFrame() {
328 return computeCombinedCovarianceInReferenceTNW().changeCovarianceFrame(referenceAtTCA, encounterFrame);
329 }
330
331 /**
332 * Compute the other collision object {@link Vector2D position} projected onto the collision plane.
333 *
334 * @return other collision object position projected onto the collision plane
335 */
336 public Vector2D computeOtherPositionInCollisionPlane() {
337
338 // Express other in reference inertial
339 final Vector3D otherInReferenceInertial = otherAtTCA.getPosition(referenceAtTCA.getFrame());
340
341 // Express other in reference TNW local orbital frame
342 final Vector3D otherPositionInReferenceTNW =
343 computeReferenceInertialToReferenceTNWTransform().transformPosition(otherInReferenceInertial);
344
345 // Express other in encounter local orbital frame
346 final Vector3D otherPositionInEncounterFrame =
347 computeReferenceTNWToEncounterFrameTransform().transformPosition(otherPositionInReferenceTNW);
348
349 return encounterFrame.projectOntoCollisionPlane(otherPositionInEncounterFrame);
350
351 }
352
353 /**
354 * Compute the other collision object {@link Vector2D position} in the rotated collision plane.
355 * <p>
356 * Uses a default zero threshold of 1e-15.
357 * <p>
358 * The coordinates are often noted xm and ym in probability of collision related papers.
359 * </p>
360 * <p>
361 * The mentioned rotation concerns the rotation that diagonalize the combined covariance matrix inside the collision
362 * plane.
363 * </p>
364 *
365 * @return other collision object position in the rotated collision plane
366 */
367 public Vector2D computeOtherPositionInRotatedCollisionPlane() {
368 return computeOtherPositionInRotatedCollisionPlane(DEFAULT_ZERO_THRESHOLD);
369
370 }
371
372 /**
373 * Compute the other collision object {@link Vector2D position} in the rotated collision plane.
374 * <p>
375 * The coordinates are often noted xm and ym in probability of collision related papers.
376 * <p>
377 * The mentioned rotation concerns the rotation that diagonalize the combined covariance matrix inside the collision
378 * plane.
379 *
380 * @param zeroThreshold threshold below which values are considered equal to zero
381 *
382 * @return other collision object position in the rotated collision plane
383 */
384 public Vector2D computeOtherPositionInRotatedCollisionPlane(final double zeroThreshold) {
385
386 // Project the other position onto the collision plane
387 final RealMatrix otherPositionInCollisionPlaneMatrix =
388 new Array2DRowRealMatrix(computeOtherPositionInCollisionPlane().toArray());
389
390 // Express other in the rotated collision plane
391 final RealMatrix otherPositionRotatedInCollisionPlane =
392 computeEncounterPlaneRotationMatrix(zeroThreshold).multiply(otherPositionInCollisionPlaneMatrix);
393
394 return new Vector2D(otherPositionRotatedInCollisionPlane.getColumn(0));
395
396 }
397
398 /**
399 * Compute the Encounter duration (s) evaluated using Coppola's formula described in : "COPPOLA, Vincent, et al.
400 * Evaluating the short encounter assumption of the probability of collision formula. 2012."
401 * <p>
402 * This method is to be used to check the validity of the short-term encounter model. The user is expected to compare the
403 * computed duration with the orbital period from both objects and draw its own conclusions.
404 * <p>
405 * It uses γ = 1e-16 as the resolution of a double is nearly 1e-16 so γ smaller than that are not meaningful to compute.
406 *
407 * @return encounter duration (s) evaluated using Coppola's formula
408 */
409 public double computeCoppolaEncounterDuration() {
410
411 // Default value for γ = 1e-16
412 final double DEFAULT_ALPHA_C = 5.864;
413
414 final RealMatrix combinedPositionalCovarianceMatrix = computeCombinedCovarianceInEncounterFrame()
415 .getMatrix().getSubMatrix(0, 2, 0, 2);
416
417 // Extract off-plane cross-term matrix
418 final RealMatrix projectionMatrix = encounterFrame.computeProjectionMatrix();
419 final RealMatrix axisNormalToCollisionPlane =
420 new Array2DRowRealMatrix(encounterFrame.getAxisNormalToCollisionPlane().toArray());
421 final RealMatrix offPlaneCrossTermMatrix =
422 projectionMatrix.multiply(combinedPositionalCovarianceMatrix.multiply(axisNormalToCollisionPlane));
423
424 // Covariance sub-matrix of the in-plane terms
425 final RealMatrix probabilityDensity =
426 encounterFrame.projectOntoCollisionPlane(combinedPositionalCovarianceMatrix);
427 final RealMatrix probabilityDensityInverse =
428 new LUDecomposition(probabilityDensity).getSolver().getInverse();
429
430 // Recurrent term in Coppola's paper : bᵀb
431 final RealMatrix b = offPlaneCrossTermMatrix.transposeMultiply(probabilityDensityInverse).transpose();
432 final double recurrentTerm = b.multiplyTransposed(b).getEntry(0, 0);
433
434 // Position uncertainty normal to collision plane
435 final double sigmaSqNormalToPlan = axisNormalToCollisionPlane.transposeMultiply(
436 combinedPositionalCovarianceMatrix.multiply(axisNormalToCollisionPlane)).getEntry(0, 0);
437 final double sigmaV = FastMath.sqrt(
438 sigmaSqNormalToPlan - b.multiplyTransposed(offPlaneCrossTermMatrix).getEntry(0, 0));
439
440 final double relativeVelocity = computeOtherRelativeToReferencePVInReferenceInertial().getVelocity().getNorm();
441
442 return (2 * FastMath.sqrt(2) * DEFAULT_ALPHA_C * sigmaV + combinedRadius * (
443 FastMath.sqrt(1 + recurrentTerm) + FastMath.sqrt(recurrentTerm))) / relativeVelocity;
444 }
445
446 /**
447 * Compute the miss distance at time of closest approach.
448 *
449 * @return miss distance
450 */
451 public double computeMissDistance() {
452
453 // Get positions expressed in the same frame at time of closest approach
454 final Vector3D referencePositionAtTCA = referenceAtTCA.getPosition();
455 final Vector3D otherPositionAtTCA = otherAtTCA.getPosition(referenceAtTCA.getFrame());
456
457 // Compute relative position
458 final Vector3D relativePosition = otherPositionAtTCA.subtract(referencePositionAtTCA);
459
460 return relativePosition.getNorm();
461 }
462
463 /**
464 * Compute the Mahalanobis distance computed with the other collision object projected onto the collision plane (commonly
465 * called B-Plane) and expressed in the rotated encounter frame (frame in which the combined covariance matrix is
466 * diagonalized, see {@link #computeEncounterPlaneRotationMatrix(double)} for more details).
467 * <p>
468 * Uses a default zero threshold of 1e-15 for the computation of the diagonalizing of the projected covariance matrix.
469 *
470 * @return Mahalanobis distance between the reference and other collision object
471 *
472 * @see <a href="https://en.wikipedia.org/wiki/Mahalanobis_distance">Mahalanobis distance</a>
473 */
474 public double computeMahalanobisDistance() {
475 return computeMahalanobisDistance(DEFAULT_ZERO_THRESHOLD);
476 }
477
478 /**
479 * Compute the Mahalanobis distance computed with the other collision object projected onto the collision plane (commonly
480 * called B-Plane) and expressed in the rotated encounter frame (frame in which the combined covariance matrix is
481 * diagonalized, see {@link #computeEncounterPlaneRotationMatrix(double)} for more details).
482 *
483 * @param zeroThreshold threshold below which values are considered equal to zero
484 *
485 * @return Mahalanobis distance between the reference and other collision object
486 *
487 * @see <a href="https://en.wikipedia.org/wiki/Mahalanobis_distance">Mahalanobis distance</a>
488 */
489 public double computeMahalanobisDistance(final double zeroThreshold) {
490 return FastMath.sqrt(computeSquaredMahalanobisDistance(zeroThreshold));
491 }
492
493 /**
494 * Compute the squared Mahalanobis distance computed with the other collision object projected onto the collision plane
495 * (commonly called B-Plane) and expressed in the rotated encounter frame (frame in which the combined covariance matrix
496 * is diagonalized, see {@link #computeEncounterPlaneRotationMatrix(double)} for more details).
497 * <p>
498 * Uses a default zero threshold of 1e-15 for the computation of the diagonalizing of the projected covariance matrix.
499 *
500 * @return squared Mahalanobis distance between the reference and other collision object
501 *
502 * @see <a href="https://en.wikipedia.org/wiki/Mahalanobis_distance">Mahalanobis distance</a>
503 */
504 public double computeSquaredMahalanobisDistance() {
505 return computeSquaredMahalanobisDistance(DEFAULT_ZERO_THRESHOLD);
506 }
507
508 /**
509 * Compute the squared Mahalanobis distance computed with the other collision object projected onto the collision plane
510 * (commonly called B-Plane) and expressed in the rotated encounter frame (frame in which the combined covariance matrix
511 * is diagonalized, see {@link #computeEncounterPlaneRotationMatrix(double)} for more details).
512 *
513 * @param zeroThreshold threshold below which values are considered equal to zero
514 *
515 * @return squared Mahalanobis distance between the reference and other collision object
516 *
517 * @see <a href="https://en.wikipedia.org/wiki/Mahalanobis_distance">Mahalanobis distance</a>
518 */
519 public double computeSquaredMahalanobisDistance(final double zeroThreshold) {
520
521 final RealMatrix otherPositionAfterRotationInCollisionPlane =
522 new Array2DRowRealMatrix(computeOtherPositionInRotatedCollisionPlane(zeroThreshold).toArray());
523
524 final RealMatrix inverseCovarianceMatrix =
525 new LUDecomposition(computeProjectedAndDiagonalizedCombinedPositionalCovarianceMatrix()).getSolver()
526 .getInverse();
527
528 return otherPositionAfterRotationInCollisionPlane.transpose().multiply(
529 inverseCovarianceMatrix.multiply(otherPositionAfterRotationInCollisionPlane)).getEntry(0, 0);
530 }
531
532 /**
533 * Takes both covariance matrices (expressed in their respective RTN local orbital frame) from reference and other
534 * collision object with which this instance was created and sum them in the reference collision object TNW local orbital
535 * frame.
536 *
537 * @return combined covariance matrix expressed in the reference collision object TNW local orbital frame
538 */
539 public StateCovariance computeCombinedCovarianceInReferenceTNW() {
540
541 // Express reference covariance in reference TNW local orbital frame
542 final RealMatrix referenceCovarianceMatrixInTNW =
543 referenceCovariance.changeCovarianceFrame(referenceAtTCA, LOFType.TNW_INERTIAL).getMatrix();
544
545 // Express other covariance in reference inertial frame
546 final RealMatrix otherCovarianceMatrixInReferenceInertial =
547 otherCovariance.changeCovarianceFrame(otherAtTCA, referenceAtTCA.getFrame()).getMatrix();
548
549 final StateCovariance otherCovarianceInReferenceInertial = new StateCovariance(
550 otherCovarianceMatrixInReferenceInertial, tca, referenceAtTCA.getFrame(),
551 OrbitParamsType.CARTESIAN, PositionAngleType.MEAN);
552
553 // Express other covariance in reference TNW local orbital frame
554 final RealMatrix otherCovarianceMatrixInReferenceTNW = otherCovarianceInReferenceInertial.changeCovarianceFrame(
555 referenceAtTCA, LOFType.TNW_INERTIAL).getMatrix();
556
557 // Return the combined covariance expressed in the reference TNW local orbital frame
558 return new StateCovariance(referenceCovarianceMatrixInTNW.add(otherCovarianceMatrixInReferenceTNW), tca,
559 LOFType.TNW_INERTIAL);
560 }
561
562 /**
563 * Compute the {@link Transform transform} from the reference collision object inertial frame of reference to its TNW
564 * local orbital frame.
565 *
566 * @return transform from the reference collision object inertial frame of reference to its TNW local orbital frame
567 */
568 private Transform computeReferenceInertialToReferenceTNWTransform() {
569 return LOFType.TNW.transformFromInertial(tca, referenceAtTCA.getPVCoordinates());
570 }
571
572 /**
573 * Compute the {@link Transform transform} from the reference collision object TNW local orbital frame to the encounter
574 * frame.
575 *
576 * @return transform from the reference collision object TNW local orbital frame to the encounter frame
577 */
578 private Transform computeReferenceTNWToEncounterFrameTransform() {
579 return LOF.transformFromLOFInToLOFOut(LOFType.TNW_INERTIAL, encounterFrame, tca,
580 referenceAtTCA.getPVCoordinates());
581 }
582
583 /**
584 * Compute the rotation matrix that diagonalize the combined positional covariance matrix projected onto the collision
585 * plane.
586 *
587 * @param zeroThreshold threshold below which values are considered equal to zero
588 *
589 * @return rotation matrix that diagonalize the combined covariance matrix projected onto the collision plane
590 */
591 private RealMatrix computeEncounterPlaneRotationMatrix(final double zeroThreshold) {
592
593 final RealMatrix combinedCovarianceMatrixInEncounterFrame =
594 computeCombinedCovarianceInEncounterFrame().getMatrix();
595
596 final RealMatrix combinedPositionalCovarianceMatrixProjectedOntoBPlane =
597 encounterFrame.projectOntoCollisionPlane(
598 combinedCovarianceMatrixInEncounterFrame.getSubMatrix(0, 2, 0, 2));
599
600 final double sigmaXSquared = combinedPositionalCovarianceMatrixProjectedOntoBPlane.getEntry(0, 0);
601 final double sigmaYSquared = combinedPositionalCovarianceMatrixProjectedOntoBPlane.getEntry(1, 1);
602 final double crossTerm = combinedPositionalCovarianceMatrixProjectedOntoBPlane.getEntry(0, 1);
603 final double correlation = crossTerm / (FastMath.sqrt(sigmaXSquared * sigmaYSquared));
604
605 // If the matrix is not initially diagonalized
606 final double theta;
607 if (FastMath.abs(crossTerm) > zeroThreshold) {
608 final double recurrentTerm = (sigmaYSquared - sigmaXSquared) / (2 * crossTerm);
609 theta = FastMath.atan(
610 recurrentTerm - FastMath.signum(correlation) * FastMath.sqrt(1 + FastMath.pow(recurrentTerm, 2)));
611 }
612 // Else, the matrix is already diagonalized
613 else {
614 // Rotation in order to have sigmaXSquared < sigmaYSquared
615 if (sigmaXSquared - sigmaYSquared > 0) {
616 theta = MathUtils.SEMI_PI;
617 }
618 // Else, there is no need for a rotation
619 else {
620 theta = 0;
621 }
622 }
623
624 final double[][] collisionPlaneRotationMatrixData = { { FastMath.cos(theta), FastMath.sin(theta) },
625 { -FastMath.sin(theta), FastMath.cos(theta) } };
626
627 return new Array2DRowRealMatrix(collisionPlaneRotationMatrixData);
628 }
629
630 /**
631 * Get the Time of Closest Approach.
632 * <p>
633 * Commonly called TCA.
634 *
635 * @return time of closest approach
636 */
637 public AbsoluteDate getTca() {
638 return tca;
639 }
640
641 /** Get reference's orbit at time of closest approach.
642 * @return reference's orbit at time of closest approach
643 */
644 public Orbit getReferenceAtTCA() {
645 return referenceAtTCA;
646 }
647
648 /** Get other's orbit at time of closest approach.
649 * @return other's orbit at time of closest approach
650 */
651 public Orbit getOtherAtTCA() {
652 return otherAtTCA;
653 }
654
655 /** Get reference's covariance.
656 * @return reference's covariance
657 */
658 public StateCovariance getReferenceCovariance() {
659 return referenceCovariance;
660 }
661
662 /** Get other's covariance.
663 * @return other's covariance
664 */
665 public StateCovariance getOtherCovariance() {
666 return otherCovariance;
667 }
668
669 /** Get combined radius.
670 * @return combined radius (m)
671 */
672 public double getCombinedRadius() {
673 return combinedRadius;
674 }
675
676 /** Get encounter local orbital frame.
677 * @return encounter local orbital frame
678 */
679 public EncounterLOF getEncounterFrame() {
680 return encounterFrame;
681 }
682
683 }