1 /* Copyright 2002-2019 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.frames; 18 19 import org.hipparchus.Field; 20 import org.hipparchus.RealFieldElement; 21 import org.hipparchus.analysis.UnivariateFunction; 22 import org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver; 23 import org.hipparchus.analysis.solvers.UnivariateSolver; 24 import org.hipparchus.exception.MathRuntimeException; 25 import org.hipparchus.geometry.euclidean.threed.FieldVector3D; 26 import org.hipparchus.geometry.euclidean.threed.Rotation; 27 import org.hipparchus.geometry.euclidean.threed.Vector3D; 28 import org.hipparchus.util.FastMath; 29 import org.hipparchus.util.MathUtils; 30 import org.orekit.bodies.BodyShape; 31 import org.orekit.bodies.FieldGeodeticPoint; 32 import org.orekit.bodies.GeodeticPoint; 33 import org.orekit.errors.OrekitException; 34 import org.orekit.time.AbsoluteDate; 35 import org.orekit.time.FieldAbsoluteDate; 36 import org.orekit.utils.Constants; 37 import org.orekit.utils.FieldPVCoordinates; 38 import org.orekit.utils.PVCoordinates; 39 import org.orekit.utils.PVCoordinatesProvider; 40 import org.orekit.utils.TimeStampedPVCoordinates; 41 42 43 /** Topocentric frame. 44 * <p>Frame associated to a position near the surface of a body shape.</p> 45 * <p> 46 * The origin of the frame is at the defining {@link GeodeticPoint geodetic point} 47 * location, and the right-handed canonical trihedra is: 48 * </p> 49 * <ul> 50 * <li>X axis in the local horizontal plane (normal to zenith direction) and 51 * following the local parallel towards East</li> 52 * <li>Y axis in the horizontal plane (normal to zenith direction) and 53 * following the local meridian towards North</li> 54 * <li>Z axis towards Zenith direction</li> 55 * </ul> 56 * @author Véronique Pommier-Maurussane 57 */ 58 public class TopocentricFrame extends Frame implements PVCoordinatesProvider { 59 60 /** Serializable UID. */ 61 private static final long serialVersionUID = -5997915708080966466L; 62 63 /** Body shape on which the local point is defined. */ 64 private final BodyShape parentShape; 65 66 /** Point where the topocentric frame is defined. */ 67 private final GeodeticPoint point; 68 69 /** Simple constructor. 70 * @param parentShape body shape on which the local point is defined 71 * @param point local surface point where topocentric frame is defined 72 * @param name the string representation 73 */ 74 public TopocentricFrame(final BodyShape parentShape, final GeodeticPoint point, 75 final String name) { 76 77 super(parentShape.getBodyFrame(), 78 new Transform(AbsoluteDate.J2000_EPOCH, 79 new Transform(AbsoluteDate.J2000_EPOCH, 80 parentShape.transform(point).negate()), 81 new Transform(AbsoluteDate.J2000_EPOCH, 82 new Rotation(point.getEast(), point.getZenith(), 83 Vector3D.PLUS_I, Vector3D.PLUS_K), 84 Vector3D.ZERO)), 85 name, false); 86 this.parentShape = parentShape; 87 this.point = point; 88 } 89 90 /** Get the body shape on which the local point is defined. 91 * @return body shape on which the local point is defined 92 */ 93 public BodyShape getParentShape() { 94 return parentShape; 95 } 96 97 /** Get the surface point defining the origin of the frame. 98 * @return surface point defining the origin of the frame 99 */ 100 public GeodeticPoint getPoint() { 101 return point; 102 } 103 104 /** Get the surface point defining the origin of the frame. 105 * @param <T> tyoe of the elements 106 * @param field of the elements 107 * @return surface point defining the origin of the frame 108 * @since 9.3 109 */ 110 public <T extends RealFieldElement<T>> FieldGeodeticPoint<T> getPoint(final Field<T> field) { 111 final T zero = field.getZero(); 112 return new FieldGeodeticPoint<>(zero.add(point.getLatitude()), 113 zero.add(point.getLongitude()), 114 zero.add(point.getAltitude())); 115 } 116 117 /** Get the zenith direction of topocentric frame, expressed in parent shape frame. 118 * <p>The zenith direction is defined as the normal to local horizontal plane.</p> 119 * @return unit vector in the zenith direction 120 * @see #getNadir() 121 */ 122 public Vector3D getZenith() { 123 return point.getZenith(); 124 } 125 126 /** Get the nadir direction of topocentric frame, expressed in parent shape frame. 127 * <p>The nadir direction is the opposite of zenith direction.</p> 128 * @return unit vector in the nadir direction 129 * @see #getZenith() 130 */ 131 public Vector3D getNadir() { 132 return point.getNadir(); 133 } 134 135 /** Get the north direction of topocentric frame, expressed in parent shape frame. 136 * <p>The north direction is defined in the horizontal plane 137 * (normal to zenith direction) and following the local meridian.</p> 138 * @return unit vector in the north direction 139 * @see #getSouth() 140 */ 141 public Vector3D getNorth() { 142 return point.getNorth(); 143 } 144 145 /** Get the south direction of topocentric frame, expressed in parent shape frame. 146 * <p>The south direction is the opposite of north direction.</p> 147 * @return unit vector in the south direction 148 * @see #getNorth() 149 */ 150 public Vector3D getSouth() { 151 return point.getSouth(); 152 } 153 154 /** Get the east direction of topocentric frame, expressed in parent shape frame. 155 * <p>The east direction is defined in the horizontal plane 156 * in order to complete direct triangle (east, north, zenith).</p> 157 * @return unit vector in the east direction 158 * @see #getWest() 159 */ 160 public Vector3D getEast() { 161 return point.getEast(); 162 } 163 164 /** Get the west direction of topocentric frame, expressed in parent shape frame. 165 * <p>The west direction is the opposite of east direction.</p> 166 * @return unit vector in the west direction 167 * @see #getEast() 168 */ 169 public Vector3D getWest() { 170 return point.getWest(); 171 } 172 173 /** Get the elevation of a point with regards to the local point. 174 * <p>The elevation is the angle between the local horizontal and 175 * the direction from local point to given point.</p> 176 * @param extPoint point for which elevation shall be computed 177 * @param frame frame in which the point is defined 178 * @param date computation date 179 * @return elevation of the point 180 */ 181 public double getElevation(final Vector3D extPoint, final Frame frame, 182 final AbsoluteDate date) { 183 184 // Transform given point from given frame to topocentric frame 185 final Transform t = frame.getTransformTo(this, date); 186 final Vector3D extPointTopo = t.transformPosition(extPoint); 187 188 // Elevation angle is PI/2 - angle between zenith and given point direction 189 return extPointTopo.getDelta(); 190 } 191 192 /** Get the elevation of a point with regards to the local point. 193 * <p>The elevation is the angle between the local horizontal and 194 * the direction from local point to given point.</p> 195 * @param <T> type of the elements 196 * @param extPoint point for which elevation shall be computed 197 * @param frame frame in which the point is defined 198 * @param date computation date 199 * @return elevation of the point 200 * @since 9.3 201 */ 202 public <T extends RealFieldElement<T>> T getElevation(final FieldVector3D<T> extPoint, final Frame frame, 203 final FieldAbsoluteDate<T> date) { 204 205 // Transform given point from given frame to topocentric frame 206 final FieldTransform<T> t = frame.getTransformTo(this, date); 207 final FieldVector3D<T> extPointTopo = t.transformPosition(extPoint); 208 209 // Elevation angle is PI/2 - angle between zenith and given point direction 210 return extPointTopo.getDelta(); 211 } 212 213 /** Get the azimuth of a point with regards to the topocentric frame center point. 214 * <p>The azimuth is the angle between the North direction at local point and 215 * the projection in local horizontal plane of the direction from local point 216 * to given point. Azimuth angles are counted clockwise, i.e positive towards the East.</p> 217 * @param extPoint point for which elevation shall be computed 218 * @param frame frame in which the point is defined 219 * @param date computation date 220 * @return azimuth of the point 221 */ 222 public double getAzimuth(final Vector3D extPoint, final Frame frame, 223 final AbsoluteDate date) { 224 225 // Transform given point from given frame to topocentric frame 226 final Transform t = getTransformTo(frame, date).getInverse(); 227 final Vector3D extPointTopo = t.transformPosition(extPoint); 228 229 // Compute azimuth 230 double azimuth = FastMath.atan2(extPointTopo.getX(), extPointTopo.getY()); 231 if (azimuth < 0.) { 232 azimuth += MathUtils.TWO_PI; 233 } 234 return azimuth; 235 236 } 237 238 /** Get the azimuth of a point with regards to the topocentric frame center point. 239 * <p>The azimuth is the angle between the North direction at local point and 240 * the projection in local horizontal plane of the direction from local point 241 * to given point. Azimuth angles are counted clockwise, i.e positive towards the East.</p> 242 * @param <T> type of the elements 243 * @param extPoint point for which elevation shall be computed 244 * @param frame frame in which the point is defined 245 * @param date computation date 246 * @return azimuth of the point 247 * @since 9.3 248 */ 249 public <T extends RealFieldElement<T>> T getAzimuth(final FieldVector3D<T> extPoint, final Frame frame, 250 final FieldAbsoluteDate<T> date) { 251 252 // Transform given point from given frame to topocentric frame 253 final FieldTransform<T> t = getTransformTo(frame, date).getInverse(); 254 final FieldVector3D<T> extPointTopo = t.transformPosition(extPoint); 255 256 // Compute azimuth 257 T azimuth = FastMath.atan2(extPointTopo.getX(), extPointTopo.getY()); 258 if (azimuth.getReal() < 0.) { 259 azimuth = azimuth.add(MathUtils.TWO_PI); 260 } 261 return azimuth; 262 263 } 264 265 /** Get the range of a point with regards to the topocentric frame center point. 266 * @param extPoint point for which range shall be computed 267 * @param frame frame in which the point is defined 268 * @param date computation date 269 * @return range (distance) of the point 270 */ 271 public double getRange(final Vector3D extPoint, final Frame frame, 272 final AbsoluteDate date) { 273 274 // Transform given point from given frame to topocentric frame 275 final Transform t = frame.getTransformTo(this, date); 276 final Vector3D extPointTopo = t.transformPosition(extPoint); 277 278 // Compute range 279 return extPointTopo.getNorm(); 280 281 } 282 283 /** Get the range of a point with regards to the topocentric frame center point. 284 * @param <T> type of the elements 285 * @param extPoint point for which range shall be computed 286 * @param frame frame in which the point is defined 287 * @param date computation date 288 * @return range (distance) of the point 289 * @since 9.3 290 */ 291 public <T extends RealFieldElement<T>> T getRange(final FieldVector3D<T> extPoint, final Frame frame, 292 final FieldAbsoluteDate<T> date) { 293 294 // Transform given point from given frame to topocentric frame 295 final FieldTransform<T> t = frame.getTransformTo(this, date); 296 final FieldVector3D<T> extPointTopo = t.transformPosition(extPoint); 297 298 // Compute range 299 return extPointTopo.getNorm(); 300 301 } 302 303 /** Get the range rate of a point with regards to the topocentric frame center point. 304 * @param extPV point/velocity for which range rate shall be computed 305 * @param frame frame in which the point is defined 306 * @param date computation date 307 * @return range rate of the point (positive if point departs from frame) 308 */ 309 public double getRangeRate(final PVCoordinates extPV, final Frame frame, 310 final AbsoluteDate date) { 311 312 // Transform given point from given frame to topocentric frame 313 final Transform t = frame.getTransformTo(this, date); 314 final PVCoordinates extPVTopo = t.transformPVCoordinates(extPV); 315 316 // Compute range rate (doppler) : relative rate along the line of sight 317 return Vector3D.dotProduct(extPVTopo.getPosition(), extPVTopo.getVelocity()) / 318 extPVTopo.getPosition().getNorm(); 319 320 } 321 322 /** Get the range rate of a point with regards to the topocentric frame center point. 323 * @param <T> type of the elements 324 * @param extPV point/velocity for which range rate shall be computed 325 * @param frame frame in which the point is defined 326 * @param date computation date 327 * @return range rate of the point (positive if point departs from frame) 328 * @since 9.3 329 */ 330 public <T extends RealFieldElement<T>> T getRangeRate(final FieldPVCoordinates<T> extPV, final Frame frame, 331 final FieldAbsoluteDate<T> date) { 332 333 // Transform given point from given frame to topocentric frame 334 final FieldTransform<T> t = frame.getTransformTo(this, date); 335 final FieldPVCoordinates<T> extPVTopo = t.transformPVCoordinates(extPV); 336 337 // Compute range rate (doppler) : relative rate along the line of sight 338 return FieldVector3D.dotProduct(extPVTopo.getPosition(), extPVTopo.getVelocity()).divide( 339 extPVTopo.getPosition().getNorm()); 340 341 } 342 343 /** 344 * Compute the limit visibility point for a satellite in a given direction. 345 * <p> 346 * This method can be used to compute visibility circles around ground stations 347 * for example, using a simple loop on azimuth, with either a fixed elevation 348 * or an elevation that depends on azimuth to take ground masks into account. 349 * </p> 350 * @param radius satellite distance to Earth center 351 * @param azimuth pointing azimuth from station 352 * @param elevation pointing elevation from station 353 * @return limit visibility point for the satellite 354 */ 355 public GeodeticPoint computeLimitVisibilityPoint(final double radius, 356 final double azimuth, final double elevation) { 357 try { 358 // convergence threshold on point position: 1mm 359 final double deltaP = 0.001; 360 final UnivariateSolver solver = 361 new BracketingNthOrderBrentSolver(deltaP / Constants.WGS84_EARTH_EQUATORIAL_RADIUS, 362 deltaP, deltaP, 5); 363 364 // find the distance such that a point in the specified direction and at the solved-for 365 // distance is exactly at the specified radius 366 final double distance = solver.solve(1000, new UnivariateFunction() { 367 /** {@inheritDoc} */ 368 public double value(final double x) { 369 final GeodeticPoint gp = pointAtDistance(azimuth, elevation, x); 370 return parentShape.transform(gp).getNorm() - radius; 371 } 372 }, 0, 2 * radius); 373 374 // return the limit point 375 return pointAtDistance(azimuth, elevation, distance); 376 377 } catch (MathRuntimeException mrte) { 378 throw new OrekitException(mrte); 379 } 380 } 381 382 /** Compute the point observed from the station at some specified distance. 383 * @param azimuth pointing azimuth from station 384 * @param elevation pointing elevation from station 385 * @param distance distance to station 386 * @return observed point 387 */ 388 public GeodeticPoint pointAtDistance(final double azimuth, final double elevation, 389 final double distance) { 390 final double cosAz = FastMath.cos(azimuth); 391 final double sinAz = FastMath.sin(azimuth); 392 final double cosEl = FastMath.cos(elevation); 393 final double sinEl = FastMath.sin(elevation); 394 final Vector3D observed = new Vector3D(distance * cosEl * sinAz, 395 distance * cosEl * cosAz, 396 distance * sinEl); 397 return parentShape.transform(observed, this, AbsoluteDate.J2000_EPOCH); 398 } 399 400 /** Get the {@link PVCoordinates} of the topocentric frame origin in the selected frame. 401 * @param date current date 402 * @param frame the frame where to define the position 403 * @return position/velocity of the topocentric frame origin (m and m/s) 404 */ 405 public TimeStampedPVCoordinates getPVCoordinates(final AbsoluteDate date, final Frame frame) { 406 return getTransformTo(frame, date).transformPVCoordinates(new TimeStampedPVCoordinates(date, 407 Vector3D.ZERO, 408 Vector3D.ZERO, 409 Vector3D.ZERO)); 410 } 411 412 }