public static Vec3Vector GaussSeidel(Vec3Matrix S, Vec3Vector B, uint iterations, IterationDirection direction = IterationDirection.Forward, TextWriter logFile = null)
{
if (logFile == null)
{
logFile = new NullWriter();
}
Vec3Vector lambda = new Vec3Vector(B.Size);
for (int i = 0; i < iterations; i++)
{
double[] err = GaussSeidelIterate(S, lambda, B, direction);
double absError = err [0];
double sigma = err [1];
logFile.WriteLine(i + 1 + " " + absError + " " + sigma);
if (absError < errorThreshold)
{
break;
}
}
return(lambda);
}
public static Vec3Matrix operator *(Vec3Matrix M1, Mat3Matrix M2)
{
if (M1.Cols != M2.Rows)
{
throw new InvalidOperationException("Invalid dimensions");
}
Vec3Matrix M = new Vec3Matrix(M1.Rows, M2.Cols);
if (M1.Diagonal && M2.Diagonal)
{
for (int i = 0; i < M1.Rows; i++)
{
M [i, i] = M1 [i, i] * M2 [i, i];
}
}
else if (M1.Diagonal)
{
for (int i = 0; i < M1.Rows; i++)
{
for (int j = 0; j < M2.Cols; j++)
{
M [i, j] = M1 [i, i] * M2 [i, j];
}
}
}
else if (M2.Diagonal)
{
for (int i = 0; i < M1.Rows; i++)
{
for (int j = 0; j < M2.Cols; j++)
{
M [i, j] = M1 [i, j] * M2 [j, j];
}
}
}
else
{
for (int i = 0; i < M1.Rows; i++)
{
for (int j = 0; j < M2.Cols; j++)
{
var e = new Vec3();
for (int k = 0; k < M1.Cols; k++)
{
e.Add(M1 [i, k] * M2 [k, j]);
}
M [i, j] = e;
}
}
}
return(M);
}
private static double[] GaussSeidelIterate(Vec3Matrix S, Vec3Vector lambda, Vec3Vector B, IterationDirection direction)
{
Vec3Vector lambda_old = lambda.Clone();
double sigma = 0;
var deltaLambda = new Vec3();
var sum = new Vec3();
if (direction == IterationDirection.Forward)
{
for (int i = 0; i < lambda.Length; i++)
{
sum.SetZero();
for (int j = 0; j < lambda.Length; j++)
{
sum += S [i, j] * lambda [j];
}
deltaLambda.Set((1.0 / S [i, i]) * (B [i] - sum));
sigma += deltaLambda.SqLength;
lambda [i] += deltaLambda;
}
}
else
{
for (int i = lambda.Length - 1; i >= 0; i--)
{
sum.SetZero();
for (int j = lambda.Length - 1; j >= 0; j--)
{
sum += S [i, j] * lambda [j];
}
deltaLambda.Set((1.0 / S [i, i]) * (B [i] - sum));
sigma += deltaLambda.SqLength;
lambda [i] += deltaLambda;
}
}
var error = new double[2] {
(lambda - lambda_old).Norm, // Absolute Error
sigma / lambda.Length // Sigma
};
return(error);
}
private void SpheresUpdate(double dt)
{
if (Spheres == null)
{
return;
}
var contactObjects = new List <Entity> ();
var contactIntersectionData = new List <Intersection> ();
var contactCollisionMatrices = new List <Mat3> ();
/* Build up broad phase collision hash grid */
var hgridObjects = AddSpheresToHGrid();
for (int i = 0; i < Spheres.Count; i++)
{
Sphere sphere = Spheres [i];
// Add gravity and take a half timestep
sphere.f.Set(sphere.m * g);
sphere.v.Add(0.5 * dt * sphere.m_inv * sphere.f);
intersections.Clear();
/* Broad phase collision detection againt other spheres */
List <Sphere> possibleCollisions = hgrid.CheckObjAgainstGrid(hgridObjects [i]);
hgrid.RemoveObject(hgridObjects [i]);
foreach (Sphere other in possibleCollisions)
{
var data = sphere.Collider.CheckIntersection(other);
foreach (var d in data)
{
d.self = sphere;
}
intersections.AddRange(data);
}
foreach (Entity entity in entities)
{
// FIXME not needed?
// This is here so that the ground doesn't accumulate velocity
entity.v.SetZero();
var data = entity.Collider.CheckIntersection(sphere);
foreach (var d in data)
{
d.self = sphere;
}
intersections.AddRange(data);
}
foreach (Intersection data in intersections)
{
const double e = 0.8;
const double mu = 0.1;
Entity other = data.entity;
var u = sphere.v - other.v;
var r = sphere.x - other.x;
var u_n = Vec3.Dot(u, data.normal) * data.normal;
var r_a = sphere.x - data.point;
var r_b = other.x - data.point;
/* Check if contact is a separating contact */
if (Vec3.Dot(u, data.normal) > 0)
{
continue;
}
var rax = Mat3.SkewSymmetric(r_a);
var rbx = Mat3.SkewSymmetric(r_b);
var I_a = sphere.I_inv;
var I_b = other.I_inv;
var M_a = sphere.m_inv;
var M_b = other.m_inv;
Mat3 K = M_a + M_b; // - (rax * I_a * rax.Transpose + rbx * I_b * rbx.Transpose);
Vec3 J = K.Inverse * (-e * u_n - u);
var j_n = Vec3.Dot(J, data.normal) * data.normal;
var j_t = J - j_n;
bool in_allowed_friction_cone = j_t.SqLength < mu * mu * j_n.SqLength;
if (!in_allowed_friction_cone)
{
Vec3 n = data.normal;
Vec3 t = j_t.UnitVector;
var j = -(1 + e) * u_n.Length / Vec3.Dot(n * K, n - mu * t);
J = -j * n - mu * j * t;
}
sphere.v.Add(sphere.m_inv * J);
sphere.omega.Add(sphere.I_inv * Vec3.Cross(r_a, J));
other.v.Add(-other.m_inv * J);
other.omega.Add(-other.I_inv * Vec3.Cross(r_b, J));
var u_new = sphere.v - other.v;
var contactTest = Vec3.Dot(data.normal, u_new);
/* Check if contact is a resting contact */
if (contactTest <= 0.3)
{
// Add to contact matrix
if (!contactObjects.Contains(other))
{
contactObjects.Add(other);
}
if (!contactObjects.Contains(sphere))
{
contactObjects.Add(sphere);
}
data.i = contactObjects.FindIndex(s => {
return(s == sphere);
});
data.j = contactObjects.FindIndex(s => {
return(s == other);
});
contactIntersectionData.Add(data);
contactCollisionMatrices.Add(K);
}
}
// Add the other half of the time step
sphere.v.Add(0.5 * dt * sphere.m_inv * sphere.f);
}
if (contactObjects.Count > 0)
{
double d = timestepsToStabilizeConstraint;
double k = 1000;
double a = 4 / (dt * (1 + 4 * d));
double b = (4 * d) / (1 + 4 * d);
var M = contactIntersectionData.Count;
var N = contactObjects.Count;
var G = new Vec3Matrix(M, N);
var CollisionMatrix = new Mat3Matrix(M, M);
var dW = new Vec3Vector(N);
var W = new Vec3Vector(N);
var q = new Vec3Vector(M);
var M_inv = new Mat3Matrix(N, N);
// Jacobian
for (int i = 0; i < M; i++)
{
int body_i = contactIntersectionData [i].i;
int body_j = contactIntersectionData [i].j;
G [i, body_i] = contactIntersectionData [i].normal;
G [i, body_j] = -contactIntersectionData [i].normal;
CollisionMatrix [i, i] = contactCollisionMatrices [i];
}
// Set constraints q
for (int i = 0; i < M; i++)
{
q [i] = -contactIntersectionData [i].distance * contactIntersectionData [i].normal;
}
// Set values to M, f, W
for (int i = 0; i < N; i++)
{
dW [i] = contactObjects [i].m_inv * contactObjects [i].f;
W [i] = contactObjects [i].v;
M_inv [i, i] = contactObjects [i].m_inv;
}
Vec3Matrix S = G * M_inv * G.Transpose;
for (int i = 0; i < S.Rows; i++)
{
var e = 4 / (dt * dt * k * (1 + 4 * d));
S [i, i].Add(e);
}
Vec3Vector B = -a * q - b * (G * W) - dt * (G * dW);
// Data collection
if (sphereTimeSteps < timestepLimit)
{
sphereTimeSteps++;
}
else if (sphereTimeSteps == timestepLimit)
{
sphereTimeSteps++;
Debug.Log("Spheres sample complete! Timestep: " + 1 / Time.fixedDeltaTime + "Hz");
sphereData.Close();
sphereData = null;
}
Vec3Vector lambda = Solver.GaussSeidel(S, B, maxSolverIterations, logFile: sphereData);
/* If lambda has negative values, clamp to zero */
foreach (Vec3 elem in lambda)
{
for (int j = 0; j < 3; j++)
{
if (elem [j] < 0)
{
elem [j] = 0;
//Debug.Log ("Negative lambda");
}
}
}
var fc = G.Transpose * lambda;
for (int i = 0; i < contactObjects.Count; i++)
{
contactObjects [i].v.Add(contactObjects [i].m_inv * fc [i]);
}
}
// Finally, integrate
foreach (var sphere in Spheres)
{
sphere.x.Add(dt * sphere.v);
}
}
private void RopeUpdate(double dt)
{
if (Rope == null)
{
return;
}
// Data collection
if (ropeTimeSteps < timestepLimit)
{
ropeTimeSteps++;
}
else if (ropeTimeSteps == timestepLimit)
{
ropeTimeSteps++;
Debug.Log("Rope sample complete! Timestep: " + 1 / Time.fixedDeltaTime + "Hz");
ropeData.Close();
ropeData = null;
}
// Add gravity
foreach (var p in Rope)
{
p.f.Set(p.m * g);
}
intersections.Clear();
CheckCollisions(Rope);
HandleCollisions();
/* Constant parameters in SPOOK */
double d = timestepsToStabilizeConstraint;
double a = 4 / (dt * (1 + 4 * d));
double b = (4 * d) / (1 + 4 * d);
var N = Rope.Count;
List <Constraint> C = Rope.constraints;
var G = new Vec3Matrix(C.Count, N); // Constraint Jacobian matrix
var M_inv = new Mat3Matrix(N, N); // Inverse Mass matrix
// All forces, velocities, generalized positions
var f = new Vec3Vector(N);
var W = new Vec3Vector(N);
var q = new Vec3Vector(C.Count);
for (int i = 0; i < C.Count; i++)
{
// Set Jacobians
var c = C [i];
Vec3[] jac = c.getJacobians(Rope);
G [i, c.body_i] = jac [0];
G [i, c.body_j] = jac [1];
// Set constraints q
q [i] = c.getConstraint(Rope);
}
// Set values to M, f, W
for (int i = 0; i < N; i++)
{
M_inv [i, i] = Mat3.Diag(Rope [i].m_inv);
f [i] = Rope [i].f;
W [i] = Rope [i].v;
}
Vec3Matrix S = G * M_inv * G.Transpose;
for (int i = 0; i < S.Rows; i++)
{
var e = 4 / (dt * dt * C [i].k * (1 + 4 * d));
S [i, i].Add(e);
}
Vec3Vector B = -a * q - b * (G * W) - dt * (G * (M_inv * f));
Vec3Vector lambda = Solver.GaussSeidel(S, B, maxSolverIterations, iterationDirection, logFile: ropeData);
Vec3Vector fc = G.Transpose * lambda;
// Integrate
for (int i = 0; i < N; i++)
{
Particle p = Rope [i];
p.v = p.v + p.m_inv * fc [i] + dt * p.m_inv * p.f;
p.x = p.x + dt * p.v;
}
AdjustIntersections();
}
