https://github.com/sueda/eol-cloth
Revision a6d03813fc8cd8b58edc6b3cef6b2f732f4d9d36 authored by Nick Weidner on 07 September 2018, 18:25:33 UTC, committed by Nick Weidner on 07 September 2018, 18:25:33 UTC
1 parent 09335f4
Tip revision: a6d03813fc8cd8b58edc6b3cef6b2f732f4d9d36 authored by Nick Weidner on 07 September 2018, 18:25:33 UTC
Point fix
Point fix
Tip revision: a6d0381
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(const 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;
}
// We are using mass lumping so the mass matrix is simplified
void nodeBasedF(const Mesh& mesh, VectorXd& f, vector<T>& M_, const Vector3d& grav)
{
for (int n = 0; n < mesh.nodes.size(); n++) {
Node* node = mesh.nodes[n];
Vert* vert = node->verts[0];
int nindex = node->index * 3;
double mass = 0.0;
for (int f = 0; f < vert->adjf.size(); f++) {
mass += (vert->adjf[f]->m * (1.0 / 3.0));
}
Vector3d mg = mass * grav;
f.segment<3>(nindex) += mg;
Matrix3d Mxx = Matrix3d::Identity() * mass;
for (int j = 0; j < 3; j++) {
M_.push_back(T(nindex + j, nindex + j, Mxx(j, j))); // We know it's a diagonal matrix
}
if (node->EoL) {
int nindexX = mesh.nodes.size() * 3 + node->EoL_index * 2;
MatrixXd F = deform_grad_v(vert);
Vector2d mFg = -mass * F.transpose() * grav;
f.segment<2>(nindexX) += mFg;
Matrix2d MXX = -mass * F.transpose() * F;
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 2; k++) {
M_.push_back(T(nindexX + j, nindexX + k, MXX(j, k)));
}
}
MatrixXd MXx = -mass * F.transpose();
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 3; k++) {
M_.push_back(T(nindexX + j, nindex + k, MXx(j, k)));
M_.push_back(T(nindex + k, nindexX + j, MXx(j, k)));
}
}
}
}
}
void fillxMI(vector<T>& MDK_, vector<T>& M_, const Matrix3d& Kxx, const Matrix3d& Mxx, int index, const Vector2d& damping, double h)
{
Matrix3d MDKxx = Mxx + damping(1) * h * h * Kxx;
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
M_.push_back(T(index + j, index + k, Mxx(j, k)));
MDK_.push_back(T(index + j, index + k, MDKxx(j, k)));
}
}
}
void fillxxMI(vector<T>& MDK_, vector<T>& M_, const Matrix3d& Kxx, const Matrix3d& Mxx, int i0, int i1, const Vector2d& damping, double h)
{
Matrix3d MDKxx = Mxx + damping(1) * h * h * Kxx;
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
M_.push_back(T(i0 + j, i1 + k, Mxx(j, k)));
M_.push_back(T(i1 + k, i0 + j, Mxx(j, k)));
MDK_.push_back(T(i0 + j, i1 + k, MDKxx(j, k)));
MDK_.push_back(T(i1 + k, i0 + j, MDKxx(j, k)));
}
}
}
void fillXMI(vector<T>& MDK_, vector<T>& M_, const Matrix2d& KXX, const Matrix2d& Mxx, int index, const Vector2d& damping, double h)
{
Matrix2d MDKXX = Mxx + damping(1) * h * h * KXX;
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 2; k++) {
M_.push_back(T(index + j, index + k, Mxx(j, k)));
MDK_.push_back(T(index + j, index + k, MDKXX(j, k)));
}
}
}
void fillXXMI(vector<T>& MDK_, vector<T>& M_, const Matrix2d& KXX, const Matrix2d& Mxx, int i0, int i1, const Vector2d& damping, double h)
{
Matrix2d MDKXX = Mxx + damping(1) * h * h * KXX;
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 2; k++) {
M_.push_back(T(i0 + j, i1 + k, Mxx(j, k)));
M_.push_back(T(i1 + k, i0 + j, Mxx(j, k)));
MDK_.push_back(T(i0 + j, i1 + k, MDKXX(j, k)));
MDK_.push_back(T(i1 + k, i0 + j, MDKXX(j, k)));
}
}
}
void fillXxMI(vector<T>& MDK_, vector<T>& M_, MatrixXd& KXx, const MatrixXd& Mxx, int i0, int i1, Vector2d& damping, double h)
{
MatrixXd MDKXx = Mxx + damping(1) * h * h * KXx;
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 3; k++) {
M_.push_back(T(i0 + j, i1 + k, Mxx(j, k)));
M_.push_back(T(i1 + k, i0 + j, Mxx(j, k)));
MDK_.push_back(T(i0 + j, i1 + k, MDKXx(j, k)));
MDK_.push_back(T(i1 + k, i0 + j, MDKXx(j, k)));
}
}
}
void fillxXMI(vector<T>& MDK_, vector<T>& M_, MatrixXd& KxX, const MatrixXd& Mxx, int i0, int i1, Vector2d& damping, double h)
{
MatrixXd MDKxX = Mxx + damping(1) * h * h * KxX;
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 2; k++) {
M_.push_back(T(i0 + j, i1 + k, Mxx(j, k)));
M_.push_back(T(i1 + k, i0 + j, Mxx(j, k)));
MDK_.push_back(T(i0 + j, i1 + k, MDKxX(j, k)));
MDK_.push_back(T(i1 + k, i0 + j, MDKxX(j, k)));
}
}
}
void fillEOLInertia(const Face* face, VectorXd& fi, MatrixXd& Mi)
{
MatrixXd Fa, Fb, Fc;
bool EOLA = face->v[0]->node->EoL; bool EOLB = face->v[1]->node->EoL; bool EOLC = face->v[2]->node->EoL;
//if (EOLA) Fa = deform_grad_v(face->v[0]);
//if (EOLB) Fb = deform_grad_v(face->v[1]);
//if (EOLC) Fc = deform_grad_v(face->v[2]);
MatrixXd F = deform_grad(face);
if (EOLA) Fa = F;
if (EOLB) Fb = F;
if (EOLC) Fc = F;
Vector3d fia = fi.segment<3>(0); Vector3d fib = fi.segment<3>(3); Vector3d fic = fi.segment<3>(6);
Matrix3d Kiaa = Mi.block<3, 3>(0, 0); Matrix3d Kiab = Mi.block<3, 3>(0, 3); Matrix3d Kiac = Mi.block<3, 3>(0, 6);
Matrix3d Kiba = Mi.block<3, 3>(3, 0); Matrix3d Kibb = Mi.block<3, 3>(3, 3); Matrix3d Kibc = Mi.block<3, 3>(3, 6);
Matrix3d Kica = Mi.block<3, 3>(6, 0); Matrix3d Kicb = Mi.block<3, 3>(6, 3); Matrix3d Kicc = Mi.block<3, 3>(6, 6);
int ja = 0; int jA = 3; int jb = 5; int jB = 8; int jc = 10; int jC = 13;
fi.segment<3>(ja) = fia; fi.segment<3>(jb) = fib; fi.segment<3>(jc) = fic;
Mi.block<3, 3>(ja, ja) = Kiaa; Mi.block<3, 3>(ja, jb) = Kiab; Mi.block<3, 3>(ja, jc) = Kiac;
/*Mi.block<3, 3>(jb, ja) = Kiba;*/ Mi.block<3, 3>(jb, jb) = Kibb; Mi.block<3, 3>(jb, jc) = Kibc;
/*Mi.block<3, 3>(jc, ja) = Kica; Mi.block<3, 3>(jc, jb) = Kicb;*/ Mi.block<3, 3>(jc, jc) = Kicc;
if (EOLA) {
fi.segment<2>(jA) = -Fa.transpose() * fia;
Mi.block<2, 2>(jA, jA) = Fa.transpose() * Kiaa * Fa;
}
if (EOLB) {
fi.segment<2>(jB) = -Fb.transpose() * fib;
Mi.block<2, 2>(jB, jB) = Fb.transpose() * Kibb * Fb;
}
if (EOLC) {
fi.segment<2>(jC) = -Fc.transpose() * fic;
Mi.block<2, 2>(jC, jC) = Fc.transpose() * Kicc * Fc;
}
if (EOLA && EOLB) {
Mi.block<2, 2>(jA, jB) = Fa.transpose() * Kiab * Fb;
}
if (EOLA && EOLC) {
Mi.block<2, 2>(jA, jC) = Fa.transpose() * Kiac * Fc;
}
if (EOLB && EOLC) {
Mi.block<2, 2>(jB, jC) = Fb.transpose() * Kibc * Fc;
}
if (EOLA) {
Mi.block<2, 3>(jA, ja) = -Fa.transpose() * Kiaa;
Mi.block<2, 3>(jA, jb) = -Fa.transpose() * Kiab;
Mi.block<2, 3>(jA, jc) = -Fa.transpose() * Kiac;
//Mi.block<3, 2>(ja, jA) = -Kiaa * Fa;
}
if (EOLB) {
//Mi.block<2, 3>(jB, ja) = -Fb.transpose() * Kiba;
Mi.block<2, 3>(jB, jb) = -Fb.transpose() * Kibb;
Mi.block<2, 3>(jB, jc) = -Fb.transpose() * Kibc;
Mi.block<3, 2>(ja, jB) = -Kiab * Fb;
//Mi.block<3, 2>(jb, jB) = -Kibb * Fb;
}
if (EOLC) {
//Mi.block<2, 3>(jC, ja) = -Fc.transpose() * Kica;
//Mi.block<2, 3>(jC, jb) = -Fc.transpose() * Kicb;
Mi.block<2, 3>(jC, jc) = -Fc.transpose() * Kicc;
Mi.block<3, 2>(ja, jC) = -Kiac * Fc;
Mi.block<3, 2>(jb, jC) = -Kibc * Fc;
//Mi.block<3, 2>(jc, jC) = -Kicc * Fc;
}
}
void fillEOLMembrane(const Face* face, VectorXd& fm, MatrixXd& Km)
{
MatrixXd Fa, Fb, Fc;
bool EOLA = face->v[0]->node->EoL; bool EOLB = face->v[1]->node->EoL; bool EOLC = face->v[2]->node->EoL;
//if (EOLA) Fa = deform_grad_v(face->v[0]);
//if (EOLB) Fb = deform_grad_v(face->v[1]);
//if (EOLC) Fc = deform_grad_v(face->v[2]);
MatrixXd F = deform_grad(face);
if (EOLA) Fa = F;
if (EOLB) Fb = F;
if (EOLC) Fc = F;
Vector3d fma = fm.segment<3>(0); Vector3d fmb = fm.segment<3>(3); Vector3d fmc = fm.segment<3>(6);
Matrix3d Kmaa = Km.block<3, 3>(0, 0); Matrix3d Kmab = Km.block<3, 3>(0, 3); Matrix3d Kmac = Km.block<3, 3>(0, 6);
Matrix3d Kmba = Km.block<3, 3>(3, 0); Matrix3d Kmbb = Km.block<3, 3>(3, 3); Matrix3d Kmbc = Km.block<3, 3>(3, 6);
Matrix3d Kmca = Km.block<3, 3>(6, 0); Matrix3d Kmcb = Km.block<3, 3>(6, 3); Matrix3d Kmcc = Km.block<3, 3>(6, 6);
int ja = 0; int jA = 3; int jb = 5; int jB = 8; int jc = 10; int jC = 13;
fm.segment<3>(ja) = fma; fm.segment<3>(jb) = fmb; fm.segment<3>(jc) = fmc;
Km.block<3, 3>(ja, ja) = Kmaa; Km.block<3, 3>(ja, jb) = Kmab; Km.block<3, 3>(ja, jc) = Kmac;
/*Km.block<3, 3>(jb, ja) = Kmba;*/ Km.block<3, 3>(jb, jb) = Kmbb; Km.block<3, 3>(jb, jc) = Kmbc;
/*Km.block<3, 3>(jc, ja) = Kmca; Km.block<3, 3>(jc, jb) = Kmcb;*/ Km.block<3, 3>(jc, jc) = Kmcc;
if (EOLA) {
fm.segment<2>(jA) = -Fa.transpose() * fma;
Km.block<2, 2>(jA, jA) = Fa.transpose() * Kmaa * Fa;
}
if (EOLB) {
fm.segment<2>(jB) = -Fb.transpose() * fmb;
Km.block<2, 2>(jB, jB) = Fb.transpose() * Kmbb * Fb;
}
if (EOLC) {
fm.segment<2>(jC) = -Fc.transpose() * fmc;
Km.block<2, 2>(jC, jC) = Fc.transpose() * Kmcc * Fc;
}
if (EOLA && EOLB) {
Km.block<2, 2>(jA, jB) = Fa.transpose() * Kmab * Fb;
}
if (EOLA && EOLC) {
Km.block<2, 2>(jA, jC) = Fa.transpose() * Kmac * Fc;
}
if (EOLB && EOLC) {
Km.block<2, 2>(jB, jC) = Fb.transpose() * Kmbc * Fc;
}
if (EOLA) {
Km.block<2, 3>(jA, ja) = -Fa.transpose() * Kmaa;
Km.block<2, 3>(jA, jb) = -Fa.transpose() * Kmab;
Km.block<2, 3>(jA, jc) = -Fa.transpose() * Kmac;
//Km.block<3, 2>(ja, jA) = -Kmaa * Fa;
}
if (EOLB) {
//Km.block<2, 3>(jB, ja) = -Fb.transpose() * Kmba;
Km.block<2, 3>(jB, jb) = -Fb.transpose() * Kmbb;
Km.block<2, 3>(jB, jc) = -Fb.transpose() * Kmbc;
Km.block<3, 2>(ja, jB) = -Kmab * Fb;
//Km.block<3, 2>(jb, jB) = -Kmbb * Fb;
}
if (EOLC) {
//Km.block<2, 3>(jC, ja) = -Fc.transpose() * Kmca;
//Km.block<2, 3>(jC, jb) = -Fc.transpose() * Kmcb;
Km.block<2, 3>(jC, jc) = -Fc.transpose() * Kmcc;
Km.block<3, 2>(ja, jC) = -Kmac * Fc;
Km.block<3, 2>(jb, jC) = -Kmbc * Fc;
//Km.block<3, 2>(jc, jC) = -Kmcc * Fc;
}
}
void faceBasedF(const Mesh& mesh, VectorXd& f, vector<T>& MDK_, vector<T>& M_, 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;
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;
Map<Vector3d>(g, grav.rows(), grav.cols()) = grav;
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];
double fi[9], Mi[81];
VectorXd fme(15), fie(15);
MatrixXd Kme(15, 15), Mie(15, 15);
ComputeMembrane(xa, xb, xc, Xa, Xb, Xc, mat->e, mat->nu, PP, QQ, Wm, fm, Km);
ComputeInertial(xa, xb, xc, Xa, Xb, Xc, g, mat->density, Wi, fi, Mi);
fme.segment<9>(0) = Map<VectorXd>(fm, 9);
Kme.block<9, 9>(0, 0) = Map<MatrixXd>(Km, 9, 9);
fie.segment<9>(0) = Map<VectorXd>(fi, 9);
Mie.block<9, 9>(0, 0) = Map<MatrixXd>(Mi, 9, 9);
Vector2d damping(mat->dampingA, mat->dampingB);
if (face->v[0]->node->EoL || face->v[1]->node->EoL || face->v[2]->node->EoL) {
fillEOLInertia(face, fie, Mie);
fillEOLMembrane(face, fme, Kme);
f.segment<3>(aindex) += (fme.segment<3>(0) + fie.segment<3>(0));
if (face->v[0]->node->EoL) f.segment<2>(aindexX) += (fme.segment<2>(3) + fie.segment<2>(3));
f.segment<3>(bindex) += (fme.segment<3>(5) + fie.segment<3>(5));
if (face->v[1]->node->EoL)f.segment<2>(bindexX) += (fme.segment<2>(8) + fie.segment<2>(8));
f.segment<3>(cindex) += (fme.segment<3>(10) + fie.segment<3>(10));
if (face->v[2]->node->EoL) f.segment<2>(cindexX) += (fme.segment<2>(13) + fie.segment<2>(13));
// Diagonal x
Matrix3d Mxx, Kxx;
Kxx = Kme.block<3, 3>(0, 0); Mxx = Mie.block<3, 3>(0, 0);
fillxMI(MDK_, M_, Kxx, Mxx, aindex, damping, h);
Kxx = Kme.block<3, 3>(5, 5); Mxx = Mie.block<3, 3>(5, 5);
fillxMI(MDK_, M_, Kxx, Mxx, bindex, damping, h);
Kxx = Kme.block<3, 3>(10, 10); Mxx = Mie.block<3, 3>(10, 10);
fillxMI(MDK_, M_, Kxx, Mxx, cindex, damping, h);
// Off-Diagonal x
Kxx = Kme.block<3, 3>(0, 5); Mxx = Mie.block<3, 3>(0, 5);
fillxxMI(MDK_, M_, Kxx, Mxx, aindex, bindex, damping, h);
Kxx = Kme.block<3, 3>(0, 10); Mxx = Mie.block<3, 3>(0, 10);
fillxxMI(MDK_, M_, Kxx, Mxx, aindex, cindex, damping, h);
Kxx = Kme.block<3, 3>(5, 10); Mxx = Mie.block<3, 3>(5, 10);
fillxxMI(MDK_, M_, Kxx, Mxx, bindex, cindex, damping, h);
// If has EOL componenent, Diagonal X
Matrix2d MXX, KXX;
if (face->v[0]->node->EoL) {
KXX = Kme.block<2, 2>(3, 3); MXX = Mie.block<2, 2>(3, 3);
fillXMI(MDK_, M_, KXX, MXX, aindexX, damping, h);
}
if (face->v[1]->node->EoL) {
KXX = Kme.block<2, 2>(8, 8); MXX = Mie.block<2, 2>(8, 8);
fillXMI(MDK_, M_, KXX, MXX, bindexX, damping, h);
}
if (face->v[2]->node->EoL) {
KXX = Kme.block<2, 2>(13, 13); MXX = Mie.block<2, 2>(13, 13);
fillXMI(MDK_, M_, KXX, MXX, cindexX, damping, h);
}
// If has EOL componenent, Off-Diagonal X
if (face->v[0]->node->EoL && face->v[1]->node->EoL) {
KXX = Kme.block<2, 2>(3, 8); MXX = Mie.block<2, 2>(3, 8);
fillXXMI(MDK_, M_, KXX, MXX, aindexX, bindexX, damping, h);
}
if (face->v[0]->node->EoL && face->v[2]->node->EoL) {
KXX = Kme.block<2, 2>(3, 13); MXX = Mie.block<2, 2>(3, 13);
fillXXMI(MDK_, M_, KXX, MXX, aindexX, cindexX, damping, h);
}
if (face->v[1]->node->EoL && face->v[2]->node->EoL) {
KXX = Kme.block<2, 2>(8, 13); MXX = Mie.block<2, 2>(8, 13);
fillXXMI(MDK_, M_, KXX, MXX, bindexX, cindexX, damping, h);
}
// X-x values
MatrixXd KXx, KxX, MXx, MxX;
if (face->v[0]->node->EoL) {
KXx = Kme.block<2, 3>(3, 0); MXx = Mie.block<2, 3>(3, 0);
fillXxMI(MDK_, M_, KXx, MXx, aindexX, aindex, damping, h);
KXx = Kme.block<2, 3>(3, 5); MXx = Mie.block<2, 3>(3, 5);
fillXxMI(MDK_, M_, KXx, MXx, aindexX, bindex, damping, h);
KXx = Kme.block<2, 3>(3, 10); MXx = Mie.block<2, 3>(3, 10);
fillXxMI(MDK_, M_, KXx, MXx, aindexX, cindex, damping, h);
//KxX = Kme.block<3, 2>(0, 3);
//fillxXMI(MDK_, KxX, aindex, aindexX, damping, h);
}
if (face->v[1]->node->EoL) {
//KXx = Kme.block<2, 3>(8, 0);
//fillXxMI(MDK_, KXx, bindexX, aindex, damping, h);
KXx = Kme.block<2, 3>(8, 5); MXx = Mie.block<2, 3>(8, 5);
fillXxMI(MDK_, M_, KXx, MXx, bindexX, bindex, damping, h);
KXx = Kme.block<2, 3>(8, 10); MXx = Mie.block<2, 3>(8, 10);
fillXxMI(MDK_, M_, KXx, MXx, bindexX, cindex, damping, h);
KxX = Kme.block<3, 2>(0, 8); MxX = Mie.block<3, 2>(0, 8);
fillxXMI(MDK_, M_, KxX, MxX, aindex, bindexX, damping, h);
}
if (face->v[2]->node->EoL) {
//KXx = Kme.block<2, 3>(13, 0);
//fillXxMI(MDK_, KXx, cindexX, aindex, damping, h);
//KXx = Kme.block<2, 3>(13, 5);
//fillXxMI(MDK_, KXx, cindexX, bindex, damping, h);
KXx = Kme.block<2, 3>(13, 10); MXx = Mie.block<2, 3>(13, 10);
fillXxMI(MDK_, M_, KXx, MXx, cindexX, cindex, damping, h);
KxX = Kme.block<3, 2>(0, 13); MxX = Mie.block<3, 2>(0, 13);
fillxXMI(MDK_, M_, KxX, MxX, aindex, cindexX, damping, h);
KxX = Kme.block<3, 2>(5, 13); MxX = Mie.block<3, 2>(5, 13);
fillxXMI(MDK_, M_, KxX, MxX, bindex, cindexX, damping, h);
}
}
else {
f.segment<3>(aindex) += fme.segment<3>(0) + fie.segment<3>(0);
f.segment<3>(bindex) += fme.segment<3>(3) + fie.segment<3>(3);
f.segment<3>(cindex) += fme.segment<3>(6) + fie.segment<3>(6);
Matrix3d Mxx, Kxx;
Kxx = Kme.block<3, 3>(0, 0); Mxx = Mie.block<3, 3>(0, 0);
fillxMI(MDK_, M_, Kxx, Mxx, aindex, damping, h);
Kxx = Kme.block<3, 3>(3, 3); Mxx = Mie.block<3, 3>(3, 3);
fillxMI(MDK_, M_, Kxx, Mxx, bindex, damping, h);
Kxx = Kme.block<3, 3>(6, 6); Mxx = Mie.block<3, 3>(6, 6);
fillxMI(MDK_, M_, Kxx, Mxx, cindex, damping, h);
Kxx = Kme.block<3, 3>(0, 3); Mxx = Mie.block<3, 3>(0, 3);
fillxxMI(MDK_, M_, Kxx, Mxx, aindex, bindex, damping, h);
Kxx = Kme.block<3, 3>(0, 6); Mxx = Mie.block<3, 3>(0, 6);
fillxxMI(MDK_, M_, Kxx, Mxx, aindex, cindex, damping, h);
Kxx = Kme.block<3, 3>(3, 6); Mxx = Mie.block<3, 3>(3, 6);
fillxxMI(MDK_, M_, Kxx, Mxx, bindex, cindex, damping, h);
}
}
}
void fillxB(vector<T>& MDK_, const Matrix3d& Kxx, int index)
{
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
MDK_.push_back(T(index + j, index + k, Kxx(j, k)));
}
}
}
void fillxxB(vector<T>& MDK_, const Matrix3d& Kxx, int i0, int i1)
{
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
MDK_.push_back(T(i0 + j, i1 + k, Kxx(j, k)));
MDK_.push_back(T(i1 + k, i0 + j, Kxx(j, k)));
}
}
}
void fillXB(vector<T>& MDK_, const Matrix2d& KXX, int index)
{
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 2; k++) {
MDK_.push_back(T(index + j, index + k, KXX(j, k)));
}
}
}
void fillXXB(vector<T>& MDK_, const Matrix2d& KXX, int i0, int i1)
{
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 2; k++) {
MDK_.push_back(T(i0 + j, i1 + k, KXX(j, k)));
MDK_.push_back(T(i1 + k, i0 + j, KXX(j, k)));
}
}
}
void fillXxB(vector<T>& MDK_, const MatrixXd& KXx, int i0, int i1)
{
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 3; k++) {
MDK_.push_back(T(i0 + j, i1 + k, KXx(j, k)));
MDK_.push_back(T(i1 + k, i0 + j, KXx(j, k)));
}
}
}
void fillxXB(vector<T>& MDK_, const MatrixXd& KxX, int i0, int i1)
{
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 2; k++) {
MDK_.push_back(T(i0 + j, i1 + k, KxX(j, k)));
MDK_.push_back(T(i1 + k, i0 + j, KxX(j, k)));
}
}
}
void fillEOLBending(const Edge* edge, const Vert* v0, const Vert* v1, const Vert* v2, const Vert* v3, VectorXd& fb, MatrixXd& Kb)
{
MatrixXd Fa, Fb, Fc, Fd;
bool EOLA = v0->node->EoL; bool EOLB = v1->node->EoL; bool EOLC = v2->node->EoL; bool EOLD = v3->node->EoL;
//if (EOLA) Fa = deform_grad_v(v0);
//if (EOLB) Fb = deform_grad_v(v1);
//if (EOLC) Fc = deform_grad_v(v2);
//if (EOLD) Fd = deform_grad_v(v3);
MatrixXd F1 = deform_grad(edge->adjf[0]);
MatrixXd F2 = deform_grad(edge->adjf[1]);
if (EOLA) Fa = (F1 + F2) / 2;
if (EOLB) Fb = (F1 + F2) / 2;
if (EOLC) Fc = (F1 + F2) / 2;
if (EOLD) Fd = (F1 + F2) / 2;
Vector3d fba = fb.segment<3>(0); Vector3d fbb = fb.segment<3>(3); Vector3d fbc = fb.segment<3>(6); Vector3d fbd = fb.segment<3>(9);
Matrix3d Kbaa = Kb.block<3, 3>(0, 0); Matrix3d Kbab = Kb.block<3, 3>(0, 3); Matrix3d Kbac = Kb.block<3, 3>(0, 6); MatrixXd Kbad = Kb.block<3, 3>(0, 9);
Matrix3d Kbba = Kb.block<3, 3>(3, 0); Matrix3d Kbbb = Kb.block<3, 3>(3, 3); Matrix3d Kbbc = Kb.block<3, 3>(3, 6); MatrixXd Kbbd = Kb.block<3, 3>(3, 9);
Matrix3d Kbca = Kb.block<3, 3>(6, 0); Matrix3d Kbcb = Kb.block<3, 3>(6, 3); Matrix3d Kbcc = Kb.block<3, 3>(6, 6); MatrixXd Kbcd = Kb.block<3, 3>(6, 9);
Matrix3d Kbda = Kb.block<3, 3>(9, 0); Matrix3d Kbdb = Kb.block<3, 3>(9, 3); Matrix3d Kbdc = Kb.block<3, 3>(9, 6); MatrixXd Kbdd = Kb.block<3, 3>(9, 9);
int ja = 0; int jA = 3; int jb = 5; int jB = 8; int jc = 10; int jC = 13; int jd = 15; int jD = 18;
fb.segment<3>(ja) = fba; fb.segment<3>(jb) = fbb; fb.segment<3>(jc) = fbc; fb.segment<3>(jd) = fbd;
Kb.block<3, 3>(ja, ja) = Kbaa; Kb.block<3, 3>(ja, jb) = Kbab; Kb.block<3, 3>(ja, jc) = Kbac; Kb.block<3, 3>(ja, jd) = Kbad;
/*Kd.block<3, 3>(jb, ja) = Kbba;*/ Kb.block<3, 3>(jb, jb) = Kbbb; Kb.block<3, 3>(jb, jc) = Kbbc; Kb.block<3, 3>(jb, jd) = Kbbd;
/*Kd.block<3, 3>(jc, ja) = Kbca; Kb.block<3, 3>(jc, jb) = Kbcb;*/ Kb.block<3, 3>(jc, jc) = Kbcc; Kb.block<3, 3>(jc, jd) = Kbcd;
/*Kd.block<3, 3>(jd, ja) = Kbda; Kb.block<3, 3>(jd, jb) = Kbdb; Kb.block<3, 3>(jd, jc) = Kbdc;*/ Kb.block<3, 3>(jd, jd) = Kbdd;
if (EOLA) {
fb.segment<2>(jA) = -Fa.transpose() * fba;
Kb.block<2, 2>(jA, jA) = Fa.transpose() * Kbaa * Fa;
}
if (EOLB) {
fb.segment<2>(jB) = -Fb.transpose() * fbb;
Kb.block<2, 2>(jB, jB) = Fb.transpose() * Kbbb * Fb;
}
if (EOLC) {
fb.segment<2>(jC) = -Fc.transpose() * fbc;
Kb.block<2, 2>(jC, jC) = Fc.transpose() * Kbcc * Fc;
}
if (EOLD) {
fb.segment<2>(jD) = -Fd.transpose() * fbd;
Kb.block<2, 2>(jD, jD) = Fd.transpose() * Kbdd * Fd;
}
if (EOLA && EOLB) {
Kb.block<2, 2>(jA, jB) = Fa.transpose() * Kbab * Fb;
}
if (EOLA && EOLC) {
Kb.block<2, 2>(jA, jC) = Fa.transpose() * Kbac * Fc;
}
if (EOLA && EOLD) {
Kb.block<2, 2>(jA, jD) = Fa.transpose() * Kbad * Fd;
}
if (EOLB && EOLC) {
Kb.block<2, 2>(jB, jC) = Fb.transpose() * Kbbc * Fc;
}
if (EOLB && EOLD) {
Kb.block<2, 2>(jB, jD) = Fb.transpose() * Kbbd * Fd;
}
if (EOLC && EOLD) {
Kb.block<2, 2>(jC, jD) = Fc.transpose() * Kbcd * Fd;
}
if (EOLA) {
Kb.block<2, 3>(jA, ja) = -Fa.transpose() * Kbaa;
Kb.block<2, 3>(jA, jb) = -Fa.transpose() * Kbab;
Kb.block<2, 3>(jA, jc) = -Fa.transpose() * Kbac;
Kb.block<2, 3>(jA, jd) = -Fa.transpose() * Kbad;
}
if (EOLB) {
//Kb.block<2, 3>(jB, ja) = -Fb.transpose() * Kbba;
Kb.block<2, 3>(jB, jb) = -Fb.transpose() * Kbbb;
Kb.block<2, 3>(jB, jc) = -Fb.transpose() * Kbbc;
Kb.block<2, 3>(jB, jd) = -Fb.transpose() * Kbbd;
Kb.block<3, 2>(ja, jB) = -Kbab * Fb;
}
if (EOLC) {
//Kb.block<2, 3>(jC, ja) = -Fc.transpose() * Kbca;
//Kb.block<2, 3>(jC, jb) = -Fc.transpose() * Kbcb;
Kb.block<2, 3>(jC, jc) = -Fc.transpose() * Kbcc;
Kb.block<2, 3>(jC, jd) = -Fc.transpose() * Kbcd;
Kb.block<3, 2>(ja, jC) = -Kbac * Fc;
Kb.block<3, 2>(jb, jC) = -Kbbc * Fc;
}
if (EOLD) {
//Kb.block<2, 3>(jD, ja) = -Fd.transpose() * Kbda;
//Kb.block<2, 3>(jD, jb) = -Fd.transpose() * Kbdb;
//Kb.block<2, 3>(jD, jc) = -Fd.transpose() * Kbdc;
Kb.block<2, 3>(jD, jd) = -Fd.transpose() * Kbdd;
Kb.block<3, 2>(ja, jD) = -Kbad * Fd;
Kb.block<3, 2>(jb, jD) = -Kbbd * Fd;
Kb.block<3, 2>(jc, jD) = -Kbcd * Fd;
}
}
void edgeBasedF(const Mesh& mesh, const Material& mat, VectorXd& f, 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) {
fillEOLBending(edge, v0, v1, v2, v3, fbe, Kbe);
f.segment<3>(aindex) += fbe.segment<3>(0);
if (to_eolA) f.segment<2>(aindexX) += fbe.segment<2>(3);
f.segment<3>(bindex) += fbe.segment<3>(5);
if (to_eolB) f.segment<2>(bindexX) += fbe.segment<2>(8);
f.segment<3>(cindex) += fbe.segment<3>(10);
if (to_eolC) f.segment<2>(cindexX) += fbe.segment<2>(13);
f.segment<3>(dindex) += fbe.segment<3>(15);
if (to_eolD) f.segment<2>(dindexX) += fbe.segment<2>(18);
Matrix3d Kxx;
Kxx = damping(1) * h * h * Kbe.block<3, 3>(0, 0);
fillxB(MDK_, Kxx, aindex);
Kxx = damping(1) * h * h * Kbe.block<3, 3>(5, 5);
fillxB(MDK_, Kxx, bindex);
Kxx = damping(1) * h * h * Kbe.block<3, 3>(10, 10);
fillxB(MDK_, Kxx, cindex);
Kxx = damping(1) * h * h * Kbe.block<3, 3>(15, 15);
fillxB(MDK_, Kxx, dindex);
Kxx = damping(1) * h * h * Kbe.block<3, 3>(0, 5);
fillxxB(MDK_, Kxx, aindex, bindex);
Kxx = damping(1) * h * h * Kbe.block<3, 3>(0, 10);
fillxxB(MDK_, Kxx, aindex, cindex);
Kxx = damping(1) * h * h * Kbe.block<3, 3>(0, 15);
fillxxB(MDK_, Kxx, aindex, dindex);
Kxx = damping(1) * h * h * Kbe.block<3, 3>(5, 10);
fillxxB(MDK_, Kxx, bindex, cindex);
Kxx = damping(1) * h * h * Kbe.block<3, 3>(5, 15);
fillxxB(MDK_, Kxx, bindex, dindex);
Kxx = damping(1) * h * h * Kbe.block<3, 3>(10, 15);
fillxxB(MDK_, Kxx, cindex, dindex);
Matrix2d KXX;
if (to_eolA) {
KXX = damping(1) * h * h * Kbe.block<2, 2>(3, 3);
fillXB(MDK_, KXX, aindexX);
}
if (to_eolB) {
KXX = damping(1) * h * h * Kbe.block<2, 2>(8, 8);
fillXB(MDK_, KXX, bindexX);
}
if (to_eolC) {
KXX = damping(1) * h * h * Kbe.block<2, 2>(13, 13);
fillXB(MDK_, KXX, cindexX);
}
if (to_eolD) {
KXX = damping(1) * h * h * Kbe.block<2, 2>(18, 18);
fillXB(MDK_, KXX, dindexX);
}
if (to_eolA && to_eolB) {
KXX = damping(1) * h * h * Kbe.block<2, 2>(3, 8);
fillXXB(MDK_, KXX, aindexX, bindexX);
}
if (to_eolA && to_eolC) {
KXX = damping(1) * h * h * Kbe.block<2, 2>(3, 13);
fillXXB(MDK_, KXX, aindexX, cindexX);
}
if (to_eolA && to_eolD) {
KXX = damping(1) * h * h * Kbe.block<2, 2>(3, 18);
fillXXB(MDK_, KXX, aindexX, dindexX);
}
if (to_eolB && to_eolC) {
KXX = damping(1) * h * h * Kbe.block<2, 2>(8, 13);
fillXXB(MDK_, KXX, bindexX, cindexX);
}
if (to_eolB && to_eolD) {
KXX = damping(1) * h * h * Kbe.block<2, 2>(8, 18);
fillXXB(MDK_, KXX, bindexX, dindexX);
}
if (to_eolC && to_eolD) {
KXX = damping(1) * h * h * Kbe.block<2, 2>(13, 18);
fillXXB(MDK_, KXX, cindexX, dindexX);
}
MatrixXd KXx, KxX;
if (to_eolA) {
KXx = damping(1) * h * h * Kbe.block<2, 3>(3, 0);
fillXxB(MDK_, KXx, aindexX, aindex);
KXx = damping(1) * h * h * Kbe.block<2, 3>(3, 5);
fillXxB(MDK_, KXx, aindexX, bindex);
KXx = damping(1) * h * h * Kbe.block<2, 3>(3, 10);
fillXxB(MDK_, KXx, aindexX, cindex);
KXx = damping(1) * h * h * Kbe.block<2, 3>(3, 15);
fillXxB(MDK_, KXx, aindexX, dindex);
}
if (to_eolB) {
//KXx = damping(1) * h * h * Kbe.block<2, 3>(8, 0);
//fillXxB(MDK_, KXx, bindexX, aindex);
KXx = damping(1) * h * h * Kbe.block<2, 3>(8, 5);
fillXxB(MDK_, KXx, bindexX, bindex);
KXx = damping(1) * h * h * Kbe.block<2, 3>(8, 10);
fillXxB(MDK_, KXx, bindexX, cindex);
KXx = damping(1) * h * h * Kbe.block<2, 3>(8, 15);
fillXxB(MDK_, KXx, bindexX, dindex);
KxX = damping(1) * h * h * Kbe.block<3, 2>(0, 8);
fillxXB(MDK_, KxX, aindex, bindexX);
}
if (to_eolC) {
//KXx = damping(1) * h * h * Kbe.block<2, 3>(13, 0);
//fillXxB(MDK_, KXx, cindexX, aindex);
//KXx = damping(1) * h * h * Kbe.block<2, 3>(13, 5);
//fillXxB(MDK_, KXx, cindexX, bindex);
KXx = damping(1) * h * h * Kbe.block<2, 3>(13, 10);
fillXxB(MDK_, KXx, cindexX, cindex);
KXx = damping(1) * h * h * Kbe.block<2, 3>(13, 15);
fillXxB(MDK_, KXx, cindexX, dindex);
KxX = damping(1) * h * h * Kbe.block<3, 2>(0, 13);
fillxXB(MDK_, KxX, aindex, cindexX);
KxX = damping(1) * h * h * Kbe.block<3, 2>(5, 13);
fillxXB(MDK_, KxX, bindex, cindexX);
}
if (to_eolD) {
//KXx = damping(1) * h * h * Kbe.block<2, 3>(18, 0);
//fillXxB(MDK_, KXx, dindexX, aindex);
//KXx = damping(1) * h * h * Kbe.block<2, 3>(18, 5);
//fillXxB(MDK_, KXx, dindexX, bindex);
//KXx = damping(1) * h * h * Kbe.block<2, 3>(18, 10);
//fillXxB(MDK_, KXx, dindexX, cindex);
KXx = damping(1) * h * h * Kbe.block<2, 3>(18, 15);
fillXxB(MDK_, KXx, dindexX, dindex);
KxX = damping(1) * h * h * Kbe.block<3, 2>(0, 18);
fillxXB(MDK_, KxX, aindex, dindexX);
KxX = damping(1) * h * h * Kbe.block<3, 2>(5, 18);
fillxXB(MDK_, KxX, bindex, dindexX);
KxX = damping(1) * h * h * Kbe.block<3, 2>(10, 18);
fillxXB(MDK_, KxX, cindex, dindexX);
}
}
else {
Matrix3d Kxx;
Kxx = damping(1) * h * h * Kbe.block<3, 3>(0, 0);
fillxB(MDK_, Kxx, aindex);
Kxx = damping(1) * h * h * Kbe.block<3, 3>(3, 3);
fillxB(MDK_, Kxx, bindex);
Kxx = damping(1) * h * h * Kbe.block<3, 3>(6, 6);
fillxB(MDK_, Kxx, cindex);
Kxx = damping(1) * h * h * Kbe.block<3, 3>(9, 9);
fillxB(MDK_, Kxx, dindex);
Kxx = damping(1) * h * h * Kbe.block<3, 3>(0, 3);
fillxxB(MDK_, Kxx, aindex, bindex);
Kxx = damping(1) * h * h * Kbe.block<3, 3>(0, 6);
fillxxB(MDK_, Kxx, aindex, cindex);
Kxx = damping(1) * h * h * Kbe.block<3, 3>(0, 9);
fillxxB(MDK_, Kxx, aindex, dindex);
Kxx = damping(1) * h * h * Kbe.block<3, 3>(3, 6);
fillxxB(MDK_, Kxx, bindex, cindex);
Kxx = damping(1) * h * h * Kbe.block<3, 3>(3, 9);
fillxxB(MDK_, Kxx, bindex, dindex);
Kxx = damping(1) * h * h * Kbe.block<3, 3>(6, 9);
fillxxB(MDK_, Kxx, cindex, dindex);
}
}
}
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_;
EoL_cutoff = mesh.nodes.size() * 3;
//nodeBasedF(mesh, f, M_, grav);
faceBasedF(mesh, f, MDK_, M_, grav, h);
edgeBasedF(mesh, mat, f, 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());
}
#ifdef EOLC_ONLINE
void Forces::drawSimple(const Mesh &mesh, shared_ptr<MatrixStack> MV, const shared_ptr<Program> prog) const
{
if (f.size() == 0) return;
for (int n = 0; n < mesh.nodes.size(); n++) {
Node* node = mesh.nodes[n];
int nodei = n * 3;
if (nodei >= f.size()) continue;
double scale = 10.0;
glColor3f(1.0f, 0.0f, 1.0f);
glBegin(GL_LINES);
glVertex3f(node->x[0], node->x[1], node->x[2]);
glVertex3f(node->x[0] + (f(nodei) * scale), node->x[1] + (f(nodei + 1) * scale), node->x[2] + (f(nodei + 2) * scale));
glEnd();
if (node->EoL) {
int nodeEi = mesh.nodes.size() * 3 + node->EoL_index * 2;
MatrixXd F = deform_grad(node->verts[0]->adjf[0]);
Vector3d dF = F * f.segment<2>(nodeEi);
glColor3f(0.0f, 0.0f, 1.0f);
glBegin(GL_LINES);
glVertex3f(node->x[0], node->x[1], node->x[2]);
glVertex3f(node->x[0] + (dF(0) * scale), node->x[1] + (dF(1) * scale), node->x[2] + (dF(2) * scale));
glEnd();
}
}
}
#endif
Computing file changes ...
