diff --git a/src/NCollection/NCollection_IndexedDataMap.hxx b/src/NCollection/NCollection_IndexedDataMap.hxx
index fdaaf77f05..967324874f 100644
--- a/src/NCollection/NCollection_IndexedDataMap.hxx
+++ b/src/NCollection/NCollection_IndexedDataMap.hxx
@@ -292,16 +292,27 @@ class NCollection_IndexedDataMap : public NCollection_BaseMap
                    const TheKeyType&      theKey1,
                    const TheItemType&     theItem)
   {
-    Standard_OutOfRange_Raise_if (theIndex < 1 || theIndex > Extent(), "NCollection_IndexedDataMap::Substitute");
+    Standard_OutOfRange_Raise_if (theIndex < 1 || theIndex > Extent(),
+                                  "NCollection_IndexedDataMap::Substitute : "
+                                  "Index is out of range");
 
     IndexedDataMapNode * p;
     // check if theKey1 is not already in the map
     Standard_Integer iK1 = Hasher::HashCode (theKey1, NbBuckets());
     p = (IndexedDataMapNode *) myData1[iK1];
-    while (p) 
+    while (p)
     {
-      if (Hasher::IsEqual (p->Key1(), theKey1)) 
-        Standard_DomainError::Raise("NCollection_IndexedDataMap::Substitute");
+      if (Hasher::IsEqual (p->Key1(), theKey1))
+      {
+        if (p->Key2() != theIndex)
+        {
+          Standard_DomainError::Raise ("NCollection_IndexedDataMap::Substitute : "
+                                       "Attempt to substitute existing key");
+        }
+        p->Key1() = theKey1;
+        p->ChangeValue() = theItem;
+        return;
+      }
       p = (IndexedDataMapNode *) p->Next();
     }
 
diff --git a/src/NCollection/NCollection_IndexedMap.hxx b/src/NCollection/NCollection_IndexedMap.hxx
index 277a7c9e4e..4c8bfd78a2 100644
--- a/src/NCollection/NCollection_IndexedMap.hxx
+++ b/src/NCollection/NCollection_IndexedMap.hxx
@@ -263,16 +263,26 @@ class NCollection_IndexedMap : public NCollection_BaseMap
   void Substitute (const Standard_Integer theIndex,
                    const TheKeyType& theKey1)
   {
-    Standard_OutOfRange_Raise_if (theIndex < 1 || theIndex > Extent(), "NCollection_IndexedMap::Substitute");
+    Standard_OutOfRange_Raise_if (theIndex < 1 || theIndex > Extent(),
+                                  "NCollection_IndexedMap::Substitute : "
+                                  "Index is out of range");
 
     IndexedMapNode * p;
     // check if theKey1 is not already in the map
     Standard_Integer iK1 = Hasher::HashCode (theKey1, NbBuckets());
     p = (IndexedMapNode *) myData1[iK1];
-    while (p) 
+    while (p)
     {
-      if (Hasher::IsEqual (p->Key1(), theKey1)) 
-        Standard_DomainError::Raise("NCollection_IndexedMap::Substitute");
+      if (Hasher::IsEqual (p->Key1(), theKey1))
+      {
+        if (p->Key2() != theIndex)
+        {
+          Standard_DomainError::Raise ("NCollection_IndexedMap::Substitute : "
+                                       "Attempt to substitute existing key");
+        }
+        p->Key1() = theKey1;
+        return;
+      }
       p = (IndexedMapNode *) p->Next();
     }
 
diff --git a/src/QANCollection/QANCollection_Test.cxx b/src/QANCollection/QANCollection_Test.cxx
index 616c76b89d..1b3db7cff1 100644
--- a/src/QANCollection/QANCollection_Test.cxx
+++ b/src/QANCollection/QANCollection_Test.cxx
@@ -21,6 +21,8 @@
 
 #include <gp_Pnt.hxx>
 
+#include <Precision.hxx>
+
 #include <NCollection_Vector.hxx>
 
 #define ItemType gp_Pnt
@@ -498,6 +500,16 @@ static void TestIndexedMap  (QANCollection_IndexedMapFunc& theM)
   // Substitute
   Random(aKey);
   aM.Substitute(1,aKey);
+  if (!aM.Contains (aKey) || aM.FindIndex (aKey) != 1)
+  {
+    printf("Error   : map does not contain valid key after substitute"); 
+  }
+  // Invoke substitute with the same key
+  aM.Substitute(1,aKey);
+  if (!aM.Contains (aKey) || aM.FindIndex (aKey) != 1)
+  {
+    printf("Error   : map does not contain valid key after substitute"); 
+  }
   // Copy constructor (including operator=)
   ////////////////////////////////QANCollection_IndexedMap aM2 = QANCollection_IndexedMap(aM);
   QANCollection_IndexedMapFunc aM2 = QANCollection_IndexedMapFunc(aM);
@@ -551,9 +563,20 @@ static void TestIndexedDataMap  (QANCollection_IDMapFunc& theM)
     aM.RemoveLast();
     printf("      successfully removed item, l=%d\n", aM.Size());
   }
-  // Substitute
+  // Substitute with different keys
   Random(aKey);
   aM.Substitute (1, aKey, anItem);
+  if (!aM.Contains (aKey) || aM.FindIndex (aKey) != 1 || !aM.FindFromKey (aKey).IsEqual (anItem, Precision::Confusion()))
+  {
+    printf("Error   : map does not contain valid key and item after substitute"); 
+  }
+  // Substitute with equal keys
+  Random(anItem);
+  aM.Substitute (1, aKey, anItem);
+  if (!aM.Contains (aKey) || aM.FindIndex (aKey) != 1 || !aM.FindFromKey (aKey).IsEqual (anItem, Precision::Confusion()))
+  {
+    printf("Error   : map does not contain valid key and item after substitute"); 
+  }
   // Copy constructor (including operator=)
   ////////////////////////////////theM = QANCollection_IDMap(aM);
   theM = QANCollection_IDMapFunc(aM);
diff --git a/src/TCollection/TCollection_IndexedDataMap.gxx b/src/TCollection/TCollection_IndexedDataMap.gxx
index de6247bf90..83b4b5168c 100644
--- a/src/TCollection/TCollection_IndexedDataMap.gxx
+++ b/src/TCollection/TCollection_IndexedDataMap.gxx
@@ -163,11 +163,11 @@ Standard_Integer TCollection_IndexedDataMap::Add(const TheKey& K1, const TheItem
 //=======================================================================
 
 void TCollection_IndexedDataMap::Substitute(const Standard_Integer I,
-					    const TheKey& K1,
-					    const TheItem& T)
+                                            const TheKey& K1,
+                                            const TheItem& T)
 {
-  Standard_OutOfRange_Raise_if(I < 1 || I > Extent(), 
-			       "IndexedMap::Substitute");
+  Standard_OutOfRange_Raise_if(I < 1 || I > Extent(), "IndexedDataMap::Substitute : "
+                                                      "Index is out of range");
   TCollection_IndexedDataMapNode** data1 = (TCollection_IndexedDataMapNode**)myData1;
   TCollection_IndexedDataMapNode* p;
 
@@ -175,8 +175,14 @@ void TCollection_IndexedDataMap::Substitute(const Standard_Integer I,
   Standard_Integer k1 = Hasher::HashCode(K1,NbBuckets());
   p = data1[k1];
   while (p) {
-    if (Hasher::IsEqual(p->Key1(),K1)) 
-      Standard_DomainError::Raise("IndexedMap::Substitute");
+    if (Hasher::IsEqual(p->Key1(),K1)) {
+      if (p->Key2() != I)
+        Standard_DomainError::Raise("IndexedDataMap::Substitute : "
+                                    "Attempt to substitute existing key");
+      p->Key1() = K1;
+      p->Value() = T;
+      return;
+    }
     p = (TCollection_IndexedDataMapNode*) p->Next();
   }
 
diff --git a/src/TCollection/TCollection_IndexedMap.gxx b/src/TCollection/TCollection_IndexedMap.gxx
index 5e4ecdca49..0b25f3fd64 100644
--- a/src/TCollection/TCollection_IndexedMap.gxx
+++ b/src/TCollection/TCollection_IndexedMap.gxx
@@ -166,10 +166,10 @@ Standard_Integer TCollection_IndexedMap::Add(const TheKey& K1)
 //=======================================================================
 
 void TCollection_IndexedMap::Substitute(const Standard_Integer I,
-					const TheKey& K1)
+                                        const TheKey& K1)
 {
-  Standard_OutOfRange_Raise_if(I < 1 || I > Extent(), 
-			       "IndexedMap::Substitute");
+  Standard_OutOfRange_Raise_if(I < 1 || I > Extent(), "IndexedMap::Substitute : "
+                                                      "Index is out of range");
   TCollection_IndexedMapNode** data1 = (TCollection_IndexedMapNode**)myData1;
   TCollection_IndexedMapNode* p;
 
@@ -177,8 +177,13 @@ void TCollection_IndexedMap::Substitute(const Standard_Integer I,
   Standard_Integer k1 = Hasher::HashCode(K1,NbBuckets());
   p = data1[k1];
   while (p) {
-    if (Hasher::IsEqual(p->Key1(),K1)) 
-      Standard_DomainError::Raise("IndexedMap::Substitute");
+    if (Hasher::IsEqual(p->Key1(),K1)) {
+      if (p->Key2() != I)
+        Standard_DomainError::Raise("IndexedMap::Substitute : "
+                                    "Attempt to substitute existing key");
+      p->Key1() = K1;
+      return;
+    }
     p = (TCollection_IndexedMapNode*) p->Next();
   }