public Sphere(Vec3 x, double r, double m)
{
if (m == 0)
{
throw new DivideByZeroException("Mass can't be zero!");
}
double i = 2.0 / 5.0 * m * r * r;
if (i == 0)
{
throw new DivideByZeroException("Inertia can't be zero!");
}
this.x = x;
this.r = r;
this.m = Mat3.Diag(m);
m_inv = Mat3.Diag(1.0 / m);
I = Mat3.Diag(i);
I_inv = Mat3.Diag(1 / i);
Collider = new SphereCollider(this);
}
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();
}