make DynamicSpatialOrganizer more sane

core/src/dynamic_spatial_organizer.hh

@@ -3,9 +3,6 @@
 
 #include <vector>
 #include <map>
-#include <cmath>
-#include <limits>
-#include <stdint.h>
 
 #include <ost/geom/geom.hh>
 
@@ -19,91 +16,43 @@ class SpatialOrganizerCell{
 
 public:
 
-  SpatialOrganizerCell(): num_items_(0),
-          capacity_(32) {
-    x_ = new Real[capacity_];
-    y_ = new Real[capacity_];
-    z_ = new Real[capacity_];
-    pointers_ = new ITEM*[capacity_];
-    distance_buffer_ = new Real[capacity_];
-  }
-
-  ~SpatialOrganizerCell() {
-    delete [] x_;
-    delete [] y_;
-    delete [] z_;
-    delete [] pointers_;
-    delete [] distance_buffer_;
+  SpatialOrganizerCell() {
+    x_.reserve(32);
+    y_.reserve(32);
+    z_.reserve(32);
+    pointers_.reserve(32);
+    distance_buffer_.reserve(32);
   }
 
   void AddItem(ITEM* item, Real x, Real y, Real z) {
-
-    int idx = this->GetIdx(item);   
-    if(idx != -1) {
-      throw promod3::Error("Item is already in spatial organizer cell!");
-    }
-
-    if(num_items_ == capacity_) {
-      capacity_ *= 2;
-
-      Real* new_x = new Real[capacity_];
-      Real* new_y = new Real[capacity_];
-      Real* new_z = new Real[capacity_];
-      ITEM** new_pointers = new ITEM*[capacity_];
-      Real* new_distance_buffer_ = new Real[capacity_];
-
-      memcpy(new_x, x_, num_items_ * sizeof(Real));
-      memcpy(new_y, y_, num_items_ * sizeof(Real));
-      memcpy(new_z, z_, num_items_ * sizeof(Real));
-      memcpy(new_pointers, pointers_, num_items_ * sizeof(ITEM*));
-
-      delete [] x_;
-      delete [] y_;
-      delete [] z_;
-      delete [] pointers_;
-      delete [] distance_buffer_;
-
-      x_ = new_x;
-      y_ = new_y;
-      z_ = new_z;
-      pointers_ = new_pointers;
-      distance_buffer_ = new_distance_buffer_;
-    }
-
-    x_[num_items_] = x;
-    y_[num_items_] = y;
-    z_[num_items_] = z;
-    pointers_[num_items_] = item;
-    ++num_items_;
+    x_.push_back(x);
+    y_.push_back(y);
+    z_.push_back(z);
+    pointers_.push_back(item);
+    distance_buffer_.push_back(0.0);
   }
 
   void RemoveItem(ITEM* item) {
-
-    if(num_items_ == 0) {
-      throw promod3::Error("Cannot remove item from empty spatial organizer cell!");
-    }
-
-    int idx = this->GetIdx(item);
-   
-    if(idx == -1) {
+    typename std::vector<ITEM*>::iterator it = 
+    std::find(pointers_.begin(), pointers_.end(), item);
+    if(it == pointers_.end()) {
       throw promod3::Error("Cannot Remove inexistent item from spatial organizer cell!");
     }
-
-    if(idx < (num_items_-1)) {
-      int num_bytes = (num_items_-(idx+1))*sizeof(Real);
-      memmove(&x_[idx],&x_[idx+1],num_bytes);
-      memmove(&y_[idx],&y_[idx+1],num_bytes);
-      memmove(&z_[idx],&z_[idx+1],num_bytes);
-      memmove(&pointers_[idx],&pointers_[idx+1], (num_items_-(idx+1))*sizeof(ITEM*));
-    }
-    --num_items_;
+    int idx = std::distance(pointers_.begin(), it);
+    x_.erase(x_.begin() + idx);
+    y_.erase(y_.begin() + idx);
+    z_.erase(z_.begin() + idx);
+    pointers_.erase(pointers_.begin() + idx);
+    distance_buffer_.pop_back();
   }
 
   void ResetPos(ITEM* item, Real x, Real y, Real z) {
-    int idx = this->GetIdx(item);
-    if(idx == -1) {
-      throw promod3::Error("Cannot reset pos of inexistent item from spatial organizer cell!");      
+    typename std::vector<ITEM*>::iterator it = 
+    std::find(pointers_.begin(), pointers_.end(), item);
+    if(it == pointers_.end()) {
+      throw promod3::Error("Cannot Remove inexistent item from spatial organizer cell!");
     }
+    int idx = std::distance(pointers_.begin(), it);
     x_[idx] = x;
     y_[idx] = y;
     z_[idx] = z;
@@ -114,15 +63,16 @@ public:
 
     Real dx, dy, dz;
     Real squared_cutoff = dist*dist;
+    int num_items = x_.size();
 
-    for(int i = 0; i < num_items_; ++i) {
+    for(int i = 0; i < num_items; ++i) {
       dx = x_[i] - x;
       dy = y_[i] - y;
       dz = z_[i] - z; 
       distance_buffer_[i] = dx*dx + dy*dy + dz*dz;
     }
 
-    for(int i = 0; i < num_items_; ++i) {
+    for(int i = 0; i < num_items; ++i) {
       if(distance_buffer_[i] <= squared_cutoff) {
         result_vec.push_back(std::make_pair(pointers_[i],std::sqrt(distance_buffer_[i])));
       }
@@ -130,22 +80,11 @@ public:
   }
 
 private:
-
-
-  int GetIdx(ITEM* ptr) {
-    for(int i = 0; i < num_items_; ++i) {
-      if(pointers_[i] == ptr) return i;
-    }
-    return -1;
-  }
-
-  int num_items_;
-  int capacity_;
-  Real* x_;
-  Real* y_;
-  Real* z_;
-  ITEM** pointers_;
-  Real* distance_buffer_;
+  std::vector<Real> x_;
+  std::vector<Real> y_;
+  std::vector<Real> z_;
+  std::vector<ITEM*> pointers_;
+  mutable std::vector<Real> distance_buffer_;
 };
 
 //! spatial organizer
@@ -332,7 +271,6 @@ public:
     return return_vec;
   }
 
-
   void Clear() {
     for(typename ItemMap::iterator i = map_.begin(); i != map_.end(); ++i) {
       delete i->second;
@@ -348,9 +286,9 @@ private:
   mutable WithinList result_buffer_;
 
   Index gen_index(const geom::Vec3& pos) const {
-    Index nrvo(static_cast<int>(round(pos[0]/delta_)),
-               static_cast<int>(round(pos[1]/delta_)),
-               static_cast<int>(round(pos[2]/delta_)));
+    Index nrvo(static_cast<int>(std::floor(pos[0]/delta_ + Real(0.5))),
+               static_cast<int>(std::floor(pos[1]/delta_ + Real(0.5))),
+               static_cast<int>(std::floor(pos[2]/delta_ + Real(0.5))));
     return nrvo;
   }
 };