@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; }