typename SurfaceBase<VertexType,EdgeType,TriangleType>::ctype SurfaceBase<VertexType,EdgeType,TriangleType>::minInteriorAngle(int n) const
{
ctype minAngle = 2*M_PI;
const std::array<int, 3>& p = triangles(n).vertices;
for (int i=0; i<3; i++){
StaticVector<ctype,3> a = vertices(p[(i+1)%3]) - vertices(p[i]);
StaticVector<ctype,3> b = vertices(p[(i+2)%3]) - vertices(p[i]);
ctype angle = acosf(a.dot(b) / (a.length() * b.length()));
if (angle<minAngle)
minAngle = angle;
}
return minAngle;
}
void ContactMapping<2,ctype>::build(const std::vector<std::array<ctype,2> >& coords1, ///< The vertex coordinates of the first surface
const std::vector<std::array<int,2> >& tri1, ///< The triangles of the first surface
const std::vector<std::array<ctype,2> >& coords2, ///< The vertices of the second surface
const std::vector<std::array<int,2> >& tri2,
const DirectionFunction<2,ctype>* domainDirection,
const DirectionFunction<2,ctype>* targetDirection
)
{
int numVertices1 = coords1.size();
int numVertices2 = coords2.size();
int nTri1 = tri1.size();
int nTri2 = tri2.size();
#if 0
printf("----- 1 -----\n");
for (int i=0; i<nTri1; i++)
printf("-- %d %d\n", tri1[2*i], tri1[2*i+1]);
printf("----- 2 -----\n");
for (int i=0; i<nTri2; i++)
printf("-- %d %d\n", tri2[2*i], tri2[2*i+1]);
#endif
// //////////////////////////////////////////////////
// Build domain surface and its normal field
// //////////////////////////////////////////////////
psurface_.domainVertices.resize(numVertices1);
for (int i=0; i<numVertices1; i++)
for (int j=0; j<2; j++)
psurface_.domainVertices[i][j] = coords1[i][j];
// Build the domain segments
psurface_.domainSegments.clear(); // may contain old stuff from previous runs
psurface_.domainSegments.resize(nTri1);
for (int i=0; i<nTri1; i++) {
psurface_.domainSegments[i].points[0] = tri1[i][0];
psurface_.domainSegments[i].points[1] = tri1[i][1];
}
// ///////////////////////////////
// Build the domain normal field
// ///////////////////////////////
std::vector<StaticVector<ctype, 2> > domainNormals;
std::vector<StaticVector<ctype, 2> > targetNormals;
computeDiscreteDomainDirections(domainDirection, domainNormals);
// //////////////////////////////////////////////////
// Build range surface and its normal field
// //////////////////////////////////////////////////
// first mark the vertices that are actually used
psurface_.targetVertices.resize(numVertices2);
for (int i=0; i<numVertices2; i++)
for (int j=0; j<2; j++)
psurface_.targetVertices[i][j] = coords2[i][j];
// /////////////////////////////////////////////////////
// Build the segments-per-vertex arrays
// /////////////////////////////////////////////////////
std::vector<std::array<int, 2> > segPerVertex1(psurface_.domainVertices.size());
for (size_t i=0; i<segPerVertex1.size(); i++)
segPerVertex1[i][0] = segPerVertex1[i][1] = -1;
for (int i=0; i<nTri1; i++) {
//printf("segment %d: %d %d -- %d %d\n", i, tri2[2*i], tri2[2*i+1], used2[tri2[2*i]],used2[tri2[2*i+1]]);
for (int j=0; j<2; j++) {
int p = tri1[i][j];
if (segPerVertex1[p][0]==-1)
segPerVertex1[p][0] = i;
else
segPerVertex1[p][1] = i;
}
}
// use this to construct the neighbor relationships between segments
for (size_t i=0; i<psurface_.domainSegments.size(); i++) {
int vertex0 = psurface_.domainSegments[i].points[0];
int other0 = (segPerVertex1[vertex0][0] == i) ? segPerVertex1[vertex0][1] : segPerVertex1[vertex0][0];
psurface_.domainSegments[i].neighbor[0] = other0;
int vertex1 = psurface_.domainSegments[i].points[1];
int other1 = (segPerVertex1[vertex1][0] == i) ? segPerVertex1[vertex1][1] : segPerVertex1[vertex1][0];
psurface_.domainSegments[i].neighbor[1] = other1;
//printf("Segment %d neighbors: %d %d\n", i, other0, other1);
}
// Build the segments-per-vertex arrays for the target vertices
std::vector<std::array<int, 2> > segPerVertex2(psurface_.targetVertices.size());
for (size_t i=0; i<segPerVertex2.size(); i++)
segPerVertex2[i][0] = segPerVertex2[i][1] = -1;
for (int i=0; i<nTri2; i++) {
//printf("segment %d: %d %d -- %d %d\n", i, tri2[2*i], tri2[2*i+1], tri2[2*i], tri2[2*i+1]);
for (int j=0; j<2; j++) {
int p = tri2[i][j];
if (segPerVertex2[p][0]==-1)
segPerVertex2[p][0] = i;
else
segPerVertex2[p][1] = i;
}
}
computeDiscreteTargetDirections(tri2, targetDirection, targetNormals);
// ///////////////////////////////////////////////////////////////////////
// Project the vertices of the target surface onto the domain surface
// ///////////////////////////////////////////////////////////////////////
const ctype eps = 1e-10;
for (size_t i=0; i<psurface_.targetVertices.size(); i++) {
ctype bestLocalPos = std::numeric_limits<ctype>::max(); // init to something
int bestSegment = -1;
ctype bestDist = std::numeric_limits<ctype>::max();
for (int j=0; j<psurface_.domainSegments.size(); j++) {
const StaticVector<ctype,2>& p0 = psurface_.domainVertices[psurface_.domainSegments[j].points[0]];
const StaticVector<ctype,2>& p1 = psurface_.domainVertices[psurface_.domainSegments[j].points[1]];
const StaticVector<ctype,2>& n0 = domainNormals[psurface_.domainSegments[j].points[0]];
const StaticVector<ctype,2>& n1 = domainNormals[psurface_.domainSegments[j].points[1]];
ctype local; // the unknown...
if (NormalProjector<ctype>::computeInverseNormalProjection(p0, p1, n0, n1,
psurface_.targetVertices[i], local)) {
// We want that the line from the domain surface to its projection
// approaches the target surface from the front side, i.e., it should
// not pass through the body represented by the target surface.
// We do a simplified test by comparing the connecting segment
// with the normal at the target surface and the normal at the
// domain surface
/** \todo Rewrite this once we have expression templates */
StaticVector<ctype,2> base;
StaticVector<ctype, 2> baseNormal;
StaticVector<ctype, 2> segment;
for (int k=0; k<2; k++) {
base[k] = (1-local)*p0[k] + local*p1[k];
baseNormal[k] = (1-local)*n0[k] + local*n1[k];
segment[k] = psurface_.targetVertices[i][k] - base[k];
}
ctype distance = segment.length2();
if (segment.dot(targetNormals[i]) > -0.0001
&& segment.dot(baseNormal) > -0.0001
&& distance > 1e-8) {
//printf("aborting %g %g %g\n", segment * targetNormals[i], segment * baseNormal, distance);
continue;
}
// There may be several inverse orthogonal projections.
// We want the shortest one.
if (distance < bestDist) {
bestDist = distance;
bestLocalPos = local;
bestSegment = j;
}
}
}
// /////////////////////////////////////////////
// We have found a valid projection
// /////////////////////////////////////////////
if (bestSegment != -1) {
typename PSurface<1,ctype>::DomainSegment& bS = psurface_.domainSegments[bestSegment];
if (bestLocalPos < eps) {
// Insert as new first element
bS.nodes.insert(bS.nodes.begin(), typename PSurface<1,ctype>::Node(0, 1, true, true, segPerVertex2[i][0], segPerVertex2[i][1]));
// Look for left neighbor segment
if (psurface_.domainSegments[bestSegment].neighbor[0] != -1) {
psurface_.domainSegments[psurface_.domainSegments[bestSegment].neighbor[0]].nodes.push_back( typename PSurface<1,ctype>::Node(1, 0, true, true,
segPerVertex2[i][0], segPerVertex2[i][1]) );
}
} else if (bestLocalPos > 1-eps) {
typename PSurface<1,ctype>::Node newNode(1, 0, true, true, segPerVertex2[i][0], segPerVertex2[i][1]);
bS.nodes.push_back(newNode);
// Look for right neighbor segment
if (psurface_.domainSegments[bestSegment].neighbor[1] != -1) {
typename PSurface<1,ctype>::DomainSegment& rightNeighborSegment = psurface_.domainSegments[psurface_.domainSegments[bestSegment].neighbor[1]];
rightNeighborSegment.nodes.insert(rightNeighborSegment.nodes.begin(),
typename PSurface<1,ctype>::Node(0, 1, true, true, segPerVertex2[i][0], segPerVertex2[i][1]));
}
} else {
int nNodes = bS.nodes.size();
bS.nodes.resize(nNodes+1);
int j=nNodes-1;
for (; j>=0; j--) {
if (bS.nodes[j].domainLocalPosition > bestLocalPos)
bS.nodes[j+1] = bS.nodes[j];
else
break;
}
bS.nodes[j+1] = typename PSurface<1,ctype>::Node(bestLocalPos, 0, false, true, segPerVertex2[i][0], segPerVertex2[i][1]);
}
}
}
// //////////////////////////////////////////////////////////////////////
// Insert missing nodes that belong to vertices of the domain segment
// //////////////////////////////////////////////////////////////////////
for (int i=0; i<psurface_.domainSegments.size(); i++) {
typename PSurface<1,ctype>::DomainSegment& cS = psurface_.domainSegments[i];
// Insert node belonging to domain vertex to the segment to the left of the vertex
if (cS.nodes.size()==0
|| !cS.nodes[0].isNodeOnVertex
|| (cS.nodes.size()==1 && cS.nodes[0].isNodeOnVertex && cS.nodes[0].domainLocalPosition > 1-eps)) {
ctype rangeLocalPosition;
int rangeSegment;
if (NormalProjector<ctype>::normalProjection(psurface_.domainVertices[cS.points[0]], domainNormals[cS.points[0]],
rangeSegment, rangeLocalPosition,
tri2, coords2)) {
typename PSurface<1,ctype>::Node newNode(0, rangeLocalPosition, true, false, rangeSegment, rangeSegment);
cS.nodes.insert(cS.nodes.begin(), newNode);
}
}
// Insert node belonging to domain vertex to the segment to the right of the vertex
if (cS.nodes.size()==0
|| !cS.nodes.back().isNodeOnVertex
|| (cS.nodes.size()==1 && cS.nodes[0].isNodeOnVertex && cS.nodes[0].domainLocalPosition < eps)) {
ctype rangeLocalPosition;
int rangeSegment;
if (NormalProjector<ctype>::normalProjection(psurface_.domainVertices[cS.points[1]], domainNormals[cS.points[1]],
rangeSegment, rangeLocalPosition,
tri2, coords2)) {
typename PSurface<1,ctype>::Node newNode(1, rangeLocalPosition, true, false, rangeSegment, rangeSegment);
cS.nodes.push_back(newNode);
}
}
}
#if 0
for (int i=0; i<psurface_.domainSegments.size(); i++) {
printf(" --- segment %d --- (%d --> %d)\n", i,
psurface_.domainSegments[i].points[0],psurface_.domainSegments[i].points[1]);
for (int j=0; j<psurface_.domainSegments[i].nodes.size(); j++)
std::cout << psurface_.domainSegments[i].nodes[j];
std::cout << std::endl;
}
#endif
// /////////////////////////////////////////////////////
// Insert edges
// /////////////////////////////////////////////////////
/** \todo Only works if the relevant domain is a single connected component */
for (int i=0; i<psurface_.domainSegments.size(); i++) {
std::vector<typename PSurface<1,ctype>::Node>& nodes = psurface_.domainSegments[i].nodes;
////////////////////////////////
for (int j=0; j<int(nodes.size())-1; j++) {
if (nodes[j].rangeSegments[0] == nodes[j+1].rangeSegments[0])
nodes[j].rightRangeSegment = nodes[j].rangeSegments[0];
else if (nodes[j].rangeSegments[0] == nodes[j+1].rangeSegments[1])
nodes[j].rightRangeSegment = nodes[j].rangeSegments[0];
else if (nodes[j].rangeSegments[1] == nodes[j+1].rangeSegments[0])
nodes[j].rightRangeSegment = nodes[j].rangeSegments[1];
else if (nodes[j].rangeSegments[1] == nodes[j+1].rangeSegments[1])
nodes[j].rightRangeSegment = nodes[j].rangeSegments[1];
else
throw(std::runtime_error("Segment of the PSurface<1> data structure is inconsistent!"));
if (nodes[j].rightRangeSegment == -1)
throw(std::runtime_error("Segment of the PSurface<1> data structure is inconsistent!"));
}
}
}
ctype CircularPatch<ctype>::distanceTo(const StaticVector<ctype,3> &p) const
{
int i, j;
ctype bestDist = std::numeric_limits<ctype>::max();
// check point against triangles
for (j=0; j<size(); j++){
const DomainTriangle<ctype>& cT = par->triangles(triangles[j]);
StaticVector<ctype,3> triPoints[3];
triPoints[0] = par->vertices(cT.vertices[0]);
triPoints[1] = par->vertices(cT.vertices[1]);
triPoints[2] = par->vertices(cT.vertices[2]);
// local base
StaticVector<ctype,3> a = triPoints[1] - triPoints[0];
StaticVector<ctype,3> b = triPoints[2] - triPoints[0];
StaticVector<ctype,3> c = a.cross(b);
c.normalize();
StaticVector<ctype,3> x = p - triPoints[0];
// write x in the new base (Cramer's rule)
StaticMatrix<ctype,3> numerator(a, b, c);
StaticMatrix<ctype,3> alphaMat(x, b, c);
StaticMatrix<ctype,3> betaMat(a, x, c);
StaticMatrix<ctype,3> gammaMat(a, b, x);
ctype alpha = alphaMat.det()/numerator.det();
ctype beta = betaMat.det()/numerator.det();
ctype gamma = gammaMat.det()/numerator.det();
// check whether orthogonal projection onto the ab plane is in triangle
bool isIn = alpha>=0 && beta>=0 && (1-alpha-beta)>=0;
if (isIn && fabs(gamma)<bestDist){
// printf("a(%1.2f %1.2f %1.2f) b(%1.2f %1.2f %1.2f) c(%1.2f %1.2f %1.2f) x(%1.2f %1.2f %1.2f)\n",
// a.x, a.y, a.z, b.x, b.y, b.z, c.x, c.y, c.z, x.x, x.y, x.z);
// printf("tri: %d, alpha = %f, beta = %f, gamma = %f\n", j, alpha, beta, gamma);
bestDist = fabs(gamma);
}
}
// check point against edges
for (i=0; i<size(); i++){
for (j=0; j<3; j++){
const DomainTriangle<ctype>& cT = par->triangles(triangles[i]);
StaticVector<ctype,3> from = par->vertices(cT.vertices[j]);
StaticVector<ctype,3> to = par->vertices(cT.vertices[(j+1)%3]);
StaticVector<ctype,3> edge = to - from;
ctype projectLength = edge.dot(p - from)/edge.length();
StaticVector<ctype,3> projection = edge/edge.length() * projectLength;
ctype orthoDist = ((p-from) - projection).length();
if (projectLength>=0 && projectLength<=edge.length() && orthoDist<bestDist)
bestDist = orthoDist;
}
}
// check point against vertices
for (i=0; i<size(); i++){
for (j=0; j<3; j++){
ctype dist = (p - par->vertices(par->triangles(triangles[i]).vertices[j])).length();
if (dist < bestDist){
bestDist = dist;
}
}
}
return bestDist;
}