void decompose(graph *g, int *decomp, int* dus, int maxVal, int maxId) {
int* cluster;
int* nextcluster;
int iter = 0;
int total_num = g -> num_nodes;
cluster = (int*) malloc(sizeof(int*) * total_num);
nextcluster = (int*) malloc(sizeof(int*) * total_num);
memset(cluster, NA, sizeof(int) * total_num);
memset(nextcluster, NA, sizeof(int) * total_num);
// int* dus = getDus(total_num, decomp, &maxVal, &maxId); // rate = 1/beta beta哪里来
VertexSet* frontier = newVertexSet(SPARSE, 1, total_num);
addVertex(frontier, maxId);
VertexSet *newFrontier;
int max_dus = 0;
for (int i = 0; i < total_num; i++) {
max_dus = max_dus >= dus[i] ? max_dus : dus[i];
}
while (frontier->size > 0) {
iter += 1;
Decomosition dec(cluster, nextcluster);
newFrontier = edgeMap(g, frontier, dec, NORETURN);
freeVertexSet(frontier);
frontier = newFrontier;
// start growing all balls i at the next iter with
// // uncluster center i and with maxDu - dus[i] < iter
// foreach vertex i in not_cluster {
// if (iter > du_max - dus[i]) {
// frontier->add(i);
// }
// }
UpdateFrontier uf(max_dus, iter, dus);
vertexMap(frontier, uf, YESRETURN);
ClusterCopy vc(cluster, nextcluster);
vertexMap(frontier, vc, NORETURN);
}
freeVertexSet(frontier);
free(cluster);
free(nextcluster);
}
void Compute(graph<vertex>& GA, commandLine P) {
long n = GA.n;
intE* radii = newA(intE,n);
long* Visited = newA(long,n), *NextVisited = newA(long,n);
{parallel_for(long i=0;i<n;i++) {
radii[i] = -1;
Visited[i] = NextVisited[i] = 0;
}}
long sampleSize = min(n,(long)64);
uintE* starts = newA(uintE,sampleSize);
{parallel_for(ulong i=0;i<sampleSize;i++) { //initial set of vertices
uintE v = hashInt(i) % n;
radii[v] = 0;
starts[i] = v;
NextVisited[v] = (long) 1<<i;
}}
vertexSubset Frontier(n,sampleSize,starts); //initial frontier of size 64
intE round = 0;
while(!Frontier.isEmpty()){
round++;
vertexMap(Frontier, Radii_Vertex_F(Visited,NextVisited));
vertexSubset output = edgeMap(GA, Frontier, Radii_F(Visited,NextVisited,radii,round),GA.m/20);
Frontier.del();
Frontier = output;
}
free(Visited); free(NextVisited); Frontier.del(); free(radii);
}
void Compute(graph<vertex>& GA, commandLine P) {
long ratio = P.getOptionLongValue("-r",1000);
long n = GA.n;
printf("Nodes: %d Ratio %d\n", n, ratio);
for(size_t i(0); i < n; ++i) {
if(i % ratio != 0)
continue;
intE* ShortestPathLen = newA(intE,n);
int* Visited = newA(int,n);
//initialize ShortestPathLen to "infinity"
{parallel_for(long i=0;i<n;i++) ShortestPathLen[i] = INT_MAX/2;}
ShortestPathLen[i] = 0;
{parallel_for(long i=0;i<n;i++) Visited[i] = 0;}
vertexSubset Frontier(n,i); //initial frontier
long round = 0;
while(!Frontier.isEmpty()){
if(round == n) {
//negative weight cycle
{parallel_for(long i=0;i<n;i++) ShortestPathLen[i] = -(INT_E_MAX/2);}
break;
}
vertexSubset output = edgeMap(GA, Frontier, BF_F(ShortestPathLen,Visited), GA.m/20, DENSE_FORWARD);
vertexMap(output,BF_Vertex_F(Visited));
Frontier.del();
Frontier = output;
round++;
}
void Foam::CV2D::writeTriangles(const fileName& fName, bool internalOnly) const
{
Info<< "Writing triangles to " << fName << nl << endl;
OFstream str(fName);
labelList vertexMap(number_of_vertices(), -2);
label verti = 0;
for
(
Triangulation::Finite_vertices_iterator vit = finite_vertices_begin();
vit != finite_vertices_end();
++vit
)
{
if (!internalOnly || !vit->farPoint())
{
vertexMap[vit->index()] = verti++;
meshTools::writeOBJ(str, toPoint3D(vit->point()));
}
}
for
(
Triangulation::Finite_faces_iterator fit = finite_faces_begin();
fit != finite_faces_end();
++fit
)
{
if
(
!internalOnly
|| (
fit->vertex(0)->internalOrBoundaryPoint()
|| fit->vertex(1)->internalOrBoundaryPoint()
|| fit->vertex(2)->internalOrBoundaryPoint()
)
)
{
str << "f";
for (label i = 0; i < 3; ++i)
{
str << " " << vertexMap[fit->vertex(i)->index()] + 1;
}
str << nl;
}
}
}
void Compute(graph<vertex>& GA, commandLine P) {
long n = GA.n;
uintE* IDs = newA(uintE,n), *prevIDs = newA(uintE,n);
{parallel_for(long i=0;i<n;i++) IDs[i] = i;} //initialize unique IDs
bool* frontier = newA(bool,n);
{parallel_for(long i=0;i<n;i++) frontier[i] = 1;}
vertexSubset Frontier(n,n,frontier); //initial frontier contains all vertices
while(!Frontier.isEmpty()){ //iterate until IDS converge
vertexMap(Frontier,CC_Vertex_F(IDs,prevIDs));
vertexSubset output = edgeMap(GA, Frontier, CC_F(IDs,prevIDs),GA.m/20);
Frontier.del();
Frontier = output;
}
Frontier.del(); free(IDs); free(prevIDs);
}
void TriMesh::RemoveUnusedVerts()
{
vector<int> vertexMap(verts.size(),-1);
vector<Vector3> newVerts(verts.size());
int numVerts=0;
for(size_t i=0;i<tris.size();i++) {
for(int k=0;k<3;k++) {
int v=tris[i][k];
if(vertexMap[v]==-1) {
vertexMap[v]=numVerts;
newVerts[numVerts]=verts[v];
numVerts++;
}
tris[i][k]=vertexMap[v];
}
}
newVerts.resize(numVerts);
swap(verts,newVerts);
}
bool dgCollisionConvexHull::Create (dgInt32 count, dgInt32 strideInBytes, const dgFloat32* const vertexArray, dgFloat32 tolerance)
{
dgInt32 stride = strideInBytes / sizeof (dgFloat32);
dgStack<dgFloat64> buffer(3 * 2 * count);
for (dgInt32 i = 0; i < count; i ++) {
buffer[i * 3 + 0] = vertexArray[i * stride + 0];
buffer[i * 3 + 1] = vertexArray[i * stride + 1];
buffer[i * 3 + 2] = vertexArray[i * stride + 2];
}
dgConvexHull3d* convexHull = new (GetAllocator()) dgConvexHull3d (GetAllocator(), &buffer[0], 3 * sizeof (dgFloat64), count, tolerance);
if (!convexHull->GetCount()) {
// this is a degenerated hull hull to add some thickness and for a thick plane
delete convexHull;
dgStack<dgVector> tmp(3 * count);
for (dgInt32 i = 0; i < count; i ++) {
tmp[i][0] = dgFloat32 (buffer[i*3 + 0]);
tmp[i][1] = dgFloat32 (buffer[i*3 + 1]);
tmp[i][2] = dgFloat32 (buffer[i*3 + 2]);
tmp[i][2] = dgFloat32 (0.0f);
}
dgObb sphere;
sphere.SetDimensions (&tmp[0][0], sizeof (dgVector), count);
dgInt32 index = 0;
dgFloat32 size = dgFloat32 (1.0e10f);
for (dgInt32 i = 0; i < 3; i ++) {
if (sphere.m_size[i] < size) {
index = i;
size = sphere.m_size[i];
}
}
dgVector normal (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
normal[index] = dgFloat32 (1.0f);
dgVector step = sphere.RotateVector (normal.Scale3 (dgFloat32 (0.05f)));
for (dgInt32 i = 0; i < count; i ++) {
dgVector p1 (tmp[i] + step);
dgVector p2 (tmp[i] - step);
buffer[i * 3 + 0] = p1.m_x;
buffer[i * 3 + 1] = p1.m_y;
buffer[i * 3 + 2] = p1.m_z;
buffer[(i + count) * 3 + 0] = p2.m_x;
buffer[(i + count) * 3 + 1] = p2.m_y;
buffer[(i + count) * 3 + 2] = p2.m_z;
}
count *= 2;
convexHull = new (GetAllocator()) dgConvexHull3d (GetAllocator(), &buffer[0], 3 * sizeof (dgFloat64), count, tolerance);
if (!convexHull->GetCount()) {
delete convexHull;
return false;
}
}
// check for degenerated faces
for (bool success = false; !success; ) {
success = true;
const dgBigVector* const hullVertexArray = convexHull->GetVertexPool();
dgStack<dgInt8> mask(convexHull->GetVertexCount());
memset (&mask[0], 1, mask.GetSizeInBytes());
for (dgConvexHull3d::dgListNode* node = convexHull->GetFirst(); node; node = node->GetNext()) {
dgConvexHull3DFace& face = node->GetInfo();
const dgBigVector& p0 = hullVertexArray[face.m_index[0]];
const dgBigVector& p1 = hullVertexArray[face.m_index[1]];
const dgBigVector& p2 = hullVertexArray[face.m_index[2]];
dgBigVector p1p0 (p1 - p0);
dgBigVector p2p0 (p2 - p0);
dgBigVector normal (p2p0 * p1p0);
dgFloat64 mag2 = normal % normal;
if (mag2 < dgFloat64 (1.0e-6f * 1.0e-6f)) {
success = false;
dgInt32 index = -1;
dgBigVector p2p1 (p2 - p1);
dgFloat64 dist10 = p1p0 % p1p0;
dgFloat64 dist20 = p2p0 % p2p0;
dgFloat64 dist21 = p2p1 % p2p1;
if ((dist10 >= dist20) && (dist10 >= dist21)) {
index = 2;
} else if ((dist20 >= dist10) && (dist20 >= dist21)) {
index = 1;
} else if ((dist21 >= dist10) && (dist21 >= dist20)) {
index = 0;
}
dgAssert (index != -1);
mask[face.m_index[index]] = 0;
}
}
if (!success) {
dgInt32 count = 0;
dgInt32 vertexCount = convexHull->GetVertexCount();
for (dgInt32 i = 0; i < vertexCount; i ++) {
if (mask[i]) {
buffer[count * 3 + 0] = hullVertexArray[i].m_x;
buffer[count * 3 + 1] = hullVertexArray[i].m_y;
buffer[count * 3 + 2] = hullVertexArray[i].m_z;
count ++;
}
}
delete convexHull;
convexHull = new (GetAllocator()) dgConvexHull3d (GetAllocator(), &buffer[0], 3 * sizeof (dgFloat64), count, tolerance);
}
}
dgAssert (convexHull);
dgInt32 vertexCount = convexHull->GetVertexCount();
if (vertexCount < 4) {
delete convexHull;
return false;
}
const dgBigVector* const hullVertexArray = convexHull->GetVertexPool();
dgPolyhedra polyhedra (GetAllocator());
polyhedra.BeginFace();
for (dgConvexHull3d::dgListNode* node = convexHull->GetFirst(); node; node = node->GetNext()) {
dgConvexHull3DFace& face = node->GetInfo();
polyhedra.AddFace (face.m_index[0], face.m_index[1], face.m_index[2]);
}
polyhedra.EndFace();
if (vertexCount > 4) {
// bool edgeRemoved = false;
// while (RemoveCoplanarEdge (polyhedra, hullVertexArray)) {
// edgeRemoved = true;
// }
// if (edgeRemoved) {
// if (!CheckConvex (polyhedra, hullVertexArray)) {
// delete convexHull;
// return false;
// }
// }
while (RemoveCoplanarEdge (polyhedra, hullVertexArray));
}
dgStack<dgInt32> vertexMap(vertexCount);
memset (&vertexMap[0], -1, vertexCount * sizeof (dgInt32));
dgInt32 mark = polyhedra.IncLRU();
dgPolyhedra::Iterator iter (polyhedra);
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &iter.GetNode()->GetInfo();
if (edge->m_mark != mark) {
if (vertexMap[edge->m_incidentVertex] == -1) {
vertexMap[edge->m_incidentVertex] = m_vertexCount;
m_vertexCount ++;
}
dgEdge* ptr = edge;
do {
ptr->m_mark = mark;
ptr->m_userData = m_edgeCount;
m_edgeCount ++;
ptr = ptr->m_twin->m_next;
} while (ptr != edge) ;
}
}
m_vertex = (dgVector*) m_allocator->Malloc (dgInt32 (m_vertexCount * sizeof (dgVector)));
m_simplex = (dgConvexSimplexEdge*) m_allocator->Malloc (dgInt32 (m_edgeCount * sizeof (dgConvexSimplexEdge)));
m_vertexToEdgeMapping = (const dgConvexSimplexEdge**) m_allocator->Malloc (dgInt32 (m_vertexCount * sizeof (dgConvexSimplexEdge*)));
for (dgInt32 i = 0; i < vertexCount; i ++) {
if (vertexMap[i] != -1) {
m_vertex[vertexMap[i]] = hullVertexArray[i];
m_vertex[vertexMap[i]].m_w = dgFloat32 (0.0f);
}
}
delete convexHull;
vertexCount = m_vertexCount;
mark = polyhedra.IncLRU();;
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &iter.GetNode()->GetInfo();
if (edge->m_mark != mark) {
dgEdge *ptr = edge;
do {
ptr->m_mark = mark;
dgConvexSimplexEdge* const simplexPtr = &m_simplex[ptr->m_userData];
simplexPtr->m_vertex = vertexMap[ptr->m_incidentVertex];
simplexPtr->m_next = &m_simplex[ptr->m_next->m_userData];
simplexPtr->m_prev = &m_simplex[ptr->m_prev->m_userData];
simplexPtr->m_twin = &m_simplex[ptr->m_twin->m_userData];
ptr = ptr->m_twin->m_next;
} while (ptr != edge) ;
}
}
m_faceCount = 0;
dgStack<char> faceMarks (m_edgeCount);
memset (&faceMarks[0], 0, m_edgeCount * sizeof (dgInt8));
dgStack<dgConvexSimplexEdge*> faceArray (m_edgeCount);
for (dgInt32 i = 0; i < m_edgeCount; i ++) {
dgConvexSimplexEdge* const face = &m_simplex[i];
if (!faceMarks[i]) {
dgConvexSimplexEdge* ptr = face;
do {
dgAssert ((ptr - m_simplex) >= 0);
faceMarks[dgInt32 (ptr - m_simplex)] = '1';
ptr = ptr->m_next;
} while (ptr != face);
faceArray[m_faceCount] = face;
m_faceCount ++;
}
}
m_faceArray = (dgConvexSimplexEdge **) m_allocator->Malloc(dgInt32 (m_faceCount * sizeof(dgConvexSimplexEdge *)));
memcpy (m_faceArray, &faceArray[0], m_faceCount * sizeof(dgConvexSimplexEdge *));
if (vertexCount > DG_CONVEX_VERTEX_CHUNK_SIZE) {
// create a face structure for support vertex
dgStack<dgConvexBox> boxTree (vertexCount);
dgTree<dgVector,dgInt32> sortTree(GetAllocator());
dgStack<dgTree<dgVector,dgInt32>::dgTreeNode*> vertexNodeList(vertexCount);
dgVector minP ( dgFloat32 (1.0e15f), dgFloat32 (1.0e15f), dgFloat32 (1.0e15f), dgFloat32 (0.0f));
dgVector maxP (-dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), dgFloat32 (0.0f));
for (dgInt32 i = 0; i < vertexCount; i ++) {
const dgVector& p = m_vertex[i];
vertexNodeList[i] = sortTree.Insert (p, i);
minP.m_x = dgMin (p.m_x, minP.m_x);
minP.m_y = dgMin (p.m_y, minP.m_y);
minP.m_z = dgMin (p.m_z, minP.m_z);
maxP.m_x = dgMax (p.m_x, maxP.m_x);
maxP.m_y = dgMax (p.m_y, maxP.m_y);
maxP.m_z = dgMax (p.m_z, maxP.m_z);
}
boxTree[0].m_box[0] = minP;
boxTree[0].m_box[1] = maxP;
boxTree[0].m_leftBox = -1;
boxTree[0].m_rightBox = -1;
boxTree[0].m_vertexStart = 0;
boxTree[0].m_vertexCount = vertexCount;
dgInt32 boxCount = 1;
dgInt32 stack = 1;
dgInt32 stackBoxPool[64];
stackBoxPool[0] = 0;
while (stack) {
stack --;
dgInt32 boxIndex = stackBoxPool[stack];
dgConvexBox& box = boxTree[boxIndex];
if (box.m_vertexCount > DG_CONVEX_VERTEX_CHUNK_SIZE) {
dgVector median (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector varian (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
for (dgInt32 i = 0; i < box.m_vertexCount; i ++) {
dgVector& p = vertexNodeList[box.m_vertexStart + i]->GetInfo();
minP.m_x = dgMin (p.m_x, minP.m_x);
minP.m_y = dgMin (p.m_y, minP.m_y);
minP.m_z = dgMin (p.m_z, minP.m_z);
maxP.m_x = dgMax (p.m_x, maxP.m_x);
maxP.m_y = dgMax (p.m_y, maxP.m_y);
maxP.m_z = dgMax (p.m_z, maxP.m_z);
median += p;
varian += p.CompProduct3 (p);
}
varian = varian.Scale3 (dgFloat32 (box.m_vertexCount)) - median.CompProduct3(median);
dgInt32 index = 0;
dgFloat64 maxVarian = dgFloat64 (-1.0e10f);
for (dgInt32 i = 0; i < 3; i ++) {
if (varian[i] > maxVarian) {
index = i;
maxVarian = varian[i];
}
}
dgVector center = median.Scale3 (dgFloat32 (1.0f) / dgFloat32 (box.m_vertexCount));
dgFloat32 test = center[index];
dgInt32 i0 = 0;
dgInt32 i1 = box.m_vertexCount - 1;
do {
for (; i0 <= i1; i0 ++) {
dgFloat32 val = vertexNodeList[box.m_vertexStart + i0]->GetInfo()[index];
if (val > test) {
break;
}
}
for (; i1 >= i0; i1 --) {
dgFloat32 val = vertexNodeList[box.m_vertexStart + i1]->GetInfo()[index];
if (val < test) {
break;
}
}
if (i0 < i1) {
dgSwap(vertexNodeList[box.m_vertexStart + i0], vertexNodeList[box.m_vertexStart + i1]);
i0++;
i1--;
}
} while (i0 <= i1);
if (i0 == 0){
i0 = box.m_vertexCount / 2;
}
if (i0 >= (box.m_vertexCount - 1)){
i0 = box.m_vertexCount / 2;
}
{
dgVector minP ( dgFloat32 (1.0e15f), dgFloat32 (1.0e15f), dgFloat32 (1.0e15f), dgFloat32 (0.0f));
dgVector maxP (-dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), dgFloat32 (0.0f));
for (dgInt32 i = i0; i < box.m_vertexCount; i ++) {
const dgVector& p = vertexNodeList[box.m_vertexStart + i]->GetInfo();
minP.m_x = dgMin (p.m_x, minP.m_x);
minP.m_y = dgMin (p.m_y, minP.m_y);
minP.m_z = dgMin (p.m_z, minP.m_z);
maxP.m_x = dgMax (p.m_x, maxP.m_x);
maxP.m_y = dgMax (p.m_y, maxP.m_y);
maxP.m_z = dgMax (p.m_z, maxP.m_z);
}
box.m_rightBox = boxCount;
boxTree[boxCount].m_box[0] = minP;
boxTree[boxCount].m_box[1] = maxP;
boxTree[boxCount].m_leftBox = -1;
boxTree[boxCount].m_rightBox = -1;
boxTree[boxCount].m_vertexStart = box.m_vertexStart + i0;
boxTree[boxCount].m_vertexCount = box.m_vertexCount - i0;
stackBoxPool[stack] = boxCount;
stack ++;
boxCount ++;
}
{
dgVector minP ( dgFloat32 (1.0e15f), dgFloat32 (1.0e15f), dgFloat32 (1.0e15f), dgFloat32 (0.0f));
dgVector maxP (-dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), dgFloat32 (0.0f));
for (dgInt32 i = 0; i < i0; i ++) {
const dgVector& p = vertexNodeList[box.m_vertexStart + i]->GetInfo();
minP.m_x = dgMin (p.m_x, minP.m_x);
minP.m_y = dgMin (p.m_y, minP.m_y);
minP.m_z = dgMin (p.m_z, minP.m_z);
maxP.m_x = dgMax (p.m_x, maxP.m_x);
maxP.m_y = dgMax (p.m_y, maxP.m_y);
maxP.m_z = dgMax (p.m_z, maxP.m_z);
}
box.m_leftBox = boxCount;
boxTree[boxCount].m_box[0] = minP;
boxTree[boxCount].m_box[1] = maxP;
boxTree[boxCount].m_leftBox = -1;
boxTree[boxCount].m_rightBox = -1;
boxTree[boxCount].m_vertexStart = box.m_vertexStart;
boxTree[boxCount].m_vertexCount = i0;
stackBoxPool[stack] = boxCount;
stack ++;
boxCount ++;
}
}
}
for (dgInt32 i = 0; i < m_vertexCount; i ++) {
m_vertex[i] = vertexNodeList[i]->GetInfo();
vertexNodeList[i]->GetInfo().m_w = dgFloat32 (i);
}
m_supportTreeCount = boxCount;
m_supportTree = (dgConvexBox*) m_allocator->Malloc(dgInt32 (boxCount * sizeof(dgConvexBox)));
memcpy (m_supportTree, &boxTree[0], boxCount * sizeof(dgConvexBox));
for (dgInt32 i = 0; i < m_edgeCount; i ++) {
dgConvexSimplexEdge* const ptr = &m_simplex[i];
dgTree<dgVector,dgInt32>::dgTreeNode* const node = sortTree.Find(ptr->m_vertex);
dgInt32 index = dgInt32 (node->GetInfo().m_w);
ptr->m_vertex = dgInt16 (index);
}
}
for (dgInt32 i = 0; i < m_edgeCount; i ++) {
dgConvexSimplexEdge* const edge = &m_simplex[i];
m_vertexToEdgeMapping[edge->m_vertex] = edge;
}
SetVolumeAndCG ();
return true;
}
bool dgCollisionConvexHull::Create (dgInt32 count, dgInt32 strideInBytes, const dgFloat32* const vertexArray, dgFloat32 tolerance)
{
dgInt32 stride = strideInBytes / sizeof (dgFloat32);
dgStack<dgFloat64> buffer(3 * count);
for (dgInt32 i = 0; i < count; i ++) {
buffer[i * 3 + 0] = vertexArray[i * stride + 0];
buffer[i * 3 + 1] = vertexArray[i * stride + 1];
buffer[i * 3 + 2] = vertexArray[i * stride + 2];
}
dgConvexHull3d* convexHull = new (GetAllocator()) dgConvexHull3d (GetAllocator(), &buffer[0], 3 * sizeof (dgFloat64), count, tolerance);
if (!convexHull->GetCount()) {
delete convexHull;
return false;
}
// check for degenerated faces
for (bool success = false; !success; ) {
success = true;
const dgBigVector* const hullVertexArray = convexHull->GetVertexPool();
dgStack<dgInt8> mask(convexHull->GetVertexCount());
memset (&mask[0], 1, mask.GetSizeInBytes());
for (dgConvexHull3d::dgListNode* node = convexHull->GetFirst(); node; node = node->GetNext()) {
dgConvexHull3DFace& face = node->GetInfo();
const dgBigVector& p0 = hullVertexArray[face.m_index[0]];
const dgBigVector& p1 = hullVertexArray[face.m_index[1]];
const dgBigVector& p2 = hullVertexArray[face.m_index[2]];
dgBigVector p1p0 (p1 - p0);
dgBigVector p2p0 (p2 - p0);
dgBigVector normal (p2p0 * p1p0);
dgFloat64 mag2 = normal % normal;
if (mag2 < dgFloat64 (1.0e-6f * 1.0e-6f)) {
success = false;
dgInt32 index = -1;
dgBigVector p2p1 (p2 - p1);
dgFloat64 dist10 = p1p0 % p1p0;
dgFloat64 dist20 = p2p0 % p2p0;
dgFloat64 dist21 = p2p1 % p2p1;
if ((dist10 >= dist20) && (dist10 >= dist21)) {
index = 2;
} else if ((dist20 >= dist10) && (dist20 >= dist21)) {
index = 1;
} else if ((dist21 >= dist10) && (dist21 >= dist20)) {
index = 0;
}
_ASSERTE (index != -1);
mask[face.m_index[index]] = 0;
}
}
if (!success) {
dgInt32 count = 0;
dgInt32 vertexCount = convexHull->GetVertexCount();
for (dgInt32 i = 0; i < vertexCount; i ++) {
if (mask[i]) {
buffer[count * 3 + 0] = hullVertexArray[i].m_x;
buffer[count * 3 + 1] = hullVertexArray[i].m_y;
buffer[count * 3 + 2] = hullVertexArray[i].m_z;
count ++;
}
}
delete convexHull;
convexHull = new (GetAllocator()) dgConvexHull3d (GetAllocator(), &buffer[0], 3 * sizeof (dgFloat64), count, tolerance);
}
}
dgInt32 vertexCount = convexHull->GetVertexCount();
const dgBigVector* const hullVertexArray = convexHull->GetVertexPool();
dgPolyhedra polyhedra (GetAllocator());
polyhedra.BeginFace();
for (dgConvexHull3d::dgListNode* node = convexHull->GetFirst(); node; node = node->GetNext()) {
dgConvexHull3DFace& face = node->GetInfo();
polyhedra.AddFace (face.m_index[0], face.m_index[1], face.m_index[2]);
}
polyhedra.EndFace();
if (vertexCount > 4) {
bool edgeRemoved = false;
while (RemoveCoplanarEdge (polyhedra, hullVertexArray)) {
edgeRemoved = true;
}
if (edgeRemoved) {
if (!CheckConvex (polyhedra, hullVertexArray)) {
return false;
}
}
}
dgInt32 maxEdgeCount = polyhedra.GetCount();
dgStack<dgEdge*> stack(1024 + maxEdgeCount);
dgEdge* firstFace = &polyhedra.GetRoot()->GetInfo();
_ASSERTE (firstFace->m_twin->m_next != firstFace);
dgInt32 stackIndex = 1;
stack[0] = firstFace;
dgStack<dgInt32> vertexMap(vertexCount);
memset (&vertexMap[0], -1, vertexCount * sizeof (dgInt32));
// m_edgeCount = 0;
// m_vertexCount = 0;
dgInt32 i1 = polyhedra.IncLRU();
while (stackIndex) {
stackIndex --;
dgEdge* const edge0 = stack[stackIndex];
if (edge0->m_mark != i1) {
if (vertexMap[edge0->m_incidentVertex] == -1) {
vertexMap[edge0->m_incidentVertex] = m_vertexCount;
m_vertexCount ++;
}
dgEdge* ptr = edge0;
do {
stack[stackIndex] = ptr->m_twin;
stackIndex++;
ptr->m_mark = i1;
ptr->m_userData = m_edgeCount;
m_edgeCount ++;
ptr = ptr->m_twin->m_next;
} while (ptr != edge0) ;
}
}
m_vertex = (dgVector*) m_allocator->Malloc (dgInt32 (m_vertexCount * sizeof (dgVector)));
m_simplex = (dgConvexSimplexEdge*) m_allocator->Malloc (dgInt32 (m_edgeCount * sizeof (dgConvexSimplexEdge)));
for (dgInt32 i = 0; i < vertexCount; i ++) {
if (vertexMap[i] != -1) {
m_vertex[vertexMap[i]] = hullVertexArray[i];
m_vertex[vertexMap[i]].m_w = dgFloat32 (1.0f);
}
}
i1 = polyhedra.IncLRU();
stackIndex = 1;
stack[0] = firstFace;
while (stackIndex) {
stackIndex --;
dgEdge* const edge0 = stack[stackIndex];
if (edge0->m_mark != i1) {
dgEdge *ptr = edge0;
do {
ptr->m_mark = i1;
stack[stackIndex] = ptr->m_twin;
stackIndex++;
dgConvexSimplexEdge* const simplexPtr = &m_simplex[ptr->m_userData];
simplexPtr->m_vertex = vertexMap[ptr->m_incidentVertex];
simplexPtr->m_next = &m_simplex[ptr->m_next->m_userData];
simplexPtr->m_prev = &m_simplex[ptr->m_prev->m_userData];
simplexPtr->m_twin = &m_simplex[ptr->m_twin->m_userData];
ptr = ptr->m_twin->m_next;
} while (ptr != edge0) ;
}
}
SetVolumeAndCG ();
m_faceCount = 0;
dgStack<char> mark (m_edgeCount);
memset (&mark[0], 0, m_edgeCount * sizeof (dgInt8));
dgStack<dgConvexSimplexEdge*> faceArray (m_edgeCount);
for (dgInt32 i = 0; i < m_edgeCount; i ++) {
dgConvexSimplexEdge* const face = &m_simplex[i];
if (!mark[i]) {
dgConvexSimplexEdge* ptr = face;
do {
_ASSERTE ((ptr - m_simplex) >= 0);
mark[dgInt32 (ptr - m_simplex)] = '1';
ptr = ptr->m_next;
} while (ptr != face);
faceArray[m_faceCount] = face;
m_faceCount ++;
}
}
m_faceArray = (dgConvexSimplexEdge **) m_allocator->Malloc(dgInt32 (m_faceCount * sizeof(dgConvexSimplexEdge *)));
memcpy (m_faceArray, &faceArray[0], m_faceCount * sizeof(dgConvexSimplexEdge *));
delete convexHull;
return true;
}
void Compute(graph<vertex>& GA, commandLine P) {
t1.start();
long start = P.getOptionLongValue("-r",0);
if(GA.V[start].getOutDegree() == 0) {
cout << "starting vertex has degree 0" << endl;
return;
}
const int procs = P.getOptionIntValue("-p",0);
if(procs > 0) setWorkers(procs);
const double t = P.getOptionDoubleValue("-t",3);
const double epsilon = P.getOptionDoubleValue("-e",0.000000001);
const uintE N = P.getOptionIntValue("-N",1);
const intE n = GA.n;
const double constant = exp(t)*epsilon/(2*(double)N);
double* psis = newA(double,N);
double* fact = newA(double,N);
fact[0] = 1;
for(long k=1;k<N;k++) fact[k] = k*fact[k-1];
double* tm = newA(double,N);
{parallel_for(long m=0;m<N;m++) tm[m] = pow(t,m);}
{parallel_for(long k=0;k<N;k++) {
psis[k] = 0;
for(long m=0;m<N-k;m++)
psis[k] += fact[k]*tm[m]/(double)fact[m+k];
}}
sparseAdditiveSet<float> x = sparseAdditiveSet<float>(10000,1,0.0);
sparseAdditiveSet<float> r = sparseAdditiveSet<float>(2,1,0.0);
x.insert(make_pair(start,0.0));
r.insert(make_pair(start,1.0));
vertexSubset Frontier(n,start);
long j = 0, totalPushes = 0;
while(Frontier.numNonzeros() > 0){
totalPushes += Frontier.numNonzeros();
uintT* Degrees = newA(uintT,Frontier.numNonzeros());
{parallel_for(long i=0;i<Frontier.numNonzeros();i++) Degrees[i] = GA.V[Frontier.s[i]].getOutDegree();}
long totalDegree = sequence::plusReduce(Degrees,Frontier.numNonzeros());
free(Degrees);
if(j+1 < N) {
long rCount = r.count();
//make bigger hash table initialized to 0.0's
sparseAdditiveSet<float> new_r = sparseAdditiveSet<float>(max(100L,min((long)n,totalDegree+rCount)),LOAD_FACTOR,0.0);
vertexMap(Frontier,Local_Update(x,r));
vertexSubset output = edgeMap(GA, Frontier, HK_F<vertex>(x,r,new_r,GA.V,t/(double)(j+1)));
r.del();
r = new_r;
if(x.m < ((uintT) 1 << log2RoundUp((uintT)(LOAD_FACTOR*min((long)n,rCount+output.numNonzeros()))))) {
sparseAdditiveSet<float> new_x = sparseAdditiveSet<float>(LOAD_FACTOR*min((long)n,rCount+output.numNonzeros()),LOAD_FACTOR,0.0); //make bigger hash table
new_x.copy(x);
x.del();
x = new_x;
}
output.del();
//compute active set (faster in practice to just scan over r)
_seq<ACLpair> vals = r.entries(activeF<vertex>(GA.V,constant/psis[j+1]));
uintE* Active = newA(uintE,vals.n);
parallel_for(long i=0;i<vals.n;i++) Active[i] = vals.A[i].first;
Frontier.del(); vals.del();
Frontier = vertexSubset(n,vals.n,Active);
j++;
} else { //last iteration
/**
Given a graph, select k random start vertices. Call these
k vertices set S. For all vertices v in the graph, find the farthest
distance between v and any of the vertices in S. Store this is distField.
Note that this implementation is faster than running K separate BFSs in
parallel because all k BFSs share a frontier. This means that for each
node we must store a bit vector of size K bits, indicating whether or not
a node has been visited yet by the kth BFS.
Note that a node v will continue to be added to the frontier as long as its
source vertex u has a different bit vector than it. This means that v is
being visited for the first time by at least one of the K BFSs. Thus we must
also increment our estimate of its radius since the radius is the FARTHEST
distance of node v to any of the u source vertices.
At the end of the algorithm distField will contain the maximum distance from
each node v in the graph to any of the K source nodes. The final radius
estimate of the graph is obtained by taking the max radius obtained from all
nodes in the graph. Note that this is an estimate because we only ran a BFS
from K nodes. If we wanted an exact radius we would have had to run a BFS from
every single node in the graph.
**/
void kBFS(graph *g, int *distField) {
int** visited;
int** nextVisited;
int* radii;
int iter = 0;
// set up globals
#pragma omp parallel for schedule(static)
for (int i = 0; i < g->num_nodes; i++)
distField[i] = NA;
radii = distField;
visited = (int**) malloc(sizeof(int*) * g->num_nodes);
nextVisited = (int**) malloc(sizeof(int*) * g->num_nodes);
for (int i = 0; i < g->num_nodes; i++) {
visited[i] = (int*) malloc(sizeof(int) * NUMWORDS);
nextVisited[i] = (int*) malloc(sizeof(int) * NUMWORDS);
memset(visited[i], 0, sizeof(int) * NUMWORDS);
memset(nextVisited[i], 0, sizeof(int) * NUMWORDS);
}
// initialize the frontier with K random nodes
srand(0);
int numSources = std::min(K, g->num_nodes);
int S[numSources]; // the set of source nodes
for (int i = 0; i < numSources; i++)
S[i] = (std::rand()/(float)RAND_MAX) * g->num_nodes;
VertexSet* frontier = newVertexSet(SPARSE, numSources, g->num_nodes);
for (int i = 0; i < numSources; i++) {
addVertex(frontier, S[i]);
}
// iterate over values 1 thru k to do initialization
VertexSet* ks = newVertexSet(SPARSE, numSources, g->num_nodes);
for (int i = 0; i < numSources; i++)
addVertex(ks, i);
Init i(S, visited, nextVisited, radii);
vertexMap(ks, i, NORETURN);
freeVertexSet(ks);
VertexSet *newFrontier;
while (frontier->size > 0) {
iter = iter + 1;
RadiiUpdate ru(visited, nextVisited, radii, iter);
newFrontier = edgeMap(g, frontier, ru);
freeVertexSet(frontier);
frontier = newFrontier;
VisitedCopy vc(visited, nextVisited);
vertexMap(frontier, vc, NORETURN);
}
for (int i = 0; i < g->num_nodes; i++) {
free(visited[i]);
free(nextVisited[i]);
}
freeVertexSet(frontier);
free(visited);
free(nextVisited);
}
void Compute(graph<vertex>& GA, commandLine P) {
t5.start();
long length = P.getOptionLongValue("-r",0); //number of words per vertex
char* oFile = P.getOptionValue("-out"); //file to write eccentricites
srand (time(NULL));
uintT seed = rand();
cout << "seed = " << seed << endl;
t0.start();
long n = GA.n;
uintE* ecc = newA(uintE,n);
uintE* ecc2 = newA(uintE,n);
{parallel_for(long i=0;i<n;i++) {
ecc[i] = ecc2[i] = 0;
}}
t0.stop();
//BEGIN COMPUTE CONNECTED COMPONENTS
t1.start();
intE* Labels = newA(intE,n);
{parallel_for(long i=0;i<n;i++) {
if(GA.V[i].getOutDegree() == 0) Labels[i] = -i-1; //singletons
else Labels[i] = INT_E_MAX;
}}
//get max degree vertex
uintE maxV = sequence::reduce<uintE>((intE)0,(intE)n,maxF<intE>(),getDegree<vertex>(GA.V));
//visit large component with BFS
CCBFS(maxV,GA,Labels);
//visit small components with label propagation
Components(GA, Labels);
//sort by component ID
intPair* CCpairs = newA(intPair,n);
{parallel_for(long i=0;i<n;i++)
if(Labels[i] < 0)
CCpairs[i] = make_pair(-Labels[i]-1,i);
else CCpairs[i] = make_pair(Labels[i],i);
}
free(Labels);
intSort::iSort(CCpairs, n, n+1,firstF<uintE,uintE>());
uintE* changes = newA(uintE,n);
changes[0] = 0;
{parallel_for(long i=1;i<n;i++)
changes[i] = (CCpairs[i].first != CCpairs[i-1].first) ? i : UINT_E_MAX;}
uintE* CCoffsets = newA(uintE,n);
uintE numCC = sequence::filter(changes, CCoffsets, n, nonMaxF());
CCoffsets[numCC] = n;
free(changes);
t1.stop();
//END COMPUTE CONNECTED COMPONENTS
//init data structures
t0.start();
length = max((long)1,min((n+63)/64,(long)length));
long* VisitedArray = newA(long,n*length);
long* NextVisitedArray = newA(long,n*length);
int* flags = newA(int,n);
{parallel_for(long i=0;i<n;i++) flags[i] = -1;}
uintE* starts = newA(uintE,n);
intPair* pairs = newA(intPair,n);
t0.stop();
//BEGIN COMPUTE ECCENTRICITES PER COMPONENT
for(long k = 0; k < numCC; k++) {
t2.start();
uintE o = CCoffsets[k];
uintE CCsize = CCoffsets[k+1] - o;
if(CCsize == 2) { //size 2 CC's have ecc of 1
ecc[CCpairs[o].second] = ecc[CCpairs[o+1].second] = 1;
t2.stop();
} else if(CCsize > 1) { //size 1 CC's already have ecc of 0
//do main computation
long myLength = min((long)length,((long)CCsize+63)/64);
//initialize bit vectors for component vertices
{parallel_for(long i=0;i<CCsize;i++) {
uintT v = CCpairs[o+i].second;
parallel_for(long j=0;j<myLength;j++)
VisitedArray[v*myLength+j] = NextVisitedArray[v*myLength+j] = 0;
}}
long sampleSize = min((long)CCsize,(long)64*myLength);
uintE* starts2 = newA(uintE,sampleSize);
//pick random vertices (could have duplicates)
{parallel_for(ulong i=0;i<sampleSize;i++) {
uintT index = hashInt(i+seed) % CCsize;
if(flags[index] == -1 && CAS(&flags[index],-1,(int)i)) {
starts[i] = CCpairs[o+index].second;
NextVisitedArray[CCpairs[o+index].second*myLength + i/64] = (long) 1<<(i%64);
} else starts[i] = UINT_E_MAX;
}}
//remove duplicates
uintE numUnique = sequence::filter(starts,starts2,sampleSize,nonMaxF());
//reset flags
parallel_for(ulong i=0;i<sampleSize;i++) {
uintT index = hashInt(i+seed) % CCsize;
if(flags[index] == i) flags[index] = -1;
}
//first phase
vertexSubset Frontier(n,numUnique,starts2); //initial frontier
//note: starts2 will be freed inside the following loop
uintE round = 0;
while(!Frontier.isEmpty()){
round++;
vertexMap(Frontier, Ecc_Vertex_F(myLength,VisitedArray,NextVisitedArray));
vertexSubset output =
edgeMap(GA, Frontier,
Ecc_F(myLength,VisitedArray,NextVisitedArray,ecc,round),
GA.m/20);
Frontier.del();
Frontier = output;
}
Frontier.del();
t2.stop();
//second phase if size of CC > 64
if(CCsize > 1024) {
//sort by ecc
t3.start();
{parallel_for(long i=0;i<CCsize;i++) {
pairs[i] = make_pair(ecc[CCpairs[o+i].second],CCpairs[o+i].second);
}}
intPair maxR = sequence::reduce(pairs,CCsize,maxFirstF());
intSort::iSort(pairs, CCsize, 1+maxR.first, firstF<uintE,uintE>());
t3.stop();
t4.start();
//reset bit vectors for component vertices
{parallel_for(long i=0;i<CCsize;i++) {
uintT v = CCpairs[o+i].second;
parallel_for(long j=0;j<myLength;j++)
VisitedArray[v*myLength+j] = NextVisitedArray[v*myLength+j] = 0;
}}
starts2 = newA(uintE,sampleSize);
//pick starting points with highest ecc ("fringe" vertices)
{parallel_for(long i=0;i<sampleSize;i++) {
intE v = pairs[CCsize-i-1].second;
starts2[i] = v;
NextVisitedArray[v*myLength + i/64] = (long) 1<<(i%64);
}}
vertexSubset Frontier2(n,sampleSize,starts2); //initial frontier
//note: starts2 will be freed inside the following loop
round = 0;
while(!Frontier2.isEmpty()){
round++;
vertexMap(Frontier2, Ecc_Vertex_F(myLength,VisitedArray,NextVisitedArray));
vertexSubset output =
edgeMap(GA, Frontier2,Ecc_F(myLength,VisitedArray,NextVisitedArray,ecc2,round), GA.m/20);
Frontier2.del();
Frontier2 = output;
}
Frontier2.del();
{parallel_for(long i=0;i<n;i++) ecc[i] = max(ecc[i],ecc2[i]);}
t4.stop();
}
}
PolygonMesh<PointType>::PolygonMesh(const PolygonMesh<PointType>& source)
:vertices(0),lastVertex(0),faces(0),lastFace(0)
{
/* Copy all vertices and associate the copies with their originals: */
Misc::HashTable<const Vertex*,Vertex*> vertexMap((source.getNumVertices()*3)/2);
for(const Vertex* vPtr=source.vertices;vPtr!=0;vPtr=vPtr->succ)
vertexMap.setEntry(typename Misc::HashTable<const Vertex*,Vertex*>::Entry(vPtr,newVertex(*vPtr)));
/* Count the number of edges in the source mesh to estimate the needed hash table size: */
const Face* fPtr;
int numEdges=0;
for(fPtr=source.faces;fPtr!=0;fPtr=fPtr->succ)
{
const Edge* firstSourceEdge=fPtr->getEdge();
const Edge* fePtr=firstSourceEdge;
do
{
++numEdges;
fePtr=fePtr->getFaceSucc();
}
while(fePtr!=firstSourceEdge);
}
Misc::HashTable<VertexPair,Edge*,VertexPair> companions(numEdges);
/* Copy faces one at a time: */
for(fPtr=source.faces;fPtr!=0;fPtr=fPtr->succ)
{
Face* face=newFace();
/* Copy all edges of the face (don't connect them to other faces yet): */
const Edge* firstSourceEdge=fPtr->getEdge();
const Edge* fePtr=firstSourceEdge;
Edge* firstEdge;
Edge* lastEdge=0;
do
{
/* Create a new edge: */
Edge* edge=newEdge();
Vertex* vPtr=vertexMap.getEntry(fePtr->getStart()).getDest();
edge->set(vPtr,face,lastEdge,0,0);
edge->sharpness=fePtr->sharpness;
vPtr->setEdge(edge);
if(lastEdge!=0)
lastEdge->setFaceSucc(edge);
else
firstEdge=edge;
lastEdge=edge;
fePtr=fePtr->getFaceSucc();
}
while(fePtr!=firstSourceEdge);
lastEdge->setFaceSucc(firstEdge);
firstEdge->setFacePred(lastEdge);
face->setEdge(firstEdge);
/* Now go around the edge loop again to connect the face to its neighbours: */
Edge* edge=firstEdge;
do
{
VertexPair vp(*edge);
typename Misc::HashTable<VertexPair,Edge*,VertexPair>::Iterator companionsIt=companions.findEntry(vp);
if(companionsIt!=companions.end())
{
/* Connect the edge to its companion: */
edge->setOpposite(companionsIt->getDest());
companionsIt->getDest()->setOpposite(edge);
}
else
{
/* Add the edge to the companion table: */
companions.setEntry(typename Misc::HashTable<VertexPair,Edge*,VertexPair>::Entry(vp,edge));
}
edge=edge->getFaceSucc();
}
while(edge!=firstEdge);
}
}
