public VectorN ScaleR(double s)
{
VectorN res = new VectorN(this);
res.Scale(s);
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 Integrate(double dt)
{
if (immovable || freezed)
{
return;
}
// Integrate velocity
forceAccum.Scale(invMass);
v.AddScaled(forceAccum, dt);
// some damping
v.Scale(Math.Pow(0.7, dt));
// Integrate position
pos.AddScaled(v, dt);
Update();
}
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;
}