Added transformation routines.

Author: interkosmos

src/dm_transform.f90

@@ -5,15 +5,18 @@ module dm_transform
     use :: dm_const
     use :: dm_error
     use :: dm_kind
-    use :: dm_la
     use :: dm_util
     implicit none (type, external)
     private
 
     public :: dm_transform_coord_2d
     public :: dm_transform_polar_3d
+    public :: dm_transform_cartesian_to_polar
+    public :: dm_transform_polar_to_cartesian
 contains
     integer function dm_transform_coord_2d(cs, ct, observs, trans, params, residuals, variance, rotation, scale_factor) result(rc)
+        use :: dm_la
+
         integer, parameter :: NRHS = 4 !! Number of columns in matrix A.
 
         real(kind=r8),              intent(inout)         :: cs(:)        !! Common control points in source system.
@@ -113,21 +116,47 @@ integer function dm_transform_polar_3d(vx, vy, vz, tx, ty, hz, v, dist_hz, x, y,
         if (present(azimuth)) then
             ! Remove the global azimuth angle of the sensor from the
             ! calculated local azimuth.
-            if (z /= 0.0) z = z - azimuth
+            if (.not. dm_equals(z, 0.0_r8)) z = z - azimuth
         end if
 
         ! Append the measured horizontal direction to the angle.
         t = z + hz
 
         ! Calculate coordinates of the target point.
-        dx = dist_hz * sin(v) * cos(t)
-        dy = dist_hz * sin(v) * sin(t)
-        dz = dist_hz * cos(v)
+        call dm_transform_polar_to_cartesian(t, v, dist_hz, dx, dy, dz)
 
         x = vx + dx
         y = vy + dy
         z = vz + dz
 
         rc = E_NONE
     end function dm_transform_polar_3d
+
+    pure elemental subroutine dm_transform_cartesian_to_polar(x, y, z, hz, v, r)
+        !! Transforms cartesian coordinates to polar (spherical) coordinates.
+        real(kind=r8), intent(in)  :: x  !! X coordinate.
+        real(kind=r8), intent(in)  :: y  !! Y coordinate.
+        real(kind=r8), intent(in)  :: z  !! Z coordinate.
+        real(kind=r8), intent(out) :: hz !! Horizontal angle (phi).
+        real(kind=r8), intent(out) :: v  !! Vertical angle (omega).
+        real(kind=r8), intent(out) :: r  !! Radius.
+
+        r  = sqrt(x**2 + y**2 + z**2)
+        hz = atan2(y, x)
+        v  = acos(z / r)
+    end subroutine dm_transform_cartesian_to_polar
+
+    pure elemental subroutine dm_transform_polar_to_cartesian(hz, v, r, x, y, z)
+        !! Transforms polar (spherical) coordinates to cartesian coordinates.
+        real(kind=r8), intent(in)  :: hz !! Horizontal angle (phi).
+        real(kind=r8), intent(in)  :: v  !! Vertical angle (omega).
+        real(kind=r8), intent(in)  :: r  !! Radius.
+        real(kind=r8), intent(out) :: x  !! X coordinate.
+        real(kind=r8), intent(out) :: y  !! Y coordinate.
+        real(kind=r8), intent(out) :: z  !! Z coordinate.
+
+        x = r * sin(v) * cos(hz)
+        y = r * sin(v) * sin(hz)
+        z = r * cos(v)
+    end subroutine dm_transform_polar_to_cartesian
 end module dm_transform