Index: /issm/trunk/src/c/objects/Elements/Penta.cpp
===================================================================
--- /issm/trunk/src/c/objects/Elements/Penta.cpp	(revision 5589)
+++ /issm/trunk/src/c/objects/Elements/Penta.cpp	(revision 5590)
@@ -489,5 +489,4 @@
 	/*Parameters*/
 	double rho_ice,gravity;
-	double surface_normal[3];
 	double bed_normal[3];
 	double bed;
@@ -596,8 +595,5 @@
 
 			/*Get normal vector to the bed */
-			SurfaceNormal(&surface_normal[0],xyz_list_tria);
-			bed_normal[0] = - surface_normal[0]; //Program is for surface, so the normal to the bed is the opposite of the result
-			bed_normal[1] = - surface_normal[1];
-			bed_normal[2] = - surface_normal[2];
+			BedNormal(&bed_normal[0],xyz_list_tria);
 
 			/*basalforce*/
@@ -2285,4 +2281,31 @@
 
 /*Penta specific routines: */
+/*FUNCTION Penta::BedNormal {{{1*/
+void Penta::BedNormal(double* bed_normal, double xyz_list[3][3]){
+
+	int i;
+	double v13[3];
+	double v23[3];
+	double normal[3];
+	double normal_norm;
+
+	for (i=0;i<3;i++){
+		v13[i]=xyz_list[0][i]-xyz_list[2][i];
+		v23[i]=xyz_list[1][i]-xyz_list[2][i];
+	}
+
+	normal[0]=v13[1]*v23[2]-v13[2]*v23[1];
+	normal[1]=v13[2]*v23[0]-v13[0]*v23[2];
+	normal[2]=v13[0]*v23[1]-v13[1]*v23[0];
+
+	normal_norm=sqrt( pow(normal[0],2)+pow(normal[1],2)+pow(normal[2],2) );
+
+	/*Bed normal is opposite to surface normal*/
+	*(bed_normal)=-normal[0]/normal_norm;
+	*(bed_normal+1)=-normal[1]/normal_norm;
+	*(bed_normal+2)=-normal[2]/normal_norm;
+
+}
+/*}}}*/
 /*FUNCTION Penta::CreateKMatrixBalancedthickness {{{1*/
 void  Penta::CreateKMatrixBalancedthickness(Mat Kgg){
@@ -2913,5 +2936,4 @@
 	double     xyz_list[numgrids][3];
 	double	  xyz_list_tria[numgrids2d][3];
-	double	  surface_normal[3];
 	double	  bed_normal[3];
 
@@ -2929,7 +2951,5 @@
 	double     D[8][8]={0.0};
 	double     D_scalar;
-	double     tBD[27][8];
 	double     DLStokes[14][14]={0.0};
-	double     tLDStokes[numdof2d][14];
 
 	/* gaussian points: */
@@ -3041,6 +3061,8 @@
 
 			/*  Do the triple product tB*D*Bprime: */
-			MatrixMultiply(&B[0][0],8,27,1,&D[0][0],8,8,0,&tBD[0][0],0);
-			MatrixMultiply(&tBD[0][0],27,8,0,&B_prime[0][0],8,27,0,&Ke_gaussian[0][0],0);
+			TripleMultiply( &B[0][0],8,27,1,
+						&D[0][0],8,8,0,
+						&B_prime[0][0],8,27,0,
+						&Ke_gaussian[0][0],0);
 
 			/*Add Ke_gaussian and Ke_gaussian to terms in pKe. Watch out for column orientation from matlab: */
@@ -3093,9 +3115,5 @@
 
 			/*Get normal vecyor to the bed */
-			SurfaceNormal(&surface_normal[0],xyz_list_tria);
-
-			bed_normal[0]=-surface_normal[0]; //Program is for surface, so the normal to the bed is the opposite of the result
-			bed_normal[1]=-surface_normal[1];
-			bed_normal[2]=-surface_normal[2];
+			BedNormal(&bed_normal[0],xyz_list_tria);
 
 			/*Calculate DL on gauss point */
@@ -3118,6 +3136,8 @@
 
 			/*  Do the triple product tL*D*L: */
-			MatrixMultiply(&LStokes[0][0],14,numdof2d,1,&DLStokes[0][0],14,14,0,&tLDStokes[0][0],0);
-			MatrixMultiply(&tLDStokes[0][0],numdof2d,14,0,&LprimeStokes[0][0],14,numdof2d,0,&Ke_drag_gaussian[0][0],0);
+			TripleMultiply( &LStokes[0][0],14,numdof2d,1,
+						&DLStokes[0][0],14,14,0,
+						&LprimeStokes[0][0],14,numdof2d,0,
+						&Ke_drag_gaussian[0][0],0);
 
 			for(i=0;i<numdof2d;i++){
@@ -4083,5 +4103,4 @@
 	double		   xyz_list_tria[numgrids2d][3];
 	double         xyz_list[numgrids][3];
-	double		   surface_normal[3];
 	double		   bed_normal[3];
 	double         bed;
@@ -4125,5 +4144,4 @@
 	double     D_scalar;
 	double     tBD[27][8];
-	double     P_terms[numdof]={0.0};
 
 	Tria*            tria=NULL;
@@ -4284,9 +4302,5 @@
 
 			/*Get normal vecyor to the bed */
-			SurfaceNormal(&surface_normal[0],xyz_list_tria);
-
-			bed_normal[0]=-surface_normal[0]; //Program is for surface, so the normal to the bed is the opposite of the result
-			bed_normal[1]=-surface_normal[1];
-			bed_normal[2]=-surface_normal[2];
+			BedNormal(&bed_normal[0],xyz_list_tria);
 
 			for(i=0;i<numgrids2d;i++){
@@ -4301,10 +4315,6 @@
 	ReduceVectorStokes(&Pe_reduced[0], &Ke_temp[0][0], &Pe_temp[0]);
 
-	for(i=0;i<numdof;i++){
-		P_terms[i]+=Pe_reduced[i];
-	}
-
-	/*Add P_terms to global vector pg: */
-	VecSetValues(pg,numdof,doflist,(const double*)P_terms,ADD_VALUES);
+	/*Add Pe_reduced to global vector pg: */
+	VecSetValues(pg,numdof,doflist,(const double*)Pe_reduced,ADD_VALUES);
 
 	/*Free ressources:*/
Index: /issm/trunk/src/c/objects/Elements/Penta.h
===================================================================
--- /issm/trunk/src/c/objects/Elements/Penta.h	(revision 5589)
+++ /issm/trunk/src/c/objects/Elements/Penta.h	(revision 5590)
@@ -123,4 +123,5 @@
 		/*}}}*/
 		/*Penta specific routines:{{{1*/
+		void	  BedNormal(double* bed_normal, double xyz_list[3][3]);
 		void	  CreateKMatrixBalancedthickness(Mat Kggg);
 		void	  CreateKMatrixBalancedvelocities(Mat Kggg);