List of usage examples for org.apache.commons.math3.geometry.euclidean.threed Rotation applyInverseTo
public Rotation applyInverseTo(Rotation r)
From source file:org.jtrfp.trcl.beh.CubeCollisionBehavior.java
@Override public void proposeCollision(WorldObject obj) { if (obj instanceof Player) { final WorldObject p = getParent(); final double[] relPos = TR.twosComplimentSubtract(obj.getPosition(), p.getPosition(), new double[3]); final Rotation rot = new Rotation(Vector3D.PLUS_K, Vector3D.PLUS_J, p.getHeading(), p.getTop()); final double[] rotPos = rot.applyInverseTo(new Vector3D(relPos)).toArray(); final double[] rotTransPos = Vect3D.add(rotPos, origin, rotTransPosVar); if (TR.twosComplimentDistance(obj.getPosition(), p.getPosition()) < 80000) if (rotTransPos[0] > 0 && rotTransPos[0] < dims[0] && rotTransPos[1] > 0 && rotTransPos[1] < dims[1] && rotTransPos[2] > 0 && rotTransPos[2] < dims[2]) { obj.probeForBehaviors(new AbstractSubmitter<DamageableBehavior>() { @Override/* w w w . j av a 2 s .c o m*/ public void submit(DamageableBehavior item) { item.proposeDamage(new GroundCollisionDamage(damageOnImpact)); } }, DamageableBehavior.class); } //end if(withinRange) } //end if(Player) }
From source file:org.jtrfp.trcl.beh.RollLevelingBehavior.java
@Override public void _tick(long tickTimeMillis) { WorldObject p = getParent();//from ww w . j a v a2 s .co m final double[] initHdng = p.getHeadingArray(); final double[] initTop = p.getTopArray(); //Escape on invalid cases if (initHdng[1] <= -1) return; if (initHdng[1] >= 1) return; if (initTop[1] == 0) return; //Create an imaginary heading/top where heading.y=0 imgHdng[0] = initHdng[0]; imgHdng[1] = 0; imgHdng[2] = initHdng[2]; Vect3D.normalize(imgHdng, imgHdng); //Create a rotation to convert back later after performing the roll adjustment. Rotation rot = new Rotation(new Vector3D(initHdng), new Vector3D(imgHdng)); Vector3D imgTop = rot.applyTo(new Vector3D(initTop)).normalize(); double topY = imgTop.getY(); if (topY == 0) return; //Retainment softener prevents gimbal swing effect when facing up or down. final double retainmentSoftener = Misc.clamp(Math.abs(initHdng[1]), 0, 1); final double softenedRetainment = retainment * (1. - retainmentSoftener) + retainmentSoftener; //Perform the roll adjustment using weighting supplied by softenedRetainer if (topY > 0) {//Rightside up, approach 1. topY = topY * softenedRetainment + 1 * (1. - softenedRetainment); } else {//Upside down, approach -1 topY = topY * softenedRetainment + -1 * (1. - softenedRetainment); } Vector3D newTop = rot.applyInverseTo(new Vector3D(imgTop.getX(), topY, imgTop.getZ()).normalize()); //Apply. This will automatically notify change. No need to change heading as it is intrinsically the same. p.setTop(newTop); }
From source file:org.orekit.attitudes.BodyCenterPointingTest.java
/** Test if body center belongs to the direction pointed by the satellite */// w ww . j a v a2 s . co m @Test public void testBodyCenterInPointingDirection() throws OrekitException { // Transform satellite position to position/velocity parameters in EME2000 frame PVCoordinates pvSatEME2000 = circ.getPVCoordinates(); // Pointing direction // ******************** // Get satellite attitude rotation, i.e rotation from EME2000 frame to satellite frame Rotation rotSatEME2000 = earthCenterAttitudeLaw.getAttitude(circ, date, circ.getFrame()).getRotation(); // Transform Z axis from satellite frame to EME2000 Vector3D zSatEME2000 = rotSatEME2000.applyInverseTo(Vector3D.PLUS_K); // Transform Z axis from EME2000 to ITRF2008 Vector3D zSatITRF2008C = eme2000ToItrf.transformVector(zSatEME2000); // Transform satellite position/velocity from EME2000 to ITRF2008 PVCoordinates pvSatITRF2008C = eme2000ToItrf.transformPVCoordinates(pvSatEME2000); // Line containing satellite point and following pointing direction Line pointingLine = new Line(pvSatITRF2008C.getPosition(), pvSatITRF2008C.getPosition().add(Constants.WGS84_EARTH_EQUATORIAL_RADIUS, zSatITRF2008C), 2.0e-8); // Check that the line contains Earth center Assert.assertTrue(pointingLine.contains(Vector3D.ZERO)); }
From source file:org.orekit.attitudes.LofOffsetPointingTest.java
/** Test if both constructors are equivalent *///w w w . j a v a 2 s . c om @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(circ.getFrame(), 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(circ.getFrame(), earthSpheric); 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(circ.getFrame(), earthSpheric); final Rotation nadirRot = nadirLaw.getAttitude(circ, date, circ.getFrame()).getRotation(); final double angleNadir = nadirRot.applyInverseTo(lofRot).getAngle(); Assert.assertEquals(0., angleNadir, Utils.epsilonAngle); }
From source file:org.orekit.attitudes.LofOffsetTest.java
/** Test if the lof offset is the one expected *///w ww. j a v a2 s .co m @Test public void testOffset() throws OrekitException, CardanEulerSingularityException { // 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 target pointing attitude provider // ************************************ // Elliptic earth shape final OneAxisEllipsoid earthShape = new OneAxisEllipsoid(6378136.460, 1 / 298.257222101, itrf); final GeodeticPoint geoTargetITRF2005 = new GeodeticPoint(FastMath.toRadians(43.36), FastMath.toRadians(1.26), 600.); // Attitude law definition from geodetic point target final TargetPointing targetLaw = new TargetPointing(circ.getFrame(), geoTargetITRF2005, earthShape); final Rotation targetRot = targetLaw.getAttitude(circ, date, circ.getFrame()).getRotation(); // Create lof aligned attitude provider // ******************************* final LofOffset lofAlignedLaw = new LofOffset(orbit.getFrame(), LOFType.VVLH); final Rotation lofAlignedRot = lofAlignedLaw.getAttitude(circ, date, circ.getFrame()).getRotation(); // Get rotation from LOF to target pointing attitude Rotation rollPitchYaw = targetRot.applyTo(lofAlignedRot.revert()).revert(); final double[] angles = rollPitchYaw.getAngles(RotationOrder.ZYX); final double yaw = angles[0]; final double pitch = angles[1]; final double roll = angles[2]; // Create lof offset attitude provider with computed roll, pitch, yaw // ************************************************************** final LofOffset lofOffsetLaw = new LofOffset(orbit.getFrame(), LOFType.VVLH, RotationOrder.ZYX, yaw, pitch, roll); final Rotation lofOffsetRot = lofOffsetLaw.getAttitude(circ, date, circ.getFrame()).getRotation(); // Compose rotations : target pointing attitudes final double angleCompo = targetRot.applyInverseTo(lofOffsetRot).getAngle(); Assert.assertEquals(0., angleCompo, Utils.epsilonAngle); }
From source file:org.orekit.attitudes.NadirPointingTest.java
/** Test in the case of a spheric earth : nadir pointing shall be * the same as earth center pointing//from w ww . j a v a 2 s . com */ @Test public void testSphericEarth() throws OrekitException { // Spheric earth shape OneAxisEllipsoid earthShape = new OneAxisEllipsoid(6378136.460, 0., itrf); // Create nadir pointing attitude provider NadirPointing nadirAttitudeLaw = new NadirPointing(FramesFactory.getEME2000(), earthShape); // Create earth center pointing attitude provider BodyCenterPointing earthCenterAttitudeLaw = new BodyCenterPointing(FramesFactory.getEME2000(), earthShape); // Create satellite position as circular parameters CircularOrbit circ = new CircularOrbit(7178000.0, 0.5e-4, -0.5e-4, FastMath.toRadians(50.), FastMath.toRadians(270.), FastMath.toRadians(5.300), PositionAngle.MEAN, FramesFactory.getEME2000(), date, mu); // Get nadir attitude Rotation rotNadir = nadirAttitudeLaw.getAttitude(circ, date, circ.getFrame()).getRotation(); // Get earth center attitude Rotation rotCenter = earthCenterAttitudeLaw.getAttitude(circ, date, circ.getFrame()).getRotation(); // For a spheric earth, earth center pointing attitude and nadir pointing attitude // shall be the same, i.e the composition of inverse earth pointing rotation // with nadir pointing rotation shall be identity. Rotation rotCompo = rotCenter.applyInverseTo(rotNadir); double angle = rotCompo.getAngle(); Assert.assertEquals(angle, 0.0, Utils.epsilonAngle); }
From source file:org.orekit.attitudes.NadirPointingTest.java
/** Test in the case of an elliptic earth : nadir pointing shall be : * - the same as earth center pointing in case of equatorial or polar position * - different from earth center pointing in any other case *///from w w w .java 2 s . co m @Test public void testNonSphericEarth() throws OrekitException { // Elliptic earth shape OneAxisEllipsoid earthShape = new OneAxisEllipsoid(6378136.460, 1 / 298.257222101, itrf); // Create nadir pointing attitude provider NadirPointing nadirAttitudeLaw = new NadirPointing(FramesFactory.getEME2000(), earthShape); // Create earth center pointing attitude provider BodyCenterPointing earthCenterAttitudeLaw = new BodyCenterPointing(FramesFactory.getEME2000(), earthShape); // Satellite on equatorial position // ********************************** KeplerianOrbit kep = new KeplerianOrbit(7178000.0, 1.e-8, FastMath.toRadians(50.), 0., 0., 0., PositionAngle.TRUE, FramesFactory.getEME2000(), date, mu); // Get nadir attitude Rotation rotNadir = nadirAttitudeLaw.getAttitude(kep, date, kep.getFrame()).getRotation(); // Get earth center attitude Rotation rotCenter = earthCenterAttitudeLaw.getAttitude(kep, date, kep.getFrame()).getRotation(); // For a satellite at equatorial position, earth center pointing attitude and nadir pointing // attitude shall be the same, i.e the composition of inverse earth pointing rotation // with nadir pointing rotation shall be identity. Rotation rotCompo = rotCenter.applyInverseTo(rotNadir); double angle = rotCompo.getAngle(); Assert.assertEquals(0.0, angle, 5.e-6); // Satellite on polar position // ***************************** CircularOrbit circ = new CircularOrbit(7178000.0, 1.e-5, 0., FastMath.toRadians(90.), 0., FastMath.toRadians(90.), PositionAngle.TRUE, FramesFactory.getEME2000(), date, mu); // Get nadir attitude rotNadir = nadirAttitudeLaw.getAttitude(circ, date, circ.getFrame()).getRotation(); // Get earth center attitude rotCenter = earthCenterAttitudeLaw.getAttitude(circ, date, circ.getFrame()).getRotation(); // For a satellite at polar position, earth center pointing attitude and nadir pointing // attitude shall be the same, i.e the composition of inverse earth pointing rotation // with nadir pointing rotation shall be identity. rotCompo = rotCenter.applyInverseTo(rotNadir); angle = rotCompo.getAngle(); Assert.assertEquals(angle, 0.0, 5.e-6); // Satellite on any position // *************************** circ = new CircularOrbit(7178000.0, 1.e-5, 0., FastMath.toRadians(50.), 0., FastMath.toRadians(90.), PositionAngle.TRUE, FramesFactory.getEME2000(), date, mu); // Get nadir attitude rotNadir = nadirAttitudeLaw.getAttitude(circ, date, circ.getFrame()).getRotation(); // Get earth center attitude rotCenter = earthCenterAttitudeLaw.getAttitude(circ, date, circ.getFrame()).getRotation(); // For a satellite at any position, earth center pointing attitude and nadir pointing // and nadir pointing attitude shall not be the same, i.e the composition of inverse earth // pointing rotation with nadir pointing rotation shall be different from identity. rotCompo = rotCenter.applyInverseTo(rotNadir); angle = rotCompo.getAngle(); Assert.assertEquals(angle, FastMath.toRadians(0.16797386586252272), Utils.epsilonAngle); }
From source file:org.orekit.attitudes.NadirPointingTest.java
/** Vertical test : check that Z satellite axis is collinear to local vertical axis, which direction is : (cos(lon)*cos(lat), sin(lon)*cos(lat), sin(lat)), where lon et lat stand for observed point coordinates (i.e satellite ones, since they are the same by construction, but that's what is to test.//from w w w . ja v a 2s .co m */ @Test public void testVertical() throws OrekitException { // Elliptic earth shape OneAxisEllipsoid earthShape = new OneAxisEllipsoid(6378136.460, 1 / 298.257222101, itrf); // Create earth center pointing attitude provider NadirPointing nadirAttitudeLaw = new NadirPointing(FramesFactory.getEME2000(), earthShape); // Satellite on any position CircularOrbit circ = new CircularOrbit(7178000.0, 1.e-5, 0., FastMath.toRadians(50.), 0., FastMath.toRadians(90.), PositionAngle.TRUE, FramesFactory.getEME2000(), date, mu); // Vertical test // *************** // Get observed ground point position/velocity TimeStampedPVCoordinates pvTargetItrf = nadirAttitudeLaw.getTargetPV(circ, date, itrf); // Convert to geodetic coordinates GeodeticPoint geoTarget = earthShape.transform(pvTargetItrf.getPosition(), itrf, date); // Compute local vertical axis double xVert = FastMath.cos(geoTarget.getLongitude()) * FastMath.cos(geoTarget.getLatitude()); double yVert = FastMath.sin(geoTarget.getLongitude()) * FastMath.cos(geoTarget.getLatitude()); double zVert = FastMath.sin(geoTarget.getLatitude()); Vector3D targetVertical = new Vector3D(xVert, yVert, zVert); // Get attitude rotation state Rotation rotSatEME2000 = nadirAttitudeLaw.getAttitude(circ, date, circ.getFrame()).getRotation(); // Get satellite Z axis in EME2000 frame Vector3D zSatEME2000 = rotSatEME2000.applyInverseTo(Vector3D.PLUS_K); Vector3D zSatItrf = FramesFactory.getEME2000().getTransformTo(itrf, date).transformVector(zSatEME2000); // Check that satellite Z axis is collinear to local vertical axis double angle = Vector3D.angle(zSatItrf, targetVertical); Assert.assertEquals(0.0, FastMath.sin(angle), Utils.epsilonTest); }
From source file:org.orekit.attitudes.TargetPointingTest.java
/** Test if both constructors are equivalent */// w w w .ja v a2 s . c om @Test public void testConstructors() throws OrekitException { // Satellite position // ******************** CircularOrbit circ = new CircularOrbit(7178000.0, 0.5e-4, -0.5e-4, FastMath.toRadians(50.), FastMath.toRadians(270.), FastMath.toRadians(5.300), PositionAngle.MEAN, FramesFactory.getEME2000(), date, mu); // Attitude laws // *************** // Elliptic earth shape OneAxisEllipsoid earthShape = new OneAxisEllipsoid(6378136.460, 1 / 298.257222101, itrf); // Target definition as a geodetic point AND as a position/velocity vector GeodeticPoint geoTargetITRF2005C = new GeodeticPoint(FastMath.toRadians(43.36), FastMath.toRadians(1.26), 600.); Vector3D pTargetITRF2005C = earthShape.transform(geoTargetITRF2005C); // Attitude law definition from geodetic point target TargetPointing geoTargetAttitudeLaw = new TargetPointing(circ.getFrame(), geoTargetITRF2005C, earthShape); // Attitude law definition from position/velocity target TargetPointing pvTargetAttitudeLaw = new TargetPointing(circ.getFrame(), itrf, pTargetITRF2005C); // Check that both attitude are the same // Get satellite rotation for target pointing law Rotation rotPv = pvTargetAttitudeLaw.getAttitude(circ, date, circ.getFrame()).getRotation(); // Get satellite rotation for nadir pointing law Rotation rotGeo = geoTargetAttitudeLaw.getAttitude(circ, date, circ.getFrame()).getRotation(); // Rotations composition Rotation rotCompo = rotGeo.applyInverseTo(rotPv); double angle = rotCompo.getAngle(); Assert.assertEquals(angle, 0.0, Utils.epsilonAngle); }
From source file:org.orekit.attitudes.TargetPointingTest.java
/** Test with nadir target : Check that when the target is the same as nadir target at date, * satellite attitude is the same as nadir attitude at the same date, but different at a different date. *///from w w w.j a v a 2 s .c o m @Test public void testNadirTarget() throws OrekitException { // Elliptic earth shape OneAxisEllipsoid earthShape = new OneAxisEllipsoid(6378136.460, 1 / 298.257222101, itrf); // Satellite on any position CircularOrbit circOrbit = new CircularOrbit(7178000.0, 1.e-5, 0., FastMath.toRadians(50.), 0., FastMath.toRadians(90.), PositionAngle.TRUE, FramesFactory.getEME2000(), date, mu); // Target attitude provider with target under satellite nadir // ******************************************************* // Definition of nadir target // Create nadir pointing attitude provider NadirPointing nadirAttitudeLaw = new NadirPointing(circOrbit.getFrame(), earthShape); // Check nadir target Vector3D pNadirTarget = nadirAttitudeLaw.getTargetPV(circOrbit, date, itrf).getPosition(); GeodeticPoint geoNadirTarget = earthShape.transform(pNadirTarget, itrf, date); // Create target attitude provider TargetPointing targetAttitudeLaw = new TargetPointing(circOrbit.getFrame(), geoNadirTarget, earthShape); // 1/ Test that attitudes are the same at date // ********************************************* // i.e the composition of inverse earth pointing rotation // with nadir pointing rotation shall be identity. // Get satellite rotation from target pointing law at date Rotation rotTarget = targetAttitudeLaw.getAttitude(circOrbit, date, circOrbit.getFrame()).getRotation(); // Get satellite rotation from nadir pointing law at date Rotation rotNadir = nadirAttitudeLaw.getAttitude(circOrbit, date, circOrbit.getFrame()).getRotation(); // Compose attitude rotations Rotation rotCompo = rotTarget.applyInverseTo(rotNadir); double angle = rotCompo.getAngle(); Assert.assertEquals(angle, 0.0, Utils.epsilonAngle); // 2/ Test that attitudes are different at a different date // ********************************************************** // Extrapolation one minute later KeplerianPropagator extrapolator = new KeplerianPropagator(circOrbit); double delta_t = 60.0; // extrapolation duration in seconds AbsoluteDate extrapDate = date.shiftedBy(delta_t); Orbit extrapOrbit = extrapolator.propagate(extrapDate).getOrbit(); // Get satellite rotation from target pointing law at date + 1min Rotation extrapRotTarget = targetAttitudeLaw.getAttitude(extrapOrbit, extrapDate, extrapOrbit.getFrame()) .getRotation(); // Get satellite rotation from nadir pointing law at date Rotation extrapRotNadir = nadirAttitudeLaw.getAttitude(extrapOrbit, extrapDate, extrapOrbit.getFrame()) .getRotation(); // Compose attitude rotations Rotation extrapRotCompo = extrapRotTarget.applyInverseTo(extrapRotNadir); double extrapAngle = extrapRotCompo.getAngle(); Assert.assertEquals(extrapAngle, FastMath.toRadians(24.684793905118823), Utils.epsilonAngle); }