KinematicClass.f90 Source File

module KinematicClass
   !! Some basic functions for Kinematics
   use, intrinsic :: iso_fortran_env
   use MathClass

   implicit none

contains

! #############################################
!> It returns 3-D rotation matrix.
   function RotationMatrix3D(rotx, roty, rotz, n, angle) result(Rmat)
      real(real64)::Rmat(3, 3)
      real(real64), optional, intent(in)::rotx, roty, rotz, n(3), angle
      integer(int32) :: i
      Rmat(:, :) = 0.0d0
      do i = 1, 3
         Rmat(i, i) = 1.0d0
      end do

      if (present(rotx)) then
         Rmat(2, 2) = cos(rotx)
         Rmat(2, 3) = -sin(rotx)
         Rmat(3, 2) = sin(rotx)
         Rmat(3, 3) = cos(rotx)
      end if

      if (present(rotx)) then
         Rmat(1, 1) = cos(rotx)
         Rmat(1, 3) = sin(rotx)
         Rmat(3, 1) = -sin(rotx)
         Rmat(3, 3) = cos(rotx)
      end if

      if (present(rotx)) then
         Rmat(2, 2) = cos(rotx)
         Rmat(2, 3) = -sin(rotx)
         Rmat(3, 2) = sin(rotx)
         Rmat(3, 3) = cos(rotx)
      end if

      if (present(n) .and. present(angle)) then
         ! Rodrigues' rotation formula
         Rmat(1, 1) = cos(angle) + n(1)*n(1)*(1.0d0 - cos(angle))
         Rmat(1, 2) = n(1)*n(2)*(1.0d0 - cos(angle)) - n(3)*sin(angle)
         Rmat(1, 3) = n(1)*n(3)*(1.0d0 + cos(angle)) - n(2)*sin(angle)
         Rmat(2, 1) = n(2)*n(1)*(1.0d0 + cos(angle)) - n(3)*sin(angle)
         Rmat(2, 2) = cos(angle) + n(2)*n(2)*(1.0d0 - cos(angle))
         Rmat(2, 3) = n(2)*n(3)*(1.0d0 - cos(angle)) - n(1)*sin(angle)
         Rmat(3, 1) = n(3)*n(1)*(1.0d0 - cos(angle)) - n(2)*sin(angle)
         Rmat(3, 2) = n(3)*n(2)*(1.0d0 + cos(angle)) - n(1)*sin(angle)
         Rmat(3, 3) = cos(angle) + n(3)*n(3)*(1.0d0 - cos(angle))
      end if

   end function
! #############################################

! #############################################
!> It rotates 3-D vector.
   function Rotation3D(vector, rotx, roty, rotz, n, angle) result(vec)
      real(real64)::vec(3)
      real(real64), intent(in)::vector(3)
      real(real64), optional, intent(in)::rotx, roty, rotz, n(3), angle

      vec(:) = matmul(RotationMatrix3D(rotx, roty, rotz, n, angle), vector)

   end function
! #############################################

end module