// Read this mesh from the FEM framework's mesh m
void readFEM(int m,TetMesh &t) {
int nNode=FEM_Mesh_get_length(m,FEM_NODE);
int nTet=FEM_Mesh_get_length(m,FEM_ELEM+0);
t.allocate(nTet, nNode);
FEM_Mesh_data(m,FEM_NODE,FEM_COORD,t.getPointArray(),0,nNode,FEM_DOUBLE,3);
FEM_Mesh_data(m,FEM_ELEM+0,FEM_CONN,t.getTetConn(),0,nTet,FEM_INDEX_0,4);
}
void ComputeMassMat::computeCompactM(SXMatrix &M,const TetMesh& mesh)const{
const int n = (int)mesh.nodes().size();
M.resize(n,n);
for(int i=0;i<(int)mesh.tets().size();i++)
assembleMass(mesh,mesh.tets()[i],_density[i],M);
}
float
max_dihedral_angle(const TetMesh& mesh)
{
// Measure maximum dihedral angle of tet mesh.
float maxAngle = 0.0f;
for (size_t t = 0; t < mesh.tSize(); ++t) {
std::vector<float> angles;
mesh.getTet(t).dihedralAngles(angles);
for (size_t i = 0; i < angles.size(); ++i) {
if (angles[i] > maxAngle) {
maxAngle = angles[i];
}
}
}
return maxAngle * 180.0f / M_PI;
}
// cut a chunk of the lattice out to roughly match the shape
static void
cut_lattice(const SDF& sdf,
TetMesh& mesh,
std::vector<float>& vphi) // phi evaluated at vertices
{
std::map<Vec3i,int> lattice_map;
// loop over tiles that overlap bounding box
int i, j, k;
for (k=0; k<sdf.phi.nk; k+=4) for (j=0; j<sdf.phi.nj; j+=4)
for (i=0; i<sdf.phi.ni; i+=4) {
for (int t=0; t<num_lattice_tets; ++t) {
// check this tet is entirely inside the grid & has a negative phi
bool good_tet = true;
bool negative_phi = false;
for (int u=0; u<4; ++u) {
int a = lattice_node[lattice_tet[t][u]][0]+i;
if (a >= sdf.phi.ni) { good_tet=false; break; }
int b = lattice_node[lattice_tet[t][u]][1]+j;
if (b >= sdf.phi.nj) { good_tet=false; break; }
int c = lattice_node[lattice_tet[t][u]][2]+k;
if (c >= sdf.phi.nk) { good_tet=false; break; }
if (sdf.phi(a,b,c) < 0)
negative_phi = true;
}
if (good_tet && negative_phi) {
// add vertices if they don't exist yet, find actual tet to add
Vec4i actual_tet(-1,-1,-1,-1);
for (int u=0; u<4; ++u) {
Vec3i vertex=lattice_node[lattice_tet[t][u]]+Vec3i(i,j,k);
std::map<Vec3i,int>::iterator p = lattice_map.find(vertex);
if (p == lattice_map.end()) {
actual_tet[u] = mesh.vSize();
lattice_map[vertex] = actual_tet[u];
mesh.verts().push_back(sdf.origin+sdf.dx*Vec3f(vertex));
vphi.push_back(sdf.phi(vertex[0],vertex[1],vertex[2]));
} else {
actual_tet[u]=p->second;
}
}
mesh.tets().push_back(actual_tet);
}
}
}
}
void ComputeMassMat::assembleMass(const TetMesh& mesh,const Vector4i& tet,const SX& density,SXMatrix &M)const{
SX influence=tetrahedron(mesh.nodes()[tet[0]],mesh.nodes()[tet[1]],
mesh.nodes()[tet[2]],mesh.nodes()[tet[3]]).volume()/20.0f;
influence*=density;
ADD_INFLUENCE_AD(0,0,M);
ADD_INFLUENCE_AD(1,1,M);
ADD_INFLUENCE_AD(2,2,M);
ADD_INFLUENCE_AD(3,3,M);
ADD_INFLUENCE_AD(1,0,M);
ADD_INFLUENCE_AD(2,0,M);
ADD_INFLUENCE_AD(3,0,M);
ADD_INFLUENCE_AD(1,2,M);
ADD_INFLUENCE_AD(2,3,M);
ADD_INFLUENCE_AD(3,1,M);
}
// assuming vphi[i] and vphi[j] have different signs, find or create a new
// vertex on the edge between them where phi interpolates to zero.
static int
cut_edge(int i, int j,
TetMesh& mesh,
std::vector<float> &vphi,
std::map<Vec2i,int> &cut_map)
{
assert((vphi[i]<0 && vphi[j]>0) || (vphi[i]>0 && vphi[j]<0));
Vec2i edge(min(i,j), max(i,j));
std::map<Vec2i,int>::iterator p = cut_map.find(edge);
if (p == cut_map.end()) { // this edge hasn't been cut yet?
int v = (int)mesh.vSize();
float alpha = vphi[i]/(vphi[i]-vphi[j]);
mesh.verts().push_back((1-alpha)*mesh.V(i) + alpha*mesh.V(j));
vphi.push_back(0);
cut_map[edge] = v;
return v;
} else { // already done this edge
return p->second;
}
}
// remove tets with corners where phi=0 but are outside the volume
static void
remove_exterior_tets(TetMesh& mesh,
const std::vector<float> &vphi,
const SDF& sdf)
{
for (size_t t=0; t<mesh.tSize(); ) {
int i, j, k, l; assign(mesh.T(t), i, j, k, l);
assert(vphi[i]<=0 && vphi[j]<=0 && vphi[k]<=0 && vphi[l]<=0);
if (vphi[i]==0 && vphi[j]==0 && vphi[k]==0 && vphi[l]==0
&& sdf((mesh.V(i)+mesh.V(j)+mesh.V(k)+mesh.V(l))/4)>0) {
mesh.T(t) = mesh.tets().back();
mesh.tets().pop_back();
} else {
++t;
}
}
}
// Read this mesh from the FEM framework's mesh m and include ghosts
void readGhostFEM(int m,TetMesh &t) {
int nNode=FEM_Mesh_get_length(m,FEM_NODE);
int ngNode=FEM_Mesh_get_length(m,FEM_NODE+FEM_GHOST);
int nTet=FEM_Mesh_get_length(m,FEM_ELEM+0);
int ngTet=FEM_Mesh_get_length(m,FEM_ELEM+0+FEM_GHOST);
t.allocate(nTet+ngTet, nNode+ngNode);
FEM_Mesh_data(m,FEM_NODE,FEM_COORD,t.getPointArray(),0,nNode,FEM_DOUBLE,3);
FEM_Mesh_data(m,FEM_ELEM+0,FEM_CONN,t.getTetConn(),0,nTet,FEM_INDEX_0,4);
// add ghost data if it exists
if (ngTet > 0) {
FEM_Mesh_data(m,FEM_ELEM+0+FEM_GHOST,FEM_CONN,t.getTet(nTet),0,ngTet,
FEM_INDEX_0,4);
if (ngNode > 0)
FEM_Mesh_data(m,FEM_NODE+FEM_GHOST,FEM_COORD,t.getPoint(nNode),0,ngNode,
FEM_DOUBLE,3);
}
t.nonGhostPt = nNode;
t.nonGhostTet = nTet;
// convert ghost node indices from negative to positive
//printf("nNode=%d ngNode=%d nTet=%d ngTet=%d\n", nNode, ngNode, nTet, ngTet);
if (ngTet > 0) {
int *tconn;
for (int i=nTet; i<ngTet+nTet; i++) {
tconn = t.getTet(i);
for (int j=0; j<4; j++) {
//printf("Tet=%d conn[%d]=%d before conversion.\n", i, j, tconn[j]);
if (tconn[j] < -1) tconn[j] = (tconn[j]*-1)-2+nNode;
//printf("Tet=%d conn[%d]=%d after conversion.\n", i, j, tconn[j]);
assert(tconn[j] < nNode+ngNode);
assert(!(tconn[j] < -1));
}
}
}
}
int main(int argc, char** argv)
{
po::variables_map var_map = read_command_line(argc,argv);
TetMesh mesh;
string mesh_file = var_map["mesh"].as<string>();
cout << "Reading in mesh from [" << mesh_file << "]..." << endl;
mesh.read_cgal(mesh_file);
mesh.freeze();
cout << "Mesh stats:" << endl;
cout << "\tNumber of vertices: " << mesh.number_of_vertices() << endl;
cout << "\tNumber of tetrahedra: " << mesh.number_of_cells() << endl;
mat bounds = mesh.find_bounding_box();
vec lb = bounds.col(0);
vec ub = bounds.col(1);
cout << "\tLower bound:" << lb.t();
cout << "\tUpper bound:" << ub.t();
RoundaboutDubinsCarSimulator dubins = RoundaboutDubinsCarSimulator(DUBINS_ACTIONS);
bool include_oob = true;
LCP L = build_lcp(&dubins,
&mesh,
DUBINS_GAMMA,
include_oob);
//string filename = var_map["lcp"].as<string>();
//L.write(filename);
string filename = "/home/epz/data/dubins.lcp";
L.write(filename);
}
void ComputeMassMat::compute(SXMatrix &M,const TetMesh&mesh, const vector<SX> &density){
assert_eq(density.size(),mesh.tets().size());
this->_density = density;
_M.setZero();
computeCompactM(_M,mesh);
const int n = mesh.nodes().size();
M.resize(n*3,n*3);
M.setZero();
for (int i = 0; i < _M.size1(); ++i){
for (int j = 0; j < _M.size2(); ++j){
if( _M.hasNZ(i,j) ){
M.elem(i*3+0,j*3+0) = _M.elem(i,j);
M.elem(i*3+1,j*3+1) = _M.elem(i,j);
M.elem(i*3+2,j*3+2) = _M.elem(i,j);
}
}
}
}
int OPS_TetMesh()
{
if (OPS_GetNumRemainingInputArgs() < 6) {
opserr<<"WARNING: want tag? nummesh? mtags? id? ndf? size? eleType? eleArgs?\n";
return -1;
}
// get tag and number mesh
int num = 2;
int idata[2];
if (OPS_GetIntInput(&num,idata) < 0) {
opserr<<"WARNING: failed to read mesh tag and number of 2D boundary mesh\n";
return -1;
}
if (OPS_GetNumRemainingInputArgs() < idata[1]+3) {
opserr<<"WARNING: want mtags? id? ndf? size? <eleType? eleArgs?>\n";
return -1;
}
// create mesh
TetMesh* mesh = new TetMesh(idata[0]);
if(OPS_addMesh(mesh) == false) {
opserr<<"WARNING: failed to add mesh\n";
return -1;
}
// get mesh tags
num = idata[1];
ID mtags(num);
if (OPS_GetIntInput(&num,&mtags(0)) < 0) {
opserr<<"WARNING: failed to read boundary mesh tags\n";
return -1;
}
mesh->setMeshTags(mtags);
// get id and ndf
num = 2;
int data[2];
if (OPS_GetIntInput(&num,data) < 0) {
opserr<<"WARNING: failed to read id and ndf\n";
return -1;
}
mesh->setID(data[0]);
mesh->setNdf(data[1]);
// get size
num = 1;
double size;
if (OPS_GetDoubleInput(&num,&size) < 0) {
opserr<<"WARNING: failed to read mesh size\n";
return -1;
}
mesh->setMeshsize(size);
// set eleArgs
if (mesh->setEleArgs() < 0) {
opserr << "WARNING: failed to set element arguments\n";
return -1;
}
// mesh
if (mesh->mesh() < 0) {
opserr<<"WARNING: failed to do triangular mesh\n";
return -1;
}
return 0;
}
void
make_tet_mesh(TetMesh& mesh,
const SDF& sdf,
FeatureSet& featureSet,
bool optimize,
bool intermediate,
bool unsafe)
{
// Initialize exact arithmetic
initialize_exact();
// Initialize mesh from acute lattice.
std::vector<float> vphi; // SDF value at each lattice vertex.
mesh.verts().resize(0);
mesh.tets().resize(0);
std::cout<<" cutting from lattice"<<std::endl;
cut_lattice(sdf, mesh, vphi);
if (intermediate) {
static const char* cutLatticeFile = "1_cut_lattice.tet";
static const char* cutLatticeInfo = "1_cut_lattice.info";
mesh.writeToFile(cutLatticeFile);
mesh.writeInfoToFile(cutLatticeInfo);
std::cout << "Mesh written to " << cutLatticeFile << std::endl;
}
// Warp any vertices close enough to phi=0 to fix sign crossings.
static const float WARP_THRESHOLD = 0.3;
std::cout<<" warping vertices"<<std::endl;
warp_vertices(WARP_THRESHOLD, mesh, vphi);
if (intermediate) {
static const char* warpVerticesFile = "2_warp_vertices.tet";
static const char* warpVerticesInfo = "2_warp_vertices.info";
mesh.writeToFile(warpVerticesFile);
mesh.writeInfoToFile(warpVerticesInfo);
std::cout << "Mesh written to " << warpVerticesFile << std::endl;
}
// Cut through tets that still poke out of the level set
std::cout<<" trimming spikes"<<std::endl;
trim_spikes(mesh, vphi);
if (intermediate) {
static const char* trimSpikesFile = "3_trim_spikes.tet";
static const char* trimSpikesInfo = "3_trim_spikes.info";
mesh.writeToFile(trimSpikesFile);
mesh.writeInfoToFile(trimSpikesInfo);
std::cout << "Mesh written to " << trimSpikesFile << std::endl;
}
// At this point, there could be some bad tets with all four vertices on
// the surface but which lie outside the level set. Get rid of them.
std::cout<<" removing exterior tets"<<std::endl;
remove_exterior_tets(mesh, vphi, sdf);
// Compact mesh and clear away unused vertices.
std::cout<<" compacting mesh"<<std::endl;
mesh.compactMesh();
if (intermediate) {
static const char* removeExtFile = "4_remove_exterior.tet";
static const char* removeExtInfo = "4_remove_exterior.info";
static const char* removeExtObj = "4_remove_exterior.obj";
mesh.writeToFile(removeExtFile);
mesh.writeInfoToFile(removeExtInfo);
std::vector<Vec3f> objVerts;
std::vector<Vec3i> objTris;
mesh.getBoundary(objVerts, objTris);
write_objfile(objVerts, objTris, removeExtObj);
std::cout << "Mesh written to " << removeExtFile << std::endl;
}
// Compute maximum dihedral angle of mesh (unoptimized, no features).
std::cout << " Maximum dihedral angle = " << max_dihedral_angle(mesh)
<< std::endl;
if (featureSet.numFeatures() > 0 || optimize)
{
// Identify boundary vertices
std::vector<int> boundary_verts;
std::vector<Vec3i> boundary_tris;
std::cout << " identifying boundary" << std::endl;
mesh.getBoundary(boundary_verts, boundary_tris);
assert(!boundary_verts.empty());
// Snap vertices to given features
std::vector<int> feature_endpoints;
std::map<int, int> vertex_feature_map;
if (featureSet.numFeatures() > 0)
{
// Move vertices to match desired features
std::cout << " matching features" << std::endl;
clock_t featureTime = clock();
match_features(mesh, featureSet, sdf.dx,
boundary_verts, boundary_tris,
feature_endpoints, vertex_feature_map,
unsafe);
featureTime = clock() - featureTime;
std::cout << " features matched in "
<< ((float)featureTime)/CLOCKS_PER_SEC
<< " seconds." << std::endl;
std::cout << " Maximum dihedral angle = "
<< max_dihedral_angle(mesh) << std::endl;
if (optimize && intermediate)
{
static const char* matchFeaturesFile = "5_match_features.tet";
static const char* matchFeaturesInfo = "5_match_features.info";
mesh.writeToFile(matchFeaturesFile);
mesh.writeInfoToFile(matchFeaturesInfo);
std::cout << "Mesh written to " << matchFeaturesFile
<< std::endl;
}
}
// Finish by optimizing the tetmesh, if desired.
if (optimize)
{
std::cout << " optimizing mesh" << std::endl;
clock_t optTime = clock();
optimize_tet_mesh(mesh, sdf,
boundary_verts,
featureSet,
feature_endpoints,
vertex_feature_map);
optTime = clock() - optTime;
std::cout << " Mesh optimization completed in "
<< ((float)optTime)/CLOCKS_PER_SEC
<< " seconds." << std::endl;
std::cout<< " Maximum dihedral angle = "
<< max_dihedral_angle(mesh) << std::endl;
}
// DEBUGGING
// Check for inverted tets
for (size_t t = 0; t < mesh.tSize(); ++t)
{
Tet tet = mesh.getTet(t);
float signedVolume = tet.volume();
if (signedVolume <= 0.0)
{
std::cerr << "Tet #" << t << " is inverted! " <<
"Volume = " << signedVolume << "; " <<
"Aspect = " << tet.aspectRatio() << std::endl <<
"{" << tet[0] << "} {" << tet[1] << "} {" << tet[2] <<
"} {" << tet[3] << "}" << std::endl;
}
}
}
}
// Find all tets with vertices where a corner has vphi>0 and trim them back.
static void
trim_spikes(TetMesh& mesh,
std::vector<float> &vphi)
{
// keep track of edges we cut - store the index of the new vertex
std::map<Vec2i,int> cut_map;
// we have a separate list for the results of trimming tets
std::vector<Vec4i> new_tets;
// go through the existing tets to see what we have to trim
for (int t=0; t<(int)mesh.tSize(); ++t) {
int p, q, r, s; assign(mesh.T(t), p, q, r, s);
if (vphi[p]<=0 && vphi[q]<=0 && vphi[r]<=0 && vphi[s]<=0)
continue; // this tet doesn't stick out
// We have a plethora of cases to deal with. Let's sort the vertices
// by their phi values and break ties with index, remembering if we
// have switched orientation or not, to cut down on the number of
// cases to handle. Use a sorting network to do the job.
bool flipped=false;
if (vphi[p]<vphi[q] || (vphi[p]==vphi[q] && p<q)) {
std::swap(p, q);
flipped = !flipped;
}
if (vphi[r]<vphi[s] || (vphi[r]==vphi[s] && r<s)) {
std::swap(r, s);
flipped = !flipped;
}
if (vphi[p]<vphi[r] || (vphi[p]==vphi[r] && p<r)) {
std::swap(p, r);
flipped = !flipped;
}
if (vphi[q]<vphi[s] || (vphi[q]==vphi[s] && q<s)) {
std::swap(q, s);
flipped = !flipped;
}
if (vphi[q]<vphi[r] || (vphi[q]==vphi[r] && q<r)) {
std::swap(q, r);
flipped = !flipped;
}
// sanity checks
assert(vphi[p]>=vphi[q] && vphi[q]>=vphi[r] && vphi[r]>=vphi[s]); //sort
assert(vphi[p]>0); // we already skipped interior tets
assert(vphi[s]<=0); // we never generate tets with all positive phi
// now do the actual trimming
if (vphi[s]==0) { // +++0 entirely outside
mesh.T(t)=mesh.tets().back(); // overwrite with last tet
mesh.tets().pop_back(); // get rid of the last one
--t; // decrement to cancel the next for-loop increment
} else if (vphi[r]>0) { // +++- just one vertex inside, three out
// replace this tet with one clipped back to the isosurface
int ps = cut_edge(p, s, mesh, vphi, cut_map),
qs = cut_edge(q, s, mesh, vphi, cut_map),
rs = cut_edge(r, s, mesh, vphi, cut_map);
if (flipped)
mesh.T(t) = Vec4i(qs, ps, rs, s);
else
mesh.T(t) = Vec4i(ps, qs, rs, s);
} else if(vphi[q]<0) { // +--- just one vertex outside, three in
// Have to tetrahedralize the resulting triangular prism.
// Note that the quad faces have to be split in a way that will
// be consistent with face-adjacent tets: our sort of pqrs with
// consistently broken ties makes this work. We cut the quad to the
// deepest vertex (phi as negative as possible).
int pq = cut_edge(p, q, mesh, vphi, cut_map),
pr = cut_edge(p, r, mesh, vphi, cut_map),
ps = cut_edge(p, s, mesh, vphi, cut_map);
if (flipped) {
mesh.T(t) = Vec4i(q, pq, r, s);
new_tets.push_back(Vec4i(r, pq, pr, s));
new_tets.push_back(Vec4i(s, pq, pr, ps));
} else {
mesh.T(t)=Vec4i(pq, q, r, s);
new_tets.push_back(Vec4i(pq, r, pr, s));
new_tets.push_back(Vec4i(pq, s, pr, ps));
}
} else if (vphi[q]>0 && vphi[r]<0) { // ++-- two vertices out, two in
int pr = cut_edge(p, r, mesh, vphi, cut_map),
ps = cut_edge(p, s, mesh, vphi, cut_map),
qr = cut_edge(q, r, mesh, vphi, cut_map),
qs = cut_edge(q, s, mesh, vphi, cut_map);
if (flipped) {
mesh.T(t) = Vec4i(qr, pr, r, s);
new_tets.push_back(Vec4i(qs, pr, qr, s));
new_tets.push_back(Vec4i(ps, pr, qs, s));
} else {
mesh.T(t) = Vec4i(pr, qr, r, s);
new_tets.push_back(Vec4i(pr, qs, qr, s));
new_tets.push_back(Vec4i(pr, ps, qs, s));
}
} else if (vphi[q]==0 && vphi[r]==0) { // +00- 1 out, 1 in, 2 surface
int ps = cut_edge(p, s, mesh, vphi, cut_map);
if (flipped)
mesh.T(t) = Vec4i(q, ps, r, s);
else
mesh.T(t) = Vec4i(ps, q, r, s);
} else if (vphi[q]==0) { // +0-- 1 out, 2 in, 1 surface
int pr = cut_edge(p, r, mesh, vphi, cut_map),
ps = cut_edge(p, s, mesh, vphi, cut_map);
if (flipped) {
mesh.T(t) = Vec4i(q, pr, r, s);
new_tets.push_back(Vec4i(q, ps, pr, s));
} else {
mesh.T(t) = Vec4i(pr, q, r, s);
new_tets.push_back(Vec4i(ps, q, pr, s));
}
} else { // ++0- two out, one in, one surface
int ps = cut_edge(p, s, mesh, vphi, cut_map),
qs = cut_edge(q, s, mesh, vphi, cut_map);
if (flipped)
mesh.T(t) = Vec4i(qs, ps, r, s);
else
mesh.T(t) = Vec4i(ps, qs, r, s);
}
}
// append all the remaining new tets
for (size_t t=0; t<new_tets.size(); ++t)
mesh.tets().push_back(new_tets[t]);
}
// Fix some edge crossings by warping vertices if it's admissible
static void
warp_vertices(const float threshold,
TetMesh& mesh,
std::vector<float>& vphi)
{
assert(threshold>=0 && threshold<=0.5);
std::vector<float> warp(mesh.vSize(), FLT_MAX);
std::vector<int> warp_nbr(mesh.vSize(), -1);
std::vector<Vec3f> d(mesh.vSize(), Vec3f(0,0,0));
// it's wasteful to iterate through tets just to look at edges; oh well
for (size_t t=0; t<mesh.tSize(); ++t) {
for (int u=0; u<3; ++u) {
int i = mesh.T(t)[u];
for (int v=u+1; v<4; ++v) {
int j = mesh.T(t)[v];
if ((vphi[i]<0 && vphi[j]>0) || (vphi[i]>0 && vphi[j]<0)) {
float alpha = vphi[i]/(vphi[i]-vphi[j]);
if (alpha < threshold) { // warp i?
float d2 = alpha*dist2(mesh.V(i), mesh.V(j));
if (d2 < warp[i]) {
warp[i] = d2;
warp_nbr[i] = j;
d[i] = alpha*(mesh.V(j)-mesh.V(i));
}
} else if (alpha > 1-threshold) { // warp j?
float d2 = (1-alpha)*dist2(mesh.V(i), mesh.V(j));
if (d2 < warp[j]) {
warp[j] = d2;
warp_nbr[j] = i;
d[j] = (1-alpha)*(mesh.V(i)-mesh.V(j));
}
}
}
}
}
}
// do the warps (also wasteful to loop over all vertices; oh well)
for (size_t i=0; i<mesh.vSize(); ++i) if(warp_nbr[i]>=0) {
mesh.V(i) += d[i];
vphi[i] = 0;
}
}
int main(int argc, char* argv[])
{
char* inputFileName = "I:/Programs/VegaFEM-v2.1/models/turtle/turtle-volumetric-homogeneous.veg";
VolumetricMesh* volumetricMesh = VolumetricMeshLoader::load(inputFileName);
if (volumetricMesh == NULL)
{
PRINT_F("load failed!");
}
else
{
PRINT_F("%d vertices, %d elements", volumetricMesh->getNumVertices(), volumetricMesh->getNumElements());
}
TetMesh* tetMesh;
if (volumetricMesh->getElementType() == VolumetricMesh::TET)
{
tetMesh = (TetMesh*) volumetricMesh;
}
else
PRINT_F("not a tet mesh\n");
CorotationalLinearFEM* deformableModel = new CorotationalLinearFEM(tetMesh);
ForceModel* forceModel = new CorotationalLinearFEMForceModel(deformableModel);
int nVtx = tetMesh->getNumVertices();
int r = 3 * nVtx;
double timestep = 0.0333;
SparseMatrix* massMatrix;
GenerateMassMatrix::computeMassMatrix(tetMesh, &massMatrix, true);
massMatrix->SaveToMatlabFormat("massMatrix.m");
PRINT_F("%d rows, %d cols\n", massMatrix->GetNumRows(), massMatrix->GetNumColumns());
int positiveDefiniteSolver = 0;
int numConstrainedDOFs = 9;
int constrainedDOFs[9]= {12,13,14,30,31,32,42,43,44};
double dampingMassCoef = 0.0;
double dampingStiffnessCoef = 0.01;
ImplicitBackwardEulerSparse* integrator = new ImplicitBackwardEulerSparse(r, timestep, massMatrix, forceModel, positiveDefiniteSolver, numConstrainedDOFs, constrainedDOFs, dampingMassCoef, dampingStiffnessCoef);
//CentralDifferencesSparse* integrator = new CentralDifferencesSparse(r, timestep, massMatrix, forceModel, numConstrainedDOFs, constrainedDOFs, dampingMassCoef, dampingStiffnessCoef);
double * f = new double[r];
int numTimesteps = 10;
double*u = new double[r];
for (int i = 0; i < numTimesteps; ++i)
{
integrator->SetExternalForcesToZero();
if (i==0)
{
for (int j = 0; j < r; j++)
f[j] = 0;
f[37] = -500;
integrator->SetExternalForces(f);
}
integrator->GetqState(u);
PRINT_F("v = [", i);
for (int ithVtx = 0; ithVtx < nVtx; ++ithVtx)
PRINT_F("%lf, %lf, %lf\n", u[ithVtx*3],u[ithVtx*3+1],u[ithVtx*3+2]);
PRINT_F("];");
integrator->DoTimestep();
}
return 0;
}
// Write this mesh to the FEM framework's mesh m
void writeFEM(int m,TetMesh &t) {
int nNode=t.getPoints();
int nTet=t.getTets();
FEM_Mesh_data(m,FEM_NODE,FEM_COORD,t.getPointArray(), 0,nNode, FEM_DOUBLE,3);
FEM_Mesh_data(m,FEM_ELEM+0,FEM_CONN,t.getTetConn(), 0,nTet, FEM_INDEX_0,4);
}
