diff --git a/include/boost/intrusive/bstree_algorithms.hpp b/include/boost/intrusive/bstree_algorithms.hpp
index b51c5b0..3d47299 100644
--- a/include/boost/intrusive/bstree_algorithms.hpp
+++ b/include/boost/intrusive/bstree_algorithms.hpp
@@ -1,6 +1,7 @@
 /////////////////////////////////////////////////////////////////////////////
 //
 // (C) Copyright Ion Gaztanaga  2007-2014
+// (C) Copyright Daniel Steck   2021
 //
 // Distributed under the Boost Software License, Version 1.0.
 //    (See accompanying file LICENSE_1_0.txt or copy at
@@ -396,38 +397,47 @@ class bstree_algorithms : public bstree_algorithms_base<NodeTraits>
       NodeTraits::set_parent(node1, NodeTraits::get_parent(node2));
       NodeTraits::set_parent(node2, temp);
 
-      //Now adjust adjacent nodes for newly inserted node 1
+      //Now adjust child nodes for newly inserted node 1
       if((temp = NodeTraits::get_left(node1))){
          NodeTraits::set_parent(temp, node1);
       }
       if((temp = NodeTraits::get_right(node1))){
          NodeTraits::set_parent(temp, node1);
       }
-      if((temp = NodeTraits::get_parent(node1)) &&
-         //The header has been already updated so avoid it
-         temp != header2){
-         if(NodeTraits::get_left(temp) == node2){
-            NodeTraits::set_left(temp, node1);
-         }
-         if(NodeTraits::get_right(temp) == node2){
-            NodeTraits::set_right(temp, node1);
-         }
-      }
-      //Now adjust adjacent nodes for newly inserted node 2
+      //Now adjust child nodes for newly inserted node 2
       if((temp = NodeTraits::get_left(node2))){
          NodeTraits::set_parent(temp, node2);
       }
       if((temp = NodeTraits::get_right(node2))){
          NodeTraits::set_parent(temp, node2);
       }
-      if((temp = NodeTraits::get_parent(node2)) &&
-         //The header has been already updated so avoid it
-         temp != header1){
-         if(NodeTraits::get_left(temp) == node1){
-            NodeTraits::set_left(temp, node2);
+
+      //Finally adjust parent nodes
+      if ((temp = NodeTraits::get_parent(node1)) == NodeTraits::get_parent(node2)) {
+         // special logic for the case where the nodes are siblings
+         const node_ptr left = NodeTraits::get_left(temp);
+         NodeTraits::set_left(temp, NodeTraits::get_right(temp));
+         NodeTraits::set_right(temp, left);
+      } else {
+         if ((temp = NodeTraits::get_parent(node1)) &&
+            //The header has been already updated so avoid it
+            temp != header2) {
+            if (NodeTraits::get_left(temp) == node2) {
+               NodeTraits::set_left(temp, node1);
+            }
+            if (NodeTraits::get_right(temp) == node2) {
+               NodeTraits::set_right(temp, node1);
+            }
          }
-         if(NodeTraits::get_right(temp) == node1){
-            NodeTraits::set_right(temp, node2);
+         if ((temp = NodeTraits::get_parent(node2)) &&
+            //The header has been already updated so avoid it
+            temp != header1) {
+            if (NodeTraits::get_left(temp) == node1) {
+               NodeTraits::set_left(temp, node2);
+            }
+            if (NodeTraits::get_right(temp) == node1) {
+               NodeTraits::set_right(temp, node2);
+            }
          }
       }
    }
diff --git a/test/swap_nodes_test.cpp b/test/swap_nodes_test.cpp
new file mode 100644
index 0000000..4603d15
--- /dev/null
+++ b/test/swap_nodes_test.cpp
@@ -0,0 +1,352 @@
+/////////////////////////////////////////////////////////////////////////////
+//
+// (C) Copyright Daniel Steck   2021
+//
+// Distributed under the Boost Software License, Version 1.0.
+//    (See accompanying file LICENSE_1_0.txt or copy at
+//          http://www.boost.org/LICENSE_1_0.txt)
+//
+// See http://www.boost.org/libs/intrusive for documentation.
+//
+/////////////////////////////////////////////////////////////////////////////
+#include <boost/intrusive/rbtree.hpp>
+#include <boost/next_prior.hpp>
+
+#include <cassert>
+#include <vector>
+
+namespace {
+
+struct Node : boost::intrusive::set_base_hook<> {};
+
+typedef boost::intrusive::rbtree<Node> Tree;
+typedef typename Tree::node_traits NodeTraits;
+
+NodeTraits::node_ptr get_parent(const Node& node) {
+   return NodeTraits::get_parent(node.this_ptr());
+}
+
+NodeTraits::node_ptr get_left(const Node& node) {
+   return NodeTraits::get_left(node.this_ptr());
+}
+
+NodeTraits::node_ptr get_right(const Node& node) {
+   return NodeTraits::get_right(node.this_ptr());
+}
+
+void perfect_binary_tree_of_height_2(std::vector<Node>& node_buffer, Tree& tree) {
+   // Perfect binary tree of height 2
+   //            3
+   //          ╱   ╲
+   //         1     5
+   //        ╱ ╲   ╱ ╲
+   //       0   2 4   6
+   assert(node_buffer.size() == 7);
+
+   const Tree::iterator it3 = tree.insert_before(tree.end(), node_buffer[3]);
+   const Tree::iterator it1 = tree.insert_before(it3, node_buffer[1]);
+   const Tree::iterator it5 = tree.insert_before(tree.end(), node_buffer[5]);
+   tree.insert_before(it1, node_buffer[0]);
+   tree.insert_before(it3, node_buffer[2]);
+   tree.insert_before(it5, node_buffer[4]);
+   tree.insert_before(tree.end(), node_buffer[6]);
+
+   // Make sure we got the tree we expected
+   assert(get_parent(node_buffer[0]) == node_buffer[1].this_ptr());
+   assert(get_left(node_buffer[0]) == NULL);
+   assert(get_right(node_buffer[0]) == NULL);
+   assert(get_parent(node_buffer[1]) == node_buffer[3].this_ptr());
+   assert(get_left(node_buffer[1]) == node_buffer[0].this_ptr());
+   assert(get_right(node_buffer[1]) == node_buffer[2].this_ptr());
+   assert(get_parent(node_buffer[2]) == node_buffer[1].this_ptr());
+   assert(get_left(node_buffer[2]) == NULL);
+   assert(get_right(node_buffer[2]) == NULL);
+   assert(get_left(node_buffer[3]) == node_buffer[1].this_ptr());
+   assert(get_right(node_buffer[3]) == node_buffer[5].this_ptr());
+   assert(get_parent(node_buffer[4]) == node_buffer[5].this_ptr());
+   assert(get_left(node_buffer[4]) == NULL);
+   assert(get_right(node_buffer[4]) == NULL);
+   assert(get_parent(node_buffer[5]) == node_buffer[3].this_ptr());
+   assert(get_left(node_buffer[5]) == node_buffer[4].this_ptr());
+   assert(get_right(node_buffer[5]) == node_buffer[6].this_ptr());
+   assert(get_parent(node_buffer[6]) == node_buffer[5].this_ptr());
+   assert(get_left(node_buffer[6]) == NULL);
+   assert(get_right(node_buffer[6]) == NULL);
+}
+
+// Test that swaps node_buffer 0 and 4 and verifies the results
+struct SwapUnrelatedLeafNodesTest {
+   std::vector<Node>& node_buffer;
+   Tree& tree;
+
+   SwapUnrelatedLeafNodesTest(std::vector<Node>& node_buffer, Tree& tree)
+      : node_buffer(node_buffer), tree(tree) {}
+
+   void check() {
+      assert(&*boost::next(tree.begin(), 0) == &node_buffer[4]);
+      assert(&*boost::next(tree.begin(), 1) == &node_buffer[1]);
+      assert(&*boost::next(tree.begin(), 2) == &node_buffer[2]);
+      assert(&*boost::next(tree.begin(), 3) == &node_buffer[3]);
+      assert(&*boost::next(tree.begin(), 4) == &node_buffer[0]);
+      assert(&*boost::next(tree.begin(), 5) == &node_buffer[5]);
+      assert(&*boost::next(tree.begin(), 6) == &node_buffer[6]);
+      assert(get_parent(node_buffer[0]) == node_buffer[5].this_ptr());
+      assert(get_left(node_buffer[0]) == NULL);
+      assert(get_right(node_buffer[0]) == NULL);
+      assert(get_left(node_buffer[1]) == node_buffer[4].this_ptr());
+      assert(get_right(node_buffer[1]) == node_buffer[2].this_ptr());
+      assert(get_parent(node_buffer[4]) == node_buffer[1].this_ptr());
+      assert(get_left(node_buffer[4]) == NULL);
+      assert(get_right(node_buffer[4]) == NULL);
+      assert(get_left(node_buffer[5]) == node_buffer[0].this_ptr());
+      assert(get_right(node_buffer[5]) == node_buffer[6].this_ptr());
+   }
+
+   void swap04() {
+      node_buffer[0].swap_nodes(node_buffer[4]);
+   }
+
+   void swap40() {
+      node_buffer[4].swap_nodes(node_buffer[0]);
+   }
+};
+
+// Test that swaps node_buffer 0 and 2 and verifies the results
+struct SwapSiblingLeafNodesTest {
+   std::vector<Node>& node_buffer;
+   Tree& tree;
+
+   SwapSiblingLeafNodesTest(std::vector<Node>& node_buffer, Tree& tree)
+      : node_buffer(node_buffer), tree(tree) {}
+
+   void check() {
+      assert(&*boost::next(tree.begin(), 0) == &node_buffer[2]);
+      assert(&*boost::next(tree.begin(), 1) == &node_buffer[1]);
+      assert(&*boost::next(tree.begin(), 2) == &node_buffer[0]);
+      assert(&*boost::next(tree.begin(), 3) == &node_buffer[3]);
+      assert(&*boost::next(tree.begin(), 4) == &node_buffer[4]);
+      assert(&*boost::next(tree.begin(), 5) == &node_buffer[5]);
+      assert(&*boost::next(tree.begin(), 6) == &node_buffer[6]);
+      assert(get_parent(node_buffer[0]) == node_buffer[1].this_ptr());
+      assert(get_left(node_buffer[0]) == NULL);
+      assert(get_right(node_buffer[0]) == NULL);
+      assert(get_left(node_buffer[1]) == node_buffer[2].this_ptr());
+      assert(get_right(node_buffer[1]) == node_buffer[0].this_ptr());
+      assert(get_parent(node_buffer[2]) == node_buffer[1].this_ptr());
+      assert(get_left(node_buffer[2]) == NULL);
+      assert(get_right(node_buffer[2]) == NULL);
+   }
+
+   void swap02() {
+      node_buffer[0].swap_nodes(node_buffer[2]);
+   }
+
+   void swap20() {
+      node_buffer[2].swap_nodes(node_buffer[0]);
+   }
+};
+
+// Test that swaps node_buffer 1 and 5 and verifies the results
+struct SwapSiblingNodesTest {
+  std::vector<Node>& node_buffer;
+  Tree& tree;
+
+  SwapSiblingNodesTest(std::vector<Node>& node_buffer, Tree& tree)
+      : node_buffer(node_buffer), tree(tree) {}
+
+  void check() {
+     assert(&*boost::next(tree.begin(), 0) == &node_buffer[0]);
+     assert(&*boost::next(tree.begin(), 1) == &node_buffer[5]);
+     assert(&*boost::next(tree.begin(), 2) == &node_buffer[2]);
+     assert(&*boost::next(tree.begin(), 3) == &node_buffer[3]);
+     assert(&*boost::next(tree.begin(), 4) == &node_buffer[4]);
+     assert(&*boost::next(tree.begin(), 5) == &node_buffer[1]);
+     assert(&*boost::next(tree.begin(), 6) == &node_buffer[6]);
+     assert(get_parent(node_buffer[0]) == node_buffer[5].this_ptr());
+     assert(get_parent(node_buffer[1]) == node_buffer[3].this_ptr());
+     assert(get_left(node_buffer[1]) == node_buffer[4].this_ptr());
+     assert(get_right(node_buffer[1]) == node_buffer[6].this_ptr());
+     assert(get_parent(node_buffer[2]) == node_buffer[5].this_ptr());
+     assert(get_left(node_buffer[3]) == node_buffer[5].this_ptr());
+     assert(get_right(node_buffer[3]) == node_buffer[1].this_ptr());
+     assert(get_parent(node_buffer[4]) == node_buffer[1].this_ptr());
+     assert(get_parent(node_buffer[5]) == node_buffer[3].this_ptr());
+     assert(get_left(node_buffer[5]) == node_buffer[0].this_ptr());
+     assert(get_right(node_buffer[5]) == node_buffer[2].this_ptr());
+     assert(get_parent(node_buffer[6]) == node_buffer[1].this_ptr());
+  }
+
+  void swap15() {
+     node_buffer[1].swap_nodes(node_buffer[5]);
+  }
+
+  void swap51() {
+     node_buffer[5].swap_nodes(node_buffer[1]);
+  }
+};
+
+// Test that swaps node_buffer 0 and 1 and verifies the results
+struct SwapWithLeftChildTest {
+  std::vector<Node>& node_buffer;
+  Tree& tree;
+
+  SwapWithLeftChildTest(std::vector<Node>& node_buffer, Tree& tree)
+      : node_buffer(node_buffer), tree(tree) {}
+
+  void check() {
+     assert(&*boost::next(tree.begin(), 0) == &node_buffer[1]);
+     assert(&*boost::next(tree.begin(), 1) == &node_buffer[0]);
+     assert(&*boost::next(tree.begin(), 2) == &node_buffer[2]);
+     assert(&*boost::next(tree.begin(), 3) == &node_buffer[3]);
+     assert(&*boost::next(tree.begin(), 4) == &node_buffer[4]);
+     assert(&*boost::next(tree.begin(), 5) == &node_buffer[5]);
+     assert(&*boost::next(tree.begin(), 6) == &node_buffer[6]);
+     assert(get_parent(node_buffer[0]) == node_buffer[3].this_ptr());
+     assert(get_left(node_buffer[0]) == node_buffer[1].this_ptr());
+     assert(get_right(node_buffer[0]) == node_buffer[2].this_ptr());
+     assert(get_parent(node_buffer[1]) == node_buffer[0].this_ptr());
+     assert(get_left(node_buffer[1]) == NULL);
+     assert(get_right(node_buffer[1]) == NULL);
+     assert(get_parent(node_buffer[2]) == node_buffer[0].this_ptr());
+     assert(get_left(node_buffer[2]) == NULL);
+     assert(get_right(node_buffer[2]) == NULL);
+  }
+
+  void swap01() {
+     node_buffer[0].swap_nodes(node_buffer[1]);
+  }
+
+  void swap10() {
+     node_buffer[1].swap_nodes(node_buffer[0]);
+  }
+};
+
+// Test that swaps node_buffer 1 and 2 and verifies the results
+struct SwapWithRightChildTest {
+  std::vector<Node>& node_buffer;
+  Tree& tree;
+
+  SwapWithRightChildTest(std::vector<Node>& node_buffer, Tree& tree)
+      : node_buffer(node_buffer), tree(tree) {}
+
+  void check() {
+     assert(&*boost::next(tree.begin(), 0) == &node_buffer[0]);
+     assert(&*boost::next(tree.begin(), 1) == &node_buffer[2]);
+     assert(&*boost::next(tree.begin(), 2) == &node_buffer[1]);
+     assert(&*boost::next(tree.begin(), 3) == &node_buffer[3]);
+     assert(&*boost::next(tree.begin(), 4) == &node_buffer[4]);
+     assert(&*boost::next(tree.begin(), 5) == &node_buffer[5]);
+     assert(&*boost::next(tree.begin(), 6) == &node_buffer[6]);
+     assert(get_parent(node_buffer[0]) == node_buffer[2].this_ptr());
+     assert(get_left(node_buffer[0]) == NULL);
+     assert(get_right(node_buffer[0]) == NULL);
+     assert(get_parent(node_buffer[1]) == node_buffer[2].this_ptr());
+     assert(get_left(node_buffer[1]) == NULL);
+     assert(get_right(node_buffer[1]) == NULL);
+     assert(get_parent(node_buffer[2]) == node_buffer[3].this_ptr());
+     assert(get_left(node_buffer[2]) == node_buffer[0].this_ptr());
+     assert(get_right(node_buffer[2]) == node_buffer[1].this_ptr());
+  }
+
+  void swap12() {
+     node_buffer[1].swap_nodes(node_buffer[2]);
+  }
+
+  void swap21() {
+     node_buffer[2].swap_nodes(node_buffer[1]);
+  }
+};
+
+}
+
+int main() {
+   { // SwapUnrelatedLeafNodesTest 0 -> 4
+      std::vector<Node> node_buffer(7);
+      Tree tree;
+      perfect_binary_tree_of_height_2(node_buffer, tree);
+      SwapUnrelatedLeafNodesTest test(node_buffer, tree);
+      test.swap04();
+      test.check();
+   }
+
+   { // SwapUnrelatedLeafNodesTest 0 <- 4
+      std::vector<Node> node_buffer(7);
+      Tree tree;
+      perfect_binary_tree_of_height_2(node_buffer, tree);
+      SwapUnrelatedLeafNodesTest test(node_buffer, tree);
+      test.swap40();
+      test.check();
+   }
+
+   { // SwapSiblingLeafNodesTest 0 -> 2
+      std::vector<Node> node_buffer(7);
+      Tree tree;
+      perfect_binary_tree_of_height_2(node_buffer, tree);
+      SwapSiblingLeafNodesTest test(node_buffer, tree);
+      test.swap02();
+      test.check();
+   }
+
+   { // SwapSiblingLeafNodesTest 0 <- 2
+      std::vector<Node> node_buffer(7);
+      Tree tree;
+      perfect_binary_tree_of_height_2(node_buffer, tree);
+      SwapSiblingLeafNodesTest test(node_buffer, tree);
+      test.swap20();
+      test.check();
+   }
+
+   { // SwapSiblingNodesTest 1 -> 5
+      std::vector<Node> node_buffer(7);
+      Tree tree;
+      perfect_binary_tree_of_height_2(node_buffer, tree);
+      SwapSiblingNodesTest test(node_buffer, tree);
+      test.swap15();
+      test.check();
+   }
+
+   { // SwapSiblingNodesTest 1 <- 5
+      std::vector<Node> node_buffer(7);
+      Tree tree;
+      perfect_binary_tree_of_height_2(node_buffer, tree);
+      SwapSiblingNodesTest test(node_buffer, tree);
+      test.swap51();
+      test.check();
+   }
+
+   { // SwapWithLeftChildTest 0 -> 1
+      std::vector<Node> node_buffer(7);
+      Tree tree;
+      perfect_binary_tree_of_height_2(node_buffer, tree);
+      SwapWithLeftChildTest test(node_buffer, tree);
+      test.swap01();
+      test.check();
+   }
+
+   { // SwapWithLeftChildTest 0 <- 1
+      std::vector<Node> node_buffer(7);
+      Tree tree;
+      perfect_binary_tree_of_height_2(node_buffer, tree);
+      SwapWithLeftChildTest test(node_buffer, tree);
+      test.swap10();
+      test.check();
+   }
+
+   { // SwapWithRightChildTest 1 -> 2
+      std::vector<Node> node_buffer(7);
+      Tree tree;
+      perfect_binary_tree_of_height_2(node_buffer, tree);
+      SwapWithRightChildTest test(node_buffer, tree);
+      test.swap12();
+      test.check();
+   }
+
+   { // SwapWithRightChildTest 1 <- 2
+      std::vector<Node> node_buffer(7);
+      Tree tree;
+      perfect_binary_tree_of_height_2(node_buffer, tree);
+      SwapWithRightChildTest test(node_buffer, tree);
+      test.swap21();
+      test.check();
+   }
+}