public Contact(Particle p1, Particle p2, VectorN _normal, double _depth) : base(p1, p2) { normal = _normal; depth = _depth; restitution = 0.7; VectorN relVelVec = new VectorN(pair[0].v); relVelVec.Sub(pair[1].v); relVel = relVelVec.Dot(normal); }
public VectorN SubR(VectorN other) { if (other.n != n) { return(null); } VectorN res = new VectorN(this); res.Sub(other); return(res); }
public override VectorN Jacobian() { VectorN n = new VectorN(pair[1].pos); n.Sub(pair[0].pos); double r = n.GetNorm(); n.Scale(r); double c = r - L; J.v[0] = -n.v[0]; J.v[1] = -n.v[1]; J.v[2] = n.v[0]; J.v[3] = n.v[1]; J.v[4] = -0 * 200; return(J); }
public void Collide() { DateTime t = DateTime.Now; contacts.Clear(); for (int i = 0; i < particles.Count - 1; i++) { for (int j = i + 1; j < particles.Count; j++) { if (particles[i].freezed && particles[j].freezed) { continue; } double sum = particles[i].radius + particles[j].radius; if (Math.Abs(particles[i].pos.v[0] - particles[j].pos.v[0]) > sum) { continue; } if (Math.Abs(particles[i].pos.v[1] - particles[j].pos.v[1]) > sum) { continue; } VectorN normal = new VectorN(particles[i].pos); normal.Sub(particles[j].pos); double dist = normal.Normalize(); if (dist < sum) { Contact c = new Contact(particles[i], particles[j], normal, sum - dist); c.Unfreeze(); contacts.Add(c); } } } TimeSpan tt = DateTime.Now.Subtract(t); CDTime = (double)(tt.TotalMilliseconds);// / (double)TimeSpan.TicksPerMillisecond; }
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; }