using norm2 function and newly introduced dNeq0/dEq0
This commit is contained in:
parent
e97f3a788d
commit
ffcc9300c5
|
@ -724,7 +724,7 @@ end function math_transpose33
|
|||
!--------------------------------------------------------------------------------------------------
|
||||
pure function math_inv33(A)
|
||||
use prec, only: &
|
||||
dNeq
|
||||
dNeq0
|
||||
|
||||
implicit none
|
||||
real(pReal),dimension(3,3),intent(in) :: A
|
||||
|
@ -737,7 +737,7 @@ pure function math_inv33(A)
|
|||
|
||||
DetA = A(1,1) * math_inv33(1,1) + A(1,2) * math_inv33(2,1) + A(1,3) * math_inv33(3,1)
|
||||
|
||||
if (dNeq(DetA,0.0_pReal)) then
|
||||
if (dNeq0(DetA)) then
|
||||
math_inv33(1,2) = -A(1,2) * A(3,3) + A(1,3) * A(3,2)
|
||||
math_inv33(2,2) = A(1,1) * A(3,3) - A(1,3) * A(3,1)
|
||||
math_inv33(3,2) = -A(1,1) * A(3,2) + A(1,2) * A(3,1)
|
||||
|
@ -762,7 +762,7 @@ end function math_inv33
|
|||
!--------------------------------------------------------------------------------------------------
|
||||
pure subroutine math_invert33(A, InvA, DetA, error)
|
||||
use prec, only: &
|
||||
dEq
|
||||
dEq0
|
||||
|
||||
implicit none
|
||||
logical, intent(out) :: error
|
||||
|
@ -776,7 +776,7 @@ pure subroutine math_invert33(A, InvA, DetA, error)
|
|||
|
||||
DetA = A(1,1) * InvA(1,1) + A(1,2) * InvA(2,1) + A(1,3) * InvA(3,1)
|
||||
|
||||
if (dEq(DetA,0.0_pReal)) then
|
||||
if (dEq0(DetA)) then
|
||||
InvA = 0.0_pReal
|
||||
error = .true.
|
||||
else
|
||||
|
@ -1212,10 +1212,10 @@ function math_qRand()
|
|||
real(pReal), dimension(3) :: rnd
|
||||
|
||||
call halton(3_pInt,rnd)
|
||||
math_qRand(1) = cos(2.0_pReal*PI*rnd(1))*sqrt(rnd(3))
|
||||
math_qRand(2) = sin(2.0_pReal*PI*rnd(2))*sqrt(1.0_pReal-rnd(3))
|
||||
math_qRand(3) = cos(2.0_pReal*PI*rnd(2))*sqrt(1.0_pReal-rnd(3))
|
||||
math_qRand(4) = sin(2.0_pReal*PI*rnd(1))*sqrt(rnd(3))
|
||||
math_qRand = [cos(2.0_pReal*PI*rnd(1))*sqrt(rnd(3)),
|
||||
sin(2.0_pReal*PI*rnd(2))*sqrt(1.0_pReal-rnd(3)),
|
||||
cos(2.0_pReal*PI*rnd(2))*sqrt(1.0_pReal-rnd(3)),
|
||||
sin(2.0_pReal*PI*rnd(1))*sqrt(rnd(3))]
|
||||
|
||||
end function math_qRand
|
||||
|
||||
|
@ -1282,7 +1282,7 @@ end function math_qNorm
|
|||
!--------------------------------------------------------------------------------------------------
|
||||
pure function math_qInv(Q)
|
||||
use prec, only: &
|
||||
dNeq
|
||||
dNeq0
|
||||
|
||||
implicit none
|
||||
real(pReal), dimension(4), intent(in) :: Q
|
||||
|
@ -1292,7 +1292,7 @@ pure function math_qInv(Q)
|
|||
math_qInv = 0.0_pReal
|
||||
|
||||
squareNorm = math_qDot(Q,Q)
|
||||
if (dNeq(squareNorm,0.0_pReal)) math_qInv = math_qConj(Q) / squareNorm
|
||||
if (dNeq0(squareNorm)) math_qInv = math_qConj(Q) / squareNorm
|
||||
|
||||
end function math_qInv
|
||||
|
||||
|
@ -1493,7 +1493,7 @@ pure function math_axisAngleToR(axis,omega)
|
|||
real(pReal), dimension(3) :: axisNrm
|
||||
real(pReal) :: norm,s,c,c1
|
||||
|
||||
norm = sqrt(math_mul3x3(axis,axis))
|
||||
norm = norm2(axis)
|
||||
if (norm > 1.0e-8_pReal) then ! non-zero rotation
|
||||
axisNrm = axis/norm ! normalize axis to be sure
|
||||
|
||||
|
@ -1623,13 +1623,13 @@ pure function math_qToEuler(qPassive)
|
|||
|
||||
q = math_qConj(qPassive) ! convert to active rotation, since formulas are defined for active rotations
|
||||
|
||||
math_qToEuler(2) = acos(1.0_pReal-2.0_pReal*(q(2)*q(2)+q(3)*q(3)))
|
||||
math_qToEuler(2) = acos(1.0_pReal-2.0_pReal*(q(2)**2+q(3)**2))
|
||||
|
||||
if (abs(math_qToEuler(2)) < 1.0e-6_pReal) then
|
||||
math_qToEuler(1) = sign(2.0_pReal*acos(math_limit(q(1),-1.0_pReal, 1.0_pReal)),q(4))
|
||||
math_qToEuler(3) = 0.0_pReal
|
||||
else
|
||||
math_qToEuler(1) = atan2(q(1)*q(3)+q(2)*q(4), q(1)*q(2)-q(3)*q(4))
|
||||
math_qToEuler(1) = atan2(+q(1)*q(3)+q(2)*q(4), q(1)*q(2)-q(3)*q(4))
|
||||
math_qToEuler(3) = atan2(-q(1)*q(3)+q(2)*q(4), q(1)*q(2)+q(3)*q(4))
|
||||
endif
|
||||
|
||||
|
@ -2097,7 +2097,7 @@ end function math_eigenvectorBasisSym33
|
|||
!--------------------------------------------------------------------------------------------------
|
||||
function math_rotationalPart33(m)
|
||||
use prec, only: &
|
||||
dEq
|
||||
dEq0
|
||||
use IO, only: &
|
||||
IO_warning
|
||||
|
||||
|
@ -2109,7 +2109,7 @@ function math_rotationalPart33(m)
|
|||
U = math_eigenvectorBasisSym33(math_mul33x33(transpose(m),m))
|
||||
Uinv = math_inv33(U)
|
||||
|
||||
inversionFailed: if (all(dEq(Uinv,0.0_pReal))) then
|
||||
inversionFailed: if (all(dEq0(Uinv))) then
|
||||
math_rotationalPart33 = math_I3
|
||||
call IO_warning(650_pInt)
|
||||
else inversionFailed
|
||||
|
|
|
@ -115,8 +115,10 @@ module prec
|
|||
prec_init, &
|
||||
prec_isNaN, &
|
||||
dEq, &
|
||||
dEq0, &
|
||||
cEq, &
|
||||
dNeq, &
|
||||
dNeq0, &
|
||||
cNeq
|
||||
|
||||
contains
|
||||
|
|
Loading…
Reference in New Issue