00001
00027 #ifndef KDCELL_H_
00028 #define KDCELL_H_
00029 #include<limits>
00030 #include<vector>
00031 #include<algorithm>
00032 #include<iostream>
00033 #include"Point3.h"
00034 #include<assert.h>
00035 #include"NodeWrapper.h"
00036 using namespace std;
00037
00038 class KdCell {
00039 public:
00040 const static int LEAF;
00041 const static int SPLIT_X;
00042 const static int SPLIT_Y;
00043 const static int SPLIT_Z;
00044 const static int MAX_POINTS_IN_CELL;
00045 bool removeFromTree;
00046 protected:
00047 Point3 min;
00048 Point3 max;
00049 const int splitType;
00050 const double splitValue;
00051 KdCell * leftChild;
00052 KdCell * rightChild;
00053 vector<NodeWrapper*> pointList;
00054
00055 public:
00056 KdCell() :
00057 min(std::numeric_limits<double>::max()), max(-1 * std::numeric_limits<
00058 double>::max()), splitType(LEAF), splitValue(numeric_limits<
00059 double>::max()) {
00060 pointList.resize(MAX_POINTS_IN_CELL);
00061 leftChild = NULL;
00062 rightChild = NULL;
00063 removeFromTree=false;
00064 }
00065 KdCell(int inSplitType, double inSplitValue) :
00066 min(0), max(0), splitType(inSplitType), splitValue(
00067 inSplitValue) {
00068 if (splitType == LEAF)
00069 pointList.resize(MAX_POINTS_IN_CELL);
00070 else
00071 pointList.resize(0);
00072 leftChild=rightChild=NULL;
00073 removeFromTree=false;
00074
00075 }
00076 bool equals(KdCell & other){
00077 if(splitType!=other.splitType)
00078 return false;
00079 if(splitValue!=other.splitValue)
00080 return false;
00081 if(min.equals(other.min)==false)
00082 return false;
00083 if(max.equals(other.max)==false)
00084 return false;
00085 if(splitType==KdCell::LEAF)
00086 return leftChild->equals(*leftChild) && rightChild->equals(*rightChild);
00087 if(pointList.size()!=other.pointList.size())
00088 return false;
00089 for(unsigned int i=0;i<pointList.size();i++){
00090 if(pointList[i]!=NULL && other.pointList[i]!=NULL){
00091 if(pointList[i]->equals(*other.pointList[i])==false)
00092 return false;
00093 }
00094 if(pointList[i]!=other.pointList[i])
00095 return false;
00096 }
00097 return true;
00098 }
00102 virtual KdCell* createNewBlankCell(int splitType, double splitValue) {
00103 cout<<"KDCELL CALLED !!!!! "<<endl;
00104 return (new KdCell(splitType, splitValue));
00105 }
00109 static void cleanupTree(KdCell * root) {
00110 if (root->splitType == LEAF) {
00111 delete root;
00112 return;
00113 }
00114 if(root->leftChild!=NULL)
00115 cleanupTree(root->leftChild);
00116 if(root->rightChild!=NULL)
00117 cleanupTree(root->rightChild);
00118 delete root;
00119 }
00120
00121
00122
00123 static KdCell * subDivide(vector<NodeWrapper*> & list, int offset,
00124 const int size, vector<double> * arr, KdCell & factory) {
00125 KdCell * toReturn;
00126 if (size <= KdCell::MAX_POINTS_IN_CELL) {
00127
00128 toReturn = factory.createNewBlankCell(KdCell::LEAF, numeric_limits<
00129 double>::max());
00130 KdCell & cell = *toReturn;
00131 for (int i = 0; i < size; i++) {
00132 cell.pointList[i] = list[offset + i];
00133 }
00134 for (int i = 0; i < size; i++) {
00135 for(int j=0;j<size;j++){
00136 if(i!=j){
00137 if(cell.pointList[i]->equals(*cell.pointList[j]))
00138 assert(false);
00139 }
00140 }
00141 }
00142 cell.computeBoundingBoxFromPoints(list, size);
00143 cell.notifyContentsRebuilt(true);
00144 } else {
00145 bool shouldClean = false;
00146 if (arr == NULL) {
00147 arr = new vector<double> (size);
00148 shouldClean = true;
00149 }
00150 Point3 min(std::numeric_limits<float>::max());
00151 Point3 max(-std::numeric_limits<float>::max());
00152 for (int i = offset; i < size; i++) {
00153 min.setIfMin(list[i]->getMin());
00154 max.setIfMax(list[i]->getMax());
00155 }
00156 Point3 diff(max);
00157 diff.sub(min);
00158 int splitTypeUsed = -1, splitType0, splitType1, splitType2;
00159 double splitValueUsed = -1;
00160 if (diff.getZ() > diff.getX() && diff.getZ() > diff.getY()) {
00161 splitType0 = KdCell::SPLIT_Z;
00162 bool comparCond = diff.getX() > diff.getY();
00163 splitType1 = comparCond ? KdCell::SPLIT_X : KdCell::SPLIT_Y;
00164 splitType2 = comparCond ? KdCell::SPLIT_Y : KdCell::SPLIT_X;
00165 } else if (diff.getY() > diff.getX()) {
00166 splitType0 = KdCell::SPLIT_Y;
00167 bool comparCond = diff.getX() > diff.getZ();
00168 splitType1 = comparCond ? KdCell::SPLIT_X : KdCell::SPLIT_Z;
00169 splitType2 = comparCond ? KdCell::SPLIT_Z : KdCell::SPLIT_X;
00170 } else {
00171 splitType0 = KdCell::SPLIT_X;
00172 bool comparCond = diff.getY() > diff.getZ();
00173 splitType1 = comparCond ? KdCell::SPLIT_Y : KdCell::SPLIT_Z;
00174 splitType2 = comparCond ? KdCell::SPLIT_Z : KdCell::SPLIT_Y;
00175 }
00176
00177
00178 splitTypeUsed = splitType0;
00179 splitValueUsed = computeSplitValue(list, offset, size, splitType0,
00180 arr);
00181 if (splitValueUsed == numeric_limits<float>::max()) {
00182 splitTypeUsed = splitType1;
00183 splitValueUsed = computeSplitValue(list, offset, size,
00184 splitType1, arr);
00185 if (splitValueUsed == numeric_limits<float>::max()) {
00186 splitTypeUsed = splitType2;
00187 splitValueUsed = computeSplitValue(list, offset, size,
00188 splitType2, arr);
00189 }
00190 }
00191
00192 if (splitValueUsed == numeric_limits<float>::max()) {
00193 assert(false && "Unable to find a valid split across any dimension!");
00194 }
00195
00196
00197 int leftCountForSplit = splitList(list, offset, size,
00198 splitValueUsed, splitTypeUsed);
00199
00200
00201
00202 if (leftCountForSplit <= 1 || leftCountForSplit >= size - 1) {
00203
00204
00205
00206
00207
00208 assert(false && "Invalid split");
00209 }
00210 toReturn
00211 = factory.createNewBlankCell(splitTypeUsed, splitValueUsed);
00212 KdCell & cell = *toReturn;
00213 cell.max.set(max);
00214 cell.min.set(min);
00215 cell.leftChild = subDivide(list, offset, leftCountForSplit, arr,
00216 factory);
00217 cell.rightChild = subDivide(list, offset + leftCountForSplit, size
00218 - leftCountForSplit, arr, factory);
00219
00220
00221 if (shouldClean == true)
00222 delete arr;
00223 }
00224 return toReturn;
00225
00226 }
00230 bool notifyContentsRebuilt(bool inChange) {
00231 return inChange;
00232 }
00236 static double computeSplitValue(vector<NodeWrapper*> & list, int offset,
00237 int size, int pSplitType, vector<double> * arr) {
00238 for (int i = 0; i < size; i++) {
00239 (*arr)[i] = findSplitComponent(*(list[offset + i]), pSplitType);
00240 }
00241
00242
00243
00244
00245
00246
00247 return findMedianGapSplit(arr, size);
00248 }
00252 static double findSplitComponent(NodeWrapper & n, int pSplitType) {
00253 if (pSplitType == KdCell::SPLIT_X)
00254 return n.getLocationX();
00255 if (pSplitType == KdCell::SPLIT_Y)
00256 return n.getLocationY();
00257 if (pSplitType == KdCell::SPLIT_Z)
00258 return n.getLocationZ();
00259 assert(false && "Invalid splitType requested in findSplitComponent");
00260 }
00264 static double findMedianGapSplit(vector<double> * arr, int size) {
00265
00266
00267
00268
00269
00270
00271 sort(arr->begin(), arr->begin()+size);
00272
00273
00274
00275
00276
00277 int start = ((size - 1) >> 1) - ((size + 7) >> 3);
00278 int end = (size >> 1) + ((size + 7) >> 3);
00279 if (start == end) {
00280
00281 assert(false && "Start==End in findMedianSplit, should not happen!");
00282 }
00283 double largestGap = 0;
00284 double splitValue = 0;
00285 double nextValue = (*arr)[start];
00286 for (int i = start; i < end; i++) {
00287 double curValue = nextValue;
00288 nextValue = (*arr)[i + 1];
00289 if ((nextValue - curValue) > largestGap) {
00290 largestGap = nextValue - curValue;
00291 splitValue = 0.5f * (curValue + nextValue);
00292 if (splitValue == nextValue) {
00293 splitValue = curValue;
00294 }
00295 }
00296 }
00297 if (largestGap <= 0) {
00298
00299 splitValue = numeric_limits<float>::max();
00300 }
00301 return splitValue;
00302
00303 }
00307 static int splitList(vector<NodeWrapper*> & list, int startIndex, int size,
00308 double pSplitValue, const int pSplitType) {
00309
00310
00311
00312 int lo = startIndex;
00313 int hi = startIndex + size - 1;
00314
00315
00316
00317 while (lo <= hi) {
00318 while (lo <= hi && pSplitValue >= findSplitComponent(*(list[lo]),
00319 pSplitType)) {
00320
00321
00322 lo++;
00323 }
00324 while (lo <= hi && pSplitValue < findSplitComponent(*(list[hi]),
00325 pSplitType)) {
00326
00327
00328 hi--;
00329 }
00330 if (lo < hi) {
00331 int index1 = lo++;
00332 int index2 = hi--;
00333 NodeWrapper *temp = list[index1];
00334 list[index1] = list[index2];
00335 list[index2] = temp;
00336 }
00337 }
00338 return lo - startIndex;
00339 }
00344 bool contains(NodeWrapper &point) {
00345 if (splitType == KdCell::LEAF) {
00346
00347 for (int i = 0; i < KdCell::MAX_POINTS_IN_CELL; i++) {
00348 NodeWrapper * myNode = pointList[i];
00349 if (myNode != NULL && (*myNode).equals(point) == true) {
00350 return true;
00351 }
00352 }
00353 return false;
00354 } else {
00355
00356 float val = findSplitComponent(point, splitType);
00357 KdCell *child = val <= splitValue ? leftChild : rightChild;
00358 if(child!=NULL)
00359 return child->contains(point);
00360 return false;
00361 }
00362 }
00366 void getAll(vector<NodeWrapper*> & allLeaves) {
00367 if (this->splitType == KdCell::LEAF) {
00368 for (int i = 0; i < KdCell::MAX_POINTS_IN_CELL; i++) {
00369 if (pointList[i] != NULL)
00370 allLeaves.push_back(pointList[i]);
00371 }
00372 } else {
00373 leftChild->getAll(allLeaves);
00374 rightChild->getAll(allLeaves);
00375 }
00376 }
00380 bool remove(NodeWrapper & nw) {
00381 bool treeChanged = false;
00382 treeChanged = removeInternal(nw,NULL,NULL);
00383 cout<<"===================AFTER REMOVAL================"
00384 <<*this<<"====================================="<<endl;
00385 return treeChanged;
00386 }
00390 bool removeInternal(NodeWrapper & nw, KdCell * parent, KdCell * grandParent) {
00391 bool treeChanged = false;
00392
00393 if (this->splitType == KdCell::LEAF) {
00394 int numPoints=0;
00395 int indexToDelete = -1;
00396 for (int i = 0; i < KdCell::MAX_POINTS_IN_CELL; i++) {
00397 if (pointList[i] != NULL){
00398 if (pointList[i]->equals(nw) == true) {
00399 indexToDelete = i;
00400 }
00401 numPoints++;
00402 }
00403 }
00404
00405 if(indexToDelete!=-1){
00406 if(numPoints==1 && parent!=NULL && grandParent!=NULL){
00407 cout<<"About to Updated subnode :: " << *grandParent<<endl;
00408 }
00409 pointList[indexToDelete] = NULL;
00410 cout<<"Removing "<<nw<<endl;
00411 treeChanged = recomputeLeafBoundingBoxIfChanges();
00412 treeChanged |= notifyContentsRebuilt(treeChanged);
00413 if(numPoints==1 && parent!=NULL && grandParent!=NULL){
00414
00415 KdCell *otherChild;
00416 if(parent->leftChild->equals(*this)){
00417 otherChild = rightChild;
00418 }
00419 else{
00420 otherChild = leftChild;
00421 }
00422
00423 if (grandParent->leftChild->equals(*parent)) {
00424 grandParent->leftChild=otherChild;
00425 } else {
00426 grandParent->rightChild=otherChild;
00427 }
00428 this->removeFromTree=true;
00429 parent->removeFromTree = true;
00430 cout<<"Updated subnode :: " << *grandParent<<endl;
00431 }
00432 }
00433 }
00434
00435 else {
00436 double nodeSplitAxisValue = findSplitComponent(nw, splitType);
00437 KdCell * child = nodeSplitAxisValue <= splitValue ? leftChild
00438 : rightChild;
00439 treeChanged = child->removeInternal(nw,this, parent);
00440 cout<<"BEFORE EX " <<*this<<endl;
00441 if(treeChanged ==true && removeFromTree==false){
00442 treeChanged |= recomputeParentBoundingBoxIfChanges();
00443 notifyContentsRebuilt(treeChanged);
00444 }
00445 }
00446 return treeChanged;
00447 }
00451 bool add(NodeWrapper & nw) {
00452 return add(NULL, this, nw);
00453 }
00457 static bool add(KdCell * parent, KdCell * current, NodeWrapper & nw) {
00458 bool treeChanged = false;
00459 if (current->splitType == KdCell::LEAF) {
00460 bool canInsert = false;
00461 for (int i = 0; i < KdCell::MAX_POINTS_IN_CELL; i++) {
00462 if (current->pointList[i] == NULL) {
00463 current->pointList[i] = &nw;
00464 canInsert = true;
00465 break;
00466 }
00467 }
00468
00469 if (canInsert == false) {
00470 if (parent == NULL) {
00471 assert(false&&"Cannot split root node, in addNode");
00472 } else {
00473 vector<NodeWrapper*>newList(KdCell::MAX_POINTS_IN_CELL + 1);
00474 for(int i=0;i<MAX_POINTS_IN_CELL;i++){
00475 for(int j=0;j<MAX_POINTS_IN_CELL;j++){
00476 if(i!=j){
00477 if(current->pointList[i]->equals(*current->pointList[j]))
00478 assert(false&& "Sharing!!");
00479 }
00480 }
00481 }
00482 for (int i = 0; i < MAX_POINTS_IN_CELL; i++)
00483 newList[i] = current->pointList[i];
00484 newList[MAX_POINTS_IN_CELL] = &nw;
00485 KdCell *newCell = subDivide(newList, 0,KdCell::MAX_POINTS_IN_CELL + 1, NULL, *current);
00486 if (parent->leftChild == current) {
00487 parent->leftChild = newCell;
00488 } else if (parent->rightChild == current) {
00489 parent->rightChild = newCell;
00490 }
00491 canInsert = true;
00492 delete current;
00493 }
00494 }
00495 treeChanged = canInsert;
00496 }
00497
00498 else {
00499 double nodeSplitAxisValue = findSplitComponent(nw,
00500 current->splitType);
00501 treeChanged = (nodeSplitAxisValue <= current->splitValue) ? add(
00502 current, current->leftChild, nw) : add(current,
00503 current->rightChild, nw);
00504 if (treeChanged) {
00505 bool change = current->addToBoundingBoxIfChanged(nw);
00506 change = current->notifyPointAdded(nw, change);
00507
00508 }
00509 }
00510 return treeChanged;
00511 }
00512
00513 private:
00517 bool notifyPointAdded(NodeWrapper & nw, bool inChange) {
00518 return inChange;
00519 }
00523 bool addToBoundingBoxIfChanged(NodeWrapper & nw) {
00524 bool retVal = min.setIfMin(nw.getLocation());
00525 retVal |= max.setIfMax(nw.getLocation());
00526 return retVal;
00527 }
00528
00532 void computeBoundingBoxFromPoints(vector<NodeWrapper *> & list, int size) {
00533 Point3 newMin(numeric_limits<double>::max());
00534 Point3 newMax(-numeric_limits<double>::max());
00535 for (int i = 0; i < size; i++) {
00536 newMin.setIfMin(list[i]->getLocation());
00537 newMax.setIfMax(list[i]->getLocation());
00538 }
00539 min.set(newMin);
00540 max.set(newMax);
00541 }
00545 bool recomputeLeafBoundingBoxIfChanges() {
00546 Point3 newMin(numeric_limits<float>::max());
00547 Point3 newMax(-numeric_limits<float>::max());
00548 for (int i = 0; i < KdCell::MAX_POINTS_IN_CELL; i++) {
00549 if (pointList[i] != NULL) {
00550 newMin.setIfMin(pointList[i]->getMin());
00551 newMax.setIfMax(pointList[i]->getMax());
00552 }
00553 }
00554 return updateBoundingBox(newMin, newMax);
00555
00556 }
00560 bool recomputeParentBoundingBoxIfChanges(){
00561 Point3 newMin(leftChild->min);
00562 newMin.setIfMin(rightChild->min);
00563 Point3 newMax(leftChild->max);
00564 newMax.setIfMax(rightChild->max);
00565 return updateBoundingBox(newMin, newMax);
00566 }
00570 bool updateBoundingBox(Point3 & newMin, Point3 & newMax) {
00571 bool retVal = false;
00572 retVal = min.setIfMin(newMin);
00573 retVal |= max.setIfMax(newMax);
00574 return retVal;
00575 }
00579 friend ostream& operator<<(ostream & s, KdCell & cell);
00580 };
00581 const int KdCell::SPLIT_X = 0;
00582 const int KdCell::SPLIT_Y = 1;
00583 const int KdCell::SPLIT_Z = 2;
00584 const int KdCell::LEAF = 3;
00585 const int KdCell::MAX_POINTS_IN_CELL = 4;
00586
00590 ostream& operator<<(ostream & s, KdCell & cell) {
00591 if (cell.splitType == KdCell::LEAF) {
00592 s << "Leaf ::[";
00593 for (int i = 0; i < KdCell::MAX_POINTS_IN_CELL; i++) {
00594 if (cell.pointList[i] != NULL)
00595 s << *cell.pointList[i] << ",";
00596 }
00597 s << "]" << std::endl;
00598 } else {
00599 s << "InnerNode(" << cell.splitType << "," << cell.splitValue;
00600 if(cell.leftChild!=NULL)
00601 s<< ") \nLEFT::[" << (*cell.leftChild);
00602 else
00603 s<<" NO-LEFT ";
00604 if(cell.rightChild!=NULL)
00605 s<< "]\nRIGHT::["<< (*cell.rightChild);
00606 else
00607 s<<" NO-RIGHT";
00608 s<< "]";
00609 }
00610 return s;
00611 }
00612 #endif
