private static unsafe void UnsafeSwap(Double *array, Int32 i, Int32 j)
{
var temp = array[i];
array[i] = array[j];
array[j] = temp;
}
public static unsafe void FastInverse2d(Double[,] input, out Double[,] output, Wavelet wavelet, Int32 Level)
{
Int32 DataLen = input.GetLength(0);
Int32 Len = wavelet.DecompositionHigh.Length;
Int32 Bound;
output = new Double[DataLen, DataLen];
Double[] buffData = new Double[DataLen];
Double[] buffLow = new Double[DataLen];
Double[] buffHigh = new Double[DataLen];
Double *RecLow = stackalloc Double[Len];
Double *RecHigh = stackalloc Double[Len];
for (int i = 0; i < Len; i++)
{
RecLow[i] = wavelet.ReconstructionLow[i];
RecHigh[i] = wavelet.ReconstructionHigh[i];
}
for (int i = 0; i < DataLen; i++)
{
for (int j = 0; j < DataLen; j++)
{
output[i, j] = input[i, j];
}
}
for (int lev = Level; lev > 0; lev--)
{
Bound = DataLen >> lev;
for (int j = 0; j < Bound << 1; j++)
{
for (int i = 0; i < Bound << 1; i++)
{
buffData[i] = output[i, j];
}
FastStepInverse(ref buffData, buffLow, buffHigh, Len, lev, RecLow, RecHigh);
for (int i = 0; i < Bound << 1; i++)
{
output[i, j] = buffData[i];
}
}
for (int i = 0; i < Bound << 1; i++)
{
for (int j = 0; j < Bound << 1; j++)
{
buffData[j] = output[i, j];
}
FastStepInverse(ref buffData, buffLow, buffHigh, Len, lev, RecLow, RecHigh);
for (int j = 0; j < Bound << 1; j++)
{
output[i, j] = buffData[j];
}
}
}
}
private unsafe UInt64 _FIXED(Double d)
{
Double *ptr = (Double *)m_BufferPtr;
* ptr = d;
UInt64 value = *(UInt64 *)ptr;
return(value);
}
public static unsafe void FastInverse1d(Double[] input, out Double[] output, Wavelet wavelet, Int32 startLevel)
{
Int32 Len = wavelet.ReconstructionLow.Length;
Int32 CircleInd;
output = new Double[input.Length];
Double[] Buff = new Double[input.Length];
Double[] BufferLow = new Double[input.Length];
Double[] BufferHigh = new Double[input.Length];
Buffer.BlockCopy(input, 0, output, 0, input.Length * 8);
Double Buf = 0;
Double *RecLow = stackalloc Double[Len];
Double *RecHigh = stackalloc Double[Len];
for (int i = 0; i < Len; i++)
{
RecLow[i] = wavelet.ReconstructionLow[i];
RecHigh[i] = wavelet.ReconstructionHigh[i];
}
fixed(Double *pbuf = Buff, pLow = BufferLow, pHigh = BufferHigh)
{
for (int level = startLevel; level > 0; level--)
{
Int32 Bound = input.Length >> level;
Int32 StartIndex = -((Len >> 1) - 1);
for (int i = 0, j = 0; i < Bound << 1; i += 2, j++)
{
pLow[i] = 0;
pHigh[i] = 0;
pLow[i + 1] = output[j];
pHigh[i + 1] = output[Bound + j];
}
for (int i = 0; i < Bound << 1; i++)
{
for (int j = StartIndex, k = 0; k < Len; j++, k++)
{
if ((StartIndex < 0) || j >= (Bound << 1))
{
CircleInd = (j % (Bound << 1) + (Bound << 1)) % (Bound << 1);
}
else
{
CircleInd = j;
}
Buf += RecLow[k] * pLow[CircleInd] + RecHigh[k] * pHigh[CircleInd];
}
StartIndex += 1;
pbuf[i] = Buf;
Buf = 0;
}
Buffer.BlockCopy(Buff, 0, output, 0, Bound * 16);
}
}
}
public static unsafe void FastForward2d(Double[,] input, out Double[,] output, Wavelet wavelet, Int32 Level)
{
Int32 DataLen = input.GetLength(0);
Int32 Len = wavelet.DecompositionHigh.Length;
Int32 Bound;
output = new Double[DataLen, DataLen];
Double[] buff = new Double[DataLen];
Double[] buffData = new Double[DataLen];
Double *DecLow = stackalloc Double[Len];
Double *DecHigh = stackalloc Double[Len];
for (int i = 0; i < Len; i++)
{
DecLow[i] = wavelet.DecompositionLow[i];
DecHigh[i] = wavelet.DecompositionHigh[i];
}
for (int i = 0; i < DataLen; i++)
{
for (int j = 0; j < DataLen; j++)
{
output[i, j] = input[i, j];
}
}
for (int lev = 0; lev < Level; lev++)
{
Bound = DataLen >> lev;
for (int i = 0; i < Bound; i++)
{
for (int j = 0; j < Bound; j++)
{
buffData[j] = output[i, j];
}
FastStepForward(ref buffData, buff, Len, lev, DecLow, DecHigh);
for (int j = 0; j < Bound; j++)
{
output[i, j] = buffData[j];
}
}
for (int j = 0; j < Bound; j++)
{
for (int i = 0; i < Bound; i++)
{
buffData[i] = output[i, j];
}
FastStepForward(ref buffData, buff, Len, lev, DecLow, DecHigh);
for (int i = 0; i < Bound; i++)
{
output[i, j] = buffData[i];
}
}
}
}
public static unsafe DwtOutput Forward(double[] input, Wavelet wavelet, Int32 level)
{
Int32 Len = wavelet.DecompositionLow.Length;
Int32 CircleInd;
double[] output = new Double[input.Length];
Double[] Buff = new Double[input.Length];
Buffer.BlockCopy(input, 0, output, 0, input.Length * 8);
Double BufScal = 0;
Double BufDet = 0;
Double *DecLow = stackalloc Double[Len];
Double *DecHigh = stackalloc Double[Len];
for (int i = 0; i < Len; i++)
{
DecLow[i] = wavelet.DecompositionLow[i];
DecHigh[i] = wavelet.DecompositionHigh[i];
}
fixed(Double *pout = output, pbuf = Buff)
{
for (int lvl = 0; lvl < level; lvl++)
{
Int32 Bound = input.Length >> lvl;
Int32 StartIndex = -((Len >> 1) - 1);
Buffer.BlockCopy(output, 0, Buff, 0, Bound * 8);
for (int i = 0; i < Bound >> 1; i++)
{
for (int j = StartIndex, k = 0; k < Len; j++, k++)
{
if ((StartIndex < 0) || j >= Bound)
{
CircleInd = ((j % Bound) + Bound) % Bound;
}
else
{
CircleInd = j;
}
BufScal += DecLow[k] * pout[CircleInd];
BufDet += DecHigh[k] * pout[CircleInd];
}
StartIndex += 2;
pbuf[i] = BufScal;
pbuf[i + (Bound >> 1)] = BufDet;
BufScal = 0;
BufDet = 0;
}
Buffer.BlockCopy(Buff, 0, output, 0, Bound * 8);
}
}
DwtOutput res = new DwtOutput(output, level, wavelet);
return(res);
}
private unsafe void LLt(Double[,] value)
{
n = value.GetLength(0);
L = new Double[n, n];
D = new Double[n];
for (int i = 0; i < D.Length; i++)
{
D[i] = 1;
}
robust = false;
Double[,] a = value;
this.positiveDefinite = true;
this.symmetric = true;
fixed(Double *ptrL = L)
{
for (int j = 0; j < n; j++)
{
Double *Lrowj = ptrL + j * n;
Double d = 0;
for (int k = 0; k < j; k++)
{
Double *Lrowk = ptrL + k * n;
Double s = 0;
for (int i = 0; i < k; i++)
{
s += Lrowk[i] * Lrowj[i];
}
Lrowj[k] = s = (a[j, k] - s) / Lrowk[k];
d += s * s;
this.symmetric = this.symmetric & (a[k, j] == a[j, k]);
}
d = a[j, j] - d;
// Use a tolerance for positive-definiteness
this.positiveDefinite &= (d > (Double)1e-14 * Math.Abs(a[j, j]));
Lrowj[j] = (Double)System.Math.Sqrt((double)System.Math.Max(d, 0));
for (int k = j + 1; k < n; k++)
{
Lrowj[k] = 0;
}
}
}
}
public static void GetBytes(Double primitive, byte[] bytes, int offset = 0)
{
unsafe
{
fixed(byte *ptr = &bytes[offset])
{
Double *primitivePtr = (Double *)ptr;
*primitivePtr = primitive;
}
}
}
public static Double ToDouble(byte[] bytes, int offset = 0)
{
unsafe
{
fixed(byte *ptr = &bytes[offset])
{
Double *primitivePtr = (Double *)ptr;
return(*primitivePtr);
}
}
}
public static void GetBytes(Double primitive, byte[] bytes, ref int offset)
{
unsafe
{
fixed(byte *ptr = &bytes[offset])
{
offset += sizeof(Double);
Double *primitivePtr = (Double *)ptr;
*primitivePtr = primitive;
}
}
}
public static Double ToDouble(byte[] bytes, ref int offset)
{
unsafe
{
fixed(byte *ptr = &bytes[offset])
{
offset += sizeof(Double);
Double *primitivePtr = (Double *)ptr;
return(*primitivePtr);
}
}
}
private static unsafe void UnsafePrint(Double *array, Int32 size, String message)
{
if (size <= 0)
{
return;
}
Console.WriteLine($"\n{message}");
for (var i = 0; i < size; ++i)
{
Console.WriteLine($"\t{array[i]}");
}
Console.WriteLine();
}
/// <summary>
/// Writes a list of System.Double integers to the current stream using the specified buffer.
/// </summary>
/// <param name="stream">The stream to write.</param>
/// <param name="array">A list of System.Double integers.</param>
/// <param name="startIndex">A position in the list where the writing starts.</param>
/// <param name="count">The number of integers to be written into the current stream.
/// <para>!!! Note this number should be no larger than the number of integers from <paramref name="startIndex" /> to the end of the array.</para></param>
/// <param name="buffer">A byte array used to temporarily store data to write.</param>
public unsafe static void WriteDoubles(this Stream stream, IList <Double> list, int startIndex, int count, byte[] buffer)
{
fixed(byte *ptr = buffer)
{
Double *iptr2 = (Double *)ptr;
for (int i = 0, j = startIndex; i < count;)
{
iptr2[i] = list[j];
++i;
++j;
}
}
stream.Write(buffer, 0, buffer.Length);
}
private static unsafe void UnsafeQuickSort(Double *array, Int32 size)
{
if (size < 2)
{
return;
}
var pivot = random.Next(size);
UnsafeSwap(array, 0, pivot);
var last = 0;
for (var index = 0; index < size; ++index)
{
if (array[index] < array[0])
{
UnsafeSwap(array, ++last, index);
}
}
UnsafeSwap(array, 0, last);
UnsafeQuickSort(array, last);
UnsafeQuickSort(array + last + 1, size - last - 1);
}
internal static extern void glMap1d(MapTarget target, Double u1, Double u2, Int32 stride, Int32 order, Double *points);
internal static extern void glVertex4dv(Double *v);
public void Read(Double *p)
{
var token = _GetNumber(true);
*p = Double.Parse(token);
}
internal static extern void glTexCoord4dv(Double *v);
internal static extern void glTexGendv(TextureCoordName coord, TextureGenParameter pname, [OutAttribute] Double * @params);
internal static extern void glRasterPos4dv(Double *v);
internal static extern void glRectdv(Double *v1, Double *v2);
internal static extern void glClipPlane(ClipPlaneName plane, Double *equation);
internal static extern void glColor3dv(Double *v);
internal static extern void glNormal3dv(Double *v);
internal static extern void glMultMatrixd(Double *m);
internal static extern void glIndexdv(Double *c);
/// <summary>
/// Constructs a new LU decomposition.
/// </summary>
/// <param name="value">The matrix A to be decomposed.</param>
/// <param name="transpose">True if the decomposition should be performed on
/// the transpose of A rather than A itself, false otherwise. Default is false.</param>
/// <param name="inPlace">True if the decomposition should be performed over the
/// <paramref name="value"/> matrix rather than on a copy of it. If true, the
/// matrix will be destroyed during the decomposition. Default is false.</param>
///
public LuDecomposition(Double[,] value, bool transpose, bool inPlace)
{
if (value == null)
{
throw new ArgumentNullException("value", "Matrix cannot be null.");
}
if (transpose)
{
this.lu = value.Transpose(inPlace);
}
else
{
this.lu = inPlace ? value : (Double[, ])value.Clone();
}
this.rows = lu.GetLength(0);
this.cols = lu.GetLength(1);
this.pivotSign = 1;
this.pivotVector = new int[rows];
for (int i = 0; i < rows; i++)
{
pivotVector[i] = i;
}
var LUcolj = new Double[rows];
unsafe
{
fixed(Double *LU = lu)
{
// Outer loop.
for (int j = 0; j < cols; j++)
{
// Make a copy of the j-th column to localize references.
for (int i = 0; i < rows; i++)
{
LUcolj[i] = lu[i, j];
}
// Apply previous transformations.
for (int i = 0; i < rows; i++)
{
Double s = 0;
// Most of the time is spent in
// the following dot product:
int kmax = Math.Min(i, j);
Double *LUrowi = &LU[i * cols];
for (int k = 0; k < kmax; k++)
{
s += LUrowi[k] * LUcolj[k];
}
LUrowi[j] = LUcolj[i] -= s;
}
// Find pivot and exchange if necessary.
int p = j;
for (int i = j + 1; i < rows; i++)
{
if (Math.Abs(LUcolj[i]) > Math.Abs(LUcolj[p]))
{
p = i;
}
}
if (p != j)
{
for (int k = 0; k < cols; k++)
{
var t = lu[p, k];
lu[p, k] = lu[j, k];
lu[j, k] = t;
}
int v = pivotVector[p];
pivotVector[p] = pivotVector[j];
pivotVector[j] = v;
pivotSign = -pivotSign;
}
// Compute multipliers.
if (j < rows && lu[j, j] != 0)
{
for (int i = j + 1; i < rows; i++)
{
lu[i, j] /= lu[j, j];
}
}
}
}
}
}
internal static extern void glLoadMatrixd(Double *m);
/// <summary>
/// Constructs a new singular value decomposition.
/// </summary>
///
/// <param name="value">
/// The matrix to be decomposed.</param>
/// <param name="computeLeftSingularVectors">
/// Pass <see langword="true"/> if the left singular vector matrix U
/// should be computed. Pass <see langword="false"/> otherwise. Default
/// is <see langword="true"/>.</param>
/// <param name="computeRightSingularVectors">
/// Pass <see langword="true"/> if the right singular vector matrix V
/// should be computed. Pass <see langword="false"/> otherwise. Default
/// is <see langword="true"/>.</param>
/// <param name="autoTranspose">
/// Pass <see langword="true"/> to automatically transpose the value matrix in
/// case JAMA's assumptions about the dimensionality of the matrix are violated.
/// Pass <see langword="false"/> otherwise. Default is <see langword="false"/>.</param>
/// <param name="inPlace">
/// Pass <see langword="true"/> to perform the decomposition in place. The matrix
/// <paramref name="value"/> will be destroyed in the process, resulting in less
/// memory comsumption.</param>
///
public unsafe SingularValueDecomposition(Double[,] value,
bool computeLeftSingularVectors, bool computeRightSingularVectors, bool autoTranspose, bool inPlace)
{
if (value == null)
{
throw new ArgumentNullException("value", "Matrix cannot be null.");
}
Double[,] a;
m = value.GetLength(0); // rows
n = value.GetLength(1); // cols
if (m == 0 || n == 0)
{
throw new ArgumentException("Matrix does not have any rows or columns.", "value");
}
if (m < n) // Check if we are violating JAMA's assumption
{
if (!autoTranspose) // Yes, check if we should correct it
{
// Warning! This routine is not guaranteed to work when A has less rows
// than columns. If this is the case, you should compute SVD on the
// transpose of A and then swap the left and right eigenvectors.
// However, as the solution found can still be useful, the exception below
// will not be thrown, and only a warning will be output in the trace.
// throw new ArgumentException("Matrix should have more rows than columns.");
System.Diagnostics.Trace.WriteLine(
"WARNING: Computing SVD on a matrix with more columns than rows.");
// Proceed anyway
a = inPlace ? value : (Double[, ])value.Clone();
}
else
{
// Transposing and swapping
a = value.Transpose(inPlace && m == n);
m = value.GetLength(1);
n = value.GetLength(0);
swapped = true;
bool aux = computeLeftSingularVectors;
computeLeftSingularVectors = computeRightSingularVectors;
computeRightSingularVectors = aux;
}
}
else
{
// Input matrix is ok
a = inPlace ? value : (Double[, ])value.Clone();
}
int nu = System.Math.Min(m, n);
int ni = System.Math.Min(m + 1, n);
s = new Double[ni];
u = new Double[m, nu];
v = new Double[n, n];
Double[] e = new Double[n];
Double[] work = new Double[m];
bool wantu = computeLeftSingularVectors;
bool wantv = computeRightSingularVectors;
fixed(Double *U = u)
fixed(Double * V = v)
fixed(Double * A = a)
{
// Will store ordered sequence of indices after sorting.
si = new int[ni]; for (int i = 0; i < ni; i++)
{
si[i] = i;
}
// Reduce A to bidiagonal form, storing the diagonal elements in s and the super-diagonal elements in e.
int nct = System.Math.Min(m - 1, n);
int nrt = System.Math.Max(0, System.Math.Min(n - 2, m));
for (int k = 0; k < System.Math.Max(nct, nrt); k++)
{
if (k < nct)
{
// Compute the transformation for the k-th column and place the k-th diagonal in s[k].
// Compute 2-norm of k-th column without under/overflow.
s[k] = 0;
for (int i = k; i < m; i++)
{
s[k] = Accord.Math.Tools.Hypotenuse(s[k], a[i, k]);
}
if (s[k] != 0)
{
if (a[k, k] < 0)
{
s[k] = -s[k];
}
for (int i = k; i < m; i++)
{
a[i, k] /= s[k];
}
a[k, k] += 1;
}
s[k] = -s[k];
}
for (int j = k + 1; j < n; j++)
{
Double *ptr_ak = A + k * n + k; // A[k,k]
Double *ptr_aj = A + k * n + j; // A[k,j]
if ((k < nct) & (s[k] != 0))
{
// Apply the transformation.
Double t = 0;
Double *ak = ptr_ak;
Double *aj = ptr_aj;
for (int i = k; i < m; i++)
{
t += (*ak) * (*aj);
ak += n; aj += n;
}
t = -t / *ptr_ak;
ak = ptr_ak;
aj = ptr_aj;
for (int i = k; i < m; i++)
{
*aj += t * (*ak);
ak += n; aj += n;
}
}
// Place the k-th row of A into e for the subsequent calculation of the row transformation.
e[j] = *ptr_aj;
}
if (wantu & (k < nct))
{
// Place the transformation in U for subsequent back
// multiplication.
for (int i = k; i < m; i++)
{
u[i, k] = a[i, k];
}
}
if (k < nrt)
{
// Compute the k-th row transformation and place the k-th super-diagonal in e[k].
// Compute 2-norm without under/overflow.
e[k] = 0;
for (int i = k + 1; i < n; i++)
{
e[k] = Accord.Math.Tools.Hypotenuse(e[k], e[i]);
}
if (e[k] != 0)
{
if (e[k + 1] < 0)
{
e[k] = -e[k];
}
for (int i = k + 1; i < n; i++)
{
e[i] /= e[k];
}
e[k + 1] += 1;
}
e[k] = -e[k];
if ((k + 1 < m) & (e[k] != 0))
{
// Apply the transformation.
for (int i = k + 1; i < m; i++)
{
work[i] = 0;
}
int k1 = k + 1;
for (int i = k1; i < m; i++)
{
Double *ai = A + (i * n) + k1;
for (int j = k1; j < n; j++, ai++)
{
work[i] += e[j] * (*ai);
}
}
for (int j = k1; j < n; j++)
{
Double t = -e[j] / e[k1];
Double *aj = A + (k1 * n) + j;
for (int i = k1; i < m; i++, aj += n)
{
*aj += t * work[i];
}
}
}
if (wantv)
{
// Place the transformation in V for subsequent back multiplication.
for (int i = k + 1; i < n; i++)
{
v[i, k] = e[i];
}
}
}
}
// Set up the final bidiagonal matrix or order p.
int p = System.Math.Min(n, m + 1);
if (nct < n)
{
s[nct] = a[nct, nct];
}
if (m < p)
{
s[p - 1] = 0;
}
if (nrt + 1 < p)
{
e[nrt] = a[nrt, p - 1];
}
e[p - 1] = 0;
// If required, generate U.
if (wantu)
{
for (int j = nct; j < nu; j++)
{
for (int i = 0; i < m; i++)
{
u[i, j] = 0;
}
u[j, j] = 1;
}
for (int k = nct - 1; k >= 0; k--)
{
if (s[k] != 0)
{
Double *ptr_uk = U + k * nu + k; // u[k,k]
Double *uk, uj;
for (int j = k + 1; j < nu; j++)
{
Double *ptr_uj = U + k * nu + j; // u[k,j]
Double t = 0;
uk = ptr_uk;
uj = ptr_uj;
for (int i = k; i < m; i++)
{
t += *uk * *uj;
uk += nu; uj += nu;
}
t = -t / *ptr_uk;
uk = ptr_uk; uj = ptr_uj;
for (int i = k; i < m; i++)
{
*uj += t * (*uk);
uk += nu; uj += nu;
}
}
uk = ptr_uk;
for (int i = k; i < m; i++)
{
*uk = -(*uk);
uk += nu;
}
u[k, k] = 1 + u[k, k];
for (int i = 0; i < k - 1; i++)
{
u[i, k] = 0;
}
}
else
{
for (int i = 0; i < m; i++)
{
u[i, k] = 0;
}
u[k, k] = 1;
}
}
}
// If required, generate V.
if (wantv)
{
for (int k = n - 1; k >= 0; k--)
{
if ((k < nrt) & (e[k] != 0))
{
// TODO: The following is a pseudo correction to make SVD
// work on matrices with n > m (less rows than columns).
// For the proper correction, compute the decomposition of the
// transpose of A and swap the left and right eigenvectors
// Original line:
// for (int j = k + 1; j < nu; j++)
// Pseudo correction:
// for (int j = k + 1; j < n; j++)
for (int j = k + 1; j < n; j++) // pseudo-correction
{
Double *ptr_vk = V + (k + 1) * n + k; // v[k + 1, k]
Double *ptr_vj = V + (k + 1) * n + j; // v[k + 1, j]
Double t = 0;
Double *vk = ptr_vk;
Double *vj = ptr_vj;
for (int i = k + 1; i < n; i++)
{
t += *vk * *vj;
vk += n; vj += n;
}
t = -t / *ptr_vk;
vk = ptr_vk; vj = ptr_vj;
for (int i = k + 1; i < n; i++)
{
*vj += t * (*vk);
vk += n; vj += n;
}
}
}
for (int i = 0; i < n; i++)
{
v[i, k] = 0;
}
v[k, k] = 1;
}
}
// Main iteration loop for the singular values.
int pp = p - 1;
int iter = 0;
while (p > 0)
{
int k, kase;
// Here is where a test for too many iterations would go.
// This section of the program inspects for
// negligible elements in the s and e arrays. On
// completion the variables kase and k are set as follows.
// kase = 1 if s(p) and e[k-1] are negligible and k<p
// kase = 2 if s(k) is negligible and k<p
// kase = 3 if e[k-1] is negligible, k<p, and
// s(k), ..., s(p) are not negligible (qr step).
// kase = 4 if e(p-1) is negligible (convergence).
for (k = p - 2; k >= -1; k--)
{
if (k == -1)
{
break;
}
if (System.Math.Abs(e[k]) <=
tiny + eps * (System.Math.Abs(s[k]) + System.Math.Abs(s[k + 1])))
{
e[k] = 0;
break;
}
}
if (k == p - 2)
{
kase = 4;
}
else
{
int ks;
for (ks = p - 1; ks >= k; ks--)
{
if (ks == k)
{
break;
}
Double t = (ks != p ? System.Math.Abs(e[ks]) : 0) +
(ks != k + 1 ? System.Math.Abs(e[ks - 1]) : 0);
if (System.Math.Abs(s[ks]) <= tiny + eps * t)
{
s[ks] = 0;
break;
}
}
if (ks == k)
{
kase = 3;
}
else if (ks == p - 1)
{
kase = 1;
}
else
{
kase = 2;
k = ks;
}
}
k++;
// Perform the task indicated by kase.
switch (kase)
{
// Deflate negligible s(p).
case 1:
{
Double f = e[p - 2];
e[p - 2] = 0;
for (int j = p - 2; j >= k; j--)
{
Double t = Accord.Math.Tools.Hypotenuse(s[j], f);
Double cs = s[j] / t;
Double sn = f / t;
s[j] = t;
if (j != k)
{
f = -sn * e[j - 1];
e[j - 1] = cs * e[j - 1];
}
if (wantv)
{
for (int i = 0; i < n; i++)
{
t = cs * v[i, j] + sn * v[i, p - 1];
v[i, p - 1] = -sn * v[i, j] + cs * v[i, p - 1];
v[i, j] = t;
}
}
}
}
break;
// Split at negligible s(k).
case 2:
{
Double f = e[k - 1];
e[k - 1] = 0;
for (int j = k; j < p; j++)
{
Double t = Accord.Math.Tools.Hypotenuse(s[j], f);
Double cs = s[j] / t;
Double sn = f / t;
s[j] = t;
f = -sn * e[j];
e[j] = cs * e[j];
if (wantu)
{
for (int i = 0; i < m; i++)
{
t = cs * u[i, j] + sn * u[i, k - 1];
u[i, k - 1] = -sn * u[i, j] + cs * u[i, k - 1];
u[i, j] = t;
}
}
}
}
break;
// Perform one qr step.
case 3:
{
// Calculate the shift.
Double scale = System.Math.Max(System.Math.Max(System.Math.Max(System.Math.Max(System.Math.Abs(s[p - 1]), System.Math.Abs(s[p - 2])), System.Math.Abs(e[p - 2])), System.Math.Abs(s[k])), System.Math.Abs(e[k]));
Double sp = s[p - 1] / scale;
Double spm1 = s[p - 2] / scale;
Double epm1 = e[p - 2] / scale;
Double sk = s[k] / scale;
Double ek = e[k] / scale;
Double b = ((spm1 + sp) * (spm1 - sp) + epm1 * epm1) / 2;
Double c = (sp * epm1) * (sp * epm1);
double shift = 0;
if ((b != 0) | (c != 0))
{
if (b < 0)
{
shift = -System.Math.Sqrt(b * b + c);
}
else
{
shift = System.Math.Sqrt(b * b + c);
}
shift = c / (b + shift);
}
Double f = (sk + sp) * (sk - sp) + (Double)shift;
Double g = sk * ek;
// Chase zeros.
for (int j = k; j < p - 1; j++)
{
Double t = Accord.Math.Tools.Hypotenuse(f, g);
Double cs = f / t;
Double sn = g / t;
if (j != k)
{
e[j - 1] = t;
}
f = cs * s[j] + sn * e[j];
e[j] = cs * e[j] - sn * s[j];
g = sn * s[j + 1];
s[j + 1] = cs * s[j + 1];
if (wantv)
{
unsafe
{
fixed(Double *ptr_vj = &v[0, j])
{
Double *vj = ptr_vj;
Double *vj1 = ptr_vj + 1;
for (int i = 0; i < n; i++)
{
/*t = cs * v[i, j] + sn * v[i, j + 1];
* v[i, j + 1] = -sn * v[i, j] + cs * v[i, j + 1];
* v[i, j] = t;*/
Double vij = *vj;
Double vij1 = *vj1;
t = cs * vij + sn * vij1;
*vj1 = -sn * vij + cs * vij1;
*vj = t;
vj += n; vj1 += n;
}
}
}
}
t = Accord.Math.Tools.Hypotenuse(f, g);
cs = f / t;
sn = g / t;
s[j] = t;
f = cs * e[j] + sn * s[j + 1];
s[j + 1] = -sn * e[j] + cs * s[j + 1];
g = sn * e[j + 1];
e[j + 1] = cs * e[j + 1];
if (wantu && (j < m - 1))
{
fixed(Double *ptr_uj = &u[0, j])
{
Double *uj = ptr_uj;
Double *uj1 = ptr_uj + 1;
for (int i = 0; i < m; i++)
{
/* t = cs * u[i, j] + sn * u[i, j + 1];
* u[i, j + 1] = -sn * u[i, j] + cs * u[i, j + 1];
* u[i, j] = t;*/
Double uij = *uj;
Double uij1 = *uj1;
t = cs * uij + sn * uij1;
*uj1 = -sn * uij + cs * uij1;
*uj = t;
uj += nu; uj1 += nu;
}
}
}
}
e[p - 2] = f;
iter = iter + 1;
}
break;
// Convergence.
case 4:
{
// Make the singular values positive.
if (s[k] <= 0)
{
s[k] = (s[k] < 0 ? -s[k] : 0);
if (wantv)
{
for (int i = 0; i <= pp; i++)
{
v[i, k] = -v[i, k];
}
}
}
// Order the singular values.
while (k < pp)
{
if (s[k] >= s[k + 1])
{
break;
}
Double t = s[k];
s[k] = s[k + 1];
s[k + 1] = t;
int ti = si[k];
si[k] = si[k + 1];
si[k + 1] = ti;
if (wantv && (k < n - 1))
{
for (int i = 0; i < n; i++)
{
t = v[i, k + 1];
v[i, k + 1] = v[i, k];
v[i, k] = t;
}
}
if (wantu && (k < m - 1))
{
for (int i = 0; i < m; i++)
{
t = u[i, k + 1];
u[i, k + 1] = u[i, k];
u[i, k] = t;
}
}
k++;
}
iter = 0;
p--;
}
break;
}
}
}
// If we are violating JAMA's assumption about
// the input dimension, we need to swap u and v.
if (swapped)
{
Double[,] temp = this.u;
this.u = this.v;
this.v = temp;
}
}
internal static extern void glMap2d(MapTarget target, Double u1, Double u2, Int32 ustride, Int32 uorder, Double v1, Double v2, Int32 vstride, Int32 vorder, Double *points);
