diff --git a/sources/reactphysics3d/mathematics/Matrix.cpp b/sources/reactphysics3d/mathematics/Matrix.cpp
index fe745c43..69f982f0 100644
--- a/sources/reactphysics3d/mathematics/Matrix.cpp
+++ b/sources/reactphysics3d/mathematics/Matrix.cpp
@@ -272,6 +272,21 @@ Matrix Matrix::identity(int dimension) throw(std::invalid_argument) {
     }
 }
 
+// TODO : Test this method
+// Fill in a sub-matrix of the current matrix with another matrix.
+// This method replaces the sub-matrix with the upper-left index (i,j) in the current matrix by another
+// "subMatrix" matrix.
+void Matrix::fillInSubMatrix(unsigned int rowIndex, unsigned int colIndex, const Matrix& subMatrix) {
+    assert(nbRow-rowIndex >= subMatrix.nbColumn);
+    assert(nbColumn-colIndex >= subMatrix.nbColumn);
+
+    // Fill in the sub-matrix
+    for (unsigned int i=0; i<subMatrix.nbRow; ++i) {
+        for (unsigned int j=0; j<subMatrix.nbColumn; ++j) {
+            array[rowIndex + i][colIndex + j] = subMatrix.array[i][j];
+        }
+    }
+}
 
 // Definition of the operator + for the sum of two matrices with references
 Matrix Matrix::operator+(const Matrix& matrix2) const throw(MathematicsException) {
@@ -406,7 +421,6 @@ bool Matrix::operator==(const Matrix& matrix2) const throw(MathematicsException)
         // Throw an exception because the matrices dimensions aren't the same
         throw MathematicsException("MathematicsException : Impossible to check if the matrices are equal because they don't have the same dimension");
     }
-
 }
 
 // TO DELETE, THIS IS JUST FOR TESTING MATRICES
diff --git a/sources/reactphysics3d/mathematics/Matrix.h b/sources/reactphysics3d/mathematics/Matrix.h
index af45cc06..65245373 100644
--- a/sources/reactphysics3d/mathematics/Matrix.h
+++ b/sources/reactphysics3d/mathematics/Matrix.h
@@ -25,6 +25,8 @@
 #include <stdexcept>
 #include <iostream>
 
+// TODO : Replace the "int" by "unsigned int"
+
 // ReactPhysics3D namespace
 namespace reactphysics3d {
 
@@ -35,8 +37,8 @@ namespace reactphysics3d {
 */
 class Matrix {
     private :
-        int nbRow;                                                                      // Number of row in the matrix
-        int nbColumn;                                                                   // Number of colum in the matrix
+        unsigned int nbRow;                                                             // Number of row in the matrix
+        unsigned int nbColumn;                                                          // Number of colum in the matrix
         double** array;                                                                 // Dynamic array that contains the values of the matrix
 
     public :
@@ -54,6 +56,7 @@ class Matrix {
         double getDeterminant() const throw(MathematicsException);                      // Return the determinant of the matrix
         double getTrace() const throw(MathematicsException);                            // Return the trace of a square matrix
         static Matrix identity(int dimension) throw(std::invalid_argument);             // Return the identity matrix I of the given dimension
+        void fillInSubMatrix(unsigned int i, unsigned int j, const Matrix& subMatrix);  // Fill in a sub-matrix of the current matrix with another matrix
 
         void display() const;                                                           // TO DELETE
 
diff --git a/sources/reactphysics3d/mathematics/Vector.h b/sources/reactphysics3d/mathematics/Vector.h
index 47ac8409..1c510e76 100644
--- a/sources/reactphysics3d/mathematics/Vector.h
+++ b/sources/reactphysics3d/mathematics/Vector.h
@@ -23,6 +23,7 @@
 // Libraries
 #include "exceptions.h"
 #include <cmath>
+#include <cassert>
 #include <iostream>
 
 // ReactPhysics3D namespace
@@ -35,8 +36,8 @@ namespace reactphysics3d {
 */
 class Vector {
     private :
-        double* tab;            // Array of the vector's components
-        int nbComponent;        // number of components in the vector
+        double* tab;                    // Array of the vector's components
+        unsigned int nbComponent;       // number of components in the vector
 
     public :
         Vector();                                                                       // Constructor without argument
@@ -50,6 +51,7 @@ class Vector {
         Vector getUnit() const throw(MathematicsException);                             // Return the corresponding unit vector
         double scalarProduct(const Vector& vector) const throw(MathematicsException);   // Scalar product of two vectors
         Vector crossProduct(const Vector& vector) const throw(MathematicsException);    // Cross product of two vectors (in 3D only)
+        void fillInSubVector(unsigned int index, const Vector& subVector);              // Replace a part of the current vector with another sub-vector
 
         // --- Overloaded operators --- //
         Vector operator+(const Vector& vector) const throw(MathematicsException);   // Overloaded operator for addition
@@ -106,6 +108,17 @@ inline double Vector::length() const {
     return sqrt(sum);
 }
 
+// Replace a part of the current vector with another sub-vector.
+// The argument "rowIndex" is the row index where the subVector starts.
+inline void Vector::fillInSubVector(unsigned int rowIndex, const Vector& subVector) {
+    assert(nbComponent-rowIndex >= subVector.nbComponent);
+
+    // For each value of the sub-vector
+    for (unsigned int i=0; i<subVector.nbComponent; ++i) {
+        tab[rowIndex + i] = subVector.getValue(i);
+    }
+}
+
 // Overloaded operator for multiplication between a number and a Vector (inline)
 inline Vector operator*(double number, const Vector& vector) {
     // Compute and return the result