public VectorN AddR(VectorN other) { if (other.n != n) { return(null); } VectorN res = new VectorN(this); res.Add(other); return(res); }
public void Simulate(double dt) { // Applying forces Collide(); long t = DateTime.Now.Ticks; if (joints.Count == 0 && contacts.Count == 0) { foreach (Particle p in particles) { p.Integrate(dt); } return; } uint n = (uint)particles.Count; int i, j; // Then build mass matrix M MatrixMN M = new MatrixMN(2 * n, 2 * n); for (i = 0, j = 0; i < n; i++, j += 2) { double mass = particles[i].invMass; if (particles[i].immovable) { mass = 0; } M.v[j][j] = mass; M.v[j + 1][j + 1] = mass; } // Build external force vector, velocity vector, position vector VectorN F = new VectorN(2 * n); VectorN V = new VectorN(2 * n); VectorN X = new VectorN(2 * n); for (i = 0, j = 0; i < n; i++, j += 2) { F.v[j] = particles[i].forceAccum.v[0]; F.v[j + 1] = particles[i].forceAccum.v[1]; if (particles[i].immovable) { V.v[j] = 0; V.v[j + 1] = 0; } else { V.v[j] = particles[i].v.v[0]; V.v[j + 1] = particles[i].v.v[1]; } X.v[j] = particles[i].pos.v[0]; X.v[j + 1] = particles[i].pos.v[1]; } // Build Jacobian J uint s = (uint)(joints.Count + contacts.Count); MatrixMN J = new MatrixMN(s, 2 * n); VectorN xi = new VectorN(s); for (i = 0; i < joints.Count; i++) { VectorN Jpartial = joints[i].Jacobian(); Particle p = joints[i].pair[0]; J.v[i][2 * p.id] = Jpartial.v[0]; J.v[i][2 * p.id + 1] = Jpartial.v[1]; p = joints[i].pair[1]; J.v[i][2 * p.id] = Jpartial.v[2]; J.v[i][2 * p.id + 1] = Jpartial.v[3]; xi.v[i] = Jpartial.v[4]; } for (j = 0, i = joints.Count; j < contacts.Count; j++, i++) { VectorN Jpartial = contacts[j].Jacobian(); Particle p = contacts[j].pair[0]; J.v[i][2 * p.id] = Jpartial.v[0]; J.v[i][2 * p.id + 1] = Jpartial.v[1]; p = contacts[j].pair[1]; J.v[i][2 * p.id] = Jpartial.v[2]; J.v[i][2 * p.id + 1] = Jpartial.v[3]; xi.v[i] = Jpartial.v[4]; } // Ax = b VectorN b = new VectorN(V); b.Scale(1 / dt); b.Sub(M.Multiply(F)); b = J.Multiply(b); xi.Scale(1 / dt); b = xi.SubR(b); MatrixMN JT = J.Transpose(); MatrixMN A = J.Multiply(M); A = A.Multiply(JT); // Solve Ax = b VectorN lambda = MatrixMN.SolveGSSOR(A, b); F.Add(JT.Multiply(lambda)); //F = M.Multiply(F); //V.AddScaled(F, dt); //X.AddScaled(V, dt); for (i = 0, j = 0; i < n; i++, j += 2) { particles[i].forceAccum.v[0] = F.v[j]; particles[i].forceAccum.v[1] = F.v[j + 1]; particles[i].v.v[0] = V.v[j]; particles[i].v.v[1] = V.v[j + 1]; particles[i].pos.v[0] = X.v[j]; particles[i].pos.v[1] = X.v[j + 1]; particles[i].Integrate(dt); particles[i].forceAccum.Clear(); } t = DateTime.Now.Ticks - t; solverTime = (double)t / (double)TimeSpan.TicksPerMillisecond; }
public void ApplyForce(VectorN f) { forceAccum.Add(f); }