1 /* Copyright 2002-2025 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.CalculusFieldElement;
20 import org.hipparchus.Field;
21 import org.hipparchus.analysis.CalculusFieldUnivariateFunction;
22 import org.hipparchus.analysis.UnivariateFunction;
23 import org.hipparchus.analysis.integration.FieldUnivariateIntegrator;
24 import org.hipparchus.analysis.integration.TrapezoidIntegrator;
25 import org.hipparchus.analysis.integration.UnivariateIntegrator;
26 import org.hipparchus.util.FastMath;
27 import org.hipparchus.util.FieldSinCos;
28 import org.hipparchus.util.MathArrays;
29 import org.hipparchus.util.MathUtils;
30 import org.hipparchus.util.SinCos;
31 import org.orekit.ssa.metrics.FieldProbabilityOfCollision;
32 import org.orekit.ssa.metrics.ProbabilityOfCollision;
33
34 /**
35 * Compute the probability of collision using the method described in :"PATERA, Russell P. Calculating collision probability
36 * for arbitrary space vehicle shapes via numerical quadrature. Journal of guidance, control, and dynamics, 2005, vol. 28, no
37 * 6, p. 1326-1328.".
38 * <p>
39 * It is one of the recommended methods to use.
40 * <p>
41 * It assumes :
42 * <ul>
43 * <li>Short encounter leading to a linear relative motion.</li>
44 * <li>Spherical collision object (in this implementation only. The method could be used on non spherical object).</li>
45 * <li>Uncorrelated positional covariance.</li>
46 * <li>Gaussian distribution of the position uncertainties.</li>
47 * <li>Deterministic velocity i.e. no velocity uncertainties.</li>
48 * </ul>
49 * It has been rewritten to use Orekit specific inputs.
50 *
51 * @author Vincent Cucchietti
52 * @since 12.0
53 */
54 public class Patera2005 extends AbstractShortTermEncounter1DNumerical2DPOCMethod {
55
56 /** Default threshold defining if miss-distance and combined radius are considered equal (+- 10 cm). */
57 private static final double DEFAULT_EQUALITY_THRESHOLD = 1e-1;
58
59 /**
60 * Default constructor built with the following trapezoid integrator:
61 * <ul>
62 * <li>Minimal iteration count of 5</li>
63 * <li>Maximum iteration count of 50000</li>
64 * </ul>.
65 */
66 public Patera2005() {
67 this(new TrapezoidIntegrator(5, TrapezoidIntegrator.TRAPEZOID_MAX_ITERATIONS_COUNT), 50000);
68 }
69
70 /**
71 * Customizable constructor.
72 *
73 * @param integrator integrator
74 * @param maxNbOfEval max number of evaluation
75 */
76 public Patera2005(final UnivariateIntegrator integrator, final int maxNbOfEval) {
77 super("PATERA_2005", integrator, maxNbOfEval);
78 }
79
80 /** {@inheritDoc} */
81 @Override
82 public ProbabilityOfCollision compute(final double xm, final double ym,
83 final double sigmaX, final double sigmaY,
84 final double radius,
85 final UnivariateIntegrator integrator,
86 final int customMaxNbOfEval) {
87
88 // Depending on miss distance and the combined radius, three distinct cases exist
89 final double value;
90 final double missDistance = FastMath.sqrt(xm * xm + ym * ym);
91
92 // reference outside the hardbody area, first part of eq(11) is equal to 0
93 if (missDistance > radius + DEFAULT_EQUALITY_THRESHOLD) {
94 final CommonPateraFunction function = new CommonPateraFunction(xm, ym, sigmaX, sigmaY, radius);
95 value = -integrator.integrate(customMaxNbOfEval, function, 0, MathUtils.TWO_PI) / MathUtils.TWO_PI;
96
97 }
98
99 // reference within the hardbody area, first part of eq(11) is equal to 1
100 else if (missDistance < radius - DEFAULT_EQUALITY_THRESHOLD) {
101 final CommonPateraFunction function = new CommonPateraFunction(xm, ym, sigmaX, sigmaY, radius);
102 value = 1 - integrator.integrate(customMaxNbOfEval, function, 0, MathUtils.TWO_PI) / MathUtils.TWO_PI;
103 }
104
105 // Peculiar case where miss distance = combined radius, r may be equal to zero so eq(9) must be used
106 else {
107 final PateraFunctionSpecialCase function = new PateraFunctionSpecialCase(xm, ym, sigmaX, sigmaY, radius);
108 value = integrator.integrate(customMaxNbOfEval, function, 0, MathUtils.TWO_PI) / MathUtils.TWO_PI;
109 }
110
111 return new ProbabilityOfCollision(value, 0, 0, getName(), isAMaximumProbabilityOfCollisionMethod());
112 }
113
114 /** {@inheritDoc} */
115 @Override
116 public <T extends CalculusFieldElement<T>> FieldProbabilityOfCollision<T> compute(final T xm, final T ym,
117 final T sigmaX, final T sigmaY,
118 final T radius,
119 final FieldUnivariateIntegrator<T> customIntegrator,
120 final int customMaxNbOfEval) {
121 // Depending on miss distance and the combined radius, three distinct cases exist
122 final Field<T> field = xm.getField();
123 final T zero = field.getZero();
124 final T one = field.getOne();
125 final T twoPiField = one.newInstance(MathUtils.TWO_PI);
126
127 final T value;
128 final double missDistance = xm.square().add(ym.square()).sqrt().getReal();
129 final double radiusReal = radius.getReal();
130
131 // Reference outside the hardbody area, first part of eq(11) is equal to 0
132 if (missDistance > radiusReal + DEFAULT_EQUALITY_THRESHOLD) {
133 final CommonFieldPateraFunction<T> function =
134 new CommonFieldPateraFunction<>(xm, ym, sigmaX, sigmaY, radius);
135 value = customIntegrator.integrate(customMaxNbOfEval, function, zero, twoPiField).divide(twoPiField).negate();
136 }
137
138 // Reference within the hardbody area, first part of eq(11) is equal to 1
139 else if (missDistance < radiusReal - DEFAULT_EQUALITY_THRESHOLD) {
140 final CommonFieldPateraFunction<T> function =
141 new CommonFieldPateraFunction<>(xm, ym, sigmaX, sigmaY, radius);
142 value = one.subtract(
143 customIntegrator.integrate(customMaxNbOfEval, function, zero, twoPiField).divide(twoPiField));
144 }
145
146 // Peculiar case where miss distance = combined radius, r may be equal to zero so eq(9) must be used
147 else {
148 final FieldPateraFunctionSpecialCase<T> function =
149 new FieldPateraFunctionSpecialCase<>(xm, ym, sigmaX, sigmaY, radius);
150 value = customIntegrator.integrate(customMaxNbOfEval, function, zero, twoPiField).divide(twoPiField);
151 }
152
153 return new FieldProbabilityOfCollision<>(value, zero, zero, getName(), isAMaximumProbabilityOfCollisionMethod());
154 }
155
156 /** {@inheritDoc} */
157 @Override
158 public ShortTermEncounter2DPOCMethodType getType() {
159 return ShortTermEncounter2DPOCMethodType.PATERA_2005;
160 }
161
162 /** Commonly used function used in equation (11) in Patera's paper. */
163 private static class CommonPateraFunction extends AbstractPateraFunction {
164
165 /**
166 * Constructor.
167 *
168 * @param xm other collision object projected position onto the collision plane in the rotated encounter frame x-axis
169 * (m)
170 * @param ym other collision object projected position onto the collision plane in the rotated encounter frame y-axis
171 * (m)
172 * @param sigmaX square root of the smallest eigen value of the diagonalized combined covariance matrix projected
173 * onto the collision plane (m)
174 * @param sigmaY square root of the biggest eigen value of the diagonalized combined covariance matrix projected onto
175 * the collision plane (m)
176 * @param radius sum of primary and secondary collision object equivalent sphere radii (m)
177 */
178 private CommonPateraFunction(final double xm, final double ym, final double sigmaX, final double sigmaY,
179 final double radius) {
180 super(xm, ym, sigmaX, sigmaY, radius);
181 }
182
183 /** {@inheritDoc} */
184 @Override
185 public double value(final double xm, final double ym, final double scaleFactor, final double sigma,
186 final double radius, final double theta) {
187
188 final SinCos sinCosTheta = FastMath.sinCos(theta);
189 final double sinTheta = sinCosTheta.sin();
190 final double cosTheta = sinCosTheta.cos();
191
192 final double xPrime = getXPrime(cosTheta);
193 final double yPrime = getYPrime(sinTheta);
194 final double rSquared = getRSquared(xPrime, yPrime);
195
196 return FastMath.exp(-0.5 * rSquared / sigma / sigma) *
197 (radius * scaleFactor * MathArrays.linearCombination(xm, cosTheta, ym, sinTheta) +
198 scaleFactor * radius * radius) / rSquared;
199 }
200 }
201
202 /**
203 * Function used in the rare case where miss distance = combined radius. It represents equation (9) in Patera's paper but
204 * has been modified to be used with Orekit specific inputs.
205 */
206 private static class PateraFunctionSpecialCase extends AbstractPateraFunction {
207
208 /**
209 * Constructor.
210 *
211 * @param xm other collision object projected position onto the collision plane in the rotated encounter frame x-axis
212 * (m)
213 * @param ym other collision object projected position onto the collision plane in the rotated encounter frame y-axis
214 * (m)
215 * @param sigmaX square root of the smallest eigen value of the diagonalized combined covariance matrix projected
216 * onto the collision plane (m)
217 * @param sigmaY square root of the biggest eigen value of the diagonalized combined covariance matrix projected onto
218 * the collision plane (m)
219 * @param radius sum of primary and secondary collision object equivalent sphere radii (m)
220 */
221 private PateraFunctionSpecialCase(final double xm, final double ym, final double sigmaX,
222 final double sigmaY, final double radius) {
223 super(xm, ym, sigmaX, sigmaY, radius);
224 }
225
226 /** {@inheritDoc} */
227 @Override
228 public double value(final double xm, final double ym, final double scaleFactor, final double sigma,
229 final double radius, final double theta) {
230
231 final SinCos sinCosTheta = FastMath.sinCos(theta);
232 final double sinTheta = sinCosTheta.sin();
233 final double cosTheta = sinCosTheta.cos();
234
235 final double xPrime = getXPrime(cosTheta);
236 final double yPrime = getYPrime(sinTheta);
237
238 final double rSquared = getRSquared(xPrime, yPrime);
239 final double sigmaSquared = sigma * sigma;
240 final double oneOverTwoSigmaSq = 1. / (2 * sigmaSquared);
241 final double rSqOverTwoSigmaSq = oneOverTwoSigmaSq * rSquared;
242
243 return radius * scaleFactor * (MathArrays.linearCombination(xm, cosTheta, ym, sinTheta) + radius) *
244 oneOverTwoSigmaSq * (1 - rSqOverTwoSigmaSq * (0.5 - rSqOverTwoSigmaSq * (1. / 6 - rSqOverTwoSigmaSq * (
245 1. / 24 - rSqOverTwoSigmaSq / 720))));
246
247 }
248 }
249
250 /** Abstract class for different functions used in Patera's paper. */
251 private abstract static class AbstractPateraFunction implements UnivariateFunction {
252 /**
253 * Position on the x-axis of the rotated encounter frame.
254 */
255 private final double xm;
256
257 /**
258 * Position on the y-axis of the rotated encounter frame.
259 */
260 private final double ym;
261
262 /**
263 * Recurrent term used in Patera 2005 formula.
264 */
265 private final double scaleFactor;
266
267 /**
268 * General sigma after symmetrization.
269 */
270 private final double sigma;
271
272 /**
273 * Hardbody radius (m).
274 */
275 private final double radius;
276
277 /**
278 * Constructor.
279 *
280 * @param xm other collision object projected position onto the collision plane in the rotated encounter frame x-axis
281 * (m)
282 * @param ym other collision object projected position onto the collision plane in the rotated encounter frame y-axis
283 * (m)
284 * @param sigmaX square root of the smallest eigen value of the diagonalized combined covariance matrix projected
285 * onto the collision plane (m)
286 * @param sigmaY square root of the biggest eigen value of the diagonalized combined covariance matrix projected onto
287 * the collision plane (m)
288 * @param radius sum of primary and secondary collision object equivalent sphere radii (m)
289 */
290 AbstractPateraFunction(final double xm, final double ym, final double sigmaX, final double sigmaY,
291 final double radius) {
292 this.xm = xm;
293 this.ym = ym;
294 this.scaleFactor = sigmaY / sigmaX;
295 this.sigma = sigmaY;
296 this.radius = radius;
297 }
298
299 /**
300 * Compute the value of the function.
301 *
302 * @param theta angle at which the function value should be evaluated
303 *
304 * @return the value of the function
305 *
306 * @throws IllegalArgumentException when the activated method itself can ascertain that a precondition, specified in
307 * the API expressed at the level of the activated method, has been violated. When Hipparchus throws an
308 * {@code IllegalArgumentException}, it is usually the consequence of checking the actual parameters passed to the
309 * method.
310 */
311 public double value(final double theta) {
312 return value(xm, ym, scaleFactor, sigma, radius, theta);
313 }
314
315 /**
316 * Compute the value of the defined Patera function for input theta.
317 *
318 * @param x secondary collision object projected position onto the collision plane in the rotated encounter frame
319 * x-axis (m)
320 * @param y secondary collision object projected position onto the collision plane in the rotated encounter frame
321 * y-axis (m)
322 * @param scale scale factor used to symmetrize the probability density of collision, equal to sigmaY / sigmaX
323 * @param symmetrizedSigma symmetrized position sigma equal to sigmaY
324 * @param combinedRadius sum of primary and secondary collision object equivalent sphere radii (m)
325 * @param theta current angle of evaluation of the deformed hardbody radius (rad)
326 *
327 * @return value of the defined Patera function for input conjunction and theta
328 */
329 public abstract double value(double x, double y, double scale, double symmetrizedSigma, double combinedRadius,
330 double theta);
331
332 /**
333 * Get current x-axis component to the deformed hardbody perimeter.
334 *
335 * @param cosTheta cos of the angle determining deformed hardbody radius x-axis component to compute
336 *
337 * @return current x-axis component to the deformed hardbody perimeter
338 */
339 public double getXPrime(final double cosTheta) {
340 return scaleFactor * (xm + radius * cosTheta);
341 }
342
343 /**
344 * Get current y-axis component to the deformed hardbody perimeter.
345 *
346 * @param sinTheta sin of the angle determining deformed hardbody radius y-axis component to compute
347 *
348 * @return current y-axis component to the deformed hardbody perimeter
349 */
350 public double getYPrime(final double sinTheta) {
351 return ym + radius * sinTheta;
352 }
353
354 /**
355 * Get current distance from the reference to the determined hardbody perimeter.
356 *
357 * @param xPrime current x-axis component to the deformed hardbody perimeter
358 * @param yPrime current y-axis component to the deformed hardbody perimeter
359 *
360 * @return current distance from the reference to the determined hardbody perimeter
361 */
362 public double getRSquared(final double xPrime, final double yPrime) {
363 return xPrime * xPrime + yPrime * yPrime;
364 }
365 }
366
367 /** Commonly used function used in equation (11) in Patera's paper. */
368 private static class CommonFieldPateraFunction<T extends CalculusFieldElement<T>>
369 extends AbstractFieldPateraFunction<T> {
370
371 /**
372 * Constructor.
373 *
374 * @param xm other collision object projected position onto the collision plane in the rotated encounter frame x-axis
375 * (m)
376 * @param ym other collision object projected position onto the collision plane in the rotated encounter frame y-axis
377 * (m)
378 * @param sigmaX square root of the smallest eigen value of the diagonalized combined covariance matrix projected
379 * onto the collision plane (m)
380 * @param sigmaY square root of the biggest eigen value of the diagonalized combined covariance matrix projected onto
381 * the collision plane (m)
382 * @param radius sum of primary and secondary collision object equivalent sphere radii (m)
383 */
384 private CommonFieldPateraFunction(final T xm, final T ym, final T sigmaX, final T sigmaY,
385 final T radius) {
386 super(xm, ym, sigmaX, sigmaY, radius);
387 }
388
389 /** {@inheritDoc} */
390 @Override
391 public T value(final T xm, final T ym, final T scaleFactor, final T sigma,
392 final T radius, final T theta) {
393
394 final FieldSinCos<T> sinCosTheta = theta.sinCos();
395 final T sinTheta = sinCosTheta.sin();
396 final T cosTheta = sinCosTheta.cos();
397
398 final T xPrime = getXPrime(cosTheta);
399 final T yPrime = getYPrime(sinTheta);
400 final T rSquared = getRSquared(xPrime, yPrime);
401
402 return rSquared.divide(sigma.square()).multiply(-0.5).exp()
403 .multiply(radius.multiply(scaleFactor).multiply(xm.multiply(cosTheta)
404 .add(ym.multiply(sinTheta)))
405 .add(scaleFactor.multiply(radius.square()))).divide(rSquared);
406 }
407
408 }
409
410 /**
411 * Function used in the rare case where miss distance = combined radius. It represents equation (9) in Patera's paper but
412 * has been modified to be used with Orekit specific inputs.
413 */
414 private static class FieldPateraFunctionSpecialCase<T extends CalculusFieldElement<T>>
415 extends AbstractFieldPateraFunction<T> {
416
417 /**
418 * Constructor.
419 *
420 * @param xm other collision object projected position onto the collision plane in the rotated encounter frame x-axis
421 * (m)
422 * @param ym other collision object projected position onto the collision plane in the rotated encounter frame y-axis
423 * (m)
424 * @param sigmaX square root of the smallest eigen value of the diagonalized combined covariance matrix projected
425 * onto the collision plane (m)
426 * @param sigmaY square root of the biggest eigen value of the diagonalized combined covariance matrix projected onto
427 * the collision plane (m)
428 * @param radius sum of primary and secondary collision object equivalent sphere radii (m)
429 */
430 private FieldPateraFunctionSpecialCase(final T xm, final T ym, final T sigmaX,
431 final T sigmaY, final T radius) {
432 super(xm, ym, sigmaX, sigmaY, radius);
433 }
434
435 /** {@inheritDoc} */
436 @Override
437 public T value(final T xm, final T ym, final T scaleFactor, final T sigma,
438 final T radius, final T theta) {
439
440 final FieldSinCos<T> sinCosTheta = theta.sinCos();
441 final T sinTheta = sinCosTheta.sin();
442 final T cosTheta = sinCosTheta.cos();
443
444 final T xPrime = scaleFactor.multiply(xm).add(scaleFactor.multiply(radius).multiply(cosTheta));
445 final T yPrime = ym.add(radius.multiply(sinTheta));
446 final T rSquared = xPrime.square().add(yPrime.square());
447 final T sigmaSquared = sigma.multiply(sigma);
448 final T oneOverTwoSigmaSq = sigmaSquared.multiply(2.).reciprocal();
449 final T rSqOverTwoSigmaSq = rSquared.multiply(oneOverTwoSigmaSq);
450
451 // Recursive approach to maximize usage of the same fielded variables
452 return radius.multiply(scaleFactor).multiply(xm.multiply(cosTheta).add(ym.multiply(sinTheta)).add(radius))
453 .multiply(oneOverTwoSigmaSq.negate()
454 .multiply(rSqOverTwoSigmaSq
455 .multiply(rSqOverTwoSigmaSq
456 .multiply(rSqOverTwoSigmaSq
457 .multiply(
458 rSqOverTwoSigmaSq.multiply(
459 -1. / 720)
460 .add(1. / 24))
461 .subtract(1. / 6))
462 .add(0.5))
463 .subtract(1.)));
464 }
465 }
466
467 /** Abstract class for different functions used in Patera's paper. */
468 private abstract static class AbstractFieldPateraFunction<T extends CalculusFieldElement<T>> implements
469 CalculusFieldUnivariateFunction<T> {
470 /**
471 * Position on the x-axis of the rotated encounter frame.
472 */
473 private final T xm;
474
475 /**
476 * Position on the y-axis of the rotated encounter frame.
477 */
478 private final T ym;
479
480 /**
481 * Recurrent term used in Patera 2005 formula.
482 */
483 private final T scaleFactor;
484
485 /**
486 * General sigma after symmetrization.
487 */
488 private final T sigma;
489
490 /**
491 * Hardbody radius (m).
492 */
493 private final T radius;
494
495 /**
496 * Constructor.
497 *
498 * @param xm other collision object projected position onto the collision plane in the rotated encounter frame x-axis
499 * (m)
500 * @param ym other collision object projected position onto the collision plane in the rotated encounter frame y-axis
501 * (m)
502 * @param sigmaX square root of the smallest eigen value of the diagonalized combined covariance matrix projected
503 * onto the collision plane (m)
504 * @param sigmaY square root of the biggest eigen value of the diagonalized combined covariance matrix projected onto
505 * the collision plane (m)
506 * @param radius sum of primary and secondary collision object equivalent sphere radii (m)
507 */
508 AbstractFieldPateraFunction(final T xm, final T ym, final T sigmaX, final T sigmaY,
509 final T radius) {
510 this.xm = xm;
511 this.ym = ym;
512 this.scaleFactor = sigmaY.divide(sigmaX);
513 this.sigma = sigmaY;
514 this.radius = radius;
515 }
516
517 /**
518 * Compute the value of the function.
519 *
520 * @param theta angle at which the function value should be evaluated
521 *
522 * @return the value of the function
523 *
524 * @throws IllegalArgumentException when the activated method itself can ascertain that a precondition, specified in
525 * the API expressed at the level of the activated method, has been violated. When Hipparchus throws an
526 * {@code IllegalArgumentException}, it is usually the consequence of checking the actual parameters passed to the
527 * method.
528 */
529 @Override
530 public T value(final T theta) {
531 return value(xm, ym, scaleFactor, sigma, radius, theta);
532 }
533
534 /**
535 * Compute the value of the defined Patera function for input theta.
536 *
537 * @param x secondary collision object projected position onto the collision plane in the rotated encounter frame
538 * x-axis (m)
539 * @param y secondary collision object projected position onto the collision plane in the rotated encounter frame
540 * y-axis (m)
541 * @param scale scale factor used to symmetrize the probability density of collision, equal to sigmaY / sigmaX
542 * @param symmetrizedSigma symmetrized position sigma equal to sigmaY
543 * @param combinedRadius sum of primary and secondary collision object equivalent sphere radii (m)
544 * @param theta current angle of evaluation of the deformed hardbody radius (rad)
545 *
546 * @return value of the defined Patera function for input conjunction and theta
547 */
548 public abstract T value(T x, T y, T scale, T symmetrizedSigma, T combinedRadius,
549 T theta);
550
551 /**
552 * Get current x-axis component to the deformed hardbody perimeter.
553 *
554 * @param cosTheta cos of the angle determining deformed hardbody radius x-axis component to compute
555 *
556 * @return current x-axis component to the deformed hardbody perimeter
557 */
558 public T getXPrime(final T cosTheta) {
559 return scaleFactor.multiply(xm).add(scaleFactor.multiply(radius).multiply(cosTheta));
560 }
561
562 /**
563 * Get current y-axis component to the deformed hardbody perimeter.
564 *
565 * @param sinTheta sin of the angle determining deformed hardbody radius y-axis component to compute
566 *
567 * @return current y-axis component to the deformed hardbody perimeter
568 */
569 public T getYPrime(final T sinTheta) {
570 return ym.add(radius.multiply(sinTheta));
571 }
572
573 /**
574 * Get current distance from the reference to the determined hardbody perimeter.
575 *
576 * @param xPrime current x-axis component to the deformed hardbody perimeter
577 * @param yPrime current y-axis component to the deformed hardbody perimeter
578 *
579 * @return current distance from the reference to the determined hardbody perimeter
580 */
581 public T getRSquared(final T xPrime, final T yPrime) {
582 return xPrime.multiply(xPrime).add(yPrime.multiply(yPrime));
583 }
584 }
585
586 }