public void GetRow()
{
Matrix2x2f m = Indexed2x2();
Assert.AreEqual(new Vector2f(0, 2), m.GetRow(0));
Assert.AreEqual(new Vector2f(1, 3), m.GetRow(1));
}
public void GetColumn()
{
Matrix2x2f m = Indexed2x2();
Assert.AreEqual(new Vector2f(0, 1), m.GetColumn(0));
Assert.AreEqual(new Vector2f(2, 3), m.GetColumn(1));
}
public void Transform(Matrix2x2f m)
{
int numVerts = Vertices.Count;
for (int i = 0; i < numVerts; i++)
{
Vertices[i].Transform(m);
}
}
public void TryInverse()
{
Matrix2x2f m = Random2x2(0);
Matrix2x2f inverse = Matrix2x2f.Identity;
m.TryInverse(ref inverse);
Assert.IsTrue((inverse * Random2x2(0)).EqualsWithError(Matrix2x2f.Identity, 1e-6f));
}
public static void EigenDecomposition(Matrix2x2f m, out float e1, out float e2)
{
// solve the characteristic polynomial
float a = 1.0f;
float b = -(m.m00 + m.m11);
float c = m.m00 * m.m11 - m.m01 * m.m10;
SolveQuadratic(a, b, c, out e1, out e2);
}
public void SetRowt()
{
Matrix2x2f m = new Matrix2x2f();
m.SetRow(0, new Vector2f(0, 2));
m.SetRow(1, new Vector2f(1, 3));
Assert.AreEqual(new Vector2f(0, 2), m.GetRow(0));
Assert.AreEqual(new Vector2f(1, 3), m.GetRow(1));
}
public void SetColumn()
{
Matrix2x2f m = new Matrix2x2f();
m.SetColumn(0, new Vector2f(0, 1));
m.SetColumn(1, new Vector2f(2, 3));
Assert.AreEqual(new Vector2f(0, 1), m.GetColumn(0));
Assert.AreEqual(new Vector2f(2, 3), m.GetColumn(1));
}
public void CreatedFromValues()
{
Matrix2x2f m = new Matrix2x2f(0, 2,
1, 3);
for (int i = 0; i < SIZE; i++)
{
Assert.AreEqual(i, m[i]);
}
}
public void MCreatedFromSingleValue()
{
float v = 1;
Matrix2x2f m = new Matrix2x2f(v);
for (int i = 0; i < SIZE; i++)
{
Assert.AreEqual(v, m[i]);
}
}
public static Matrix2x2f QRDecomposition(Matrix2x2f m)
{
Vector2f a = m.GetColumn(0).Normalized;
Matrix2x2f q = new Matrix2x2f();
q.SetColumn(0, a);
q.SetColumn(1, a.PerpendicularCCW);
return(q);
}
Matrix2x2f Indexed2x2()
{
Matrix2x2f m = new Matrix2x2f();
for (int i = 0; i < SIZE; i++)
{
m[i] = i;
}
return(m);
}
public void CreatedFromArray()
{
float[] d = new float[] { 0, 1, 2, 3 };
Matrix2x2f m = new Matrix2x2f(d);
for (int i = 0; i < SIZE; i++)
{
Assert.AreEqual(i, m[i]);
}
}
float FrobeniusNorm(Matrix2x2f m)
{
float f = 0.0f;
for (int i = 0; i < 2; ++i)
{
for (int j = 0; j < 2; ++j)
{
f += m[i, j] * m[i, j];
}
}
return((float)Math.Sqrt(f));
}
public void Transpose()
{
Matrix2x2f m = new Matrix2x2f();
for (int i = 0; i < HALF_SIZE; i++)
{
for (int j = 0; j < HALF_SIZE; j++)
{
m[j, i] = i + j * HALF_SIZE;
}
}
Assert.AreEqual(Indexed2x2().Transpose, m);
}
public void IndexAssignedTo()
{
Matrix2x2f m = new Matrix2x2f();
for (int i = 0; i < SIZE; i++)
{
m[i] = i;
}
for (int i = 0; i < SIZE; i++)
{
Assert.AreEqual(i, m[i]);
}
}
public void CreatedFromArray2()
{
float[,] d = new float[, ] {
{ 0, 2 },
{ 1, 3 }
};
Matrix2x2f m = new Matrix2x2f(d);
for (int i = 0; i < SIZE; i++)
{
Assert.AreEqual(i, m[i]);
}
}
public static Matrix2x2f PolarDecomposition(Matrix2x2f m)
{
Matrix2x2f q = m + new Matrix2x2f(m.m11, -m.m10, -m.m01, m.m00);
Vector2f c0 = q.GetColumn(0);
Vector2f c1 = q.GetColumn(1);
float s = c0.Magnitude;
q.SetColumn(0, c0 / s);
q.SetColumn(1, c1 / s);
return(q);
}
/// <summary>
/// deformation gradient
/// </summary>
private Matrix2x2f CalcDeformation(Vector2f[] x, Matrix2x2f invM)
{
Vector2f e1 = x[1] - x[0];
Vector2f e2 = x[2] - x[0];
Matrix2x2f m = new Matrix2x2f();
m.SetColumn(0, e1);
m.SetColumn(1, e2);
// mapping from material coordinates to world coordinates
Matrix2x2f f = m * invM;
return(f);
}
Matrix2x2f Random2x2(int seed)
{
Random rnd = new Random(seed);
Matrix2x2f m = new Matrix2x2f();
for (int i = 0; i < HALF_SIZE; i++)
{
for (int j = 0; j < HALF_SIZE; j++)
{
m[i, j] = (float)rnd.NextDouble();
}
}
return(m);
}
public FEMElement(Vector2f[] x)
{
Vector2f e1 = x[1] - x[0];
Vector2f e2 = x[2] - x[0];
Vector2f e3 = x[2] - x[1];
Matrix2x2f m = new Matrix2x2f();
m.SetColumn(0, e1);
m.SetColumn(1, e2);
mInvDm = m.Inverse;
mB = new Vector2f[3];
mB[0] = e3.PerpendicularCCW;
mB[1] = e2.PerpendicularCW;
mB[2] = e1.PerpendicularCCW;
}
public void Index2AssignedTo()
{
Matrix2x2f m = new Matrix2x2f();
for (int i = 0; i < HALF_SIZE; i++)
{
for (int j = 0; j < HALF_SIZE; j++)
{
m[i, j] = i + j * HALF_SIZE;
}
}
for (int i = 0; i < HALF_SIZE; i++)
{
for (int j = 0; j < HALF_SIZE; j++)
{
Assert.AreEqual(i + j * HALF_SIZE, m[i, j]);
}
}
}
/// <summary>
/// Creates matrix fixed.
/// </summary>
/// <param name="m"></param>
/// <returns></returns>
public Float2x2 Fixed(Matrix2x2f m)
{
ConstantOperation c = dag.CreateFixed(m);
return(new Float2x2(c.Outputs[0], this));
}
public virtual void Transform(Matrix2x2f m)
{
}
public override void Transform(Matrix2x2f m)
{
Position = m * Position;
}
/// <summary>
/// calculate Green's non-linear strain tensor
/// </summary>
private Matrix2x2f CalcGreenStrainTensor(Matrix2x2f f)
{
Matrix2x2f e = (f.Transpose * f - Matrix2x2f.Identity) * 0.5f;
return(e);
}
private void UpdateForces(float dt, bool performFracture)
{
for (int i = 0; i < Particles.Count; ++i)
{
Particles[i].max = 0;
if (Particles[i].invMass > 0.0f)
{
Particles[i].f += Gravity / Particles[i].invMass;
}
else
{
Particles[i].f += Vector2f.Zero - Drag * Particles[i].v;
}
}
Vector2f[] x = new Vector2f[3];
Vector2f[] v = new Vector2f[3];
for (int i = 0; i < Triangles.Count; ++i)
{
Triangle tri = Triangles[i];
FEMElement elem = Elements[i];
x[0] = Particles[tri.i].p;
x[1] = Particles[tri.j].p;
x[2] = Particles[tri.k].p;
v[0] = Particles[tri.i].v;
v[1] = Particles[tri.j].v;
v[2] = Particles[tri.k].v;
if (performFracture)
{
Matrix2x2f f = CalcDeformation(x, elem.mInvDm);
Matrix2x2f q = Decomposition2x2f.QRDecomposition(f);
// strain
Matrix2x2f e = CalcCauchyStrainTensor(q.Transpose * f);
// update plastic strain
float ef = FrobeniusNorm(e);
if (ef > Yield)
{
elem.mEp += e * dt * Creep;
}
const float epmax = 0.6f;
if (ef > epmax)
{
elem.mEp *= epmax / ef;
}
// adjust strain
e -= elem.mEp;
Matrix2x2f s = CalcStressTensor(e, LameLambda, LameMu);
// damping forces
Matrix2x2f dfdt = CalcDeformation(v, elem.mInvDm);
Matrix2x2f dedt = CalcCauchyStrainTensorDt(q.Transpose * dfdt);
Matrix2x2f dsdt = CalcStressTensor(dedt, Damping, Damping);
Matrix2x2f p = s + dsdt;
float e1, e2;
Decomposition2x2f.EigenDecomposition(p, out e1, out e2);
float me = Mathf.Max(e1, e2);
if (me > Toughness)
{
// calculate Eigenvector corresponding to max Eigenvalue
Vector2f ev = q * (new Vector2f(p.m01, me - p.m00)).Normalized;
// pick a random vertex to split on
int splitNode = Rnd.Next(0, 2);
// don't fracture immovable nodes
if (Particles[GetVertex(tri, splitNode)].invMass == 0.0f)
{
break;
}
// fracture plane perpendicular to ev
Vector3f plane = new Vector3f(ev.x, ev.y, -Vector2f.Dot(ev, Particles[GetVertex(tri, splitNode)].p));
FEMFractureEvent fracture = new FEMFractureEvent();
fracture.Tri = i;
fracture.Node = splitNode;
fracture.Plane = plane;
//Fracture not implemented so these fracture planes are not used.
Fractures.Add(fracture);
}
// calculate force on each edge due to stress and distribute to the nodes
Vector2f f1 = q * p * elem.mB[0];
Vector2f f2 = q * p * elem.mB[1];
Vector2f f3 = q * p * elem.mB[2];
Particles[tri.i].f -= f1 / 3.0f;
Particles[tri.j].f -= f2 / 3.0f;
Particles[tri.k].f -= f3 / 3.0f;
}
else
{
//This was the code used when fracturing was disabled
//in the original. It seems very unstable for me. maybe
//a bug or precision issue. Not used atm.
Matrix2x2f f = CalcDeformation(x, elem.mInvDm);
// elastic forces
Matrix2x2f e = CalcGreenStrainTensor(f);
Matrix2x2f s = CalcStressTensor(e, LameLambda, LameMu);
// damping forces
Matrix2x2f dfdt = CalcDeformation(v, elem.mInvDm);
Matrix2x2f dedt = CalcGreenStrainTensorDt(f, dfdt);
Matrix2x2f dsdt = CalcStressTensor(dedt, Damping, Damping);
Matrix2x2f p = s + dsdt;
float e1, e2;
Decomposition2x2f.EigenDecomposition(p, out e1, out e2);
float me = Mathf.Max(e1, e2);
Matrix2x2f finv = f.Transpose.Inverse;
Vector2f f1 = p * (finv * elem.mB[0]);
Vector2f f2 = p * (finv * elem.mB[1]);
Vector2f f3 = p * (finv * elem.mB[2]);
Particles[tri.i].f -= f1 / 3.0f;
Particles[tri.j].f -= f2 / 3.0f;
Particles[tri.k].f -= f3 / 3.0f;
Particles[tri.i].max += me / 3.0f;
Particles[tri.j].max += me / 3.0f;
Particles[tri.k].max += me / 3.0f;
}
}
}
/// <summary>
/// calculate isotropic Hookean stress tensor, lambda and mu are the Lame parameters
/// </summary>
private Matrix2x2f CalcStressTensor(Matrix2x2f e, float lambda, float mu)
{
Matrix2x2f s = Matrix2x2f.Identity * e.Trace * lambda + e * mu * 2.0f;
return(s);
}
/// <summary>
/// calculate time derivative of Cauchy's strain tensor
/// </summary>
private Matrix2x2f CalcCauchyStrainTensorDt(Matrix2x2f dfdt)
{
Matrix2x2f e = (dfdt + dfdt.Transpose) * 0.5f;
return(e);
}
/// <summary>
/// calculate Cauchy's linear strain tensor
/// </summary>
private Matrix2x2f CalcCauchyStrainTensor(Matrix2x2f f)
{
Matrix2x2f e = (f + f.Transpose) * 0.5f - Matrix2x2f.Identity;
return(e);
}
/// <summary>
/// calculate time derivative of Green's strain
/// </summary>
private Matrix2x2f CalcGreenStrainTensorDt(Matrix2x2f f, Matrix2x2f dfdt)
{
Matrix2x2f e = (f * dfdt.Transpose + dfdt * f.Transpose) * 0.5f;
return(e);
}