1 /* Copyright 2002-2016 CS Systèmes d'Information
2 * Licensed to CS Systèmes d'Information (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.orbits;
18
19 import java.io.Serializable;
20
21 import org.hipparchus.geometry.euclidean.threed.Vector3D;
22 import org.hipparchus.linear.DecompositionSolver;
23 import org.hipparchus.linear.MatrixUtils;
24 import org.hipparchus.linear.QRDecomposition;
25 import org.hipparchus.linear.RealMatrix;
26 import org.hipparchus.util.FastMath;
27 import org.hipparchus.util.MathArrays;
28 import org.orekit.errors.OrekitIllegalArgumentException;
29 import org.orekit.errors.OrekitException;
30 import org.orekit.errors.OrekitInternalError;
31 import org.orekit.errors.OrekitMessages;
32 import org.orekit.frames.Frame;
33 import org.orekit.frames.Transform;
34 import org.orekit.time.AbsoluteDate;
35 import org.orekit.time.TimeInterpolable;
36 import org.orekit.time.TimeShiftable;
37 import org.orekit.time.TimeStamped;
38 import org.orekit.utils.PVCoordinatesProvider;
39 import org.orekit.utils.TimeStampedPVCoordinates;
40
41 /**
42 * This class handles orbital parameters.
43
44 * <p>
45 * For user convenience, both the Cartesian and the equinoctial elements
46 * are provided by this class, regardless of the canonical representation
47 * implemented in the derived class (which may be classical keplerian
48 * elements for example).
49 * </p>
50 * <p>
51 * The parameters are defined in a frame specified by the user. It is important
52 * to make sure this frame is consistent: it probably is inertial and centered
53 * on the central body. This information is used for example by some
54 * force models.
55 * </p>
56 * <p>
57 * The object <code>OrbitalParameters</code> is guaranteed to be immutable.
58 * </p>
59 * @author Luc Maisonobe
60 * @author Guylaine Prat
61 * @author Fabien Maussion
62 * @author Véronique Pommier-Maurussane
63 */
64 public abstract class Orbit
65 implements TimeStamped, TimeShiftable<Orbit>, TimeInterpolable<Orbit>,
66 Serializable, PVCoordinatesProvider {
67
68 /** Serializable UID. */
69 private static final long serialVersionUID = 438733454597999578L;
70
71 /** Frame in which are defined the orbital parameters. */
72 private final Frame frame;
73
74 /** Date of the orbital parameters. */
75 private final AbsoluteDate date;
76
77 /** Value of mu used to compute position and velocity (m³/s²). */
78 private final double mu;
79
80 /** Computed PVCoordinates. */
81 private transient TimeStampedPVCoordinates pvCoordinates;
82
83 /** Jacobian of the orbital parameters with mean angle with respect to the Cartesian coordinates. */
84 private transient double[][] jacobianMeanWrtCartesian;
85
86 /** Jacobian of the Cartesian coordinates with respect to the orbital parameters with mean angle. */
87 private transient double[][] jacobianWrtParametersMean;
88
89 /** Jacobian of the orbital parameters with eccentric angle with respect to the Cartesian coordinates. */
90 private transient double[][] jacobianEccentricWrtCartesian;
91
92 /** Jacobian of the Cartesian coordinates with respect to the orbital parameters with eccentric angle. */
93 private transient double[][] jacobianWrtParametersEccentric;
94
95 /** Jacobian of the orbital parameters with true angle with respect to the Cartesian coordinates. */
96 private transient double[][] jacobianTrueWrtCartesian;
97
98 /** Jacobian of the Cartesian coordinates with respect to the orbital parameters with true angle. */
99 private transient double[][] jacobianWrtParametersTrue;
100
101 /** Default constructor.
102 * Build a new instance with arbitrary default elements.
103 * @param frame the frame in which the parameters are defined
104 * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
105 * @param date date of the orbital parameters
106 * @param mu central attraction coefficient (m^3/s^2)
107 * @exception IllegalArgumentException if frame is not a {@link
108 * Frame#isPseudoInertial pseudo-inertial frame}
109 */
110 protected Orbit(final Frame frame, final AbsoluteDate date, final double mu)
111 throws IllegalArgumentException {
112 ensurePseudoInertialFrame(frame);
113 this.date = date;
114 this.mu = mu;
115 this.pvCoordinates = null;
116 this.frame = frame;
117 jacobianMeanWrtCartesian = null;
118 jacobianWrtParametersMean = null;
119 jacobianEccentricWrtCartesian = null;
120 jacobianWrtParametersEccentric = null;
121 jacobianTrueWrtCartesian = null;
122 jacobianWrtParametersTrue = null;
123 }
124
125 /** Set the orbit from Cartesian parameters.
126 *
127 * <p> The acceleration provided in {@code pvCoordinates} is accessible using
128 * {@link #getPVCoordinates()} and {@link #getPVCoordinates(Frame)}. All other methods
129 * use {@code mu} and the position to compute the acceleration, including
130 * {@link #shiftedBy(double)} and {@link #getPVCoordinates(AbsoluteDate, Frame)}.
131 *
132 * @param pvCoordinates the position and velocity in the inertial frame
133 * @param frame the frame in which the {@link TimeStampedPVCoordinates} are defined
134 * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
135 * @param mu central attraction coefficient (m^3/s^2)
136 * @exception IllegalArgumentException if frame is not a {@link
137 * Frame#isPseudoInertial pseudo-inertial frame}
138 */
139 protected Orbit(final TimeStampedPVCoordinates pvCoordinates, final Frame frame, final double mu)
140 throws IllegalArgumentException {
141 ensurePseudoInertialFrame(frame);
142 this.date = pvCoordinates.getDate();
143 this.mu = mu;
144 if (pvCoordinates.getAcceleration().getNormSq() == 0) {
145 // the acceleration was not provided,
146 // compute it from Newtonian attraction
147 final double r2 = pvCoordinates.getPosition().getNormSq();
148 final double r3 = r2 * FastMath.sqrt(r2);
149 this.pvCoordinates = new TimeStampedPVCoordinates(pvCoordinates.getDate(),
150 pvCoordinates.getPosition(),
151 pvCoordinates.getVelocity(),
152 new Vector3D(-mu / r3, pvCoordinates.getPosition()));
153 } else {
154 this.pvCoordinates = pvCoordinates;
155 }
156 this.frame = frame;
157 }
158
159 /** Get the orbit type.
160 * @return orbit type
161 */
162 public abstract OrbitType getType();
163
164 /** Ensure the defining frame is a pseudo-inertial frame.
165 * @param frame frame to check
166 * @exception IllegalArgumentException if frame is not a {@link
167 * Frame#isPseudoInertial pseudo-inertial frame}
168 */
169 private static void ensurePseudoInertialFrame(final Frame frame)
170 throws IllegalArgumentException {
171 if (!frame.isPseudoInertial()) {
172 throw new OrekitIllegalArgumentException(OrekitMessages.NON_PSEUDO_INERTIAL_FRAME,
173 frame.getName());
174 }
175 }
176
177 /** Get the frame in which the orbital parameters are defined.
178 * @return frame in which the orbital parameters are defined
179 */
180 public Frame getFrame() {
181 return frame;
182 }
183
184 /** Get the semi-major axis.
185 * <p>Note that the semi-major axis is considered negative for hyperbolic orbits.</p>
186 * @return semi-major axis (m)
187 */
188 public abstract double getA();
189
190 /** Get the first component of the equinoctial eccentricity vector.
191 * @return first component of the equinoctial eccentricity vector
192 */
193 public abstract double getEquinoctialEx();
194
195 /** Get the second component of the equinoctial eccentricity vector.
196 * @return second component of the equinoctial eccentricity vector
197 */
198 public abstract double getEquinoctialEy();
199
200 /** Get the first component of the inclination vector.
201 * @return first component of the inclination vector
202 */
203 public abstract double getHx();
204
205 /** Get the second component of the inclination vector.
206 * @return second component of the inclination vector
207 */
208 public abstract double getHy();
209
210 /** Get the eccentric longitude argument.
211 * @return E + ω + Ω eccentric longitude argument (rad)
212 */
213 public abstract double getLE();
214
215 /** Get the true longitude argument.
216 * @return v + ω + Ω true longitude argument (rad)
217 */
218 public abstract double getLv();
219
220 /** Get the mean longitude argument.
221 * @return M + ω + Ω mean longitude argument (rad)
222 */
223 public abstract double getLM();
224
225 // Additional orbital elements
226
227 /** Get the eccentricity.
228 * @return eccentricity
229 */
230 public abstract double getE();
231
232 /** Get the inclination.
233 * @return inclination (rad)
234 */
235 public abstract double getI();
236
237 /** Get the central acceleration constant.
238 * @return central acceleration constant
239 */
240 public double getMu() {
241 return mu;
242 }
243
244 /** Get the keplerian period.
245 * <p>The keplerian period is computed directly from semi major axis
246 * and central acceleration constant.</p>
247 * @return keplerian period in seconds, or positive infinity for hyperbolic orbits
248 */
249 public double getKeplerianPeriod() {
250 final double a = getA();
251 return (a < 0) ? Double.POSITIVE_INFINITY : 2.0 * FastMath.PI * a * FastMath.sqrt(a / mu);
252 }
253
254 /** Get the keplerian mean motion.
255 * <p>The keplerian mean motion is computed directly from semi major axis
256 * and central acceleration constant.</p>
257 * @return keplerian mean motion in radians per second
258 */
259 public double getKeplerianMeanMotion() {
260 final double absA = FastMath.abs(getA());
261 return FastMath.sqrt(mu / absA) / absA;
262 }
263
264 /** Get the date of orbital parameters.
265 * @return date of the orbital parameters
266 */
267 public AbsoluteDate getDate() {
268 return date;
269 }
270
271 /** Get the {@link TimeStampedPVCoordinates} in a specified frame.
272 * @param outputFrame frame in which the position/velocity coordinates shall be computed
273 * @return pvCoordinates in the specified output frame
274 * @exception OrekitException if transformation between frames cannot be computed
275 * @see #getPVCoordinates()
276 */
277 public TimeStampedPVCoordinates getPVCoordinates(final Frame outputFrame)
278 throws OrekitException {
279 if (pvCoordinates == null) {
280 pvCoordinates = initPVCoordinates();
281 }
282
283 // If output frame requested is the same as definition frame,
284 // PV coordinates are returned directly
285 if (outputFrame == frame) {
286 return pvCoordinates;
287 }
288
289 // Else, PV coordinates are transformed to output frame
290 final Transform t = frame.getTransformTo(outputFrame, date);
291 return t.transformPVCoordinates(pvCoordinates);
292 }
293
294 /** {@inheritDoc} */
295 public TimeStampedPVCoordinates getPVCoordinates(final AbsoluteDate otherDate, final Frame otherFrame)
296 throws OrekitException {
297 return shiftedBy(otherDate.durationFrom(getDate())).getPVCoordinates(otherFrame);
298 }
299
300
301 /** Get the {@link TimeStampedPVCoordinates} in definition frame.
302 * @return pvCoordinates in the definition frame
303 * @see #getPVCoordinates(Frame)
304 */
305 public TimeStampedPVCoordinates getPVCoordinates() {
306 if (pvCoordinates == null) {
307 pvCoordinates = initPVCoordinates();
308 }
309 return pvCoordinates;
310 }
311
312 /** Compute the position/velocity coordinates from the canonical parameters.
313 * @return computed position/velocity coordinates
314 */
315 protected abstract TimeStampedPVCoordinates initPVCoordinates();
316
317 /** Get a time-shifted orbit.
318 * <p>
319 * The orbit can be slightly shifted to close dates. This shift is based on
320 * a simple keplerian model. It is <em>not</em> intended as a replacement
321 * for proper orbit and attitude propagation but should be sufficient for
322 * small time shifts or coarse accuracy.
323 * </p>
324 * @param dt time shift in seconds
325 * @return a new orbit, shifted with respect to the instance (which is immutable)
326 */
327 public abstract Orbit shiftedBy(final double dt);
328
329 /** Compute the Jacobian of the orbital parameters with respect to the Cartesian parameters.
330 * <p>
331 * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
332 * respect to Cartesian coordinate j. This means each row corresponds to one orbital parameter
333 * whereas columns 0 to 5 correspond to the Cartesian coordinates x, y, z, xDot, yDot and zDot.
334 * </p>
335 * @param type type of the position angle to use
336 * @param jacobian placeholder 6x6 (or larger) matrix to be filled with the Jacobian, if matrix
337 * is larger than 6x6, only the 6x6 upper left corner will be modified
338 */
339 public void getJacobianWrtCartesian(final PositionAngle type, final double[][] jacobian) {
340
341 final double[][] cachedJacobian;
342 synchronized (this) {
343 switch (type) {
344 case MEAN :
345 if (jacobianMeanWrtCartesian == null) {
346 // first call, we need to compute the jacobian and cache it
347 jacobianMeanWrtCartesian = computeJacobianMeanWrtCartesian();
348 }
349 cachedJacobian = jacobianMeanWrtCartesian;
350 break;
351 case ECCENTRIC :
352 if (jacobianEccentricWrtCartesian == null) {
353 // first call, we need to compute the jacobian and cache it
354 jacobianEccentricWrtCartesian = computeJacobianEccentricWrtCartesian();
355 }
356 cachedJacobian = jacobianEccentricWrtCartesian;
357 break;
358 case TRUE :
359 if (jacobianTrueWrtCartesian == null) {
360 // first call, we need to compute the jacobian and cache it
361 jacobianTrueWrtCartesian = computeJacobianTrueWrtCartesian();
362 }
363 cachedJacobian = jacobianTrueWrtCartesian;
364 break;
365 default :
366 throw new OrekitInternalError(null);
367 }
368 }
369
370 // fill the user provided array
371 for (int i = 0; i < cachedJacobian.length; ++i) {
372 System.arraycopy(cachedJacobian[i], 0, jacobian[i], 0, cachedJacobian[i].length);
373 }
374
375 }
376
377 /** Compute the Jacobian of the Cartesian parameters with respect to the orbital parameters.
378 * <p>
379 * Element {@code jacobian[i][j]} is the derivative of Cartesian coordinate i of the orbit with
380 * respect to orbital parameter j. This means each row corresponds to one Cartesian coordinate
381 * x, y, z, xdot, ydot, zdot whereas columns 0 to 5 correspond to the orbital parameters.
382 * </p>
383 * @param type type of the position angle to use
384 * @param jacobian placeholder 6x6 (or larger) matrix to be filled with the Jacobian, if matrix
385 * is larger than 6x6, only the 6x6 upper left corner will be modified
386 */
387 public void getJacobianWrtParameters(final PositionAngle type, final double[][] jacobian) {
388
389 final double[][] cachedJacobian;
390 synchronized (this) {
391 switch (type) {
392 case MEAN :
393 if (jacobianWrtParametersMean == null) {
394 // first call, we need to compute the jacobian and cache it
395 jacobianWrtParametersMean = createInverseJacobian(type);
396 }
397 cachedJacobian = jacobianWrtParametersMean;
398 break;
399 case ECCENTRIC :
400 if (jacobianWrtParametersEccentric == null) {
401 // first call, we need to compute the jacobian and cache it
402 jacobianWrtParametersEccentric = createInverseJacobian(type);
403 }
404 cachedJacobian = jacobianWrtParametersEccentric;
405 break;
406 case TRUE :
407 if (jacobianWrtParametersTrue == null) {
408 // first call, we need to compute the jacobian and cache it
409 jacobianWrtParametersTrue = createInverseJacobian(type);
410 }
411 cachedJacobian = jacobianWrtParametersTrue;
412 break;
413 default :
414 throw new OrekitInternalError(null);
415 }
416 }
417
418 // fill the user-provided array
419 for (int i = 0; i < cachedJacobian.length; ++i) {
420 System.arraycopy(cachedJacobian[i], 0, jacobian[i], 0, cachedJacobian[i].length);
421 }
422
423 }
424
425 /** Create an inverse Jacobian.
426 * @param type type of the position angle to use
427 * @return inverse Jacobian
428 */
429 private double[][] createInverseJacobian(final PositionAngle type) {
430
431 // get the direct Jacobian
432 final double[][] directJacobian = new double[6][6];
433 getJacobianWrtCartesian(type, directJacobian);
434
435 // invert the direct Jacobian
436 final RealMatrix matrix = MatrixUtils.createRealMatrix(directJacobian);
437 final DecompositionSolver solver = new QRDecomposition(matrix).getSolver();
438 return solver.getInverse().getData();
439
440 }
441
442 /** Compute the Jacobian of the orbital parameters with mean angle with respect to the Cartesian parameters.
443 * <p>
444 * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
445 * respect to Cartesian coordinate j. This means each row correspond to one orbital parameter
446 * whereas columns 0 to 5 correspond to the Cartesian coordinates x, y, z, xDot, yDot and zDot.
447 * </p>
448 * @return 6x6 Jacobian matrix
449 * @see #computeJacobianEccentricWrtCartesian()
450 * @see #computeJacobianTrueWrtCartesian()
451 */
452 protected abstract double[][] computeJacobianMeanWrtCartesian();
453
454 /** Compute the Jacobian of the orbital parameters with eccentric angle with respect to the Cartesian parameters.
455 * <p>
456 * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
457 * respect to Cartesian coordinate j. This means each row correspond to one orbital parameter
458 * whereas columns 0 to 5 correspond to the Cartesian coordinates x, y, z, xDot, yDot and zDot.
459 * </p>
460 * @return 6x6 Jacobian matrix
461 * @see #computeJacobianMeanWrtCartesian()
462 * @see #computeJacobianTrueWrtCartesian()
463 */
464 protected abstract double[][] computeJacobianEccentricWrtCartesian();
465
466 /** Compute the Jacobian of the orbital parameters with true angle with respect to the Cartesian parameters.
467 * <p>
468 * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
469 * respect to Cartesian coordinate j. This means each row correspond to one orbital parameter
470 * whereas columns 0 to 5 correspond to the Cartesian coordinates x, y, z, xDot, yDot and zDot.
471 * </p>
472 * @return 6x6 Jacobian matrix
473 * @see #computeJacobianMeanWrtCartesian()
474 * @see #computeJacobianEccentricWrtCartesian()
475 */
476 protected abstract double[][] computeJacobianTrueWrtCartesian();
477
478 /** Add the contribution of the Keplerian motion to parameters derivatives
479 * <p>
480 * This method is used by integration-based propagators to evaluate the part of Keplerian
481 * motion to evolution of the orbital state.
482 * </p>
483 * @param type type of the position angle in the state
484 * @param gm attraction coefficient to use
485 * @param pDot array containing orbital state derivatives to update (the Keplerian
486 * part must be <em>added</em> to the array components, as the array may already
487 * contain some non-zero elements corresponding to non-Keplerian parts)
488 */
489 public abstract void addKeplerContribution(final PositionAngle type, final double gm, double[] pDot);
490
491 /** Fill a Jacobian half row with a single vector.
492 * @param a coefficient of the vector
493 * @param v vector
494 * @param row Jacobian matrix row
495 * @param j index of the first element to set (row[j], row[j+1] and row[j+2] will all be set)
496 */
497 protected static void fillHalfRow(final double a, final Vector3D v, final double[] row, final int j) {
498 row[j] = a * v.getX();
499 row[j + 1] = a * v.getY();
500 row[j + 2] = a * v.getZ();
501 }
502
503 /** Fill a Jacobian half row with a linear combination of vectors.
504 * @param a1 coefficient of the first vector
505 * @param v1 first vector
506 * @param a2 coefficient of the second vector
507 * @param v2 second vector
508 * @param row Jacobian matrix row
509 * @param j index of the first element to set (row[j], row[j+1] and row[j+2] will all be set)
510 */
511 protected static void fillHalfRow(final double a1, final Vector3D v1, final double a2, final Vector3D v2,
512 final double[] row, final int j) {
513 row[j] = MathArrays.linearCombination(a1, v1.getX(), a2, v2.getX());
514 row[j + 1] = MathArrays.linearCombination(a1, v1.getY(), a2, v2.getY());
515 row[j + 2] = MathArrays.linearCombination(a1, v1.getZ(), a2, v2.getZ());
516 }
517
518 /** Fill a Jacobian half row with a linear combination of vectors.
519 * @param a1 coefficient of the first vector
520 * @param v1 first vector
521 * @param a2 coefficient of the second vector
522 * @param v2 second vector
523 * @param a3 coefficient of the third vector
524 * @param v3 third vector
525 * @param row Jacobian matrix row
526 * @param j index of the first element to set (row[j], row[j+1] and row[j+2] will all be set)
527 */
528 protected static void fillHalfRow(final double a1, final Vector3D v1, final double a2, final Vector3D v2,
529 final double a3, final Vector3D v3,
530 final double[] row, final int j) {
531 row[j] = MathArrays.linearCombination(a1, v1.getX(), a2, v2.getX(), a3, v3.getX());
532 row[j + 1] = MathArrays.linearCombination(a1, v1.getY(), a2, v2.getY(), a3, v3.getY());
533 row[j + 2] = MathArrays.linearCombination(a1, v1.getZ(), a2, v2.getZ(), a3, v3.getZ());
534 }
535
536 /** Fill a Jacobian half row with a linear combination of vectors.
537 * @param a1 coefficient of the first vector
538 * @param v1 first vector
539 * @param a2 coefficient of the second vector
540 * @param v2 second vector
541 * @param a3 coefficient of the third vector
542 * @param v3 third vector
543 * @param a4 coefficient of the fourth vector
544 * @param v4 fourth vector
545 * @param row Jacobian matrix row
546 * @param j index of the first element to set (row[j], row[j+1] and row[j+2] will all be set)
547 */
548 protected static void fillHalfRow(final double a1, final Vector3D v1, final double a2, final Vector3D v2,
549 final double a3, final Vector3D v3, final double a4, final Vector3D v4,
550 final double[] row, final int j) {
551 row[j] = MathArrays.linearCombination(a1, v1.getX(), a2, v2.getX(), a3, v3.getX(), a4, v4.getX());
552 row[j + 1] = MathArrays.linearCombination(a1, v1.getY(), a2, v2.getY(), a3, v3.getY(), a4, v4.getY());
553 row[j + 2] = MathArrays.linearCombination(a1, v1.getZ(), a2, v2.getZ(), a3, v3.getZ(), a4, v4.getZ());
554 }
555
556 /** Fill a Jacobian half row with a linear combination of vectors.
557 * @param a1 coefficient of the first vector
558 * @param v1 first vector
559 * @param a2 coefficient of the second vector
560 * @param v2 second vector
561 * @param a3 coefficient of the third vector
562 * @param v3 third vector
563 * @param a4 coefficient of the fourth vector
564 * @param v4 fourth vector
565 * @param a5 coefficient of the fifth vector
566 * @param v5 fifth vector
567 * @param row Jacobian matrix row
568 * @param j index of the first element to set (row[j], row[j+1] and row[j+2] will all be set)
569 */
570 protected static void fillHalfRow(final double a1, final Vector3D v1, final double a2, final Vector3D v2,
571 final double a3, final Vector3D v3, final double a4, final Vector3D v4,
572 final double a5, final Vector3D v5,
573 final double[] row, final int j) {
574 final double[] a = new double[] {
575 a1, a2, a3, a4, a5
576 };
577 row[j] = MathArrays.linearCombination(a, new double[] {
578 v1.getX(), v2.getX(), v3.getX(), v4.getX(), v5.getX()
579 });
580 row[j + 1] = MathArrays.linearCombination(a, new double[] {
581 v1.getY(), v2.getY(), v3.getY(), v4.getY(), v5.getY()
582 });
583 row[j + 2] = MathArrays.linearCombination(a, new double[] {
584 v1.getZ(), v2.getZ(), v3.getZ(), v4.getZ(), v5.getZ()
585 });
586 }
587
588 /** Fill a Jacobian half row with a linear combination of vectors.
589 * @param a1 coefficient of the first vector
590 * @param v1 first vector
591 * @param a2 coefficient of the second vector
592 * @param v2 second vector
593 * @param a3 coefficient of the third vector
594 * @param v3 third vector
595 * @param a4 coefficient of the fourth vector
596 * @param v4 fourth vector
597 * @param a5 coefficient of the fifth vector
598 * @param v5 fifth vector
599 * @param a6 coefficient of the sixth vector
600 * @param v6 sixth vector
601 * @param row Jacobian matrix row
602 * @param j index of the first element to set (row[j], row[j+1] and row[j+2] will all be set)
603 */
604 protected static void fillHalfRow(final double a1, final Vector3D v1, final double a2, final Vector3D v2,
605 final double a3, final Vector3D v3, final double a4, final Vector3D v4,
606 final double a5, final Vector3D v5, final double a6, final Vector3D v6,
607 final double[] row, final int j) {
608 final double[] a = new double[] {
609 a1, a2, a3, a4, a5, a6
610 };
611 row[j] = MathArrays.linearCombination(a, new double[] {
612 v1.getX(), v2.getX(), v3.getX(), v4.getX(), v5.getX(), v6.getX()
613 });
614 row[j + 1] = MathArrays.linearCombination(a, new double[] {
615 v1.getY(), v2.getY(), v3.getY(), v4.getY(), v5.getY(), v6.getY()
616 });
617 row[j + 2] = MathArrays.linearCombination(a, new double[] {
618 v1.getZ(), v2.getZ(), v3.getZ(), v4.getZ(), v5.getZ(), v6.getZ()
619 });
620 }
621
622 }