polishing
This commit is contained in:
parent
9e0a0ee166
commit
9f4e354b12
|
@ -27,7 +27,7 @@
|
||||||
! USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
! USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||||
! ###################################################################
|
! ###################################################################
|
||||||
|
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
!> @author Marc De Graef, Carnegie Mellon University
|
!> @author Marc De Graef, Carnegie Mellon University
|
||||||
!> @author Martin Diehl, Max-Planck-Institut für Eisenforschung GmbH
|
!> @author Martin Diehl, Max-Planck-Institut für Eisenforschung GmbH
|
||||||
!> @brief rotation storage and conversion
|
!> @brief rotation storage and conversion
|
||||||
|
@ -44,7 +44,7 @@
|
||||||
! Convention 5: the rotation angle ω is limited to the interval [0, π]
|
! Convention 5: the rotation angle ω is limited to the interval [0, π]
|
||||||
! Convention 6: the real part of a quaternion is positive, Re(q) > 0
|
! Convention 6: the real part of a quaternion is positive, Re(q) > 0
|
||||||
! Convention 7: P = -1
|
! Convention 7: P = -1
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
|
|
||||||
module rotations
|
module rotations
|
||||||
use IO
|
use IO
|
||||||
|
@ -111,60 +111,64 @@ subroutine rotations_init
|
||||||
end subroutine rotations_init
|
end subroutine rotations_init
|
||||||
|
|
||||||
|
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
! Return rotation in different representations
|
! Return rotation in different representations.
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
pure function asQuaternion(self)
|
pure function asQuaternion(self)
|
||||||
|
|
||||||
class(tRotation), intent(in) :: self
|
class(tRotation), intent(in) :: self
|
||||||
real(pReal), dimension(4) :: asQuaternion
|
real(pReal), dimension(4) :: asQuaternion
|
||||||
|
|
||||||
|
|
||||||
asQuaternion = self%q
|
asQuaternion = self%q
|
||||||
|
|
||||||
end function asQuaternion
|
end function asQuaternion
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
pure function asEulers(self)
|
pure function asEulers(self)
|
||||||
|
|
||||||
class(tRotation), intent(in) :: self
|
class(tRotation), intent(in) :: self
|
||||||
real(pReal), dimension(3) :: asEulers
|
real(pReal), dimension(3) :: asEulers
|
||||||
|
|
||||||
|
|
||||||
asEulers = qu2eu(self%q)
|
asEulers = qu2eu(self%q)
|
||||||
|
|
||||||
end function asEulers
|
end function asEulers
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
pure function asAxisAngle(self)
|
pure function asAxisAngle(self)
|
||||||
|
|
||||||
class(tRotation), intent(in) :: self
|
class(tRotation), intent(in) :: self
|
||||||
real(pReal), dimension(4) :: asAxisAngle
|
real(pReal), dimension(4) :: asAxisAngle
|
||||||
|
|
||||||
|
|
||||||
asAxisAngle = qu2ax(self%q)
|
asAxisAngle = qu2ax(self%q)
|
||||||
|
|
||||||
end function asAxisAngle
|
end function asAxisAngle
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
pure function asMatrix(self)
|
pure function asMatrix(self)
|
||||||
|
|
||||||
class(tRotation), intent(in) :: self
|
class(tRotation), intent(in) :: self
|
||||||
real(pReal), dimension(3,3) :: asMatrix
|
real(pReal), dimension(3,3) :: asMatrix
|
||||||
|
|
||||||
|
|
||||||
asMatrix = qu2om(self%q)
|
asMatrix = qu2om(self%q)
|
||||||
|
|
||||||
end function asMatrix
|
end function asMatrix
|
||||||
|
|
||||||
|
!--------------------------------------------------------------------------------------------------
|
||||||
!---------------------------------------------------------------------------------------------------
|
! Initialize rotation from different representations.
|
||||||
! Initialize rotation from different representations
|
!--------------------------------------------------------------------------------------------------
|
||||||
!---------------------------------------------------------------------------------------------------
|
|
||||||
subroutine fromQuaternion(self,qu)
|
subroutine fromQuaternion(self,qu)
|
||||||
|
|
||||||
class(tRotation), intent(out) :: self
|
class(tRotation), intent(out) :: self
|
||||||
real(pReal), dimension(4), intent(in) :: qu
|
real(pReal), dimension(4), intent(in) :: qu
|
||||||
|
|
||||||
|
|
||||||
if (dNeq(norm2(qu),1.0_pReal,1.0e-8_pReal)) call IO_error(402,ext_msg='fromQuaternion')
|
if (dNeq(norm2(qu),1.0_pReal,1.0e-8_pReal)) call IO_error(402,ext_msg='fromQuaternion')
|
||||||
|
|
||||||
self%q = qu
|
self%q = qu
|
||||||
|
|
||||||
end subroutine fromQuaternion
|
end subroutine fromQuaternion
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
subroutine fromEulers(self,eu,degrees)
|
subroutine fromEulers(self,eu,degrees)
|
||||||
|
|
||||||
class(tRotation), intent(out) :: self
|
class(tRotation), intent(out) :: self
|
||||||
|
@ -173,6 +177,7 @@ subroutine fromEulers(self,eu,degrees)
|
||||||
|
|
||||||
real(pReal), dimension(3) :: Eulers
|
real(pReal), dimension(3) :: Eulers
|
||||||
|
|
||||||
|
|
||||||
if (.not. present(degrees)) then
|
if (.not. present(degrees)) then
|
||||||
Eulers = eu
|
Eulers = eu
|
||||||
else
|
else
|
||||||
|
@ -185,7 +190,7 @@ subroutine fromEulers(self,eu,degrees)
|
||||||
self%q = eu2qu(Eulers)
|
self%q = eu2qu(Eulers)
|
||||||
|
|
||||||
end subroutine fromEulers
|
end subroutine fromEulers
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
subroutine fromAxisAngle(self,ax,degrees,P)
|
subroutine fromAxisAngle(self,ax,degrees,P)
|
||||||
|
|
||||||
class(tRotation), intent(out) :: self
|
class(tRotation), intent(out) :: self
|
||||||
|
@ -196,6 +201,7 @@ subroutine fromAxisAngle(self,ax,degrees,P)
|
||||||
real(pReal) :: angle
|
real(pReal) :: angle
|
||||||
real(pReal),dimension(3) :: axis
|
real(pReal),dimension(3) :: axis
|
||||||
|
|
||||||
|
|
||||||
if (.not. present(degrees)) then
|
if (.not. present(degrees)) then
|
||||||
angle = ax(4)
|
angle = ax(4)
|
||||||
else
|
else
|
||||||
|
@ -215,51 +221,54 @@ subroutine fromAxisAngle(self,ax,degrees,P)
|
||||||
self%q = ax2qu([axis,angle])
|
self%q = ax2qu([axis,angle])
|
||||||
|
|
||||||
end subroutine fromAxisAngle
|
end subroutine fromAxisAngle
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
subroutine fromMatrix(self,om)
|
subroutine fromMatrix(self,om)
|
||||||
|
|
||||||
class(tRotation), intent(out) :: self
|
class(tRotation), intent(out) :: self
|
||||||
real(pReal), dimension(3,3), intent(in) :: om
|
real(pReal), dimension(3,3), intent(in) :: om
|
||||||
|
|
||||||
|
|
||||||
if (dNeq(math_det33(om),1.0_pReal,tol=1.0e-5_pReal)) &
|
if (dNeq(math_det33(om),1.0_pReal,tol=1.0e-5_pReal)) &
|
||||||
call IO_error(402,ext_msg='fromMatrix')
|
call IO_error(402,ext_msg='fromMatrix')
|
||||||
|
|
||||||
self%q = om2qu(om)
|
self%q = om2qu(om)
|
||||||
|
|
||||||
end subroutine fromMatrix
|
end subroutine fromMatrix
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
|
|
||||||
|
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
!> @brief: Rotate a rotation
|
!> @brief: Compose rotations.
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
pure elemental function rotRot__(self,R) result(rRot)
|
pure elemental function rotRot__(self,R) result(rRot)
|
||||||
|
|
||||||
type(tRotation) :: rRot
|
type(tRotation) :: rRot
|
||||||
class(tRotation), intent(in) :: self,R
|
class(tRotation), intent(in) :: self,R
|
||||||
|
|
||||||
rRot = tRotation(multiply_quaternion(self%q,R%q))
|
|
||||||
|
rRot = tRotation(multiplyQuaternion(self%q,R%q))
|
||||||
call rRot%standardize()
|
call rRot%standardize()
|
||||||
|
|
||||||
end function rotRot__
|
end function rotRot__
|
||||||
|
|
||||||
|
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
!> @brief quaternion representation with positive q
|
!> @brief Convert to quaternion representation with positive q(1).
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
pure elemental subroutine standardize(self)
|
pure elemental subroutine standardize(self)
|
||||||
|
|
||||||
class(tRotation), intent(inout) :: self
|
class(tRotation), intent(inout) :: self
|
||||||
|
|
||||||
|
|
||||||
if (sign(1.0_pReal,self%q(1)) < 0.0_pReal) self%q = - self%q
|
if (sign(1.0_pReal,self%q(1)) < 0.0_pReal) self%q = - self%q
|
||||||
|
|
||||||
end subroutine standardize
|
end subroutine standardize
|
||||||
|
|
||||||
|
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
!> @author Marc De Graef, Carnegie Mellon University
|
!> @author Marc De Graef, Carnegie Mellon University
|
||||||
!> @brief Rotate a vector passively (default) or actively.
|
!> @brief Rotate a vector passively (default) or actively.
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
pure function rotVector(self,v,active) result(vRot)
|
pure function rotVector(self,v,active) result(vRot)
|
||||||
|
|
||||||
real(pReal), dimension(3) :: vRot
|
real(pReal), dimension(3) :: vRot
|
||||||
|
@ -270,6 +279,7 @@ pure function rotVector(self,v,active) result(vRot)
|
||||||
real(pReal), dimension(4) :: v_normed, q
|
real(pReal), dimension(4) :: v_normed, q
|
||||||
logical :: passive
|
logical :: passive
|
||||||
|
|
||||||
|
|
||||||
if (present(active)) then
|
if (present(active)) then
|
||||||
passive = .not. active
|
passive = .not. active
|
||||||
else
|
else
|
||||||
|
@ -280,22 +290,20 @@ pure function rotVector(self,v,active) result(vRot)
|
||||||
vRot = v
|
vRot = v
|
||||||
else
|
else
|
||||||
v_normed = [0.0_pReal,v]/norm2(v)
|
v_normed = [0.0_pReal,v]/norm2(v)
|
||||||
if (passive) then
|
q = merge(multiplyQuaternion(self%q, multiplyQuaternion(v_normed, conjugateQuaternion(self%q))), &
|
||||||
q = multiply_quaternion(self%q, multiply_quaternion(v_normed, conjugate_quaternion(self%q)))
|
multiplyQuaternion(conjugateQuaternion(self%q), multiplyQuaternion(v_normed, self%q)), &
|
||||||
else
|
passive)
|
||||||
q = multiply_quaternion(conjugate_quaternion(self%q), multiply_quaternion(v_normed, self%q))
|
|
||||||
endif
|
|
||||||
vRot = q(2:4)*norm2(v)
|
vRot = q(2:4)*norm2(v)
|
||||||
endif
|
endif
|
||||||
|
|
||||||
end function rotVector
|
end function rotVector
|
||||||
|
|
||||||
|
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
!> @author Marc De Graef, Carnegie Mellon University
|
!> @author Marc De Graef, Carnegie Mellon University
|
||||||
!> @brief Rotate a rank-2 tensor passively (default) or actively.
|
!> @brief Rotate a rank-2 tensor passively (default) or actively.
|
||||||
!> @details: Rotation is based on rotation matrix
|
!> @details: Rotation is based on rotation matrix
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
pure function rotTensor2(self,T,active) result(tRot)
|
pure function rotTensor2(self,T,active) result(tRot)
|
||||||
|
|
||||||
real(pReal), dimension(3,3) :: tRot
|
real(pReal), dimension(3,3) :: tRot
|
||||||
|
@ -319,11 +327,11 @@ pure function rotTensor2(self,T,active) result(tRot)
|
||||||
end function rotTensor2
|
end function rotTensor2
|
||||||
|
|
||||||
|
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
!> @brief Rotate a rank-4 tensor passively (default) or actively.
|
!> @brief Rotate a rank-4 tensor passively (default) or actively.
|
||||||
!> @details: rotation is based on rotation matrix
|
!> @details: rotation is based on rotation matrix
|
||||||
!! ToDo: Need to check active/passive !!!
|
!! ToDo: Need to check active/passive !!!
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
pure function rotTensor4(self,T,active) result(tRot)
|
pure function rotTensor4(self,T,active) result(tRot)
|
||||||
|
|
||||||
real(pReal), dimension(3,3,3,3) :: tRot
|
real(pReal), dimension(3,3,3,3) :: tRot
|
||||||
|
@ -351,11 +359,11 @@ pure function rotTensor4(self,T,active) result(tRot)
|
||||||
end function rotTensor4
|
end function rotTensor4
|
||||||
|
|
||||||
|
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
!> @brief Rotate a rank-4 stiffness tensor in Voigt 6x6 notation passively (default) or actively.
|
!> @brief Rotate a rank-4 stiffness tensor in Voigt 6x6 notation passively (default) or actively.
|
||||||
!> @details: https://scicomp.stackexchange.com/questions/35600
|
!> @details: https://scicomp.stackexchange.com/questions/35600
|
||||||
!! ToDo: Need to check active/passive !!!
|
!! ToDo: Need to check active/passive !!!
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
pure function rotStiffness(self,C,active) result(cRot)
|
pure function rotStiffness(self,C,active) result(cRot)
|
||||||
|
|
||||||
real(pReal), dimension(6,6) :: cRot
|
real(pReal), dimension(6,6) :: cRot
|
||||||
|
@ -391,24 +399,24 @@ pure function rotStiffness(self,C,active) result(cRot)
|
||||||
end function rotStiffness
|
end function rotStiffness
|
||||||
|
|
||||||
|
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
!> @brief Misorientation.
|
!> @brief Calculate misorientation.
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
pure elemental function misorientation(self,other)
|
pure elemental function misorientation(self,other)
|
||||||
|
|
||||||
type(tRotation) :: misorientation
|
type(tRotation) :: misorientation
|
||||||
class(tRotation), intent(in) :: self, other
|
class(tRotation), intent(in) :: self, other
|
||||||
|
|
||||||
|
|
||||||
misorientation%q = multiply_quaternion(other%q, conjugate_quaternion(self%q))
|
misorientation%q = multiplyQuaternion(other%q, conjugateQuaternion(self%q))
|
||||||
|
|
||||||
end function misorientation
|
end function misorientation
|
||||||
|
|
||||||
|
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
!> @author Marc De Graef, Carnegie Mellon University
|
!> @author Marc De Graef, Carnegie Mellon University
|
||||||
!> @brief Convert unit quaternion to rotation matrix.
|
!> @brief Convert unit quaternion to rotation matrix.
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
pure function qu2om(qu) result(om)
|
pure function qu2om(qu) result(om)
|
||||||
|
|
||||||
real(pReal), intent(in), dimension(4) :: qu
|
real(pReal), intent(in), dimension(4) :: qu
|
||||||
|
@ -436,10 +444,10 @@ pure function qu2om(qu) result(om)
|
||||||
end function qu2om
|
end function qu2om
|
||||||
|
|
||||||
|
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
!> @author Marc De Graef, Carnegie Mellon University
|
!> @author Marc De Graef, Carnegie Mellon University
|
||||||
!> @brief Convert unit quaternion to Euler angles.
|
!> @brief Convert unit quaternion to Bunge Euler angles.
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
pure function qu2eu(qu) result(eu)
|
pure function qu2eu(qu) result(eu)
|
||||||
|
|
||||||
real(pReal), intent(in), dimension(4) :: qu
|
real(pReal), intent(in), dimension(4) :: qu
|
||||||
|
@ -466,10 +474,10 @@ pure function qu2eu(qu) result(eu)
|
||||||
end function qu2eu
|
end function qu2eu
|
||||||
|
|
||||||
|
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
!> @author Marc De Graef, Carnegie Mellon University
|
!> @author Marc De Graef, Carnegie Mellon University
|
||||||
!> @brief convert unit quaternion to axis angle pair
|
!> @brief Convert unit quaternion to axis-angle pair.
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
pure function qu2ax(qu) result(ax)
|
pure function qu2ax(qu) result(ax)
|
||||||
|
|
||||||
real(pReal), intent(in), dimension(4) :: qu
|
real(pReal), intent(in), dimension(4) :: qu
|
||||||
|
@ -477,6 +485,7 @@ pure function qu2ax(qu) result(ax)
|
||||||
|
|
||||||
real(pReal) :: omega, s
|
real(pReal) :: omega, s
|
||||||
|
|
||||||
|
|
||||||
if (dEq0(sum(qu(2:4)**2))) then
|
if (dEq0(sum(qu(2:4)**2))) then
|
||||||
ax = [ 0.0_pReal, 0.0_pReal, 1.0_pReal, 0.0_pReal ] ! axis = [001]
|
ax = [ 0.0_pReal, 0.0_pReal, 1.0_pReal, 0.0_pReal ] ! axis = [001]
|
||||||
elseif (dNeq0(qu(1))) then
|
elseif (dNeq0(qu(1))) then
|
||||||
|
@ -490,11 +499,11 @@ pure function qu2ax(qu) result(ax)
|
||||||
end function qu2ax
|
end function qu2ax
|
||||||
|
|
||||||
|
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
!> @author Martin Diehl, Max-Planck-Institut für Eisenforschung GmbH
|
!> @author Martin Diehl, Max-Planck-Institut für Eisenforschung GmbH
|
||||||
!> @brief convert rotation matrix to unit quaternion
|
!> @brief Convert rotation matrix to unit quaternion.
|
||||||
!> @details the original formulation (direct conversion) had (numerical?) issues
|
!> @details the original formulation (direct conversion) had (numerical?) issues
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
pure function om2qu(om) result(qu)
|
pure function om2qu(om) result(qu)
|
||||||
|
|
||||||
real(pReal), intent(in), dimension(3,3) :: om
|
real(pReal), intent(in), dimension(3,3) :: om
|
||||||
|
@ -503,6 +512,7 @@ pure function om2qu(om) result(qu)
|
||||||
real(pReal) :: trace,s
|
real(pReal) :: trace,s
|
||||||
trace = math_trace33(om)
|
trace = math_trace33(om)
|
||||||
|
|
||||||
|
|
||||||
if(trace > 0.0_pReal) then
|
if(trace > 0.0_pReal) then
|
||||||
s = 0.5_pReal / sqrt(trace+1.0_pReal)
|
s = 0.5_pReal / sqrt(trace+1.0_pReal)
|
||||||
qu = [0.25_pReal/s, (om(3,2)-om(2,3))*s,(om(1,3)-om(3,1))*s,(om(2,1)-om(1,2))*s]
|
qu = [0.25_pReal/s, (om(3,2)-om(2,3))*s,(om(1,3)-om(3,1))*s,(om(2,1)-om(1,2))*s]
|
||||||
|
@ -525,17 +535,18 @@ pure function om2qu(om) result(qu)
|
||||||
end function om2qu
|
end function om2qu
|
||||||
|
|
||||||
|
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
!> @author Marc De Graef, Carnegie Mellon University
|
!> @author Marc De Graef, Carnegie Mellon University
|
||||||
!> @brief convert orientation matrix to Euler angles
|
!> @brief Convert orientation matrix to Bunge Euler angles.
|
||||||
!> @details Two step check for special cases to avoid invalid operations (not needed for python)
|
!> @details Two step check for special cases to avoid invalid operations (not needed for python)
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
pure function om2eu(om) result(eu)
|
pure function om2eu(om) result(eu)
|
||||||
|
|
||||||
real(pReal), intent(in), dimension(3,3) :: om
|
real(pReal), intent(in), dimension(3,3) :: om
|
||||||
real(pReal), dimension(3) :: eu
|
real(pReal), dimension(3) :: eu
|
||||||
real(pReal) :: zeta
|
real(pReal) :: zeta
|
||||||
|
|
||||||
|
|
||||||
if (dNeq(abs(om(3,3)),1.0_pReal,1.e-8_pReal)) then
|
if (dNeq(abs(om(3,3)),1.0_pReal,1.e-8_pReal)) then
|
||||||
zeta = 1.0_pReal/sqrt(math_clip(1.0_pReal-om(3,3)**2,1e-64_pReal,1.0_pReal))
|
zeta = 1.0_pReal/sqrt(math_clip(1.0_pReal-om(3,3)**2,1e-64_pReal,1.0_pReal))
|
||||||
eu = [atan2(om(3,1)*zeta,-om(3,2)*zeta), &
|
eu = [atan2(om(3,1)*zeta,-om(3,2)*zeta), &
|
||||||
|
@ -550,10 +561,10 @@ pure function om2eu(om) result(eu)
|
||||||
end function om2eu
|
end function om2eu
|
||||||
|
|
||||||
|
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
!> @author Marc De Graef, Carnegie Mellon University
|
!> @author Marc De Graef, Carnegie Mellon University
|
||||||
!> @brief convert orientation matrix to axis angle pair
|
!> @brief Convert orientation matrix to axis-angle pair.
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
function om2ax(om) result(ax)
|
function om2ax(om) result(ax)
|
||||||
|
|
||||||
real(pReal), intent(in), dimension(3,3) :: om
|
real(pReal), intent(in), dimension(3,3) :: om
|
||||||
|
@ -565,6 +576,7 @@ function om2ax(om) result(ax)
|
||||||
real(pReal), dimension(3,3) :: VR, devNull, om_
|
real(pReal), dimension(3,3) :: VR, devNull, om_
|
||||||
integer :: ierr, i
|
integer :: ierr, i
|
||||||
|
|
||||||
|
|
||||||
om_ = om
|
om_ = om
|
||||||
|
|
||||||
! first get the rotation angle
|
! first get the rotation angle
|
||||||
|
@ -586,10 +598,10 @@ function om2ax(om) result(ax)
|
||||||
end function om2ax
|
end function om2ax
|
||||||
|
|
||||||
|
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
!> @author Marc De Graef, Carnegie Mellon University
|
!> @author Marc De Graef, Carnegie Mellon University
|
||||||
!> @brief Euler angles to unit quaternion
|
!> @brief Convert Bunge Euler angles to unit quaternion.
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
pure function eu2qu(eu) result(qu)
|
pure function eu2qu(eu) result(qu)
|
||||||
|
|
||||||
real(pReal), intent(in), dimension(3) :: eu
|
real(pReal), intent(in), dimension(3) :: eu
|
||||||
|
@ -597,6 +609,7 @@ pure function eu2qu(eu) result(qu)
|
||||||
real(pReal), dimension(3) :: ee
|
real(pReal), dimension(3) :: ee
|
||||||
real(pReal) :: cPhi, sPhi
|
real(pReal) :: cPhi, sPhi
|
||||||
|
|
||||||
|
|
||||||
ee = 0.5_pReal*eu
|
ee = 0.5_pReal*eu
|
||||||
|
|
||||||
cPhi = cos(ee(2))
|
cPhi = cos(ee(2))
|
||||||
|
@ -611,10 +624,10 @@ pure function eu2qu(eu) result(qu)
|
||||||
end function eu2qu
|
end function eu2qu
|
||||||
|
|
||||||
|
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
!> @author Marc De Graef, Carnegie Mellon University
|
!> @author Marc De Graef, Carnegie Mellon University
|
||||||
!> @brief Euler angles to orientation matrix
|
!> @brief Convert Euler angles to orientation matrix.
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
pure function eu2om(eu) result(om)
|
pure function eu2om(eu) result(om)
|
||||||
|
|
||||||
real(pReal), intent(in), dimension(3) :: eu
|
real(pReal), intent(in), dimension(3) :: eu
|
||||||
|
@ -622,6 +635,7 @@ pure function eu2om(eu) result(om)
|
||||||
|
|
||||||
real(pReal), dimension(3) :: c, s
|
real(pReal), dimension(3) :: c, s
|
||||||
|
|
||||||
|
|
||||||
c = cos(eu)
|
c = cos(eu)
|
||||||
s = sin(eu)
|
s = sin(eu)
|
||||||
|
|
||||||
|
@ -640,10 +654,10 @@ pure function eu2om(eu) result(om)
|
||||||
end function eu2om
|
end function eu2om
|
||||||
|
|
||||||
|
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
!> @author Marc De Graef, Carnegie Mellon University
|
!> @author Marc De Graef, Carnegie Mellon University
|
||||||
!> @brief convert euler to axis angle
|
!> @brief Convert Bunge Euler angles to axis-angle pair.
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
pure function eu2ax(eu) result(ax)
|
pure function eu2ax(eu) result(ax)
|
||||||
|
|
||||||
real(pReal), intent(in), dimension(3) :: eu
|
real(pReal), intent(in), dimension(3) :: eu
|
||||||
|
@ -651,6 +665,7 @@ pure function eu2ax(eu) result(ax)
|
||||||
|
|
||||||
real(pReal) :: t, delta, tau, alpha, sigma
|
real(pReal) :: t, delta, tau, alpha, sigma
|
||||||
|
|
||||||
|
|
||||||
t = tan(eu(2)*0.5_pReal)
|
t = tan(eu(2)*0.5_pReal)
|
||||||
sigma = 0.5_pReal*(eu(1)+eu(3))
|
sigma = 0.5_pReal*(eu(1)+eu(3))
|
||||||
delta = 0.5_pReal*(eu(1)-eu(3))
|
delta = 0.5_pReal*(eu(1)-eu(3))
|
||||||
|
@ -669,10 +684,10 @@ pure function eu2ax(eu) result(ax)
|
||||||
end function eu2ax
|
end function eu2ax
|
||||||
|
|
||||||
|
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
!> @author Marc De Graef, Carnegie Mellon University
|
!> @author Marc De Graef, Carnegie Mellon University
|
||||||
!> @brief convert axis angle pair to quaternion
|
!> @brief Convert axis-angle pair to unit quaternion.
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
pure function ax2qu(ax) result(qu)
|
pure function ax2qu(ax) result(qu)
|
||||||
|
|
||||||
real(pReal), intent(in), dimension(4) :: ax
|
real(pReal), intent(in), dimension(4) :: ax
|
||||||
|
@ -692,10 +707,10 @@ pure function ax2qu(ax) result(qu)
|
||||||
end function ax2qu
|
end function ax2qu
|
||||||
|
|
||||||
|
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
!> @author Marc De Graef, Carnegie Mellon University
|
!> @author Marc De Graef, Carnegie Mellon University
|
||||||
!> @brief convert axis angle pair to orientation matrix
|
!> @brief Convert axis-angle pair to orientation matrix.
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
pure function ax2om(ax) result(om)
|
pure function ax2om(ax) result(om)
|
||||||
|
|
||||||
real(pReal), intent(in), dimension(4) :: ax
|
real(pReal), intent(in), dimension(4) :: ax
|
||||||
|
@ -703,6 +718,7 @@ pure function ax2om(ax) result(om)
|
||||||
|
|
||||||
real(pReal) :: q, c, s, omc
|
real(pReal) :: q, c, s, omc
|
||||||
|
|
||||||
|
|
||||||
c = cos(ax(4))
|
c = cos(ax(4))
|
||||||
s = sin(ax(4))
|
s = sin(ax(4))
|
||||||
omc = 1.0_pReal-c
|
omc = 1.0_pReal-c
|
||||||
|
@ -728,15 +744,16 @@ pure function ax2om(ax) result(om)
|
||||||
end function ax2om
|
end function ax2om
|
||||||
|
|
||||||
|
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
!> @author Marc De Graef, Carnegie Mellon University
|
!> @author Marc De Graef, Carnegie Mellon University
|
||||||
!> @brief convert axis angle pair to Euler angles
|
!> @brief Convert axis-angle pair to Bunge Euler angles.
|
||||||
!---------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
pure function ax2eu(ax) result(eu)
|
pure function ax2eu(ax) result(eu)
|
||||||
|
|
||||||
real(pReal), intent(in), dimension(4) :: ax
|
real(pReal), intent(in), dimension(4) :: ax
|
||||||
real(pReal), dimension(3) :: eu
|
real(pReal), dimension(3) :: eu
|
||||||
|
|
||||||
|
|
||||||
eu = om2eu(ax2om(ax))
|
eu = om2eu(ax2om(ax))
|
||||||
|
|
||||||
end function ax2eu
|
end function ax2eu
|
||||||
|
@ -745,33 +762,32 @@ end function ax2eu
|
||||||
!--------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
!> @brief Multiply two quaternions.
|
!> @brief Multiply two quaternions.
|
||||||
!--------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
pure function multiply_quaternion(qu1,qu2)
|
pure function multiplyQuaternion(qu1,qu2)
|
||||||
|
|
||||||
real(pReal), dimension(4), intent(in) :: qu1, qu2
|
real(pReal), dimension(4), intent(in) :: qu1, qu2
|
||||||
real(pReal), dimension(4) :: multiply_quaternion
|
real(pReal), dimension(4) :: multiplyQuaternion
|
||||||
|
|
||||||
|
|
||||||
multiply_quaternion(1) = qu1(1)*qu2(1) - qu1(2)*qu2(2) - qu1(3)*qu2(3) - qu1(4)*qu2(4)
|
multiplyQuaternion(1) = qu1(1)*qu2(1) - qu1(2)*qu2(2) - qu1(3)*qu2(3) - qu1(4)*qu2(4)
|
||||||
multiply_quaternion(2) = qu1(1)*qu2(2) + qu1(2)*qu2(1) + P * (qu1(3)*qu2(4) - qu1(4)*qu2(3))
|
multiplyQuaternion(2) = qu1(1)*qu2(2) + qu1(2)*qu2(1) + P * (qu1(3)*qu2(4) - qu1(4)*qu2(3))
|
||||||
multiply_quaternion(3) = qu1(1)*qu2(3) + qu1(3)*qu2(1) + P * (qu1(4)*qu2(2) - qu1(2)*qu2(4))
|
multiplyQuaternion(3) = qu1(1)*qu2(3) + qu1(3)*qu2(1) + P * (qu1(4)*qu2(2) - qu1(2)*qu2(4))
|
||||||
multiply_quaternion(4) = qu1(1)*qu2(4) + qu1(4)*qu2(1) + P * (qu1(2)*qu2(3) - qu1(3)*qu2(2))
|
multiplyQuaternion(4) = qu1(1)*qu2(4) + qu1(4)*qu2(1) + P * (qu1(2)*qu2(3) - qu1(3)*qu2(2))
|
||||||
|
|
||||||
end function multiply_quaternion
|
end function multiplyQuaternion
|
||||||
|
|
||||||
|
|
||||||
!--------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
!> @brief Calculate conjugate complex of a quaternion.
|
!> @brief Calculate conjugate complex of a quaternion.
|
||||||
!--------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
pure function conjugate_quaternion(qu)
|
pure function conjugateQuaternion(qu)
|
||||||
|
|
||||||
real(pReal), dimension(4), intent(in) :: qu
|
real(pReal), dimension(4), intent(in) :: qu
|
||||||
real(pReal), dimension(4) :: conjugate_quaternion
|
real(pReal), dimension(4) :: conjugateQuaternion
|
||||||
|
|
||||||
|
|
||||||
conjugate_quaternion = [qu(1), -qu(2), -qu(3), -qu(4)]
|
conjugateQuaternion = [qu(1), -qu(2), -qu(3), -qu(4)]
|
||||||
|
|
||||||
|
end function conjugateQuaternion
|
||||||
end function conjugate_quaternion
|
|
||||||
|
|
||||||
|
|
||||||
!--------------------------------------------------------------------------------------------------
|
!--------------------------------------------------------------------------------------------------
|
||||||
|
@ -780,8 +796,8 @@ end function conjugate_quaternion
|
||||||
subroutine selfTest()
|
subroutine selfTest()
|
||||||
|
|
||||||
type(tRotation) :: R
|
type(tRotation) :: R
|
||||||
real(pReal), dimension(4) :: qu, ax, ro
|
real(pReal), dimension(4) :: qu, ax
|
||||||
real(pReal), dimension(3) :: x, eu, ho, v3
|
real(pReal), dimension(3) :: x, eu, v3
|
||||||
real(pReal), dimension(3,3) :: om, t33
|
real(pReal), dimension(3,3) :: om, t33
|
||||||
real(pReal), dimension(3,3,3,3) :: t3333
|
real(pReal), dimension(3,3,3,3) :: t3333
|
||||||
real(pReal), dimension(6,6) :: C
|
real(pReal), dimension(6,6) :: C
|
||||||
|
|
Loading…
Reference in New Issue