diff --git a/kmath-tensors/src/commonMain/kotlin/space/kscience/kmath/tensors/core/internal/linUtils.kt b/kmath-tensors/src/commonMain/kotlin/space/kscience/kmath/tensors/core/internal/linUtils.kt index aba6167ce..2c9a0cf8d 100644 --- a/kmath-tensors/src/commonMain/kotlin/space/kscience/kmath/tensors/core/internal/linUtils.kt +++ b/kmath-tensors/src/commonMain/kotlin/space/kscience/kmath/tensors/core/internal/linUtils.kt @@ -17,6 +17,7 @@ import space.kscience.kmath.tensors.core.DoubleTensor import space.kscience.kmath.tensors.core.DoubleTensorAlgebra import space.kscience.kmath.tensors.core.IntTensor import kotlin.math.abs +import kotlin.math.max import kotlin.math.min import kotlin.math.sqrt @@ -349,3 +350,298 @@ internal fun DoubleTensorAlgebra.svdHelper( matrixV.mutableBuffer.array()[matrixV.bufferStart + i] = vBuffer[i] } } + +internal fun pythag(a: Double, b: Double): Double { + val at: Double = abs(a) + val bt: Double = abs(b) + val ct: Double + val result: Double + if (at > bt) { + ct = bt / at + result = at * sqrt(1.0 + ct * ct) + } else if (bt > 0.0) { + ct = at / bt + result = bt * sqrt(1.0 + ct * ct) + } else result = 0.0 + return result +} + +internal fun SIGN(a: Double, b: Double): Double { + if (b >= 0.0) + return abs(a) + else + return -abs(a) +} +internal fun MutableStructure2D.svdGolabKahanHelper(u: MutableStructure2D, w: BufferedTensor, v: MutableStructure2D) { + val shape = this.shape + val m = shape.component1() + val n = shape.component2() + var f = 0.0 + val rv1 = DoubleArray(n) + var s = 0.0 + var scale = 0.0 + var anorm = 0.0 + var g = 0.0 + var l = 0 + val epsilon = 1e-10 + for (i in 0 until n) { + /* left-hand reduction */ + l = i + 1 + rv1[i] = scale * g + g = 0.0 + s = 0.0 + scale = 0.0 + if (i < m) { + for (k in i until m) { + scale += abs(this[k, i]); + } + if (abs(scale) > epsilon) { + for (k in i until m) { + this[k, i] = (this[k, i] / scale) + s += this[k, i] * this[k, i] + } + f = this[i, i] + if (f >= 0) { + g = (-1) * abs(sqrt(s)) + } else { + g = abs(sqrt(s)) + } + val h = f * g - s + this[i, i] = f - g + if (i != n - 1) { + for (j in l until n) { + s = 0.0 + for (k in i until m) { + s += this[k, i] * this[k, j] + } + f = s / h + for (k in i until m) { + this[k, j] += f * this[k, i] + } + } + } + for (k in i until m) { + this[k, i] = this[k, i] * scale + } + } + } + + w.mutableBuffer.array()[w.bufferStart + i] = scale * g + /* right-hand reduction */ + g = 0.0 + s = 0.0 + scale = 0.0 + if (i < m && i != n - 1) { + for (k in l until n) { + scale += abs(this[i, k]) + } + if (abs(scale) > epsilon) { + for (k in l until n) { + this[i, k] = this[i, k] / scale + s += this[i, k] * this[i, k] + } + f = this[i, l] + if (f >= 0) { + g = (-1) * abs(sqrt(s)) + } else { + g = abs(sqrt(s)) + } + val h = f * g - s + this[i, l] = f - g + for (k in l until n) { + rv1[k] = this[i, k] / h + } + if (i != m - 1) { + for (j in l until m) { + s = 0.0 + for (k in l until n) { + s += this[j, k] * this[i, k] + } + for (k in l until n) { + this[j, k] += s * rv1[k] + } + } + } + for (k in l until n) { + this[i, k] = this[i, k] * scale + } + } + } + anorm = max(anorm, (abs(w.mutableBuffer.array()[w.bufferStart + i]) + abs(rv1[i]))); + } + + for (i in n - 1 downTo 0) { + if (i < n - 1) { + if (abs(g) > epsilon) { + for (j in l until n) { + v[j, i] = (this[i, j] / this[i, l]) / g + } + for (j in l until n) { + s = 0.0 + for (k in l until n) + s += this[i, k] * v[k, j] + for (k in l until n) + v[k, j] += s * v[k, i] + } + } + for (j in l until n) { + v[i, j] = 0.0 + v[j, i] = 0.0 + } + } + v[i, i] = 1.0 + g = rv1[i] + l = i + } + + for (i in min(n, m) - 1 downTo 0) { + l = i + 1 + g = w.mutableBuffer.array()[w.bufferStart + i] + for (j in l until n) { + this[i, j] = 0.0 + } + if (abs(g) > epsilon) { + g = 1.0 / g + for (j in l until n) { + s = 0.0 + for (k in l until m) { + s += this[k, i] * this[k, j] + } + f = (s / this[i, i]) * g + for (k in i until m) { + this[k, j] += f * this[k, i] + } + } + for (j in i until m) { + this[j, i] *= g + } + } else { + for (j in i until m) { + this[j, i] = 0.0 + } + } + this[i, i] += 1.0 + } + + var flag = 0 + var nm = 0 + var c = 0.0 + var h = 0.0 + var y = 0.0 + var z = 0.0 + var x = 0.0 + for (k in n - 1 downTo 0) { + for (its in 1 until 30) { + flag = 1 + for (newl in k downTo 0) { + nm = newl - 1 + if (abs(rv1[newl]) + anorm == anorm) { + flag = 0 + l = newl + break + } + if (abs(w.mutableBuffer.array()[w.bufferStart + nm]) + anorm == anorm) { + l = newl + break + } + } + + if (flag != 0) { + c = 0.0 + s = 1.0 + for (i in l until k) { + f = s * rv1[i] + rv1[i] = c * rv1[i] + if (abs(f) + anorm == anorm) { + break + } + h = pythag(f, g) + w.mutableBuffer.array()[w.bufferStart + i] = h + h = 1.0 / h + c = g * h + s = (-f) * h + for (j in 0 until m) { + y = this[j, nm] + z = this[j, i] + this[j, nm] = y * c + z * s + this[j, i] = z * c - y * s + } + } + } + + z = w.mutableBuffer.array()[w.bufferStart + k] + if (l == k) { + if (z < 0.0) { + w.mutableBuffer.array()[w.bufferStart + k] = -z + for (j in 0 until n) + v[j, k] = -v[j, k] + } + break + } + +// надо придумать, что сделать - выкинуть ошибку? +// if (its == 30) { +// return +// } + + x = w.mutableBuffer.array()[w.bufferStart + l] + nm = k - 1 + y = w.mutableBuffer.array()[w.bufferStart + nm] + g = rv1[nm] + h = rv1[k] + f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0 * h * y) + g = pythag(f, 1.0) + f = ((x - z) * (x + z) + h * ((y / (f + SIGN(g, f))) - h)) / x + c = 1.0 + s = 1.0 + + var i = 0 + for (j in l until nm + 1) { + i = j + 1 + g = rv1[i] + y = w.mutableBuffer.array()[w.bufferStart + i] + h = s * g + g = c * g + z = pythag(f, h) + rv1[j] = z + c = f / z + s = h / z + f = x * c + g * s + g = g * c - x * s + h = y * s + y *= c + + for (jj in 0 until n) { + x = v[jj, j]; + z = v[jj, i]; + v[jj, j] = x * c + z * s; + v[jj, i] = z * c - x * s; + } + z = pythag(f, h) + w.mutableBuffer.array()[w.bufferStart + j] = z + if (abs(z) > epsilon) { + z = 1.0 / z + c = f * z + s = h * z + } + f = c * g + s * y + x = c * y - s * g + for (jj in 0 until m) { + y = this[jj, j] + z = this[jj, i] + this[jj, j] = y * c + z * s + this[jj, i] = z * c - y * s + } + } + rv1[l] = 0.0 + rv1[k] = f + w.mutableBuffer.array()[w.bufferStart + k] = x + } + } + + for (i in 0 until m) { + for (j in 0 until n) { + u[j, i] = this[i, j] + } + } +}