Index: issm/trunk-jpl/src/c/analyses/BalancethicknessAnalysis.cpp
===================================================================
--- issm/trunk-jpl/src/c/analyses/BalancethicknessAnalysis.cpp	(revision 16832)
+++ issm/trunk-jpl/src/c/analyses/BalancethicknessAnalysis.cpp	(revision 16843)
@@ -115,5 +115,189 @@
 /*Finite Element Analysis*/
 ElementMatrix* BalancethicknessAnalysis::CreateKMatrix(Element* element){/*{{{*/
-	_error_("not implemented yet");
+
+	if(!element->IsOnBed()) return NULL;
+	Element* basalelement = element->SpawnBasalElement();
+
+	int meshtype;
+	element->FindParam(&meshtype,MeshTypeEnum);
+
+	switch(element->FiniteElement()){
+		case P1Enum: case P2Enum:
+			return CreateKMatrixCG(basalelement);
+		case P1DGEnum:
+			return CreateKMatrixDG(basalelement);
+		default:
+			_error_("Element type " << EnumToStringx(element->FiniteElement()) << " not supported yet");
+	}
+}/*}}}*/
+ElementMatrix* BalancethicknessAnalysis::CreateKMatrixCG(Element* element){/*{{{*/
+
+	/*Intermediaries */
+	int        stabilization;
+	int        meshtype;
+	IssmDouble Jdet,D_scalar,h;
+	IssmDouble vel,vx,vy,dvxdx,dvydy;
+	IssmDouble dvx[2],dvy[2];
+	IssmDouble* xyz_list = NULL;
+
+	/*Fetch number of nodes and dof for this finite element*/
+	int numnodes = element->GetNumberOfNodes();
+
+	/*Initialize Element vector and other vectors*/
+	ElementMatrix* Ke     = element->NewElementMatrix();
+	IssmDouble*    B      = xNew<IssmDouble>(2*numnodes);
+	IssmDouble*    Bprime = xNew<IssmDouble>(2*numnodes);
+	IssmDouble     D[2][2];
+
+	/*Retrieve all inputs and parameters*/
+	element->GetVerticesCoordinates(&xyz_list);
+	element->FindParam(&meshtype,MeshTypeEnum);
+	element->FindParam(&stabilization,BalancethicknessStabilizationEnum);
+	Input* vxaverage_input=NULL;
+	Input* vyaverage_input=NULL;
+	if(meshtype==Mesh2DhorizontalEnum){
+		vxaverage_input=element->GetInput(VxEnum); _assert_(vxaverage_input);
+		vyaverage_input=element->GetInput(VyEnum); _assert_(vyaverage_input);
+	}
+	else{
+		vxaverage_input=element->GetInput(VxAverageEnum); _assert_(vxaverage_input);
+		vyaverage_input=element->GetInput(VyAverageEnum); _assert_(vyaverage_input);
+	}
+	h = element->CharacteristicLength();
+
+	/* Start  looping on the number of gaussian points: */
+	Gauss* gauss=element->NewGauss(2);
+	for(int ig=gauss->begin();ig<gauss->end();ig++){
+		gauss->GaussPoint(ig);
+
+		element->JacobianDeterminant(&Jdet,xyz_list,gauss);
+		GetB(B,element,xyz_list,gauss);
+		GetBprime(Bprime,element,xyz_list,gauss);
+
+		vxaverage_input->GetInputValue(&vx,gauss);
+		vyaverage_input->GetInputValue(&vy,gauss);
+		vxaverage_input->GetInputDerivativeValue(&dvx[0],xyz_list,gauss);
+		vyaverage_input->GetInputDerivativeValue(&dvy[0],xyz_list,gauss);
+		dvxdx=dvx[0];
+		dvydy=dvy[1];
+		D_scalar=gauss->weight*Jdet;
+
+		D[0][0]=D_scalar*dvxdx;
+		D[0][1]=0.;
+		D[1][0]=0.;
+		D[1][1]=D_scalar*dvydy;
+		TripleMultiply(B,2,numnodes,1,
+					&D[0][0],2,2,0,
+					B,2,numnodes,0,
+					&Ke->values[0],1);
+
+		D[0][0]=D_scalar*vx;
+		D[1][1]=D_scalar*vy;
+		TripleMultiply(B,2,numnodes,1,
+					&D[0][0],2,2,0,
+					Bprime,2,numnodes,0,
+					&Ke->values[0],1);
+
+		if(stabilization==1){
+			/*Streamline upwinding*/
+			vel=sqrt(vx*vx+vy*vy);
+			D[0][0]=h/(2*vel)*vx*vx;
+			D[1][0]=h/(2*vel)*vy*vx;
+			D[0][1]=h/(2*vel)*vx*vy;
+			D[1][1]=h/(2*vel)*vy*vy;
+		}
+		else if(stabilization==2){
+			/*SSA*/
+			vxaverage_input->GetInputAverage(&vx);
+			vyaverage_input->GetInputAverage(&vy);
+			D[0][0]=h/2.0*fabs(vx);
+			D[0][1]=0.;
+			D[1][0]=0.;
+			D[1][1]=h/2.0*fabs(vy);
+		}
+		if(stabilization==1 || stabilization==2){
+			D[0][0]=D_scalar*D[0][0];
+			D[1][0]=D_scalar*D[1][0];
+			D[0][1]=D_scalar*D[0][1];
+			D[1][1]=D_scalar*D[1][1];
+			TripleMultiply(Bprime,2,numnodes,1,
+						&D[0][0],2,2,0,
+						Bprime,2,numnodes,0,
+						&Ke->values[0],1);
+		}
+	}
+
+	/*Clean up and return*/
+	xDelete<IssmDouble>(xyz_list);
+	xDelete<IssmDouble>(B);
+	xDelete<IssmDouble>(Bprime);
+	delete gauss;
+	return Ke;
+}/*}}}*/
+ElementMatrix* BalancethicknessAnalysis::CreateKMatrixDG(Element* element){/*{{{*/
+
+	/*Intermediaries */
+	int        meshtype;
+	IssmDouble Jdet,D_scalar,vx,vy,dvxdx,dvydy,vel;
+	IssmDouble dvx[2],dvy[2];
+	IssmDouble* xyz_list = NULL;
+
+	/*Fetch number of nodes and dof for this finite element*/
+	int numnodes = element->GetNumberOfNodes();
+
+	/*Initialize Element vector and other vectors*/
+	ElementMatrix* Ke     = element->NewElementMatrix();
+	IssmDouble*    B      = xNew<IssmDouble>(2*numnodes);
+	IssmDouble*    Bprime = xNew<IssmDouble>(2*numnodes);
+	IssmDouble     D[2][2];
+
+	/*Retrieve all inputs and parameters*/
+	element->GetVerticesCoordinates(&xyz_list);
+	element->FindParam(&meshtype,MeshTypeEnum);
+	Input* vxaverage_input=NULL;
+	Input* vyaverage_input=NULL;
+	if(meshtype==Mesh2DhorizontalEnum){
+		vxaverage_input=element->GetInput(VxEnum); _assert_(vxaverage_input);
+		vyaverage_input=element->GetInput(VyEnum); _assert_(vyaverage_input);
+	}
+	else{
+		vxaverage_input=element->GetInput(VxAverageEnum); _assert_(vxaverage_input);
+		vyaverage_input=element->GetInput(VyAverageEnum); _assert_(vyaverage_input);
+	}
+
+	/* Start  looping on the number of gaussian points: */
+	Gauss* gauss=element->NewGauss(2);
+	for(int ig=gauss->begin();ig<gauss->end();ig++){
+		gauss->GaussPoint(ig);
+
+		element->JacobianDeterminant(&Jdet,xyz_list,gauss);
+		vxaverage_input->GetInputValue(&vx,gauss);
+		vyaverage_input->GetInputValue(&vy,gauss);
+		vxaverage_input->GetInputDerivativeValue(&dvx[0],xyz_list,gauss);
+		vyaverage_input->GetInputDerivativeValue(&dvy[0],xyz_list,gauss);
+		D_scalar=gauss->weight*Jdet;
+
+		/*WARNING: B and Bprime are inverted compared to CG*/
+		GetB(Bprime,element,xyz_list,gauss);
+		GetBprime(B,element,xyz_list,gauss);
+
+		D_scalar = - gauss->weight*Jdet;
+		D[0][0]  = D_scalar*vx;
+		D[0][1]  = 0.;
+		D[1][0]  = 0.;
+		D[1][1]  = D_scalar*vy;
+		TripleMultiply(B,2,numnodes,1,
+					&D[0][0],2,2,0,
+					Bprime,2,numnodes,0,
+					&Ke->values[0],1);
+
+	}
+
+	/*Clean up and return*/
+	xDelete<IssmDouble>(xyz_list);
+	xDelete<IssmDouble>(B);
+	xDelete<IssmDouble>(Bprime);
+	delete gauss;
+	return Ke;
 }/*}}}*/
 ElementVector* BalancethicknessAnalysis::CreatePVector(Element* element){/*{{{*/
@@ -213,4 +397,59 @@
 	delete gauss;
 	return pe;
+}/*}}}*/
+void BalancethicknessAnalysis::GetB(IssmDouble* B,Element* element,IssmDouble* xyz_list,Gauss* gauss){/*{{{*/
+	/*Compute B  matrix. B=[B1 B2 B3] where Bi is of size 3*NDOF2. 
+	 * For node i, Bi can be expressed in the actual coordinate system
+	 * by: 
+	 *       Bi=[ N ]
+	 *          [ N ]
+	 * where N is the finiteelement function for node i.
+	 *
+	 * We assume B_prog has been allocated already, of size: 2x(NDOF1*numnodes)
+	 */
+
+	/*Fetch number of nodes for this finite element*/
+	int numnodes = element->GetNumberOfNodes();
+
+	/*Get nodal functions*/
+	IssmDouble* basis=xNew<IssmDouble>(numnodes);
+	element->NodalFunctions(basis,gauss);
+
+	/*Build B: */
+	for(int i=0;i<numnodes;i++){
+		B[numnodes*0+i] = basis[i];
+		B[numnodes*1+i] = basis[i];
+	}
+
+	/*Clean-up*/
+	xDelete<IssmDouble>(basis);
+}/*}}}*/
+void BalancethicknessAnalysis::GetBprime(IssmDouble* Bprime,Element* element,IssmDouble* xyz_list,Gauss* gauss){/*{{{*/
+	/*Compute B'  matrix. B'=[B1' B2' B3'] where Bi' is of size 3*NDOF2. 
+	 * For node i, Bi' can be expressed in the actual coordinate system
+	 * by: 
+	 *       Bi_prime=[ dN/dx ]
+	 *                [ dN/dy ]
+	 * where N is the finiteelement function for node i.
+	 *
+	 * We assume B' has been allocated already, of size: 3x(NDOF2*numnodes)
+	 */
+
+	/*Fetch number of nodes for this finite element*/
+	int numnodes = element->GetNumberOfNodes();
+
+	/*Get nodal functions derivatives*/
+	IssmDouble* dbasis=xNew<IssmDouble>(2*numnodes);
+	element->NodalFunctionsDerivatives(dbasis,xyz_list,gauss);
+
+	/*Build B': */
+	for(int i=0;i<numnodes;i++){
+		Bprime[numnodes*0+i] = dbasis[0*numnodes+i];
+		Bprime[numnodes*1+i] = dbasis[1*numnodes+i];
+	}
+
+	/*Clean-up*/
+	xDelete<IssmDouble>(dbasis);
+
 }/*}}}*/
 void BalancethicknessAnalysis::GetSolutionFromInputs(Vector<IssmDouble>* solution,Element* element){/*{{{*/
