// 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 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 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);
}
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);
}