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