StVKReducedInternalForces::StVKReducedInternalForces(int r, double * U, VolumetricMesh * volumetricMesh, StVKElementABCD * precomputedABCDIntegrals, int initOnly, bool addGravity_, double g_, int verbose_): precomputedIntegrals(precomputedABCDIntegrals), unitReducedGravityForce(NULL), reducedGravityForce(NULL), addGravity(addGravity_), g(g_), useSingleThread(0), shallowCopy(0), verbose(verbose_)
{
int numElements = volumetricMesh->getNumElements();
lambdaLame = (double*) malloc (sizeof(double) * numElements);
muLame = (double*) malloc (sizeof(double) * numElements);
for(int el=0; el<numElements; el++)
{
VolumetricMesh::Material * material = volumetricMesh->getElementMaterial(el);
VolumetricMesh::ENuMaterial * eNuMaterial = downcastENuMaterial(material);
if (eNuMaterial == NULL)
{
printf("Error: StVKReducedInternalForces: mesh does not consist of E, nu materials.\n");
throw 1;
}
lambdaLame[el] = eNuMaterial->getLambda();
muLame[el] = eNuMaterial->getMu();
}
InitComputation(r, U, volumetricMesh);
if (!initOnly)
ProcessElements(0, volumetricMesh->getNumElements());
InitGravity();
}
StVKStiffnessMatrix::StVKStiffnessMatrix(StVKInternalForces * stVKInternalForces)
{
precomputedIntegrals = stVKInternalForces->GetPrecomputedIntegrals();
volumetricMesh = stVKInternalForces->GetVolumetricMesh();
numElementVertices = volumetricMesh->getNumElementVertices();
int numElements = volumetricMesh->getNumElements();
lambdaLame = (double*) malloc (sizeof(double) * numElements);
muLame = (double*) malloc (sizeof(double) * numElements);
for(int el=0; el<numElements; el++)
{
VolumetricMesh::Material * material = volumetricMesh->getElementMaterial(el);
VolumetricMesh::ENuMaterial * eNuMaterial = downcastENuMaterial(material);
if (eNuMaterial == NULL)
{
printf("Error: mesh does not consist of E, nu materials.\n");
throw 1;
}
lambdaLame[el] = eNuMaterial->getLambda();
muLame[el] = eNuMaterial->getMu();
}
// build stiffness matrix skeleton
SparseMatrix * stiffnessMatrixTopology;
GetStiffnessMatrixTopology(&stiffnessMatrixTopology);
// build acceleration indices
row_ = (int**) malloc (sizeof(int*) * numElements);
column_ = (int**) malloc (sizeof(int*) * numElements);
for (int el=0; el < numElements; el++)
{
row_[el] = (int*) malloc (sizeof(int) * numElementVertices);
column_[el] = (int*) malloc (sizeof(int) * numElementVertices * numElementVertices);
for(int ver=0; ver<numElementVertices; ver++)
row_[el][ver] = volumetricMesh->getVertexIndex(el, ver);
// seek for value row[j] in list associated with row[i]
for(int i=0; i<numElementVertices; i++)
for(int j=0; j<numElementVertices; j++)
column_[el][numElementVertices * i + j] =
stiffnessMatrixTopology->GetInverseIndex(3*row_[el][i],3*row_[el][j]) / 3;
}
delete(stiffnessMatrixTopology);
}
StVKHessianTensor::StVKHessianTensor(StVKStiffnessMatrix * stVKStiffnessMatrix_): stVKStiffnessMatrix(stVKStiffnessMatrix_), volumetricMesh(stVKStiffnessMatrix->GetVolumetricMesh()), precomputedIntegrals(stVKStiffnessMatrix->GetPrecomputedIntegrals())
{
numVertices_ = volumetricMesh->getNumVertices();
numElements_ = volumetricMesh->getNumElements();
numElementVertices = volumetricMesh->getNumElementVertices();
lambdaLame = (double*) malloc (sizeof(double) * numElements_);
muLame = (double*) malloc (sizeof(double) * numElements_);
for(int el=0; el<numElements_; el++)
{
VolumetricMesh::Material * material = volumetricMesh->getElementMaterial(el);
VolumetricMesh::ENuMaterial * eNuMaterial = downcastENuMaterial(material);
if (eNuMaterial == NULL)
{
printf("Error: StVKHessianTensor: mesh does not consist of E, nu materials.\n");
throw 1;
}
lambdaLame[el] = eNuMaterial->getLambda();
muLame[el] = eNuMaterial->getMu();
}
}
StVKInternalForces::StVKInternalForces(VolumetricMesh * volumetricMesh_, StVKElementABCD * precomputedABCDIntegrals_, bool addGravity_, double g_): volumetricMesh(volumetricMesh_), precomputedIntegrals(precomputedABCDIntegrals_), gravityForce(NULL), addGravity(addGravity_), g(g_)
{
int numElements = volumetricMesh->getNumElements();
lambdaLame = (double*) malloc (sizeof(double) * numElements);
muLame = (double*) malloc (sizeof(double) * numElements);
for(int el=0; el<numElements; el++)
{
VolumetricMesh::Material * material = volumetricMesh->getElementMaterial(el);
VolumetricMesh::ENuMaterial * eNuMaterial = downcastENuMaterial(material);
if (eNuMaterial == NULL)
{
printf("Error: mesh does not consist of E, nu materials.\n");
throw 1;
}
lambdaLame[el] = eNuMaterial->getLambda();
muLame[el] = eNuMaterial->getMu();
}
buffer = (double*) malloc (sizeof(double) * 3 * volumetricMesh->getNumVertices());
numElementVertices = volumetricMesh->getNumElementVertices();
InitGravity();
}
CorotationalLinearFEM::CorotationalLinearFEM(TetMesh * tetMesh_) : tetMesh(tetMesh_)
{
numVertices = tetMesh->getNumVertices();
// store the undeformed positions
undeformedPositions = (double*) malloc (sizeof(double) * 3 * numVertices);
for(int i=0; i < numVertices; i++)
{
Vec3d * v = tetMesh->getVertex(i);
for(int j=0; j<3; j++)
undeformedPositions[3*i+j] = (*v)[j];
}
int numElements = tetMesh->getNumElements();
MInverse = (double**) malloc (sizeof(double*) * numElements);
for(int el = 0; el < numElements; el++)
{
// get the integer indices of the tet vertices
int vtxIndex[4];
for(int vtx=0; vtx<4; vtx++)
vtxIndex[vtx] = tetMesh->getVertexIndex(el, vtx);
/*
Form matrix:
M = [ v0 v1 v2 v3 ]
[ 1 1 1 1 ]
*/
double M[16]; // row-major
for(int vtx=0; vtx<4; vtx++)
for(int dim=0; dim<3; dim++)
M[4 * dim + vtx] = undeformedPositions[3 * vtxIndex[vtx] + dim];
M[12] = M[13] = M[14] = M[15] = 1.0;
// invert M and cache inverse (see [Mueller 2004])
MInverse[el] = (double*) malloc (sizeof(double) * 16);
inverse4x4(M, MInverse[el]);
}
// build acceleration indices for fast writing to the global stiffness matrix
SparseMatrix * sparseMatrix;
GetStiffnessMatrixTopology(&sparseMatrix);
BuildRowColumnIndices(sparseMatrix);
delete(sparseMatrix);
// compute stiffness matrices for all the elements in the undeformed configuration
KElementUndeformed = (double**) malloc (sizeof(double*) * numElements);
for (int el = 0; el < numElements; el++)
{
double * MInv = MInverse[el];
// Form stiffness matrix of the element in the undeformed configuration.
// The procedure below is standard in FEM solid mechanics.
// This code implements the equations given in Ahmed A. Shabana: Theory of Vibration, Volume II: Discrete and Continuous Systems, Springer--Verlag, New York, NY, 1990.
double B[72] =
{ MInv[0], 0, 0, MInv[4], 0, 0, MInv[8], 0, 0, MInv[12], 0, 0,
0, MInv[1], 0, 0, MInv[5], 0, 0, MInv[9], 0, 0, MInv[13], 0,
0, 0, MInv[2], 0, 0, MInv[6], 0, 0, MInv[10], 0, 0, MInv[14],
MInv[1], MInv[0], 0, MInv[5], MInv[4], 0, MInv[9], MInv[8],
0, MInv[13], MInv[12], 0, 0, MInv[2], MInv[1], 0, MInv[6], MInv[5],
0, MInv[10], MInv[9], 0, MInv[14], MInv[13], MInv[2], 0, MInv[0],
MInv[6], 0, MInv[4], MInv[10], 0, MInv[8], MInv[14], 0, MInv[12] };
// compute elasticity stiffness tensor
double E[36]; // 6 x 6 matrix, stored row-major (symmetric, so row vs column-major storage makes no difference anyway)
VolumetricMesh::Material * material = tetMesh->getElementMaterial(el);
// check if material is ENuMaterial (i.e., isotropic)
VolumetricMesh::ENuMaterial * eNuMaterial = downcastENuMaterial(material);
if (eNuMaterial != NULL)
{
// material is isotropic, specified by E, nu
// compute Lame coefficients
double lambda = eNuMaterial->getLambda();
double mu = eNuMaterial->getMu();
double Et[36] = { lambda + 2 * mu, lambda, lambda, 0, 0, 0,
lambda, lambda + 2 * mu, lambda, 0, 0, 0,
lambda, lambda, lambda + 2 * mu, 0, 0, 0,
0, 0, 0, mu, 0, 0,
0, 0, 0, 0, mu, 0,
0, 0, 0, 0, 0, mu };
memcpy(E, Et, sizeof(double) * 36);
}
else
{
// orthotropic material
// we follow the following references:
// Yijing Li and Jernej Barbic: Stable Orthotropic Materials, Symposium on Computer Animation 2014
// http://en.wikipedia.org/wiki/Orthotropic_material
// http://www.solidmechanics.org/text/Chapter3_2/Chapter3_2.htm
// test if material is OrthotropicMaterial (i.e., orthotropic)
VolumetricMesh::OrthotropicMaterial * orthotropicMaterial = downcastOrthotropicMaterial(material);
if (orthotropicMaterial != NULL)
{
double E1 = orthotropicMaterial->getE1();
double E2 = orthotropicMaterial->getE2();
double E3 = orthotropicMaterial->getE3();
double nu12 = orthotropicMaterial->getNu12();
double nu23 = orthotropicMaterial->getNu23();
double nu31 = orthotropicMaterial->getNu31();
double G12 = orthotropicMaterial->getG12();
double G23 = orthotropicMaterial->getG23();
double G31 = orthotropicMaterial->getG31();
double nu21 = nu12 * E2 / E1;
double nu32 = nu23 * E3 / E2;
double nu13 = nu31 * E1 / E3;
double Y = 1.0 / (1.0 - nu12 * nu21 - nu23 * nu32 - nu31 * nu13 - 2.0 * nu21 * nu32 * nu13);
double ELocal[36] = { E1 * (1.0 - nu23 * nu32) * Y, E1 * (nu21 + nu31 * nu23) * Y, E1 * (nu31 + nu21 * nu32) * Y, 0.0, 0.0, 0.0,
E1 * (nu21 + nu31 * nu23) * Y, E2 * (1.0 - nu13 * nu31) * Y, E2 * (nu32 + nu12 * nu31) * Y, 0.0, 0.0, 0.0,
E1 * (nu31 + nu21 * nu32) * Y, E2 * (nu32 + nu12 * nu31) * Y, E3 * (1.0 - nu12 * nu21) * Y, 0.0, 0.0, 0.0,
0, 0, 0, G12, 0, 0,
0, 0, 0, 0, G23, 0,
0, 0, 0, 0, 0, G31 };
//memcpy(E, ELocal, sizeof(double) * 36); // debug
double R[9]; // row-major
orthotropicMaterial->getR(R);
// rotate Elocal into the basis given by the columns of R
#define Relt(i,j) (R[3*(i)+(j)])
double rotator[36];
for(int i=0; i<3; i++)
for(int j=0; j<3; j++)
{
rotator[6 * i + j] = Relt(i,j) * Relt(i,j);
rotator[6 * i + 3 + j] = 2.0 * Relt(i, j) * Relt(i, (j+1) % 3);
rotator[6 * (i + 3) + j] = Relt(i, j) * Relt((i+1) % 3, j);
rotator[6 * (i + 3) + 3 + j] = Relt(i, j) * Relt((i+1) % 3, (j+1) % 3) + Relt(i, (j+1) % 3) * Relt((i+1) % 3, j);
}
#undef Relt
// debug
//memset(rotator, 0, sizeof(double) * 36);
//for(int i=0; i<6; i++)
//rotator[6*i+i] = 1.0;
// E = rotator * ELocal * rotator^T
double buffer[36];
memset(buffer, 0, sizeof(double) * 36);
// buffer = ELocal * rotator^T
for(int i=0; i<6; i++)
for(int j=0; j<6; j++)
for(int k=0; k<6; k++)
buffer[6 * i + j] += ELocal[6 * i + k] * rotator[6 * j + k];
// E = rotator * buffer
memset(E, 0, sizeof(double) * 36);
for(int i=0; i<6; i++)
for(int j=0; j<6; j++)
for(int k=0; k<6; k++)
E[6 * i + j] += rotator[6 * i + k] * buffer[6 * k + j];
}
else
{
printf("Error: CorotationalLinearFEM: unknown material encounted in the mesh.\n");
throw 1;
}
}
// EB = E * B
double EB[72];
memset(EB, 0, sizeof(double) * 72);
for (int i=0; i<6; i++)
for (int j=0; j<12; j++)
for (int k=0; k<6; k++)
EB[12 * i + j] += E[6 * i + k] * B[12 * k + j];
// KElementUndeformed[el] = B^T * EB
KElementUndeformed[el] = (double*) calloc (144, sizeof(double)); // element stiffness matrix
for (int i=0; i<12; i++)
for (int j=0; j<12; j++)
for (int k=0; k<6; k++)
KElementUndeformed[el][12 * i + j] += B[12 * k + i] * EB[12 * k + j];
// KElementUndeformed[el] *= volume
double volume = TetMesh::getTetVolume(tetMesh->getVertex(el,0), tetMesh->getVertex(el,1), tetMesh->getVertex(el,2), tetMesh->getVertex(el,3));
for(int i=0; i<144; i++)
KElementUndeformed[el][i] *= volume;
}
}
ReducedStVKCubatureForceModel::ReducedStVKCubatureForceModel(const int &r,VolumetricMesh *volumetricMesh,double *U,
const int &cubica_num, const double *cubica_weights,const unsigned int *cubica_elements,
double **restpos,bool addGravity, double g)
{
r_=r;
add_gravity_=addGravity;
g_=g;
cubica_num_=cubica_num;
cubica_weights_ = new double[cubica_num_];
cubica_elements_ = new double[cubica_num_];
// std::cout<<"b\n";
for(int i=0;i<cubica_num_;++i)
{
cubica_weights_[i]=cubica_weights[i];
cubica_elements_[i]=cubica_elements[i];
}
// std::cout<<"c\n";
volumetric_mesh_ = volumetricMesh;
U_ = new double*[3*volumetricMesh->getNumVertices()];
for(int i=0;i<3*volumetricMesh->getNumVertices();++i)
{
U_[i] = new double[r_];
for(int j=0;j<r_;++j)
U_[i][j]=0.0;
}
// std::cout<<"d\n";
int num=volumetricMesh->getNumVertices();
//change *U to **U_,U_[vert_idx][basis_idx]
// Matrix<double> Umatrix((int)(3*num),(int)r_);
// Matrix<double> M((int)(3*num),(int)(3*num));
for(int j=0;j<r_;++j)
for(int i=0;i<3*num;++i)
{
U_[i][j]=U[3*num*j+i];
// Umatrix(i,j)=U_[i][j];
}
// for(int i=0;i<3*num;++i)
// for(int j=0;j<3*num;++j)
// {
// if(i==j)
// M(i,j)=1.0/8144;
// else
// M(i,j)=0.0;
// }
//
// Matrix<double> temp=Umatrix.MultiplyT(M);
// Matrix<double> temp1=temp*Umatrix;
// for(int i=0;i<r_;++i)
// {
// for(int j=0;j<r_;++j)
// {
// std::cout<<temp1(i,j)<<",";
// }
// std::cout<<"\n";
// }
// getchar();
//test U is normalized or not
// double test=0.0;
// for(int i=0;i<3*num;++i)
// {
// test+=U_[1][i]*U_[2][i]*(1.0/8144);
// }
// std::cout<<test<<"\n";
restpos_ = new double*[cubica_num_];
for(int i=0;i<cubica_num_;++i)
{
restpos_[i]=new double[12];
for(int j=0;j<12;++j)
restpos_[i][j]=restpos[i][j];
}
//init cubica subbasis
cubica_subBasis_=new double**[(int)cubica_num_];
for(int i=0;i<cubica_num_;++i)
{
cubica_subBasis_[i]=new double*[12];
for(int j=0;j<12;++j)
{
cubica_subBasis_[i][j]=new double[(int)r_];
}
}
for(int i=0;i<cubica_num_;++i)
{
int ele=cubica_elements_[i];
for(int j=0;j<4;++j)
{
int vertID=volumetric_mesh_->getVertexIndex(ele,j);
for(int k=0;k<r_;++k)
{
cubica_subBasis_[i][3*j][k]=U_[3*vertID][k];
cubica_subBasis_[i][3*j+1][k]=U_[3*vertID+1][k];
cubica_subBasis_[i][3*j+2][k]=U_[3*vertID+2][k];
}
}
}
deformed_=new double[12];
memset(deformed_,0.0,sizeof(double)*12);
// modal_matrix_ = new ModalMatrix(volumetric_mesh_->getNumVertices(),r_,U);
VolumetricMesh::Material * material = volumetric_mesh_->getElementMaterial(0);
VolumetricMesh::ENuMaterial * eNuMaterial = downcastENuMaterial(material);
if (eNuMaterial == NULL)
{
std::cout<<"Error: mesh does not consist of E, nu materials.\n";
exit(0);
}
lamda_ = eNuMaterial->getLambda();
mu_ = eNuMaterial->getMu();
gf_=new double[(int)r];
// std::cout<<"f\n";
InitGravity(U);
}