diff --git a/lib/include/openGJK/openGJK.h b/lib/include/openGJK/openGJK.h
index 49bea49..6d96ef6 100644
--- a/lib/include/openGJK/openGJK.h
+++ b/lib/include/openGJK/openGJK.h
@@ -67,6 +67,8 @@ struct simplex {
   double vrtx    [4][3]; /**< Coordinates of simplex's vertices. 		*/ 
   int    wids    [4];    /**< Label of the simplex's vertices. 			*/  
   double lambdas [4];    /**< Barycentric coordiantes for each vertex.  */
+  double  p [4][3];     /**< Points of P that form the simplex  */  
+  double  q [4][3];	    /**< Points of Q that form the simplex  */ 
 };
 
 
diff --git a/lib/src/openGJK.c b/lib/src/openGJK.c
index a964726..18d383e 100644
--- a/lib/src/openGJK.c
+++ b/lib/src/openGJK.c
@@ -779,6 +779,8 @@ double gjk( struct bd bd1, struct bd bd2, struct simplex *s) {
     bd1.s[i] = bd1.coord[0][i];
     bd2.s[i] = bd2.coord[0][i];
     s->vrtx[0][i] = v[i];
+	s->p[0][i] = bd1.s[i];
+	s->q[0][i] = bd2.s[i];
   }
 
   /* Begin GJK iteration */
@@ -818,6 +820,12 @@ double gjk( struct bd bd1, struct bd bd2, struct simplex *s) {
     /* Invoke distance sub-algorithm */
     subalgorithm ( s, v );
 
+	for (i = 0; i < 3; i++)
+	{
+		s->p[s->nvrtx - 1][i] = bd1.s[i];
+		s->q[s->nvrtx - 1][i] = bd2.s[i];
+	}
+
     /* 3rd exit condition */
     for (i = 0; i < s->nvrtx; i++)
     {