https://github.com/sueda/eol-cloth
Revision 2323d9f6471bbf5dc63979216a3ed3c97ff3538a authored by Weidner on 13 July 2018, 15:37:58 UTC, committed by Weidner on 13 July 2018, 15:37:58 UTC
1 parent db932cf
Tip revision: 2323d9f6471bbf5dc63979216a3ed3c97ff3538a authored by Weidner on 13 July 2018, 15:37:58 UTC
Solver solving using old deform grad method. Still some bugs, but going to clean up force filling
Solver solving using old deform grad method. Still some bugs, but going to clean up force filling
Tip revision: 2323d9f
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 "ComputeInertiaEOL.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 fm[9], Km[81];
VectorXd fme(9);
MatrixXd Kme(15, 15);
ComputeMembrane(xa, xb, xc, Xa, Xb, Xc, mat->e, mat->nu, PP, QQ, Wm, fm, Km);
fme = Map<VectorXd>(fm, 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) {
double fi[15], Mi[225];
ComputeInertiaEOL(xa, xb, xc, Xa, Xb, Xc, g, mat->density, Wi, fi, Mi);
VectorXd fie = Map<VectorXd>(fi, 15);
MatrixXd Mie = Map<MatrixXd>(Mi, 15, 15);
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, 3, 3, 3); Matrix3d Kmac = Kme.block(0, 6, 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, 3) = Kmaa; Kme.block(ja, jb, 3, 3) = Kmab; Kme.block(ja, jc, 3, 3) = Kmac;
Kme.block(jb, ja, 3, 3) = Kmba; Kme.block(jb, jb, 3, 3) = Kmbb; Kme.block(jb, jc, 3, 3) = Kmbc;
Kme.block(jc, ja, 3, 3) = Kmca; Kme.block(jc, jb, 3, 3) = Kmcb; Kme.block(jc, jc, 3, 3) = Kmcc;
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>(3));
if (face->v[1]->node->EoL_index != -1)f.segment<2>(bindexX) += -F.transpose() * (fie.segment<3>(5) + 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(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>(6));
if (face->v[2]->node->EoL_index != -1) f.segment<2>(cindexX) += -F.transpose() * (fie.segment<3>(10) + 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, 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 {
double fi[9], Mi[81];
ComputeInertial(xa, xb, xc, Xa, Xb, Xc, g, mat->density, Wi, fi, Mi);
VectorXd fie = Map<VectorXd>(fi, 9);
MatrixXd Mie = Map<MatrixXd>(Mi, 9, 9);
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, 3) = Kbaa; Kbe.block(ja, jb, 3, 3) = Kbab; Kbe.block(ja, jc, 3, 3) = Kbac; Kbe.block(ja, jd, 3, 3) = Kbad;
Kbe.block(jb, ja, 3, 3) = Kbba; Kbe.block(jb, jb, 3, 3) = Kbbb; Kbe.block(jb, jc, 3, 3) = Kbbc; Kbe.block(jb, jd, 3, 3) = Kbbd;
Kbe.block(jc, ja, 3, 3) = Kbca; Kbe.block(jc, jb, 3, 3) = Kbcb; Kbe.block(jc, jc, 3, 3) = Kbcc; Kbe.block(jc, jd, 3, 3) = Kbcd;
Kbe.block(jd, ja, 3, 3) = Kbda; Kbe.block(jd, jb, 3, 3) = Kbdb; Kbe.block(jd, jc, 3, 3) = Kbdc; Kbe.block(jd, jd, 3, 3) = Kbdd;
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>(3);
if (to_eolB) f.segment<2>(bindexX) += -F.transpose() * 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(10, 10, 3, 3);
f.segment<3>(cindex) += fbe.segment<3>(6);
if (to_eolC) f.segment<2>(cindexX) += -F.transpose() * 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(15, 15, 3, 3);
f.segment<3>(dindex) += fbe.segment<3>(9);
if (to_eolD) f.segment<2>(dindexX) += -F.transpose() * 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, 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());
}
Computing file changes ...
