public static ImplicitMatrix <T> operator *(ImplicitMatrix <T> m1, ImplicitMatrix <T> m2)
{
if (m1.Columns != m2.Rows)
{
throw new Exception("Matrix multiplication error, row of m1 isn't equal to the column count of m2");
}
ImplicitMatrix <T> output = new ImplicitMatrix <T>(m1.Rows, m2.Columns);
for (int i = 0; i < output.Rows; i++)
{
for (int j = 0; j < output.Columns; j++)
{
CalculatingUnit <T> item = bop.Zero();
foreach (MatrixItem mItem1 in m1.mRf[i])
{
if (m2.mCf[j].Exists(x => x.row == mItem1.column))
{
MatrixItem mItem2 = m2.mCf[j].Find(x => x.row == mItem1.column);
item = bop.Add(item, bop.Multiply(mItem1.value, mItem2.value));
}
}
output[i, j] = item;
}
}
return(output);
}
/// <summary>
/// Creates an Identy matrix
/// </summary>
/// <param name="n"></param>
/// <returns>an nxn matrix</returns>
public static ImplicitMatrix <T> Identity(int n = 4)
{
ImplicitMatrix <T> m = new ImplicitMatrix <T>(n, n);
for (int i = 0; i < n; i++)
{
m[i, i] = bop.One();
}
return(m);
}
public static ImplicitMatrix <T> operator *(ImplicitMatrix <T> m, T scaler)
{
ImplicitMatrix <T> result = new ImplicitMatrix <T>(m.Rows, m.Columns);
foreach (List <MatrixItem> matrixItems in m.mRf)
{
foreach (MatrixItem matrixItem in matrixItems)
{
result[matrixItem.row, matrixItem.column] = matrixItem.value * scaler;
}
}
return(result);
}
public ImplicitMatrix <T> Transpose()
{
ImplicitMatrix <T> result = new ImplicitMatrix <T>(Columns, Rows);
foreach (List <MatrixItem> matrixItems in mRf)
{
foreach (MatrixItem matrixItem in matrixItems)
{
result[matrixItem.column, matrixItem.row] = matrixItem.value;
}
}
return(result);
}
public ImplicitMatrix <U> ConvertTo <U>() where U : IComparable <U>
{
ImplicitMatrix <U> result = new ImplicitMatrix <U>(this.Rows, this.Columns);
foreach (List <MatrixItem> matrixItems in mRf)
{
foreach (MatrixItem matrixItem in matrixItems)
{
result[matrixItem.row, matrixItem.column] = bop.ConvertTo <U>(matrixItem.value);
}
}
return(result);
}
public static ImplicitMatrix <T> operator -(ImplicitMatrix <T> m1)
{
ImplicitMatrix <T> output = new ImplicitMatrix <T>(m1.Rows, m1.Columns);
foreach (List <MatrixItem> matrixItems in m1.mCf)
{
foreach (MatrixItem matrixItem in matrixItems)
{
output[matrixItem.row, matrixItem.column] = bop.Negate(matrixItem.value);
}
}
return(output);
}
private void DoConstraints(List <IConstraint> constraints, List <Particle> particles, float dt, float ks, float kd)
{
ImplicitMatrix <float> W = ImplicitMatrix <float> .Identity(particles.Count * 2);
HyperPoint <float> Q = new HyperPoint <float>(particles.Count * 2);
HyperPoint <float> q = new HyperPoint <float>(particles.Count * 2);
HyperPoint <float> qDot = new HyperPoint <float>(particles.Count * 2);
HyperPoint <float> QDak = new HyperPoint <float>(particles.Count * 2);
for (int i = 0; i < particles.Count; i++)
{
q[i * 2 + 0] = particles[i].Position[0];
q[i * 2 + 1] = particles[i].Position[1];
qDot[i * 2 + 0] = particles[i].Velocity[0];
qDot[i * 2 + 1] = particles[i].Velocity[1];
W[i * 2 + 0, i * 2 + 0] = 1 / particles[i].Massa;
W[i * 2 + 1, i * 2 + 1] = 1 / particles[i].Massa;
Q[i * 2 + 0] = particles[i].ForceAccumulator[0];
Q[i * 2 + 1] = particles[i].ForceAccumulator[1];
}
HyperPoint <float> c = new HyperPoint <float>(constraints.Count);
HyperPoint <float> cDot = new HyperPoint <float>(constraints.Count);
for (int i = 0; i < constraints.Count; i++)
{
c[i] = constraints[i].Calculate(particles);
cDot[i] = constraints[i].CalculateTD(particles);
}
ImplicitMatrix <float> J = JacobianMatrix.Create(particles, constraints);
ImplicitMatrix <float> JDot = JacobianMatrix.CreateDerivative(particles, constraints);
ImplicitMatrix <float> JT = J.Transpose();
ImplicitMatrix <float> A = J * W * JT;
HyperPoint <float> b1 = -JDot * qDot;
HyperPoint <float> b2 = J * W * Q;
HyperPoint <float> b3 = ks * c;
HyperPoint <float> b4 = kd * cDot;
HyperPoint <float> b = b1 - b2 - b3 - b4;
HyperPoint <float> x;
LinearSolver.ConjGrad(b.Dim, A, out x, b, ConstraintsEpsilon, Steps);
QDak = JT * x;
for (int i = 0; i < particles.Count; i++)
{
particles[i].ForceConstraint = new HyperPoint <float>(QDak[i * 2 + 0], QDak[i * 2 + 1]);
}
}
public static ImplicitMatrix <float> Create(List <Particle> particles, List <IConstraint> c)
{
ImplicitMatrix <float> output = new ImplicitMatrix <float>(c.Count, particles.Count * 2);
for (int i = 0; i < c.Count; i++)
{
var constraints = c[i].CalculateQD(particles);
foreach (ResultingConstraint resultingConstraint in constraints)
{
if (float.IsNaN(resultingConstraint.Constraint[0]) || float.IsNaN(resultingConstraint.Constraint[1]))
{
throw new NaNException();
}
output[i, resultingConstraint.ParticleIndex *2 + 0] = resultingConstraint.Constraint[0];
output[i, resultingConstraint.ParticleIndex *2 + 1] = resultingConstraint.Constraint[1];
}
}
return(output);
}
public static float ConjGrad(int n, ImplicitMatrix <float> A, out HyperPoint <float> x, HyperPoint <float> b, float epsilon, int steps)
{
int i, iMax;
float alpha, beta, rSqrLen, rSqrLenOld, u;
HyperPoint <float> r = new HyperPoint <float>(n);
HyperPoint <float> d = new HyperPoint <float>(n);
HyperPoint <float> t = new HyperPoint <float>(n);
HyperPoint <float> temp = new HyperPoint <float>(n);
x = b;
r = b;
temp = A * x;
r = r - temp;
rSqrLen = r.GetLengthSquared();
d = r;
iMax = steps != 0 ? steps : MaxSteps;
i = 0;
if (rSqrLen > epsilon)
{
while (i < iMax)
{
i++;
t = A * d;
u = HyperPoint <float> .DotProduct(d, t);
if (u == 0)
{
Console.Out.WriteLine("(SolveConjGrad) d'Ad = 0\n");
break;
}
// How far should we go?
alpha = rSqrLen / u;
// Take a step along direction d
temp = d;
temp = temp * alpha;
x = x + temp;
if ((i & 0x3F) != 0)
{
temp = t;
temp = temp * alpha;
r = r - temp;
}
else
{
// For stability, correct r every 64th iteration
r = b;
temp = A * x;
r = r - temp;
}
rSqrLenOld = rSqrLen;
rSqrLen = r.GetLengthSquared();
// Converged! Let's get out of here
if (rSqrLen <= epsilon)
{
break;
}
// Change direction: d = r + beta * d
beta = rSqrLen / rSqrLenOld;
d = d * beta;
d = d + r;
}
}
steps = i;
return(rSqrLen);
}
