@Test
public void testSpin() throws OrekitException {
AbsoluteDate date =
new AbsoluteDate(
new DateComponents(1970, 01, 01),
new TimeComponents(3, 25, 45.6789),
TimeScalesFactory.getUTC());
KeplerianOrbit orbit =
new KeplerianOrbit(
7178000.0,
1.e-4,
FastMath.toRadians(50.),
FastMath.toRadians(10.),
FastMath.toRadians(20.),
FastMath.toRadians(30.),
PositionAngle.MEAN,
FramesFactory.getEME2000(),
date,
3.986004415e14);
final AttitudeProvider law =
new LofOffsetPointing(
earthSpheric,
new LofOffset(orbit.getFrame(), LOFType.VVLH, RotationOrder.XYX, 0.1, 0.2, 0.3),
Vector3D.PLUS_K);
Propagator propagator = new KeplerianPropagator(orbit, law);
double h = 0.01;
SpacecraftState sMinus = propagator.propagate(date.shiftedBy(-h));
SpacecraftState s0 = propagator.propagate(date);
SpacecraftState sPlus = propagator.propagate(date.shiftedBy(h));
// check spin is consistent with attitude evolution
double errorAngleMinus =
Rotation.distance(
sMinus.shiftedBy(h).getAttitude().getRotation(), s0.getAttitude().getRotation());
double evolutionAngleMinus =
Rotation.distance(sMinus.getAttitude().getRotation(), s0.getAttitude().getRotation());
Assert.assertEquals(0.0, errorAngleMinus, 1.0e-6 * evolutionAngleMinus);
double errorAnglePlus =
Rotation.distance(
s0.getAttitude().getRotation(), sPlus.shiftedBy(-h).getAttitude().getRotation());
double evolutionAnglePlus =
Rotation.distance(s0.getAttitude().getRotation(), sPlus.getAttitude().getRotation());
Assert.assertEquals(0.0, errorAnglePlus, 1.0e-6 * evolutionAnglePlus);
Vector3D spin0 = s0.getAttitude().getSpin();
Vector3D reference =
AngularCoordinates.estimateRate(
sMinus.getAttitude().getRotation(), sPlus.getAttitude().getRotation(), 2 * h);
Assert.assertTrue(spin0.getNorm() > 1.0e-3);
Assert.assertEquals(0.0, spin0.subtract(reference).getNorm(), 1.0e-10);
}
/** Test if both constructors are equivalent */
@Test
public void testLof() throws OrekitException {
// Satellite position
final CircularOrbit circ =
new CircularOrbit(
7178000.0,
0.5e-4,
-0.5e-4,
FastMath.toRadians(0.),
FastMath.toRadians(270.),
FastMath.toRadians(5.300),
PositionAngle.MEAN,
FramesFactory.getEME2000(),
date,
mu);
// Create lof aligned law
// ************************
final LofOffset lofLaw = new LofOffset(circ.getFrame(), LOFType.VVLH);
final LofOffsetPointing lofPointing =
new LofOffsetPointing(earthSpheric, lofLaw, Vector3D.PLUS_K);
final Rotation lofRot = lofPointing.getAttitude(circ, date, circ.getFrame()).getRotation();
// Compare to body center pointing law
// *************************************
final BodyCenterPointing centerLaw = new BodyCenterPointing(earthSpheric.getBodyFrame());
final Rotation centerRot = centerLaw.getAttitude(circ, date, circ.getFrame()).getRotation();
final double angleBodyCenter = centerRot.applyInverseTo(lofRot).getAngle();
Assert.assertEquals(0., angleBodyCenter, Utils.epsilonAngle);
// Compare to nadir pointing law
// *******************************
final NadirPointing nadirLaw = new NadirPointing(earthSpheric);
final Rotation nadirRot = nadirLaw.getAttitude(circ, date, circ.getFrame()).getRotation();
final double angleNadir = nadirRot.applyInverseTo(lofRot).getAngle();
Assert.assertEquals(0., angleNadir, Utils.epsilonAngle);
}
@Override
protected void ComputeUntypedRate(IContinuousState ROstate, IContinuousRate WOrate) {
SailBoatState etat = (SailBoatState) ROstate;
SailBoatRate taux = (SailBoatRate) WOrate;
BateauAVoileInit init = (BateauAVoileInit) getInitParameters();
BateauAVoileFeatures tno = (BateauAVoileFeatures) getFeatures();
// if(CurrentDate().toString().compareTo("01/09/2014 06:00:00.62")==0)
// System.out.println("ici");
// identification des axes du bateau
Rotation rz = new Rotation(Vector3D.PLUS_K, etat.getTheta());
Vector3D axe_x = rz.applyTo(Vector3D.PLUS_I);
Vector3D axe_y_horizontal = rz.applyTo(Vector3D.PLUS_J);
Rotation r_axe_x = new Rotation(axe_x, etat.getPhi());
Vector3D axe_y = r_axe_x.applyTo(axe_y_horizontal);
Vector3D axe_z = r_axe_x.applyTo(Vector3D.PLUS_K);
// Points application des forces
Vector3D CcG = axe_y_horizontal.scalarMultiply(tno.getCg_CC_Max() * Math.sin(etat.getPhi()));
Vector3D CcVc = axe_x.scalarMultiply(tno.getrV());
Vector3D CcRg = axe_x.scalarMultiply(-tno.getrG());
Vector3D CcCV = CcVc.add(voile.getCV());
// bilan des forces
Vector3D poids = Environment.g.scalarMultiply(tno.getM());
Vector3D fv = voile.getFv();
Vector3D fg = gouvernail.getFg();
Vector3D f_friction = etat.getVel().scalarMultiply(-tno.getAlphaF());
Vector3D fAntiDerive =
axe_y_horizontal.scalarMultiply(
-Math.signum(etat.getVel().dotProduct(axe_y_horizontal))
* 0.5
* 1000
* (0.4 * Math.pow(etat.getVel().dotProduct(axe_y_horizontal) * 1.5, 2)));
// archimede compense les composantes selon z du gouvernail et de la voile
Vector3D archimede =
poids
.scalarMultiply(-1)
.add(Vector3D.PLUS_K.scalarMultiply(-Vector3D.PLUS_K.dotProduct(fg.add(fv))));
forces.clear();
// forces.add(new Force(CcG,poids));
// forces.add(new Force(Vector3D.ZERO,archimede));
forces.add(new Force(CcCV, fv));
forces.add(new Force(CcRg, fg));
forces.add(new Force(CcG, etat.getVel()));
// bilan des moments en Cc suivant l'axe x du bateau
// contribution de la force vélique
M1 =
axe_z
.scalarMultiply(Math.abs(CcCV.dotProduct(axe_z)))
.crossProduct(axe_y.scalarMultiply(fv.dotProduct(axe_y)))
.dotProduct(axe_x);
// contribution du poids
M2 = CcG.crossProduct(poids).dotProduct(axe_x);
// antifriction selon
M3 = -tno.getAlphaTheta() * etat.getPhi_point();
// bilan des moments en Cc suivant l'axe z du bateau
// contribution de la force vélique
M4 = CcVc.crossProduct(axe_y.scalarMultiply(fv.dotProduct(axe_y))).dotProduct(axe_z);
// contribution du gouvernail
M5 = CcRg.crossProduct(axe_y.scalarMultiply(fg.dotProduct(axe_y))).dotProduct(axe_z);
// contribution du couple anti friction
M6 = -tno.getAlphaTheta() * etat.getTheta_point();
taux.setCurvRate(etat.getVel().getNorm()); // abscisse curviligne
taux.setPosRate(etat.getVel());
taux.setVelRate(
poids
.add(archimede)
.add(fv)
.add(fg)
.add(f_friction)
.scalarMultiply(1 / tno.getM())
.add(fAntiDerive));
taux.setPhi_rate(etat.getPhi_point());
taux.setPhi_point_rate((M1 + M2 + M3) * 1.0 / tno.getJx());
taux.setTheta_rate(etat.getTheta_point());
taux.setTheta_point_rate((M4 + M5 + M6) * 1.0 / tno.getJz());
}
// create a rotation matrix. Angle in degrees. Axis ex: new Vector3D(0,0,1) for Z axis
private static RealMatrix RotationMatrix(double angle, Vector3D axis) {
Rotation Mrot = new Rotation(axis, Math.toRadians(angle));
double[][] Mdata = Mrot.getMatrix();
RealMatrix M = MatrixUtils.createRealMatrix(Mdata);
return M;
}