diff --git a/trunk/fcl/include/fcl/broad_phase_collision.h b/trunk/fcl/include/fcl/broad_phase_collision.h
index a78675465fb15077f8b33ad4634ddbcdc0db0f3a..9eacf4f727ad613aa3631dde6119805f39920e59 100644
--- a/trunk/fcl/include/fcl/broad_phase_collision.h
+++ b/trunk/fcl/include/fcl/broad_phase_collision.h
@@ -49,7 +49,7 @@
 #include <list>
 #include <iostream>
 #include <boost/unordered_map.hpp>
-
+#include <map>
 #include <boost/bind.hpp>
 
 namespace fcl
@@ -276,6 +276,12 @@ public:
   /** \brief update the condition of manager */
   void update();
 
+  /** \brief update the manager by explicitly given the object updated */
+  void update(CollisionObject* updated_obj);
+
+  /** \brief update the manager by explicitly given the set of objects update */
+  void update(const std::vector<CollisionObject*>& updated_objs);
+
   /** \brief clear the manager */
   void clear();
 
@@ -336,6 +342,8 @@ protected:
 
   // the size of the scene
   AABB scene_limit;
+
+  std::map<CollisionObject*, AABB> obj_aabb_map; // store the map between objects and their aabbs. will make update more convenient
 };
 
 
@@ -356,6 +364,8 @@ void SpatialHashingCollisionManager<HashTable>::registerObject(CollisionObject*
   }
   else
     objs_outside_scene_limit.push_back(obj);
+
+  obj_aabb_map[obj] = obj_aabb;
 }
 
 template<typename HashTable>
@@ -375,6 +385,8 @@ void SpatialHashingCollisionManager<HashTable>::unregisterObject(CollisionObject
   }
   else
     objs_outside_scene_limit.remove(obj);
+
+  obj_aabb_map.erase(obj);
 }
 
 template<typename HashTable>
@@ -389,15 +401,71 @@ void SpatialHashingCollisionManager<HashTable>::update()
 
   std::list<CollisionObject*>::const_iterator it;
   for(it = objs.begin(); it != objs.end(); ++it)
-    registerObject(*it);
+  {
+    CollisionObject* obj = *it;
+    const AABB& obj_aabb = obj->getAABB();
+    AABB overlap_aabb;
+
+    if(scene_limit.overlap(obj_aabb, overlap_aabb))
+    {
+      if(!scene_limit.contain(obj_aabb))
+        objs_outside_scene_limit.push_back(obj);
+    
+      hash_table->insert(overlap_aabb, obj);
+    }
+    else
+      objs_outside_scene_limit.push_back(obj);
+
+    obj_aabb_map[obj] = obj_aabb;
+  }
 }
 
+template<typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::update(CollisionObject* updated_obj)
+{
+  const AABB& new_aabb = updated_obj->getAABB();
+  const AABB& old_aabb = obj_aabb_map[updated_obj];
+
+  if(!scene_limit.contain(old_aabb)) // previously not completely in scene limit
+  {
+    if(scene_limit.contain(new_aabb))
+    {
+      std::list<CollisionObject*>::iterator find_it = std::find(objs_outside_scene_limit.begin(),
+                                                                objs_outside_scene_limit.end(),
+                                                                updated_obj);
+    
+      objs_outside_scene_limit.erase(find_it);
+    }
+  }
+  else if(!scene_limit.contain(new_aabb)) // previous completely in scenelimit, now not
+      objs_outside_scene_limit.push_back(updated_obj);
+  
+  AABB old_overlap_aabb;
+  if(scene_limit.overlap(old_aabb, old_overlap_aabb))
+    hash_table->remove(old_overlap_aabb, updated_obj);
+
+  AABB new_overlap_aabb;
+  if(scene_limit.overlap(new_aabb, new_overlap_aabb))
+     hash_table->insert(new_overlap_aabb, updated_obj);
+
+  obj_aabb_map[updated_obj] = new_aabb;
+}
+
+template<typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::update(const std::vector<CollisionObject*>& updated_objs)
+{
+  for(size_t i = 0; i < updated_objs.size(); ++i)
+    update(updated_objs[i]);
+}
+
+
 template<typename HashTable>
 void SpatialHashingCollisionManager<HashTable>::clear()
 {
   objs.clear();
   hash_table->clear();
   objs_outside_scene_limit.clear();
+  obj_aabb_map.clear();
 }
 
 template<typename HashTable>
@@ -663,6 +731,8 @@ public:
     elist[0] = NULL;
     elist[1] = NULL;
     elist[2] = NULL;
+
+    optimal_axis = 0;
   }
 
   ~SaPCollisionManager()
@@ -685,6 +755,12 @@ public:
   /** \brief update the condition of manager */
   void update();
 
+  /** \brief update the manager by explicitly given the object updated */
+  void update(CollisionObject* updated_obj);
+
+  /** \brief update the manager by explicitly given the set of objects update */
+  void update(const std::vector<CollisionObject*>& updated_objs);
+
   /** \brief clear the manager */
   void clear();
 
@@ -782,12 +858,25 @@ protected:
   {
     SaPPair(CollisionObject* a, CollisionObject* b)
     {
-      obj1 = a;
-      obj2 = b;
+      if(a < b)
+      {
+        obj1 = a;
+        obj2 = b;
+      }
+      else
+      {
+        obj1 = b;
+        obj2 = a;
+      }
     }
 
     CollisionObject* obj1;
     CollisionObject* obj2;
+
+    bool operator == (const SaPPair& other) const
+    {
+      return ((obj1 == other.obj1) && (obj2 == other.obj2));
+    }
   };
 
   /** Functor to help unregister one object */
@@ -822,10 +911,12 @@ protected:
 
     bool operator() (const SaPPair& pair)
     {
-      return ((pair.obj1 == obj1) && (pair.obj2 == obj2)) || ((pair.obj1 == obj2) && (pair.obj2 == obj1));
+      return (pair.obj1 == obj1) && (pair.obj2 == obj2);
     }
   };
 
+  void update_(SaPAABB* updated_aabb);
+
   void updateVelist() 
   {
     for(int coord = 0; coord < 3; ++coord)
@@ -845,6 +936,7 @@ protected:
   /** \brief End point list for x, y, z coordinates */
   EndPoint* elist[3];
   
+  /** \brief vector version of elist, for acceleration */
   std::vector<EndPoint*> velist[3];
 
   /** \brief SAP interval list */
@@ -855,9 +947,45 @@ protected:
 
   size_t optimal_axis;
 
+  std::map<CollisionObject*, SaPAABB*> obj_aabb_map;
+
   bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, BVH_REAL& min_dist) const;
 
   bool collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
+
+  void addToOverlapPairs(const SaPPair& p)
+  {
+    bool repeated = false;
+    for(std::list<SaPPair>::iterator it = overlap_pairs.begin();
+        it != overlap_pairs.end();
+        ++it)
+    {
+      if(*it == p)
+      {
+        repeated = true;
+        break;
+      }
+    }
+
+    if(!repeated)
+      overlap_pairs.push_back(p);
+  }
+
+  void removeFromOverlapPairs(const SaPPair& p)
+  {
+    for(std::list<SaPPair>::iterator it = overlap_pairs.begin();
+        it != overlap_pairs.end();
+        ++it)
+    {
+      if(*it == p)
+      {
+        overlap_pairs.erase(it);
+        break;
+      }
+    }
+
+    // or overlap_pairs.remove_if(isNotValidPair(p));
+  }
 };
 
 
@@ -985,6 +1113,11 @@ public:
       interval_trees[i] = NULL;
   }
 
+  ~IntervalTreeCollisionManager()
+  {
+    clear();
+  }
+
   /** \brief remove one object from the manager */
   void registerObject(CollisionObject* obj);
 
@@ -997,6 +1130,12 @@ public:
   /** \brief update the condition of manager */
   void update();
 
+  /** \brief update the manager by explicitly given the object updated */
+  void update(CollisionObject* updated_obj);
+
+  /** \brief update the manager by explicitly given the set of objects update */
+  void update(const std::vector<CollisionObject*>& updated_objs);
+
   /** \brief clear the manager */
   void clear();
 
@@ -1070,6 +1209,8 @@ protected:
   /** \brief  interval tree manages the intervals */
   IntervalTree* interval_trees[3];
 
+  std::map<CollisionObject*, SAPInterval*> obj_interval_maps[3];
+
   /** \brief tag for whether the interval tree is maintained suitably */
   bool setup_;
 };
diff --git a/trunk/fcl/include/fcl/hash.h b/trunk/fcl/include/fcl/hash.h
index 1274e8263a613c34487dd0ff4400fb3d43bccc2f..b8b15c84ef0811ce3181aae18715687f79a3e14e 100644
--- a/trunk/fcl/include/fcl/hash.h
+++ b/trunk/fcl/include/fcl/hash.h
@@ -54,6 +54,7 @@ protected:
   typedef std::list<Data> Bin;
   std::vector<Bin> table_;
   HashFnc h_;
+  size_t table_size_;
 public:
   SimpleHashTable(const HashFnc& h) : h_(h)
   {
@@ -67,6 +68,7 @@ public:
     }
 
     table_.resize(size);
+    table_size_ = size;
   }
 
   void insert(Key key, Data value)
@@ -105,6 +107,7 @@ public:
   void clear() 
   {
     table_.clear();
+    table_.resize(table_size_);
   }
 };
 
diff --git a/trunk/fcl/include/fcl/interval_tree.h b/trunk/fcl/include/fcl/interval_tree.h
index 94f290a571a3fbb786f0c21a1a947cf7789cd1dd..6624c9c6f2ff42ce8371e28f8d470f7f33a9ffb2 100644
--- a/trunk/fcl/include/fcl/interval_tree.h
+++ b/trunk/fcl/include/fcl/interval_tree.h
@@ -122,6 +122,9 @@ public:
   /** \brief Delete one node of the interval tree */
   SimpleInterval* deleteNode(IntervalTreeNode* node);
 
+  void deleteNode(SimpleInterval* ivl);
+
+
   /** \brief Insert one node of the interval tree */
   IntervalTreeNode* insert(SimpleInterval* new_interval);
 
@@ -152,6 +155,9 @@ protected:
   /** \brief recursively print a subtree */
   void recursivePrint(IntervalTreeNode* node) const;
 
+  /** \brief recursively find the node corresponding to the interval */
+  IntervalTreeNode* recursiveSearch(IntervalTreeNode* node, SimpleInterval* ivl) const;
+
   /** \brief Travels up to the root fixing the max_high fields after an insertion or deletion */
   void fixupMaxHigh(IntervalTreeNode* node);
 
diff --git a/trunk/fcl/src/broad_phase_collision.cpp b/trunk/fcl/src/broad_phase_collision.cpp
index 1cc433b3ab4efbd053e606d207fe2cc1ed3462e3..ca6560cd16fba0e4ae436de4b562f650c1d20658 100644
--- a/trunk/fcl/src/broad_phase_collision.cpp
+++ b/trunk/fcl/src/broad_phase_collision.cpp
@@ -709,6 +709,7 @@ void SaPCollisionManager::unregisterObject(CollisionObject* obj)
   }
 
   AABB_arr.erase(it);
+  obj_aabb_map.erase(obj);
 
   if(it == AABB_arr.end())
     return;
@@ -765,6 +766,7 @@ void SaPCollisionManager::registerObjects(const std::vector<CollisionObject*>& o
       sapaabb->lo->aabb = sapaabb;
       sapaabb->hi->aabb = sapaabb;
       AABB_arr.push_back(sapaabb);
+      obj_aabb_map[other_objs[i]] = sapaabb;
     }
 
 
@@ -920,6 +922,8 @@ void SaPCollisionManager::registerObject(CollisionObject* obj)
 
   AABB_arr.push_back(curr);
 
+  obj_aabb_map[obj] = curr;
+
   updateVelist();
 }
 
@@ -935,158 +939,170 @@ void SaPCollisionManager::setup()
   optimal_axis = axis;
 }
 
-void SaPCollisionManager::update()
+void SaPCollisionManager::update_(SaPAABB* updated_aabb)
 {
-  for(std::list<SaPAABB*>::const_iterator it = AABB_arr.begin(); it != AABB_arr.end(); ++it)
-  {
-    SaPAABB* current = *it;
+  if(updated_aabb->cached.equal(updated_aabb->obj->getAABB()))
+    return;
 
-    Vec3f new_min = current->obj->getAABB().min_;
-    Vec3f new_max = current->obj->getAABB().max_;
+  SaPAABB* current = updated_aabb;
 
-    SaPAABB dummy;
-    dummy.cached = current->obj->getAABB();
+  Vec3f new_min = current->obj->getAABB().min_;
+  Vec3f new_max = current->obj->getAABB().max_;
 
-    EndPoint lo, hi;
-    dummy.lo = &lo;
-    dummy.hi = &hi;
+  SaPAABB dummy;
+  dummy.cached = current->obj->getAABB();
 
-    lo.minmax = 0;
-    lo.aabb = &dummy;
-    hi.minmax = 1;
-    hi.aabb = &dummy;
+  for(int coord = 0; coord < 3; ++coord)
+  {
+    int direction; // -1 reverse, 0 nochange, 1 forward
+    EndPoint* temp;
 
+    if(current->lo->getVal(coord) > new_min[coord])
+      direction = -1;
+    else if(current->lo->getVal(coord) < new_min[coord])
+      direction = 1;
+    else direction = 0;
 
-    for(int coord = 0; coord < 3; ++coord)
+    if(direction == -1)
     {
-      int direction; // -1 reverse, 0 nochange, 1 forward
-      EndPoint* temp;
-
-      if(current->lo->getVal()[coord] > new_min[coord])
-        direction = -1;
-      else if(current->lo->getVal()[coord] < new_min[coord])
-        direction = 1;
-      else direction = 0;
-
-      if(direction == -1)
+      //first update the "lo" endpoint of the interval
+      if(current->lo->prev[coord] != NULL)
       {
-        //first update the "lo" endpoint of the interval
-        if(current->lo->prev[coord] != NULL)
+        temp = current->lo;
+        while((temp != NULL) && (temp->getVal(coord) > new_min[coord]))
         {
-          temp = current->lo;
-          while((temp != NULL) && (temp->getVal()[coord] > new_min[coord]))
-          {
-            if(temp->minmax == 1)
-              if(temp->aabb->cached.overlap(dummy.cached))
-                overlap_pairs.push_back(SaPPair(temp->aabb->obj, current->obj));
-            temp = temp->prev[coord];
-          }
-
-          if(temp == NULL)
-          {
-            current->lo->prev[coord]->next[coord] = current->lo->next[coord];
-            current->lo->next[coord]->prev[coord] = current->lo->prev[coord];
-            current->lo->prev[coord] = NULL;
-            current->lo->next[coord] = elist[coord];
-            elist[coord]->prev[coord] = current->lo;
-            elist[coord] = current->lo;
-          }
-          else
-          {
-            current->lo->prev[coord]->next[coord] = current->lo->next[coord];
-            current->lo->next[coord]->prev[coord] = current->lo->prev[coord];
-            current->lo->prev[coord] = temp;
-            current->lo->next[coord] = temp->next[coord];
-            temp->next[coord]->prev[coord] = current->lo;
-            temp->next[coord] = current->lo;
-          }
+          if(temp->minmax == 1)
+            if(temp->aabb->cached.overlap(dummy.cached))
+              addToOverlapPairs(SaPPair(temp->aabb->obj, current->obj));
+          temp = temp->prev[coord];
         }
 
-        current->lo->getVal()[coord] = new_min[coord];
-
-        // update hi end point
-        temp = current->hi;
-        while(temp->getVal()[coord] > new_max[coord])
+        if(temp == NULL)
         {
-          if((temp->minmax == 0) && (temp->aabb->cached.overlap(current->cached)))
-            overlap_pairs.remove_if(isNotValidPair(temp->aabb->obj, current->obj));
-          temp = temp->prev[coord];
+          current->lo->prev[coord]->next[coord] = current->lo->next[coord];
+          current->lo->next[coord]->prev[coord] = current->lo->prev[coord];
+          current->lo->prev[coord] = NULL;
+          current->lo->next[coord] = elist[coord];
+          elist[coord]->prev[coord] = current->lo;
+          elist[coord] = current->lo;
         }
-
-        current->hi->prev[coord]->next[coord] = current->hi->next[coord];
-        if(current->hi->next[coord] != NULL)
-          current->hi->next[coord]->prev[coord] = current->hi->prev[coord];
-        current->hi->prev[coord] = temp;
-        current->hi->next[coord] = temp->next[coord];
-        if(temp->next[coord] != NULL)
-          temp->next[coord]->prev[coord] = current->hi;
-        temp->next[coord] = current->hi;
-
-        current->hi->getVal()[coord] = new_max[coord];
-      }
-      else if(direction == 1)
-      {
-        //here, we first update the "hi" endpoint.
-        if(current->hi->next[coord] != NULL)
+        else
         {
-          temp = current->hi;
-          while((temp->next[coord] != NULL) && (temp->getVal()[coord] < new_max[coord]))
-          {
-            if(temp->minmax == 0)
-              if(temp->aabb->cached.overlap(dummy.cached))
-                overlap_pairs.push_back(SaPPair(temp->aabb->obj, current->obj));
-            temp = temp->next[coord];
-          }
-
-          if(temp->getVal()[coord] < new_max[coord])
-          {
-            current->hi->prev[coord]->next[coord] = current->hi->next[coord];
-            current->hi->next[coord]->prev[coord] = current->hi->prev[coord];
-            current->hi->prev[coord] = temp;
-            current->hi->next[coord] = NULL;
-            temp->next[coord] = current->hi;
-          }
-          else
-          {
-            current->hi->prev[coord]->next[coord] = current->hi->next[coord];
-            current->hi->next[coord]->prev[coord] = current->hi->prev[coord];
-            current->hi->prev[coord] = temp->prev[coord];
-            current->hi->next[coord] = temp;
-            temp->prev[coord]->next[coord] = current->hi;
-            temp->prev[coord] = current->hi;
-          }
+          current->lo->prev[coord]->next[coord] = current->lo->next[coord];
+          current->lo->next[coord]->prev[coord] = current->lo->prev[coord];
+          current->lo->prev[coord] = temp;
+          current->lo->next[coord] = temp->next[coord];
+          temp->next[coord]->prev[coord] = current->lo;
+          temp->next[coord] = current->lo;
         }
+      }
 
-        current->hi->getVal()[coord] = new_max[coord];
+      current->lo->getVal(coord) = new_min[coord];
 
-        //then, update the "lo" endpoint of the interval.
-        temp = current->lo;
+      // update hi end point
+      temp = current->hi;
+      while(temp->getVal(coord) > new_max[coord])
+      {
+        if((temp->minmax == 0) && (temp->aabb->cached.overlap(current->cached)))
+          removeFromOverlapPairs(SaPPair(temp->aabb->obj, current->obj));
+        temp = temp->prev[coord];
+      }
+
+      current->hi->prev[coord]->next[coord] = current->hi->next[coord];
+      if(current->hi->next[coord] != NULL)
+        current->hi->next[coord]->prev[coord] = current->hi->prev[coord];
+      current->hi->prev[coord] = temp;
+      current->hi->next[coord] = temp->next[coord];
+      if(temp->next[coord] != NULL)
+        temp->next[coord]->prev[coord] = current->hi;
+      temp->next[coord] = current->hi;
 
-        while(temp->getVal()[coord] < new_min[coord])
+      current->hi->getVal(coord) = new_max[coord];
+    }
+    else if(direction == 1)
+    {
+      //here, we first update the "hi" endpoint.
+      if(current->hi->next[coord] != NULL)
+      {
+        temp = current->hi;
+        while((temp->next[coord] != NULL) && (temp->getVal(coord) < new_max[coord]))
         {
-          if((temp->minmax == 1) && (temp->aabb->cached.overlap(current->cached)))
-            overlap_pairs.remove_if(isNotValidPair(temp->aabb->obj, current->obj));
+          if(temp->minmax == 0)
+            if(temp->aabb->cached.overlap(dummy.cached))
+              addToOverlapPairs(SaPPair(temp->aabb->obj, current->obj));
           temp = temp->next[coord];
         }
 
-        if(current->lo->prev[coord] != NULL)
-          current->lo->prev[coord]->next[coord] = current->lo->next[coord];
-        else
-          elist[coord] = current->lo->next[coord];
-        current->lo->next[coord]->prev[coord] = current->lo->prev[coord];
-        current->lo->prev[coord] = temp->prev[coord];
-        current->lo->next[coord] = temp;
-        if(temp->prev[coord] != NULL)
-          temp->prev[coord]->next[coord] = current->lo;
+        if(temp->getVal(coord) < new_max[coord])
+        {
+          current->hi->prev[coord]->next[coord] = current->hi->next[coord];
+          current->hi->next[coord]->prev[coord] = current->hi->prev[coord];
+          current->hi->prev[coord] = temp;
+          current->hi->next[coord] = NULL;
+          temp->next[coord] = current->hi;
+        }
         else
-          elist[coord] = current->lo;
-        temp->prev[coord] = current->lo;
-        current->lo->getVal()[coord] = new_min[coord];
+        {
+          current->hi->prev[coord]->next[coord] = current->hi->next[coord];
+          current->hi->next[coord]->prev[coord] = current->hi->prev[coord];
+          current->hi->prev[coord] = temp->prev[coord];
+          current->hi->next[coord] = temp;
+          temp->prev[coord]->next[coord] = current->hi;
+          temp->prev[coord] = current->hi;
+        }
       }
+
+      current->hi->getVal(coord) = new_max[coord];
+
+      //then, update the "lo" endpoint of the interval.
+      temp = current->lo;
+
+      while(temp->getVal(coord) < new_min[coord])
+      {
+        if((temp->minmax == 1) && (temp->aabb->cached.overlap(current->cached)))
+          removeFromOverlapPairs(SaPPair(temp->aabb->obj, current->obj));
+        temp = temp->next[coord];
+      }
+
+      if(current->lo->prev[coord] != NULL)
+        current->lo->prev[coord]->next[coord] = current->lo->next[coord];
+      else
+        elist[coord] = current->lo->next[coord];
+      current->lo->next[coord]->prev[coord] = current->lo->prev[coord];
+      current->lo->prev[coord] = temp->prev[coord];
+      current->lo->next[coord] = temp;
+      if(temp->prev[coord] != NULL)
+        temp->prev[coord]->next[coord] = current->lo;
+      else
+        elist[coord] = current->lo;
+      temp->prev[coord] = current->lo;
+      current->lo->getVal(coord) = new_min[coord];
     }
   }
+}
+
+void SaPCollisionManager::update(CollisionObject* updated_obj)
+{
+  update_(obj_aabb_map[updated_obj]);
+
+  updateVelist();
+}
+
+void SaPCollisionManager::update(const std::vector<CollisionObject*>& updated_objs)
+{
+  for(size_t i = 0; i < updated_objs.size(); ++i)
+    update_(obj_aabb_map[updated_objs[i]]);
+
+  updateVelist();
+}
+
+void SaPCollisionManager::update()
+{
+  for(std::list<SaPAABB*>::const_iterator it = AABB_arr.begin(); it != AABB_arr.end(); ++it)
+  {
+    update_(*it);
+  }
 
-  // update velist
   updateVelist();
 }
 
@@ -1113,6 +1129,8 @@ void SaPCollisionManager::clear()
   velist[0].clear();
   velist[1].clear();
   velist[2].clear();
+
+  obj_aabb_map.clear();
 }
 
 void SaPCollisionManager::getObjects(std::vector<CollisionObject*>& objs) const
@@ -1472,6 +1490,26 @@ void IntervalTreeCollisionManager::unregisterObject(CollisionObject* obj)
     if(cur_id < end_id)
       endpoints[2].resize(endpoints[2].size() - 2);
   }
+
+  // update the interval tree
+  if(obj_interval_maps[0].find(obj) != obj_interval_maps[0].end())
+  {
+    SAPInterval* ivl1 = obj_interval_maps[0][obj];
+    SAPInterval* ivl2 = obj_interval_maps[1][obj];
+    SAPInterval* ivl3 = obj_interval_maps[2][obj];
+
+    interval_trees[0]->deleteNode(ivl1);
+    interval_trees[1]->deleteNode(ivl2);
+    interval_trees[2]->deleteNode(ivl3);
+
+    delete ivl1;
+    delete ivl2;
+    delete ivl3;
+
+    obj_interval_maps[0].erase(obj);
+    obj_interval_maps[1].erase(obj);             
+    obj_interval_maps[2].erase(obj);
+  }
 }
 
 void IntervalTreeCollisionManager::registerObject(CollisionObject* obj)
@@ -1522,9 +1560,14 @@ void IntervalTreeCollisionManager::setup()
         SAPInterval* ivl1 = new SAPInterval(obj->getAABB().min_[0], obj->getAABB().max_[0], obj);
         SAPInterval* ivl2 = new SAPInterval(obj->getAABB().min_[1], obj->getAABB().max_[1], obj);
         SAPInterval* ivl3 = new SAPInterval(obj->getAABB().min_[2], obj->getAABB().max_[2], obj);
+
         interval_trees[0]->insert(ivl1);
         interval_trees[1]->insert(ivl2);
         interval_trees[2]->insert(ivl3);
+
+        obj_interval_maps[0][obj] = ivl1;
+        obj_interval_maps[1][obj] = ivl2;
+        obj_interval_maps[2][obj] = ivl3;
       }
     }
 
@@ -1564,11 +1607,80 @@ void IntervalTreeCollisionManager::update()
 
 }
 
+
+void IntervalTreeCollisionManager::update(CollisionObject* updated_obj)
+{
+  AABB old_aabb;
+  const AABB& new_aabb = updated_obj->getAABB();
+  for(int i = 0; i < 3; ++i)
+  {
+    std::map<CollisionObject*, SAPInterval*>::const_iterator it = obj_interval_maps[i].find(updated_obj);
+    interval_trees[i]->deleteNode(it->second);
+    old_aabb.min_[i] = it->second->low;
+    old_aabb.max_[i] = it->second->high;
+    it->second->low = new_aabb.min_[i];
+    it->second->high = new_aabb.max_[i];
+    interval_trees[i]->insert(it->second);
+  }
+
+  EndPoint dummy;
+  std::vector<EndPoint>::iterator it;
+  for(int i = 0; i < 3; ++i)
+  {
+    dummy.value = old_aabb.min_[i];
+    it = std::lower_bound(endpoints[i].begin(), endpoints[i].end(), dummy, boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2));
+    for(; it != endpoints[i].end(); ++it)
+    {
+      if(it->obj == updated_obj && it->minmax == 0)
+      {
+        it->value = new_aabb.min_[i];
+        break;
+      }
+    }
+
+    dummy.value = old_aabb.max_[i];
+    it = std::lower_bound(endpoints[i].begin(), endpoints[i].end(), dummy, boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2));
+    for(; it != endpoints[i].end(); ++it)
+    {
+      if(it->obj == updated_obj && it->minmax == 0)
+      {
+        it->value = new_aabb.max_[i];
+        break;
+      }
+    }         
+
+    std::sort(endpoints[i].begin(), endpoints[i].end(), boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2));
+  }
+}
+
+void IntervalTreeCollisionManager::update(const std::vector<CollisionObject*>& updated_objs)
+{
+  for(size_t i = 0; i < updated_objs.size(); ++i)
+    update(updated_objs[i]);
+}
+
 void IntervalTreeCollisionManager::clear()
 {
   endpoints[0].clear();
   endpoints[1].clear();
   endpoints[2].clear();
+
+  for(int i = 0; i < 3; ++i)
+    obj_interval_maps[i].clear();
+
+  delete interval_trees[0]; interval_trees[0] = NULL;
+  delete interval_trees[1]; interval_trees[1] = NULL;
+  delete interval_trees[2]; interval_trees[2] = NULL;
+
+  for(int i = 0; i < 3; ++i)
+  {
+    for(std::map<CollisionObject*, SAPInterval*>::const_iterator it = obj_interval_maps[i].begin();
+        it != obj_interval_maps[i].end(); ++it)
+    {
+      delete it->second;
+    }
+  }
+
   setup_ = false;
 }
 
@@ -1920,7 +2032,7 @@ void DynamicAABBTreeCollisionManager::registerObjects(const std::vector<Collisio
     table.rehash(other_objs.size());
     for(size_t i = 0; i < other_objs.size(); ++i)
     {
-      DynamicAABBNode* node = new DynamicAABBNode;
+      DynamicAABBNode* node = new DynamicAABBNode; // node will be managed by the dtree
       node->bv = other_objs[i]->getAABB();
       node->parent = NULL;
       node->childs[1] = NULL;
diff --git a/trunk/fcl/src/interval_tree.cpp b/trunk/fcl/src/interval_tree.cpp
index bcbea9e2ef8bdcc228d965efeaf871da2bc954ca..d96dc2fa6a2c41416dfe604d8772845eeb161d2f 100644
--- a/trunk/fcl/src/interval_tree.cpp
+++ b/trunk/fcl/src/interval_tree.cpp
@@ -52,7 +52,6 @@ IntervalTreeNode::IntervalTreeNode(SimpleInterval* new_interval) :
 
 IntervalTreeNode::~IntervalTreeNode() {}
 
-
 IntervalTree::IntervalTree()
 {
   nil = new IntervalTreeNode;
@@ -74,6 +73,43 @@ IntervalTree::IntervalTree()
   recursion_node_stack[0].start_node = NULL;
 }
 
+IntervalTree::~IntervalTree()
+{
+  IntervalTreeNode* x = root->left;
+  std::deque<IntervalTreeNode*> nodes_to_free;
+
+  if(x != nil)
+  {
+    if(x->left != nil)
+    {
+      nodes_to_free.push_back(x->left);
+    }
+    if(x->right != nil)
+    {
+      nodes_to_free.push_back(x->right);
+    }
+
+    delete x;
+    while( nodes_to_free.size() > 0)
+    {
+      x = nodes_to_free.back();
+      nodes_to_free.pop_back();
+      if(x->left != nil)
+      {
+        nodes_to_free.push_back(x->left);
+      }
+      if(x->right != nil)
+      {
+        nodes_to_free.push_back(x->right);
+      }
+      delete x;
+    }
+  }
+  delete nil;
+  delete root;
+  free(recursion_node_stack);
+}
+
 
 void IntervalTree::leftRotate(IntervalTreeNode* x)
 {
@@ -290,43 +326,6 @@ void IntervalTree::recursivePrint(IntervalTreeNode* x) const
   }
 }
 
-IntervalTree::~IntervalTree()
-{
-  IntervalTreeNode* x = root->left;
-  std::deque<IntervalTreeNode*> nodes_to_free;
-
-  if(x != nil)
-  {
-    if(x->left != nil)
-    {
-      nodes_to_free.push_back(x->left);
-    }
-    if(x->right != nil)
-    {
-      nodes_to_free.push_back(x->right);
-    }
-
-    delete x;
-    while( nodes_to_free.size() > 0)
-    {
-      x = nodes_to_free.back();
-      nodes_to_free.pop_back();
-      if(x->left != nil)
-      {
-        nodes_to_free.push_back(x->left);
-      }
-      if(x->right != nil)
-      {
-        nodes_to_free.push_back(x->right);
-      }
-      delete x;
-    }
-  }
-  delete nil;
-  delete root;
-  free(recursion_node_stack);
-}
-
 
 void IntervalTree::print() const
 {
@@ -406,6 +405,29 @@ void IntervalTree::deleteFixup(IntervalTreeNode* x)
   x->red = false;
 }
 
+void IntervalTree::deleteNode(SimpleInterval* ivl)
+{
+  IntervalTreeNode* node = recursiveSearch(root, ivl);
+  if(node)
+    deleteNode(node);
+}
+
+IntervalTreeNode* IntervalTree::recursiveSearch(IntervalTreeNode* node, SimpleInterval* ivl) const
+{
+  if(node != nil)
+  {
+    if(node->stored_interval == ivl)
+      return node;
+    
+    IntervalTreeNode* left = recursiveSearch(node->left, ivl);
+    if(left != nil) return left;
+    IntervalTreeNode* right = recursiveSearch(node->right, ivl);
+    if(right != nil) return right;
+  } 
+  
+  return nil;
+}
+
 SimpleInterval* IntervalTree::deleteNode(IntervalTreeNode* z)
 {
   IntervalTreeNode* y;