// returns 1 on success, 0 if there are no cubes
extern int firstCube( int r, int n, int cube[] ) {
int origR = r;
memset(cube, -1, n*sizeof(*cube) );
while( r >= 2 ) {
cube[bdd_level2var(LEVEL(r))] = 0;
r = LOW(r);
}
if( r == 0 ) return nextCube( origR, n, cube );
else return 1;
}
void rmNearestNeighbor::searchPoints(LookUpPoint * p)
{
rmU32 nFoundPoints = 0;
FoundPoint fp;
if (p->nLevelsSearched < 4) {
rmU32 * system = (p->nLevelsSearched < 2) ? system3 : system5;
rmU32 edgeLenght = system[1];
rmU32 size = (edgeLenght * edgeLenght * edgeLenght);
rmU32 offset = (systemCur[0] + systemCur[1] + systemCur[2]) * ((edgeLenght == 5) ? 2 : 1);
curBlock = buffer2;
memcpy(buffer2, searchLevel[p->nLevelsSearched], size * sizeof(rmU64));
for (rmU32 i = 0; i != 3; ++i) {
if (p->xyzInCube[i] != 0) {
initShift(system, i, p->xyzInCube[i]);
for (curBlock = buffer2 + size; curBlock != buffer2;) {
--curBlock;
shift();
}
}
}
curCube = (cubes + p->iCube - offset);
init3dLoop(edgeLenght, edgeLenght, edgeLenght);
while (curCube != 0) {
if ((curCube->pointSpread & *curBlock) != 0) {
for (LookUpPoint ** cp = curCube->points + curCube->nPoints; cp != curCube->points;) {
--cp;
if (((**cp).location & *curBlock) != 0) {
if (*cp != p) {
fp.point = *cp;
fp.distance = (*((**cp).pos) - *(p->pos)).magnitude();
rmU32 & fa = foundAt[fp.point - points];
if (fa != (-1)) {
FoundPoint * fp2 = &foundPoints[fa];
if (fp2->distance > fp.distance) {
if (fp2[fp2->offsetToNext].offsetToNext != 0) {
foundAt[fp2[fp2->offsetToNext].point - points] = (fp2 - &foundPoints[0]);
fp2[fp2->offsetToNext].offsetToNext += fp2->offsetToNext;
}
*fp2 = fp2[fp2->offsetToNext];
foundPoints.push_back(fp);
++nFoundPoints;
}
}
else {
foundPoints.push_back(fp);
++nFoundPoints;
}
}
}
}
}
++curBlock;
nextCube();
}
}
else {
for (LookUpPoint * p2 = points; p2 != endPoints; ++p2) {
if (p2 != p) {
fp.point = p2;
fp.distance = (*(p2->pos) - *(p->pos)).magnitude();
rmU32 & fa = foundAt[fp.point - points];
if (fa != (-1)) {
FoundPoint * fp2 = &foundPoints[fa];
if (fp2->distance > fp.distance) {
if (fp2[fp2->offsetToNext].offsetToNext != 0) {
foundAt[fp2[fp2->offsetToNext].point - points] = (fp2 - &foundPoints[0]);
fp2[fp2->offsetToNext].offsetToNext += fp2->offsetToNext;
}
*fp2 = fp2[fp2->offsetToNext];
foundPoints.push_back(fp);
++nFoundPoints;
}
}
else {
foundPoints.push_back(fp);
++nFoundPoints;
}
}
}
}
if (nFoundPoints != 0) {
rmI32 offset;
FoundPoint * oldP = &foundPoints[0];
FoundPoint * newP = &foundPoints[foundPoints.size() - nFoundPoints];
FoundPoint * endP = newP + nFoundPoints;
sort((foundPoints.end() - nFoundPoints), foundPoints.end(), isInRightOrder);
rmU32 i = newP - oldP;
for (; newP != endP; ++newP) {
foundAt[newP->point - points] = i++;
while (newP->distance > oldP[oldP->offsetToNext].distance) {
rmI32 dd = oldP - &foundPoints[0];
oldP += oldP->offsetToNext;
}
offset = (newP - oldP);
newP->offsetToNext = oldP->offsetToNext - offset;
oldP->offsetToNext = offset;
oldP = newP;
}
}
++(p->nLevelsSearched);
}
