# This file is a part of Julia. License is MIT: http://julialang.org/license
# givensAlgorithm functions are derived from LAPACK, see below
abstract AbstractRotation{T}
transpose(R::AbstractRotation) = error("transpose not implemented for $(typeof(R)). Consider using conjugate transpose (') instead of transpose (.').")
function *{T,S}(R::AbstractRotation{T}, A::AbstractVecOrMat{S})
TS = typeof(zero(T)*zero(S) + zero(T)*zero(S))
A_mul_B!(convert(AbstractRotation{TS}, R), TS == S ? copy(A) : convert(AbstractArray{TS}, A))
end
function A_mul_Bc{T,S}(A::AbstractVecOrMat{T}, R::AbstractRotation{S})
TS = typeof(zero(T)*zero(S) + zero(T)*zero(S))
A_mul_Bc!(TS == T ? copy(A) : convert(AbstractArray{TS}, A), convert(AbstractRotation{TS}, R))
end
immutable Givens{T} <: AbstractRotation{T}
i1::Int
i2::Int
c::T
s::T
end
type Rotation{T} <: AbstractRotation{T}
rotations::Vector{Givens{T}}
end
convert{T}(::Type{Givens{T}}, G::Givens{T}) = G
convert{T}(::Type{Givens{T}}, G::Givens) = Givens(G.i1, G.i2, convert(T, G.c), convert(T, G.s))
convert{T}(::Type{Rotation{T}}, R::Rotation{T}) = R
convert{T}(::Type{Rotation{T}}, R::Rotation) = Rotation{T}([convert(Givens{T}, g) for g in R.rotations])
convert{T}(::Type{AbstractRotation{T}}, G::Givens) = convert(Givens{T}, G)
convert{T}(::Type{AbstractRotation{T}}, R::Rotation) = convert(Rotation{T}, R)
ctranspose(G::Givens) = Givens(G.i1, G.i2, conj(G.c), -G.s)
ctranspose{T}(R::Rotation{T}) = Rotation{T}(reverse!([ctranspose(r) for r in R.rotations]))
realmin2(::Type{Float32}) = reinterpret(Float32, 0x26000000)
realmin2(::Type{Float64}) = reinterpret(Float64, 0x21a0000000000000)
realmin2{T}(::Type{T}) = (twopar = 2one(T); twopar^trunc(Integer,log(realmin(T)/eps(T))/log(twopar)/twopar))
# derived from LAPACK's dlartg
# Copyright:
# Univ. of Tennessee
# Univ. of California Berkeley
# Univ. of Colorado Denver
# NAG Ltd.
function givensAlgorithm{T<:AbstractFloat}(f::T, g::T)
zeropar = zero(T)
onepar = one(T)
twopar = 2one(T)
safmin = realmin(T)
epspar = eps(T)
safmn2 = realmin2(T)
safmx2 = onepar/safmn2
if g == 0
cs = onepar
sn = zeropar
r = f
elseif f == 0
cs = zeropar
sn = onepar
r = g
else
f1 = f
g1 = g
scalepar = max(abs(f1), abs(g1))
if scalepar >= safmx2
count = 0
while true
count += 1
f1 *= safmn2
g1 *= safmn2
scalepar = max(abs(f1), abs(g1))
if scalepar < safmx2 break end
end
r = sqrt(f1*f1 + g1*g1)
cs = f1/r
sn = g1/r
for i = 1:count
r *= safmx2
end
elseif scalepar <= safmn2
count = 0
while true
count += 1
f1 *= safmx2
g1 *= safmx2
scalepar = max(abs(f1), abs(g1))
if scalepar > safmn2 break end
end
r = sqrt(f1*f1 + g1*g1)
cs = f1/r
sn = g1/r
for i = 1:count
r *= safmn2
end
else
r = sqrt(f1*f1 + g1*g1)
cs = f1/r
sn = g1/r
end
if abs(f) > abs(g) && cs < 0
cs = -cs
sn = -sn
r = -r
end
end
return cs, sn, r
end
# derived from LAPACK's zlartg
# Copyright:
# Univ. of Tennessee
# Univ. of California Berkeley
# Univ. of Colorado Denver
# NAG Ltd.
function givensAlgorithm{T<:AbstractFloat}(f::Complex{T}, g::Complex{T})
twopar, onepar, zeropar = 2one(T), one(T), zero(T)
czero = zero(Complex{T})
abs1(ff) = max(abs(real(ff)), abs(imag(ff)))
safmin = realmin(T)
epspar = eps(T)
safmn2 = realmin2(T)
safmx2 = onepar/safmn2
scalepar = max(abs1(f), abs1(g))
fs = f
gs = g
count = 0
if scalepar >= safmx2
while true
count += 1
fs *= safmn2
gs *= safmn2
scalepar *= safmn2
if scalepar < safmx2 break end
end
elseif scalepar <= safmn2
if g == 0
cs = onepar
sn = czero
r = f
return cs, sn, r
end
while true
count -= 1
fs *= safmx2
gs *= safmx2
scalepar *= safmx2
if scalepar > safmn2 break end
end
end
f2 = abs2(fs)
g2 = abs2(gs)
if f2 <= max(g2, onepar)*safmin
# This is a rare case: F is very small.
if f == 0
cs = zero(T)
r = hypot(real(g), imag(g))
# do complex/real division explicitly with two real divisions
d = hypot(real(gs), imag(gs))
sn = complex(real(gs)/d, -imag(gs)/d)
return cs, sn, r
end
f2s = hypot(real(fs), imag(fs))
# g2 and g2s are accurate
# g2 is at least safmin, and g2s is at least safmn2
g2s = sqrt(g2)
# error in cs from underflow in f2s is at most
# unfl / safmn2 .lt. sqrt(unfl*eps) .lt. eps
# if max(g2,one)=g2, then f2 .lt. g2*safmin,
# and so cs .lt. sqrt(safmin)
# if max(g2,one)=one, then f2 .lt. safmin
# and so cs .lt. sqrt(safmin)/safmn2 = sqrt(eps)
# therefore, cs = f2s/g2s / sqrt( 1 + (f2s/g2s)**2 ) = f2s/g2s
cs = f2s/g2s
# make sure abs(ff) = 1
# do complex/real division explicitly with 2 real divisions
if abs1(f) > 1
d = hypot(real(f), imag(f))
ff = complex(real(f)/d, imag(f)/d)
else
dr = safmx2*real(f)
di = safmx2*imag(f)
d = hypot(dr, di)
ff = complex(dr/d, di/d)
end
sn = ff*complex(real(gs)/g2s, -imag(gs)/g2s)
r = cs*f + sn*g
else
# This is the most common case.
# Neither F2 nor F2/G2 are less than SAFMIN
# F2S cannot overflow, and it is accurate
f2s = sqrt(onepar + g2/f2)
# do the f2s(real)*fs(complex) multiply with two real multiplies
r = complex(f2s*real(fs), f2s*imag(fs))
cs = onepar/f2s
d = f2 + g2
# do complex/real division explicitly with two real divisions
sn = complex(real(r)/d, imag(r)/d)
sn *= conj(gs)
if count != 0
if count > 0
for i = 1:count
r *= safmx2
end
else
for i = 1:-count
r *= safmn2
end
end
end
end
return cs, sn, r
end
function givens{T}(f::T, g::T, i1::Integer, i2::Integer)
if i1 >= i2
throw(ArgumentError("second index must be larger than the first"))
end
c, s, r = givensAlgorithm(f, g)
Givens(i1, i2, convert(T, c), convert(T, s)), r
end
function givens{T}(A::AbstractMatrix{T}, i1::Integer, i2::Integer, col::Integer)
if i1 >= i2
throw(ArgumentError("second index must be larger than the first"))
end
c, s, r = givensAlgorithm(A[i1,col], A[i2,col])
Givens(i1, i2, convert(T, c), convert(T, s)), r
end
getindex(G::Givens, i::Integer, j::Integer) = i == j ? (i == G.i1 || i == G.i2 ? G.c : one(G.c)) : (i == G.i1 && j == G.i2 ? G.s : (i == G.i2 && j == G.i1 ? -G.s : zero(G.s)))
A_mul_B!(G1::Givens, G2::Givens) = error("Operation not supported. Consider *")
function A_mul_B!(G::Givens, A::AbstractVecOrMat)
m, n = size(A, 1), size(A, 2)
if G.i2 > m
throw(DimensionMismatch("column indices for rotation are outside the matrix"))
end
@inbounds @simd for i = 1:n
tmp = G.c*A[G.i1,i] + G.s*A[G.i2,i]
A[G.i2,i] = G.c*A[G.i2,i] - conj(G.s)*A[G.i1,i]
A[G.i1,i] = tmp
end
return A
end
function A_mul_Bc!(A::AbstractVecOrMat, G::Givens)
m, n = size(A, 1), size(A, 2)
if G.i2 > n
throw(DimensionMismatch("column indices for rotation are outside the matrix"))
end
@inbounds @simd for i = 1:m
tmp = G.c*A[i,G.i1] + conj(G.s)*A[i,G.i2]
A[i,G.i2] = G.c*A[i,G.i2] - G.s*A[i,G.i1]
A[i,G.i1] = tmp
end
return A
end
function A_mul_B!(G::Givens, R::Rotation)
push!(R.rotations, G)
return R
end
function A_mul_B!(R::Rotation, A::AbstractMatrix)
@inbounds for i = 1:length(R.rotations)
A_mul_B!(R.rotations[i], A)
end
return A
end
function A_mul_Bc!(A::AbstractMatrix, R::Rotation)
@inbounds for i = 1:length(R.rotations)
A_mul_Bc!(A, R.rotations[i])
end
return A
end
*{T}(G1::Givens{T}, G2::Givens{T}) = Rotation(push!(push!(Givens{T}[], G2), G1))