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 Vec3Vector(Vec3Vector v) : base(v.Size)
{
for (int i = 0; i < Size; i++)
{
this [i] = new Vec3(v [i]);
}
}
public static Vec3Vector operator*(Mat3Matrix M, Vec3Vector v)
{
if (M.Cols != v.Size)
{
throw new InvalidOperationException("Invalid dimensions");
}
Vec3Vector vec = new Vec3Vector(M.Rows);
if (M.Diagonal)
{
for (int i = 0; i < M.Rows; i++)
{
vec [i] = M [i, i] * v [i];
}
}
else
{
for (int i = 0; i < M.Rows; i++)
{
for (int j = 0; j < M.Cols; j++)
{
vec [i].Add(M [i, j] * v [j]);
}
}
}
return(vec);
}
public static Vec3Vector operator *(Vector <T> v1, Vec3Vector v2)
{
Vec3Vector nv = new Vec3Vector(v1.Size);
for (int i = 0; i < v1.Size; i++)
{
nv [i] = (double)(object)v1 [i] * v2 [i];
}
return(nv);
}
public static Vec3Vector operator *(double k, Vec3Vector v)
{
Vec3Vector nv = new Vec3Vector(v.Size);
for (int i = 0; i < v.Size; i++)
{
nv [i] = k * v [i];
}
return(nv);
}
public static double Dot(Vec3Vector v1, Vec3Vector v2)
{
double dot = 0;
for (int i = 0; i < v1.Length; i++)
{
dot += Vec3.Dot(v1 [i], v2 [i]);
}
return(dot);
}
public Vec3Vector Clone()
{
var clone = new Vec3Vector(Size);
for (int i = 0; i < Size; i++)
{
clone [i] = new Vec3(this [i]);
}
return(clone);
}
public static Vec3Vector operator -(Vec3Vector v1, Vec3Vector v2)
{
if (v1.Size != v2.Size)
{
throw new InvalidOperationException("Invalid dimensions");
}
Vec3Vector nv = new Vec3Vector(v1.Size);
for (int i = 0; i < v1.Size; i++)
{
nv [i] = v1 [i] - (Vec3)(object)v2 [i];
}
return(nv);
}
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();
}