public Camera (Game game,Vector3 pos, Vector3 target, Vector3 up)
: base (game)
{
view = matrix.createlookat(pos, target, up);
projection = matrix.createperspectivefieldofview(
mathhelper.PiOver4,
(float)Game.Window.ClientBounds.Width / (float)Game.Window.ClientBounds.Height,
1, 100);
}
public static matrix cov(matrix A)
{
/*
* matrix R = new matrix(A.size2, A.size2);
* matrix Q = A.copy();
* matrix.qr_gs_decomp(Q,R);
* matrix invR = matrix.rtri_invert(R);
* matrix S = invR*invR.T;
*/
matrix ATA = A.T * A;
matrix Q = ATA.copy();
matrix R = new matrix(ATA.size2, ATA.size2);
matrix.qr_gs_decomp(Q, R);
matrix S = matrix.qr_gs_inverse(Q, R);
return(S);
}
public static matrix create_matrix(int rows, int columns)
{
var result = new matrix();
int r, c;
result.row = new matrix_row[rows];
for (r = 0; r < rows; r++)
{
result.row[r] = new matrix_row();
result.row[r].col = new double[columns];
for (c = 0; c < columns; c++)
{
result.row[r].col[c] = 0;
}
}
return(result);
}
public static void Main()
{
Write("==== C ====\n");
Write("Checking the implementation of the Givens rotation for QR-factorization on random matrix A:\n");
matrix a = rndMat.randomMatrix(5, 5);
vector b = rndMat.randomVector(5);
a.print("A = ");
Write("Checking by solving Ax=b for random vector b:\n");
b.print("b = ");
qrDecompositionGivens qrGivens = new qrDecompositionGivens(a);
vector x = qrGivens.solve(b);
Write("The found solution x is:\n");
x.print("x = ");
Write("Checking the solution by calculating 0=Ax-b:\n");
vector diff = a * x - b;
diff.print("A*x-b = ");
} //Main
public void update(particleSystem pS)
{
vector r = FriedChiken.getResidual();
matrix J = FriedChiken.getJacobian();
currentVolume = 0;
for (int i = 0; i < J.nCol; i++)
{
J[number, i] = 0;
}
for (int i = 0; i < (int)elemList.Count; i++)
{
elements.element e = elemList[i];
e.copyFrom(pS.particles);
e.Update();
e.Merge(pS, J, this.number);
currentVolume += e.Volume;
}
r[number] = currentVolume - refVolume;
}
public lsq_qr(double[] x, double[] y, double[] dy, Func <double, double>[] F)
{
A = new matrix(x.Length, F.Length);
for (int i = 0; i < F.Length; i++)
{
for (int j = 0; j < x.Length; j++)
{
A[i][j] = F[i](x[j]) / dy[j]; // Row/column convention interchanged
}
}
vector b = new vector(x.Length);
for (int i = 0; i < x.Length; i++)
{
b[i] = y[i] / dy[i];
}
qr AQR = new qr(A);
c = AQR.solve(b);
}
public static void Main(string[] args)
{
//Process.GetCurrentProcess().PriorityClass = ProcessPriorityClass.High;
int max = 300; // max n
int step = 10;
double scale = 7.0 / 135000; // 300^3*scale = 1400
Stopwatch timer = new Stopwatch();
WriteLine(string.Format("# {0, -6} {1, -8} {2, -8}", "n", "cyclic", "n^3"));
for (int n = 10; n < max; n += step)
{
matrix A = GenRandSymMatrix(n, n);
timer.Start();
cyclic(A);
timer.Stop();
WriteLine($"{n, -8} {timer.ElapsedMilliseconds, -8:F3} {Pow(n, 3)*scale, -8:F3}");
timer.Reset();
}
}
public static matrix makeRandSymMatrix(int n)
{
var rand = new System.Random();
matrix A = new matrix(n, n);
for (int i = 0; i < n; i++)
{
A[i, i] = rand.NextDouble();
}
for (int i = 0; i < n; i++)
{
for (int j = i + 1; j < n; j++)
{
double Aij = rand.NextDouble();
A[i, j] = Aij;
A[j, i] = Aij;
}
}
return(A);
}
using static RNG; // Secure RNG
class main { public static int Main()
{
int n_2 = RandomInt(3, 5);
vector b = new vector(n_2);
var A_2 = new matrix(n_2, n_2);
for (int i = 0; i < n_2; i = i + 1)
{
RandomDouble(0, 1); for (int j = 0; j < n_2; j = j + 1)
{
A_2[j, i] = RandomDouble(-20, 20); b[j] = RandomDouble(-20, 20);
}
}
(A_2).print("\nA = ");
(b).print("\nb = ");
(GivensSolve(Givens(A_2), b)).print("\nx = ");
(Givens(A_2) * GivensSolve(Givens(A_2), b)).print("\nAx = ");
WriteLine($"\nDet(A) = {Det(A_2)}");
return(0);
}
static void A2()
{
WriteLine("Problem A2");
var rand = new System.Random();
int n = 2 + rand.Next(20);
matrix A = make_random_matrix(n, n);
vector b = make_random_vector(n);
WriteLine("Random square matrix A:");
A.print("A = ");
WriteLine("Random vector b:");
b.print("b = ");
qr_decomp_GS decomposition = new qr_decomp_GS(A);
vector x = decomposition.solve(b);
x.print("Solution x = ");
(A * x - b).print("A*x-b = ");
}
/// <summary>
///
/// </summary>
/// <param name="N">要素の次元</param>
/// <param name="dim">１節点の自由度、3か6が普通</param>
public integralPoint(int N, matrix _nodes, int dim) : base()
{
//親要素の節点
this.nodes = _nodes;
__N = N;
this.dof = __dim * _nodes.nRow;
metric = new matrix(N, N).eye();
refMetric = new matrix(N, N).zeros();
invMetric = new matrix(N, N).eye();
refInvMetric = new matrix(N, N).eye();
stress = new matrix(N, N).eye();
stress2 = new matrix(N, N).eye();
strain = new matrix(N, N).zeros();
difMetric = new matrixVector(N, N, dof);
covariantBases = new matrix(N, __dim).zeros();
number = new matrixINT(N, N).zeros();
gravity = new vector(dim).zeros();
gravity[dim - 1] = 1.0;
energyDensity = 0;
}
public static vector qr_gs_solve(matrix Q, matrix R, vector b)
{
vector x = new vector(R.size1);
// Make system triangular
vector c = Q.transpose() * b;
for (int i = x.size - 1; i >= 0; i--)
{
double s = 0;
int k = i + 1;
while (k < x.size)
{
s += R[i, k] * x[k];
k++;
}
x[i] = 1 / R[i, i] * (c[i] - s);
}
return(x);
}
public static Tuple <vector, matrix> box(int n)
{
double s = 1.0 / (n + 1);
matrix H = new matrix(n, n);
for (int i = 0; i < n - 1; i++)
{
matrix.set(H, i, i, -2);
matrix.set(H, i, i + 1, 1);
matrix.set(H, i + 1, i, 1);
}
matrix.set(H, n - 1, n - 1, -2);
H = -1 / s / s * H;
var res = new jacobi_diagonalization(H);
vector e = res.get_eigenvalues();
matrix V = res.get_eigenvectors();
return(new Tuple <vector, matrix>(e, V));
}
public static matrix jacobian(Func <vector, vector> f, vector x, double dx = 1e-10)
{
vector fs = f(x);
vector xj = new vector(x.size); for (int i = 0; i < x.size; i++)
{
xj[i] = x[i];
}
matrix J = new matrix(fs.size, x.size);
for (int i = 0; i < x.size; i++)
{
for (int j = 0; j < x.size; j++)
{
xj[j] = xj[j] + dx;
J[j][i] = (f(xj)[i] - fs[i]) / dx;
xj = x.copy();
}
}
return(J);
}
private static void givensRot(matrix A, matrix U, int p, int q)
{
// Perform a rotation on A: G^T*A and updates U --> U*G = U'
double App = A[p, p], Aqq = A[q, q], Apq = A[p, q], Aqp = A[q, p];
double theta = Atan2(Aqp - Apq, App + Aqq);
double c = Cos(theta), s = Sin(theta);
for (int i = 0; i < A.size2; i++)
{
// A --> A' = G^T * A
double Api = A[p, i], Aqi = A[q, i];
A[p, i] = c * Api + s * Aqi;
A[q, i] = c * Aqi - s * Api;
// U --> U' = U*G
double Uip = U[i, p], Uiq = U[i, q];
U[i, p] = c * Uip + s * Uiq;
U[i, q] = c * Uiq - s * Uip;
}
}
}//end randmatrix
public static matrix randmatrix_sym(int n, int m)
{
matrix A = new matrix(n, m);
Random ra = new Random();
//diagonal
for (int n1 = 0; n1 < n; n1++)
{
A[n1, n1] = ra.NextDouble();
} // end for
for (int n2 = 1; n2 < n; n2++)
{
for (int n3 = 0; n3 < n2; n3++)
{
A[n2, n3] = ra.NextDouble();
A[n3, n2] = A[n2, n3];
}
}
return(A);
} // end ransmatrix_sys
// Helper function. Returns diaginal matrix D and resets A to its symetric form after evaluation
static public matrix diag_cyclic_complete(matrix A, matrix V)
{
vector d = new vector(A.size1);
diag_cyclic(A, V, d);
for (int i = 0; i < A.size1; i++)
{
for (int j = i + 1; j < A.size2; j++)
{
A[i, j] = A[j, i];
}
}
matrix D = new matrix(A.size1, A.size2);
for (int i = 0; i < d.size; i++)
{
D[i, i] = d[i];
}
return(D);
}
public givens(matrix A){
G = A.copy();
int n=G.size1;
int m=G.size2;
double theta;
double xp;
double xq;
for(int q=0;q<m;q++){ /* Iterating over the columns */
for(int p=q+1;p<n;p++){ /* Iterating over all rows below the diagonal */
theta=Atan2(G[p,q],G[q,q]); /* Recalculating the relevant rows (the angles are saved in the matrix instead of the 0's) */
for(int k=q;k<m;k++){
xp=Cos(theta)*G[q,k]+Sin(theta)*G[p,k];
xq=-Sin(theta)*G[q,k]+Cos(theta)*G[p,k];
G[q,k]=xp;
G[p,k]=xq;
}
G[p,q]=theta;
}
}
}//givens
// Generate matrix containing non-linear parameters for the gaussian test
// functions transformed to center of mass system
/* public matrix generateA() { */
/* matrix A = new matrix(nParticles - 1, nParticles -1); */
/* List<double> alphas = makeAlphas(); */
/* // Generate values for every entry in the matrix A */
/* for (int k = 0; k < nParticles - 1; k++) { */
/* for (int l = 0; l < k; l++) { */
/* double akl = A_kl(k,l,alphas); */
/* A[l,k] = akl; */
/* A[k,l] = akl; */
/* } */
/* A[k,k] = A_kl(k,k,alphas); */
/* } */
/* return A; */
/* } */
public matrix generateA()
{
matrix A = new matrix(nParticles - 1, nParticles - 1);
matrix B = new matrix(A.rows, A.cols);
for (int i = 0; i < A.cols; i++)
{
for (int j = 0; j < A.cols; j++)
{
if (i == j)
{
A[i, j] = Math.Log(1 - r.NextDouble()) / -1;
}
B[i, j] = Math.Log(1 - r.NextDouble()) / -1;
}
}
matrix Q = new QRdecomposition(B).Q;
return(Q * A * Q.transpose());
}
// Below are two modified versions of the above algorithm in which the errors are collected to monitor the convergence
public static void generate_convergences(int iteration, ref matrix A, ref matrix I, double e_0, vector v_0, double e_J, double tau = 1e-6, double eps = 1e-6, int n_max = 999, int updates = 999)
{
matrix As; double s = e_0;
As = A - s * I;
qr As_QR = new qr(As);
List <double> errors = new List <double>();
List <double> errors_s = new List <double>();
generate_errors(As_QR, ref A, ref I, ref errors, ref errors_s, s, updates, e_J, v_0, tau, eps);
var outfile = new System.IO.StreamWriter($"./plotfiles/convergence_{iteration}.txt", append: false);
for (int k = 0; k < errors.Count; k++)
{
outfile.WriteLine($"{k} {errors[k]} {errors_s[k]}");
}
outfile.Close();
}
static void Main()
{
int tend = 100;
double resulution = 1; // steps pr time unit for plot (dose not effect ODE)
vector t = linspace(0, tend, (int)(resulution * (tend + 1)));
double h = 1e-5;
double acc = 1e-4;
double eps = 1e-4;
// Initial coniditions found at: https://arxiv.org/pdf/math/0011268.pdf
double x10 = 0.97000436;
double y10 = -0.24308753;
double x20 = -x10;
double y20 = -y10;
double x30 = 0;
double y30 = 0;
double v3x0 = -0.93240737;
double v3y0 = -0.86473146;
double v2x0 = -0.5 * v3x0;
double v2y0 = -0.5 * v3y0;
double v1x0 = v2x0;
double v1y0 = v2y0;
vector Y0 = new vector(new double[] { x10, y10, v1x0, v1y0, x20, y20, v2x0, v2y0, x30, y30, v3x0, v3y0 });
vector param = new vector(1, 1, 1);
Func <double, vector, vector> f = (T, Y) => diffExplesit(T, Y, param);
matrix yres = ode_integrator.driver(f, t, Y0, h, acc, eps);
System.IO.StreamWriter outputfile = new System.IO.StreamWriter("out.plotC.txt", append: false);
for (int i = 0; i < t.size; i++)
{
outputfile.WriteLine("{0} {1} {2} {3} {4} {5} {6}", t[i], yres[i][0], yres[i][1], yres[i][4], yres[i][5], yres[i][8], yres[i][9]);
}
outputfile.Close();
}
static void Main()
{
int n = 7, m = n;
var A = new matrix(n, m);
var rnd = new System.Random();
for (int i = 0; i < A.size1; i++)
{
for (int j = 0; j < A.size2; j++)
{
A[i, j] = 2 * (rnd.NextDouble() - 1);
}
}
A.print("random matrix A:");
var b = new vector(m);
for (int i = 0; i < b.size; i++)
{
b[i] = rnd.NextDouble();
}
b.print("random vector b:\n");
var qra = new qrdecomposition(A);
var x = qra.solve(b);
x.print("solution x to system A*x=b:\n");
var Ax = A * x;
Ax.print("check: A*x (should be equal b):\n");
if (vector.approx(Ax, b))
{
WriteLine("test passed");
}
else
{
WriteLine("test failed");
}
var B = qra.inverse();
(B * A).print("A^-1*A=");
(A * B).print("A*A^-1=");
}
// returns (min, steps)
public static (vector, int) qnewton(Func <vector, double> f, vector x0, double eps)
{
int n = 0; // total number of steps
vector x = x0.copy(), s; // position vectors
double fx = f(x), fxs; // function values
vector gx = gradient(f, x), gxs; // gradient vectors
matrix B = matrix.id(x.size); // inverse Hessian, initialized to I
while (eps < gx.norm()) // continue until the sum of diffs is almost zero
{
n++;
vector Dx = -B * gx; // eq 6
double min = 1.0 / Pow(2, limit), l = 1.0; // min is the smallest step allowed, l is lambda
do // backtracking
{
s = l * Dx; // step, eq 8
fxs = f(x + s);
if (l < min)
{
B = matrix.id(x.size); // advised to reset B if l < min by the text
break;
}
l /= 2; // halve the step size
}while (!(fxs < fx + a * s.dot(gx))); // armijo condition
gxs = gradient(f, x + s);
vector y = gxs - gx; // eq 12
vector u = s - B * y;
double denom = u.dot(y);
if (Abs(denom) > eps) // eq 18, abs very important
{
B.update(u, u, 1 / denom); // dmitri has apparently already specified this operation
}
// B += u.dot(u)/denom;
// prepare for next iteration
x = x + s;
gx = gxs;
fx = fxs;
}
return(x, n);
}
public jcbi_cycl(matrix A, bool val_by_val = false, int evalnum = 1, bool invert = false)
{ // constructor of jacobi diag object through sweep
sweep = 0;
n = A.size1;
v = new matrix(n, n);
d = new vector(n);
// diagonal of A, copy into vector d
for (int i = 0; i < n; i++)
{
d[i] = A[i, i];
}
// set v diagonals to 1, off-diagonals 0
for (int i = 0; i < n; i++)
{
v[i, i] = 1.0;
for (int j = i + 1; j < n; j++)
{
v[i, j] = 0.0; v[j, i] = 0.0;
}
}
if (val_by_val)
{
for (p = 0; p < evalnum; p++)
{
do
{
rot_vbv(A, evalnum, invert);
} while (cond);
}
}
else
{
do
{
rot_sweep(A);
} while (cond);
}
}
} // Solve
public matrix inverse()
{
int n = Q.size1;
int m = Q.size2;
if (n != m)
{
Error.Write("Matrix must be a square matrix!");
}
matrix S = new matrix(n, m);
vector e = new vector(n);
for (int i = 0; i < n; i++)
{
e[i] = 1;
S[i] = solve(e);
e[i] = 0;
}
return(S);
}
public static void decomp(matrix A, matrix R)
{
int m = A.size2;
double ujvi, ujuj;
for (int i = 0; i < m; i++)
{
for (int j = 0; j < i; j++)
{
ujvi = A[j].dot(A[i]); ujuj = A[j].dot(A[j]);
R[j, i] = ujvi / ujuj; // a_ij
A[i] -= R[j, i] * A[j];
R[j, i] *= R[j, j];
}
R[i, i] = A[i].norm();
}
for (int i = 0; i < m; i++)
{
A[i] = A[i] / R[i, i]; // At this point, Q[i] = A[i]
}
}
// Generates the Lambda matrix
// TODO: Check lambda
private matrix generateLambda()
{
int nParticles = particles.Count;
matrix Lambda = new matrix(nParticles - 1, nParticles - 1);
for (int i = 0; i < nParticles - 1; i++)
{
for (int j = 0; j < nParticles - 1; j++)
{
double lambda_ij = 0;
for (int k = 0; k < nParticles; k++)
{
lambda_ij += U[i, k] * U[j, k] / particles[k].getMass();
}
Lambda[i, j] = lambda_ij;
}
}
return(Lambda);
}
public static (int, int, vector) simplex_update(matrix simplex, vector f_values, int d)
{
int hi = 0;
int lo = 0;
for (int i = 1; i < d + 1; i++)
{
if (f_values[i] < f_values[lo])
{
lo = i;
}
if (f_values[i] > f_values[hi])
{
hi = i;
}
}
vector centroid = calcCentroid(simplex, hi);
return(hi, lo, centroid);
}
public gs(matrix A)
{
Q = (A).copy();
int n = A.size1;
int m = A.size2;
Debug.Assert(n >= m);
R = new matrix(m, m);
for (int i = 0; i < m; i++)
{
R[i, i] = Q[i].norm();
Q[i] /= R[i, i];
for (int j = i + 1; j < m; j++)
{
R[i, j] = Q[i].dot(Q[j]);
Q[j] -= Q[i] * R[i, j];
}
}
}
public qrdecomposition(matrix M)
{
matrix A = M.copy();
for (int q = 0; q < A.size2; q++)
{
for (int p = q + 1; p < A.size1; p++)
{
double theta = Atan2(A[p, q], A[q, q]);
double c = Cos(theta), s = Sin(theta);
for (int k = q; k < A.size2; k++)
{
double xq = A[q, k], xp = A[p, k];
A[q, k] = xq * c + xp * s;
A[p, k] = -xq * s + xp * c;
}
A[p, q] = theta;
}
}
QR = A;
}
public static matrix operator*(matrix a, matrix b)
{
var c = new matrix(a.size1, b.size2);
for (int k = 0; k < a.size2; k++)
{
for (int j = 0; j < b.size2; j++)
{
double bkj = b[k, j];
var cj = c.data[j];
var ak = a.data[k];
int n = a.size1;
for (int i = 0; i < n; i++)
{
//c[i,j]+=a[i,k]*b[k,j];
cj[i] += ak[i] * bkj;
}
}
}
return(c);
}
public static void StagePole( rotpol[] r, int n, out CCEuler.EulerPole[] s_pole, out double[] s_angle)
{
int o;
matrix[] m = new matrix[100];
matrix[] mt = new matrix[100];
matrix[] ms = new matrix[100];
s_pole = new CCEuler.EulerPole[100];
s_angle = new double[100];
CCEuler.matrix_1(out m[1].ma);
CCEuler.matrix_1(out mt[0].ma);
for (o = 0; o <= n - 1; o++)
{
CCEuler.calc_pole2matrix(out m[o].ma, r[o].p, r[o].Angle);
CCEuler.calc_pole2matrix(out mt[o].ma, r[o].p, -r[o].Angle);
}
for (o = 0; o < n-1; o++)
{
CCEuler.matrix_mult(mt[o].ma, m[o+1 ].ma, out ms[o+1].ma);
CCEuler.calc_matrix2pole(ms[o+1].ma, out s_pole[o+1], out s_angle[o+1]);
}
}
/*
public int rows(){return data[0].size;}
public int cols(){return data.GetLength(0);}
*/
public matrix(matrix b)
{
nrows=b.rows; ncols=b.cols;
data = new double[nrows*ncols];
System.Array.Copy(b.data,data,b.data.Length);
}
public matrix transpose()
{
// int ncols=this.cols, nrows=this.rows;
matrix c = new matrix(ncols,nrows);
for(int ir=0;ir<nrows;ir++)
for(int ic=0;ic<ncols;ic++) c[ic,ir]=this[ir,ic];
return c;
}
public matrix copy()
{
matrix c = new matrix(this);
return c;
}
public static matrix operator ^(matrix a, matrix b)
{
int ac=a.cols, ar=a.rows, br=b.rows;
matrix c = new matrix(ac,br);
for(int ir=0;ir<ac;ir++)
for(int ic=0;ic<br;ic++)
{
double s=0;
for(int irar=ir*ar, icbr=ic*br; irar<ir*ar+ar;)
// for(int irar=ir*ar, icbr=ic*br, k=0;k<ar; k++)
s+=a.data[irar++]*b.data[icbr++];
// s+=a.data[k+ir*ar]*b.data[k+ic*br];
c.data[ir+ic*ar]=s;
}
/*
{
double s=0; int ar=a.rows;
for(int k=0;k<ar;k++)
s+=a[k,ir]*b[k,ic];
c[ir,ic]=s;
}
*/
return c;
}
public static matrix operator +(matrix a, matrix b)
{
matrix c = new matrix(a.rows,a.cols);
for(int i=0;i<a.rows;i++)
for(int j=0;j<a.cols;j++)
c[i,j]=a[i,j]+b[i,j];
return c;
}
public static matrix operator *(matrix a, matrix b)
{
int n=a.rows, m=b.cols;
if(a.cols != b.rows) System.Console.Error.Write("matrix mismatch\n");
var c = new matrix(n,m);
for (int k=0;k<a.cols;k++) for (int j=0;j<m;j++){
double tmp=b.data[k+j*b.rows];
// for (int i=0;i<n;i++){
for (int jn=j*n, kn=k*n; jn<j*n+n;){
// c.data[i+j*n]+=a.data[i+k*n]*tmp;
c.data[jn++]+=a.data[kn++]*tmp;
// c[i,j]+=a[i,k]*tmp;
}
}
return c;
}
