diff --git a/src/main/java/org/orekit/rugged/api/Rugged.java b/src/main/java/org/orekit/rugged/api/Rugged.java
index 2c17f835c0675b8f064b0b9df282afc6674847d0..fa1cffd06eb88c3315c79f6be0b1fa6eda0efaf0 100644
--- a/src/main/java/org/orekit/rugged/api/Rugged.java
+++ b/src/main/java/org/orekit/rugged/api/Rugged.java
@@ -590,6 +590,7 @@ public class Rugged {
* @param sensorA line sensor A
* @param dateA current date for sensor A
* @param pixelA pixel index for sensor A
+ * @param scToBodyA spacecraftToBody transform for sensor A
* @param sensorB line sensor B
* @param dateB current date for sensor B
* @param pixelB pixel index for sensor B
@@ -598,6 +599,7 @@ public class Rugged {
*/
public double distanceBetweenLOS(final LineSensor sensorA,
final AbsoluteDate dateA, final int pixelA,
+ final SpacecraftToObservedBody scToBodyA,
final LineSensor sensorB,
final AbsoluteDate dateB, final int pixelB)
throws RuggedException {
@@ -605,34 +607,21 @@ public class Rugged {
//final LineSensor sensorA = getLineSensor(sensorNameA);
//final LineSensor sensorB = getLineSensor(sensorNameB);
- // Get sensors's position and LOS
- // compute the approximate transform between spacecraft and observed body
-
- final Transform scToInertA = scToBody.getScToInertial(dateA);
- //final Transform inertToBodyA = scToBody.getInertialToBody(dateA);
- //final Transform approximate = new Transform(dateA, scToInertA, inertToBodyA);
-
-
- // compute location of each pixel
- final Vector3D sA = scToInertA.transformPosition(sensorA.getPosition());
- final Vector3D vA = scToInertA.transformVector(sensorA.getLOS(dateA, pixelA));
-
- //TODO scToBody should be found from ruggedB instance
- final Transform scToInertB = scToBody.getScToInertial(dateB);
- //final Transform inertToBodyB = scToBody.getInertialToBody(dateB);
- //final Transform approximate = new Transform(dateA, scToInert, inertToBody);
-
- // compute location of each pixel
- final Vector3D sB = scToInertB.transformPosition(sensorB.getPosition());
- final Vector3D vB = scToInertB.transformVector(sensorB.getLOS(dateB, pixelB));
-
-
-
-
- //final Vector3D vA = sensorA.getLOS(dateA, pixelA); // V_a : line of sight's vectorA
- //final Vector3D vB = sensorB.getLOS(dateB, pixelB); // V_b
- //final Vector3D sA = sensorA.getPosition(); // S_a : sensorA 's position
- //final Vector3D sB = sensorB.getPosition(); // S_b
+ // Compute the approximate transforms between spacecraft and observed body
+ final Transform scToInertA = scToBodyA.getScToInertial(dateA); // from rugged instance A
+ final Transform scToInertB = scToBody.getScToInertial(dateB); // from (current) rugged instance B
+
+ // Get sensors's position and LOS into local frame
+ final Vector3D vALocal = sensorA.getLOS(dateA, pixelA); // V_a : line of sight's vectorA
+ final Vector3D vBLocal = sensorB.getLOS(dateB, pixelB); // V_b
+ final Vector3D sALocal = sensorA.getPosition(); // S_a : sensorA 's position
+ final Vector3D sBLocal = sensorB.getPosition(); // S_b
+
+ // Get sensors's position and LOS into inertial frame
+ final Vector3D sA = scToInertA.transformPosition(sALocal);
+ final Vector3D vA = scToInertA.transformVector(vALocal);
+ final Vector3D sB = scToInertB.transformPosition(sBLocal);
+ final Vector3D vB = scToInertB.transformVector(vBLocal);
final Vector3D vBase= sB.subtract(sA); // S_b - S_a
final double svA = Vector3D.dotProduct(vBase, vA); // SV_a = (S_b - S_a).V_a
@@ -659,6 +648,7 @@ public class Rugged {
* @param sensorA line sensor A
* @param dateA current date for sensor A
* @param pixelA pixel index for sensor A
+ * @param scToBodyA spacecraftToBody transform for sensor A
* @param sensorB line sensor B
* @param dateB current date for sensor B
* @param pixelB pixel index for sensor B
@@ -669,6 +659,7 @@ public class Rugged {
*/
private DerivativeStructure[] distanceBetweenLOSDerivatives(final LineSensor sensorA,
final AbsoluteDate dateA, final int pixelA,
+ final SpacecraftToObservedBody scToBodyA,
final LineSensor sensorB,
final AbsoluteDate dateB, final int pixelB,
final DSGenerator generator)
@@ -677,18 +668,31 @@ public class Rugged {
//final LineSensor sensorA = getLineSensor(sensorNameA);
//final LineSensor sensorB = getLineSensor(sensorNameB);
- // Get sensors's LOS
- final FieldVector3D<DerivativeStructure> vA = sensorA.getLOSDerivatives(dateA, pixelA, generator); // V_a : line of sight's vectorA
- final FieldVector3D<DerivativeStructure> vB = sensorB.getLOSDerivatives(dateB, pixelB, generator); // V_b
+ // Compute the approximate transforms between spacecraft and observed body
+ final Transform scToInertA = scToBodyA.getScToInertial(dateA); // from rugged instance A
+ final Transform scToInertB = scToBody.getScToInertial(dateB); // from (current) rugged instance B
+
+ // Get sensors's LOS into local frame
+ final FieldVector3D<DerivativeStructure> vALocal = sensorA.getLOSDerivatives(dateA, pixelA, generator);
+ final FieldVector3D<DerivativeStructure> vBLocal = sensorB.getLOSDerivatives(dateB, pixelB, generator);
+
+ // Get sensors's LOS into inertial frame
+ final FieldVector3D<DerivativeStructure> vA = scToInertA.transformPosition(vALocal); // V_a : line of sight's vectorA
+ final FieldVector3D<DerivativeStructure> vB = scToInertB.transformPosition(vBLocal); // V_b
- // Get sensors's position. TODO: check if we have to implement getPositionDerivatives() method & CO
+ // Get sensors's position into local frame TODO: check if we have to implement getPositionDerivatives() method & CO
final Vector3D sAtmp = sensorA.getPosition(); // S_a : sensorA 's position
- final Vector3D sBtmp = sensorB.getPosition(); // S_b
+ final Vector3D sBtmp = sensorB.getPosition(); // S_b : sensorB 's position
final DerivativeStructure scaleFactor = FieldVector3D.dotProduct(vA.normalize(), vA.normalize()); // V_a.V_a=1
// Build a vector from position vector and a scale factor (equals to 1).
- // The vector built will be scaleFactor * sA for example.
- final FieldVector3D<DerivativeStructure> sA = new FieldVector3D<DerivativeStructure>(scaleFactor, sAtmp);
- final FieldVector3D<DerivativeStructure> sB = new FieldVector3D<DerivativeStructure>(scaleFactor, sBtmp);
+ // The vector built will be scaleFactor * sAtmp for example.
+ final FieldVector3D<DerivativeStructure> sALocal = new FieldVector3D<DerivativeStructure>(scaleFactor, sAtmp);
+ final FieldVector3D<DerivativeStructure> sBLocal = new FieldVector3D<DerivativeStructure>(scaleFactor, sBtmp);
+
+ // Get sensors's position into inertial frame
+ final FieldVector3D<DerivativeStructure> sA = scToInertA.transformPosition(sALocal);
+ final FieldVector3D<DerivativeStructure> sB = scToInertB.transformPosition(sBLocal);
+
// final FieldVector3D<DerivativeStructure> sA = sensorA.getPositionDerivatives(); // S_a : sensorA 's position
// final FieldVector3D<DerivativeStructure> sB = sensorB.getPositionDerivatives(); // S_b
@@ -815,11 +819,13 @@ public class Rugged {
final AbsoluteDate dateB = lineSensorB.getDate(spB.getLineNumber());
final int pixelA = (int)spA.getPixelNumber(); // Note: Rugged don't deal with half-pixel
final int pixelB = (int)spB.getPixelNumber();
+ final SpacecraftToObservedBody scToBodyA = ruggedA.getScToBody();
final DerivativeStructure[] ilResult =
distanceBetweenLOSDerivatives(lineSensorA,
dateA,
pixelA,
+ scToBodyA,
lineSensorB,
dateB,
pixelB,
@@ -1296,5 +1302,12 @@ public class Rugged {
}
return sensor;
}
+
+ /** Get converter between spacecraft and body
+ * @return the scToBody
+ */
+ public SpacecraftToObservedBody getScToBody() {
+ return scToBody;
+ }
}
diff --git a/src/tutorials/java/AffinagePleiades/SensorToSensorMeasureGenerator.java b/src/tutorials/java/AffinagePleiades/SensorToSensorMeasureGenerator.java
index 53c16350e1a9eb6af7ac8bd6da2f2110a2a8032c..57d80ebbdbc503ddc18a3e24223f4aa228de644c 100644
--- a/src/tutorials/java/AffinagePleiades/SensorToSensorMeasureGenerator.java
+++ b/src/tutorials/java/AffinagePleiades/SensorToSensorMeasureGenerator.java
@@ -32,6 +32,7 @@ import org.hipparchus.random.Well19937a;
import org.hipparchus.util.FastMath;
import org.orekit.bodies.GeodeticPoint;
import org.orekit.rugged.utils.DSGenerator;
+import org.orekit.rugged.utils.SpacecraftToObservedBody;
import java.util.Locale;
@@ -133,8 +134,12 @@ public class SensorToSensorMeasureGenerator {
// TODO test if distance is 0.0 with crossing LOS
final AbsoluteDate dateB = sensorB.getDate(sensorPixelB.getLineNumber());
double pixelB = sensorPixelB.getPixelNumber();
+
+ // Get spacecraft to body transform of rugged instance A
+ final SpacecraftToObservedBody scToBodyA = ruggedA.getScToBody();
+
//TODO work with double pixel values
- double distance = ruggedA.distanceBetweenLOS(sensorA,dateA, pixelA, sensorB ,dateB,(int) pixelB);
+ double distance = ruggedB.distanceBetweenLOS(sensorA,dateA, pixelA, scToBodyA, sensorB ,dateB,(int) pixelB);
System.out.format(Locale.US,"distance %f %n",distance);
mapping.addMapping(new SensorPixel(line, pixelA),