Forces.cpp
#include "Forces.h"
#ifdef EOLC_ONLINE
#define GLEW_STATIC
#include <GL/glew.h>
#include <GLFW/glfw3.h>
#define GLM_FORCE_RADIANS
#include <glm/glm.hpp>
#include <glm/gtc/type_ptr.hpp>
#include "glm/ext.hpp"
#include "online/Program.h"
#include "online/MatrixStack.h"
#endif // EOLC_ONLINE
#include "conversions.h"
#include "UtilEOL.h"
#include "ComputeBending.h"
#include "ComputeInertial.h"
#include "ComputeMembrane.h"
#include "external\ArcSim\util.hpp"
#include <iostream>
#include <utility>
using namespace std;
using namespace Eigen;
typedef Eigen::Triplet<double> T;
Matrix2d poldec(Matrix2d M) {
double m11 = M(0, 0);
double m12 = M(0, 1);
double m21 = M(1, 0);
double m22 = M(1, 1);
double detM = m11*m22 - m12*m21;
int sign = 1;
if (detM < 0) {
sign = -1;
}
else if (detM == 0) {
sign = 0;
}
Matrix2d MM;
MM << m22, -m21, -m12, m11;
Matrix2d Q = M + sign*MM;
double clen = Q.col(0).norm();
Q = Q / clen;
return Q;
}
void faceBasedF(const Mesh& mesh, VectorXd& f, vector<T>& M_, vector<T>& MDK_, const Vector3d& grav, double h)
{
for (int i = 0; i < mesh.faces.size(); i++) {
Face* face = mesh.faces[i];
double xa[3], xb[3], xc[3];
double Xa[2], Xb[2], Xc[2];
double g[3], PP[6], QQ[4];
double Wi[1], Wm[1];
const Material* mat = face->material;
Vector3d txa = v2e(face->v[0]->node->x),
txb = v2e(face->v[1]->node->x),
txc = v2e(face->v[2]->node->x);
Vector2d tXa = v322e(face->v[0]->u),
tXb = v322e(face->v[1]->u),
tXc = v322e(face->v[2]->u);
Map<Vector3d>(xa, 3) = txa;
Map<Vector3d>(xb, 3) = txb;
Map<Vector3d>(xc, 3) = txc;
Map<Vector2d>(Xa, 2) = tXa;
Map<Vector2d>(Xb, 2) = tXb;
Map<Vector2d>(Xc, 2) = tXc;
Map<Vector3d>(g, grav.rows(), grav.cols()) = grav;
MatrixXd Dxt(3, 2);
MatrixXd DX(2, 2);
Dxt << (txb - txa), (txc - txa);
DX << (tXb - tXa), (tXc - tXa);
Vector3d normm = (txb - txa).cross(txc - txa);
Vector3d Pxm = (txb - txa) / (txb - txa).norm();
Vector3d Pym = normm.cross(Pxm);
Pym = Pym / Pym.norm();
MatrixXd Pm(2, 3);
Pm << Pxm.transpose(), Pym.transpose();
MatrixXd Fm = Dxt * DX.inverse();
Matrix2d Fbarm = Pm*Fm;
Matrix2d Qm = poldec(Fbarm);
Map<MatrixXd>(PP, Pm.rows(), Pm.cols()) = Pm;
Map<Matrix2d>(QQ, Qm.rows(), Qm.cols()) = Qm;
int aindex = face->v[0]->node->index * 3;
int bindex = face->v[1]->node->index * 3;
int cindex = face->v[2]->node->index * 3;
int aindexX = mesh.nodes.size() * 3 + face->v[0]->node->EoL_index * 2;
int bindexX = mesh.nodes.size() * 3 + face->v[1]->node->EoL_index * 2;
int cindexX = mesh.nodes.size() * 3 + face->v[2]->node->EoL_index * 2;
double fi[9], fm[9], Mi[81], Km[81];
VectorXd fie(15), fme(15);
MatrixXd Mie(15, 15), Kme(15, 15);
ComputeInertial(xa, xb, xc, Xa, Xb, Xc, g, mat->density, Wi, fi, Mi);
ComputeMembrane(xa, xb, xc, Xa, Xb, Xc, mat->e, mat->nu, PP, QQ, Wm, fm, Km);
fie.segment<9>(0) = Map<VectorXd>(fi, 9);
fme.segment<9>(0) = Map<VectorXd>(fm, 9);
Mie.block<9, 9>(0, 0) = Map<MatrixXd>(Mi, 9, 9);
Kme.block<9, 9>(0, 0) = Map<MatrixXd>(Km, 9, 9);
Vector2d damping(mat->dampingA, mat->dampingB);
if (face->v[0]->node->EoL || face->v[1]->node->EoL || face->v[2]->node->EoL) {
MatrixXd F = deform_grad(face);
int ja = 0; int jA = 3; int jb = 5; int jB = 8; int jc = 10; int jC = 13;
Matrix3d Kmaa = Kme.block(0, 0, 3, 3); Matrix3d Kmab = Kme.block(0, jb, 3, 3); Matrix3d Kmac = Kme.block(0, jc, 3, 3);
Matrix3d Kmba = Kme.block(3, 0, 3, 3); Matrix3d Kmbb = Kme.block(3, 3, 3, 3); Matrix3d Kmbc = Kme.block(3, 6, 3, 3);
Matrix3d Kmca = Kme.block(6, 0, 3, 3); Matrix3d Kmcb = Kme.block(6, 3, 3, 3); Matrix3d Kmcc = Kme.block(6, 6, 3, 3);
Kme.block(ja, jA, 3, 2) = -Kmaa * F; Kme.block(ja, jB, 3, 2) = -Kmab * F; Kme.block(ja, jC, 3, 2) = -Kmac * F;
Kme.block(jb, jA, 3, 2) = -Kmba * F; Kme.block(jb, jB, 3, 2) = -Kmbb * F; Kme.block(jb, jC, 3, 2) = -Kmbc * F;
Kme.block(jc, jA, 3, 2) = -Kmca * F; Kme.block(jc, jB, 3, 2) = -Kmcb * F; Kme.block(jc, jC, 3, 2) = -Kmcc * F;
Kme.block(jA, ja, 2, 3) = -F.transpose() * Kmaa; Kme.block(jA, jb, 2, 3) = -F.transpose() * Kmab; Kme.block(jA, jc, 2, 3) = -F.transpose() * Kmac;
Kme.block(jB, ja, 2, 3) = -F.transpose() * Kmba; Kme.block(jB, jb, 2, 3) = -F.transpose() * Kmbb; Kme.block(jB, jc, 2, 3) = -F.transpose() * Kmbc;
Kme.block(jC, ja, 2, 3) = -F.transpose() * Kmca; Kme.block(jC, jb, 2, 3) = -F.transpose() * Kmcb; Kme.block(jC, jc, 2, 3) = -F.transpose() * Kmcc;
Kme.block(jA, jA, 2, 2) = F.transpose() * Kmaa * F; Kme.block(jA, jB, 2, 2) = F.transpose() * Kmab * F; Kme.block(jA, jC, 2, 2) = F.transpose() * Kmac * F;
Kme.block(jB, jA, 2, 2) = F.transpose() * Kmba * F; Kme.block(jB, jB, 2, 2) = F.transpose() * Kmbb * F; Kme.block(jB, jC, 2, 2) = F.transpose() * Kmbc * F;
Kme.block(jC, jA, 2, 2) = F.transpose() * Kmca * F; Kme.block(jC, jB, 2, 2) = F.transpose() * Kmcb * F; Kme.block(jC, jC, 2, 2) = F.transpose() * Kmcc * F;
// x values
Matrix3d Maa = Mie.block(0, 0, 3, 3);
Matrix3d Kaa = Kme.block(0, 0, 3, 3);
Matrix3d MDKaa = Maa + damping(0) * h * Maa + damping(1) * h * h * Kaa;
f.segment<3>(face->v[0]->node->index * 3) += (fie.segment<3>(0) + fme.segment<3>(0));
if (face->v[0]->node->EoL_index != -1) f.segment<2>(aindexX) += -F.transpose() * (fie.segment<3>(0) + fme.segment<3>(0));
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
M_.push_back(T(aindex + j, aindex + k, Maa(j, k)));
MDK_.push_back(T(aindex + j, aindex + k, MDKaa(j, k)));
}
}
Matrix3d Mbb = Mie.block(5, 5, 3, 3);
Matrix3d Kbb = Kme.block(5, 5, 3, 3);
Matrix3d MDKbb = Mbb + damping(0) * h * Mbb + damping(1) * h * h * Kbb;
f.segment<3>(face->v[1]->node->index * 3) += (fie.segment<3>(5) + fme.segment<3>(5));
if (face->v[1]->node->EoL_index != -1)f.segment<2>(bindexX) += -F.transpose() * (fie.segment<3>(5) + fme.segment<3>(5));
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
M_.push_back(T(bindex + j, bindex + k, Mbb(j, k)));
MDK_.push_back(T(bindex + j, bindex + k, MDKbb(j, k)));
}
}
Matrix3d Mcc = Mie.block(10, 10, 3, 3);
Matrix3d Kcc = Kme.block(10, 10, 3, 3);
Matrix3d MDKcc = Mcc + damping(0) * h * Mcc + damping(1) * h * h * Kcc;
f.segment<3>(face->v[2]->node->index * 3) += (fie.segment<3>(10) + fme.segment<3>(10));
if (face->v[2]->node->EoL_index != -1) f.segment<2>(cindexX) += -F.transpose() * (fie.segment<3>(10) + fme.segment<3>(10));
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
M_.push_back(T(cindex + j, cindex + k, Mcc(j, k)));
MDK_.push_back(T(cindex + j, cindex + k, MDKcc(j, k)));
}
}
Matrix3d Mab = Mie.block(0, 5, 3, 3);
Matrix3d Kab = Kme.block(0, 5, 3, 3);
Matrix3d MDKab = Mab + damping(0) * h * Mab + damping(1) * h * h * Kab;
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
M_.push_back(T(aindex + j, bindex + k, Mab(j, k)));
M_.push_back(T(bindex + k, aindex + j, Mab(j, k)));
MDK_.push_back(T(aindex + j, bindex + k, MDKab(j, k)));
MDK_.push_back(T(bindex + k, aindex + j, MDKab(j, k)));
}
}
Matrix3d Mac = Mie.block(0, 10, 3, 3);
Matrix3d Kac = Kme.block(0, 10, 3, 3);
Matrix3d MDKac = Mac + damping(0) * h * Mac + damping(1) * h * h * Kac;
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
M_.push_back(T(aindex + j, cindex + k, Mac(j, k)));
M_.push_back(T(cindex + k, aindex + j, Mac(j, k)));
MDK_.push_back(T(aindex + j, cindex + k, MDKac(j, k)));
MDK_.push_back(T(cindex + k, aindex + j, MDKac(j, k)));
}
}
Matrix3d Mbc = Mie.block(5, 10, 3, 3);
Matrix3d Kbc = Kme.block(5, 10, 3, 3);
Matrix3d MDKbc = Mbc + damping(0) * h * Mbc + damping(1) * h * h * Kbc;
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
M_.push_back(T(bindex + j, cindex + k, Mbc(j, k)));
M_.push_back(T(cindex + k, bindex + j, Mbc(j, k)));
MDK_.push_back(T(bindex + j, cindex + k, MDKbc(j, k)));
MDK_.push_back(T(cindex + k, bindex + j, MDKbc(j, k)));
}
}
// X values
if (face->v[0]->node->EoL_index != -1) {
Matrix2d MAA = Mie.block(3, 3, 2, 2);
Matrix2d KAA = Kme.block(3, 3, 2, 2);
Matrix2d MDKAA = MAA + damping(0) * h * MAA + damping(1) * h * h * KAA;
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 2; k++) {
M_.push_back(T(aindexX + j, aindexX + k, MAA(j, k)));
MDK_.push_back(T(aindexX + j, aindexX + k, MDKAA(j, k)));
}
}
}
if (face->v[1]->node->EoL_index != -1) {
Matrix2d MBB = Mie.block(8, 8, 2, 2);
Matrix2d KBB = Kme.block(8, 8, 2, 2);
Matrix2d MDKBB = MBB + damping(0) * h * MBB + damping(1) * h * h * KBB;
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 2; k++) {
M_.push_back(T(bindexX + j, bindexX + k, MBB(j, k)));
MDK_.push_back(T(bindexX + j, bindexX + k, MDKBB(j, k)));
}
}
}
if (face->v[2]->node->EoL_index != -1) {
Matrix2d MCC = Mie.block(13, 13, 2, 2);
Matrix2d KCC = Kme.block(13, 13, 2, 2);
Matrix2d MDKCC = MCC + damping(0) * h * MCC + damping(1) * h * h * KCC;
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 2; k++) {
M_.push_back(T(cindexX + j, cindexX + k, MCC(j, k)));
MDK_.push_back(T(cindexX + j, cindexX + k, MDKCC(j, k)));
}
}
}
if (face->v[0]->node->EoL_index != -1 && face->v[1]->node->EoL_index != -1) {
Matrix2d MAB = Mie.block(3, 8, 2, 2);
Matrix2d KAB = Kme.block(3, 8, 2, 2);
Matrix2d MDKAB = MAB + damping(0) * h * MAB + damping(1) * h * h * KAB;
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 2; k++) {
M_.push_back(T(aindexX + j, bindexX + k, MAB(j, k)));
M_.push_back(T(bindexX + k, aindexX + j, MAB(j, k)));
MDK_.push_back(T(aindexX + j, bindexX + k, MDKAB(j, k)));
MDK_.push_back(T(bindexX + k, aindexX + j, MDKAB(j, k)));
}
}
}
if (face->v[0]->node->EoL_index != -1 && face->v[2]->node->EoL_index != -1) {
Matrix2d MAC = Mie.block(3, 13, 2, 2);
Matrix2d KAC = Kme.block(3, 13, 2, 2);
Matrix2d MDKAC = MAC + damping(0) * h * MAC + damping(1) * h * h * KAC;
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 2; k++) {
M_.push_back(T(aindexX + j, cindexX + k, MAC(j, k)));
M_.push_back(T(cindexX + k, aindexX + j, MAC(j, k)));
MDK_.push_back(T(aindexX + j, cindexX + k, MDKAC(j, k)));
MDK_.push_back(T(cindexX + k, aindexX + j, MDKAC(j, k)));
}
}
}
if (face->v[1]->node->EoL_index != -1 && face->v[2]->node->EoL_index != -1) {
Matrix2d MBC = Mie.block(8, 13, 2, 2);
Matrix2d KBC = Kme.block(8, 13, 2, 2);
Matrix2d MDKBC = MBC + damping(0) * h * MBC + damping(1) * h * h * KBC;
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 2; k++) {
M_.push_back(T(bindexX + j, cindexX + k, MBC(j, k)));
M_.push_back(T(cindexX + k, bindexX + j, MBC(j, k)));
MDK_.push_back(T(bindexX + j, cindexX + k, MDKBC(j, k)));
MDK_.push_back(T(cindexX + k, bindexX + j, MDKBC(j, k)));
}
}
}
// x-X values
if (face->v[0]->node->EoL_index != -1) {
MatrixXd MAa = Mie.block(3, 0, 2, 3);
MatrixXd KAa = Kme.block(3, 0, 2, 3);
MatrixXd MDKAa = MAa + damping(0) * h * MAa + damping(1) * h * h * KAa;
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 3; k++) {
M_.push_back(T(aindexX + j, aindex + k, MAa(j, k)));
M_.push_back(T(aindex + k, aindexX + j, MAa(j, k)));
MDK_.push_back(T(aindexX + j, aindex + k, MDKAa(j, k)));
MDK_.push_back(T(aindex + k, aindexX + j, MDKAa(j, k)));
}
}
MatrixXd MAb = Mie.block(3, 5, 2, 3);
MatrixXd KAb = Kme.block(3, 5, 2, 3);
MatrixXd MDKAb = MAb + damping(0) * h * MAb + damping(1) * h * h * KAb;
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 3; k++) {
M_.push_back(T(aindexX + j, bindex + k, MAb(j, k)));
M_.push_back(T(bindex + k, aindexX + j, MAb(j, k)));
MDK_.push_back(T(aindexX + j, bindex + k, MDKAb(j, k)));
MDK_.push_back(T(bindex + k, aindexX + j, MDKAb(j, k)));
}
}
MatrixXd MAc = Mie.block(3, 10, 2, 3);
MatrixXd KAc = Kme.block(3, 10, 2, 3);
MatrixXd MDKAc = MAc + damping(0) * h * MAc + damping(1) * h * h * KAc;
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 3; k++) {
M_.push_back(T(aindexX + j, cindex + k, MAc(j, k)));
M_.push_back(T(cindex + k, aindexX + j, MAc(j, k)));
MDK_.push_back(T(aindexX + j, cindex + k, MDKAc(j, k)));
MDK_.push_back(T(cindex + k, aindexX + j, MDKAc(j, k)));
}
}
}
if (face->v[1]->node->EoL_index != -1) {
MatrixXd MBa = Mie.block(8, 0, 2, 3);
MatrixXd KBa = Kme.block(8, 0, 2, 3);
MatrixXd MDKBa = MBa + damping(0) * h * MBa + damping(1) * h * h * KBa;
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 3; k++) {
M_.push_back(T(bindexX + j, aindex + k, MBa(j, k)));
M_.push_back(T(aindex + k, bindexX + j, MBa(j, k)));
MDK_.push_back(T(bindexX + j, aindex + k, MDKBa(j, k)));
MDK_.push_back(T(aindex + k, bindexX + j, MDKBa(j, k)));
}
}
MatrixXd MBb = Mie.block(8, 5, 2, 3);
MatrixXd KBb = Kme.block(8, 5, 2, 3);
MatrixXd MDKBb = MBb + damping(0) * h * MBb + damping(1) * h * h * KBb;
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 3; k++) {
M_.push_back(T(bindexX + j, bindex + k, MBb(j, k)));
M_.push_back(T(bindex + k, bindexX + j, MBb(j, k)));
MDK_.push_back(T(bindexX + j, bindex + k, MDKBb(j, k)));
MDK_.push_back(T(bindex + k, bindexX + j, MDKBb(j, k)));
}
}
MatrixXd MBc = Mie.block(8, 10, 2, 3);
MatrixXd KBc = Kme.block(8, 10, 2, 3);
MatrixXd MDKBc = MBc + damping(0) * h * MBc + damping(1) * h * h * KBc;
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 3; k++) {
M_.push_back(T(bindexX + j, cindex + k, MBc(j, k)));
M_.push_back(T(cindex + k, bindexX + j, MBc(j, k)));
MDK_.push_back(T(bindexX + j, cindex + k, MDKBc(j, k)));
MDK_.push_back(T(cindex + k, bindexX + j, MDKBc(j, k)));
}
}
}
if (face->v[2]->node->EoL_index != -1) {
MatrixXd MCa = Mie.block(13, 0, 2, 3);
MatrixXd KCa = Kme.block(13, 0, 2, 3);
MatrixXd MDKCa = MCa + damping(0) * h * MCa + damping(1) * h * h * KCa;
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 3; k++) {
M_.push_back(T(cindexX + j, aindex + k, MCa(j, k)));
M_.push_back(T(aindex + k, cindexX + j, MCa(j, k)));
MDK_.push_back(T(cindexX + j, aindex + k, MDKCa(j, k)));
MDK_.push_back(T(aindex + k, cindexX + j, MDKCa(j, k)));
}
}
MatrixXd MCb = Mie.block(13, 5, 2, 3);
MatrixXd KCb = Kme.block(13, 5, 2, 3);
MatrixXd MDKCb = MCb + damping(0) * h * MCb + damping(1) * h * h * KCb;
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 3; k++) {
M_.push_back(T(cindexX + j, bindex + k, MCb(j, k)));
M_.push_back(T(bindex + k, cindexX + j, MCb(j, k)));
MDK_.push_back(T(cindexX + j, bindex + k, MDKCb(j, k)));
MDK_.push_back(T(bindex + k, cindexX + j, MDKCb(j, k)));
}
}
MatrixXd MCc = Mie.block(13, 10, 2, 3);
MatrixXd KCc = Kme.block(13, 10, 2, 3);
MatrixXd MDKCc = MCc + damping(0) * h * MCc + damping(1) * h * h * KCc;
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 3; k++) {
M_.push_back(T(cindexX + j, cindex + k, MCc(j, k)));
M_.push_back(T(cindex + k, cindexX + j, MCc(j, k)));
MDK_.push_back(T(cindexX + j, cindex + k, MDKCc(j, k)));
MDK_.push_back(T(cindex + k, cindexX + j, MDKCc(j, k)));
}
}
}
}
else {
Matrix3d Maa = Mie.block(0, 0, 3, 3);
Matrix3d Kaa = Kme.block(0, 0, 3, 3);
Matrix3d MDKaa = Maa + damping(0) * h * Maa + damping(1) * h * h * Kaa;
f.segment<3>(aindex) += (fie.segment<3>(0) + fme.segment<3>(0));
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
M_.push_back(T(aindex + j, aindex + k, Maa(j, k)));
MDK_.push_back(T(aindex + j, aindex + k, MDKaa(j, k)));
}
}
Matrix3d Mbb = Mie.block(3, 3, 3, 3);
Matrix3d Kbb = Kme.block(3, 3, 3, 3);
Matrix3d MDKbb = Mbb + damping(0) * h * Mbb + damping(1) * h * h * Kbb;
f.segment<3>(bindex) += (fie.segment<3>(3) + fme.segment<3>(3));
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
M_.push_back(T(bindex + j, bindex + k, Mbb(j, k)));
MDK_.push_back(T(bindex + j, bindex + k, MDKbb(j, k)));
}
}
Matrix3d Mcc = Mie.block(6, 6, 3, 3);
Matrix3d Kcc = Kme.block(6, 6, 3, 3);
Matrix3d MDKcc = Mcc + damping(0) * h * Mcc + damping(1) * h * h * Kcc;
f.segment<3>(cindex) += (fie.segment<3>(6) + fme.segment<3>(6));
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
M_.push_back(T(cindex + j, cindex + k, Mcc(j, k)));
MDK_.push_back(T(cindex + j, cindex + k, MDKcc(j, k)));
}
}
Matrix3d Mab = Mie.block(0, 3, 3, 3);
Matrix3d Kab = Kme.block(0, 3, 3, 3);
Matrix3d MDKab = Mab + damping(0) * h * Mab + damping(1) * h * h * Kab;
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
M_.push_back(T(aindex + j, bindex + k, Mab(j, k)));
M_.push_back(T(bindex + k, aindex + j, Mab(j, k)));
MDK_.push_back(T(aindex + j, bindex + k, MDKab(j, k)));
MDK_.push_back(T(bindex + k, aindex + j, MDKab(j, k)));
}
}
Matrix3d Mac = Mie.block(0, 6, 3, 3);
Matrix3d Kac = Kme.block(0, 6, 3, 3);
Matrix3d MDKac = Mac + damping(0) * h * Mac + damping(1) * h * h * Kac;
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
M_.push_back(T(aindex + j, cindex + k, Mac(j, k)));
M_.push_back(T(cindex + k, aindex + j, Mac(j, k)));
MDK_.push_back(T(aindex + j, cindex + k, MDKac(j, k)));
MDK_.push_back(T(cindex + k, aindex + j, MDKac(j, k)));
}
}
Matrix3d Mbc = Mie.block(3, 6, 3, 3);
Matrix3d Kbc = Kme.block(3, 6, 3, 3);
Matrix3d MDKbc = Mbc + damping(0) * h * Mbc + damping(1) * h * h * Kbc;
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
M_.push_back(T(bindex + j, cindex + k, Mbc(j, k)));
M_.push_back(T(cindex + k, bindex + j, Mbc(j, k)));
MDK_.push_back(T(bindex + j, cindex + k, MDKbc(j, k)));
MDK_.push_back(T(cindex + k, bindex + j, MDKbc(j, k)));
}
}
}
}
}
void edgeBasedF(const Mesh& mesh, const Material& mat, VectorXd& f, vector<T>& M_, vector<T>& MDK_, double h)
{
for (int e = 0; e < mesh.edges.size(); e++) {
if (mesh.edges[e]->adjf[0] == NULL || mesh.edges[e]->adjf[1] == NULL) {
continue;
}
Edge* edge = mesh.edges[e];
Face* f0 = edge->adjf[0], *f1 = edge->adjf[1];
Node* n0 = edge->n[0], *n1 = edge->n[1];
Vert* v0 = n0->verts[0], *v1 = n1->verts[0];
Vert* v2 = get_other_vert(f0, v0, v1), *v3 = get_other_vert(f1, v0, v1);
Node* n2 = v2->node, *n3 = v3->node;
double xa[3], xb[3], xc[3], xd[3];
double Xa[2], Xb[2], Xc[2], Xd[2];
Vector3d txa = v2e(n0->x),
txb = v2e(n1->x),
txc = v2e(n2->x),
txd = v2e(n3->x);
Vector2d tXa = v322e(v0->u),
tXb = v322e(v1->u),
tXc = v322e(v2->u),
tXd = v322e(v3->u);
Map<Vector3d>(xa, 3) = txa;
Map<Vector3d>(xb, 3) = txb;
Map<Vector3d>(xc, 3) = txc;
Map<Vector3d>(xd, 3) = txd;
Map<Vector2d>(Xa, 2) = tXa;
Map<Vector2d>(Xb, 2) = tXb;
Map<Vector2d>(Xc, 2) = tXc;
Map<Vector2d>(Xd, 2) = tXd;
bool to_eolA = n0->EoL;
bool to_eolB = n1->EoL;
bool to_eolC = n2->EoL;
bool to_eolD = n3->EoL;
int aindex = n0->index * 3;
int bindex = n1->index * 3;
int cindex = n2->index * 3;
int dindex = n3->index * 3;
int aindexX = mesh.nodes.size() * 3 + n0->EoL_index * 2;
int bindexX = mesh.nodes.size() * 3 + n1->EoL_index * 2;
int cindexX = mesh.nodes.size() * 3 + n2->EoL_index * 2;
int dindexX = mesh.nodes.size() * 3 + n3->EoL_index * 2;
double Wb[1], fb[12], Kb[144];
Vector2d damping(mat.dampingA, mat.dampingB);
VectorXd fbe(20);
MatrixXd Kbe(20, 20);
ComputeBending(xa, xb, xc, xd, Xa, Xb, Xc, Xd, mat.beta, Wb, fb, Kb);
fbe.segment<12>(0) = Map<VectorXd>(fb, 12);
Kbe.block<12, 12>(0, 0) = Map<MatrixXd>(Kb, 12, 12);
if (to_eolA || to_eolB || to_eolC || to_eolD) {
MatrixXd F1 = deform_grad(f0);
MatrixXd F2 = deform_grad(f1);
MatrixXd F = (F1 + F2) / 2;
int ja = 0; int jA = 3; int jb = 5; int jB = 8; int jc = 10; int jC = 13; int jd = 15; int jD = 18;
Matrix3d Kbaa = Kbe.block(0, 0, 3, 3); Matrix3d Kbab = Kbe.block(0, 3, 3, 3); Matrix3d Kbac = Kbe.block(0, 6, 3, 3); Matrix3d Kbad = Kbe.block(0, 9, 3, 3);
Matrix3d Kbba = Kbe.block(3, 0, 3, 3); Matrix3d Kbbb = Kbe.block(3, 3, 3, 3); Matrix3d Kbbc = Kbe.block(3, 6, 3, 3); Matrix3d Kbbd = Kbe.block(3, 9, 3, 3);
Matrix3d Kbca = Kbe.block(6, 0, 3, 3); Matrix3d Kbcb = Kbe.block(6, 3, 3, 3); Matrix3d Kbcc = Kbe.block(6, 6, 3, 3); Matrix3d Kbcd = Kbe.block(6, 9, 3, 3);
Matrix3d Kbda = Kbe.block(9, 0, 3, 3); Matrix3d Kbdb = Kbe.block(9, 3, 3, 3); Matrix3d Kbdc = Kbe.block(9, 6, 3, 3); Matrix3d Kbdd = Kbe.block(9, 9, 3, 3);
Kbe.block(ja, jA, 3, 2) = -Kbaa * F; Kbe.block(ja, jB, 3, 2) = -Kbab * F; Kbe.block(ja, jC, 3, 2) = -Kbac * F; Kbe.block(ja, jD, 3, 2) = -Kbad * F;
Kbe.block(jb, jA, 3, 2) = -Kbba * F; Kbe.block(jb, jB, 3, 2) = -Kbbb * F; Kbe.block(jb, jC, 3, 2) = -Kbbc * F; Kbe.block(jb, jD, 3, 2) = -Kbbd * F;
Kbe.block(jc, jA, 3, 2) = -Kbca * F; Kbe.block(jc, jB, 3, 2) = -Kbcb * F; Kbe.block(jc, jC, 3, 2) = -Kbcc * F; Kbe.block(jc, jD, 3, 2) = -Kbcd * F;
Kbe.block(jd, jA, 3, 2) = -Kbda * F; Kbe.block(jd, jB, 3, 2) = -Kbdb * F; Kbe.block(jd, jC, 3, 2) = -Kbdc * F; Kbe.block(jd, jD, 3, 2) = -Kbdd * F;
Kbe.block(jA, ja, 2, 3) = -F.transpose() * Kbaa; Kbe.block(jA, jb, 2, 3) = -F.transpose() * Kbab; Kbe.block(jA, jc, 2, 3) = -F.transpose() * Kbac; Kbe.block(jA, jd, 2, 3) = -F.transpose() * Kbad;
Kbe.block(jB, ja, 2, 3) = -F.transpose() * Kbba; Kbe.block(jB, jb, 2, 3) = -F.transpose() * Kbbb; Kbe.block(jB, jc, 2, 3) = -F.transpose() * Kbbc; Kbe.block(jB, jd, 2, 3) = -F.transpose() * Kbbd;
Kbe.block(jC, ja, 2, 3) = -F.transpose() * Kbca; Kbe.block(jC, jb, 2, 3) = -F.transpose() * Kbcb; Kbe.block(jC, jc, 2, 3) = -F.transpose() * Kbcc; Kbe.block(jC, jd, 2, 3) = -F.transpose() * Kbcd;
Kbe.block(jD, ja, 2, 3) = -F.transpose() * Kbda; Kbe.block(jD, jb, 2, 3) = -F.transpose() * Kbdb; Kbe.block(jD, jc, 2, 3) = -F.transpose() * Kbdc; Kbe.block(jD, jd, 2, 3) = -F.transpose() * Kbdd;
Kbe.block(jA, jA, 2, 2) = F.transpose() * Kbaa * F; Kbe.block(jA, jB, 2, 2) = F.transpose() * Kbab * F; Kbe.block(jA, jC, 2, 2) = F.transpose() * Kbac * F; Kbe.block(jA, jD, 2, 2) = F.transpose() * Kbad * F;
Kbe.block(jB, jA, 2, 2) = F.transpose() * Kbba * F; Kbe.block(jB, jB, 2, 2) = F.transpose() * Kbbb * F; Kbe.block(jB, jC, 2, 2) = F.transpose() * Kbbc * F; Kbe.block(jB, jD, 2, 2) = F.transpose() * Kbbd * F;
Kbe.block(jC, jA, 2, 2) = F.transpose() * Kbca * F; Kbe.block(jC, jB, 2, 2) = F.transpose() * Kbcb * F; Kbe.block(jC, jC, 2, 2) = F.transpose() * Kbcc * F; Kbe.block(jC, jD, 2, 2) = F.transpose() * Kbcd * F;
Kbe.block(jD, jA, 2, 2) = F.transpose() * Kbda * F; Kbe.block(jD, jB, 2, 2) = F.transpose() * Kbdb * F; Kbe.block(jD, jC, 2, 2) = F.transpose() * Kbdc * F; Kbe.block(jD, jD, 2, 2) = F.transpose() * Kbdd * F;
// x values
Matrix3d K00 = damping(1) * h * h * Kbe.block(0, 0, 3, 3);
f.segment<3>(aindex) += fbe.segment<3>(0);
if (to_eolA) f.segment<2>(aindexX) += -F.transpose() * fbe.segment<3>(0);
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
MDK_.push_back(T(aindex + j, aindex + k, K00(j, k)));
}
}
Matrix3d K11 = damping(1) * h * h * Kbe.block(5, 5, 3, 3);
f.segment<3>(bindex) += fbe.segment<3>(5);
if (to_eolB) f.segment<2>(bindexX) += -F.transpose() * fbe.segment<3>(5);
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
MDK_.push_back(T(bindex + j, bindex + k, K11(j, k)));
}
}
Matrix3d K22 = damping(1) * h * h * Kbe.block(10, 10, 3, 3);
f.segment<3>(cindex) += fbe.segment<3>(10);
if (to_eolC) f.segment<2>(cindexX) += -F.transpose() * fbe.segment<3>(10);
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
MDK_.push_back(T(cindex + j, cindex + k, K22(j, k)));
}
}
Matrix3d K33 = damping(1) * h * h * Kbe.block(15, 15, 3, 3);
f.segment<3>(dindex) += fbe.segment<3>(15);
if (to_eolD) f.segment<2>(dindexX) += -F.transpose() * fbe.segment<3>(15);
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
MDK_.push_back(T(dindex + j, dindex + k, K33(j, k)));
}
}
Matrix3d K01 = damping(1) * h * h * Kbe.block(0, 5, 3, 3);
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
MDK_.push_back(T(aindex + j, bindex + k, K01(j, k)));
MDK_.push_back(T(bindex + k, aindex + j, K01(j, k)));
}
}
Matrix3d K02 = damping(1) * h * h * Kbe.block(0, 10, 3, 3);
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
MDK_.push_back(T(aindex + j, cindex + k, K02(j, k)));
MDK_.push_back(T(cindex + k, aindex + j, K02(j, k)));
}
}
Matrix3d K03 = damping(1) * h * h * Kbe.block(0, 15, 3, 3);
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
MDK_.push_back(T(aindex + j, dindex + k, K03(j, k)));
MDK_.push_back(T(dindex + k, aindex + j, K03(j, k)));
}
}
Matrix3d K12 = damping(1) * h * h * Kbe.block(5, 10, 3, 3);
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
MDK_.push_back(T(bindex + j, cindex + k, K12(j, k)));
MDK_.push_back(T(cindex + k, bindex + j, K12(j, k)));
}
}
Matrix3d K13 = damping(1) * h * h * Kbe.block(5, 15, 3, 3);
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
MDK_.push_back(T(bindex + j, dindex + k, K13(j, k)));
MDK_.push_back(T(dindex + k, bindex + j, K13(j, k)));
}
}
Matrix3d K23 = damping(1) * h * h * Kbe.block(10, 15, 3, 3);
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
MDK_.push_back(T(cindex + j, dindex + k, K23(j, k)));
MDK_.push_back(T(dindex + k, cindex + j, K23(j, k)));
}
}
// X values
if (to_eolA) {
Matrix2d K00X = damping(1) * h * h * Kbe.block(3, 3, 2, 2);
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 2; k++) {
MDK_.push_back(T(aindexX + j, aindexX + k, K00X(j, k)));
}
}
}
if (to_eolB) {
Matrix2d K11X = damping(1) * h * h * Kbe.block(8, 8, 2, 2);
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 2; k++) {
MDK_.push_back(T(bindexX + j, bindexX + k, K11X(j, k)));
}
}
}
if (to_eolC) {
Matrix2d K22X = damping(1) * h * h * Kbe.block(13, 13, 2, 2);
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 2; k++) {
MDK_.push_back(T(cindexX + j, cindexX + k, K22X(j, k)));
}
}
}
if (to_eolD) {
Matrix2d K33X = damping(1) * h * h * Kbe.block(18, 18, 2, 2);
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 2; k++) {
MDK_.push_back(T(dindexX + j, dindexX + k, K33X(j, k)));
}
}
}
if (to_eolA && to_eolB) {
Matrix2d K01X = damping(1) * h * h * Kbe.block(3, 8, 2, 2);
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 2; k++) {
MDK_.push_back(T(aindexX + j, bindexX + k, K01X(j, k)));
MDK_.push_back(T(bindexX + k, aindexX + j, K01X(j, k)));
}
}
}
if (to_eolA && to_eolC) {
Matrix2d K02X = damping(1) * h * h * Kbe.block(3, 13, 2, 2);
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 2; k++) {
MDK_.push_back(T(aindexX + j, cindexX + k, K02X(j, k)));
MDK_.push_back(T(cindexX + k, aindexX + j, K02X(j, k)));
}
}
}
if (to_eolA && to_eolD) {
Matrix2d K03X = damping(1) * h * h * Kbe.block(3, 18, 2, 2);
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 2; k++) {
MDK_.push_back(T(aindexX + j, dindexX + k, K03X(j, k)));
MDK_.push_back(T(dindexX + k, aindexX + j, K03X(j, k)));
}
}
}
if (to_eolB && to_eolC) {
Matrix2d K12X = damping(1) * h * h * Kbe.block(8, 13, 2, 2);
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 2; k++) {
MDK_.push_back(T(bindexX + j, cindexX + k, K12X(j, k)));
MDK_.push_back(T(cindexX + k, bindexX + j, K12X(j, k)));
}
}
}
if (to_eolB && to_eolD) {
Matrix2d K13X = damping(1) * h * h * Kbe.block(8, 18, 2, 2);
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 2; k++) {
MDK_.push_back(T(bindexX + j, dindexX + k, K13X(j, k)));
MDK_.push_back(T(dindexX + k, bindexX + j, K13X(j, k)));
}
}
}
if (to_eolC && to_eolD) {
Matrix2d K23X = damping(1) * h * h * Kbe.block(13, 18, 2, 2);
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 2; k++) {
MDK_.push_back(T(cindexX + j, dindexX + k, K23X(j, k)));
MDK_.push_back(T(dindexX + k, cindexX + j, K23X(j, k)));
}
}
}
// x-X values
if (to_eolA) {
MatrixXd KAa = damping(1) * h * h * Kbe.block(3, 0, 2, 3);
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 3; k++) {
MDK_.push_back(T(aindexX + j, aindex + k, KAa(j, k)));
MDK_.push_back(T(aindex + k, aindexX + j, KAa(j, k)));
}
}
MatrixXd KAb = damping(1) * h * h * Kbe.block(3, 5, 2, 3);
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 3; k++) {
MDK_.push_back(T(aindexX + j, bindex + k, KAb(j, k)));
MDK_.push_back(T(bindex + k, aindexX + j, KAb(j, k)));
}
}
MatrixXd KAc = damping(1) * h * h * Kbe.block(3, 10, 2, 3);
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 3; k++) {
MDK_.push_back(T(aindexX + j, cindex + k, KAc(j, k)));
MDK_.push_back(T(cindex + k, aindexX + j, KAc(j, k)));
}
}
MatrixXd KAd = damping(1) * h * h * Kbe.block(3, 15, 2, 3);
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 3; k++) {
MDK_.push_back(T(aindexX + j, dindex + k, KAd(j, k)));
MDK_.push_back(T(dindex + k, aindexX + j, KAd(j, k)));
}
}
}
if (to_eolB) {
MatrixXd KBa = damping(1) * h * h * Kbe.block(8, 0, 2, 3);
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 3; k++) {
MDK_.push_back(T(bindexX + j, aindex + k, KBa(j, k)));
MDK_.push_back(T(aindex + k, bindexX + j, KBa(j, k)));
}
}
MatrixXd KBb = damping(1) * h * h * Kbe.block(8, 5, 2, 3);
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 3; k++) {
MDK_.push_back(T(bindexX + j, bindex + k, KBb(j, k)));
MDK_.push_back(T(bindex + k, bindexX + j, KBb(j, k)));
}
}
MatrixXd KBc = damping(1) * h * h * Kbe.block(8, 10, 2, 3);
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 3; k++) {
MDK_.push_back(T(bindexX + j, cindex + k, KBc(j, k)));
MDK_.push_back(T(cindex + k, bindexX + j, KBc(j, k)));
}
}
MatrixXd KBd = damping(1) * h * h * Kbe.block(8, 15, 2, 3);
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 3; k++) {
MDK_.push_back(T(bindexX + j, dindex + k, KBc(j, k)));
MDK_.push_back(T(dindex + k, bindexX + j, KBc(j, k)));
}
}
}
if (to_eolC) {
MatrixXd KCa = damping(1) * h * h * Kbe.block(13, 0, 2, 3);
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 3; k++) {
MDK_.push_back(T(cindexX + j, aindex + k, KCa(j, k)));
MDK_.push_back(T(aindex + k, cindexX + j, KCa(j, k)));
}
}
MatrixXd KCb = damping(1) * h * h * Kbe.block(13, 5, 2, 3);
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 3; k++) {
MDK_.push_back(T(cindexX + j, bindex + k, KCb(j, k)));
MDK_.push_back(T(bindex + k, cindexX + j, KCb(j, k)));
}
}
MatrixXd KCc = damping(1) * h * h * Kbe.block(13, 10, 2, 3);
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 3; k++) {
MDK_.push_back(T(cindexX + j, cindex + k, KCc(j, k)));
MDK_.push_back(T(cindex + k, cindexX + j, KCc(j, k)));
}
}
MatrixXd KCd = damping(1) * h * h * Kbe.block(13, 15, 2, 3);
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 3; k++) {
MDK_.push_back(T(cindexX + j, dindex + k, KCd(j, k)));
MDK_.push_back(T(dindex + k, cindexX + j, KCd(j, k)));
}
}
}
if (to_eolD) {
MatrixXd KDa = damping(1) * h * h * Kbe.block(18, 0, 2, 3);
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 3; k++) {
MDK_.push_back(T(dindexX + j, aindex + k, KDa(j, k)));
MDK_.push_back(T(aindex + k, dindexX + j, KDa(j, k)));
}
}
MatrixXd KDb = damping(1) * h * h * Kbe.block(18, 5, 2, 3);
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 3; k++) {
MDK_.push_back(T(dindexX + j, bindex + k, KDb(j, k)));
MDK_.push_back(T(bindex + k, dindexX + j, KDb(j, k)));
}
}
MatrixXd KDc = damping(1) * h * h * Kbe.block(18, 10, 2, 3);
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 3; k++) {
MDK_.push_back(T(dindexX + j, cindex + k, KDc(j, k)));
MDK_.push_back(T(cindex + k, dindexX + j, KDc(j, k)));
}
}
MatrixXd KDd = damping(1) * h * h * Kbe.block(18, 15, 2, 3);
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 3; k++) {
MDK_.push_back(T(dindexX + j, dindex + k, KDd(j, k)));
MDK_.push_back(T(dindex + k, dindexX + j, KDd(j, k)));
}
}
}
}
else {
Matrix3d K00 = damping(1) * h * h * Kbe.block(0, 0, 3, 3);
f.segment<3>(aindex) += fbe.segment<3>(0);
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
MDK_.push_back(T(aindex + j, aindex + k, K00(j, k)));
}
}
Matrix3d K11 = damping(1) * h * h * Kbe.block(3, 3, 3, 3);
f.segment<3>(bindex) += fbe.segment<3>(3);
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
MDK_.push_back(T(bindex + j, bindex + k, K11(j, k)));
}
}
Matrix3d K22 = damping(1) * h * h * Kbe.block(6, 6, 3, 3);
f.segment<3>(cindex) += fbe.segment<3>(6);
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
MDK_.push_back(T(cindex + j, cindex + k, K22(j, k)));
}
}
Matrix3d K33 = damping(1) * h * h * Kbe.block(9, 9, 3, 3);
f.segment<3>(dindex) += fbe.segment<3>(9);
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
MDK_.push_back(T(dindex + j, dindex + k, K33(j, k)));
}
}
Matrix3d K01 = damping(1) * h * h * Kbe.block(0, 3, 3, 3);
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
MDK_.push_back(T(aindex + j, bindex + k, K01(j, k)));
MDK_.push_back(T(bindex + k, aindex + j, K01(j, k)));
}
}
Matrix3d K02 = damping(1) * h * h * Kbe.block(0, 6, 3, 3);
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
MDK_.push_back(T(aindex + j, cindex + k, K02(j, k)));
MDK_.push_back(T(cindex + k, aindex + j, K02(j, k)));
}
}
Matrix3d K03 = damping(1) * h * h * Kbe.block(0, 9, 3, 3);
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
MDK_.push_back(T(aindex + j, dindex + k, K03(j, k)));
MDK_.push_back(T(dindex + k, aindex + j, K03(j, k)));
}
}
Matrix3d K12 = damping(1) * h * h * Kbe.block(3, 6, 3, 3);
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
MDK_.push_back(T(bindex + j, cindex + k, K12(j, k)));
MDK_.push_back(T(cindex + k, bindex + j, K12(j, k)));
}
}
Matrix3d K13 = damping(1) * h * h * Kbe.block(3, 9, 3, 3);
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
MDK_.push_back(T(bindex + j, dindex + k, K13(j, k)));
MDK_.push_back(T(dindex + k, bindex + j, K13(j, k)));
}
}
Matrix3d K23 = damping(1) * h * h * Kbe.block(6, 9, 3, 3);
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
MDK_.push_back(T(cindex + j, dindex + k, K23(j, k)));
MDK_.push_back(T(dindex + k, cindex + j, K23(j, k)));
}
}
}
}
}
void Forces::fill(const Mesh& mesh, const Material& mat, const Vector3d& grav, double h)
{
f.resize(mesh.nodes.size() * 3 + mesh.EoL_Count * 2);
f.setZero();
vector<T> M_;
vector<T> MDK_;
faceBasedF(mesh, f, M_, MDK_, grav, h);
edgeBasedF(mesh, mat, f, M_, MDK_, h);
M.resize(mesh.nodes.size() * 3 + mesh.EoL_Count * 2, mesh.nodes.size() * 3 + mesh.EoL_Count * 2);
MDK.resize(mesh.nodes.size() * 3 + mesh.EoL_Count * 2, mesh.nodes.size() * 3 + mesh.EoL_Count * 2);
M.setFromTriplets(M_.begin(), M_.end());
MDK.setFromTriplets(MDK_.begin(), MDK_.end());
}