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> CreateDerivative(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].CalculateQDTD(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); }