void HxCPDSpatialGraphWarp::resamplePairs(McDArray<McVec3f>& p1,
McDArray<McVec3f>& p2) {
int numPairs = p1.size();
for (int i = p1.size() - 1; i > -1; i--) {
bool resetI = false;
for (int j = i - 1; j > -1; j--) {
// std::cout<<"\nCompare "<<i<<" and "<<j;
McVec3f set1Coord = p1[i];
McVec3f set2Coord = p1[j];
float dist = (set1Coord - set2Coord).length();
if (dist < portSampleDist.getValue()) {
std::cout << "\ndist between " << i << " and " << j << " is "
<< dist;
}
if (dist < portSampleDist.getValue()) {
p1.remove(j, 1);
p2.remove(j, 1);
resetI = true;
}
}
if (resetI) {
i = p1.size() - 1;
}
}
std::cout << "\n" << p1.size() << " point from " << numPairs
<< " left after resampling.";
}
// `resamplePairs()` removes points that are closer than `sampleDist` in `p1`.
// The corresponding points are also removed from `p2`. The two arrays are
// required to have the same size.
static void resamplePairs(McDArray<McVec3f>& p1, McDArray<McVec3f>& p2,
const float sampleDist) {
mcrequire(p1.size() == p2.size());
const int numPairs = p1.size();
for (int i = p1.size() - 1; i > -1; i--) {
bool resetI = false;
for (int j = i - 1; j > -1; j--) {
const McVec3f set1Coord = p1[i];
const McVec3f set2Coord = p1[j];
const float dist = (set1Coord - set2Coord).length();
if (dist < sampleDist) {
p1.remove(j, 1);
p2.remove(j, 1);
resetI = true;
}
}
if (resetI)
i = p1.size() - 1;
}
}
// Computes all variables that form a connected component in the
// adjacenceMatrix.
// The connected component chosen is arbitrary - it takes the first it finds.
bool BruteForceOptMatching::getConnectedComponent(
const McDMatrix<int>& adjacenceMatrix,
McDMatrix<int>& adjacenceMatrixWithoutConnctedComponent,
McDArray<int>& connComp) {
adjacenceMatrixWithoutConnctedComponent.resize(adjacenceMatrix.nRows(),
adjacenceMatrix.nCols());
memcpy(adjacenceMatrixWithoutConnctedComponent.dataPtr(),
adjacenceMatrix.dataPtr(),
sizeof(int) * adjacenceMatrix.nRows() * adjacenceMatrix.nCols());
// find first a startpoint
int start = -1;
connComp.resize(0);
for (int i = 0; i < adjacenceMatrix.nRows(); i++) {
for (int j = i; j < adjacenceMatrix.nCols(); j++) {
if (adjacenceMatrix[i][j] == 1) {
start = i;
break;
}
}
}
if (start == -1)
return false;
McDArray<int> queue;
queue.append(start);
connComp.clear();
while (queue.size() > 0) {
int cur = queue.last();
connComp.append(cur);
queue.pop_back();
for (int i = 0; i < adjacenceMatrixWithoutConnctedComponent.nCols();
i++) {
if (adjacenceMatrixWithoutConnctedComponent[cur][i] == 1) {
queue.push(i);
adjacenceMatrixWithoutConnctedComponent[cur][i] = 0;
}
}
}
// remove duplicates
connComp.sort(&mcStandardCompare);
int cur = connComp.last();
for (int i = connComp.size() - 2; i >= 0; i--) {
if (cur == connComp[i])
connComp.remove(i, 1);
else
cur = connComp[i];
}
return true;
}
