///\bug This function does not properly compute the centroid.
float3 Polygon::Centroid() const
{
float3 centroid = float3::zero;
for(int i = 0; i < NumVertices(); ++i)
centroid += Vertex(i);
return centroid / (float)NumVertices();
}
///\bug This function does not properly compute the centroid.
vec Polygon::Centroid() const
{
if (NumVertices() == 0)
return vec::nan;
vec centroid = vec::zero;
for(int i = 0; i < NumVertices(); ++i)
centroid += Vertex(i);
return centroid / (float)NumVertices();
}
Polyhedron Polygon::ToPolyhedron() const
{
Polyhedron poly;
poly.v = p;
poly.f.push_back(Polyhedron::Face());
poly.f.push_back(Polyhedron::Face());
for(int i = 0; i < NumVertices(); ++i)
{
poly.f[0].v.push_back(i);
poly.f[1].v.push_back(NumVertices()-1-i);
}
return poly;
}
void BasicMesh::Write(const string &filename) const {
string content;
// write verts
for (int i = 0,offset=0; i < NumVertices(); ++i) {
content += "v ";
content += to_string(verts(i, 0)) + " "; ++offset;
content += to_string(verts(i, 1)) + " "; ++offset;
content += to_string(verts(i, 2)) + "\n"; ++offset;
}
// write texture coordinates
for (int i = 0; i < texcoords.rows(); ++i) {
content += "vt ";
content += to_string(texcoords(i, 0)) + " ";
content += to_string(texcoords(i, 1)) + "\n";
}
// write faces together with texture coordinates indices
for (int i = 0, offset = 0; i < NumFaces(); ++i) {
content += "f ";
content += to_string(faces(i, 0) + 1) + "/" + to_string(face_tex_index(i, 0) + 1) + "/0" + " "; ++offset;
content += to_string(faces(i, 1) + 1) + "/" + to_string(face_tex_index(i, 1) + 1) + "/0" + " "; ++offset;
content += to_string(faces(i, 2) + 1) + "/" + to_string(face_tex_index(i, 2) + 1) + "/0" + "\n"; ++offset;
}
ofstream fout(filename);
fout << content << endl;
fout.close();
}
std::shared_ptr<Geom> TriSoup::CreateGeom() const
{
if(NumVertices() > std::numeric_limits<unsigned short>::max())
return CreateGeom<unsigned int>(*this);
else
return CreateGeom<unsigned short>(*this);
}
void CalculateBoundingBox(vector_t& vMinOut, vector_t& vMaxOut,
TIterator begin, TIterator end,
TAAPos& aaPos)
{
if(begin == end){
VecSet(vMinOut, 0);
VecSet(vMaxOut, 0);
return;
}
vMinOut = vMaxOut = aaPos[GetVertex(*begin, 0)];
const int dim = vector_t::Size;
// iterate over all elements and find min and max values
vector_t tmin, tmax;
for(TIterator iter = begin; iter != end; ++iter){
typename TIterator::value_type elem = *iter;
for(size_t i_vrt = 0; i_vrt < NumVertices(elem); ++i_vrt){
for(int i = 0; i < dim; ++i){
vector_t& v = aaPos[GetVertex(elem, i_vrt)];
if(v[i] < vMinOut[i])
vMinOut[i] = v[i];
else if(v[i] > vMaxOut[i])
vMaxOut[i] = v[i];
}
}
}
}
bool Polyhedron::IsConvex() const
{
// This function is O(n^2).
/** @todo Real-Time Collision Detection, p. 64:
A faster O(n) approach is to compute for each face F of P the centroid C of F,
and for all neighboring faces G of F test if C lies behind the supporting plane of
G. If some C fails to lie behind the supporting plane of one or more neighboring
faces, P is concave, and is otherwise assumed convex. However, note that just as the
corresponding polygonal convexity test may fail for a pentagram this test may fail for,
for example, a pentagram extruded out of its plane and capped at the ends. */
for(int f = 0; f < NumFaces(); ++f)
{
Plane p = FacePlane(f);
for(int i = 0; i < NumVertices(); ++i)
{
float d = p.SignedDistance(Vertex(i));
if (d > 1e-3f) // Tolerate a small epsilon error.
{
printf("Distance of vertex %d from plane %d: %f", i, f, d);
return false;
}
}
}
return true;
}
//finds the index of this vertex in the VertexMapTable.
//if not found, returns -1
INT cOFF_READER::FindIndexInVertexMap(INT gridVertexIndex)
{
for (INT i=0; i < NumVertices(); i++){
if (m_vertexMapTable[i] == gridVertexIndex)
return i;
}
return -1;
}
AABB Polygon::MinimalEnclosingAABB() const
{
AABB aabb;
aabb.SetNegativeInfinity();
for(int i = 0; i < NumVertices(); ++i)
aabb.Enclose(Vertex(i));
return aabb;
}
void update_mvertex_network(UVertexNetwork vn, MVertexNetwork& mvn) {
assert(vn.vertices == mvn.vertices.get());
assert(vn.cpoints == mvn.cpoints.get());
// mvn.num_vertices = NumVertices(vn);
// mvn.num_cpoints = NumCPoints(vn);
SetNumVertices(NumVertices(vn), &mvn);
SetNumCPoints(NumCPoints(vn), &mvn);
}
void BasicMesh::UpdateVertices(const VectorXd &vertices) {
boost::timer::auto_cpu_timer timer("[BasicMesh] Vertices update time = %w seconds.\n");
const int num_vertices = NumVertices();
#pragma omp parallel for
for(int i=0;i<num_vertices;++i) {
const int offset = i * 3;
verts(i, 0) = vertices(offset+0);
verts(i, 1) = vertices(offset+1);
verts(i, 2) = vertices(offset+2);
}
}
void TriSoup::RemapData( const std::vector<int>& faceOrder )
{
ASSERT(faceOrder.size() == m_faces.size());
// use the new cache aware index buffer to reorder the vertex buffer data. Create a map
// from idxOld -> idxNew.
const int numVerts = NumVertices();
std::vector<int> vertRemap(numVerts, -1);
std::vector<Vertex> newVerts(NumVertices());
std::vector<Face> newFaces(NumFaces());
int curVert = 0;
for(int i = 0, c = faceOrder.size(); i < c; ++i)
{
// put the face in the correct order
ASSERT(faceOrder[i] < NumFaces());
newFaces[i] = m_faces[faceOrder[i]];
// remap the vertices in the current face
for(int j = 0; j < 3; ++j)
{
int v0 = newFaces[i].m_vertices[j];
if(vertRemap[v0] == -1)
{
newVerts[curVert] = m_vertices[v0];
vertRemap[v0] = curVert;
++curVert;
}
newFaces[i].m_vertices[j] = vertRemap[v0];
}
}
ASSERT(curVert == (int)vertRemap.size());
m_faces.swap(newFaces);
m_vertices.swap(newVerts);
}
int Polyhedron::ExtremeVertex(const float3 &direction) const
{
int mostExtreme = -1;
float mostExtremeDist = -FLT_MAX;
for(int i = 0; i < NumVertices(); ++i)
{
float d = Dot(direction, Vertex(i));
if (d > mostExtremeDist)
{
mostExtremeDist = d;
mostExtreme = i;
}
}
return mostExtreme;
}
float3 Polygon::ExtremePoint(const float3 &direction) const
{
float3 mostExtreme = float3::nan;
float mostExtremeDist = -FLT_MAX;
for(int i = 0; i < NumVertices(); ++i)
{
float3 pt = Vertex(i);
float d = Dot(direction, pt);
if (d > mostExtremeDist)
{
mostExtremeDist = d;
mostExtreme = pt;
}
}
return mostExtreme;
}
vec Polygon::ExtremePoint(const vec &direction, float &projectionDistance) const
{
vec mostExtreme = vec::nan;
projectionDistance = -FLOAT_INF;
for(int i = 0; i < NumVertices(); ++i)
{
vec pt = Vertex(i);
float d = Dot(direction, pt);
if (d > projectionDistance)
{
projectionDistance = d;
mostExtreme = pt;
}
}
return mostExtreme;
}
void TriSoup::Merge(const TriSoup* other)
{
const int prevNumVerts = NumVertices();
for(int i = 0, c = other->NumVertices(); i < c; ++i)
AddVertex(other->m_vertices[i].m_pos);
for(int i = 0, c = other->NumFaces(); i < c; ++i)
{
int indices[3];
other->GetFace(i, indices);
for(int j = 0; j < 3; ++j)
indices[j] += prevNumVerts;
AddFace(indices[0], indices[1], indices[2]);
}
}
void BasicMesh::BuildHalfEdgeMesh() {
vhandles.clear();
vhandles_map.clear();
fhandles.clear();
fhandles_map.clear();
hemesh.clear();
const int num_verts = NumVertices();
const int num_faces = NumFaces();
vhandles.resize(num_verts);
for(int i=0;i<num_verts;++i) {
const auto& pi = verts.row(i);
vhandles[i] = hemesh.add_vertex(HalfEdgeMesh::Point(pi[0], pi[1], pi[2]));
vhandles_map[vhandles[i]] = i;
}
fhandles.resize(num_faces);
for(int i=0;i<num_faces;++i) {
const auto& fi = faces.row(i);
fhandles[i] = hemesh.add_face(
vector<HalfEdgeMesh::VertexHandle>{vhandles[fi[0]],
vhandles[fi[1]],
vhandles[fi[2]]});
fhandles_map[fhandles[i]] = i;
}
// For debugging
#if 0
try
{
if ( !OpenMesh::IO::write_mesh(hemesh, "mesh.obj") )
{
std::cerr << "Cannot write mesh to file 'output.off'" << std::endl;
exit(1);
}
}
catch( std::exception& x )
{
std::cerr << x.what() << std::endl;
exit(1);
}
#endif
}
Sphere::Sphere(int numSubdiv, bool smooth) : _vertices(NULL), _indices(NULL), _numVerts(0) { // not exception-safe
_numVerts = NumVertices(numSubdiv);
_vertices = new Mesh::Vertex[_numVerts];
_indices = new Mesh::Index[_numVerts];
Mesh::Vertex* vert = _vertices;
unsigned numTris = NumTriangles(numSubdiv);
for(Mesh::Index i = 0; i < 20; ++i) {
Subdivide(&vert, numSubdiv, smooth,
g_vertices[g_indices[i][0]].position,
g_vertices[g_indices[i][1]].position,
g_vertices[g_indices[i][2]].position);
}
for(Mesh::Index i = 0; i < _numVerts; i++) {
_indices[i] = i;
}
}
static void EmitAreaRenderModel(area_t *a, FILE *fp)
{
EmitHeader("rmodel", fp);
EmitString(fp, "area%04i", a->areanumber);
BeginMesh();
for (areatri_t *t = a->trilist->head; t; t = t->next)
{
// fixme: function to create these
meshvertex_t v0, v1, v2;
v0.xyz = t->vertices[0];
v0.normal = t->normals[0];
v1.xyz = t->vertices[1];
v1.normal = t->normals[1];
v2.xyz = t->vertices[2];
v2.normal = t->normals[2];
InsertTri(v0, v1, v2);
}
EndMesh();
// emit the vertex block
int numvertices = NumVertices();
EmitInt(numvertices, fp);
for (int i = 0; i < numvertices; i++)
{
meshvertex_t v = GetVertex(i);
EmitFloat(v.xyz[0], fp);
EmitFloat(v.xyz[1], fp);
EmitFloat(v.xyz[2], fp);
EmitFloat(v.normal[0], fp);
EmitFloat(v.normal[1], fp);
EmitFloat(v.normal[2], fp);
}
// emit the index block
int numindicies = NumIndicies();
EmitInt(numindicies, fp);
for (int i = 0; i < numindicies; i++)
EmitInt(GetIndex(i), fp);
}
float3 Polygon::PointOnEdge(float normalizedDistance) const
{
if (p.empty())
return float3::nan;
if (p.size() < 2)
return p[0];
normalizedDistance = Frac(normalizedDistance); // Take modulo 1 so we have the range [0,1[.
float perimeter = Perimeter();
float d = normalizedDistance * perimeter;
for(int i = 0; i < NumVertices(); ++i)
{
LineSegment edge = Edge(i);
float len = edge.Length();
assume(len != 0.f && "Degenerate Polygon detected!");
if (d <= len)
return edge.GetPoint(d / len);
d -= len;
}
mathassert(false && "Polygon::PointOnEdge reached end of loop which shouldn't!");
return p[0];
}
/** The implementation of this function is based on the paper
"Kong, Everett, Toussant. The Graham Scan Triangulates Simple Polygons."
See also p. 772-775 of Geometric Tools for Computer Graphics.
The running time of this function is O(n^2). */
std::vector<Triangle> Polygon::Triangulate() const
{
assume(IsPlanar());
std::vector<Triangle> t;
// Handle degenerate cases.
if (NumVertices() < 3)
return t;
if (NumVertices() == 3)
{
t.push_back(Triangle(Vertex(0), Vertex(1), Vertex(2)));
return t;
}
std::vector<float2> p2d;
std::vector<int> polyIndices;
for(int v = 0; v < NumVertices(); ++v)
{
p2d.push_back(MapTo2D(v));
polyIndices.push_back(v);
}
// Clip ears of the polygon until it has been reduced to a triangle.
int i = 0;
int j = 1;
int k = 2;
size_t numTries = 0; // Avoid creating an infinite loop.
while(p2d.size() > 3 && numTries < p2d.size())
{
if (float2::OrientedCCW(p2d[i], p2d[j], p2d[k]) && IsAnEar(p2d, i, k))
{
// The vertex j is an ear. Clip it off.
t.push_back(Triangle(p[polyIndices[i]], p[polyIndices[j]], p[polyIndices[k]]));
p2d.erase(p2d.begin() + j);
polyIndices.erase(polyIndices.begin() + j);
// The previous index might now have become an ear. Move back one index to see if so.
if (i > 0)
{
i = (i + (int)p2d.size() - 1) % p2d.size();
j = (j + (int)p2d.size() - 1) % p2d.size();
k = (k + (int)p2d.size() - 1) % p2d.size();
}
numTries = 0;
}
else
{
// The vertex at j is not an ear. Move to test next vertex.
i = j;
j = k;
k = (k+1) % p2d.size();
++numTries;
}
}
assume(p2d.size() == 3);
if (p2d.size() > 3) // If this occurs, then the polygon is NOT counter-clockwise oriented.
return t;
/*
{
// For conveniency, create a copy that has the winding order fixed, and triangulate that instead.
// (Causes a large performance hit!)
Polygon p2 = *this;
for(size_t i = 0; i < p2.p.size()/2; ++i)
std::swap(p2.p[i], p2.p[p2.p.size()-1-i]);
return p2.Triangulate();
}
*/
// Add the last poly.
t.push_back(Triangle(p[polyIndices[0]], p[polyIndices[1]], p[polyIndices[2]]));
return t;
}
/* The main GJK distance routine. This routine implements the routine
* of Gilbert, Johnson and Keerthi, as described in the paper (GJK88)
* listed below. It also optionally runs my speed-up extension to this
* algorithm, as described in (Cam97).
*
* The first 4 parameters are two pairs of parameters, one pair for
* each hull; each pair is an object data-structure, plus a
* transformation data-structure. These data-structures are defined
* to this code in gjk.h, and are designed to be opaque to this code;
* the data is accessed through selectors, iterators and prediciates,
* which are discussed below.
*
* The 5th and 6th parameters are point arrays, that are set to the
* coordinates of two witness points (one within each convex hull)
* that realise the minimum distance between them.
*
* The actual return value for the function is the square of the
* minimum distance between the hulls, which is equal to the (square
* of the) distance between the witness points.
*
* The 7th parameter is a pointer to a simplex_point structure. If
* this is non-NULL then a special form of the witness points is
* stored in the structure by the routine, suitable for passing to
* this routine as seed points for any further calls involving these
* two objects. The 8th parameter is a flag, which when set tells the
* routine to use the given simplex_point structure instance as as
* seed, otherwise it just uses any seed. (If the 7th parameter is
* NULL then no harm is done.)
*
* Note that with this version one field of the simplex_point structure
* can be used to pass back the confidence region for the routine: when
* the routine returns with distance squared equal to D*d, it means that
* the true distance is known to lie between D-(E/D) and D, where E is
* the positive value returned in the `error' field of the simplex_point.
* Equivalently; the true value of the distance squared is less than or equal
* to the value returned DSQ, with an error bound width of 2E - E*E/DSQ.
* (In `normal' cases E is very small, and so the error bound width 2E can
* be sensibly used.)
*
* The code will attempt to return with E<=EPSILON, which the user
* can set, but will in any event return with some value of E. In particular,
* the code should return even with EPSILON set to zero.
*
* Alternatively, either or both of the pointer values for the witness
* points can be zero, in which case those witness points are not
* returned. The caller can later extract the coordinates of the
* witness points from the simplex_point structure by using the
* function gjk_extract_point.
*
* Returning to the opaque data-structures used to describe the objects
* and their transformations. For an object then the minimum information
* required is a list of vertices. The vertices themselves are another
* opaque type, accessed through the type VertexID. The following macros
* are defined for the vertices:
*
* InvalidVertexID a VertexID which cannot point to a valid vertex
* FirstVertex( obj) an arbitrary first vertex for the object
* NumVertices( obj) the number of vertices in the object
* IncrementVertex(o,v) set vertex to the next vertex
* ValidVertex( obj, v) says whether the VertexID is valid for obj
* LastVertex( obj, v) is this the last vertex?
* SupportValue(o,v,d) returns support value for v in direction d
*
* Optionally, the object data-structures encode the wireframe of the objects;
* this is used in my extended GJK algorithm to greatly speed up the routine
* in many cases. For an object the predicate ValidRing( obj) says whether
* this information is provided, in which case the edges that surround any
* can be accessed and traversed with the following:
*
* FirstEdge( obj, vertex) Returns the first edge (type EdgeID)
* IncrementEdge( obj, edge) Sets edge to the next edge
* ValidEdge( obj, edge) Indicates whether edge is a real edge
* VertexOfEdge( obj, edge) Returns the (other) vertex of an edge
*
* With this information this routine runs in expected constant time
* for tracking operations and small relative motions. If the
* information is not supplied the the routine reverts to using the
* original GJK technique, which takes time roughly linear in the number
* of vertices. (As a rough rule of thumb, this difference becomes
* measurable at around 10 vertices per hull, and important at about
* 20 vertices per hull.)
*
* Transformations are stored in data-structures given by opaque type
* Transform, for which the following operations need to be defined:
*
* IdentityTransform( t) Might t be an identity transformation?
* ExtractTranslation( t, v) Set v to the translation component of t
* ApplyTransform( t,o,v,tgt) Apply transform to vertex v of o, result in tgt
* ApplyInverseRotation(t,d,r) Apply inverse of t to direction d, result in r
*
* Notes:
* + it is OK for IdentityTransform( t) to return false when t is
* in fact the identity (with a small time penalty)
* + ExtractTranslation equivalently sets v to where the origin is
* transformed to by t
*
* References:
* (GJK88) "A Fast Procedure for Computing the Distance between Complex
* Objects in Three-Dimensional Space" by EG Gilbert, DW Johnson and SS
* Keerthi, IEEE Trans Robotics and Automation 4(2):193--203, April 1988.
*
* (Cam97) "A Comparison of Two Fast Algorithms for Computing the Distance
* between Convex Polyhedra" by Stephen Cameron, IEEE Trans Robotics and
* Automation 13(6):915-920, December 1997.
*
*/
REAL gjk_distance(
Object obj1, Transform tr1,
Object obj2, Transform tr2,
REAL wpt1[DIM], REAL wpt2[DIM],
struct simplex_point * simplex, int use_seed
) {
VertexID v, p, maxp, minp;
REAL minus_minv, maxv, sqrd, g_val;
REAL displacementv[DIM], reverse_displacementv[DIM];
REAL local_witness1[DIM], local_witness2[DIM];
REAL local_fdisp[DIM], local_rdisp[DIM], trv[DIM];
REAL * fdisp, * rdisp;
struct simplex_point local_simplex;
int d, compute_both_witnesses, use_default, first_iteration, max_iterations;
double oldsqrd;
assert( NumVertices( obj1)>0 && NumVertices( obj2)>0 );
use_default = first_iteration = 1;
#ifdef CONSTRUCT_TABLES
initialise_simplex_distance();
/* will return immediately if already initialised */
#else
assert( PRE_DEFINED_TABLE_DIM >= DIM );
#endif /* CONSTRUCT_TABLES */
compute_both_witnesses = ( wpt1!=0 ) || ( wpt2!=0 ) ||
( tr1!=0 ) || ( tr2!=0 );
if ( wpt1==0 )
wpt1 = local_witness1;
if ( wpt2==0 )
wpt2 = local_witness2;
fdisp = IdentityTransform( tr1) ? displacementv : local_fdisp;
rdisp = IdentityTransform( tr2) ? reverse_displacementv : local_rdisp;
if ( simplex==0 ) {
use_seed = 0;
simplex = & local_simplex;
}
if ( use_seed==0 ) {
simplex->simplex1[0] = 0; simplex->simplex2[0] = 0;
simplex->npts = 1; simplex->lambdas[0] = ONE;
simplex->last_best1 = 0; simplex->last_best2 = 0;
add_simplex_vertex( simplex, 0,
obj1, FirstVertex( obj1), tr1,
obj2, FirstVertex( obj2), tr2);
}
else {
/* If we are being told to use this seed point, there
is a good chance that the near point will be on
the current simplex. Besides, if we don't confirm
that the seed point given satisfies the invariant
(that the witness points given are the closest points
on the current simplex) things can and will fall down.
*/
for ( v=0 ; v<simplex->npts ; v++ )
add_simplex_vertex( simplex, v,
obj1, simplex->simplex1[v], tr1,
obj2, simplex->simplex2[v], tr2);
}
/* Now the main loop. We first compute the distance between the
current simplicies, the check whether this gives the globally
correct answer, and if not construct new simplices and try again.
*/
max_iterations = NumVertices( obj1)*NumVertices( obj2) ;
/* in practice we never see more than about 6 iterations. */
/* Counting the iterations in this way should not be necessary;
a while( 1) should do just as well. */
while ( max_iterations-- > 0 ) {
if ( simplex->npts==1 ) { /* simple case */
simplex->lambdas[0] = ONE;
}
else { /* normal case */
compute_subterms( simplex);
if ( use_default ) {
use_default = default_distance( simplex);
}
if ( !use_default ) {
backup_distance( simplex);
}
}
/* compute at least the displacement vectors given by the
simplex_point structure. If we are to provide both witness
points, it's slightly faster to compute those first.
*/
if ( compute_both_witnesses ) {
compute_point( wpt1, simplex->npts, simplex->coords1,
simplex->lambdas);
compute_point( wpt2, simplex->npts, simplex->coords2,
simplex->lambdas);
overd( d) {
displacementv[ d] = wpt2[d] - wpt1[d];
reverse_displacementv[ d] = - displacementv[d];
}
}
else {
overd( d) {
displacementv[d] = 0;
for ( p=0 ; p<simplex->npts ; p++ )
displacementv[d] +=
DO_MULTIPLY( simplex->lambdas[p],
simplex->coords2[p][d] - simplex->coords1[p][d]);
reverse_displacementv[ d] = - displacementv[d];
}
}
sqrd = OTHER_DOT_PRODUCT( displacementv, displacementv);
/* if we are using a c-space simplex with DIM_PLUS_ONE
points, this is interior to the simplex, and indicates
that the original hulls overlap, as does the distance
between them being too small. */
if ( sqrd<EPSILON ) {
simplex->error = EPSILON;
return sqrd;
}
if ( ! IdentityTransform( tr1) )
ApplyInverseRotation( tr1, displacementv, fdisp);
if ( ! IdentityTransform( tr2) )
ApplyInverseRotation( tr2, reverse_displacementv, rdisp);
/* find the point in obj1 that is maximal in the
direction displacement, and the point in obj2 that
is minimal in direction displacement;
*/
maxp = support_function(
obj1,
( use_seed ? simplex->last_best1 : InvalidVertexID),
&maxv, fdisp
);
minp = support_function(
obj2,
( use_seed ? simplex->last_best2 : InvalidVertexID),
&minus_minv, rdisp
);
/* Now apply the G-test on this pair of points */
INCREMENT_G_TEST_COUNTER;
g_val = sqrd + maxv + minus_minv;
if ( ! IdentityTransform( tr1) ) {
ExtractTranslation( tr1, trv);
g_val += OTHER_DOT_PRODUCT( displacementv, trv);
}
if ( ! IdentityTransform( tr2) ) {
ExtractTranslation( tr2, trv);
g_val += OTHER_DOT_PRODUCT( reverse_displacementv, trv);
}
if ( g_val < 0.0 ) /* not sure how, but it happens! */
g_val = 0;
if ( g_val < EPSILON ) {
/* then no better points - finish */
simplex->error = g_val;
return sqrd;
}
/* check for good calculation above */
if ( (first_iteration || (sqrd < oldsqrd))
&& (simplex->npts <= DIM ) ) {
/* Normal case: add the new c-space points to the current
simplex, and call simplex_distance() */
simplex->simplex1[ simplex->npts] = simplex->last_best1 = maxp;
simplex->simplex2[ simplex->npts] = simplex->last_best2 = minp;
simplex->lambdas[ simplex->npts] = ZERO;
add_simplex_vertex( simplex, simplex->npts,
obj1, maxp, tr1,
obj2, minp, tr2);
simplex->npts++;
oldsqrd = sqrd;
first_iteration = 0;
use_default = 1;
continue;
}
/* Abnormal cases! */
if ( use_default ) {
use_default = 0;
}
else { /* give up trying! */
simplex->error = g_val;
return sqrd;
}
} /* end of `while ( 1 )' */
bool SORE_Graphics::GraphicsArray::HasRoomFor(size_t numIndices, size_t numVertices) const
{
return !(NumIndices() + numIndices > std::numeric_limits<unsigned short>::max() ||
NumVertices() + numVertices > std::numeric_limits<unsigned short>::max());
}
void BasicMesh::Subdivide() {
// Loop subdivision
// NOTE The indices of the original set of vertices do not change after
// subdivision. The new vertices are simply added to the set of vertices.
// However, the faces change their indices after subdivision. See how new
// faces are added to the face set for details.
// In short, the new mesh is created as follows:
// [old vertices]
// [new vertices]
// [faces]
// For each edge, compute its center point
struct edge_t {
edge_t() {}
edge_t(int s, int t) : s(s), t(t) {}
edge_t(const edge_t& e) : s(e.s), t(e.t) {}
bool operator<(const edge_t& other) const {
if(s < other.s) return true;
else if( s > other.s ) return false;
else return t < other.t;
}
int s, t;
};
struct face_edge_t {
face_edge_t() {}
face_edge_t(int fidx, edge_t e) : fidx(fidx), e(e) {}
bool operator<(const face_edge_t& other) const {
if(fidx < other.fidx) return true;
else if(fidx > other.fidx) return false;
return e < other.e;
}
int fidx;
edge_t e;
};
const int num_faces = NumFaces();
map<edge_t, Vector3d> midpoints;
// iterate over all edges
for (HalfEdgeMesh::EdgeIter e=hemesh.edges_begin(); e!=hemesh.edges_end(); ++e) {
auto heh = hemesh.halfedge_handle(e, 0);
auto hefh = hemesh.halfedge_handle(e, 1);
auto v0h = hemesh.to_vertex_handle(heh);
auto v1h = hemesh.to_vertex_handle(hefh);
int v0idx = vhandles_map[v0h];
int v1idx = vhandles_map[v1h];
auto v0 = verts.row(v0idx);
auto v1 = verts.row(v1idx);
if(hemesh.is_boundary(*e)) {
// simply compute the mid point
midpoints.insert(make_pair(edge_t(v0idx, v1idx),
0.5 * (v0 + v1)));
} else {
// use [1/8, 3/8, 3/8, 1/8] weights
auto v2h = hemesh.to_vertex_handle(hemesh.next_halfedge_handle(heh));
auto v3h = hemesh.to_vertex_handle(hemesh.next_halfedge_handle(hefh));
int v2idx = vhandles_map[v2h];
int v3idx = vhandles_map[v3h];
auto v2 = verts.row(v2idx);
auto v3 = verts.row(v3idx);
midpoints.insert(make_pair(edge_t(v0idx, v1idx), (v0 * 3 + v1 * 3 + v2 + v3) / 8.0));
}
}
// Now create a new set of vertices and faces
const int num_verts = NumVertices() + midpoints.size();
MatrixX3d new_verts(num_verts, 3);
// Smooth these points
for(int i=0;i<NumVertices();++i) {
auto vh = vhandles[i];
if(hemesh.is_boundary(vh)) {
// use [1/8, 6/8, 1/8] weights
auto heh = hemesh.halfedge_handle(vh);
if(heh.is_valid()) {
assert(hemesh.is_boundary(hemesh.edge_handle(heh)));
auto prev_heh = hemesh.prev_halfedge_handle(heh);
auto to_vh = hemesh.to_vertex_handle(heh);
auto from_vh = hemesh.from_vertex_handle(prev_heh);
Vector3d p = 6 * verts.row(i);
p += verts.row(vhandles_map[to_vh]);
p += verts.row(vhandles_map[from_vh]);
p /= 8.0;
new_verts.row(i) = p;
}
} else {
// loop through the neighbors and count them
int valence = 0;
Vector3d p(0, 0, 0);
for(auto vvit = hemesh.vv_iter(vh); vvit.is_valid(); ++vvit) {
++valence;
p += verts.row(vhandles_map[*vvit]);
}
const double PI = 3.1415926535897;
const double wa = (0.375 + 0.25 * cos(2.0 * PI / valence));
const double w = (0.625 - wa * wa);
p *= (w / valence);
p += verts.row(i) * (1 - w);
new_verts.row(i) = p;
}
}
// Add the midpoints
map<edge_t, int> midpoints_indices;
int new_idx = NumVertices();
for(auto p : midpoints) {
midpoints_indices.insert(make_pair(p.first, new_idx));
midpoints_indices.insert(make_pair(edge_t(p.first.t, p.first.s), new_idx));
new_verts.row(new_idx) = p.second;
++new_idx;
}
// Process the texture coordinates
map<face_edge_t, Vector2d> midpoints_texcoords;
for(int fidx=0;fidx<NumFaces();++fidx){
int j[] = {1, 2, 0};
for(int i=0;i<3;++i) {
int v0idx = faces(fidx, i);
int v1idx = faces(fidx, j[i]);
// if v0 = f[index_of(v0)], the tv0 = tf[index_of(v0)]
int tv0idx = face_tex_index(fidx, i);
// if v1 = f[index_of(v1)], the tv1 = tf[index_of(v1)]
int tv1idx = face_tex_index(fidx, j[i]);
auto t0 = texcoords.row(tv0idx);
auto t1 = texcoords.row(tv1idx);
// the texture coordinates is always the mid point
midpoints_texcoords.insert(make_pair(face_edge_t(fidx, edge_t(v0idx, v1idx)),
0.5 * (t0 + t1)));
}
}
const int num_texcoords = texcoords.rows() + midpoints_texcoords.size();
MatrixX2d new_texcoords(num_texcoords, 2);
// Just copy the existing texture coordinates
new_texcoords.topRows(texcoords.rows()) = texcoords;
// Tex-coords for the mid points
map<face_edge_t, int> midpoints_texcoords_indices;
int new_texcoords_idx = texcoords.rows();
for(auto p : midpoints_texcoords) {
//cout << p.first.fidx << ": " << p.first.e.s << "->" << p.first.e.t << endl;
//getchar();
midpoints_texcoords_indices.insert(make_pair(p.first,
new_texcoords_idx));
midpoints_texcoords_indices.insert(make_pair(face_edge_t(p.first.fidx, edge_t(p.first.e.t, p.first.e.s)),
new_texcoords_idx));
new_texcoords.row(new_texcoords_idx) = p.second;
++new_texcoords_idx;
}
MatrixX3i new_faces(num_faces*4, 3);
MatrixX3i new_face_tex_index(num_faces*4, 3);
for(int i=0;i<num_faces;++i) {
// vertex indices of the original triangle
auto vidx0 = faces(i, 0);
auto vidx1 = faces(i, 1);
auto vidx2 = faces(i, 2);
// texture coordinates indices of the original triangle
auto tvidx0 = face_tex_index(i, 0);
auto tvidx1 = face_tex_index(i, 1);
auto tvidx2 = face_tex_index(i, 2);
// indices of the mid points
int nvidx01 = midpoints_indices[edge_t(vidx0, vidx1)];
int nvidx12 = midpoints_indices[edge_t(vidx1, vidx2)];
int nvidx20 = midpoints_indices[edge_t(vidx2, vidx0)];
// indices of the texture coordinates of the mid points
int tnvidx01 = midpoints_texcoords_indices.at(face_edge_t(i, edge_t(vidx0, vidx1)));
int tnvidx12 = midpoints_texcoords_indices.at(face_edge_t(i, edge_t(vidx1, vidx2)));
int tnvidx20 = midpoints_texcoords_indices.at(face_edge_t(i, edge_t(vidx2, vidx0)));
// add the 4 new faces
new_faces.row(i*4+0) = Vector3i(vidx0, nvidx01, nvidx20);
new_faces.row(i*4+1) = Vector3i(nvidx20, nvidx01, nvidx12);
new_faces.row(i*4+2) = Vector3i(nvidx20, nvidx12, vidx2);
new_faces.row(i*4+3) = Vector3i(nvidx01, vidx1, nvidx12);
new_face_tex_index.row(i*4+0) = Vector3i(tvidx0, tnvidx01, tnvidx20);
new_face_tex_index.row(i*4+1) = Vector3i(tnvidx20, tnvidx01, tnvidx12);
new_face_tex_index.row(i*4+2) = Vector3i(tnvidx20, tnvidx12, tvidx2);
new_face_tex_index.row(i*4+3) = Vector3i(tnvidx01, tvidx1, tnvidx12);
}
verts = new_verts;
faces = new_faces;
texcoords = new_texcoords;
face_tex_index = new_face_tex_index;
// Update the normals after subdivision
ComputeNormals();
}
void TriSoup::ReindexTriangles(int lruSize, std::vector<int>& outFaceOrder)
{
////////////////////////////////////////
// Init vert and face arrays.
const int numVerts = NumVertices();
const int numTris = NumFaces();
std::vector<CacheSort::Vertex> verts(numVerts);
std::vector<CacheSort::Face> faces(numTris);
for(int i = 0, c = NumFaces(); i < c; ++i)
{
faces[i].m_indices[0] = m_faces[i].m_vertices[0];
faces[i].m_indices[1] = m_faces[i].m_vertices[1];
faces[i].m_indices[2] = m_faces[i].m_vertices[2];
}
CacheSort::InitValence(verts, faces);
outFaceOrder = std::vector<int>(numTris, -1);
////////////////////////////////////////
// Finished init. now start the main loop
CacheSort::VertexLRU lru(lruSize+3); // 3 extra to hold up to 3 verts which get pushed off.
const int *cache = lru.GetVertexCache();
int cacheSize = lru.GetCacheSize();
int idxDrawList = 0;
int curFace = CacheSort::FindBestFaceGlobal(verts, faces);
while(curFace >= 0)
{
ASSERT( idxDrawList < numTris );
// Add this face to the draw list.
outFaceOrder[idxDrawList++] = curFace;
lru.AddFace(&faces[curFace], verts);
// decrement current valency so we don't keep computing scores for already drawn faces
CacheSort::Face::RemoveConnections(curFace, verts, faces);
// Recompute scores of all verts in the LRU
for(int i = 0; i < cacheSize && cache[i] >= 0; ++i)
{
int idxVert = cache[i];
verts[idxVert].ComputeScore(cacheSize);
}
int bestFace = -1;
float bestFaceScore = 0.f;
// Recompute scores of all faces whose scores would change due to changes in the LRU
for(int i = 0; i < cacheSize && cache[i] >= 0; ++i)
{
int idxVert = cache[i];
for(int j = 0; j < verts[idxVert].m_valency; ++j)
{
int idxFace = verts[idxVert].m_clientFaces[j];
float faceScore =
verts[ faces[idxFace].m_indices[0] ].m_score +
verts[ faces[idxFace].m_indices[1] ].m_score +
verts[ faces[idxFace].m_indices[2] ].m_score ;
if (faceScore > bestFaceScore) {
bestFace = idxFace;
bestFaceScore = faceScore;
}
}
}
// Truncate the extra 3 verts. This just means if the next face includes one of the removed verts, it will be added instead of
// swapped, which will push more verts off. Also it means those 3 verts will NOT be included in scoring next iteration.
lru.Truncate(lruSize, verts);
// If for some reason the valency information doesn't link adjacent faces, then just pick the best global face again.
if( bestFace < 0 )
curFace = CacheSort::FindBestFaceGlobal(verts, faces);
else
curFace = bestFace;
}
}
bool Polyhedron::EulerFormulaHolds() const
{
return NumVertices() + NumFaces() - NumEdges() == 2;
}
