pspline 5.0.5 → 5.1.0

data/ext/pspline/include/basis/marray_ext.h

@@ -0,0 +1,100 @@
+#ifndef _MARRAY_H_
+#define _MARRAY_H_
+/*******************************************************************************
+	marray utility.
+*******************************************************************************/
+template <class T>
+T ** marray_alloc(char *mm, size_t nr, size_t sr, size_t sc)
+{
+	T **m = (T **)mm;
+	 m[0] = (T *)(mm = mm + sr);
+	for (size_t i = 1; i < nr; ++i) m[i] = (T *)(mm = mm + sc);
+	return m;
+}
+
+template <typename T>
+T ** create_marray(size_t nr, size_t nc, size_t a = 0)
+{
+	size_t sr = nr * sizeof(T *);
+	size_t sc = nc * sizeof(T) * (a + 1);
+	char * mm = (char *)malloc(sr + nr * sc);
+	if (mm == NULL) throw "allocate error, create_marray";
+	return marray_alloc<T>(mm, nr, sr, sc);
+}
+
+template <typename T>
+T ** marray_view_alloc(T **m, int nr, int nc, T *v, int a = 0)
+{
+	m[0] = v;
+	for (int i = 1; i < nr; i++) m[i] = m[i-1] + nc * (a + 1);
+	m[nr] = v;
+	return m;
+}
+
+template <typename T>
+T ** create_marray_view(T *v, int nr, int nc, int a = 0)
+{
+	T **m = (T**)malloc((nr+1) * sizeof(T*));
+	return marray_view_alloc(m, nr, nc, v, a);
+}
+
+template <typename T>
+T ** carray_alloc(char *mm, size_t sr, size_t n, size_t *s, size_t a = 0)
+{
+	T **m = (T**)mm;
+	 m[0] = (T*)(mm = mm + sr);
+	for (size_t i = 1; i < n; ++i) m[i] = (T*)(mm = mm + s[i-1] * (a + 1) * sizeof(T));
+	return m;
+}
+
+template <typename T>
+T ** create_carray(size_t n, size_t *s, size_t a = 0)
+{
+	size_t c = 0;
+	for (size_t i = 0; i < n; ++i) c += s[i];
+	size_t sr = n * sizeof(T*);
+	size_t sc = c * sizeof(T) * (a + 1);
+	char * mm = (char *)malloc(sr + sc);
+	if (mm == NULL) throw "allocate error, create_carray";
+	return carray_alloc<T>(mm, sr, n, s, a);
+}
+
+template <typename T>
+T ** carray_view_alloc(T **m, size_t n, size_t *s, T *d, size_t a = 0)
+{
+	m[0] = d;
+	for (size_t i = 1; i < n; ++i) m[i] = m[i-1] + s[i-1] * (a+1);
+	m[n] = d;
+	return m;
+}
+
+template <typename T>
+T ** create_carray_view(T *d, size_t n, size_t *s, size_t a = 0)
+{
+	T **m = (T**)malloc((n+1) * sizeof(T*));
+	m[n] = NULL;
+	return d == NULL ? m : carray_view_alloc<T>(m, n, s, d, a);
+}
+
+/*
+	Ｔ行列	[[T00,...],...] : T**
+*/
+#define T_MALLOC(T,i,j) create_marray<T>((i),(j))
+#define T_MALLOC_VIEW(T,v,i,j) create_marray_view<T>((v),(i),(j))
+/*
+	実数行列	[[X00,...,X0(c-1)],...,[X(r-1)0,...,X(r-1)(c-1)]] : marray
+*/
+#define MALLOC(r,c) create_marray<double>((r),(c))
+#define MALLOC_VIEW(v,r,c) create_marray_view<double>((v),(r),(c))
+/*
+	ＴC行列	[[T00,...],...] : T**
+*/
+#define T_CALLOC(T,i,j) create_carray<T>((i),(j))
+#define T_CALLOC_VIEW(T,v,i,j) create_carray<T>((v),(i),(j))
+/*
+	C行列	[[X00,...,X0(c-1)],...,[X(r-1)0,...,X(r-1)(c-1)]] : marray
+*/
+#define CALLOC(r,c) create_carray<double>((r),(c))
+#define CALLOC_VIEW(v,r,c) create_carray_view<double>((v),(r),(c))
+
+#endif

data/ext/pspline/include/basis/matrix_luc_ext.h

@@ -0,0 +1,300 @@
+#ifndef _MATRIX_LU_H_
+#define _MATRIX_LU_H_
+#define _MATRIX_LU_C_
+#include "basis/gabs.h"
+#include "basis/marray_ext.h"
+/*******************************************************************************
+  Crout : LU分解 クラウト法
+
+	U0j = a0j;						j =  0,...,n-1
+	Uij = aij - ΣLik*Ukj;			i <= j, k = 0,...,i-1
+	Lij =(aij - ΣLik*Ukj)/Ujj; 	i >  j, k = 0,...,j-1
+*******************************************************************************/
+template<class T> void luc_decomp(T * a, size_t n, size_t * p, int & s)
+{
+	size_t i, j, k, L;
+	T *ai, *ak;
+
+	s = 1;
+	for (ak = a, k = 0; k < n-1; ++k, ak += n) {
+		T akk = ak[k]; L = k;
+		// ピボット選択
+		for (j = k+1; j < n; ++j)
+			if (gabs(akk) < gabs(ak[j])) { L = j; akk = ak[j]; }
+		// ピボット列交換
+		if (L != k) {
+			for (ai = a, i = 0; i < n; ++i, ai += n) {
+			T w = ai[k]; ai[k] = ai[L]; ai[L] = w;  	// a[*,k] <=> a[*,L]
+			}  s *= -1;
+		}	 p[k] = L;
+		// 前進消去
+		for (ai = ak + n, i = k+1; i < n; ++i, ai += n) {
+			T aik = ai[k] / akk;
+			for (j = k+1; j < n; ++j) ai[j] -= aik * ak[j];
+			ai[k] = aik;
+		}
+	}
+}
+
+template<class T, class S> void luc_subst(T * a, size_t n, size_t * p, S * b)
+{
+	size_t i, j, js = n, k; S sum;
+	T *ai;
+	// 前進代入
+	for (ai = a, i = 0; i < n; ++i, ai += n) {
+		sum = b[i];
+		if (js < n)
+			for (j = js; j < i; ++j) sum -= ai[j] * b[j];
+		else if (sum != 0) js = i;
+		b[i] = sum;
+	}
+	// 後退代入
+	for (k = n; k > 0; --k) {
+		i = k - 1; ai -= n;
+		sum = b[i];
+		for (j = n-1; j > i; --j) sum -= ai[j] * b[j];
+		b[i] = sum / ai[i];
+	}
+	// 解の保存
+	for (k = n-1; k > 0; --k) {
+		i = k - 1; j = p[i];
+		if (i != j) { sum = b[j]; b[j] = b[i]; b[i] = sum; }
+	}
+}
+
+template<class T, class S> void luc_subst(T * a, size_t n, size_t * p, S * x, int K)
+{
+	S sum, *su, *sv, **b = T_MALLOC_VIEW(S, x, n, K);
+	size_t i, j, k, l, js = n;
+	T* ai;
+	// 前進代入
+	for (ai = a, i = 0; i < n; ++i, ai += n) {
+		su = b[i];
+		for (l = 0; l < size_t(K); ++l) {
+			sum = su[l];
+			if (js < n)
+				for (j = js; j < i; ++j) sum -= ai[j] * b[j][l];
+			else if (sum != 0) js = i;
+			su[l] = sum;
+		}
+	}
+	// 後退代入
+	for (k = n; k > 0; --k) {
+		i = k - 1; ai -= n;
+		su = b[i];
+		for (l = 0; l < size_t(K); ++l) {
+			sum = su[l];
+			for (j = n-1; j > i; --j) sum -= ai[j] * b[j][l];
+			su[l] = sum / ai[i];
+		}
+	}
+	// 解の保存
+	for (k = n-1; k > 0; --k) {
+		i = k - 1; j = p[i];
+		su = b[i]; sv = b[j];
+		if (i != j) for (l = 0; l < size_t(K); ++l) {
+			sum = sv[l]; sv[l] = su[l]; su[l] = sum;
+		}
+	}
+	free((void*)b);
+}
+
+template <class T, class P> void luc_decomp(P& A, size_t * p, int& s)
+//	T = double, P = marray<double> || marray_view<double>
+{
+	size_t N = A.rows(), a = A.atom();
+	size_t i, j, k, l, L;
+	real<T> big, tmp; varray<T> v(N, a);
+
+	s = 1;
+	for (i = 0; i < N; ++i) {
+		big = 0.0;
+		for (j = 0; j < N; ++j) {
+			real<T> aij(a, A(i,j));
+			if ((tmp = gabs(aij)) > big) big = tmp;
+		}
+		if (big == 0.0) throw "Singular matrix in routine luc_decomp";
+		tmp = 1.0 / big;
+		qd_ass(a, v(i), (double*)tmp);
+	}
+	for (j = 0; j < N; j++) {
+		L = j; big = 0.0;
+		for (i = 1; i < N; i++) {
+			l = j < i ? j : i;
+			T *aij = A(i,j);
+			for (k = 0; k < l; k++) {
+				T u[a+1], *aik = A(i,k), *akj = A(k,j);
+			//	aij -= A[i][k] * A[k][j];
+				qd_mul(a, u, aik, akj);
+				qd_sub(a, aij, aij, u);
+			}
+		// ピボット選択
+			if (i >= j) {
+				real<T> Aij(a, aij), vi(v[i]);
+				if ((tmp = gabs(Aij) * vi) > big) { big = tmp; L = i; }
+			}
+		}	p[j] = L;
+		// ピボット行交換
+		if (L != j) {
+			A.row_swap(j, L);	// A[j,*] <=> A[L,*]
+			qd_ass(a, v(L), v(j));
+			  s *= -1;
+		}
+		real<T> ajj(a, A(j,j));
+		if (ajj == 0.0) throw "Divide by zero in luc_decomp";
+		if (j < N-1) for (i = j+1; i < N; i++) {
+			T *u = ajj, *aij = A(i,j);
+		//	A[i][j] /= ajj;
+			qd_div(a, aij, aij, u);
+		}
+	}
+}
+
+template <class T, class P> void luc_subst(const P& A, size_t * p, varray<T>& B)
+//	T = double, P = marray<double> || marray_view<double>
+{
+	size_t N = A.rows(), a = A.atom();
+	size_t i, j, k, js = N; T u[a+1];
+	// 前進代入
+	for (i = 0; i < N; ++i) {
+		k = p[i];
+		real<T> sum(a, B(k));
+		if (i != k) qd_ass(a, B(k), B(i));
+		T *s = sum;
+		if (js < N) 
+			for (j = js; j < i; ++j) {
+			//	sum -= A[i][j] * B[j];
+				qd_mul(a, u, A(i,j), B(j));
+				qd_sub(a, s, s, u);
+			}
+		else if (sum != 0) js = i;
+		qd_ass(a, B(i), s);
+	}
+	// 後退代入
+	for (k = N; k > 0; --k) {
+		i = k - 1;
+		for (j = N-1; j > i; --j) {
+		//	B[i] -= A[i][j] * B[j];
+			qd_mul(a, u, A(i,j), B(j));
+			qd_sub(a, B(i), B(i), u);
+		}
+		//	B[i] /= A[i][i];
+		qd_div(a, B(i), B(i), A(i,i));
+	}
+}
+
+template <class T, class P> void luc_subst(const P& a, size_t * p, marray_view<T>& x)
+//	T = double, P = marray<double> || marray_view<double>
+{
+	size_t K = x.cols();
+	for (size_t i = 0; i < K; ++i) { varray<T> v = x.col(i); luc_subst<T>(a, p, v); }
+}
+
+template <class T, class P, class S> real<T> luc_solve(P& A, S& B)
+//	T = double, P = marray<double> || marray_view<double>, S = varray_view<double> || marray_view<double>
+{
+	size_t N = A.rows(), a = A.atom();
+	size_t *p = new size_t[N];
+	int s; luc_decomp<T>(A, p, s); luc_subst<T>(A, p, B);
+	real<T> det(a); det = s; T *u = det;
+	for (size_t k = 0; k < N; ++k) qd_mul(a, u, u, A(k,k)); // det *= A[k][k];
+	delete[] p;
+	return det;
+}
+
+#define lu_decomp luc_decomp
+#define lu_subst  luc_subst
+#define lu_solve  luc_solve
+
+template <typename T, typename P> void invert(P& A)
+//	T = double, P = marray<double> || marray_view<double>;
+{
+	size_t N = A.rows(), a = A.atom();
+	size_t i, j, k, *p = new size_t[N];
+	varray<T> B(N, a);
+
+	for (k = 0; k < N; ++k)
+	{
+		real<T> Akk(a, A(k,k)); j = k;
+		for (i = k+1; i < N; ++i) {
+			real<T> Aik(a, A(i,k));
+			if (gabs(Aik) > gabs(Akk)) { j = i; Akk = real<T>(a, A(i,k)); }
+		}	if (j != k) A.row_swap(k,j);
+			p[k] = j;
+		for (i = 0; i < N; ++i) {
+			// B[i] = A[i][k];
+			T *s = B(i), *t = A(i,k); for (j = 0; j <= a; ++j) s[j] = t[j];
+			// A[i][k] = (i == k) ? 1.0 : 0.0;
+			s = A(i,k); for (j = 0; j <= a; ++j) s[j] = (j == 0) && (i == k) ? 1.0 : 0.0;
+			// A[k][i] /= Akk;
+			t = A(k,i); qd_div(a, t, t, s = Akk);
+		}
+		for (i = 0; i < N; ++i)
+			if (i != k)
+				for (j = 0; j < N; ++j) {
+					T u[a + 1], *v = A(i,j), *s = B(i), *t = A(k,j);
+					// A[i][j] -= B[i] * A[k][j];
+					qd_mul(a, u, s, t);
+					qd_sub(a, v, v, u);
+				}
+	}
+	for (k = N; k > 0; --k)
+	{
+		j = k - 1;
+		if (p[j] != j) for (i = 0; i < N; ++i) {
+			T *u = A(i,j), *v = A(i,p[j]);
+			for (size_t l = 0; l <= a; ++l) {
+				T w = u[l]; u[l] = v[l]; v[l] = w;
+			}
+		}
+	}
+	delete[] p;
+}
+
+template <typename T>
+void invert(T *A, size_t N)
+{
+	size_t i, j, k, *P = new size_t[N]; T *B = new T[N];
+
+	T *Ak = A;
+	for (k = 0; k < N; ++k, Ak += N)
+	{
+		T Akk = Ak[k]; j = k;
+		T *Ai = Ak, *Aj = Ak;
+		for (i = k+1; i < N; ++i) {
+			Ai += N;
+			if (gabs(Ai[k]) > gabs(Akk)) {
+				j = i; Akk = Ai[k]; Aj = Ai;
+			}
+		}	if (j != k) for (i = 0; i < N; ++i) {
+				T W = Ak[i]; Ak[i] = Aj[i]; Aj[i] = W;
+			}
+			P[k] = j;
+		Ai = A;
+		for (i = 0; i < N; ++i, Ai += N) {
+			B[i] = Ai[k]; Ai[k] = (i == k) ? 1 : 0;
+			Ak[i] /= Akk;
+		}
+		for (j = N; j > 0; --j) {
+			i = j - 1;
+			Ai -= N;
+			if (i != k)
+				for (size_t l = 0; l < N; ++l)
+					Ai[l] -= B[i] * Ak[l];
+		}
+	}
+	delete[] B;
+	for (k = N; k > 0; --k)
+	{
+		j = k - 1;
+		if (P[j] != j) {
+			T *Ai = A;
+			for (i = 0; i < N; ++i, Ai += N) {
+				T W = Ai[j]; Ai[j] = Ai[P[j]]; Ai[P[j]] = W;
+			}
+		}
+	}
+	delete[] P;
+}
+
+#endif

data/ext/pspline/include/basis/matrix_lud_ext.h

@@ -0,0 +1,298 @@
+#ifndef _MATRIX_LU_H_
+#define _MATRIX_LU_H_
+#define _MATRIX_LU_D_
+#include "basis/gabs.h"
+#include "basis/marray_ext.h"
+/*******************************************************************************
+	Matrix operation
+
+  Doolittle : LU分解 ドゥーリトル法
+
+	Li0 = ai0;						i =  0,...,n-1
+	Lij = aij - ΣLik*Ukj;			i >= j, k = 0,...,j-1
+	Uij =(aij - ΣLik*Ukj)/Lii; 	i <  j, k = 0,...,i-1
+*******************************************************************************/
+template <class T> void lud_decomp(T * a, size_t n, size_t * p, int & s)
+{
+	size_t i, j, k, l, L;
+	T *ai, *ak;
+
+	s = 1;
+	for (ai = a, i = 0; i < n; i++, ai += n) {
+		T aii = ai[i]; L = i;
+		for (j = 1; j < n; j++) {
+			T aij = ai[j];
+			l = i < j ? i : j;
+			for (ak = a, k = 0; k < l; k++, ak += n) aij -= ai[k] * ak[j];
+			ai[j] = aij;
+			// ピボット選択
+			if ((j == i) || ((j > i) && (gabs(aii) < gabs(aij)))) { L = j; aii = aij; }
+		}
+		// ピボット列交換
+		if (L != i) {
+			for (ak = a, k = 0; k < n; ++k, ak += n) {
+				T w = ak[i]; ak[i] = ak[L]; ak[L] = w;  	// a[*,i] <=> a[*,L]
+			}	s *= -1;
+		}	p[i] = L;
+		if (i < n-1) for (j = i+1; j < n; j++) ai[j] /= aii;
+	}
+}
+
+template <class T, class S> void lud_subst(T * a, size_t n, size_t * p, S * b)
+{
+	size_t i, j, k, js = n; S sum;
+	T *ai;
+	// 前進代入
+	for (ai = a, i = 0; i < n; ++i, ai += n) {
+		sum = b[i];
+		if (js < n)
+			for (j = js; j < i; ++j) sum -= ai[j] * b[j];
+		else if (sum != 0) js = i;
+		b[i] = sum / ai[i];
+	}	ai -= n;
+	// 後退代入
+	for (k = n-1; k > 0; --k) {
+		i = k - 1; ai -= n;
+		sum = b[i];
+		for (j = n-1; j > i; --j) sum -= ai[j] * b[j];
+		b[i] = sum;
+	}
+	// 解の保存
+	for (k = n-1; k > 0; --k) {
+		i = k - 1; j = p[i];
+		if (i != j) { sum = b[j]; b[j] = b[i]; b[i] = sum; }
+	}
+}
+
+template <class T, class S> void lud_subst(T * a, size_t n, size_t * p, S * x, int K)
+{
+	S sum, *su, *sv, **b = T_MALLOC_VIEW(S, x, n, K);
+	size_t i, j, k, l, js = n;
+	T *ai;
+	// 前進代入
+	for (ai = a, i = 0; i < n; ++i, ai += n) {
+		su = b[i];
+		for (l = 0; l < size_t(K); ++l) {
+			sum = su[l];
+			if (js < n)
+				for (j = js; j < i; ++j) sum -= ai[j] * b[j][l];
+			else if (sum != 0) js = i;
+			su[l] = sum / ai[i];
+		}
+	}	ai -= n;
+	// 後退代入
+	for (k = n-1; k > 0; --k) {
+		i = k - 1; ai -= n;
+		su = b[i];
+		for (l = 0; l < size_t(K); ++l) {
+			sum = su[l];
+			for (j = n-1; j > i; --j) sum -= ai[j] * b[j][l];
+			su[l] = sum;
+		}
+	}
+	// 解の保存
+	for (k = n-1; k > 0; --k) {
+		i = k - 1; j = p[i];
+		su = b[i]; sv = b[j];
+		if (i != j) for (l = 0; l < size_t(K); ++l) {
+			sum = sv[l]; sv[l] = su[l]; su[l] = sum;
+		}
+	}
+	free((void*)b);
+}
+
+template<class T, class P> void lud_decomp(P& A, size_t * p, int &s)
+//	T = double, P = marray<double> || marray_view<double>
+{
+	size_t N = A.rows(), a = A.atom();
+	size_t i, j, k, L;
+	real<T> big, tmp; varray<T> v(N, a);
+
+	s = 1;
+	for (i = 0; i < N; ++i) {
+		big = 0.0;
+		for (j = 0; j < N; ++j) {
+			real<T> aij(a, A(i,j));
+			if ((tmp = gabs(aij)) > big) big = tmp;
+		}
+		if (big == 0.0) throw "Singular matrix in routine lud_decomp";
+		tmp = 1.0 / big;
+		qd_ass(a, v(i), (double*)tmp);
+	}
+	for (k = 0; k < N-1; ++k) {
+		// 陰的ピボット選択
+		L = k; big = 0.0;
+		for (i = k+1; i < N; ++i) {
+			real<T> aik(a, A(i,k)), vi(v[i]);
+			if ((tmp = gabs(aik * vi)) > big ) { big = tmp; L = i; }
+		}
+		// ピボット行交換
+		if (L != k) {
+			A.row_swap(k, L); 	// A[k,*] <=> A[L,*]
+			qd_ass(a, v(L), v(k));
+			  s *= -1;
+		}	p[k] = L;
+		// 前進消去
+		real<T> akk(a, A(k,k));
+		if (akk == 0.0) throw "Divide by zero in lud_decomp";
+		for (j = k+1; j < N; ++j) {
+			T w[a+1], *u = akk;
+			//	A[k][j] = A[k][j] / akk;
+			qd_div(a, A(k,j), A(k,j), u);
+			for (i = k+1; i < N; ++i) {
+			//	A[i][j] -= A[i][k] * A[k][j];
+				qd_mul(a, w, A(i,k), A(k,j));
+				qd_sub(a, A(i,j), A(i,j), w);
+			}
+		}
+	}
+}
+
+template<class T, class P> void lud_subst(const P& A, size_t * p, varray<T>& B)
+//	T = double, P = marray<double> || marray_view<double>
+{
+	size_t N = A.rows(), a = A.atom();
+	size_t i, j, k, js = N; T w[a+1];
+	// 前進代入
+	for (i = 0; i < N; ++i) {
+		k = (i < N-1) ? p[i] : i;
+		real<T> sum(a, B(k));
+		if (i != k) qd_ass(a, B(k), B(i));
+		T *s = sum;
+		if (js < N) {
+			for (j = js; j < i; ++j) {
+			//	sum -= A[i][j] * B[j];
+				qd_mul(a, w, A(i,j), B(j));
+				qd_sub(a, s, s, w);
+			}
+		}
+		else if (sum != 0) js = i;
+	//	B[i] = sum / A[i][i];
+		qd_div(a, B(i), s, A(i,i));
+	}
+	// 後退代入
+	for (k = N - 1; k > 0; --k) {
+		i = k - 1;
+		for (j = N - 1; j > i; --j) {
+			//	B[i] -= A[i][j] * B[j];
+			qd_mul(a, w, A(i,j), B(j));
+			qd_sub(a, B(i), B(i), w);
+		}
+	}
+}
+
+template <class T, class P> void lud_subst(const P& a, size_t * p, marray_view<T>& x)
+//	T = double, P = marray<double> || marray_view<double>
+{
+	size_t K = x.cols();
+	for (size_t i = 0; i < K; ++i) { varray<T> v = x.col(i); lud_subst<T>(a, p, v); }
+}
+
+template <class T, class P, class S> real<T> lud_solve(P& A, S& B)
+//	T = double, P = marray<double> || marray_view<double>, S = varray_view<double> || marray_view<double>
+{
+	size_t N = A.rows(), a = A.atom();
+	size_t *p = new size_t[N];
+	int s; lud_decomp<T>(A, p, s); lud_subst<T>(A, p, B);
+	real<T> det(a); det = s; T *u = det;
+	for (size_t k = 0; k < N; ++k) qd_mul(a, u, u, A(k,k)); // det *= A[k][k];
+	delete[] p;
+	return det;
+}
+
+#define lu_decomp lud_decomp
+#define lu_subst  lud_subst
+#define lu_solve  lud_solve
+
+template <typename T, typename P> void invert(P& A)
+//	T = double, P = marray<double> || marray_view<double>;
+{
+	size_t N = A.rows(), a = A.atom();
+	size_t i, j, k, *p = new size_t[N];
+	varray<T> B(N, a);
+
+	for (k = 0; k < N; ++k)
+	{
+		real<T> Akk(a, A(k,k)); j = k;
+		for (i = k+1; i < N; ++i) {
+			real<T> Aik(a, A(i,k));
+			if (gabs(Aik) > gabs(Akk)) { j = i; Akk = real<T>(a, A(i,k)); }
+		}	if (j != k) A.row_swap(k,j);
+			p[k] = j;
+		for (i = 0; i < N; ++i) {
+			// B[i] = A[i][k];
+			T *s = B(i), *t = A(i,k); for (j = 0; j <= a; ++j) s[j] = t[j];
+			// A[i][k] = (i == k) ? 1.0 : 0.0;
+			s = A(i,k); for (j = 0; j <= a; ++j) s[j] = (j == 0) && (i == k) ? 1.0 : 0.0;
+			// A[k][i] /= Akk;
+			t = A(k,i); qd_div(a, t, t, s = Akk);
+		}
+		for (i = 0; i < N; ++i)
+			if (i != k)
+				for (j = 0; j < N; ++j) {
+					T u[a + 1], *v = A(i,j), *s = B(i), *t = A(k,j);
+					// A[i][j] -= B[i] * A[k][j];
+					qd_mul(a, u, s, t);
+					qd_sub(a, v, v, u);
+				}
+	}
+	for (k = N; k > 0; --k)
+	{
+		j = k - 1;
+		if (p[j] != j) for (i = 0; i < N; ++i) {
+			T *u = A(i,j), *v = A(i,p[j]);
+			for (size_t l = 0; l <= a; ++l) {
+				T w = u[l]; u[l] = v[l]; v[l] = w;
+			}
+		}
+	}
+	delete[] p;
+}
+
+template <typename T>
+void invert(T *A, size_t N)
+{
+	size_t i, j, k, *P = new size_t[N]; T *B = new T[N];
+
+	T *Ak = A;
+	for (k = 0; k < N; ++k, Ak += N)
+	{
+		T Akk = Ak[k]; j = k;
+		T *Ai = Ak, *Aj = Ak;
+		for (i = k+1; i < N; ++i) {
+			Ai += N;
+			if (gabs(Ai[k]) > gabs(Akk)) {
+				j = i; Akk = Ai[k]; Aj = Ai;
+			}
+		}	if (j != k) for (i = 0; i < N; ++i) {
+				T W = Ak[i]; Ak[i] = Aj[i]; Aj[i] = W;
+			}
+			P[k] = j;
+		Ai = A;
+		for (i = 0; i < N; ++i, Ai += N) {
+			B[i] = Ai[k]; Ai[k] = (i == k) ? 1 : 0;
+			Ak[i] /= Akk;
+		}
+		for (j = N; j > 0; --j) {
+			i = j - 1;
+			Ai -= N;
+			if (i != k)
+				for (size_t l = 0; l < N; ++l)
+					Ai[l] -= B[i] * Ak[l];
+		}
+	}
+	delete[] B;
+	for (k = N; k > 0; --k)
+	{
+		j = k - 1;
+		if (P[j] != j) {
+			T *Ai = A;
+			for (i = 0; i < N; ++i, Ai += N) {
+				T W = Ai[j]; Ai[j] = Ai[P[j]]; Ai[P[j]] = W;
+			}
+		}
+	}
+	delete[] P;
+}
+
+#endif