using norm2 function and newly introduced dNeq0/dEq0
This commit is contained in:
Martin Diehl
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
|
||||
|
|
@ -1100,7 +1100,7 @@ pure function math_Plain99to3333(m99)
|
|||
integer(pInt) :: i,j
|
||||
|
||||
forall (i=1_pInt:9_pInt,j=1_pInt:9_pInt) math_Plain99to3333(mapPlain(1,i),mapPlain(2,i),&
|
||||
mapPlain(1,j),mapPlain(2,j)) = m99(i,j)
|
||||
mapPlain(1,j),mapPlain(2,j)) = m99(i,j)
|
||||
|
||||
end function math_Plain99to3333
|
||||
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -1599,7 +1599,7 @@ pure function math_qToR(q)
|
|||
|
||||
S = reshape( [0.0_pReal, -q(4), q(3), &
|
||||
q(4), 0.0_pReal, -q(2), &
|
||||
-q(3), q(2), 0.0_pReal],[3,3]) ! notation is transposed
|
||||
-q(3), q(2), 0.0_pReal],[3,3]) ! notation is transposed
|
||||
|
||||
math_qToR = (2.0_pReal * q(1)*q(1) - 1.0_pReal) * math_I3 &
|
||||
+ 2.0_pReal * T - 2.0_pReal * q(1) * S
|
||||
|
|
@ -1623,17 +1623,17 @@ 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
|
||||
|
||||
math_qToEuler = merge(math_qToEuler + [2.0_pReal*PI, PI, 2.0_pReal*PI], & ! ensure correct range
|
||||
math_qToEuler = merge(math_qToEuler + [2.0_pReal*PI, PI, 2.0_pReal*PI], & ! ensure correct range
|
||||
math_qToEuler, math_qToEuler<0.0_pReal)
|
||||
|
||||
end function math_qToEuler
|
||||
|
|
@ -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