better handling of corner cases
copy and paste from python code
This commit is contained in:
parent
e0c2dbc948
commit
dbca47f113
|
@ -464,6 +464,8 @@ subroutine selfTest
|
||||||
real(pReal), dimension(4) :: qu
|
real(pReal), dimension(4) :: qu
|
||||||
type(quaternion) :: q, q_2
|
type(quaternion) :: q, q_2
|
||||||
|
|
||||||
|
if(dNeq(abs(P),1.0_pReal)) call IO_error(0,ext_msg='P not in {-1,+1}')
|
||||||
|
|
||||||
call random_number(qu)
|
call random_number(qu)
|
||||||
qu = (qu-0.5_pReal) * 2.0_pReal
|
qu = (qu-0.5_pReal) * 2.0_pReal
|
||||||
q = quaternion(qu)
|
q = quaternion(qu)
|
||||||
|
|
|
@ -581,7 +581,7 @@ pure function om2eu(om) result(eu)
|
||||||
real(pReal), dimension(3) :: eu
|
real(pReal), dimension(3) :: eu
|
||||||
real(pReal) :: zeta
|
real(pReal) :: zeta
|
||||||
|
|
||||||
if (abs(om(3,3)) < 1.0_pReal) then
|
if(dNeq(abs(om(3,3)),1.0_pReal,1.e-9_pReal)) then
|
||||||
zeta = 1.0_pReal/sqrt(1.0_pReal-om(3,3)**2.0_pReal)
|
zeta = 1.0_pReal/sqrt(1.0_pReal-om(3,3)**2.0_pReal)
|
||||||
eu = [atan2(om(3,1)*zeta,-om(3,2)*zeta), &
|
eu = [atan2(om(3,1)*zeta,-om(3,2)*zeta), &
|
||||||
acos(om(3,3)), &
|
acos(om(3,3)), &
|
||||||
|
@ -589,7 +589,7 @@ pure function om2eu(om) result(eu)
|
||||||
else
|
else
|
||||||
eu = [atan2(om(1,2),om(1,1)), 0.5_pReal*PI*(1.0_pReal-om(3,3)),0.0_pReal ]
|
eu = [atan2(om(1,2),om(1,1)), 0.5_pReal*PI*(1.0_pReal-om(3,3)),0.0_pReal ]
|
||||||
end if
|
end if
|
||||||
|
where(abs(eu) < 1.e-8_pReal) eu = 0.0_pReal
|
||||||
where(eu<0.0_pReal) eu = mod(eu+2.0_pReal*PI,[2.0_pReal*PI,PI,2.0_pReal*PI])
|
where(eu<0.0_pReal) eu = mod(eu+2.0_pReal*PI,[2.0_pReal*PI,PI,2.0_pReal*PI])
|
||||||
|
|
||||||
end function om2eu
|
end function om2eu
|
||||||
|
|
Loading…
Reference in New Issue