private CLCalc.Program.Variable LUBackSubstitute(CLCalc.Program.Variable MLUDecomp, double[] b, int n, CLCalc.Program.Variable varindx)
{
CLCalc.Program.Variable varx = new Program.Variable(b);
CLCalc.Program.Variable varN = new Program.Variable(new int[1] { n });
int[] J = new int[1];
CLCalc.Program.Variable varJ = new Program.Variable(J);
CLCalc.Program.Variable[] args = new Program.Variable[] { MLUDecomp, varx, varindx, varN, varJ };
int[] max = new int[1];
//ajeita o vetor com respeito as trocas de linha
max[0] = 1;
doubleLUUnscramble.Execute(args, max);
//Forward subst
for (int i = n - 1; i >= 1; i--)
{
max[0] = i;
doubleLUForwardSubs.Execute(args, max);
}
//Backward subst
for (int j = n - 1; j >= 1; j--)
{
max[0] = 1;
J[0] = j;
varJ.WriteToDevice(J);
doubleLUDivide.Execute(args, max);
max[0] = j;
doubleLUBackSubs.Execute(args, max);
}
//Primeiro elemento
max[0] = 1;
J[0] = 0;
varJ.WriteToDevice(J);
doubleLUDivide.Execute(args, max);
return varx;
}
/// <summary>Calculates LU decomposition of M matrix</summary>
/// <param name="M">Matrix to decompose</param>
/// <param name="n">Matrix dimension</param>
/// <param name="varindx">Swap index</param>
private CLCalc.Program.Variable LUDecomp(double[,] M, int n, out CLCalc.Program.Variable varindx)
{
//arguments and work_dim
CLCalc.Program.Variable[] args;
int[] max;
//Matrix to vector
double[] vecM = MatrixToVector(M, ref n, ref n);
CLCalc.Program.Variable varM = new Program.Variable(vecM);
//Scaling transformation
double[] vv = new double[n];
CLCalc.Program.Variable varvv = new Program.Variable(vv);
max = new int[1] { n };
args = new CLCalc.Program.Variable[] { varM, varvv };
doubleLUScale.Execute(args, max);
//In order LU factorization (Crout)
int[] J = new int[1] { 0 };
CLCalc.Program.Variable varJ = new Program.Variable(J);
int[] N = new int[1] { n };
CLCalc.Program.Variable varN = new Program.Variable(N);
int[] indx = new int[n];
varindx = new Program.Variable(indx);
args = new Program.Variable[] { varM, varJ, varN, varindx, varvv };
for (J[0] = 0; J[0] < n; J[0]++)
{
varJ.WriteToDevice(J);
max[0] = J[0];
doubleLUCalcBetas.Execute(args, max);
max[0] = n - J[0];
doubleLUCalcAlphas.Execute(args, max);
max[0] = 1;
doubleLUCalcPivo.Execute(args, max);
max[0] = n;
doubleLUTrocaCols.Execute(args, max);
if (J[0] != n - 1)
{
max[0] = n - J[0] - 1;
doubleLUDivByPivot.Execute(args, max);
}
}
return varM;
}
/// <summary>Returns the sum of two matrices</summary>
/// <param name="M1">Matrix 1</param>
/// <param name="M2">Matrix 2</param>
public double[,] MatrixSum(double[,] M1, double[,] M2)
{
//Creates OpenCL variables to store data
int maxi = M1.GetLength(0);
int maxj = M1.GetLength(1);
double[] vecM1 = new double[maxi * maxj];
double[] vecM2 = new double[maxi * maxj];
if (M2.GetLength(0) != maxi || M2.GetLength(1) != maxj) throw new Exception("Incompatible dimensions");
for (int i = 0; i < maxi; i++)
{
for (int j = 0; j < maxj; j++)
{
vecM1[i + maxi * j] = M1[i, j];
vecM2[i + maxi * j] = M2[i, j];
}
}
CLCalc.Program.Variable varM1 = new Program.Variable(vecM1);
CLCalc.Program.Variable varM2 = new Program.Variable(vecM2);
CLCalc.Program.Variable[] args = new Program.Variable[2] { varM1, varM2 };
int[] max = new int[1] { maxi * maxj };
doubleVecSum.Execute(args, max);
//Escreve o resultado em varM1; devo ler os resultados de la
varM1.ReadFromDeviceTo(vecM1);
return VectorToMatrix(vecM1, ref maxi, ref maxj);
}
/// <summary>Matrix multiplication</summary>
/// <param name="M1">Matrix 1</param>
/// <param name="M2">Matrix 2</param>
public double[,] MatrixMultiply(double[,] M1, double[,] M2)
{
//M pxq, N qxr
int p = M1.GetLength(0);
int q = M1.GetLength(1);
int r = M2.GetLength(1);
if (q != M2.GetLength(0)) throw new Exception("Matrix dimensions do not match for multiplication");
double[] vecM1 = MatrixToVector(M1, ref p, ref q);
double[] vecM2 = MatrixToVector(M2, ref q, ref r);
double[] vecResp = new double[p * r];
CLCalc.Program.Variable varResp = new Program.Variable(vecResp);
CLCalc.Program.Variable varM1 = new Program.Variable(vecM1);
CLCalc.Program.Variable varM2 = new Program.Variable(vecM2);
//Finaliza a soma dos elementos
int[] vecQ = new int[1] { q };
CLCalc.Program.Variable varQ = new Program.Variable(vecQ);
CLCalc.Program.Variable[] args = new Program.Variable[4] { varResp, varM1, varM2, varQ };
int[] max = new int[2] { p, r };
doubleMatrixMult.Execute(args, max);
varResp.ReadFromDeviceTo(vecResp);
varResp.Dispose();
return VectorToMatrix(vecResp, ref p, ref r);
}
/// <summary>Solves linear system Mx = b by LU decomposition. Returns x</summary>
/// <param name="M">Matrix M</param>
/// <param name="b">Vector b</param>
/// <param name="maxAbsErr">Maximum acceptable absolute error</param>
/// <param name="maxIters">Maximum iterations</param>
public double[] LinSolve(double[,] M, double[] b, double maxAbsErr, int maxIters)
{
int n = M.GetLength(0);
if (M.GetLength(1) != n) throw new Exception("Matrix not square");
if (b.Length != n) throw new Exception("Incompatible vector dimension");
double[] x = new double[b.Length];
double[] errx = new double[b.Length];
CLCalc.Program.Variable varindx;
CLCalc.Program.Variable MLUDecomp = LUDecomp(M, n, out varindx);
double[] localMLUDecomp = new double[n*n];
MLUDecomp.ReadFromDeviceTo(localMLUDecomp);
//Backsubstitution
CLCalc.Program.Variable varx = LUBackSubstitute(MLUDecomp, b, n, varindx);
//Error control
CLCalc.Program.Variable varerrx = new Program.Variable(errx);
double[] vecM = MatrixToVector(M, ref n, ref n);
CLCalc.Program.Variable varM = new Program.Variable(vecM);
CLCalc.Program.Variable varb = new Program.Variable(b);
CLCalc.Program.Variable[] args = new Program.Variable[] { varM, varb, varerrx, varx };
int[] max = new int[] { n };
double absErr = maxAbsErr + 1;
int iter = 0;
while (absErr > maxAbsErr && iter < maxIters)
{
CLCalc.Program.Variable vardelta = LUBackSubstitute(MLUDecomp, errx, n, varindx);
//Acopla correção na solução
CLCalc.Program.Variable[] args2 = new Program.Variable[] { vardelta, varx };
doubleLUSubErr.Execute(args2, max);
doubleSolveError.Execute(args, max);
varerrx.ReadFromDeviceTo(errx);
absErr = 0;
for (int j = 0; j < n; j++) absErr += errx[j] * errx[j];
absErr = (double)Math.Sqrt(absErr);
iter++;
}
varx.ReadFromDeviceTo(x);
return x;
}
/// <summary>Gauss Seidel method for iterative linear system solving. Returns unknown x</summary>
/// <param name="M">Matrix M so that Mx=b</param>
/// <param name="x">Initial estimate</param>
/// <param name="b">Known vector b</param>
/// <param name="Iterations">Gauss-Seidel iterations per step</param>
/// <param name="MaxIter">Maximum number of times Gauss-Seidel iterations</param>
/// <param name="totalError">Desired sqrt(Sum(error[i]^2))*number of equations</param>
/// <param name="err">Estimated absolute error per component</param>
public double[] LeastSquaresGS(double[,] M, double[] b, double[] x, int Iterations, int MaxIter, double totalError, out double[] err)
{
//M pxp
int p = M.GetLength(0);
int q = M.GetLength(1);
//Consistencia
if (p != b.Length) throw new Exception("Matrix and vector b dimensions not compatible");
if (q != x.Length) throw new Exception("Matrix and initial guess x dimensions not compatible");
double[] vecM = MatrixToVector(M, ref p, ref q);
//Calculo de MtM e Mtb
CLCalc.Program.Variable varMtM = new Program.Variable(new double[q * q]);
CLCalc.Program.Variable varMtb = new Program.Variable(new double[q]);
{
CLCalc.Program.Variable varM = new Program.Variable(vecM);
CLCalc.Program.Variable varb = new Program.Variable(b);
CLCalc.Program.Variable varp = new Program.Variable(new int[1] { p });
CLCalc.Program.Variable[] arg = new Program.Variable[] { varM, varMtM, varp };
int[] maxs = new int[2] { q, q };
doubleCalcMtM.Execute(arg, maxs);
arg = new Program.Variable[] { varM, varb, varMtb, varp };
maxs = new int[1] { q };
doubleCalcMtb.Execute(arg, maxs);
varM.Dispose(); varb.Dispose(); varp.Dispose();
}
//Solucao do sistema
CLCalc.Program.Variable varx = new Program.Variable(x);
CLCalc.Program.Variable varerrx = new Program.Variable(x);
CLCalc.Program.Variable[] args = new Program.Variable[3] { varMtM, varMtb, varx };
CLCalc.Program.Variable[] args2 = new Program.Variable[4] { varMtM, varMtb, varerrx, varx };
int[] max = new int[1] { q };
double[] resp = new double[q];
err = new double[q];
double absErr = totalError + 1;
int i = 0;
while (i < MaxIter && absErr > totalError * (double)q)
//while (i < MaxIter && absErr > totalError)
{
for (int j = 0; j < Iterations; j++)
{
doubleGaussSeidel.Execute(args, max);
i++;
}
//Calcula o erro
doubleGaussSeidelError.Execute(args2, max);
varerrx.ReadFromDeviceTo(err);
absErr = 0;
for (int j = 0; j < q; j++) absErr += err[j] * err[j];
absErr = (double)Math.Sqrt(absErr);
}
//Retorna a resposta
varx.ReadFromDeviceTo(resp);
varerrx.ReadFromDeviceTo(err);
//Limpa memoria
varMtM.Dispose(); varx.Dispose(); varerrx.Dispose(); varMtb.Dispose();
return resp;
}
/// <summary>Initializes physics program. Components indexes: [i] - x, [i+1] - y, [i+2] - z</summary>
/// <param name="nParticles">Number of particles</param>
public floatBodyPhysics(int nParticles)
{
string[] s = new string[] { CollisionAppliers, ForceAppliers, ConstAccelMotionEDOSolver };
Program.Compile(s);
//Kernels
MotionStep = new Program.Kernel("constAccelStep");
Kernel_ApplyGravity = new Program.Kernel("ApplyGravity");
Kernel_FloorCollision = new Program.Kernel("FloorCollision");
Kernel_SelfCollision = new Program.Kernel("SelfCollision");
Kernel_WallCollision = new Program.Kernel("WallCollision");
Kernel_ResetForces = new Program.Kernel("ResetForces");
Kernel_ResetCloseNeighbors = new Program.Kernel("ResetCloseNeighbors");
float[] t = new float[1] { 0 };
float[] gg = new float[3] { 0, 0, 0 };
step = new float[1] { 0 };
//Tamanho de alocacao de velocidades e posicoes
float[] aloc = new float[nParticles * 3];
//Tamanho de alocacao de caracteristicas das particulas
float[] alocPart = new float[nParticles];
//3*Nparticulas
CL_pos = new CLCalc.Program.Variable(aloc);
CL_vel = new CLCalc.Program.Variable(aloc);
CL_forces = new CLCalc.Program.Variable(aloc);
//Nparticulas
closeNeighbors = new int[nParticles];
CL_closeNeighbors = new CLCalc.Program.Variable(closeNeighbors);
for (int i = 0; i < nParticles; i++) alocPart[i] = 1f; //inicializa massas como 1 e tamanhos de colisao como 1
CL_masses = new CLCalc.Program.Variable(alocPart);
CL_collisionSizes = new CLCalc.Program.Variable(alocPart);
//escalares
CL_t = new CLCalc.Program.Variable(t);
CL_step = new CLCalc.Program.Variable(step);
//gravidade
CL_g = new CLCalc.Program.Variable(gg);
//Argumentos de funcoes
stepArgs = new CLCalc.Program.Variable[] { CL_t, CL_step, CL_forces, CL_masses, CL_pos, CL_vel };
applyGravArgs = new CLCalc.Program.Variable[] { CL_forces, CL_masses, CL_g };
floorCollisionArgs = new CLCalc.Program.Variable[] { CL_vel, CL_pos, CL_collisionSizes };
wallCollisionArgs = floorCollisionArgs;
selfCollisionArgs = new CLCalc.Program.Variable[] { CL_vel, CL_pos, CL_masses, CL_forces, CL_closeNeighbors, CL_collisionSizes };
resetForcesArgs = new Program.Variable[] { CL_forces };
resetCloseNeighborsArgs = new Program.Variable[] { CL_closeNeighbors };
nArgs = new int[1] { nParticles * 3 };
nPartics = new int[1] { nParticles };
nPartics2 = new int[2] { nParticles, nParticles };
}