Index: /issm/trunk-jpl/src/c/classes/Loads/Penpair.cpp
===================================================================
--- /issm/trunk-jpl/src/c/classes/Loads/Penpair.cpp	(revision 20659)
+++ /issm/trunk-jpl/src/c/classes/Loads/Penpair.cpp	(revision 20660)
@@ -321,28 +321,25 @@
 ElementMatrix* Penpair::PenaltyCreateKMatrixStressbalanceFS(IssmDouble kmax){/*{{{*/
 
-	const int  numdof=NUMVERTICES*NDOF3;
+	int        numdof,numdof2,N;
 	IssmDouble penalty_offset;
 
 	/*Initialize Element vector and return if necessary*/
-	ElementMatrix* Ke=new ElementMatrix(nodes,NUMVERTICES,this->parameters);
+	ElementMatrix* Ke=new ElementMatrix(nodes,NUMVERTICES,this->parameters,FSvelocityEnum);
 
 	/*recover parameters: */
 	parameters->FindParam(&penalty_offset,StressbalancePenaltyFactorEnum);
 
-	//Create elementary matrix: add penalty to 
-	Ke->values[0*numdof+0]=+kmax*pow(10.,penalty_offset);
-	Ke->values[0*numdof+3]=-kmax*pow(10.,penalty_offset);
-	Ke->values[3*numdof+0]=-kmax*pow(10.,penalty_offset);
-	Ke->values[3*numdof+3]=+kmax*pow(10.,penalty_offset);
-
-	Ke->values[1*numdof+1]=+kmax*pow(10.,penalty_offset);
-	Ke->values[1*numdof+4]=-kmax*pow(10.,penalty_offset);
-	Ke->values[4*numdof+1]=-kmax*pow(10.,penalty_offset);
-	Ke->values[4*numdof+4]=+kmax*pow(10.,penalty_offset);
-
-	Ke->values[2*numdof+2]=+kmax*pow(10.,penalty_offset);
-	Ke->values[2*numdof+5]=-kmax*pow(10.,penalty_offset);
-	Ke->values[5*numdof+2]=-kmax*pow(10.,penalty_offset);
-	Ke->values[5*numdof+5]=+kmax*pow(10.,penalty_offset);
+	/*Get number of dof for these two nodes*/
+	numdof =this->nodes[0]->GetNumberOfDofs(FSApproximationEnum,GsetEnum);
+	numdof2=this->nodes[1]->GetNumberOfDofs(FSApproximationEnum,GsetEnum);
+	N=NUMVERTICES*numdof;
+
+	/*Add penalty to Element matrix*/
+	for(int i=0;i<numdof;i++){
+		Ke->values[         i*N+i       ]=+kmax*pow(10.,penalty_offset);
+		Ke->values[         i*N+numdof+i]=-kmax*pow(10.,penalty_offset);
+		Ke->values[(numdof+i)*N+i       ]=-kmax*pow(10.,penalty_offset);
+		Ke->values[(numdof+i)*N+numdof+i]=+kmax*pow(10.,penalty_offset);
+	}
 
 	/*Clean up and return*/
@@ -397,4 +394,5 @@
 	numdof =this->nodes[0]->GetNumberOfDofs(NoneApproximationEnum,GsetEnum);
 	numdof2=this->nodes[1]->GetNumberOfDofs(NoneApproximationEnum,GsetEnum);
+	_assert_(numdof==numdof2);
 	N=NUMVERTICES*numdof;