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;
}
