// Create a polygonal mesh for this torus using parameterization as follows:
// u = [0, 2*Pi] traces a circle in the x-y plane with radius torusRadius,
// which is the centroid of the torus. A point P on this circle is
// given by P = torusRadius*~[cos(u) sin(u) 0].
// v = [0, 2*Pi] traces a circle arond the cross-section (or tube) of the
// torus with radius tubeRadius, at a given u. A point Q on this circle
// is given by Q = (torusRadius + tubeRadius*cos(v))*e1 + tubeRadius*(~[0 0 1]*sin(v))
// where e1 = ~[sin(u) cos(u) 0]. The tube circle is in a plane spanned
// by e1 and the z-axis.
void ContactGeometry::Torus::Impl::createPolygonalMesh(PolygonalMesh& mesh) const {
// TODO add resolution argument
const int numSides = 12; //*resolution;
const int numSlices = 36; //*resolution;
// add vertices
for (int i = 0; i < numSlices; ++i) {
Real u = Real((i*2*SimTK_PI)/numSlices);
UnitVec3 e1(std::sin(u), std::cos(u), 0); // torus circle aligned with z-axis (z-axis through hole)
for (int j = 0; j < numSides; ++j) {
Real v = Real((j*2*SimTK_PI)/numSides);
Vec3 vtx = (torusRadius + tubeRadius*std::cos(v))*e1 + tubeRadius*std::sin(v)*Vec3(0,0,1); // use ZAXIS?
mesh.addVertex(vtx);
}
}
// add faces, be careful to wrap indices for the last slice
int numVertices = mesh.getNumVertices();
// cout << "num verts = " << numVertices << endl;
for (int i = 0; i < numVertices; ++i) {
// cout << "v" << i << ": " << mesh.getVertexPosition(i) << endl;
// define counter-clockwise quad faces
Array_<int> faceIndices;
faceIndices.push_back(i); // u_i,v_i
faceIndices.push_back((i+1)%numVertices); // u_i, v_i+1
faceIndices.push_back((i+1+numSides)%numVertices); // u_i+1, v_i+1
faceIndices.push_back((i+numSides)%numVertices); // u_i+1, v_i
mesh.addFace(faceIndices);
}
}
void ContactGeometry::TriangleMesh::Impl::
createPolygonalMesh(PolygonalMesh& mesh) const {
for (unsigned vx=0; vx < vertices.size(); ++vx)
mesh.addVertex(vertices[vx].pos);
for (unsigned fx=0; fx < faces.size(); ++fx) {
const Face& face = faces[fx];
const ArrayViewConst_<int> verts(face.vertices, face.vertices+3);
mesh.addFace(verts);
}
}
void testLoadObjFile() {
string file;
file += "# This is a comment\n";
file += "v -1.0 1.0 2.0\n";
file += "v -2.0 2.0 3.0\n";
file += "v -3.0 3.0 \\\n";
file += "4.0\n";
file += "v -4.0 4.0 5.0\n";
file += "v -5.0 5.0 6.0\n";
file += "f 1 2 3\n";
file += "f 2// 3/4/5 4//2\n";
file += "f -3 -2/3/4 -1\n";
file += "f 1 3\\\n";
file += "5 2\n";
PolygonalMesh mesh;
stringstream stream(file);
mesh.loadObjFile(stream);
ASSERT(mesh.getNumVertices() == 5);
ASSERT(mesh.getNumFaces() == 4);
for (int i = 0; i < mesh.getNumVertices(); i++) {
const Vec3& pos = mesh.getVertexPosition(i);
ASSERT(pos[0] == -(i+1));
ASSERT(pos[1] == i+1);
ASSERT(pos[2] == i+2);
}
for (int i = 0; i < 3; i++) {
ASSERT(mesh.getNumVerticesForFace(i) == 3);
ASSERT(mesh.getFaceVertex(i, 0) == i);
ASSERT(mesh.getFaceVertex(i, 1) == i+1);
ASSERT(mesh.getFaceVertex(i, 2) == i+2);
}
ASSERT(mesh.getNumVerticesForFace(3) == 4);
ASSERT(mesh.getFaceVertex(3, 0) == 0);
ASSERT(mesh.getFaceVertex(3, 1) == 2);
ASSERT(mesh.getFaceVertex(3, 2) == 4);
ASSERT(mesh.getFaceVertex(3, 3) == 1);
}
void testCreateMesh() {
PolygonalMesh mesh;
ASSERT(mesh.getNumFaces() == 0);
ASSERT(mesh.getNumVertices() == 0);
ASSERT(mesh.addVertex(Vec3(0)) == 0);
ASSERT(mesh.addVertex(Vec3(0, 1, 0)) == 1);
ASSERT(mesh.addVertex(Vec3(0, 0, 1)) == 2);
Array_<int> v;
v.push_back(1);
v.push_back(2);
v.push_back(0);
ASSERT(mesh.addFace(v) == 0);
ASSERT(mesh.getNumFaces() == 1);
ASSERT(mesh.getNumVertices() == 3);
ASSERT(mesh.getFaceVertex(0, 0) == 1);
ASSERT(mesh.getFaceVertex(0, 1) == 2);
ASSERT(mesh.getFaceVertex(0, 2) == 0);
ASSERT(mesh.getVertexPosition(0) == Vec3(0));
ASSERT(mesh.getVertexPosition(1) == Vec3(0, 1, 0));
ASSERT(mesh.getVertexPosition(2) == Vec3(0, 0, 1));
// Make sure copy and assignment are shallow.
PolygonalMesh mesh2(mesh); // shallow copy construction
ASSERT(&mesh2.getImpl() == &mesh.getImpl());
ASSERT(mesh.getImplHandleCount()==2);
PolygonalMesh mesh3;
mesh3 = mesh; // shallow assignment
ASSERT(&mesh3.getImpl() == &mesh.getImpl());
ASSERT(mesh.getImplHandleCount()==3);
PolygonalMesh mesh4;
mesh4.copyAssign(mesh); // deep copy
ASSERT(mesh4.getNumVertices() == mesh.getNumVertices());
ASSERT(&mesh4.getImpl() != &mesh.getImpl());
ASSERT(mesh4.getImplHandleCount()==1);
ASSERT(mesh.getImplHandleCount()==3);
}
void flexionFDSimulationWithHitMap(Model& model)
{
addFlexionController(model);
//addExtensionController(model);
//addTibialLoads(model, 0);
model.setUseVisualizer(true);
// init system
std::time_t result = std::time(nullptr);
std::cout << "\nBefore initSystem() " << std::asctime(std::localtime(&result)) << endl;
SimTK::State& si = model.initSystem();
result = std::time(nullptr);
std::cout << "\nAfter initSystem() " << std::asctime(std::localtime(&result)) << endl;
// set gravity
//model.updGravityForce().setGravityVector(si, Vec3(-9.80665,0,0));
//model.updGravityForce().setGravityVector(si, Vec3(0,0,0));
//setHipAngle(model, si, 90);
//setKneeAngle(model, si, 0, false, false);
//model.equilibrateMuscles( si);
MultibodySystem& system = model.updMultibodySystem();
SimbodyMatterSubsystem& matter = system.updMatterSubsystem();
GeneralForceSubsystem forces(system);
ContactTrackerSubsystem tracker(system);
CompliantContactSubsystem contactForces(system, tracker);
contactForces.setTrackDissipatedEnergy(true);
//contactForces.setTransitionVelocity(1e-3);
for (int i=0; i < matter.getNumBodies(); ++i) {
MobilizedBodyIndex mbx(i);
if (i==19 || i==20 || i==22)// || i==15 || i==16)
{
MobilizedBody& mobod = matter.updMobilizedBody(mbx);
std::filebuf fb;
//cout << mobod.updBody().
if (i==19)
fb.open ( "../resources/femur_lat_r.obj",std::ios::in);
else if (i==20)
fb.open ( "../resources/femur_med_r.obj",std::ios::in);
else if (i==22)
fb.open ( "../resources/tibia_upper_r.obj",std::ios::in);
//else if (i==15)
//fb.open ( "../resources/meniscus_lat_r.obj",std::ios::in);
//else if (i==16)
//fb.open ( "../resources/meniscus_med_r.obj",std::ios::in);
std::istream is(&fb);
PolygonalMesh polMesh;
polMesh.loadObjFile(is);
fb.close();
SimTK::ContactGeometry::TriangleMesh mesh(polMesh);
ContactSurface contSurf;//(mesh, ContactMaterial(1.0e6, 1, 1, 0.03, 0.03), 0.001);
if (i==19 || i==20 || i==22)
contSurf = ContactSurface(mesh, ContactMaterial(10, 1, 1, 0.03, 0.03), 0.001);
//else if (i==15 || i==16)
//contSurf = ContactSurface(mesh, ContactMaterial(10, 3, 1, 0.03, 0.03), 0.001);
DecorativeMesh showMesh(mesh.createPolygonalMesh());
showMesh.setOpacity(0.5);
mobod.updBody().addDecoration( showMesh);
mobod.updBody().addContactSurface(contSurf);
}
}
ModelVisualizer& viz(model.updVisualizer());
//Visualizer viz(system);
viz.updSimbodyVisualizer().addDecorationGenerator(new HitMapGenerator(system,contactForces));
viz.updSimbodyVisualizer().setMode(Visualizer::RealTime);
viz.updSimbodyVisualizer().setDesiredBufferLengthInSec(1);
viz.updSimbodyVisualizer().setDesiredFrameRate(30);
viz.updSimbodyVisualizer().setGroundHeight(-3);
viz.updSimbodyVisualizer().setShowShadows(true);
Visualizer::InputSilo* silo = new Visualizer::InputSilo();
viz.updSimbodyVisualizer().addInputListener(silo);
Array_<std::pair<String,int> > runMenuItems;
runMenuItems.push_back(std::make_pair("Go", GoItem));
runMenuItems.push_back(std::make_pair("Replay", ReplayItem));
runMenuItems.push_back(std::make_pair("Quit", QuitItem));
viz.updSimbodyVisualizer().addMenu("Run", RunMenuId, runMenuItems);
Array_<std::pair<String,int> > helpMenuItems;
helpMenuItems.push_back(std::make_pair("TBD - Sorry!", 1));
viz.updSimbodyVisualizer().addMenu("Help", HelpMenuId, helpMenuItems);
system.addEventReporter(new MyReporter(system,contactForces,ReportInterval));
//system.addEventReporter(new Visualizer::Reporter(viz.updSimbodyVisualizer(), ReportInterval));
// Check for a Run->Quit menu pick every <x> second.
system.addEventHandler(new UserInputHandler(*silo, 0.001));
system.realizeTopology();
//Show ContactSurfaceIndex for each contact surface
// for (int i=0; i < matter.getNumBodies(); ++i) {
//MobilizedBodyIndex mbx(i);
// const MobilizedBody& mobod = matter.getMobilizedBody(mbx);
// const int nsurfs = mobod.getBody().getNumContactSurfaces();
//printf("mobod %d has %d contact surfaces\n", (int)mbx, nsurfs);
////cout << "mobod with mass: " << (float)mobod.getBodyMass(si) << " has " << nsurfs << " contact surfaces" << endl;
// //for (int i=0; i<nsurfs; ++i) {
// //printf("%2d: index %d\n", i,
// //(int)tracker.getContactSurfaceIndex(mbx,i));
// //}
// }
//cout << "tracker num of surfaces: " << tracker.getNumSurfaces() << endl;
//State state = system.getDefaultState();
//viz.report(state);
State& state = model.initializeState();
viz.updSimbodyVisualizer().report(state);
// Add reporters
ForceReporter* forceReporter = new ForceReporter(&model);
model.addAnalysis(forceReporter);
CustomAnalysis* customReporter = new CustomAnalysis(&model, "r");
model.addAnalysis(customReporter);
// Create the integrator and manager for the simulation.
SimTK::RungeKuttaMersonIntegrator integrator(model.getMultibodySystem());
//SimTK::CPodesIntegrator integrator(model.getMultibodySystem());
//integrator.setAccuracy(.01);
//integrator.setAccuracy(1e-3);
//integrator.setFixedStepSize(0.001);
Manager manager(model, integrator);
// Define the initial and final simulation times
double initialTime = 0.0;
double finalTime = 0.2;
// Integrate from initial time to final time
manager.setInitialTime(initialTime);
manager.setFinalTime(finalTime);
std::cout<<"\n\nIntegrating from "<<initialTime<<" to " <<finalTime<<std::endl;
result = std::time(nullptr);
std::cout << "\nBefore integrate(si) " << std::asctime(std::localtime(&result)) << endl;
manager.integrate(state);
result = std::time(nullptr);
std::cout << "\nAfter integrate(si) " << std::asctime(std::localtime(&result)) << endl;
// Save the simulation results
Storage statesDegrees(manager.getStateStorage());
statesDegrees.print("../outputs/states_flex.sto");
model.updSimbodyEngine().convertRadiansToDegrees(statesDegrees);
statesDegrees.setWriteSIMMHeader(true);
statesDegrees.print("../outputs/states_degrees_flex.mot");
// force reporter results
forceReporter->getForceStorage().print("../outputs/force_reporter_flex.mot");
//customReporter->print( "../outputs/custom_reporter_flex.mot");
//cout << "You can choose 'Replay'" << endl;
int menuId, item;
unsigned int frameRate = 5;
do {
cout << "Please choose 'Replay' or 'Quit'" << endl;
viz.updInputSilo().waitForMenuPick(menuId, item);
if (item != ReplayItem && item != QuitItem)
cout << "\aDude... follow instructions!\n";
if (item == ReplayItem)
{
cout << "Type desired frame rate (integer) for playback and press Enter (default = 1) : ";
//frameRate = cin.get();
cin >> frameRate;
if (cin.fail())
{
cout << "Not an int. Setting default frame rate." << endl;
cin.clear();
cin.ignore(std::numeric_limits<int>::max(),'\n');
frameRate = 1;
}
//cout << "saveEm size: " << saveEm.size() << endl;
for (unsigned int i=0; i<saveEm.size(); i++)
{
viz.updSimbodyVisualizer().drawFrameNow(saveEm.getElt(i));
if (frameRate == 0)
frameRate = 1;
usleep(1000000/frameRate);
}
}
} while (menuId != RunMenuId || item != QuitItem);
}
ContactGeometry::TriangleMesh::Impl::Impl
(const PolygonalMesh& mesh, bool smooth)
: ContactGeometryImpl(), smooth(smooth)
{ // Create the mesh, triangulating faces as necessary.
Array_<Vec3> vertexPositions;
Array_<int> faceIndices;
for (int i = 0; i < mesh.getNumVertices(); i++)
vertexPositions.push_back(mesh.getVertexPosition(i));
for (int i = 0; i < mesh.getNumFaces(); i++) {
int numVert = mesh.getNumVerticesForFace(i);
if (numVert < 3)
continue; // Ignore it.
if (numVert == 3) {
faceIndices.push_back(mesh.getFaceVertex(i, 0));
faceIndices.push_back(mesh.getFaceVertex(i, 1));
faceIndices.push_back(mesh.getFaceVertex(i, 2));
}
else if (numVert == 4) {
// Split it into two triangles.
faceIndices.push_back(mesh.getFaceVertex(i, 0));
faceIndices.push_back(mesh.getFaceVertex(i, 1));
faceIndices.push_back(mesh.getFaceVertex(i, 2));
faceIndices.push_back(mesh.getFaceVertex(i, 2));
faceIndices.push_back(mesh.getFaceVertex(i, 3));
faceIndices.push_back(mesh.getFaceVertex(i, 0));
}
else {
// Add a vertex at the center, then split it into triangles.
Vec3 center(0);
for (int j = 0; j < numVert; j++)
center += vertexPositions[mesh.getFaceVertex(i, j)];
center /= numVert;
vertexPositions.push_back(center);
int newIndex = vertexPositions.size()-1;
for (int j = 0; j < numVert-1; j++) {
faceIndices.push_back(mesh.getFaceVertex(i, j));
faceIndices.push_back(mesh.getFaceVertex(i, j+1));
faceIndices.push_back(newIndex);
}
// Close the face (thanks, Alexandra Zobova).
faceIndices.push_back(mesh.getFaceVertex(i, numVert-1));
faceIndices.push_back(mesh.getFaceVertex(i, 0));
faceIndices.push_back(newIndex);
}
}
init(vertexPositions, faceIndices);
// Make sure the mesh normals are oriented correctly.
Vec3 origin(0);
for (int i = 0; i < 3; i++)
origin += vertices[faces[0].vertices[i]].pos;
origin /= 3; // this is the face centroid
const UnitVec3 direction = -faces[0].normal;
// Calculate a ray origin that is guaranteed to be outside the
// mesh. If the topology is right (face 0 normal points outward), we'll be
// outside on the side containing face 0. If it is wrong, we'll be outside
// on the opposite side of the mesh. Then we'll shoot a ray back along the
// direction we came from (that is, towards the interior of the mesh from
// outside). We'll hit *some* face. If the topology is right, the hit
// face's normal will be pointing back at us. If it is wrong, the face
// normal will also be pointing inwards, in roughly the same direction as
// the ray.
origin -= max(obb.bounds.getSize())*direction;
Real distance;
int face;
Vec2 uv;
bool intersects = intersectsRay(origin, direction, distance, face, uv);
assert(intersects);
// Now dot the hit face normal with the ray direction; correct topology
// will have them pointing in more-or-less opposite directions.
if (dot(faces[face].normal, direction) > 0) {
// We need to invert the mesh topology.
for (int i = 0; i < (int) faces.size(); i++) {
Face& f = faces[i];
int temp = f.vertices[0];
f.vertices[0] = f.vertices[1];
f.vertices[1] = temp;
temp = f.edges[1];
f.edges[1] = f.edges[2];
f.edges[2] = temp;
f.normal *= -1;
}
for (int i = 0; i < (int) vertices.size(); i++)
vertices[i].normal *= -1;
}
}
void VisualizerProtocol::drawPolygonalMesh(const PolygonalMesh& mesh, const Transform& X_GM, const Vec3& scale, const Vec4& color, int representation) {
const void* impl = &mesh.getImpl();
map<const void*, unsigned short>::const_iterator iter = meshes.find(impl);
if (iter != meshes.end()) {
// This mesh was already cached; just reference it by index number.
drawMesh(X_GM, scale, color, (short)representation, iter->second, 0);
return;
}
// This is a new mesh, so we need to send it to the visualizer. Build lists
// of vertices and faces, triangulating as necessary.
vector<float> vertices;
vector<unsigned short> faces;
for (int i = 0; i < mesh.getNumVertices(); i++) {
Vec3 pos = mesh.getVertexPosition(i);
vertices.push_back((float) pos[0]);
vertices.push_back((float) pos[1]);
vertices.push_back((float) pos[2]);
}
for (int i = 0; i < mesh.getNumFaces(); i++) {
int numVert = mesh.getNumVerticesForFace(i);
if (numVert < 3)
continue; // Ignore it.
if (numVert == 3) {
faces.push_back((unsigned short) mesh.getFaceVertex(i, 0));
faces.push_back((unsigned short) mesh.getFaceVertex(i, 1));
faces.push_back((unsigned short) mesh.getFaceVertex(i, 2));
}
else if (numVert == 4) {
// Split it into two triangles.
faces.push_back((unsigned short) mesh.getFaceVertex(i, 0));
faces.push_back((unsigned short) mesh.getFaceVertex(i, 1));
faces.push_back((unsigned short) mesh.getFaceVertex(i, 2));
faces.push_back((unsigned short) mesh.getFaceVertex(i, 2));
faces.push_back((unsigned short) mesh.getFaceVertex(i, 3));
faces.push_back((unsigned short) mesh.getFaceVertex(i, 0));
}
else {
// Add a vertex at the center, then split it into triangles.
Vec3 center(0);
for (int j = 0; j < numVert; j++) {
Vec3 pos = mesh.getVertexPosition(mesh.getFaceVertex(i,j));
center += pos;
}
center /= numVert;
vertices.push_back((float) center[0]);
vertices.push_back((float) center[1]);
vertices.push_back((float) center[2]);
const unsigned newIndex = (unsigned)(vertices.size()/3-1);
for (int j = 0; j < numVert-1; j++) {
faces.push_back((unsigned short) mesh.getFaceVertex(i, j));
faces.push_back((unsigned short) mesh.getFaceVertex(i, j+1));
faces.push_back((unsigned short) newIndex);
}
// Close the face (thanks, Alexandra Zobova).
faces.push_back((unsigned short) mesh.getFaceVertex(i, numVert-1));
faces.push_back((unsigned short) mesh.getFaceVertex(i, 0));
faces.push_back((unsigned short) newIndex);
}
}
SimTK_ERRCHK1_ALWAYS(vertices.size() <= 65535*3,
"VisualizerProtocol::drawPolygonalMesh()",
"Can't display a DecorativeMesh with more than 65535 vertices;"
" received one with %llu.", (unsigned long long)vertices.size());
SimTK_ERRCHK1_ALWAYS(faces.size() <= 65535*3,
"VisualizerProtocol::drawPolygonalMesh()",
"Can't display a DecorativeMesh with more than 65535 vertices;"
" received one with %llu.", (unsigned long long)faces.size());
const int index = NumPredefinedMeshes + (int)meshes.size();
SimTK_ERRCHK_ALWAYS(index <= 65535,
"VisualizerProtocol::drawPolygonalMesh()",
"Too many unique DecorativeMesh objects; max is 65535.");
meshes[impl] = (unsigned short)index; // insert new mesh
WRITE(outPipe, &DefineMesh, 1);
unsigned short numVertices = (unsigned short)(vertices.size()/3);
unsigned short numFaces = (unsigned short)(faces.size()/3);
WRITE(outPipe, &numVertices, sizeof(short));
WRITE(outPipe, &numFaces, sizeof(short));
WRITE(outPipe, &vertices[0], (unsigned)(vertices.size()*sizeof(float)));
WRITE(outPipe, &faces[0], (unsigned)(faces.size()*sizeof(short)));
drawMesh(X_GM, scale, color, (short) representation, (unsigned short)index, 0);
}
PolygonalMesh* Polygonizer::computeSurfaceNerLinear(float epsilon, float tau)
{
bool ***isIn = new bool**[dimZ+1];
float ***value = new float**[dimZ+1];
for(int i=0; i<dimZ+1; i++){
isIn[i] = new bool*[dimY+1];
value[i] = new float*[dimY+1];
for(int j=0; j<dimY+1; j++){
isIn[i][j] = new bool[dimX+1];
value[i][j] = new float[dimX+1];
for(int k=0; k<dimX+1; k++){
isIn[i][j][k] = false;
value[i][j][k] = 0;
}
}
}
float o[3];
o[0] = originX;
o[1] = originY;
o[2] = originZ;
float space[3];
space[0] = spaceX;
space[1] = spaceY;
space[2] = spaceZ;
int dim[3];
dim[0] = dimX;
dim[1] = dimY;
dim[2] = dimZ;
func->asignValueToVoxels(value, isIn, o, space, dim);
int ***index = new int**[dimZ+1];
for(int i=0; i<dimZ+1; i++){
index[i] = new int*[dimY+1];
for(int j=0; j<dimY+1; j++){
index[i][j] = new int[dimX+1];
for(int k=0; k<dimX+1; k++)
index[i][j][k] = -1;
}
}
int current = 0;
int face_N = 0;
for(int i=0; i<dimZ; i++)
for(int j=0; j<dimY; j++)
for(int k=0; k<dimX-1; k++){
if(!isIn[i][j][k] || !isIn[i][j][k+1])
continue;
if((value[i][j][k] > 0 && value[i][j][k+1] <= 0) ||
(value[i][j][k] <= 0 && value[i][j][k+1] > 0)){
face_N++;
if(index[i][j][k+1] < 0){
index[i][j][k+1] = current;
current++;
}
if(index[i][j+1][k+1] < 0){
index[i][j+1][k+1] = current;
current++;
}
if(index[i+1][j][k+1] < 0){
index[i+1][j][k+1] = current;
current++;
}
if(index[i+1][j+1][k+1] < 0){
index[i+1][j+1][k+1] = current;
current++;
}
}
}
for(int i=0; i<dimX; i++)
for(int j=0; j<dimZ; j++)
for(int k=0; k<dimY-1; k++){
if(!isIn[j][k][i] || !isIn[j][k+1][i])
continue;
if((value[j][k][i] > 0 && value[j][k+1][i] <= 0) ||
(value[j][k][i] <= 0&& value[j][k+1][i] > 0)){
face_N++;
if(index[j][k+1][i] < 0){
index[j][k+1][i] = current;
current++;
}
if(index[j+1][k+1][i] < 0){
index[j+1][k+1][i] = current;
current++;
}
if(index[j][k+1][i+1] < 0){
index[j][k+1][i+1] = current;
current++;
}
if(index[j+1][k+1][i+1] < 0){
index[j+1][k+1][i+1] = current;
current++;
}
}
}
for(int i=0; i<dimY; i++)
for(int j=0; j<dimX; j++)
for(int k=0; k<dimZ-1; k++){
if(!isIn[k][i][j] || !isIn[k+1][i][j])
continue;
if((value[k][i][j] > 0 && value[k+1][i][j] <= 0) ||
(value[k][i][j] <= 0 && value[k+1][i][j] > 0)){
face_N++;
if(index[k+1][i][j] < 0){
index[k+1][i][j] = current;
current++;
}
if(index[k+1][i+1][j] < 0){
index[k+1][i+1][j] = current;
current++;
}
if(index[k+1][i][j+1] < 0){
index[k+1][i][j+1] = current;
current++;
}
if(index[k+1][i+1][j+1] < 0){
index[k+1][i+1][j+1] = current;
current++;
}
}
}
PolygonalMesh* mesh = new PolygonalMesh;
int vertex_N = current;
mesh->setVertexCount(vertex_N);
float (*vertex)[3] = mesh->vertex;
int* degree = mesh->degree_f = new int[vertex_N];
current = 0;
for(int i=0; i<vertex_N; i++){
vertex[i][0] = vertex[i][1] = vertex[i][2] = 0;
degree[i] = 0;
}
mesh->setFaceCount(face_N);
double (*Q)[10] = new double[vertex_N][10];
for(int i=0; i<vertex_N; i++)
MAT_INIT(Q[i]);
for(int i=0; i<face_N; i++)
mesh->setPolygonCount(i, 4);
face_N = 0;
int **face = mesh->face;
bool flag = false;
for(int i=0; i<dimZ; i++)
for(int j=0; j<dimY; j++)
for(int k=0; k<dimX-1; k++){
if(!isIn[i][j][k] || !isIn[i][j][k+1])
continue;
if(value[i][j][k] > 0 && value[i][j][k+1] <= 0){
face[face_N][0] = index[i][j][k+1];
face[face_N][1] = index[i][j+1][k+1];
face[face_N][2] = index[i+1][j+1][k+1];
face[face_N][3] = index[i+1][j][k+1];
face_N++;
flag = true;
}
else if(value[i][j][k] <= 0 && value[i][j][k+1] > 0){
face[face_N][0] = index[i][j][k+1];
face[face_N][1] = index[i+1][j][k+1];
face[face_N][2] = index[i+1][j+1][k+1];
face[face_N][3] = index[i][j+1][k+1];
face_N++;
flag = true;
}
if(!flag)
continue;
flag = false;
float p[3], s[3], e[3];
s[0] = originX + k*spaceX;
e[0] = originX + (k+1)*spaceX;
s[1] = e[1] = originY + j*spaceY;
s[2] = e[2] = originZ + i*spaceZ;
float f1 = value[i][j][k];
float f2 = value[i][j][k+1];
searchZero(p, s, e, f1, f2, epsilon);
float g[3];
//func->gradient(g, p[0], p[1], p[2]);
double len = PolygonalMesh::LENGTH(g);
double nor[3];
nor[0] = g[0]/len;
nor[1] = g[1]/len;
nor[2] = g[2]/len;
double d = -PolygonalMesh::DOT(nor, p);
double Q_tmp[10];
MATRIX(Q_tmp, nor, d);
int i0 = index[i][j][k+1];
MAT_SUM(Q[i0], Q_tmp);
vertex[i0][0] += p[0];
vertex[i0][1] += p[1];
vertex[i0][2] += p[2];
degree[i0]++;
i0 = index[i][j+1][k+1];
MAT_SUM(Q[i0], Q_tmp);
vertex[i0][0] += p[0];
vertex[i0][1] += p[1];
vertex[i0][2] += p[2];
degree[i0]++;
i0 = index[i+1][j+1][k+1];
MAT_SUM(Q[i0], Q_tmp);
vertex[i0][0] += p[0];
vertex[i0][1] += p[1];
vertex[i0][2] += p[2];
degree[i0]++;
i0 = index[i+1][j][k+1];
MAT_SUM(Q[i0], Q_tmp);
vertex[i0][0] += p[0];
vertex[i0][1] += p[1];
vertex[i0][2] += p[2];
degree[i0]++;
}
for(int i=0; i<dimX; i++)
for(int j=0; j<dimZ; j++)
for(int k=0; k<dimY-1; k++){
if(!isIn[j][k][i] || !isIn[j][k+1][i])
continue;
if(value[j][k][i] > 0 && value[j][k+1][i] <= 0){
face[face_N][0] = index[j][k+1][i];
face[face_N][1] = index[j+1][k+1][i];
face[face_N][2] = index[j+1][k+1][i+1];
face[face_N][3] = index[j][k+1][i+1];
face_N++;
flag = true;
}
else if(value[j][k][i] <= 0 && value[j][k+1][i] > 0){
face[face_N][0] = index[j][k+1][i];
face[face_N][1] = index[j][k+1][i+1];
face[face_N][2] = index[j+1][k+1][i+1];
face[face_N][3] = index[j+1][k+1][i];
face_N++;
flag = true;
}
if(!flag)
continue;
flag = false;
float p[3], s[3], e[3];
s[1] = originY + k*spaceY;
e[1] = originY + (k+1)*spaceY;
s[2] = e[2] = originZ + j*spaceZ;
s[0] = e[0] = originX + i*spaceX;
float f1 = value[j][k][i];
float f2 = value[j][k+1][i];
searchZero(p, s, e, f1, f2, epsilon);
float g[3];
//func->gradient(g, p[0], p[1], p[2]);
double len = PolygonalMesh::LENGTH(g);
double nor[3];
nor[0] = g[0]/len;
nor[1] = g[1]/len;
nor[2] = g[2]/len;
double d = -PolygonalMesh::DOT(nor, p);
double Q_tmp[10];
MATRIX(Q_tmp, nor, d);
int i0 = index[j][k+1][i];
MAT_SUM(Q[i0], Q_tmp);
vertex[i0][0] += p[0];
vertex[i0][1] += p[1];
vertex[i0][2] += p[2];
degree[i0]++;
i0 = index[j+1][k+1][i];
MAT_SUM(Q[i0], Q_tmp);
vertex[i0][0] += p[0];
vertex[i0][1] += p[1];
vertex[i0][2] += p[2];
degree[i0]++;
i0 = index[j+1][k+1][i+1];
MAT_SUM(Q[i0], Q_tmp);
vertex[i0][0] += p[0];
vertex[i0][1] += p[1];
vertex[i0][2] += p[2];
degree[i0]++;
i0 = index[j][k+1][i+1];
MAT_SUM(Q[i0], Q_tmp);
vertex[i0][0] += p[0];
vertex[i0][1] += p[1];
vertex[i0][2] += p[2];
degree[i0]++;
}
for(int i=0; i<dimY; i++)
for(int j=0; j<dimX; j++)
for(int k=0; k<dimZ-1; k++){
if(!isIn[k][i][j] || !isIn[k+1][i][j])
continue;
if(value[k][i][j] > 0 && value[k+1][i][j] <= 0){
face[face_N][0] = index[k+1][i][j];
face[face_N][1] = index[k+1][i][j+1];
face[face_N][2] = index[k+1][i+1][j+1];
face[face_N][3] = index[k+1][i+1][j];
face_N++;
flag = true;
}
else if(value[k][i][j] <= 0 && value[k+1][i][j] > 0){
face[face_N][0] = index[k+1][i][j];
face[face_N][1] = index[k+1][i+1][j];
face[face_N][2] = index[k+1][i+1][j+1];
face[face_N][3] = index[k+1][i][j+1];
face_N++;
flag = true;
}
if(!flag)
continue;
flag = false;
float p[3], s[3], e[3];
s[2] = originZ + k*spaceZ;
e[2] = originZ + (k+1)*spaceZ;
s[0] = e[0] = originX + j*spaceX;
s[1] = e[1] = originY + i*spaceY;
float f1 = value[k][i][j];
float f2 = value[k+1][i][j];
searchZero(p, s, e, f1, f2, epsilon);
float g[3];
//func->gradient(g, p[0], p[1], p[2]);
double len = PolygonalMesh::LENGTH(g);
double nor[3];
nor[0] = g[0]/len;
nor[1] = g[1]/len;
nor[2] = g[2]/len;
double d = -PolygonalMesh::DOT(nor, p);
double Q_tmp[10];
MATRIX(Q_tmp, nor, d);
int i0 = index[k+1][i][j];
MAT_SUM(Q[i0], Q_tmp);
vertex[i0][0] += p[0];
vertex[i0][1] += p[1];
vertex[i0][2] += p[2];
degree[i0]++;
i0 = index[k+1][i][j+1];
MAT_SUM(Q[i0], Q_tmp);
vertex[i0][0] += p[0];
vertex[i0][1] += p[1];
vertex[i0][2] += p[2];
degree[i0]++;
i0 = index[k+1][i+1][j+1];
MAT_SUM(Q[i0], Q_tmp);
vertex[i0][0] += p[0];
vertex[i0][1] += p[1];
vertex[i0][2] += p[2];
degree[i0]++;
i0 = index[k+1][i+1][j];
MAT_SUM(Q[i0], Q_tmp);
vertex[i0][0] += p[0];
vertex[i0][1] += p[1];
vertex[i0][2] += p[2];
degree[i0]++;
}
//FOR SVD
vnl_matrix< float > A( 3, 3, 0. );
vnl_vector<float> b( 3, 0. );
for(int i=0; i<vertex_N; i++){
if(degree[i] == 0)
continue;
vertex[i][0] /= degree[i];
vertex[i][1] /= degree[i];
vertex[i][2] /= degree[i];
continue;
A[0][0] = (float)Q[i][0];
A[1][0] = A[0][1] = (float)Q[i][1];
A[2][0] = A[0][2] = (float)Q[i][2];
A[1][1] = (float)Q[i][3];
A[1][2] = A[2][1] = (float)Q[i][4];
A[2][2] = (float)Q[i][5];
float Av[3];
MAT_BY_VEC(Av, Q[i], vertex[i]);
b[1] = -(float)Q[i][6] - Av[0];
b[2] = -(float)Q[i][7] - Av[1];
b[3] = -(float)Q[i][8] - Av[2];
vnl_svd<float> svd( A );
svd.zero_out_absolute( tau );
vnl_vector< float > x = svd.solve( b );
if(fabs(x[1]) > spaceX || fabs(x[2]) > spaceY || fabs(x[3]) > spaceZ)
continue;
mesh->vertex[i][0] += x[1];
mesh->vertex[i][1] += x[2];
mesh->vertex[i][2] += x[3];
}
for(int i=0; i<dimZ+1; i++){
for(int j=0; j<dimY+1; j++){
delete[] isIn[i][j];
delete[] index[i][j];
delete[] value[i][j];
}
delete[] isIn[i];
delete[] index[i];
delete[] value[i];
}
delete[] isIn;
delete[] index;
delete[] Q;
delete[] value;
return mesh;
}
int main() {
try
{ std::cout << "Current working directory: "
<< Pathname::getCurrentWorkingDirectory() << std::endl;
// Create the system.
MultibodySystem system; system.setUseUniformBackground(true);
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
Force::UniformGravity gravity(forces, matter, 0*Vec3(2, -9.8, 0));
ContactTrackerSubsystem tracker(system);
CompliantContactSubsystem contactForces(system, tracker);
contactForces.setTrackDissipatedEnergy(true);
GeneralContactSubsystem OLDcontact(system);
const ContactSetIndex OLDcontactSet = OLDcontact.createContactSet();
contactForces.setTransitionVelocity(1e-3);
std::ifstream meshFile1, meshFile2;
PolygonalMesh femurMesh;
meshFile1.open("ContactBigMeshes_Femur.obj");
femurMesh.loadObjFile(meshFile1); meshFile1.close();
PolygonalMesh patellaMesh;
meshFile2.open("ContactBigMeshes_Patella.obj");
patellaMesh.loadObjFile(meshFile2); meshFile2.close();
ContactGeometry::TriangleMesh femurTri(femurMesh);
ContactGeometry::TriangleMesh patellaTri(patellaMesh);
DecorativeMesh showFemur(femurTri.createPolygonalMesh());
Array_<DecorativeLine> femurNormals;
const Real NormalLength = .02;
//for (int fx=0; fx < femurTri.getNumFaces(); ++fx)
// femurNormals.push_back(
// DecorativeLine(femurTri.findCentroid(fx),
// femurTri.findCentroid(fx)
// + NormalLength*femurTri.getFaceNormal(fx)));
DecorativeMesh showPatella(patellaTri.createPolygonalMesh());
Array_<DecorativeLine> patellaNormals;
//for (int fx=0; fx < patellaTri.getNumFaces(); ++fx)
// patellaNormals.push_back(
// DecorativeLine(patellaTri.findCentroid(fx),
// patellaTri.findCentroid(fx)
// + NormalLength*patellaTri.getFaceNormal(fx)));
// This transform has the meshes close enough that their OBBs overlap
// but in the end none of the faces are touching.
const Transform X_FP(
Rotation(Mat33( 0.97107625831404454, 0.23876955530133021, 0,
-0.23876955530133021, 0.97107625831404454, 0,
0, 0, 1), true),
Vec3(0.057400580865008571, 0.43859170879135373,
-0.00016506240185135300)
);
const Real fFac =1; // to turn off friction
const Real fDis = .5*0.2; // to turn off dissipation
const Real fVis = .1*.1; // to turn off viscous friction
const Real fK = 100*1e6; // pascals
// Put femur on ground at origin
matter.Ground().updBody().addDecoration(Vec3(0,0,0),
showFemur.setColor(Cyan).setOpacity(.2));
matter.Ground().updBody().addContactSurface(Vec3(0,0,0),
ContactSurface(femurTri,
ContactMaterial(fK*.01,fDis*.9,fFac*.8,fFac*.7,fVis*10),
.01 /*thickness*/));
Body::Rigid patellaBody(MassProperties(1.0, Vec3(0), Inertia(1)));
patellaBody.addDecoration(Transform(),
showPatella.setColor(Red).setOpacity(.2));
patellaBody.addContactSurface(Transform(),
ContactSurface(patellaTri,
ContactMaterial(fK*.001,fDis*.9,fFac*.8,fFac*.7,fVis*10),
.01 /*thickness*/));
MobilizedBody::Free patella(matter.Ground(), Transform(Vec3(0)),
patellaBody, Transform(Vec3(0)));
//// The old way ...
//OLDcontact.addBody(OLDcontactSet, ball,
// pyramid, Transform());
//OLDcontact.addBody(OLDcontactSet, matter.updGround(),
// ContactGeometry::HalfSpace(), Transform(R_xdown, Vec3(0,-3,0)));
//ElasticFoundationForce ef(forces, OLDcontact, OLDcontactSet);
//Real stiffness = 1e6, dissipation = 0.01, us = 0.1,
// ud = 0.05, uv = 0.01, vt = 0.01;
////Real stiffness = 1e6, dissipation = 0.1, us = 0.8,
//// ud = 0.7, uv = 0.01, vt = 0.01;
//ef.setBodyParameters(ContactSurfaceIndex(0),
// stiffness, dissipation, us, ud, uv);
//ef.setTransitionVelocity(vt);
//// end of old way.
Visualizer viz(system);
Visualizer::Reporter& reporter = *new Visualizer::Reporter(viz, ReportInterval);
viz.addDecorationGenerator(new ForceArrowGenerator(system,contactForces));
MyReporter& myRep = *new MyReporter(system,contactForces,ReportInterval);
system.addEventReporter(&myRep);
system.addEventReporter(&reporter);
// Initialize the system and state.
system.realizeTopology();
State state = system.getDefaultState();
viz.report(state);
printf("Reference state -- hit ENTER\n");
cout << "t=" << state.getTime()
<< " q=" << patella.getQAsVector(state)
<< " u=" << patella.getUAsVector(state)
<< endl;
char c=getchar();
patella.setQToFitTransform(state, ~X_FP);
viz.report(state);
printf("Initial state -- hit ENTER\n");
cout << "t=" << state.getTime()
<< " q=" << patella.getQAsVector(state)
<< " u=" << patella.getUAsVector(state)
<< endl;
c=getchar();
// Simulate it.
const clock_t start = clock();
RungeKutta3Integrator integ(system);
TimeStepper ts(system, integ);
ts.initialize(state);
ts.stepTo(2.0);
const double timeInSec = (double)(clock()-start)/CLOCKS_PER_SEC;
const int evals = integ.getNumRealizations();
cout << "Done -- took " << integ.getNumStepsTaken() << " steps in " <<
timeInSec << "s for " << ts.getTime() << "s sim (avg step="
<< (1000*ts.getTime())/integ.getNumStepsTaken() << "ms) "
<< (1000*ts.getTime())/evals << "ms/eval\n";
printf("Using Integrator %s at accuracy %g:\n",
integ.getMethodName(), integ.getAccuracyInUse());
printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(), integ.getNumStepsAttempted());
printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(), integ.getNumProjections());
while(true) {
for (int i=0; i < (int)saveEm.size(); ++i) {
viz.report(saveEm[i]);
}
getchar();
}
} catch (const std::exception& e) {
std::printf("EXCEPTION THROWN: %s\n", e.what());
exit(1);
} catch (...) {
std::printf("UNKNOWN EXCEPTION THROWN\n");
exit(1);
}
return 0;
}
PolygonalMesh* Polygonizer::computeSurfaceNetSIG02(float epsilon, float tau)
{
float p[3];
bool ***isIn = new bool**[dimZ+1];
bool ***isValid = new bool**[dimZ+1];
for(int i=0; i<dimZ+1; i++){
isIn[i] = new bool*[dimY+1];
isValid[i] = new bool*[dimY+1];
p[2] = originZ + i*spaceZ;
for(int j=0; j<dimY+1; j++){
isIn[i][j] = new bool[dimX+1];
isValid[i][j] = new bool[dimX+1];
p[1] = originY + j*spaceY;
for(int k=0; k<dimX+1; k++){
p[0] = originX + k*spaceX;
isIn[i][j][k]
= (func->value(p[0], p[1], p[2], isValid[i][j][k]) > 0);
}
}
}
int ***index = new int**[dimZ+1];
for(int i=0; i<dimZ+1; i++){
index[i] = new int*[dimY+1];
for(int j=0; j<dimY+1; j++){
index[i][j] = new int[dimX+1];
for(int k=0; k<dimX+1; k++)
index[i][j][k] = -1;
}
}
int current = 0;
int face_N = 0;
for(int i=0; i<dimZ; i++)
for(int j=0; j<dimY; j++)
for(int k=0; k<dimX-1; k++){
if(!isValid[i][j][k] || !isValid[i][j][k+1])
continue;
if((!isIn[i][j][k] && isIn[i][j][k+1]) ||
(isIn[i][j][k] && !isIn[i][j][k+1])){
face_N++;
if(index[i][j][k+1] < 0){
index[i][j][k+1] = current;
current++;
}
if(index[i][j+1][k+1] < 0){
index[i][j+1][k+1] = current;
current++;
}
if(index[i+1][j][k+1] < 0){
index[i+1][j][k+1] = current;
current++;
}
if(index[i+1][j+1][k+1] < 0){
index[i+1][j+1][k+1] = current;
current++;
}
}
}
for(int i=0; i<dimX; i++)
for(int j=0; j<dimZ; j++)
for(int k=0; k<dimY-1; k++){
if(!isValid[j][k][i] || !isValid[j][k+1][i])
continue;
if((!isIn[j][k][i] && isIn[j][k+1][i]) ||
(isIn[j][k][i] && !isIn[j][k+1][i])){
face_N++;
if(index[j][k+1][i] < 0){
index[j][k+1][i] = current;
current++;
}
if(index[j+1][k+1][i] < 0){
index[j+1][k+1][i] = current;
current++;
}
if(index[j][k+1][i+1] < 0){
index[j][k+1][i+1] = current;
current++;
}
if(index[j+1][k+1][i+1] < 0){
index[j+1][k+1][i+1] = current;
current++;
}
}
}
for(int i=0; i<dimY; i++)
for(int j=0; j<dimX; j++)
for(int k=0; k<dimZ-1; k++){
if(!isValid[k][i][j] || !isValid[k+1][i][j])
continue;
if((!isIn[k][i][j] && isIn[k+1][i][j]) ||
(isIn[k][i][j] && !isIn[k+1][i][j])){
face_N++;
if(index[k+1][i][j] < 0){
index[k+1][i][j] = current;
current++;
}
if(index[k+1][i+1][j] < 0){
index[k+1][i+1][j] = current;
current++;
}
if(index[k+1][i][j+1] < 0){
index[k+1][i][j+1] = current;
current++;
}
if(index[k+1][i+1][j+1] < 0){
index[k+1][i+1][j+1] = current;
current++;
}
}
}
PolygonalMesh* mesh = new PolygonalMesh;
int vertex_N = current;
mesh->setVertexCount(vertex_N);
float (*vertex)[3] = mesh->vertex;
int* degree = mesh->degree_f = new int[vertex_N];
current = 0;
for(int i=0; i<vertex_N; i++){
vertex[i][0] = vertex[i][1] = vertex[i][2] = 0;
degree[i] = 0;
}
/*
for(i=0; i<dimZ+1; i++)
for(int j=0; j<dimY+1; j++)
for(int k=0; k<dimX+1; k++)
if(index[i][j][k] >= 0){
index[i][j][k] = current;
vertex[current][0] = spaceX*((float)k-0.5f) + originX;
vertex[current][1] = spaceY*((float)j-0.5f) + originY;
vertex[current][2] = spaceZ*((float)i-0.5f) + originZ;
current++;
}
*/
mesh->setFaceCount(face_N);
double (*Q)[10] = new double[vertex_N][10];
for(int i=0; i<vertex_N; i++)
MAT_INIT(Q[i]);
for(int i=0; i<face_N; i++)
mesh->setPolygonCount(i, 4);
face_N = 0;
int **face = mesh->face;
bool flag = false;
for(int i=0; i<dimZ; i++)
for(int j=0; j<dimY; j++)
for(int k=0; k<dimX-1; k++){
if(!isValid[i][j][k] || !isValid[i][j][k+1])
continue;
if(isIn[i][j][k] && !isIn[i][j][k+1]){
face[face_N][0] = index[i][j][k+1];
face[face_N][1] = index[i][j+1][k+1];
face[face_N][2] = index[i+1][j+1][k+1];
face[face_N][3] = index[i+1][j][k+1];
face_N++;
flag = true;
}
else if(!isIn[i][j][k] && isIn[i][j][k+1]){
face[face_N][0] = index[i][j][k+1];
face[face_N][1] = index[i+1][j][k+1];
face[face_N][2] = index[i+1][j+1][k+1];
face[face_N][3] = index[i][j+1][k+1];
face_N++;
flag = true;
}
if(!flag)
continue;
flag = false;
float p[3], s[3], e[3];
s[0] = originX + k*spaceX;
e[0] = originX + (k+1)*spaceX;
s[1] = e[1] = originY + j*spaceY;
s[2] = e[2] = originZ + i*spaceZ;
bisection(p, s, e, epsilon);
float g[3];
func->gradient(g, p[0], p[1], p[2]);
double len = PolygonalMesh::LENGTH(g);
//if((float)len == 0)
//continue;
double nor[3];
nor[0] = g[0]/len;
nor[1] = g[1]/len;
nor[2] = g[2]/len;
double d = -PolygonalMesh::DOT(nor, p);
double Q_tmp[10];
MATRIX(Q_tmp, nor, d);
int i0 = index[i][j][k+1];
MAT_SUM(Q[i0], Q_tmp);
vertex[i0][0] += p[0];
vertex[i0][1] += p[1];
vertex[i0][2] += p[2];
degree[i0]++;
i0 = index[i][j+1][k+1];
MAT_SUM(Q[i0], Q_tmp);
vertex[i0][0] += p[0];
vertex[i0][1] += p[1];
vertex[i0][2] += p[2];
degree[i0]++;
i0 = index[i+1][j+1][k+1];
MAT_SUM(Q[i0], Q_tmp);
vertex[i0][0] += p[0];
vertex[i0][1] += p[1];
vertex[i0][2] += p[2];
degree[i0]++;
i0 = index[i+1][j][k+1];
MAT_SUM(Q[i0], Q_tmp);
vertex[i0][0] += p[0];
vertex[i0][1] += p[1];
vertex[i0][2] += p[2];
degree[i0]++;
}
for(int i=0; i<dimX; i++)
for(int j=0; j<dimZ; j++)
for(int k=0; k<dimY-1; k++){
if(!isValid[j][k][i] || !isValid[j][k+1][i])
continue;
if(isIn[j][k][i] && !isIn[j][k+1][i]){
face[face_N][0] = index[j][k+1][i];
face[face_N][1] = index[j+1][k+1][i];
face[face_N][2] = index[j+1][k+1][i+1];
face[face_N][3] = index[j][k+1][i+1];
face_N++;
flag = true;
}
else if(!isIn[j][k][i] && isIn[j][k+1][i]){
face[face_N][0] = index[j][k+1][i];
face[face_N][1] = index[j][k+1][i+1];
face[face_N][2] = index[j+1][k+1][i+1];
face[face_N][3] = index[j+1][k+1][i];
face_N++;
flag = true;
}
if(!flag)
continue;
flag = false;
float p[3], s[3], e[3];
s[1] = originY + k*spaceY;
e[1] = originY + (k+1)*spaceY;
s[2] = e[2] = originZ + j*spaceZ;
s[0] = e[0] = originX + i*spaceX;
bisection(p, s, e, epsilon);
float g[3];
func->gradient(g, p[0], p[1], p[2]);
double len = PolygonalMesh::LENGTH(g);
//if((float)len == 0)
//continue;
double nor[3];
nor[0] = g[0]/len;
nor[1] = g[1]/len;
nor[2] = g[2]/len;
double d = -PolygonalMesh::DOT(nor, p);
double Q_tmp[10];
MATRIX(Q_tmp, nor, d);
int i0 = index[j][k+1][i];
MAT_SUM(Q[i0], Q_tmp);
vertex[i0][0] += p[0];
vertex[i0][1] += p[1];
vertex[i0][2] += p[2];
degree[i0]++;
i0 = index[j+1][k+1][i];
MAT_SUM(Q[i0], Q_tmp);
vertex[i0][0] += p[0];
vertex[i0][1] += p[1];
vertex[i0][2] += p[2];
degree[i0]++;
i0 = index[j+1][k+1][i+1];
MAT_SUM(Q[i0], Q_tmp);
vertex[i0][0] += p[0];
vertex[i0][1] += p[1];
vertex[i0][2] += p[2];
degree[i0]++;
i0 = index[j][k+1][i+1];
MAT_SUM(Q[i0], Q_tmp);
vertex[i0][0] += p[0];
vertex[i0][1] += p[1];
vertex[i0][2] += p[2];
degree[i0]++;
}
for(int i=0; i<dimY; i++)
for(int j=0; j<dimX; j++)
for(int k=0; k<dimZ-1; k++){
if(!isValid[k][i][j] || !isValid[k+1][i][j])
continue;
if(isIn[k][i][j] && !isIn[k+1][i][j]){
face[face_N][0] = index[k+1][i][j];
face[face_N][1] = index[k+1][i][j+1];
face[face_N][2] = index[k+1][i+1][j+1];
face[face_N][3] = index[k+1][i+1][j];
face_N++;
flag = true;
}
else if(!isIn[k][i][j] && isIn[k+1][i][j]){
face[face_N][0] = index[k+1][i][j];
face[face_N][1] = index[k+1][i+1][j];
face[face_N][2] = index[k+1][i+1][j+1];
face[face_N][3] = index[k+1][i][j+1];
face_N++;
flag = true;
}
if(!flag)
continue;
flag = false;
float p[3], s[3], e[3];
s[2] = originZ + k*spaceZ;
e[2] = originZ + (k+1)*spaceZ;
s[0] = e[0] = originX + j*spaceX;
s[1] = e[1] = originY + i*spaceY;
bisection(p, s, e, epsilon);
float g[3];
func->gradient(g, p[0], p[1], p[2]);
double len = PolygonalMesh::LENGTH(g);
//if((float)len == 0)
//continue;
double nor[3];
nor[0] = g[0]/len;
nor[1] = g[1]/len;
nor[2] = g[2]/len;
double d = -PolygonalMesh::DOT(nor, p);
double Q_tmp[10];
MATRIX(Q_tmp, nor, d);
int i0 = index[k+1][i][j];
MAT_SUM(Q[i0], Q_tmp);
vertex[i0][0] += p[0];
vertex[i0][1] += p[1];
vertex[i0][2] += p[2];
degree[i0]++;
i0 = index[k+1][i][j+1];
MAT_SUM(Q[i0], Q_tmp);
vertex[i0][0] += p[0];
vertex[i0][1] += p[1];
vertex[i0][2] += p[2];
degree[i0]++;
i0 = index[k+1][i+1][j+1];
MAT_SUM(Q[i0], Q_tmp);
vertex[i0][0] += p[0];
vertex[i0][1] += p[1];
vertex[i0][2] += p[2];
degree[i0]++;
i0 = index[k+1][i+1][j];
MAT_SUM(Q[i0], Q_tmp);
vertex[i0][0] += p[0];
vertex[i0][1] += p[1];
vertex[i0][2] += p[2];
degree[i0]++;
}
//FOR SVD
vnl_matrix< float > A( 3, 3, 0. );
vnl_vector< float > b(3, 0.), x(3, 0.);
for(int i=0; i<vertex_N; i++){
if(degree[i] == 0)
continue;
vertex[i][0] /= degree[i];
vertex[i][1] /= degree[i];
vertex[i][2] /= degree[i];
continue;
A[0][0] = (float)Q[i][0];
A[1][0] = A[0][1] = (float)Q[i][1];
A[2][0] = A[0][2] = (float)Q[i][2];
A[1][1] = (float)Q[i][3];
A[1][2] = A[2][1] = (float)Q[i][4];
A[2][2] = (float)Q[i][5];
float Av[3];
MAT_BY_VEC(Av, Q[i], vertex[i]);
b[0] = -(float)Q[i][6] - Av[0];
b[1] = -(float)Q[i][7] - Av[1];
b[2] = -(float)Q[i][8] - Av[2];
vnl_svd<float> svd( A );
svd.zero_out_absolute( 0.0000001 );
x = svd.solve( b );
if(fabs(x[1]) > spaceX || fabs(x[2]) > spaceY || fabs(x[3]) > spaceZ)
continue;
mesh->vertex[i][0] += x[1];
mesh->vertex[i][1] += x[2];
mesh->vertex[i][2] += x[3];
}
for(int i=0; i<dimZ+1; i++){
for(int j=0; j<dimY+1; j++){
delete[] isIn[i][j];
delete[] index[i][j];
}
delete[] isIn[i];
delete[] index[i];
}
delete[] isIn;
delete[] index;
delete[] Q;
delete[] isValid;
return mesh;
}
PolygonalMesh* Polygonizer::computeSurfaceNet()
{
float p[3];
bool ***isIn = new bool**[dimZ+1];
bool ***isValid = new bool**[dimZ+1];
for(int i=0; i<dimZ+1; i++){
isIn[i] = new bool*[dimY+1];
isValid[i] = new bool*[dimY+1];
p[2] = originZ + i*spaceZ;
for(int j=0; j<dimY+1; j++){
isIn[i][j] = new bool[dimX+1];
isValid[i][j] = new bool[dimX+1];
p[1] = originY + j*spaceY;
for(int k=0; k<dimX+1; k++){
p[0] = originX + k*spaceX;
isIn[i][j][k]
= (func->value(p[0], p[1], p[2], isValid[i][j][k]) > 0);
}
}
}
int ***index = new int**[dimZ+1];
for(int i=0; i<dimZ+1; i++){
index[i] = new int*[dimY+1];
for(int j=0; j<dimY+1; j++){
index[i][j] = new int[dimX+1];
for(int k=0; k<dimX+1; k++)
index[i][j][k] = -1;
}
}
int current = 0;
int face_N = 0;
for(int i=0; i<dimZ; i++)
for(int j=0; j<dimY; j++)
for(int k=0; k<dimX-1; k++){
if(!isValid[i][j][k] || !isValid[i][j][k+1])
continue;
if((!isIn[i][j][k] && isIn[i][j][k+1]) ||
(isIn[i][j][k] && !isIn[i][j][k+1])){
face_N++;
if(index[i][j][k+1] < 0){
index[i][j][k+1] = current;
current++;
}
if(index[i][j+1][k+1] < 0){
index[i][j+1][k+1] = current;
current++;
}
if(index[i+1][j][k+1] < 0){
index[i+1][j][k+1] = current;
current++;
}
if(index[i+1][j+1][k+1] < 0){
index[i+1][j+1][k+1] = current;
current++;
}
}
}
for(int i=0; i<dimX; i++)
for(int j=0; j<dimZ; j++)
for(int k=0; k<dimY-1; k++){
if(!isValid[j][k][i] || !isValid[j][k+1][i])
continue;
if((!isIn[j][k][i] && isIn[j][k+1][i]) ||
(isIn[j][k][i] && !isIn[j][k+1][i])){
face_N++;
if(index[j][k+1][i] < 0){
index[j][k+1][i] = current;
current++;
}
if(index[j+1][k+1][i] < 0){
index[j+1][k+1][i] = current;
current++;
}
if(index[j][k+1][i+1] < 0){
index[j][k+1][i+1] = current;
current++;
}
if(index[j+1][k+1][i+1] < 0){
index[j+1][k+1][i+1] = current;
current++;
}
}
}
for(int i=0; i<dimY; i++)
for(int j=0; j<dimX; j++)
for(int k=0; k<dimZ-1; k++){
if(!isValid[k][i][j] || !isValid[k+1][i][j])
continue;
if((!isIn[k][i][j] && isIn[k+1][i][j]) ||
(isIn[k][i][j] && !isIn[k+1][i][j])){
face_N++;
if(index[k+1][i][j] < 0){
index[k+1][i][j] = current;
current++;
}
if(index[k+1][i+1][j] < 0){
index[k+1][i+1][j] = current;
current++;
}
if(index[k+1][i][j+1] < 0){
index[k+1][i][j+1] = current;
current++;
}
if(index[k+1][i+1][j+1] < 0){
index[k+1][i+1][j+1] = current;
current++;
}
}
}
PolygonalMesh* mesh = new PolygonalMesh;
int vertex_N = current;
mesh->setVertexCount(vertex_N);
float (*vertex)[3] = mesh->vertex;
current = 0;
for(int i=0; i<dimZ+1; i++)
for(int j=0; j<dimY+1; j++)
for(int k=0; k<dimX+1; k++)
if(index[i][j][k] >= 0){
index[i][j][k] = current;
vertex[current][0] = spaceX*((float)k-0.5f) + originX;
vertex[current][1] = spaceY*((float)j-0.5f) + originY;
vertex[current][2] = spaceZ*((float)i-0.5f) + originZ;
current++;
}
mesh->setFaceCount(face_N);
for(int i=0; i<face_N; i++)
mesh->setPolygonCount(i, 4);
face_N = 0;
int **face = mesh->face;
for(int i=0; i<dimZ; i++)
for(int j=0; j<dimY; j++)
for(int k=0; k<dimX-1; k++){
if(!isValid[i][j][k] || !isValid[i][j][k+1])
continue;
if(isIn[i][j][k] && !isIn[i][j][k+1]){
face[face_N][0] = index[i][j][k+1];
face[face_N][1] = index[i][j+1][k+1];
face[face_N][2] = index[i+1][j+1][k+1];
face[face_N][3] = index[i+1][j][k+1];
face_N++;
}
else if(!isIn[i][j][k] && isIn[i][j][k+1]){
face[face_N][0] = index[i][j][k+1];
face[face_N][1] = index[i+1][j][k+1];
face[face_N][2] = index[i+1][j+1][k+1];
face[face_N][3] = index[i][j+1][k+1];
face_N++;
}
}
for(int i=0; i<dimX; i++)
for(int j=0; j<dimZ; j++)
for(int k=0; k<dimY-1; k++){
if(!isValid[j][k][i] || !isValid[j][k+1][i])
continue;
if(isIn[j][k][i] && !isIn[j][k+1][i]){
face[face_N][0] = index[j][k+1][i];
face[face_N][1] = index[j+1][k+1][i];
face[face_N][2] = index[j+1][k+1][i+1];
face[face_N][3] = index[j][k+1][i+1];
face_N++;
}
else if(!isIn[j][k][i] && isIn[j][k+1][i]){
face[face_N][0] = index[j][k+1][i];
face[face_N][1] = index[j][k+1][i+1];
face[face_N][2] = index[j+1][k+1][i+1];
face[face_N][3] = index[j+1][k+1][i];
face_N++;
}
}
for(int i=0; i<dimY; i++)
for(int j=0; j<dimX; j++)
for(int k=0; k<dimZ-1; k++){
if(!isValid[k][i][j] || !isValid[k+1][i][j])
continue;
if(isIn[k][i][j] && !isIn[k+1][i][j]){
face[face_N][0] = index[k+1][i][j];
face[face_N][1] = index[k+1][i][j+1];
face[face_N][2] = index[k+1][i+1][j+1];
face[face_N][3] = index[k+1][i+1][j];
face_N++;
}
else if(!isIn[k][i][j] && isIn[k+1][i][j]){
face[face_N][0] = index[k+1][i][j];
face[face_N][1] = index[k+1][i+1][j];
face[face_N][2] = index[k+1][i+1][j+1];
face[face_N][3] = index[k+1][i][j+1];
face_N++;
}
}
for(int i=0; i<dimZ+1; i++){
for(int j=0; j<dimY+1; j++){
delete[] isIn[i][j];
delete[] index[i][j];
}
delete[] isIn[i];
delete[] index[i];
}
delete[] isIn;
delete[] index;
return mesh;
}
PolygonalMesh* Polygonizer::computeSurfaceNetOctTree(float tol, int min, int max)
{
OctTreeP oct;
oct.func = func;
oct.originX = originX;
oct.originY = originY;
oct.originZ = originZ;
oct.sizeX = dimX*spaceX;
oct.sizeY = dimY*spaceY;
oct.sizeZ = dimZ*spaceZ;
oct.constructStandard(min, max);
int vertex_N, face_N;
oct.countVertexAndFace(vertex_N, face_N);
PolygonalMesh* mesh = new PolygonalMesh;
mesh->setVertexCount(vertex_N);
mesh->setFaceCount(face_N);
for(int i=0; i<face_N; i++)
mesh->setPolygonCount(i, 4);
double (*Q)[10] = new double[vertex_N][10];
for(int i=0; i<vertex_N; i++)
MAT_INIT(Q[i]);
oct.simplify(Q, tol);
oct.polygonize2(mesh->vertex, mesh->face);
//return mesh;
//oct.simplify(Q, tol);
//oct.polygonize2(mesh->vertex, mesh->face);
//FOR SVD
vnl_matrix< float > A( 3, 3, 0. );
vnl_vector<float> b( 3, 0. ), x( 3, 0. );
for(int i=0; i<vertex_N; i++){
A[0][0] = Q[i][0];
A[1][0] = A[0][1] = Q[i][1];
A[2][0] = A[0][2] = Q[i][2];
A[1][1] = Q[i][3];
A[1][2] = A[2][1] = Q[i][4];
A[2][2] = Q[i][5];
float tmp[3];
MAT_BY_VEC(tmp, Q[i], mesh->vertex[i]);
b[1] = -Q[i][6] - tmp[0];
b[2] = -Q[i][7] - tmp[1];
b[3] = -Q[i][8] - tmp[2];
vnl_svd< float > svd( A );
svd.zero_out_relative( 0.025 );
x = svd.solve( b );
mesh->vertex[i][0] += x[1];
mesh->vertex[i][1] += x[2];
mesh->vertex[i][2] += x[3];
}
delete[] Q;
return mesh;
}
//2D mesh for cross sections. (for the paper)
PolygonalMesh* HRBF::generateCrossSection(float o[], float t1[], float t2[], int n, int m)
{
PolygonalMesh *mesh = new PolygonalMesh;
mesh->setVertexCount(n*m);
mesh->setFaceCount((n-1)*(m-1));
int **face = mesh->face;
float (*vertex)[3] = mesh->vertex;
float *v = mesh->value = new float[mesh->vertex_N];
mesh->isValid = new bool[mesh->vertex_N];
mesh->isCovered = new bool[mesh->vertex_N];
double t3[3];
PolygonalMesh::CROSS(t3, t1, t2);
double len = PolygonalMesh::LENGTH(t3);
//0.3 for kernal
t3[0] /= -0.4*len;
t3[1] /= -0.4*len;
t3[2] /= -0.4*len;
int i;
for(i = 0; i< mesh->vertex_N; i++)
mesh->isValid[i] = true;
for(i=0; i<n; i++){
float p1[3];
p1[0] = o[0] + t1[0]*i;
p1[1] = o[1] + t1[1]*i;
p1[2] = o[2] + t1[2]*i;
for(int j=0; j<m; j++){
float p2[3];
p2[0] = t2[0]*j;
p2[1] = t2[1]*j;
p2[2] = t2[2]*j;
int index = i*m+j;
vertex[index][0] = p1[0] + p2[0];
vertex[index][1] = p1[1] + p2[1];
vertex[index][2] = p1[2] + p2[2];
bool flag;
float f = value(vertex[index][0], vertex[index][1], vertex[index][2], flag);//);//
if(_isnan(f)){
v[index] = 0;
continue;
}
/*
vertex[index][0] += f*(float)t3[0];
vertex[index][1] += f*(float)t3[1];
vertex[index][2] += f*(float)t3[2];
*/
v[index] = f;
mesh->isCovered[index] = flag;
//if(!flag)
//v[index] = 1000000;
//else
//v[index] = -10000000;
}
}
for(i=0; i<n-1; i++){
for(int j=0; j<m-1; j++){
int index = i*(m-1)+j;
mesh->setPolygonCount(index, 4);
face[index][3] = i*m+j;
face[index][2] = (i+1)*m+j;
face[index][1] = (i+1)*m+j+1;
face[index][0] = i*m+j+1;
//bool flag = (v[face[index][0]] > 0);
/*
if(PolygonalMesh::LENGTH(vertex[face[index][0]]) > 5 ||
PolygonalMesh::LENGTH(vertex[face[index][1]]) > 5 ||
PolygonalMesh::LENGTH(vertex[face[index][2]]) > 5 ||
PolygonalMesh::LENGTH(vertex[face[index][3]]) > 5)
face[index][0] = -1;
*/
/*
for(int k=1; k<4; k++){
if(flag != (v[face[index][k]] > 0)){
v[i*n+j] = -100;
}
}*/
/*
if(v[face[index][0]] == v[face[index][1]]
== v[face[index][2]] == v[face[index][3]] == 0)
v[i*n+j] = 1000000;*/
}
}
mesh->computeFaceNormal();
return mesh;
}
int main() {
try {
// Create the system.
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
matter.setShowDefaultGeometry(false);
CableTrackerSubsystem cables(system);
GeneralForceSubsystem forces(system);
Force::Gravity gravity(forces, matter, -YAxis, 9.81);
// Force::GlobalDamper(forces, matter, 5);
system.setUseUniformBackground(true); // no ground plane in display
MobilizedBody Ground = matter.Ground(); // convenient abbreviation
// Read in some bones.
PolygonalMesh femur;
PolygonalMesh tibia;
femur.loadVtpFile("CableOverBicubicSurfaces-femur.vtp");
tibia.loadVtpFile("CableOverBicubicSurfaces-tibia.vtp");
femur.scaleMesh(30);
tibia.scaleMesh(30);
// Build a pendulum
Body::Rigid pendulumBodyFemur( MassProperties(1.0, Vec3(0, -5, 0),
UnitInertia(1).shiftFromCentroid(Vec3(0, 5, 0))));
pendulumBodyFemur.addDecoration(Transform(), DecorativeMesh(femur).setColor(Vec3(0.8, 0.8, 0.8)));
Body::Rigid pendulumBodyTibia( MassProperties(1.0, Vec3(0, -5, 0),
UnitInertia(1).shiftFromCentroid(Vec3(0, 5, 0))));
pendulumBodyTibia.addDecoration(Transform(), DecorativeMesh(tibia).setColor(Vec3(0.8, 0.8, 0.8)));
Rotation z180(Pi, YAxis);
MobilizedBody::Pin pendulumFemur( matter.updGround(),
Transform(Vec3(0, 0, 0)),
pendulumBodyFemur,
Transform(Vec3(0, 0, 0)) );
Rotation rotZ45(-Pi/4, ZAxis);
MobilizedBody::Pin pendulumTibia( pendulumFemur,
Transform(rotZ45, Vec3(0, -12, 0)),
pendulumBodyTibia,
Transform(Vec3(0, 0, 0)) );
Real initialPendulumOffset = -0.25*Pi;
Constraint::PrescribedMotion pres(matter,
new Function::Sinusoid(0.25*Pi, 0.2*Pi, 0*initialPendulumOffset), pendulumTibia, MobilizerQIndex(0));
// Build a wrapping cable path
CablePath path2(cables, Ground, Vec3(1, 3, 1), // origin
pendulumTibia, Vec3(1, -4, 0)); // termination
// Create a bicubic surface
Vec3 patchOffset(0, -5, -1);
Rotation rotZ90(0.5*Pi, ZAxis);
Rotation rotX90(0.2*Pi, XAxis);
Rotation patchRotation = rotZ90 * rotX90 * rotZ90;
Transform patchTransform(patchRotation, patchOffset);
Real patchScaleX = 2.0;
Real patchScaleY = 2.0;
Real patchScaleF = 0.75;
const int Nx = 4, Ny = 4;
const Real xData[Nx] = { -2, -1, 1, 2 };
const Real yData[Ny] = { -2, -1, 1, 2 };
const Real fData[Nx*Ny] = { 2, 3, 3, 1,
0, 1.5, 1.5, 0,
0, 1.5, 1.5, 0,
2, 3, 3, 1 };
const Vector x_(Nx, xData);
const Vector y_(Ny, yData);
const Matrix f_(Nx, Ny, fData);
Vector x = patchScaleX*x_;
Vector y = patchScaleY*y_;
Matrix f = patchScaleF*f_;
BicubicSurface patch(x, y, f, 0);
Real highRes = 30;
Real lowRes = 1;
PolygonalMesh highResPatchMesh = patch.createPolygonalMesh(highRes);
PolygonalMesh lowResPatchMesh = patch.createPolygonalMesh(lowRes);
pendulumFemur.addBodyDecoration(patchTransform,
DecorativeMesh(highResPatchMesh).setColor(Cyan).setOpacity(.75));
pendulumFemur.addBodyDecoration(patchTransform,
DecorativeMesh(lowResPatchMesh).setRepresentation(DecorativeGeometry::DrawWireframe));
Vec3 patchP(-0.5,-1,2), patchQ(-0.5,1,2);
pendulumFemur.addBodyDecoration(patchTransform,
DecorativePoint(patchP).setColor(Green).setScale(2));
pendulumFemur.addBodyDecoration(patchTransform,
DecorativePoint(patchQ).setColor(Red).setScale(2));
CableObstacle::Surface patchObstacle(path2, pendulumFemur, patchTransform,
ContactGeometry::SmoothHeightMap(patch));
patchObstacle.setContactPointHints(patchP, patchQ);
patchObstacle.setDisabledByDefault(true);
// Sphere
Real sphRadius = 1.5;
Vec3 sphOffset(0, -0.5, 0);
Rotation sphRotation(0*Pi, YAxis);
Transform sphTransform(sphRotation, sphOffset);
CableObstacle::Surface tibiaSphere(path2, pendulumTibia, sphTransform,
ContactGeometry::Sphere(sphRadius));
Vec3 sphP(1.5,-0.5,0), sphQ(1.5,0.5,0);
tibiaSphere.setContactPointHints(sphP, sphQ);
pendulumTibia.addBodyDecoration(sphTransform,
DecorativeSphere(sphRadius).setColor(Red).setOpacity(0.5));
// Make cable a spring
CableSpring cable2(forces, path2, 50., 18., 0.1);
Visualizer viz(system);
viz.setShowFrameNumber(true);
system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
system.addEventReporter(new ShowStuff(system, cable2, 0.02));
// Initialize the system and state.
system.realizeTopology();
State state = system.getDefaultState();
system.realize(state, Stage::Position);
viz.report(state);
cout << "path2 init length=" << path2.getCableLength(state) << endl;
cout << "Hit ENTER ...";
getchar();
// path1.setIntegratedCableLengthDot(state, path1.getCableLength(state));
// Simulate it.
saveStates.clear(); saveStates.reserve(2000);
// RungeKutta3Integrator integ(system);
RungeKuttaMersonIntegrator integ(system);
// CPodesIntegrator integ(system);
// integ.setAllowInterpolation(false);
integ.setAccuracy(1e-5);
TimeStepper ts(system, integ);
ts.initialize(state);
ShowStuff::showHeading(cout);
const Real finalTime = 10;
const double startTime = realTime();
ts.stepTo(finalTime);
cout << "DONE with " << finalTime
<< "s simulated in " << realTime()-startTime
<< "s elapsed.\n";
while (true) {
cout << "Hit ENTER FOR REPLAY, Q to quit ...";
const char ch = getchar();
if (ch=='q' || ch=='Q') break;
for (unsigned i=0; i < saveStates.size(); ++i)
viz.report(saveStates[i]);
}
} catch (const std::exception& e) {
cout << "EXCEPTION: " << e.what() << "\n";
}
}
BreakableMesh::BreakableMesh(const PolygonalMesh& m) {
this->points = m.getPoints();
this->polygons = m.getPolygons();
this->edges = m.getSegments();
this->region = m.getRegion();
}
int main(int argc, char** argv) {
cout << "Welcome to Le Fancy Vase Drawer.\n" << endl;
printf("Instructions:\n"\
"r,g,b - Change colour of mesh to red, green, blue\n"\
"k - Colour the mesh black\n"\
"p - RAINBOW.\n"\
"1,3 - Zoom in, zoom out\n"\
"w,s - Move camera up, down\n"\
"a,d - Rotate mesh left, right\n"\
"q,e - Move gaze point up, down\n"\
"z,c - Move camera and gaze point up, down\n");
float viewingAngle = 60.; // degrees
float aspectRatio = 1;
float N = 5.; // near plane
float F = 30.; // far plane
float t = N * tan(CV_PI / 360 * viewingAngle); // top
float b = -t; // bottom
float r = aspectRatio*t; // right
float l = -r; // left
int w = 512,
h = 512;
bool camChanged = true;
bool rainbow = true;
int rotationInc = 5;
int roll = 0;
namedWindow(wndName, CV_WINDOW_AUTOSIZE);
// used as row vectors, so they can be appended to Matrix easily
Mat e = (Mat_<float>(1, 3) << 30., 30., 22.); // camera vector. 15, 15, 10
Mat g = (Mat_<float>(1, 3) << 0., 0., 18.); // a point through which the gaze direction unit vector n points to
Mat p = (Mat_<float>(1, 3) << 0., 0., 1.); // x, y, z, w
Mat n, u, v;
Mat screen(w, h, CV_8UC3);
int flip = 1; // -1 to flip along X
Mat Mv(0, 3, CV_32FC1);
Mat S1T1Mp = (Mat_<float>(4, 4) <<
(2*N)/(r-l), 0, (r+l)/(r-l), 0,
0, (2*N)/(t-b), (t+b)/(t-b), 0,
0, 0, -(F+N)/(F-N), -2*F*N/(F-N),
0, 0, -1, 0
);
Mat WS2T2 = (Mat_<float>(4, 4) <<
w/2, 0, 0, w/2,
0, flip*h/2, 0, -h/2+h,
0, 0, 1, 0,
0, 0, 0, 1
);
// container for screen coords
vector<Point2i> coords;
// background colour and line colour
Scalar bgColour(255, 255, 255);
Scalar lineColour(0);
// HSV and BGR matrices for colours of the rainbow
int sat = 200, val = 200;
Mat hsv(Size(1,1),CV_8UC3, Scalar(10,sat,val)), bgr;
// the polygonal mesh object
PolygonalMesh poly;
poly.readFromFile("PolyVase.xml");
bool normalChanged = true;
char c = -1; // input char
while (true) {
if (camChanged) {
if (normalChanged) {
// normalize vector from camera to gaze point
normalize(e - g, n);
// generate vectors describing camera plane
//u = (getRotationMatrix(n, rotationInc) * u.t()).t();
//v = (getRotationMatrix(n, rotationInc) * v.t()).t();
// normalize to keep window same size
//normalize(u, u);
//normalize(v, v);
u = p.cross(n);
v = u.cross(n);
u = (getRotationMatrix(n, roll) * u.t()).t();
v = (getRotationMatrix(n, roll) * v.t()).t();
normalize(u, u);
normalize(v, v);
normalChanged = false;
}
// construct matrix for world coords to camera viewing coords
Mv = Mat(0, 3, CV_32FC1);
Mv.push_back(u);
Mv.push_back(v);
Mv.push_back(n);
Mv = Mv.t();
Mv.push_back(Mat((Mat_<float>(1, 3) << -e.dot(u), -e.dot(v), -e.dot(n))));
Mv = Mv.t();
Mv.push_back(Mat((Mat_<float>(1, 4) << 0, 0, 0, 1))); // works
//scale, transformation, and projection matrix
S1T1Mp = (Mat_<float>(4, 4) <<
(2*N)/(r-l), 0, (r+l)/(r-l), 0,
0, (2*N)/(t-b), (t+b)/(t-b), 0,
0, 0, -(F+N)/(F-N), -2*F*N/(F-N),
0, 0, -1, 0
);
// scal, transform from viewing volume to canonical viewing volume.
// Flip along X-axis is desired
WS2T2 = (Mat_<float>(4, 4) <<
w/2, 0, 0, w/2,
0, flip*h/2, 0, -h/2+h,
0, 0, 1, 0,
0, 0, 0, 1
);
// colour the background
screen.setTo(bgColour);
camChanged = false;
}
coords.clear();
coords.reserve(poly.vertsH.size());
for (int i = 0; i < poly.vertsH.size(); i++) {
// Apply transformations to convert from world coordinates to screen coords
Mat pt = WS2T2 * (S1T1Mp * (Mv * poly.vertsH[i]));
// Perspective divide
pt /= pt.at<float>(3, 0);
// store generated coordinate in coords container
coords.push_back(Point2f((int)pt.at<float>(0), (int)pt.at<float>(1)));
}
if (rainbow) {
hsv = Mat(Size(1, 1), CV_8UC3, Scalar(10, sat, val));
}
for (int i = 0; i < poly.faces.size(); i++) {
Face& f = poly.faces[i];
Normal faceNormal = poly.norms[f.data[Face::NORM]];
Mat tv = poly.vertsH[f.data[Face::PT0]]; // triangle 1st vertex
// generate camera to triangle vector
Vec3f camToTri(tv.at<float>(X) - e.at<float>(X),
tv.at<float>(Y) - e.at<float>(Y),
tv.at<float>(Z) - e.at<float>(Z));
// compute dot product to determine which faces to draw.
float b = faceNormal.dot(camToTri); // camToTri.dot(faceNormal);
if (rainbow) {
// increment hue value, then convert from HSV to BGR
Vec3b clr = hsv.at<Vec3b>(0, 0);
clr[0]++;
hsv.at<Vec3b>(0, 0) = clr;
cvtColor(hsv, bgr, CV_HSV2BGR);
Vec3b bgr3 = bgr.at<Vec3b>(0, 0);
lineColour = Scalar(bgr3[0], bgr3[1], bgr3[2]);
}
if (b >= 0) {
// draw triangle from 3 face coords
line(screen, coords[f.data[Face::PT0]], coords[f.data[Face::PT1]], lineColour);
line(screen, coords[f.data[Face::PT0]], coords[f.data[Face::PT2]], lineColour);
line(screen, coords[f.data[Face::PT1]], coords[f.data[Face::PT2]], lineColour);
}
}
// draw the image to the screen
imshow(wndName, screen);
c = waitKey(0);
switch (c) {
case 'w':
// move camera up
e.at<float>(Z) += 1;
camChanged = true;
normalChanged = true;
break;
case 's':
// move camera down
e.at<float>(Z) -= 1;
camChanged = true;
normalChanged = true;
break;
case 'a':{
// rotate by 5 degrees along z axis
float a = rotationInc/180.*CV_PI;
e = ((Mat_<float>(3, 3) <<
cos(a), sin(a), 0,
-sin(a), cos(a), 0,
0, 0, 1) * e.t()).t();
camChanged = true;
normalChanged = true;
}
break;
case 'd':{
// rotate by -5 degrees along z axis
float a = -rotationInc/180.*CV_PI;
e = ((Mat_<float>(3, 3) <<
cos(a), sin(a), 0,
-sin(a), cos(a), 0,
0, 0, 1) * e.t()).t();
camChanged = true;
normalChanged = true;
}
break;
case 'y': {
roll += rotationInc;
// rotate camera
// rotate around unit vector 'n' -- from gaze point to cam
//u = (getRotationMatrix(n, rotationInc) * u.t()).t();
//v = (getRotationMatrix(n, rotationInc) * v.t()).t();
// normalize to keep window same size
//normalize(u, u);
//normalize(v, v);
normalChanged = true;
camChanged = true;
}
break;
case 'u':
roll -= rotationInc;
//u = (getRotationMatrix(n, -rotationInc) * u.t()).t();
//v = (getRotationMatrix(n, -rotationInc) * v.t()).t();
//normalize(u, u);
//normalize(v, v);
normalChanged = true;
camChanged = true;
break;
case 'q':
// shift gaze vector down
g.at<float>(Z) -= 1;
camChanged = true;
normalChanged = true;
break;
case 'e':
// shift gaze vector up
g.at<float>(Z) += 1;
camChanged = true;
normalChanged = true;
break;
case 'c':
// move camera and gaze vector up
g.at<float>(Z) += 1;
e.at<float>(Z) += 1;
camChanged = true;
break;
case 'z':
// move camera and gaze vector down
g.at<float>(Z) -= 1;
e.at<float>(Z) -= 1;
camChanged = true;
break;
case '1':
// move camera closer to object by 10%
e *= 0.9;
camChanged = true;
break;
case '3':
// move cam further from object by 10%
e *= 1.1;
camChanged = true;
break;
case 'r':
// colour the mesh red, green, blue, black (next 4 cases)
lineColour = Scalar(0, 0, 255);
rainbow = false;
break;
case 'g':
lineColour = Scalar(0, 150, 0);
rainbow = false;
break;
case 'b':
lineColour = Scalar(255, 0, 0);
rainbow = false;
break;
case 'k':
lineColour = Scalar(0, 0, 0);
rainbow = false;
break;
case 'p':
//set the rainbow flag
rainbow = true;
break;
default:
break;
}
}
// chillout until the user has hit a key
waitKey();
getchar();
}