diff --git a/desmume/src/gfx3d.cpp b/desmume/src/gfx3d.cpp
index be7c24b41..e61cffb14 100644
--- a/desmume/src/gfx3d.cpp
+++ b/desmume/src/gfx3d.cpp
@@ -414,6 +414,8 @@ void gfx3d_glStoreMatrix(u32 v)
 	if(mymode==0)
 		v = 0;
 
+	if(v==31) v=30; //? what should happen in this case?
+
 	MatrixStackLoadMatrix (&mtxStack[mymode], v&31, mtxCurrent[mymode]);
 	if(mymode==2)
 		MatrixStackLoadMatrix (&mtxStack[1], v&31, mtxCurrent[1]);
@@ -428,6 +430,8 @@ void gfx3d_glRestoreMatrix(u32 v)
 	if(mymode==0)
 		v = 0;
 
+	if(v==31) v=30; //? what should happen in this case?
+
 	MatrixCopy (mtxCurrent[mymode], MatrixStackGetPos(&mtxStack[mymode], v&31));
 	if (mymode == 2)
 		MatrixCopy (mtxCurrent[1], MatrixStackGetPos(&mtxStack[1], v&31));
diff --git a/desmume/src/matrix.cpp b/desmume/src/matrix.cpp
index 2f9cf21d0..d9c3bd780 100644
--- a/desmume/src/matrix.cpp
+++ b/desmume/src/matrix.cpp
@@ -22,6 +22,7 @@
 #include <string.h>
 #include <stdlib.h>
 #include <math.h>
+#include <assert.h>
 #include "matrix.h"
 
 extern "C" {
@@ -213,6 +214,7 @@ float * MatrixStackPopMatrix (MatrixStack *stack, int size)
 
 float * MatrixStackGetPos (MatrixStack *stack, int pos)
 {
+	assert(pos<31);
 	return &stack->matrix[pos*16];
 }
 
@@ -223,6 +225,7 @@ float * MatrixStackGet (MatrixStack *stack)
 
 void MatrixStackLoadMatrix (MatrixStack *stack, int pos, const float *ptr)
 {
+	assert(pos<31);
 	MatrixCopy (&stack->matrix[pos*16], ptr);
 }