From 40d38ebf55f38c65f81548645b5b213459e055b1 Mon Sep 17 00:00:00 2001 From: Martin Diehl Date: Sat, 8 Dec 2018 08:02:55 +0100 Subject: [PATCH] added rotation conversions modified versions from 3Drotations code (available on GitHub) by Marc De Graef --- src/CMakeLists.txt | 22 +- src/Lambert.f90 | 213 +++++++ src/commercialFEM_fileList.f90 | 3 + src/prec.f90 | 4 +- src/quaternions.f90 | 433 +++++++++++++ src/rotations.f90 | 1088 ++++++++++++++++++++++++++++++++ 6 files changed, 1757 insertions(+), 6 deletions(-) create mode 100644 src/Lambert.f90 create mode 100644 src/quaternions.f90 create mode 100644 src/rotations.f90 diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 2e4462243..4210a79b3 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -49,18 +49,30 @@ add_library(FEsolving OBJECT "FEsolving.f90") add_dependencies(FEsolving DEBUG) list(APPEND OBJECTFILES $) -add_library(DAMASK_MATH OBJECT "math.f90") -add_dependencies(DAMASK_MATH FEsolving) -list(APPEND OBJECTFILES $) +add_library(MATH OBJECT "math.f90") +add_dependencies(MATH DEBUG) +list(APPEND OBJECTFILES $) + +add_library(QUATERNIONS OBJECT "quaternions.f90") +add_dependencies(QUATERNIONS MATH) +list(APPEND OBJECTFILES $) + +add_library(LAMBERT OBJECT "Lambert.f90") +add_dependencies(LAMBERT MATH) +list(APPEND OBJECTFILES $) + +add_library(ROTATIONS OBJECT "rotations.f90") +add_dependencies(ROTATIONS LAMBERT QUATERNIONS) +list(APPEND OBJECTFILES $) # SPECTRAL solver and FEM solver use different mesh files if (PROJECT_NAME STREQUAL "DAMASK_spectral") add_library(MESH OBJECT "mesh.f90") - add_dependencies(MESH DAMASK_MATH) + add_dependencies(MESH ROTATIONS FEsolving) list(APPEND OBJECTFILES $) elseif (PROJECT_NAME STREQUAL "DAMASK_FEM") add_library(FEZoo OBJECT "FEM_zoo.f90") - add_dependencies(FEZoo DAMASK_MATH) + add_dependencies(FEZoo ROTATIONS FEsolving) list(APPEND OBJECTFILES $) add_library(MESH OBJECT "meshFEM.f90") add_dependencies(MESH FEZoo) diff --git a/src/Lambert.f90 b/src/Lambert.f90 new file mode 100644 index 000000000..ab939bcc6 --- /dev/null +++ b/src/Lambert.f90 @@ -0,0 +1,213 @@ +! ################################################################### +! Copyright (c) 2013-2015, Marc De Graef/Carnegie Mellon University +! All rights reserved. +! +! Redistribution and use in source and binary forms, with or without modification, are +! permitted provided that the following conditions are met: +! +! - Redistributions of source code must retain the above copyright notice, this list +! of conditions and the following disclaimer. +! - Redistributions in binary form must reproduce the above copyright notice, this +! list of conditions and the following disclaimer in the documentation and/or +! other materials provided with the distribution. +! - Neither the names of Marc De Graef, Carnegie Mellon University nor the names +! of its contributors may be used to endorse or promote products derived from +! this software without specific prior written permission. +! +! THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +! AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +! IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +! ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +! LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +! DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +! SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +! CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +! OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE +! USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +! ################################################################### + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +! +!> @brief everything that has to do with the modified Lambert projections +! +!> @details This module contains a number of projection functions for the modified +!> Lambert projection between square lattice and 2D hemisphere, hexagonal lattice +!> and 2D hemisphere, as well as the more complex mapping between a 3D cubic grid +!> and the unit quaternion hemisphere with positive scalar component. In addition, there +!> are some other projections, such as the stereographic one. Each function is named +!> by the projection, the dimensionality of the starting grid, and the forward or inverse +!> character. For each function, there is also a single precision and a double precision +!> version, but we use the interface formalism to have only a single call. The Forward +!> mapping is taken to be the one from the simple grid to the curved grid. Since the module +!> deals with various grids, we also add a few functions/subroutines that apply symmetry +!> operations on those grids. +!> References: +!> D. Rosca, A. Morawiec, and M. De Graef. “A new method of constructing a grid +!> in the space of 3D rotations and its applications to texture analysis”. +!> Modeling and Simulations in Materials Science and Engineering 22, 075013 (2014). +!-------------------------------------------------------------------------- +module Lambert + use math + use prec + + implicit none + + real(pReal), private :: & + sPi = sqrt(PI), & + pref = sqrt(6.0_pReal/PI), & + ! the following constants are used for the cube to quaternion hemisphere mapping + ap = PI**(2.0_pReal/3.0_pReal), & + sc = 0.897772786961286_pReal, & ! a/ap + beta = 0.962874509979126_pReal, & ! pi^(5/6)/6^(1/6)/2 + R1 = 1.330670039491469_pReal, & ! (3pi/4)^(1/3) + r2 = sqrt(2.0_pReal), & + pi12 = PI/12.0_pReal, & + prek = 1.643456402972504_pReal, & ! R1 2^(1/4)/beta + r24 = sqrt(24.0_pReal) + + private + public :: & + LambertCubeToBall, & + LambertBallToCube + private :: & + GetPyramidOrder + +contains + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief map from 3D cubic grid to 3D ball +!-------------------------------------------------------------------------- +function LambertCubeToBall(cube) result(ball) + use, intrinsic :: IEEE_ARITHMETIC + + implicit none + real(pReal), intent(in), dimension(3) :: cube + real(pReal), dimension(3) :: ball, LamXYZ, XYZ + real(pReal) :: T(2), c, s, q + real(pReal), parameter :: eps = 1.0e-8_pReal + integer(pInt), dimension(3) :: p + integer(pInt), dimension(2) :: order + + if (maxval(abs(cube)) > ap/2.0+eps) then + ball = IEEE_value(cube,IEEE_positive_inf) + return + end if + + ! transform to the sphere grid via the curved square, and intercept the zero point + center: if (all(dEq0(cube))) then + ball = 0.0_pReal + else center + ! get pyramide and scale by grid parameter ratio + p = GetPyramidOrder(cube) + XYZ = cube(p) * sc + + ! intercept all the points along the z-axis + special: if (all(dEq0(XYZ(1:2)))) then + LamXYZ = [ 0.0_pReal, 0.0_pReal, pref * XYZ(3) ] + else special + order = merge( [2,1], [1,2], abs(XYZ(2)) <= abs(XYZ(1))) ! order of absolute values of XYZ + q = pi12 * XYZ(order(1))/XYZ(order(2)) ! smaller by larger + c = cos(q) + s = sin(q) + q = prek * XYZ(order(2))/ sqrt(r2-c) + T = [ (r2*c - 1.0), r2 * s] * q + + ! transform to sphere grid (inverse Lambert) + ! [note that there is no need to worry about dividing by zero, since XYZ(3) can not become zero] + c = sum(T**2) + s = Pi * c/(24.0*XYZ(3)**2) + c = sPi * c / r24 / XYZ(3) + q = sqrt( 1.0 - s ) + LamXYZ = [ T(order(2)) * q, T(order(1)) * q, pref * XYZ(3) - c ] + endif special + + ! reverse the coordinates back to the regular order according to the original pyramid number + ball = LamXYZ(p) + + endif center + +end function LambertCubeToBall + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief map from 3D ball to 3D cubic grid +!-------------------------------------------------------------------------- +pure function LambertBallToCube(xyz) result(cube) + use, intrinsic :: IEEE_ARITHMETIC + + implicit none + real(pReal), intent(in), dimension(3) :: xyz + real(pReal), dimension(3) :: cube, xyz1, xyz3 + real(pReal), dimension(2) :: Tinv, xyz2 + real(pReal) :: rs, qxy, q2, sq2, q, tt + integer(pInt) , dimension(3) :: p + + rs = norm2(xyz) + if (rs > R1) then + cube = IEEE_value(cube,IEEE_positive_inf) + return + endif + + center: if (all(dEq0(xyz))) then + cube = 0.0_pReal + else center + p = GetPyramidOrder(xyz) + xyz3 = xyz(p) + + ! inverse M_3 + xyz2 = xyz3(1:2) * sqrt( 2.0*rs/(rs+abs(xyz3(3))) ) + + ! inverse M_2 + qxy = sum(xyz2**2) + + special: if (dEq0(qxy)) then + Tinv = 0.0 + else special + q2 = qxy + maxval(abs(xyz2))**2 + sq2 = sqrt(q2) + q = (beta/r2/R1) * sqrt(q2*qxy/(q2-maxval(abs(xyz2))*sq2)) + tt = (minval(abs(xyz2))**2+maxval(abs(xyz2))*sq2)/r2/qxy + Tinv = q * sign(1.0,xyz2) * merge([ 1.0_pReal, acos(math_clip(tt,-1.0_pReal,1.0_pReal))/pi12], & + [ acos(math_clip(tt,-1.0_pReal,1.0_pReal))/pi12, 1.0_pReal], & + abs(xyz2(2)) <= abs(xyz2(1))) + endif special + + ! inverse M_1 + xyz1 = [ Tinv(1), Tinv(2), sign(1.0,xyz3(3)) * rs / pref ] /sc + + ! reverst the coordinates back to the regular order according to the original pyramid number + cube = xyz1(p) + + endif center + +end function LambertBallToCube + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief determine to which pyramid a point in a cubic grid belongs +!-------------------------------------------------------------------------- +pure function GetPyramidOrder(xyz) + + implicit none + real(pReal),intent(in),dimension(3) :: xyz + integer(pInt), dimension(3) :: GetPyramidOrder + + if (((abs(xyz(1)) <= xyz(3)).and.(abs(xyz(2)) <= xyz(3))) .or. & + ((abs(xyz(1)) <= -xyz(3)).and.(abs(xyz(2)) <= -xyz(3)))) then + GetPyramidOrder = [1,2,3] + else if (((abs(xyz(3)) <= xyz(1)).and.(abs(xyz(2)) <= xyz(1))) .or. & + ((abs(xyz(3)) <= -xyz(1)).and.(abs(xyz(2)) <= -xyz(1)))) then + GetPyramidOrder = [2,3,1] + else if (((abs(xyz(1)) <= xyz(2)).and.(abs(xyz(3)) <= xyz(2))) .or. & + ((abs(xyz(1)) <= -xyz(2)).and.(abs(xyz(3)) <= -xyz(2)))) then + GetPyramidOrder = [3,1,2] + else + GetPyramidOrder = -1 ! should be impossible, but might simplify debugging + end if + +end function GetPyramidOrder + +end module Lambert diff --git a/src/commercialFEM_fileList.f90 b/src/commercialFEM_fileList.f90 index 36f0244ef..75f540524 100644 --- a/src/commercialFEM_fileList.f90 +++ b/src/commercialFEM_fileList.f90 @@ -11,6 +11,9 @@ #include "debug.f90" #include "config.f90" #include "math.f90" +#include "quaternions.f90" +#include "Lambert.f90" +#include "rotations.f90" #include "FEsolving.f90" #include "mesh.f90" #include "material.f90" diff --git a/src/prec.f90 b/src/prec.f90 index 0f942b3c1..bc7f523d0 100644 --- a/src/prec.f90 +++ b/src/prec.f90 @@ -30,7 +30,9 @@ module prec integer(pInt), allocatable, dimension(:) :: realloc_lhs_test - type, public :: group_float !< variable length datatype used for storage of state + real(pReal), parameter, public :: epsijk = -1.0_pReal !< parameter for orientation conversion. ToDo: Better place? + + type, public :: group_float !< variable length datatype used for storage of state real(pReal), dimension(:), pointer :: p end type group_float diff --git a/src/quaternions.f90 b/src/quaternions.f90 new file mode 100644 index 000000000..60b8d387d --- /dev/null +++ b/src/quaternions.f90 @@ -0,0 +1,433 @@ +! ################################################################### +! Copyright (c) 2013-2015, Marc De Graef/Carnegie Mellon University +! All rights reserved. +! +! Redistribution and use in source and binary forms, with or without modification, are +! permitted provided that the following conditions are met: +! +! - Redistributions of source code must retain the above copyright notice, this list +! of conditions and the following disclaimer. +! - Redistributions in binary form must reproduce the above copyright notice, this +! list of conditions and the following disclaimer in the documentation and/or +! other materials provided with the distribution. +! - Neither the names of Marc De Graef, Carnegie Mellon University nor the names +! of its contributors may be used to endorse or promote products derived from +! this software without specific prior written permission. +! +! THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +! AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +! IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +! ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +! LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +! DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +! SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +! CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +! OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE +! USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +! ################################################################### + +module quaternions + + use prec + implicit none + + public + type, public :: quaternion + real(pReal) :: w = 0.0_pReal + real(pReal) :: x = 0.0_pReal + real(pReal) :: y = 0.0_pReal + real(pReal) :: z = 0.0_pReal + + + contains + procedure, private :: add__ + procedure, private :: pos__ + generic, public :: operator(+) => add__,pos__ + + procedure, private :: sub__ + procedure, private :: neg__ + generic, public :: operator(-) => sub__,neg__ + + procedure, private :: mul_quat__ + procedure, private :: mul_scal__ + generic, public :: operator(*) => mul_quat__, mul_scal__ + + procedure, private :: div_quat__ + procedure, private :: div_scal__ + generic, public :: operator(/) => div_quat__, div_scal__ + + procedure, private :: eq__ + generic, public :: operator(==) => eq__ + + procedure, private :: neq__ + generic, public :: operator(/=) => neq__ + + procedure, private :: pow_quat__ + procedure, private :: pow_scal__ + generic, public :: operator(**) => pow_quat__, pow_scal__ + + procedure, private :: abs__ + procedure, private :: dot_product__ + procedure, private :: conjg__ + procedure, private :: exp__ + procedure, private :: log__ + + procedure, public :: homomorphed => quat_homomorphed + + !procedure,private :: quat_write + !generic :: write(formatted) => quat_write + + end type + +interface assignment (=) + module procedure assign_quat__ + module procedure assign_vec__ +end interface assignment (=) + +interface quaternion + module procedure init__ +end interface quaternion + +interface abs + procedure abs__ +end interface abs + +interface dot_product + procedure dot_product__ +end interface dot_product + +interface conjg + module procedure conjg__ +end interface conjg + +interface exp + module procedure exp__ +end interface exp + +interface log + module procedure log__ +end interface log + +contains + + +!-------------------------------------------------------------------------- +!> constructor for a quaternion from a 4-vector +!-------------------------------------------------------------------------- +type(quaternion) pure function init__(array) + + implicit none + real(pReal), intent(in), dimension(4) :: array + + init__%w=array(1) + init__%x=array(2) + init__%y=array(3) + init__%z=array(4) + +end function init__ + + +!-------------------------------------------------------------------------- +!> assing a quaternion +!-------------------------------------------------------------------------- +elemental subroutine assign_quat__(self,other) + + implicit none + type(quaternion), intent(out) :: self + type(quaternion), intent(in) :: other + + self%w = other%w + self%x = other%x + self%y = other%y + self%z = other%z + +end subroutine assign_quat__ + + +!-------------------------------------------------------------------------- +!> assing a 4-vector +!-------------------------------------------------------------------------- +pure subroutine assign_vec__(self,other) + + implicit none + type(quaternion), intent(out) :: self + real(pReal), intent(in), dimension(4) :: other + + self%w = other(1) + self%x = other(2) + self%y = other(3) + self%z = other(4) + +end subroutine assign_vec__ + + +!-------------------------------------------------------------------------- +!> addition of two quaternions +!-------------------------------------------------------------------------- +type(quaternion) elemental function add__(self,other) + + implicit none + class(quaternion), intent(in) :: self,other + + add__%w = self%w + other%w + add__%x = self%x + other%x + add__%y = self%y + other%y + add__%z = self%z + other%z + +end function add__ + + +!-------------------------------------------------------------------------- +!> unary positive operator +!-------------------------------------------------------------------------- +type(quaternion) elemental function pos__(self) + + implicit none + class(quaternion), intent(in) :: self + + pos__%w = self%w + pos__%x = self%x + pos__%y = self%y + pos__%z = self%z + +end function pos__ + + +!-------------------------------------------------------------------------- +!> subtraction of two quaternions +!-------------------------------------------------------------------------- +type(quaternion) elemental function sub__(self,other) + + implicit none + class(quaternion), intent(in) :: self,other + + sub__%w = self%w - other%w + sub__%x = self%x - other%x + sub__%y = self%y - other%y + sub__%z = self%z - other%z + +end function sub__ + + +!-------------------------------------------------------------------------- +!> unary positive operator +!-------------------------------------------------------------------------- +type(quaternion) elemental function neg__(self) + + implicit none + class(quaternion), intent(in) :: self + + neg__%w = -self%w + neg__%x = -self%x + neg__%y = -self%y + neg__%z = -self%z + +end function neg__ + + +!-------------------------------------------------------------------------- +!> multiplication of two quaternions +!-------------------------------------------------------------------------- +type(quaternion) elemental function mul_quat__(self,other) + + implicit none + class(quaternion), intent(in) :: self, other + + mul_quat__%w = self%w*other%w - self%x*other%x - self%y*other%y - self%z*other%z + mul_quat__%x = self%w*other%x + self%x*other%w + epsijk * (self%y*other%z - self%z*other%y) + mul_quat__%y = self%w*other%y + self%y*other%w + epsijk * (self%z*other%x - self%x*other%z) + mul_quat__%z = self%w*other%z + self%z*other%w + epsijk * (self%x*other%y - self%y*other%x) + +end function mul_quat__ + + +!-------------------------------------------------------------------------- +!> multiplication of quaternions with scalar +!-------------------------------------------------------------------------- +type(quaternion) elemental function mul_scal__(self,scal) + + implicit none + class(quaternion), intent(in) :: self + real(pReal), intent(in) :: scal + + mul_scal__%w = self%w*scal + mul_scal__%x = self%x*scal + mul_scal__%y = self%y*scal + mul_scal__%z = self%z*scal + +end function mul_scal__ + + +!-------------------------------------------------------------------------- +!> division of two quaternions +!-------------------------------------------------------------------------- +type(quaternion) elemental function div_quat__(self,other) + + implicit none + class(quaternion), intent(in) :: self, other + + div_quat__ = self * (conjg(other)/(abs(other)**2.0_pReal)) + +end function div_quat__ + + +!-------------------------------------------------------------------------- +!> divisiont of quaternions by scalar +!-------------------------------------------------------------------------- +type(quaternion) elemental function div_scal__(self,scal) + + implicit none + class(quaternion), intent(in) :: self + real(pReal), intent(in) :: scal + + div_scal__ = [self%w,self%x,self%y,self%z]/scal + +end function div_scal__ + + +!-------------------------------------------------------------------------- +!> equality of two quaternions +!-------------------------------------------------------------------------- +logical elemental function eq__(self,other) + implicit none + class(quaternion), intent(in) :: self,other + + eq__ = all(dEq([ self%w, self%x, self%y, self%z], & + [other%w,other%x,other%y,other%z])) + +end function eq__ + + +!-------------------------------------------------------------------------- +!> inequality of two quaternions +!-------------------------------------------------------------------------- +logical elemental function neq__(self,other) + + implicit none + class(quaternion), intent(in) :: self,other + + neq__ = .not. self%eq__(other) + +end function neq__ + + +!-------------------------------------------------------------------------- +!> quaternion to the power of a scalar +!-------------------------------------------------------------------------- +type(quaternion) elemental function pow_scal__(self,expon) + + implicit none + class(quaternion), intent(in) :: self + real(pReal), intent(in) :: expon + + pow_scal__ = exp(log(self)*expon) + +end function pow_scal__ + + +!-------------------------------------------------------------------------- +!> quaternion to the power of a quaternion +!-------------------------------------------------------------------------- +type(quaternion) elemental function pow_quat__(self,expon) + + implicit none + class(quaternion), intent(in) :: self + type(quaternion), intent(in) :: expon + + pow_quat__ = exp(log(self)*expon) + +end function pow_quat__ + + +!-------------------------------------------------------------------------- +!> exponential of a quaternion +!> ToDo: Lacks any check for invalid operations +!-------------------------------------------------------------------------- +type(quaternion) elemental function exp__(self) + + implicit none + class(quaternion), intent(in) :: self + real(pReal) :: absImag + + absImag = norm2([self%x, self%y, self%z]) + + exp__ = exp(self%w) * [ cos(absImag), & + self%x/absImag * sin(absImag), & + self%y/absImag * sin(absImag), & + self%z/absImag * sin(absImag)] + +end function exp__ + + +!-------------------------------------------------------------------------- +!> logarithm of a quaternion +!> ToDo: Lacks any check for invalid operations +!-------------------------------------------------------------------------- +type(quaternion) elemental function log__(self) + + implicit none + class(quaternion), intent(in) :: self + real(pReal) :: absImag + + absImag = norm2([self%x, self%y, self%z]) + + log__ = [log(abs(self)), & + self%x/absImag * acos(self%w/abs(self)), & + self%y/absImag * acos(self%w/abs(self)), & + self%z/absImag * acos(self%w/abs(self))] + +end function log__ + + +!-------------------------------------------------------------------------- +!> norm of a quaternion +!-------------------------------------------------------------------------- +real(pReal) elemental function abs__(a) + + implicit none + class(quaternion), intent(in) :: a + + abs__ = norm2([a%w,a%x,a%y,a%z]) + +end function abs__ + + +!-------------------------------------------------------------------------- +!> dot product of two quaternions +!-------------------------------------------------------------------------- +real(pReal) elemental function dot_product__(a,b) + + implicit none + class(quaternion), intent(in) :: a,b + + dot_product__ = a%w*b%w + a%x*b%x + a%y*b%y + a%z*b%z + +end function dot_product__ + + +!-------------------------------------------------------------------------- +!> conjugate complex of a quaternion +!-------------------------------------------------------------------------- +type(quaternion) elemental function conjg__(a) + + implicit none + class(quaternion), intent(in) :: a + + conjg__ = quaternion([a%w, -a%x, -a%y, -a%z]) + +end function conjg__ + + +!-------------------------------------------------------------------------- +!> homomorphed quaternion of a quaternion +!-------------------------------------------------------------------------- +type(quaternion) elemental function quat_homomorphed(a) + + implicit none + class(quaternion), intent(in) :: a + + quat_homomorphed = quaternion(-[a%w,a%x,a%y,a%z]) + +end function quat_homomorphed + +end module quaternions diff --git a/src/rotations.f90 b/src/rotations.f90 new file mode 100644 index 000000000..830f49553 --- /dev/null +++ b/src/rotations.f90 @@ -0,0 +1,1088 @@ +! ################################################################### +! Copyright (c) 2013-2014, Marc De Graef/Carnegie Mellon University +! All rights reserved. +! +! Redistribution and use in source and binary forms, with or without modification, are +! permitted provided that the following conditions are met: +! +! - Redistributions of source code must retain the above copyright notice, this list +! of conditions and the following disclaimer. +! - Redistributions in binary form must reproduce the above copyright notice, this +! list of conditions and the following disclaimer in the documentation and/or +! other materials provided with the distribution. +! - Neither the names of Marc De Graef, Carnegie Mellon University nor the names +! of its contributors may be used to endorse or promote products derived from +! this software without specific prior written permission. +! +! THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +! AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +! IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +! ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +! LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +! DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +! SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +! CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +! OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE +! USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +! ################################################################### + +module rotations + use prec + use quaternions + + implicit none + type, public :: rotation + type(quaternion), private :: q + contains + procedure, public :: asEulerAngles => asEulerAngles + procedure, public :: asAxisAnglePair => asAxisAnglePair + procedure, public :: asRodriguesFrankVector => asRodriguesFrankVector + procedure, public :: asRotationMatrix => asRotationMatrix + procedure, public :: rotVector + procedure, public :: rotTensor + end type + + interface rotation + module procedure init + end interface + +contains + +type(rotation) function init(eu,ax,om,qu,cu,ho,ro) + real(pReal), intent(in), optional, dimension(3) :: eu, cu, ho + real(pReal), intent(in), optional, dimension(4) :: qu, ax, ro + real(pReal), intent(in), optional, dimension(3,3) :: om + + if (count([present(eu),present(ax),present(om),present(qu),& + present(cu),present(ho),present(ro)]) > 1_pInt) write(6,*) 'invalid' + + if (present(eu)) then + init%q = eu2qu(eu) + elseif (present(ax)) then + init%q = ax2qu(ax) + elseif (present(om)) then + init%q = om2qu(om) + elseif (present(qu)) then + init%q = quaternion(qu) + elseif (present(cu)) then + init%q = cu2qu(cu) + elseif (present(ho)) then + init%q = ho2qu(ho) + elseif (present(ro)) then + init%q = ro2qu(ro) + else + init%q = quaternion([1.0_pReal,0.0_pReal,0.0_pReal,0.0_pReal]) + endif + +end function + +function asEulerAngles(this) + class(rotation), intent(in) :: this + real(pReal), dimension(3) :: asEulerAngles + + asEulerAngles = qu2eu(this%q) + +end function asEulerAngles + + +function asAxisAnglePair(this) + class(rotation), intent(in) :: this + real(pReal), dimension(4) :: asAxisAnglePair + + asAxisAnglePair = qu2ax(this%q) + +end function asAxisAnglePair + + +function asRotationMatrix(this) + class(rotation), intent(in) :: this + real(pReal), dimension(3,3) :: asRotationMatrix + + asRotationMatrix = qu2om(this%q) + +end function asRotationMatrix + + +function asRodriguesFrankVector(this) + class(rotation), intent(in) :: this + real(pReal), dimension(4) :: asRodriguesFrankVector + + asRodriguesFrankVector = qu2ro(this%q) +end function asRodriguesFrankVector + + +function asHomochoric(this) + class(rotation), intent(in) :: this + real(pReal), dimension(3) :: asHomochoric + + asHomochoric = qu2ho(this%q) + +end function asHomochoric + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief rotates a vector passively (default) or actively +!-------------------------------------------------------------------------- +function rotVector(this,v,active) + class(rotation), intent(in) :: this + logical, intent(in), optional :: active + real(pReal),intent(in),dimension(3) :: v + real(pReal),dimension(3) :: rotVector + type(quaternion) :: q + + if (dEq(norm2(v),1.0_pReal,tol=1.0e-15_pReal)) then + passive: if (merge(.not. active, .true., present(active))) then + q = this%q * (quaternion([0.0_pReal, v(1), v(2), v(3)]) * conjg(this%q) ) + else passive + q = conjg(this%q) * (quaternion([0.0_pReal, v(1), v(2), v(3)]) * this%q ) + endif passive + rotVector = [q%x,q%y,q%z] + else + passive2: if (merge(.not. active, .true., present(active))) then + rotVector = matmul(this%asRotationMatrix(),v) + else passive2 + rotVector = matmul(transpose(this%asRotationMatrix()),v) + endif passive2 + endif + +end function rotVector + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief rotate a second rank tensor using a rotation matrix, active or passive (single precision) +!-------------------------------------------------------------------------- +function rotTensor(this,m,active) + class(rotation), intent(in) :: this + real(pReal),intent(in),dimension(3,3) :: m + logical, intent(in), optional :: active + real(pReal),dimension(3,3) :: rotTensor + + passive: if (merge(.not. active, .true., present(active))) then + rotTensor = matmul(matmul(this%asRotationMatrix(),m),transpose(this%asRotationMatrix())) + else passive + rotTensor = matmul(matmul(transpose(this%asRotationMatrix()),m),this%asRotationMatrix()) + endif passive + +end function rotTensor + + +!-------------------------------------------------------------------------- +!-------------------------------------------------------------------------- +! here we start with a series of conversion routines between representations +!-------------------------------------------------------------------------- +!-------------------------------------------------------------------------- + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief Euler angles to orientation matrix [Morawiec, page 28] +!-------------------------------------------------------------------------- +pure function eu2om(eu) result(om) + + implicit none + real(pReal), intent(in), dimension(3) :: eu !< Euler angles in radians + real(pReal), dimension(3,3) :: om !< output orientation matrix + real(pReal), dimension(3) :: c, s + + c = cos(eu) + s = sin(eu) + + om(1,1) = c(1)*c(3)-s(1)*s(3)*c(2) + om(1,2) = s(1)*c(3)+c(1)*s(3)*c(2) + om(1,3) = s(3)*s(2) + om(2,1) = -c(1)*s(3)-s(1)*c(3)*c(2) + om(2,2) = -s(1)*s(3)+c(1)*c(3)*c(2) + om(2,3) = c(3)*s(2) + om(3,1) = s(1)*s(2) + om(3,2) = -c(1)*s(2) + om(3,3) = c(2) + + where(dEq0(om)) om = 0.0_pReal + +end function eu2om + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief convert euler to axis angle +!-------------------------------------------------------------------------- +pure function eu2ax(eu) result(ax) + use math, only: & + PI + + implicit none + real(pReal), intent(in), dimension(3) :: eu !< Euler angles in radians + real(pReal), dimension(4) :: ax + real(pReal) :: t, delta, tau, alpha, sigma + + t = tan(eu(2)*0.5) + sigma = 0.5*(eu(1)+eu(3)) + delta = 0.5*(eu(1)-eu(3)) + tau = sqrt(t**2+sin(sigma)**2) + + alpha = merge(PI, 2.0*atan(tau/cos(sigma)), dEq(sigma,PI*0.5_pReal,tol=1.0e-15_pReal)) + + if (dEq0(alpha)) then ! return a default identity axis-angle pair + ax = [ 0.0_pReal, 0.0_pReal, 1.0_pReal, 0.0_pReal ] + else + ax(1:3) = -epsijk/tau * [ t*cos(delta), t*sin(delta), sin(sigma) ] ! passive axis-angle pair so a minus sign in front + ax(4) = alpha + if (alpha < 0.0) ax = -ax ! ensure alpha is positive + end if + +end function eu2ax + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief Euler angles to Rodrigues vector +!-------------------------------------------------------------------------- +pure function eu2ro(eu) result(ro) + use, intrinsic :: IEEE_ARITHMETIC + use math, only: & + PI + + implicit none + real(pReal), intent(in), dimension(3) :: eu !< Euler angles in radians + real(pReal), dimension(4) :: ro + + ro = eu2ax(eu) ! convert to axis angle representation + if (ro(4) >= PI) then + ro(4) = IEEE_value(ro(4),IEEE_positive_inf) + elseif(dEq0(ro(4))) then + ro = [ 0.0_pReal, 0.0_pReal, epsijk, 0.0_pReal ] + else + ro(4) = tan(ro(4)*0.5) + end if + +end function eu2ro + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief Euler angles to quaternion +!-------------------------------------------------------------------------- +pure function eu2qu(eu) result(qu) + + implicit none + real(pReal), intent(in), dimension(3) :: eu + type(quaternion) :: qu + real(pReal), dimension(3) :: ee + real(pReal) :: cPhi, sPhi + + ee = 0.5_pReal*eu + + cPhi = cos(ee(2)) + sPhi = sin(ee(2)) + + ! passive quaternion + qu = quaternion([ cPhi*cos(ee(1)+ee(3)), & + -epsijk*sPhi*cos(ee(1)-ee(3)), & + -epsijk*sPhi*sin(ee(1)-ee(3)), & + -epsijk*cPhi*sin(ee(1)+ee(3))]) + if(qu%w < 0.0_pReal) qu = qu%homomorphed() + +end function eu2qu + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief orientation matrix to euler angles +!-------------------------------------------------------------------------- +pure function om2eu(om) result(eu) + use math, only: & + PI + + implicit none + real(pReal), intent(in), dimension(3,3) :: om !< orientation matrix + real(pReal), dimension(3) :: eu + real(pReal) :: zeta + + if (dEq(abs(om(3,3)),1.0_pReal,1.0e-15_pReal)) then + eu = [ atan2( om(1,2),om(1,1)), 0.5*PI*(1-om(3,3)),0.0_pReal ] + else + zeta = 1.0_pReal/sqrt(1.0_pReal-om(3,3)**2.0_pReal) + eu = [atan2(om(3,1)*zeta,-om(3,2)*zeta), & + acos(om(3,3)), & + atan2(om(1,3)*zeta, om(2,3)*zeta)] + end if + where(eu<0.0_pReal) eu = mod(eu+2.0_pReal*PI,[2.0_pReal*PI,PI,2.0_pReal*PI]) + +end function om2eu + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief Axis angle pair to orientation matrix +!-------------------------------------------------------------------------- +pure function ax2om(ax) result(om) + + implicit none + real(pReal), intent(in), dimension(4) :: ax + real(pReal), dimension(3,3) :: om !< orientation matrix + real(pReal) :: q, c, s, omc + integer(pInt) :: i + + c = cos(ax(4)) + s = sin(ax(4)) + omc = 1.0-c + + forall(i=1:3) om(i,i) = ax(i)**2*omc + c + + q = omc*ax(1)*ax(2) + om(1,2) = q + s*ax(3) + om(2,1) = q - s*ax(3) + + q = omc*ax(2)*ax(3) + om(2,3) = q + s*ax(1) + om(3,2) = q - s*ax(1) + + q = omc*ax(3)*ax(1) + om(3,1) = q + s*ax(2) + om(1,3) = q - s*ax(2) + + if (epsijk > 0.0) om = transpose(om) + +end function ax2om + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief Quaternion to Euler angles [Morawiec page 40, with errata !!!! ] +!-------------------------------------------------------------------------- +pure function qu2eu(qu) result(eu) + use math, only: & + PI + + implicit none + type(quaternion), intent(in) :: qu !< quaternion + real(pReal), dimension(3) :: eu + real(pReal) :: q12, q03, chi, chiInv + + q03 = qu%w**2+qu%z**2 + q12 = qu%x**2+qu%y**2 + chi = sqrt(q03*q12) + + degenerated: if (dEq0(chi)) then + eu = merge([atan2(-epsijk*2.0*qu%w*qu%z,qu%w**2-qu%z**2), 0.0_pReal, 0.0_pReal], & + [atan2(2.0*qu%x*qu%y,qu%x**2-qu%y**2), PI, 0.0_pReal], & + dEq0(q12)) + else degenerated + chiInv = 1.0/chi + eu = [atan2((-epsijk*qu%w*qu%y+qu%x*qu%z)*chi, (-epsijk*qu%w*qu%x-qu%y*qu%z)*chi ), & + atan2( 2.0*chi, q03-q12 ), & + atan2(( epsijk*qu%w*qu%y+qu%x*qu%z)*chi, (-epsijk*qu%w*qu%x+qu%y*qu%z)*chi )] + endif degenerated + where(eu<0.0_pReal) eu = mod(eu+2.0_pReal*PI,[2.0_pReal*PI,PI,2.0_pReal*PI]) + +end function qu2eu + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief Axis angle pair to homochoric +!-------------------------------------------------------------------------- +pure function ax2ho(ax) result(ho) + + + real(pReal), intent(in), dimension(4) :: ax !< axis angle in degree/radians? + real(pReal), dimension(3) :: ho + real(pReal) :: f + + f = 0.75 * ( ax(4) - sin(ax(4)) ) + f = f**(1.0/3.0) + ho = ax(1:3) * f + +end function ax2ho + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief Homochoric to axis angle pair +!-------------------------------------------------------------------------- +pure function ho2ax(ho) result(ax) + + implicit none + real(pReal), intent(in), dimension(3) :: ho !< homochoric coordinates + real(pReal), dimension(4) :: ax + integer(pInt) :: i + real(pReal) :: hmag_squared, s, hm + real(pReal), parameter, dimension(16) :: & + tfit = [ 1.0000000000018852_pReal, -0.5000000002194847_pReal, & + -0.024999992127593126_pReal, -0.003928701544781374_pReal, & + -0.0008152701535450438_pReal, -0.0002009500426119712_pReal, & + -0.00002397986776071756_pReal, -0.00008202868926605841_pReal, & + +0.00012448715042090092_pReal, -0.0001749114214822577_pReal, & + +0.0001703481934140054_pReal, -0.00012062065004116828_pReal, & + +0.000059719705868660826_pReal, -0.00001980756723965647_pReal, & + +0.000003953714684212874_pReal, -0.00000036555001439719544_pReal ] + + ! normalize h and store the magnitude + hmag_squared = sum(ho**2.0_pReal) + if (dEq0(hmag_squared)) then + ax = [ 0.0_pReal, 0.0_pReal, 1.0_pReal, 0.0_pReal ] + else + hm = hmag_squared + + ! convert the magnitude to the rotation angle + s = tfit(1) + tfit(2) * hmag_squared + do i=3,16 + hm = hm*hmag_squared + s = s + tfit(i) * hm + end do + ax = [ho/sqrt(hmag_squared), 2.0_pReal*acos(s)] + end if + +end function ho2ax + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief convert orientation matrix to axis angle +!-------------------------------------------------------------------------- +function om2ax(om) result(ax) + use IO, only: & + IO_error + use math, only: & + math_clip, & + math_trace33 + + implicit none + real(pReal), intent(in) :: om(3,3) + real(pReal) :: ax(4) + + real(pReal) :: t + real(pReal), dimension(3) :: Wr, Wi + real(pReal), dimension(10) :: WORK + real(pReal), dimension(3,3) :: VR, devNull, o + integer(pInt) :: INFO, LWORK, i + + external :: dgeev,sgeev + + o = om + + ! first get the rotation angle + t = 0.5_pReal * (math_trace33(om) - 1.0) + ax(4) = acos(math_clip(t,-1.0_pReal,1.0_pReal)) + + if (dEq0(ax(4))) then + ax(1:3) = [ 0.0, 0.0, 1.0 ] + else + ! set some initial LAPACK variables + INFO = 0 + ! first initialize the parameters for the LAPACK DGEEV routines + LWORK = 20 + + ! call the eigenvalue solver +#if (FLOAT==8) + call dgeev('N','V',3,o,3,Wr,Wi,devNull,3,VR,3,WORK,LWORK,INFO) +#elif (FLOAT==4) + call sgeev('N','V',3,o,3,Wr,Wi,devNull,3,VR,3,WORK,LWORK,INFO) +#else + NO SUITABLE PRECISION FOR REAL SELECTED, STOPPING COMPILATION +#endif + if (INFO /= 0) call IO_error(0_pInt,ext_msg='Error in om2ax/(s/d)geev: (S/D)GEEV return not zero') + i = maxloc(merge(1.0_pReal,0.0_pReal,cEq(cmplx(Wr,Wi,pReal),cmplx(1.0_pReal,0.0_pReal,pReal),tol=1.0e-14_pReal)),dim=1) ! poor substitute for findloc + ax(1:3) = VR(1:3,i) + where ( dNeq0([om(2,3)-om(3,2), om(3,1)-om(1,3), om(1,2)-om(2,1)])) & + ax(1:3) = sign(ax(1:3),-epsijk *[om(2,3)-om(3,2), om(3,1)-om(1,3), om(1,2)-om(2,1)]) + endif + +end function om2ax + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief Rodrigues vector to axis angle pair +!-------------------------------------------------------------------------- +pure function ro2ax(ro) result(ax) + use, intrinsic :: IEEE_ARITHMETIC + use math, only: & + PI + + implicit none + real(pReal), intent(in), dimension(4) :: ro !< homochoric coordinates + real(pReal), dimension(4) :: ax + real(pReal) :: ta, angle + + ta = ro(4) + + if (dEq0(ta)) then + ax = [ 0.0, 0.0, 1.0, 0.0 ] + elseif (.not. IEEE_is_finite(ta)) then + ax = [ ro(1), ro(2), ro(3), PI ] + else + angle = 2.0*atan(ta) + ta = 1.0/norm2(ro(1:3)) + ax = [ ro(1)/ta, ro(2)/ta, ro(3)/ta, angle ] + end if + +end function ro2ax + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief convert axis angle to Rodrigues +!-------------------------------------------------------------------------- +pure function ax2ro(ax) result(ro) + use, intrinsic :: IEEE_ARITHMETIC + use math, only: & + PI + + implicit none + real(pReal), intent(in), dimension(4) :: ax !< axis angle in degree/radians? + real(pReal), dimension(4) :: ro + real(pReal), parameter :: thr = 1.0E-7 + + if (dEq0(ax(4))) then + ro = [ 0.0_pReal, 0.0_pReal, epsijk, 0.0_pReal ] + else + ro(1:3) = ax(1:3) + ! we need to deal with the 180 degree case + ro(4) = merge(IEEE_value(ro(4),IEEE_positive_inf),tan(ax(4)*0.5 ),abs(ax(4)-PI) < thr) + end if + +end function ax2ro + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief convert axis angle to quaternion +!-------------------------------------------------------------------------- +pure function ax2qu(ax) result(qu) + + implicit none + real(pReal), intent(in), dimension(4) :: ax + type(quaternion) :: qu + real(pReal) :: c, s + + + if (dEq0(ax(4))) then + qu = quaternion([ 1.0_pReal, 0.0_pReal, 0.0_pReal, 0.0_pReal ]) + else + c = cos(ax(4)*0.5) + s = sin(ax(4)*0.5) + qu = quaternion([ c, ax(1)*s, ax(2)*s, ax(3)*s ]) + end if + +end function ax2qu + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief convert rodrigues to homochoric +!-------------------------------------------------------------------------- +pure function ro2ho(ro) result(ho) + use, intrinsic :: IEEE_ARITHMETIC + use math, only: & + PI + + implicit none + real(pReal), intent(in), dimension(4) :: ro + real(pReal), dimension(3) :: ho + real(pReal) :: f + + if (dEq0(norm2(ro(1:3)))) then + ho = [ 0.0, 0.0, 0.0 ] + else + f = merge(2.0*atan(ro(4)) - sin(2.0*atan(ro(4))),PI, IEEE_is_finite(ro(4))) + ho = ro(1:3) * (0.75_pReal*f)**(1.0/3.0) + end if + +end function ro2ho + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief convert a quaternion to a 3x3 matrix +!-------------------------------------------------------------------------- +pure function qu2om(qu) result(om) + + implicit none + type(quaternion), intent(in) :: qu + real(pReal), dimension(3,3) :: om + real(pReal) :: qq + + qq = qu%w**2-(qu%x**2 + qu%y**2 + qu%z**2) + + + om(1,1) = qq+2.0*qu%x*qu%x + om(2,2) = qq+2.0*qu%y*qu%y + om(3,3) = qq+2.0*qu%z*qu%z + + om(1,2) = 2.0*(qu%x*qu%y-qu%w*qu%z) + om(2,3) = 2.0*(qu%y*qu%z-qu%w*qu%x) + om(3,1) = 2.0*(qu%z*qu%x-qu%w*qu%y) + om(2,1) = 2.0*(qu%y*qu%x+qu%w*qu%z) + om(3,2) = 2.0*(qu%z*qu%y+qu%w*qu%x) + om(1,3) = 2.0*(qu%x*qu%z+qu%w*qu%y) + + if (epsijk < 0.0) om = transpose(om) + +end function qu2om + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief convert a 3x3 rotation matrix to a unit quaternion (see Morawiec, page 37) +!-------------------------------------------------------------------------- +function om2qu(om) result(qu) + + implicit none + real(pReal), intent(in), dimension(3,3) :: om + type(quaternion) :: qu + real(pReal), dimension(4) :: qu_A + real(pReal), dimension(4) :: s + + s = [+om(1,1) +om(2,2) +om(3,3) +1.0_pReal, & + +om(1,1) -om(2,2) -om(3,3) +1.0_pReal, & + -om(1,1) +om(2,2) -om(3,3) +1.0_pReal, & + -om(1,1) -om(2,2) +om(3,3) +1.0_pReal] + + qu_A = sqrt(max(s,0.0_pReal))*0.5_pReal*[1.0_pReal,epsijk,epsijk,epsijk] + qu_A = qu_A/norm2(qu_A) + + if(any(dEq(abs(qu_A),1.0_pReal,1.0e-15_pReal))) & + where (.not.(dEq(abs(qu_A),1.0_pReal,1.0e-15_pReal))) qu_A = 0.0_pReal + + if (om(3,2) < om(2,3)) qu_A(2) = -qu_A(2) + if (om(1,3) < om(3,1)) qu_A(3) = -qu_A(3) + if (om(2,1) < om(1,2)) qu_A(4) = -qu_A(4) + + qu = quaternion(qu_A) + !qu_A = om2ax(om) + !if(any(qu_A(1:3) * [qu%x,qu%y,qu%z] < 0.0)) print*, 'sign error' + +end function om2qu + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief convert quaternion to axis angle +!-------------------------------------------------------------------------- +pure function qu2ax(qu) result(ax) + use math, only: & + PI + + implicit none + type(quaternion), intent(in) :: qu + real(pReal), dimension(4) :: ax + real(pReal) :: omega, s + + omega = 2.0 * acos(qu%w) + ! if the angle equals zero, then we return the rotation axis as [001] + if (dEq0(omega)) then + ax = [ 0.0, 0.0, 1.0, 0.0 ] + elseif (dNeq0(qu%w)) then + s = sign(1.0_pReal,qu%w)/sqrt(qu%x**2+qu%y**2+qu%z**2) + ax = [ qu%x*s, qu%y*s, qu%z*s, omega ] + else + ax = [ qu%x, qu%y, qu%z, PI ] + end if + +end function qu2ax + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief convert quaternion to Rodrigues +!-------------------------------------------------------------------------- +pure function qu2ro(qu) result(ro) + use, intrinsic :: IEEE_ARITHMETIC + + type(quaternion), intent(in) :: qu + real(pReal), dimension(4) :: ro + real(pReal) :: s + real(pReal), parameter :: thr = 1.0e-8_pReal + + if (qu%w < thr) then + ro = [qu%x, qu%y, qu%z, IEEE_value(ro(4),IEEE_positive_inf)] + else + s = norm2([qu%x,qu%y,qu%z]) + ro = merge ( [ 0.0_pReal, 0.0_pReal, epsijk, 0.0_pReal] , & + [ qu%x/s, qu%y/s, qu%z/s, tan(acos(qu%w))], & + s < thr) + end if + +end function qu2ro + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief convert quaternion to homochoric +!-------------------------------------------------------------------------- +pure function qu2ho(qu) result(ho) + + implicit none + type(quaternion), intent(in) :: qu + real(pReal), dimension(3) :: ho + real(pReal) :: omega, f + + omega = 2.0 * acos(qu%w) + + if (dEq0(omega)) then + ho = [ 0.0, 0.0, 0.0 ] + else + ho = [qu%x, qu%y, qu%z] + f = 0.75 * ( omega - sin(omega) ) + ho = ho/norm2(ho)* f**(1.0/3.0) + end if + +end function qu2ho + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief convert homochoric to cubochoric +!-------------------------------------------------------------------------- +function ho2cu(ho) result(cu) + use Lambert, only: LambertBallToCube + + implicit none + real(pReal), intent(in), dimension(3) :: ho + real(pReal), dimension(3) :: cu + + cu = LambertBallToCube(ho) + +end function ho2cu + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief convert cubochoric to homochoric +!-------------------------------------------------------------------------- +function cu2ho(cu) result(ho) + use Lambert, only: LambertCubeToBall + + implicit none + real(pReal), intent(in), dimension(3) :: cu + real(pReal), dimension(3) :: ho + + ho = LambertCubeToBall(cu) + +end function cu2ho + +!-------------------------------------------------------------------------- +!-------------------------------------------------------------------------- +! and here are a bunch of transformation routines that are derived from the others +!-------------------------------------------------------------------------- +!-------------------------------------------------------------------------- +!-------------------------------------------------------------------------- + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief Rodrigues vector to Euler angles +!-------------------------------------------------------------------------- +pure function ro2eu(ro) result(eu) + + implicit none + real(pReal), intent(in), dimension(4) :: ro !< Rodrigues vector + real(pReal), dimension(3) :: eu + + eu = om2eu(ro2om(ro)) + +end function ro2eu + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief convert euler to homochoric +!-------------------------------------------------------------------------- +pure function eu2ho(eu) result(ho) + + implicit none + real(pReal), intent(in), dimension(3) :: eu + real(pReal), dimension(3) :: ho + + ho = ax2ho(eu2ax(eu)) + +end function eu2ho + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief convert orientation matrix to Rodrigues +!-------------------------------------------------------------------------- +pure function om2ro(om) result(ro) + + implicit none + real(pReal), intent(in), dimension(3,3) :: om + real(pReal), dimension(4) :: ro + + ro = eu2ro(om2eu(om)) + +end function om2ro + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief convert orientation matrix to homochoric +!-------------------------------------------------------------------------- +function om2ho(om) result(ho) + + implicit none + real(pReal), intent(in), dimension(3,3) :: om + real(pReal), dimension(3) :: ho + + ho = ax2ho(om2ax(om)) + +end function om2ho + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief convert axis angle to euler +!-------------------------------------------------------------------------- +pure function ax2eu(ax) result(eu) + + implicit none + real(pReal), intent(in), dimension(4) :: ax + real(pReal), dimension(3) :: eu + + eu = om2eu(ax2om(ax)) + +end function ax2eu + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief convert rodrigues to orientation matrix +!-------------------------------------------------------------------------- +pure function ro2om(ro) result(om) + + implicit none + real(pReal), intent(in), dimension(4) :: ro + real(pReal), dimension(3,3) :: om + + om = ax2om(ro2ax(ro)) + +end function ro2om + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief convert rodrigues to quaternion +!-------------------------------------------------------------------------- +pure function ro2qu(ro) result(qu) + + implicit none + real(pReal), intent(in), dimension(4) :: ro + type(quaternion) :: qu + + qu = ax2qu(ro2ax(ro)) + +end function ro2qu + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief convert homochoric to euler +!-------------------------------------------------------------------------- +pure function ho2eu(ho) result(eu) + + implicit none + real(pReal), intent(in), dimension(3) :: ho + real(pReal), dimension(3) :: eu + + eu = ax2eu(ho2ax(ho)) + +end function ho2eu + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief convert homochoric to orientation matrix +!-------------------------------------------------------------------------- +pure function ho2om(ho) result(om) + + implicit none + real(pReal), intent(in), dimension(3) :: ho + real(pReal), dimension(3,3) :: om + + om = ax2om(ho2ax(ho)) + +end function ho2om + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief convert homochoric to Rodrigues +!-------------------------------------------------------------------------- +pure function ho2ro(ho) result(ro) + + implicit none + real(pReal), intent(in), dimension(3) :: ho + real(pReal), dimension(4) :: ro + + + ro = ax2ro(ho2ax(ho)) + +end function ho2ro + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief convert homochoric to quaternion +!-------------------------------------------------------------------------- +pure function ho2qu(ho) result(qu) + + implicit none + real(pReal), intent(in), dimension(3) :: ho + type(quaternion) :: qu + + qu = ax2qu(ho2ax(ho)) + +end function ho2qu + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief convert euler angles to cubochoric +!-------------------------------------------------------------------------- +function eu2cu(eu) result(cu) + + implicit none + real(pReal), intent(in), dimension(3) :: eu !< Bunge-Euler angles in radians + real(pReal), dimension(3) :: cu + + cu = ho2cu(eu2ho(eu)) + +end function eu2cu + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief convert orientation matrix to cubochoric +!-------------------------------------------------------------------------- +function om2cu(om) result(cu) + + implicit none + real(pReal), intent(in), dimension(3,3) :: om !< rotation matrix + real(pReal), dimension(3) :: cu + + cu = ho2cu(om2ho(om)) + +end function om2cu + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief convert axis angle to cubochoric +!-------------------------------------------------------------------------- +function ax2cu(ax) result(cu) + + implicit none + real(pReal), intent(in), dimension(4) :: ax !< axis angle in degree/radians? + real(pReal), dimension(3) :: cu + + cu = ho2cu(ax2ho(ax)) + +end function ax2cu + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief convert Rodrigues to cubochoric +!-------------------------------------------------------------------------- +function ro2cu(ro) result(cu) + + implicit none + real(pReal), intent(in), dimension(4) :: ro !< Rodrigues vector + real(pReal), dimension(3) :: cu + + cu = ho2cu(ro2ho(ro)) + +end function ro2cu + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief convert quaternion to cubochoric +!-------------------------------------------------------------------------- +function qu2cu(qu) result(cu) + + implicit none + type(quaternion), intent(in) :: qu ! unit quaternion + real(pReal), dimension(3) :: cu + + cu = ho2cu(qu2ho(qu)) + +end function qu2cu + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief convert cubochoric to euler angles +!-------------------------------------------------------------------------- +function cu2eu(cu) result(eu) + + implicit none + real(pReal), intent(in), dimension(3) :: cu ! cubochoric? + real(pReal), dimension(3) :: eu + + eu = ho2eu(cu2ho(cu)) + +end function cu2eu + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief convert cubochoric to orientation matrix +!-------------------------------------------------------------------------- +function cu2om(cu) result(om) + + implicit none + real(pReal), intent(in), dimension(3) :: cu ! cubochoric? + real(pReal), dimension(3,3) :: om + + om = ho2om(cu2ho(cu)) + +end function cu2om + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief convert cubochoric to axis angle +!-------------------------------------------------------------------------- +function cu2ax(cu) result(ax) + + implicit none + real(pReal), intent(in), dimension(3) :: cu ! cubochoric? + real(pReal), dimension(4) :: ax + + ax = ho2ax(cu2ho(cu)) + +end function cu2ax + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief convert cubochoric to Rodrigues +!-------------------------------------------------------------------------- +function cu2ro(cu) result(ro) + + implicit none + real(pReal), intent(in), dimension(3) :: cu ! cubochoric? + real(pReal), dimension(4) :: ro + + ro = ho2ro(cu2ho(cu)) + +end function cu2ro + + +!-------------------------------------------------------------------------- +!> @author Marc De Graef, Carnegie Mellon University +!> @brief convert cubochoric to quaternion +!-------------------------------------------------------------------------- +function cu2qu(cu) result(qu) + + implicit none + real(pReal), intent(in), dimension(3) :: cu ! cubochoric? + type(quaternion) :: qu ! cubochoric? + + qu = ho2qu(cu2ho(cu)) + +end function cu2qu + +end module rotations