public void Add3(int id0, int id1, int id2, int id3, Vector[] lcoords, Vector[] lforces)
{
if (forceij == null)
{
return;
}
Vector ud01 = (lcoords[1] - lcoords[0]).UnitVector();
Vector ud02 = (lcoords[2] - lcoords[0]).UnitVector();
Vector ud03 = (lcoords[3] - lcoords[0]).UnitVector();
Vector ud12 = (lcoords[2] - lcoords[1]).UnitVector();
Vector ud13 = (lcoords[3] - lcoords[1]).UnitVector();
Vector ud23 = (lcoords[3] - lcoords[2]).UnitVector();
// f01 f02 f03 f12 f13 f23
// [ ud01_x, ud02_x, ud03_x, 0, 0, 0] [f01] [lf0x]
// [-ud01_x, 0, 0, ud12_x, ud13_x, 0] [f02] [lf1x]
// [ 0, -ud02_x, 0, -ud12_x, 0, ud23_x] * [f03] = [lf2x]
// [ 0, 0, -ud03_x, 0, -ud13_x, -ud23_x] * [f03] = [lf3x]
// [f12]
// [ ud01_y, ud02_y, ud03_y, 0, 0, 0] [f13] [lf0y]
// [-ud01_y, 0, 0, ud12_y, ud13_y, 0] [f23] [lf1y]
// [ 0, -ud02_y, 0, -ud12_y, 0, ud23_y] [lf2y]
// [ 0, 0, -ud03_y, 0, -ud13_y, -ud23_y] [lf3y]
//
// [ ud01_z, ud02_z, ud03_z, 0, 0, 0] [lf0z]
// [-ud01_z, 0, 0, ud12_z, ud13_z, 0] [lf1z]
// [ 0, -ud02_z, 0, -ud12_z, 0, ud23_z] [lf2z]
// [ 0, 0, -ud03_z, 0, -ud13_z, -ud23_z] [lf3z]
/////////////////////////////////////////////////
// A * x = b
MatrixByArr A = new double[12, 6];
Vector b = new double[12];
for (int i = 0; i < 3; i++)
{
A[i + 3 * 0, 0] = ud01[i]; A[i + 3 * 0, 1] = ud02[i]; A[i + 3 * 0, 2] = ud03[i]; A[i + 3 * 0, 3] = 0; A[i + 3 * 0, 4] = 0; A[i + 3 * 0, 5] = 0; b[i + 3 * 0] = lforces[0][i];
A[i + 3 * 1, 0] = -ud01[i]; A[i + 3 * 1, 1] = 0; A[i + 3 * 1, 2] = 0; A[i + 3 * 1, 3] = ud12[i]; A[i + 3 * 1, 4] = ud13[i]; A[i + 3 * 1, 5] = 0; b[i + 3 * 1] = lforces[1][i];
A[i + 3 * 2, 0] = 0; A[i + 3 * 2, 1] = -ud02[i]; A[i + 3 * 2, 2] = 0; A[i + 3 * 2, 3] = -ud12[i]; A[i + 3 * 2, 4] = 0; A[i + 3 * 2, 5] = ud23[i]; b[i + 3 * 2] = lforces[2][i];
A[i + 3 * 3, 0] = 0; A[i + 3 * 3, 1] = 0; A[i + 3 * 3, 2] = -ud03[i]; A[i + 3 * 3, 3] = 0; A[i + 3 * 3, 4] = -ud13[i]; A[i + 3 * 3, 5] = -ud23[i]; b[i + 3 * 3] = lforces[3][i];
}
InfoPack extra = HDebug.IsDebuggerAttached ? new InfoPack() : null;
Matrix pinvA = NumericSolver.Pinv(A, extra);
HDebug.Assert(extra.GetValueInt("rank") == 6);
Vector x = LinAlg.MV(pinvA, b);
Vector f01 = ud01 * x[0];
Vector f02 = ud02 * x[1];
Vector f03 = ud03 * x[2];
Vector f12 = ud12 * x[3];
Vector f13 = ud13 * x[4];
Vector f23 = ud23 * x[5];
if (HDebug.IsDebuggerAttached)
{
// check net force
Vector sumforces = lforces[0];
for (int i = 1; i < lforces.Length; i++)
{
sumforces += lforces[i];
}
HDebug.AssertTolerance(0.00000001, sumforces);
Vector f0 = f01 + f02 + f03;
Vector f1 = -f01 + f12 + f13;
Vector f2 = -f02 - f12 + f23;
Vector f3 = -f03 - f13 - f23;
HDebug.AssertTolerance(0.00000001, f0 - lforces[0]);
HDebug.AssertTolerance(0.00000001, f1 - lforces[1]);
HDebug.AssertTolerance(0.00000001, f2 - lforces[2]);
HDebug.AssertTolerance(0.00000001, f3 - lforces[3]);
}
this[id0, id1] += f01; this[id0, id2] += f02; this[id0, id3] += f03; this[id1, id2] += f12; this[id1, id3] += f13; this[id2, id3] += f23;
this[id1, id0] += -f01; this[id2, id0] += -f02; this[id3, id0] += -f03; this[id2, id1] += -f12; this[id3, id1] += -f13; this[id3, id2] += -f23;
}
public void Add3(int id0, int id1, int id2, Vector[] lcoords, Vector[] lforces)
{
if (forceij == null)
{
return;
}
Vector ud01 = (lcoords[1] - lcoords[0]).UnitVector();
Vector ud12 = (lcoords[2] - lcoords[1]).UnitVector();
Vector ud20 = (lcoords[0] - lcoords[2]).UnitVector();
// [ ud01_x, 0, -ud20_x] [f01] [lf0x]
// [-ud01_x, ud12_x, 0] [f12] [lf1x]
// [ 0, -ud12_x, ud20_x] * [f20] = [lf2x]
//
// [ ud01_y, 0, -ud20_y] [lf0y]
// [-ud01_y, ud12_y, 0] [lf1y]
// [ 0, -ud12_y, ud20_y] [lf2y]
//
// [ ud01_z, 0, -ud20_z] [lf0z]
// [-ud01_z, ud12_z, 0] [lf1z]
// [ 0, -ud12_z, ud20_z] [lf2z]
/////////////////////////////////////////////////
// A * x = b
MatrixByArr A = new double[9, 3];
Vector b = new double[9];
for (int i = 0; i < 3; i++)
{
A[i + 3 * 0, 0] = ud01[i]; A[i + 3 * 0, 1] = 0; A[i + 3 * 0, 2] = -ud20[i]; b[i + 3 * 0] = lforces[0][i];
A[i + 3 * 1, 0] = -ud01[i]; A[i + 3 * 1, 1] = ud12[i]; A[i + 3 * 1, 2] = 0; b[i + 3 * 1] = lforces[1][i];
A[i + 3 * 2, 0] = 0; A[i + 3 * 2, 1] = -ud12[i]; A[i + 3 * 2, 2] = ud20[i]; b[i + 3 * 2] = lforces[2][i];
}
InfoPack extra = HDebug.IsDebuggerAttached ? new InfoPack() : null;
Matrix pinvA = NumericSolver.Pinv(A, extra);
HDebug.Assert(extra.GetValueInt("rank") == 3);
Vector x = LinAlg.MV(pinvA, b);
Vector f01 = ud01 * x[0];
Vector f12 = ud12 * x[1];
Vector f20 = ud20 * x[2];
if (HDebug.IsDebuggerAttached)
{
// check net force
Vector sumforces = lforces[0];
for (int i = 1; i < lforces.Length; i++)
{
sumforces += lforces[i];
}
HDebug.AssertTolerance(0.00000001, sumforces);
Vector f0 = f01 - f20;
Vector f1 = f12 - f01;
Vector f2 = -f12 + f20;
HDebug.AssertTolerance(0.00000001, f0 - lforces[0]);
HDebug.AssertTolerance(0.00000001, f1 - lforces[1]);
HDebug.AssertTolerance(0.00000001, f2 - lforces[2]);
}
this[id0, id1] += f01; this[id1, id2] += f12; this[id2, id0] += f20;
this[id1, id0] += -f01; this[id2, id1] += -f12; this[id0, id2] += -f20;
}
