Index: /issm/trunk/src/c/objects/Penta.cpp
===================================================================
--- /issm/trunk/src/c/objects/Penta.cpp	(revision 91)
+++ /issm/trunk/src/c/objects/Penta.cpp	(revision 92)
@@ -14,4 +14,5 @@
 #include "../EnumDefinitions/EnumDefinitions.h"
 #include "../shared/shared.h"
+#include "../include/typedefs.h"
 
 Penta::Penta(){
@@ -31,4 +32,6 @@
 	for(i =0;i<6;i++){
 		node_ids[i] = penta_node_ids[i]; 
+		node_offsets[i]=UNDEF;
+		nodes[i]=NULL;
 		h[i] = penta_h[i]; 
 		s[i] = penta_s[i]; 
@@ -39,4 +42,8 @@
 		geothermalflux[i] = penta_geothermalflux[i]; 
 	}
+	matice=NULL;
+	matice_offset=UNDEF;
+	matpar=NULL;
+	matpar_offset=UNDEF;
 
 	friction_type = penta_friction_type; 
@@ -65,5 +72,9 @@
 	printf("   mparid: %i\n",mparid);
 
-	printf("   nodes=[%i,%i,%i,%i,%i,%i]\n",node_ids[0],node_ids[1],node_ids[2],node_ids[3],node_ids[4],node_ids[5]);
+	printf("   node_ids=[%i,%i,%i,%i,%i,%i]\n",node_ids[0],node_ids[1],node_ids[2],node_ids[3],node_ids[4],node_ids[5]);
+	printf("   node_offsets=[%i,%i,%i,%i,%i,%i]\n",node_offsets[0],node_offsets[1],node_offsets[2],node_offsets[3],node_offsets[4],node_offsets[5]);
+	printf("   matice_offset=%i\n",matice_offset);
+	printf("   matpar_offset=%i\n",matpar_offset);
+
 	printf("   h=[%i,%i,%i,%i,%i,%i]\n",h[0],h[1],h[2],h[3],h[4],h[5]);
 	printf("   s=[%i,%i,%i,%i,%i,%i]\n",s[0],s[1],s[2],s[3],s[4],s[5]);
@@ -108,4 +119,10 @@
 	memcpy(marshalled_dataset,&mparid,sizeof(mparid));marshalled_dataset+=sizeof(mparid);
 	memcpy(marshalled_dataset,&node_ids,sizeof(node_ids));marshalled_dataset+=sizeof(node_ids);
+	memcpy(marshalled_dataset,&nodes,sizeof(nodes));marshalled_dataset+=sizeof(nodes);
+	memcpy(marshalled_dataset,&node_offsets,sizeof(node_offsets));marshalled_dataset+=sizeof(node_offsets);
+	memcpy(marshalled_dataset,&matice,sizeof(matice));marshalled_dataset+=sizeof(matice);
+	memcpy(marshalled_dataset,&matice_offset,sizeof(matice_offset));marshalled_dataset+=sizeof(matice_offset);
+	memcpy(marshalled_dataset,&matpar,sizeof(matpar));marshalled_dataset+=sizeof(matpar);
+	memcpy(marshalled_dataset,&matpar_offset,sizeof(matpar_offset));marshalled_dataset+=sizeof(matpar_offset);
 	memcpy(marshalled_dataset,&h,sizeof(h));marshalled_dataset+=sizeof(h);
 	memcpy(marshalled_dataset,&s,sizeof(s));marshalled_dataset+=sizeof(s);
@@ -133,5 +150,33 @@
 int   Penta::MarshallSize(){
 
-	return sizeof(id)+sizeof(mid)+sizeof(mparid)+sizeof(node_ids)+sizeof(h)+sizeof(s)+sizeof(b)+sizeof(k)+sizeof(friction_type)+sizeof(p)+sizeof(q)+sizeof(shelf)+sizeof(onbed)+sizeof(onsurface)+sizeof(meanvel)+sizeof(epsvel)+sizeof(collapse)+sizeof(melting)+sizeof(accumulation)+sizeof(geothermalflux)+sizeof(artdiff)+sizeof(thermal_steadystate) +sizeof(int); //sizeof(int) for enum type
+	return sizeof(id)+
+		sizeof(mid)+
+		sizeof(mparid)+
+		sizeof(node_ids)+
+		sizeof(nodes)+
+		sizeof(node_offsets)+
+		sizeof(matice)+
+		sizeof(matice_offset)+
+		sizeof(matpar)+
+		sizeof(matpar_offset)+
+		sizeof(h)+
+		sizeof(s)+
+		sizeof(b)+
+		sizeof(k)+
+		sizeof(friction_type)+
+		sizeof(p)+
+		sizeof(q)+
+		sizeof(shelf)+
+		sizeof(onbed)+
+		sizeof(onsurface)+
+		sizeof(meanvel)+
+		sizeof(epsvel)+
+		sizeof(collapse)+
+		sizeof(melting)+
+		sizeof(accumulation)+
+		sizeof(geothermalflux)+
+		sizeof(artdiff)+
+		sizeof(thermal_steadystate) +
+		sizeof(int); //sizeof(int) for enum type
 }
 
@@ -142,4 +187,5 @@
 void  Penta::Demarshall(char** pmarshalled_dataset){
 
+	int i;
 	char* marshalled_dataset=NULL;
 
@@ -154,4 +200,10 @@
 	memcpy(&mparid,marshalled_dataset,sizeof(mparid));marshalled_dataset+=sizeof(mparid);
 	memcpy(&node_ids,marshalled_dataset,sizeof(node_ids));marshalled_dataset+=sizeof(node_ids);
+	memcpy(&nodes,marshalled_dataset,sizeof(nodes));marshalled_dataset+=sizeof(nodes);
+	memcpy(&node_offsets,marshalled_dataset,sizeof(node_offsets));marshalled_dataset+=sizeof(node_offsets);
+	memcpy(&matice,marshalled_dataset,sizeof(matice));marshalled_dataset+=sizeof(matice);
+	memcpy(&matice_offset,marshalled_dataset,sizeof(matice_offset));marshalled_dataset+=sizeof(matice_offset);
+	memcpy(&matpar,marshalled_dataset,sizeof(matpar));marshalled_dataset+=sizeof(matpar);
+	memcpy(&matpar_offset,marshalled_dataset,sizeof(matpar_offset));marshalled_dataset+=sizeof(matpar_offset);
 	memcpy(&h,marshalled_dataset,sizeof(h));marshalled_dataset+=sizeof(h);
 	memcpy(&s,marshalled_dataset,sizeof(s));marshalled_dataset+=sizeof(s);
@@ -173,4 +225,9 @@
 	memcpy(&thermal_steadystate,marshalled_dataset,sizeof(thermal_steadystate));marshalled_dataset+=sizeof(thermal_steadystate);
 
+	/*nodes and materials are not pointing to correct objects anymore:*/
+	for(i=0;i<6;i++)nodes[i]=NULL;
+	matice=NULL;
+	matpar=NULL;
+
 	/*return: */
 	*pmarshalled_dataset=marshalled_dataset;
@@ -189,11 +246,29 @@
 }
 
+
 #undef __FUNCT__ 
 #define __FUNCT__ "Penta::Configure"
-void  Penta::Configure(void* loads,void* nodes,void* materials){
-
-	throw ErrorException(__FUNCT__," not supported yet!");
-
-}
+void  Penta::Configure(void* ploadsin,void* pnodesin,void* pmaterialsin){
+
+	int i;
+	
+	DataSet* loadsin=NULL;
+	DataSet* nodesin=NULL;
+	DataSet* materialsin=NULL;
+
+	/*Recover pointers :*/
+	loadsin=(DataSet*)ploadsin;
+	nodesin=(DataSet*)pnodesin;
+	materialsin=(DataSet*)pmaterialsin;
+
+	/*Link this element with its nodes, ie find pointers to the nodes in the nodes dataset.: */
+	ResolvePointers((Object**)nodes,node_ids,node_offsets,6,nodesin);
+	
+	/*Same for materials: */
+	ResolvePointers((Object**)&matice,&mid,&matice_offset,1,materialsin);
+	ResolvePointers((Object**)&matpar,&mparid,&matpar_offset,1,materialsin);
+
+}
+
 #undef __FUNCT__ 
 #define __FUNCT__ "Penta::CreateKMatrix"
@@ -201,6 +276,266 @@
 void  Penta::CreateKMatrix(Mat Kgg,ParameterInputs* inputs,int analysis_type){
 
-	throw ErrorException(__FUNCT__," not supported yet!");
-
+	/*Just branch to the correct element stiffness matrix generator, according to the type of analysis we are carrying out: */
+	if ((analysis_type==DiagnosticHorizAnalysisEnum()) || (analysis_type==ControlAnalysisEnum())){
+
+		CreateKMatrixDiagnosticHoriz( Kgg,inputs,analysis_type);
+
+	}
+	else{
+		throw ErrorException(__FUNCT__,exprintf("%s%i%s\n","analysis: ",analysis_type," not supported yet"));
+	}
+
+}
+
+
+#undef __FUNCT__ 
+#define __FUNCT__ "Penta:CreateKMatrixDiagnosticHoriz"
+void Penta::CreateKMatrixDiagnosticHoriz( Mat Kgg, ParameterInputs* inputs, int analysis_type){
+
+
+	/* local declarations */
+	int             i,j;
+
+	/* node data: */
+	const int    numgrids=6;
+	const int    numdofs=2*numgrids;
+	double       xyz_list[numgrids][3];
+	int          doflist[numdofs];
+	int          numberofdofspernode;
+	
+	
+	/* 3d gaussian points: */
+	int     num_gauss,ig;
+	double* first_gauss_area_coord  =  NULL;
+	double* second_gauss_area_coord =  NULL;
+	double* third_gauss_area_coord  =  NULL;
+	double* fourth_gauss_vert_coord  =  NULL;
+	double* area_gauss_weights           =  NULL;
+	double* vert_gauss_weights           =  NULL;
+	int     ig1,ig2;
+	double  gauss_weight1,gauss_weight2;
+	double  gauss_l1l2l3l4[4];
+	int     order_area_gauss;
+	int     num_vert_gauss;
+	int     num_area_gauss;
+	double  gauss_weight;
+	
+	/* 2d gaussian point: */
+	int     num_gauss2d;
+	double* first_gauss_area_coord2d  =  NULL;
+	double* second_gauss_area_coord2d =  NULL;
+	double* third_gauss_area_coord2d  =  NULL;
+	double* gauss_weights2d=NULL;
+	double  gauss_l1l2l3[3];
+
+	/* material data: */
+	double viscosity; //viscosity
+
+	/* strain rate: */
+	double epsilon[5]; /* epsilon=[exx,eyy,exy,exz,eyz];*/
+
+	/* matrices: */
+	double B[5][numdofs];
+	double Bprime[5][numdofs];
+	double L[2][numdofs];
+	double D[5][5]={{ 0,0,0,0,0 },{0,0,0,0,0},{0,0,0,0,0},{0,0,0,0,0},{0,0,0,0,0}};              // material matrix, simple scalar matrix.
+	double D_scalar;
+	double DL[2][2]={{ 0,0 },{0,0}}; //for basal drag
+	double DL_scalar;
+
+	/* local element matrices: */
+	double Ke_gg[numdofs][numdofs]; //local element stiffness matrix 
+	double Ke_gg_gaussian[numdofs][numdofs]; //stiffness matrix evaluated at the gaussian point.
+	double Ke_gg_drag_gaussian[numdofs][numdofs]; //stiffness matrix contribution from drag
+	double Jdet;
+	
+	/*slope: */
+	double  slope[2]={0.0,0.0};
+	double  slope_magnitude;
+
+	/*input parameters for structural analysis (diagnostic): */
+	double* velocity_param=NULL;
+	double  vxvy_list[numgrids][2];
+	double  thickness;
+
+	/*friction: */
+	double  alpha2_list[3];
+	double  alpha2;
+
+	double MAXSLOPE=.06; // 6 %
+	double MOUNTAINKEXPONENT=10;
+
+	/*Collapsed formulation: */
+	Tria*  tria=NULL;
+
+	/*Figure out if this pentaelem is collapsed. If so, then bailout, except if it is at the 
+	  bedrock, in which case we spawn a tria element using the 3 first grids, and use it to build 
+	  the stiffness matrix. */
+
+	if ((collapse==1) && (onbed==0)){
+		/*This element should be collapsed, but this element is not on the bedrock, therefore all its 
+		 * dofs have already been frozen! Do nothing: */
+		return;
+	}
+	else if ((collapse==1) && (onbed==1)){
+
+		/*This element should be collapsed into a tria element at its base. Create this tria element, 
+		 *and use its CreateKMatrix functionality to fill the global stiffness matrix: */
+		tria=SpawnTria(0,1,2); //grids 0, 1 and 2 make the new tria.
+		tria->CreateKMatrix(Kgg,inputs, analysis_type);
+		delete tria;
+		return;
+	}
+	else{
+
+		/*Implement standard penta element: */
+
+		/*recover extra inputs from users, at current convergence iteration: */
+		velocity_param=ParameterInputsRecover(inputs,"velocity"); 
+
+		/* Get node coordinates and dof list: */
+		GetElementNodeData( &xyz_list[0][0], nodes, numgrids);
+		GetDofList(&doflist[0],&numberofdofspernode);
+
+		/* Set Ke_gg to 0: */
+		for(i=0;i<numdofs;i++){
+			for(j=0;j<numdofs;j++){
+				Ke_gg[i][j]=0.0;
+			}
+		}
+
+		/*Initialize vxvy_list: */
+		for(i=0;i<numgrids;i++){
+			if(velocity_param){
+				vxvy_list[i][0]=velocity_param[doflist[i*numberofdofspernode+0]];
+				vxvy_list[i][1]=velocity_param[doflist[i*numberofdofspernode+1]];
+			}
+			else{
+				vxvy_list[i][0]=0;
+				vxvy_list[i][1]=0;
+			}
+		}
+			
+		#ifdef _DEBUGELEMENTS_
+		if(my_rank==RANK && id==ELID){ 
+			printf("El id %i Rank %i PentaElement input list before gaussian loop: \n",ELID,RANK); 
+			printf("   rho_ice: %g\n",matice->GetRhoIce());
+			printf("   rho_water:%g \n",matice->GetRhoWater());
+			printf("   gravity: %g\n",matpar->GetG());
+			printf("   Velocity: \n");
+			for (i=0;i<numgrids;i++){
+				printf("      grid %i  [%g,%g,%g]\n",i,vxvy_list[i][0],vxvy_list[i][1],vxvy_list[i][2]);
+			}
+			printf("   B [%g %g %g %g %g %g]\n",B_list[0],B_list[1],B_list[2],B_list[3],B_list[4],B_list[5]);
+			printf("   K [%g %g %g %g %g %g]\n",K_list[0],K_list[1],K_list[2],K_list[3],K_list[4],K_list[5]);
+			printf("   thickness [%g %g %g %g %g %g]\n",thickness_list[0],thickness_list[1],thickness_list[2],thickness_list[3],thickness_list[4],thickness_list[5]);
+			printf("   surface [%g %g %g %g %g %g]\n",surface_list[0],surface_list[1],surface_list[2],surface_list[3],surface_list[4],surface_list[5]);
+			printf("   bed [%g %g %g %g %g %g]\n",bed_list[0],bed_list[1],bed_list[2],bed_list[3],bed_list[4],bed_list[5]);
+			printf("   temperature_average [%g %g %g %g %g %g]\n",temperature_average_list[0],temperature_average_list[1],temperature_average_list[2],temperature_average_list[3],temperature_average_list[4],temperature_average_list[5]);
+		}
+		#endif
+
+		/*Get gaussian points and weights. Penta is an extrusion of a Tria, we therefore 
+		  get tria gaussian points as well as segment gaussian points. For tria gaussian 
+		  points, order of integration is 2, because we need to integrate the product tB*D*B' 
+		  which is a polynomial of degree 3 (see GaussTria for more details). For segment gaussian 
+		  points, same deal, which yields 3 gaussian points.*/
+
+		order_area_gauss=5;
+		num_vert_gauss=5;
+
+		GaussPenta( &num_area_gauss, &first_gauss_area_coord, &second_gauss_area_coord, &third_gauss_area_coord, &area_gauss_weights, &fourth_gauss_vert_coord,&vert_gauss_weights,order_area_gauss,num_vert_gauss);
+
+		#ifdef _DEBUGGAUSS_
+		if(my_rank==RANK && id==ELID){ 
+			printf("El id %i Rank %i PentaElement gauss points\n",ELID,RANK); 
+			for (i=0;i<num_area_gauss;i++){
+				printf("   Area Gauss coord %i: %lf %lf %lf Weight: %lf\n",i,*(first_gauss_area_coord+i),*(second_gauss_area_coord+i),*(third_gauss_area_coord+i),*(area_gauss_weights+i));
+			}
+			for (i=0;i<num_vert_gauss;i++){
+				printf("   Vert Gauss coord %i: %lf Weight: %lf\n",i,*(fourth_gauss_vert_coord+i),*(vert_gauss_weights+i));
+			}	
+		}
+		#endif
+		/* Start  looping on the number of gaussian points: */
+		for (ig1=0; ig1<num_area_gauss; ig1++){
+			for (ig2=0; ig2<num_vert_gauss; ig2++){
+			
+				/*Pick up the gaussian point: */
+				gauss_weight1=*(area_gauss_weights+ig1);
+				gauss_weight2=*(vert_gauss_weights+ig2);
+				gauss_weight=gauss_weight1*gauss_weight2;
+				
+				
+				gauss_l1l2l3l4[0]=*(first_gauss_area_coord+ig1); 
+				gauss_l1l2l3l4[1]=*(second_gauss_area_coord+ig1);
+				gauss_l1l2l3l4[2]=*(third_gauss_area_coord+ig1);
+				gauss_l1l2l3l4[3]=*(fourth_gauss_vert_coord+ig2);
+
+
+				/*Get strain rate from velocity: */
+				GetStrainRate(&epsilon[0],&vxvy_list[0][0],&xyz_list[0][0],gauss_l1l2l3l4);
+			
+				/*Get viscosity: */
+				matice->GetViscosity3d(&viscosity, &epsilon[0]);
+
+				/*Get B and Bprime matrices: */
+				GetB(&B[0][0], &xyz_list[0][0], gauss_l1l2l3l4);
+				GetBPrime(&Bprime[0][0], &xyz_list[0][0], gauss_l1l2l3l4);
+
+				/* Get Jacobian determinant: */
+				GetJacobianDeterminant(&Jdet, &xyz_list[0][0],gauss_l1l2l3l4);
+	
+				/*Build the D matrix: we plug the gaussian weight, the thickness, the viscosity, and the jacobian determinant 
+				  onto this scalar matrix, so that we win some computational time: */
+				D_scalar=viscosity*gauss_weight*Jdet;
+				for (i=0;i<5;i++){
+					D[i][i]=D_scalar;
+				}
+		
+				/*  Do the triple product tB*D*Bprime: */
+				TripleMultiply( &B[0][0],5,numdofs,1,
+						&D[0][0],5,5,0,
+						&Bprime[0][0],5,numdofs,0,
+						&Ke_gg_gaussian[0][0],0);
+
+				/* Add the Ke_gg_gaussian, and optionally Ke_gg_drag_gaussian onto Ke_gg: */
+				for( i=0; i<numdofs; i++){
+					for(j=0;j<numdofs;j++){
+						Ke_gg[i][j]+=Ke_gg_gaussian[i][j];
+					}
+				}
+			} //for (ig2=0; ig2<num_vert_gauss; ig2++)
+		} //for (ig1=0; ig1<num_area_gauss; ig1++)
+		
+
+		/*Add Ke_gg to global matrix Kgg: */
+		MatSetValues(Kgg,numdofs,doflist,numdofs,doflist,(const double*)Ke_gg,ADD_VALUES);
+	
+		//Deal with 2d friction at the bedrock interface
+		if((onbed && !shelf)){
+
+			/*Build a tria element using the 3 grids of the base of the penta. Then use 
+			 * the tria functionality to build a friction stiffness matrix on these 3
+			 * grids: */
+
+			tria=SpawnTria(0,1,2); //grids 0, 1 and 2 make the new tria.
+			tria->CreateKMatrixDiagnosticHorizFriction(Kgg,inputs,analysis_type);
+			delete tria;
+		}
+
+	} 
+		
+	cleanup_and_return: 
+	xfree((void**)&first_gauss_area_coord);
+	xfree((void**)&second_gauss_area_coord);
+	xfree((void**)&third_gauss_area_coord);
+	xfree((void**)&fourth_gauss_vert_coord);
+	xfree((void**)&area_gauss_weights);
+	xfree((void**)&vert_gauss_weights);
+	xfree((void**)&first_gauss_area_coord2d);
+	xfree((void**)&second_gauss_area_coord2d);
+	xfree((void**)&third_gauss_area_coord2d);
+	xfree((void**)&gauss_weights2d);
 }
 
@@ -208,6 +543,14 @@
 #define __FUNCT__ "Penta::CreatePVector"
 void  Penta::CreatePVector(Vec pg, ParameterInputs* inputs, int analysis_type){
-	
-	throw ErrorException(__FUNCT__," not supported yet!");
+
+	/*Just branch to the correct element stiffness matrix generator, according to the type of analysis we are carrying out: */
+	if ((analysis_type==DiagnosticHorizAnalysisEnum()) || (analysis_type==ControlAnalysisEnum())){
+
+		CreatePVectorDiagnosticHoriz( pg,inputs,analysis_type);
+
+	}
+	else{
+		throw ErrorException(__FUNCT__,exprintf("%s%i%s\n","analysis: ",analysis_type," not supported yet"));
+	}	
 
 }
@@ -215,9 +558,55 @@
 #define __FUNCT__ "Penta::UpdateFromInputs"
 void  Penta::UpdateFromInputs(ParameterInputs* inputs){
-	
-	throw ErrorException(__FUNCT__," not supported yet!");
-
-}
-
+
+	int i;
+	int doflist[MAXDOFSPERNODE*3];
+	int numberofdofspernode;
+
+	double* thickness=NULL;
+	double* bed=NULL;
+	double* surface=NULL;
+	double* drag=NULL;
+	double* temperature_average_param=NULL;
+	double* flow_law_param=NULL;
+	double temperature_average;
+	double B_average;
+
+
+
+	/*Get dof list: */
+	GetDofList(&doflist[0],&numberofdofspernode);
+
+	/*Update internal data if inputs holds new values: */
+	thickness=ParameterInputsRecover(inputs,"thickness");
+	bed=ParameterInputsRecover(inputs,"bed");
+	surface=ParameterInputsRecover(inputs,"surface");
+	drag=ParameterInputsRecover(inputs,"drag");
+
+	for(i=0;i<5;i++){
+		if(thickness)h[i]=thickness[doflist[i*numberofdofspernode+0]];
+		if(bed)b[i]=bed[doflist[i*numberofdofspernode+0]];
+		if(surface)s[i]=surface[doflist[i*numberofdofspernode+0]];
+		if(drag)k[i]=drag[doflist[i*numberofdofspernode+0]];
+	}
+
+	//Update material if necessary
+	temperature_average_param=ParameterInputsRecover(inputs,"temperature_average");
+	flow_law_param=ParameterInputsRecover(inputs,"B");
+	if(temperature_average_param){
+		for(i=0;i<3;i++) temperature_average+=temperature_average_param[doflist[i*numberofdofspernode+0]];
+		temperature_average=temperature_average/3.0;
+		B_average=Paterson(temperature_average);
+		matice->SetB(B_average);
+	}
+
+	//Update material if B is provided.
+	if(flow_law_param){
+		for(i=0;i<3;i++) B_average+=flow_law_param[doflist[i*numberofdofspernode+0]];
+		B_average=B_average/3.0;
+		matice->SetB(B_average);
+	}
+
+}
+		
 Matpar* Penta::GetMatPar(){
 	return matpar;
@@ -259,20 +648,54 @@
 #define __FUNCT__ "Penta::Du"
 void  Penta::Du(Vec du_g,double* u_g,double* u_g_obs,ParameterInputs* inputs,int analysis_type){
-	
-	throw ErrorException(__FUNCT__," not supported yet!");
-
-
+
+	int i;
+	Tria* tria=NULL;
+	
+	/*Bail out if this element does not touch the surface: */
+	if (onsurface){
+		return;
+	}
+	else{
+		
+		tria=SpawnTria(3,4,5); //grids 0, 1 and 2 make the new tria.
+		tria->Du(du_g,u_g,u_g_obs,inputs,analysis_type);
+		delete tria;
+		return;
+	}
 }
 
 #undef __FUNCT__ 
 #define __FUNCT__ "Penta::Gradj"
-void  Penta::Gradj(Vec grad_g,double* u_g,double* lambda_g,ParameterInputs* inputs,int analysis_type,char* control_type){
-	throw ErrorException(__FUNCT__," not supported yet!");
-}
+void  Penta::Gradj(Vec grad_g,double* velocity,double* adjoint,ParameterInputs* inputs,int analysis_type,char* control_type){
+
+	if (strcmp(control_type,"drag")==0){
+		GradjDrag( grad_g,velocity,adjoint,inputs,analysis_type);
+	}
+	else if (strcmp(control_type,"B")==0){
+		GradjB( grad_g, velocity, adjoint, inputs,analysis_type);
+	}
+	else throw ErrorException(__FUNCT__,exprintf("%s%s","control type not supported yet: ",control_type));
+}
+
 #undef __FUNCT__ 
 #define __FUNCT__ "Penta::GradjDrag"
 void  Penta::GradjDrag(Vec grad_g,double* u_g,double* lambda_g,ParameterInputs* inputs,int analysis_type){
-	throw ErrorException(__FUNCT__," not supported yet!");
-}
+	
+	
+	Tria* tria=NULL;
+	
+	/*Bail out if this element does not touch the bedrock: */
+	if (!onbed){
+		return;
+	}
+	else{
+		
+		tria=SpawnTria(0,1,2); //grids 0, 1 and 2 make the new tria.
+		tria->GradjDrag( grad_g,u_g,lambda_g,inputs,analysis_type);
+		delete tria;
+		return;
+	}
+}
+
 #undef __FUNCT__ 
 #define __FUNCT__ "Penta::GradjB"
@@ -283,5 +706,709 @@
 #undef __FUNCT__ 
 #define __FUNCT__ "Penta::Misfit"
-double Penta::Misfit(double* u_g,double* u_g_obs,ParameterInputs* inputs,int analysis_type){
-	throw ErrorException(__FUNCT__," not supported yet!");
-}
+double Penta::Misfit(double* velocity,double* obs_velocity,ParameterInputs* inputs,int analysis_type){
+	
+	double J;
+	Tria* tria=NULL;
+	
+	/*Bail out if this element does not touch the surface: */
+	if (!onsurface){
+		return 0;
+	}
+	else{
+
+		/*use the Tria::CreateMisfit routine, on a Tria Element which is made of the 3 upper grids at the surface of the ice sheet. : */
+		tria=SpawnTria(3,4,5); 
+		J=tria->Misfit( velocity,obs_velocity,inputs,analysis_type);
+		delete tria;
+		return J;
+	}
+}
+		
+#undef __FUNCT__ 
+#define __FUNCT__ "Penta::SpawnTria"
+Tria*  Penta::SpawnTria(int g0, int g1, int g2){
+
+	/*out of grids g0,g1 and g2 from Penta, build a tria element: */
+	Tria* tria=NULL;
+	double   tria_h[3];
+	double   tria_s[3];
+	double   tria_b[3];
+	double   tria_c[3];
+	double   tria_k[3];
+	
+	/*configuration: */
+	int tria_node_ids[3];
+	Node* tria_nodes[3];
+	int tria_node_offsets[3];
+
+	tria_h[0]=h[g0];
+	tria_h[1]=h[g1];
+	tria_h[2]=h[g2];
+
+	tria_s[0]=s[g0];
+	tria_s[1]=s[g1];
+	tria_s[2]=s[g2];
+
+	tria_b[0]=b[g0];
+	tria_b[1]=b[g1];
+	tria_b[2]=b[g2];
+
+	tria_k[0]=k[g0];
+	tria_k[1]=k[g1];
+	tria_k[2]=k[g2];
+
+	tria_node_ids[0]=node_ids[g0];
+	tria_node_ids[1]=node_ids[g1];
+	tria_node_ids[2]=node_ids[g2];
+
+	tria_nodes[0]=nodes[g0]; 
+	tria_nodes[1]=nodes[g1];
+	tria_nodes[2]=nodes[g2];
+
+	tria_node_offsets[0]=node_offsets[g0];
+	tria_node_offsets[1]=node_offsets[g1];
+	tria_node_offsets[2]=node_offsets[g2];
+
+	tria= new Tria(id,mid,mparid,tria_node_ids,tria_h,tria_s,tria_b,tria_k, friction_type,p,q,shelf,meanvel,epsvel);
+
+	tria->NodeConfiguration(tria_node_ids,tria_nodes,tria_node_offsets);
+	tria->MaticeConfiguration(matice,matice_offset);
+	tria->MatparConfiguration(matpar,matpar_offset);
+
+	return tria;
+
+}
+
+
+void  Penta::GetDofList(int* doflist,int* pnumberofdofspernode){
+
+	int i,j;
+	int doflist_per_node[MAXDOFSPERNODE];
+	int numberofdofspernode;
+	
+	for(i=0;i<6;i++){
+		nodes[i]->GetDofList(&doflist_per_node[0],&numberofdofspernode);
+		for(j=0;j<numberofdofspernode;j++){
+			doflist[i*numberofdofspernode+j]=doflist_per_node[j];
+		}
+	}
+
+	/*Assign output pointers:*/
+	*pnumberofdofspernode=numberofdofspernode;
+
+}
+
+
+#undef __FUNCT__ 
+#define __FUNCT__ "Penta::GetStrainRate"
+void Penta::GetStrainRate(double* epsilon, double* velocity, double* xyz_list, double* gauss_l1l2l3l4){
+
+	int i;
+	const int numgrids=6;
+	const int NDOF2=2;
+
+	double B[5][NDOF2*numgrids];
+
+	/*Get B matrix: */
+	GetB(&B[0][0], xyz_list, gauss_l1l2l3l4);
+
+	#ifdef _DEBUG_
+	_printf_("B for grid1 : [ %lf   %lf  \n",B[0][0],B[0][1]);
+	_printf_("              [ %lf   %lf  \n",B[1][0],B[1][1]);
+	_printf_("              [ %lf   %lf  ]\n",B[2][0],B[2][1]);
+	_printf_("              [ %lf   %lf  ]\n",B[3][0],B[3][1]);
+	_printf_("              [ %lf   %lf  ]\n",B[4][0],B[4][1]);
+	
+	_printf_("B for grid2 : [ %lf   %lf  \n",B[0][2],B[0][3]);
+	_printf_("              [ %lf   %lf  \n",B[1][2],B[1][3]);
+	_printf_("              [ %lf   %lf  ]\n",B[2][2],B[2][3]);
+	_printf_("              [ %lf   %lf  ]\n",B[3][2],B[3][3]);
+	_printf_("              [ %lf   %lf  ]\n",B[4][2],B[4][3]);
+	
+	_printf_("B for grid3 : [ %lf   %lf  \n", B[0][4],B[0][5]);
+	_printf_("              [ %lf   %lf  \n", B[1][4],B[1][5]);
+	_printf_("              [ %lf   %lf  ]\n",B[2][4],B[2][5]);
+	_printf_("              [ %lf   %lf  ]\n",B[3][4],B[3][5]);
+	_printf_("              [ %lf   %lf  ]\n",B[4][4],B[4][5]);
+	
+	_printf_("B for grid4 : [ %lf   %lf  \n", B[0][6],B[0][7]);
+	_printf_("              [ %lf   %lf  \n", B[1][6],B[1][7]);
+	_printf_("              [ %lf   %lf  ]\n",B[2][6],B[2][7]);
+	_printf_("              [ %lf   %lf  ]\n",B[3][6],B[3][7]);
+	_printf_("              [ %lf   %lf  ]\n",B[4][6],B[4][7]);
+				
+	_printf_("B for grid5 : [ %lf   %lf  \n", B[0][8],B[0][9]);
+	_printf_("              [ %lf   %lf  \n", B[1][8],B[1][9]);
+	_printf_("              [ %lf   %lf  ]\n",B[2][8],B[2][9]);
+	_printf_("              [ %lf   %lf  ]\n",B[3][8],B[3][9]);
+	_printf_("              [ %lf   %lf  ]\n",B[4][8],B[4][9]);
+
+	_printf_("B for grid6 : [ %lf   %lf  \n", B[0][10],B[0][11]);
+	_printf_("              [ %lf   %lf  \n", B[1][10],B[1][11]);
+	_printf_("              [ %lf   %lf  ]\n",B[2][10],B[2][11]);
+	_printf_("              [ %lf   %lf  ]\n",B[3][10],B[3][11]);
+	_printf_("              [ %lf   %lf  ]\n",B[4][10],B[4][11]);
+
+	for (i=0;i<numgrids;i++){
+		_printf_("Velocity for grid %i %lf %lf\n",i,*(vxvy_list+2*i+0),*(vxvy_list+2*i+1));
+	}
+	#endif
+
+	/*Multiply B by velocity, to get strain rate: */
+	MatrixMultiply( &B[0][0],5,NDOF2*numgrids,0,
+			              velocity,NDOF2*numgrids,1,0,
+						  epsilon,0);
+
+}
+
+#undef __FUNCT__ 
+#define __FUNCT__ "Penta::GetB"
+void Penta::GetB(double* B, double* xyz_list, double* gauss_l1l2l3l4){
+
+	/*Compute B  matrix. B=[B1 B2 B3 B4 B5 B6] where Bi is of size 5*NDOF2. 
+	 * For grid i, Bi can be expressed in the basic coordinate system
+	 * by: 
+	 *       Bi_basic=[ dh/dx          0      ]
+	 *                [   0           dh/dy   ]
+	 *                [ 1/2*dh/dy  1/2*dh/dx  ]
+	 *                [ 1/2*dh/dz      0      ]
+	 *                [  0         1/2*dh/dz  ]
+	 * where h is the interpolation function for grid i.
+	 *
+	 * We assume B has been allocated already, of size: 5x(NDOF2*numgrids)
+	 */
+	
+	int i;
+	const int numgrids=6;
+	const int NDOF3=3;
+	const int NDOF2=2;
+
+	double dh1dh2dh3dh4dh5dh6_basic[NDOF3][numgrids];
+
+	/*Get dh1dh2dh3dh4dh5dh6_basic in basic coordinate system: */
+	GetNodalFunctionsDerivativesBasic(&dh1dh2dh3dh4dh5dh6_basic[0][0],xyz_list, gauss_l1l2l3l4);
+
+	#ifdef _DEBUG_ 
+	for (i=0;i<numgrids;i++){
+		_printf_("Node %i  dh/dx=%lf dh/dy=%lf dh/dz=%lf\n",i,dh1dh2dh3dh4dh5dh6_basic[0][i],dh1dh2dh3dh4dh5dh6_basic[1][i],dh1dh2dh3dh4dh5dh6_basic[2][i]);
+	}
+	#endif
+
+	/*Build B: */
+	for (i=0;i<numgrids;i++){
+		*(B+NDOF2*numgrids*0+NDOF2*i)=dh1dh2dh3dh4dh5dh6_basic[0][i]; 
+		*(B+NDOF2*numgrids*0+NDOF2*i+1)=0.0;
+
+		*(B+NDOF2*numgrids*1+NDOF2*i)=0.0;
+		*(B+NDOF2*numgrids*1+NDOF2*i+1)=dh1dh2dh3dh4dh5dh6_basic[1][i];
+
+		*(B+NDOF2*numgrids*2+NDOF2*i)=(float).5*dh1dh2dh3dh4dh5dh6_basic[1][i]; 
+		*(B+NDOF2*numgrids*2+NDOF2*i+1)=(float).5*dh1dh2dh3dh4dh5dh6_basic[0][i]; 
+
+		*(B+NDOF2*numgrids*3+NDOF2*i)=(float).5*dh1dh2dh3dh4dh5dh6_basic[2][i]; 
+		*(B+NDOF2*numgrids*3+NDOF2*i+1)=0.0;
+		
+		*(B+NDOF2*numgrids*4+NDOF2*i)=0.0;
+		*(B+NDOF2*numgrids*4+NDOF2*i+1)=(float).5*dh1dh2dh3dh4dh5dh6_basic[2][i]; 
+	}
+
+}
+
+
+#undef __FUNCT__ 
+#define __FUNCT__ "Penta::GetBPrime"
+void Penta::GetBPrime(double* B, double* xyz_list, double* gauss_l1l2l3l4){
+
+	/*Compute B  prime matrix. B=[B1 B2 B3 B4 B5 B6] where Bi is of size 5*NDOF2. 
+	 * For grid i, Bi can be expressed in the basic coordinate system
+	 * by: 
+	 *       Bi_basic=[ 2*dh/dx     dh/dy   ]
+	 *                [   dh/dx    2*dh/dy  ]
+	 *                [ dh/dy      dh/dx    ]
+	 *                [ dh/dz         0     ]
+	 *                [  0         dh/dz    ]
+	 * where h is the interpolation function for grid i.
+	 *
+	 * We assume B has been allocated already, of size: 5x(NDOF2*numgrids)
+	 */
+	
+	int i;
+	const int NDOF3=3;
+	const int NDOF2=2;
+	const int numgrids=6;
+
+	double dh1dh2dh3dh4dh5dh6_basic[NDOF3][numgrids];
+
+	/*Get dh1dh2dh3dh4dh5dh6_basic in basic coordinate system: */
+	GetNodalFunctionsDerivativesBasic(&dh1dh2dh3dh4dh5dh6_basic[0][0],xyz_list, gauss_l1l2l3l4);
+
+	#ifdef _DEBUG_ 
+	for (i=0;i<numgrids;i++){
+		_printf_("Node %i  dh/dx=%lf dh/dy=%lf dh/dz=%lf\n",i,dh1dh2dh3dh4dh5dh6_basic[0][i],dh1dh2dh3dh4dh5dh6_basic[1][i],dh1dh2dh3dh4dh5dh6_basic[2][i]);
+	}
+	#endif
+
+	/*Build BPrime: */
+	for (i=0;i<numgrids;i++){
+		*(B+NDOF2*numgrids*0+NDOF2*i)=2.0*dh1dh2dh3dh4dh5dh6_basic[0][i]; 
+		*(B+NDOF2*numgrids*0+NDOF2*i+1)=dh1dh2dh3dh4dh5dh6_basic[1][i];
+
+		*(B+NDOF2*numgrids*1+NDOF2*i)=dh1dh2dh3dh4dh5dh6_basic[0][i];
+		*(B+NDOF2*numgrids*1+NDOF2*i+1)=2.0*dh1dh2dh3dh4dh5dh6_basic[1][i];
+
+		*(B+NDOF2*numgrids*2+NDOF2*i)=dh1dh2dh3dh4dh5dh6_basic[1][i]; 
+		*(B+NDOF2*numgrids*2+NDOF2*i+1)=dh1dh2dh3dh4dh5dh6_basic[0][i]; 
+
+		*(B+NDOF2*numgrids*3+NDOF2*i)=dh1dh2dh3dh4dh5dh6_basic[2][i]; 
+		*(B+NDOF2*numgrids*3+NDOF2*i+1)=0.0;
+		
+		*(B+NDOF2*numgrids*4+NDOF2*i)=0.0;
+		*(B+NDOF2*numgrids*4+NDOF2*i+1)=dh1dh2dh3dh4dh5dh6_basic[2][i]; 
+	}
+}
+
+#undef __FUNCT__ 
+#define __FUNCT__ "Penta::GetJacobianDeterminant" 
+void Penta::GetJacobianDeterminant(double*  Jdet, double* xyz_list,double* gauss_l1l2l3l4){
+
+	/*On a penta, Jacobian varies according to coordinates. We need to get the Jacobian, and take 
+	 * the determinant of it: */
+	const int NDOF3=3;
+	
+	double J[NDOF3][NDOF3];
+
+	GetJacobian(&J[0][0],xyz_list,gauss_l1l2l3l4);
+
+	*Jdet= J[0][0]*J[1][1]*J[2][2]-J[0][0]*J[1][2]*J[2][1]-J[1][0]*J[0][1]*J[2][2]+J[1][0]*J[0][2]*J[2][1]+J[2][0]*J[0][1]*J[1][2]-J[2][0]*J[0][2]*J[1][1];
+	if(*Jdet<0){
+		//printf("%s%s\n",__FUNCT__," error message: negative jacobian determinant!");
+	}
+}
+
+#undef __FUNCT__
+#define __FUNCT__ "Penta::GetNodalFunctionsDerivativesBasic" 
+void Penta::GetNodalFunctionsDerivativesBasic(double* dh1dh2dh3dh4dh5dh6_basic,double* xyz_list, double* gauss_l1l2l3l4){
+	
+	/*This routine returns the values of the nodal functions derivatives  (with respect to the basic coordinate system: */
+
+	
+	int i;
+	const int NDOF3=3;
+	const int numgrids=6;
+
+	double dh1dh2dh3dh4dh5dh6_param[NDOF3][numgrids];
+	double Jinv[NDOF3][NDOF3];
+
+	/*Get derivative values with respect to parametric coordinate system: */
+	GetNodalFunctionsDerivativesParams(&dh1dh2dh3dh4dh5dh6_param[0][0], gauss_l1l2l3l4); 
+
+	/*Get Jacobian invert: */
+	GetJacobianInvert(&Jinv[0][0], xyz_list, gauss_l1l2l3l4);
+
+	/*Build dh1dh2dh3_basic: 
+	 *
+	 * [dhi/dx]= Jinv*[dhi/dr]
+	 * [dhi/dy]       [dhi/ds]
+	 * [dhi/dz]       [dhi/dn]
+	 */
+
+	for (i=0;i<numgrids;i++){
+		*(dh1dh2dh3dh4dh5dh6_basic+numgrids*0+i)=Jinv[0][0]*dh1dh2dh3dh4dh5dh6_param[0][i]+Jinv[0][1]*dh1dh2dh3dh4dh5dh6_param[1][i]+Jinv[0][2]*dh1dh2dh3dh4dh5dh6_param[2][i];
+		*(dh1dh2dh3dh4dh5dh6_basic+numgrids*1+i)=Jinv[1][0]*dh1dh2dh3dh4dh5dh6_param[0][i]+Jinv[1][1]*dh1dh2dh3dh4dh5dh6_param[1][i]+Jinv[1][2]*dh1dh2dh3dh4dh5dh6_param[2][i];
+		*(dh1dh2dh3dh4dh5dh6_basic+numgrids*2+i)=Jinv[2][0]*dh1dh2dh3dh4dh5dh6_param[0][i]+Jinv[2][1]*dh1dh2dh3dh4dh5dh6_param[1][i]+Jinv[2][2]*dh1dh2dh3dh4dh5dh6_param[2][i];
+	}
+
+}
+
+#undef __FUNCT__
+#define __FUNCT__ "Penta::GetJacobian" 
+void Penta::GetJacobian(double* J, double* xyz_list,double* gauss_l1l2l3l4){
+
+	const int NDOF3=3;
+	int i,j;
+	
+	/*The Jacobian is constant over the element, discard the gaussian points. 
+	 * J is assumed to have been allocated of size NDOF2xNDOF2.*/
+
+	double A1,A2,A3; //area coordinates
+	double xi,eta,zi; //parametric coordinates
+
+	double x1,x2,x3,x4,x5,x6;
+	double y1,y2,y3,y4,y5,y6;
+	double z1,z2,z3,z4,z5,z6;
+	
+	double sqrt3=sqrt(3.0);
+	
+	/*Figure out xi,eta and zi (parametric coordinates), for this gaussian point: */
+	A1=gauss_l1l2l3l4[0];
+	A2=gauss_l1l2l3l4[1];
+	A3=gauss_l1l2l3l4[2];
+
+	xi=A2-A1;
+	eta=sqrt3*A3;
+	zi=gauss_l1l2l3l4[3];
+
+	x1=*(xyz_list+3*0+0);
+	x2=*(xyz_list+3*1+0);
+	x3=*(xyz_list+3*2+0);
+	x4=*(xyz_list+3*3+0);
+	x5=*(xyz_list+3*4+0);
+	x6=*(xyz_list+3*5+0);
+	
+	y1=*(xyz_list+3*0+1);
+	y2=*(xyz_list+3*1+1);
+	y3=*(xyz_list+3*2+1);
+	y4=*(xyz_list+3*3+1);
+	y5=*(xyz_list+3*4+1);
+	y6=*(xyz_list+3*5+1);
+
+	z1=*(xyz_list+3*0+2);
+	z2=*(xyz_list+3*1+2);
+	z3=*(xyz_list+3*2+2);
+	z4=*(xyz_list+3*3+2);
+	z5=*(xyz_list+3*4+2);
+	z6=*(xyz_list+3*5+2);
+
+
+	*(J+NDOF3*0+0)=1.0/4.0*(x1-x2-x4+x5)*zi+1.0/4.0*(-x1+x2-x4+x5);
+	*(J+NDOF3*1+0)=sqrt3/12.0*(x1+x2-2*x3-x4-x5+2*x6)*zi+sqrt3/12.0*(-x1-x2+2*x3-x4-x5+2*x6);
+	*(J+NDOF3*2+0)=sqrt3/12.0*(x1+x2-2*x3-x4-x5+2*x6)*eta+1/4*(x1-x2-x4+x5)*xi +1.0/4.0*(-x1+x5-x2+x4);
+
+	*(J+NDOF3*0+1)=1.0/4.0*(y1-y2-y4+y5)*zi+1.0/4.0*(-y1+y2-y4+y5);
+	*(J+NDOF3*1+1)=sqrt3/12.0*(y1+y2-2*y3-y4-y5+2*y6)*zi+sqrt3/12.0*(-y1-y2+2*y3-y4-y5+2*y6);
+	*(J+NDOF3*2+1)=sqrt3/12.0*(y1+y2-2*y3-y4-y5+2*y6)*eta+1.0/4.0*(y1-y2-y4+y5)*xi+1.0/4.0*(y4-y1+y5-y2);
+
+	*(J+NDOF3*0+2)=1.0/4.0*(z1-z2-z4+z5)*zi+1.0/4.0*(-z1+z2-z4+z5);
+	*(J+NDOF3*1+2)=sqrt3/12.0*(z1+z2-2*z3-z4-z5+2*z6)*zi+sqrt3/12.0*(-z1-z2+2*z3-z4-z5+2*z6);
+	*(J+NDOF3*2+2)=sqrt3/12.0*(z1+z2-2*z3-z4-z5+2*z6)*eta+1.0/4.0*(z1-z2-z4+z5)*xi+1.0/4.0*(-z1+z5-z2+z4);
+
+	#ifdef _DEBUG_
+	for(i=0;i<3;i++){
+		for (j=0;j<3;j++){
+			printf("%lf ",*(J+NDOF3*i+j));
+		}
+		printf("\n");
+	}
+	#endif
+}
+
+#undef __FUNCT__
+#define __FUNCT__ "Penta::GetNodalFunctionsDerivativesParams" 
+void Penta::GetNodalFunctionsDerivativesParams(double* dl1dl2dl3dl4dl5dl6,double* gauss_l1l2l3l4){
+	
+	/*This routine returns the values of the nodal functions derivatives  (with respect to the 
+	 * natural coordinate system) at the gaussian point. Those values vary along xi,eta,z */
+
+	const int numgrids=6;
+	double A1,A2,A3,z;
+	double sqrt3=sqrt(3.0);
+	
+	A1=gauss_l1l2l3l4[0]; //first area coordinate value. In term of xi and eta: A1=(1-xi)/2-eta/(2*sqrt(3));
+	A2=gauss_l1l2l3l4[1]; //second area coordinate value In term of xi and eta: A2=(1+xi)/2-eta/(2*sqrt(3));
+	A3=gauss_l1l2l3l4[2]; //third area coordinate value  In term of xi and eta: A3=y/sqrt(3);
+	z=gauss_l1l2l3l4[3]; //fourth vertical coordinate value. Corresponding nodal function: (1-z)/2 and (1+z)/2
+
+
+	/*First nodal function derivatives. The corresponding nodal function is N=A1*(1-z)/2. Its derivatives follow*/
+	*(dl1dl2dl3dl4dl5dl6+numgrids*0+0)=-1.0/2.0*(1.0-z)/2.0;
+	*(dl1dl2dl3dl4dl5dl6+numgrids*1+0)=-1.0/2.0/sqrt3*(1.0-z)/2.0;
+	*(dl1dl2dl3dl4dl5dl6+numgrids*2+0)=-1.0/2.0*A1;
+
+	/*Second nodal function: The corresponding nodal function is N=A2*(1-z)/2. Its derivatives follow*/
+	*(dl1dl2dl3dl4dl5dl6+numgrids*0+1)=1.0/2.0*(1.0-z)/2.0;
+	*(dl1dl2dl3dl4dl5dl6+numgrids*1+1)=-1.0/2.0/sqrt3*(1.0-z)/2.0;
+	*(dl1dl2dl3dl4dl5dl6+numgrids*2+1)=-1.0/2.0*A2;
+
+	/*Third nodal function: The corresponding nodal function is N=A3*(1-z)/2. Its derivatives follow*/
+	*(dl1dl2dl3dl4dl5dl6+numgrids*0+2)=0.0;
+	*(dl1dl2dl3dl4dl5dl6+numgrids*1+2)=1.0/sqrt3*(1.0-z)/2.0;
+	*(dl1dl2dl3dl4dl5dl6+numgrids*2+2)=-1.0/2.0*A3;
+
+	/*Fourth nodal function: The corresponding nodal function is N=A1*(1+z)/2. Its derivatives follow*/
+	*(dl1dl2dl3dl4dl5dl6+numgrids*0+3)=-1.0/2.0*(1.0+z)/2.0;
+	*(dl1dl2dl3dl4dl5dl6+numgrids*1+3)=-1.0/2.0/sqrt3*(1.0+z)/2.0;
+	*(dl1dl2dl3dl4dl5dl6+numgrids*2+3)=1.0/2.0*A1;
+
+	/*Fifth nodal function: The corresponding nodal function is N=A2*(1+z)/2. Its derivatives follow*/
+	*(dl1dl2dl3dl4dl5dl6+numgrids*0+4)=1.0/2.0*(1.0+z)/2.0;
+	*(dl1dl2dl3dl4dl5dl6+numgrids*1+4)=-1.0/2.0/sqrt3*(1.0+z)/2.0;
+	*(dl1dl2dl3dl4dl5dl6+numgrids*2+4)=1.0/2.0*A2;
+
+	/*Sixth nodal function: The corresponding nodal function is N=A3*(1+z)/2. Its derivatives follow*/
+	*(dl1dl2dl3dl4dl5dl6+numgrids*0+5)=0.0;
+	*(dl1dl2dl3dl4dl5dl6+numgrids*1+5)=1.0/sqrt3*(1.0+z)/2.0;
+	*(dl1dl2dl3dl4dl5dl6+numgrids*2+5)=1.0/2.0*A3;
+}
+
+#undef __FUNCT__ 
+#define __FUNCT__ "Penta::GetJacobianInvert"
+void Penta::GetJacobianInvert(double*  Jinv, double* xyz_list,double* gauss_l1l2l3l4){
+
+	double Jdet;
+	const int NDOF3=3;
+
+	/*Call Jacobian routine to get the jacobian:*/
+	GetJacobian(Jinv, xyz_list, gauss_l1l2l3l4);
+
+	/*Invert Jacobian matrix: */
+	MatrixInverse(Jinv,NDOF3,NDOF3,NULL,0,&Jdet);
+}
+
+
+
+#undef __FUNCT__ 
+#define __FUNCT__ "Penta::CreatePVectorDiagnosticHoriz"
+void Penta::CreatePVectorDiagnosticHoriz( Vec pg, ParameterInputs* inputs,int analysis_type){
+
+	int i,j;
+
+	/* node data: */
+	const int    numgrids=6;
+	const int    NDOF2=2;
+	const int    numdofs=NDOF2*numgrids;
+	double       xyz_list[numgrids][3];
+	int          doflist[numdofs];
+	int          numberofdofspernode;
+	
+	/* parameters: */
+	double  slope[NDOF2];
+	double  driving_stress_baseline;
+	double  thickness;
+
+	/* gaussian points: */
+	int     num_gauss,ig;
+	double* first_gauss_area_coord  =  NULL;
+	double* second_gauss_area_coord =  NULL;
+	double* third_gauss_area_coord  =  NULL;
+	double* fourth_gauss_vert_coord  =  NULL;
+	double* area_gauss_weights      =  NULL;
+	double* vert_gauss_weights      =  NULL;
+	double  gauss_l1l2l3l4[4];
+	int     order_area_gauss;
+	int     num_vert_gauss;
+	int     num_area_gauss;
+	int     ig1,ig2;
+	double  gauss_weight1,gauss_weight2;
+	double  gauss_weight;
+
+	/* Jacobian: */
+	double Jdet;
+
+	/*nodal functions: */
+	double l1l2l3l4l5l6[6];
+
+	/*element vector at the gaussian points: */
+	double  pe_g[numdofs];
+	double  pe_g_gaussian[numdofs];
+
+	/*Spawning: */
+	Tria* tria=NULL;
+
+
+	/*Figure out if this pentaelem is collapsed. If so, then bailout, except if it is at the 
+	  bedrock, in which case we spawn a tria element using the 3 first grids, and use it to build 
+	  the load vector. */
+
+	if ((collapse==1) && (onbed==0)){
+		/*This element should be collapsed, but this element is not on the bedrock, therefore all its 
+		 * dofs have already been frozen! Do nothing: */
+		return;
+	}
+	else if ((collapse==1) && (onbed==1)){
+
+		/*This element should be collapsed into a tria element at its base. Create this tria element, 
+		 *and use its CreatePVector functionality to return an elementary load vector: */
+		tria=SpawnTria(0,1,2); //grids 0, 1 and 2 make the new tria.
+		tria->CreatePVector(pg,inputs, analysis_type);
+		delete tria;
+		return;
+	}
+	else{
+
+		/*Implement standard penta element: */
+
+		/* Get node coordinates and dof list: */
+		GetElementNodeData( &xyz_list[0][0], nodes, numgrids);
+		GetDofList(&doflist[0],&numberofdofspernode);
+
+		/* Set pe_g to 0: */
+		for(i=0;i<numdofs;i++) pe_g[i]=0.0;
+
+		/*Get gaussian points and weights :*/
+		order_area_gauss=2;
+		num_vert_gauss=3;
+
+		GaussPenta( &num_area_gauss, &first_gauss_area_coord, &second_gauss_area_coord, &third_gauss_area_coord, &area_gauss_weights, &fourth_gauss_vert_coord,&vert_gauss_weights,order_area_gauss,num_vert_gauss);
+		#ifdef _DEBUG_ 
+		for (i=0;i<num_area_gauss;i++){
+			_printf_("Area Gauss coord %i: %lf %lf %lf Weight: %lf\n",i,*(first_gauss_area_coord+i),*(second_gauss_area_coord+i),*(third_gauss_area_coord+i),*(area_gauss_weights+i));
+		}
+		for (i=0;i<num_vert_gauss;i++){
+			_printf_("Vert Gauss coord %i: %lf Weight: %lf\n",i,*(fourth_gauss_vert_coord+i),*(vert_gauss_weights+i));
+		}
+		#endif
+	
+		/* Start  looping on the number of gaussian points: */
+		for (ig1=0; ig1<num_area_gauss; ig1++){
+			for (ig2=0; ig2<num_vert_gauss; ig2++){
+			
+				/*Pick up the gaussian point: */
+				gauss_weight1=*(area_gauss_weights+ig1);
+				gauss_weight2=*(vert_gauss_weights+ig2);
+				gauss_weight=gauss_weight1*gauss_weight2;
+				
+				gauss_l1l2l3l4[0]=*(first_gauss_area_coord+ig1); 
+				gauss_l1l2l3l4[1]=*(second_gauss_area_coord+ig1);
+				gauss_l1l2l3l4[2]=*(third_gauss_area_coord+ig1);
+				gauss_l1l2l3l4[3]=*(fourth_gauss_vert_coord+ig2);
+		
+				/*Compute thickness at gaussian point: */
+				GetParameterValue(&thickness, &h[0],gauss_l1l2l3l4);
+
+				/*Compute slope at gaussian point: */
+				GetParameterDerivativeValue(&slope[0], &s[0],&xyz_list[0][0], gauss_l1l2l3l4);
+
+				/* Get Jacobian determinant: */
+				GetJacobianDeterminant(&Jdet, &xyz_list[0][0],gauss_l1l2l3l4);
+		
+				 /*Get nodal functions: */
+				GetNodalFunctions(l1l2l3l4l5l6, gauss_l1l2l3l4);
+
+				/*Compute driving stress: */
+				driving_stress_baseline=matpar->GetRhoIce()*matpar->GetG();
+
+				/*Build pe_g_gaussian vector: */
+				for (i=0;i<numgrids;i++){
+					for (j=0;j<NDOF2;j++){
+						pe_g_gaussian[i*NDOF2+j]=-driving_stress_baseline*slope[j]*Jdet*gauss_weight*l1l2l3l4l5l6[i];
+					}
+				}
+
+				/*Add pe_g_gaussian vector to pe_g: */
+				for( i=0; i<numdofs; i++)pe_g[i]+=pe_g_gaussian[i];
+
+			} //for (ig2=0; ig2<num_vert_gauss; ig2++)
+		} //for (ig1=0; ig1<num_area_gauss; ig1++)
+
+	} //else if ((collapse==1) && (onbed==1))
+
+	/*Add pe_g to global vector pg: */
+	VecSetValues(pg,numdofs,doflist,(const double*)pe_g,ADD_VALUES);
+
+	cleanup_and_return: 
+	xfree((void**)&first_gauss_area_coord);
+	xfree((void**)&second_gauss_area_coord);
+	xfree((void**)&third_gauss_area_coord);
+	xfree((void**)&fourth_gauss_vert_coord);
+	xfree((void**)&area_gauss_weights);
+	xfree((void**)&vert_gauss_weights);
+
+}
+
+#undef __FUNCT__ 
+#define __FUNCT__ "Penta::CreateKMatrix"
+void Penta::GetParameterValue(double* pvalue, double* v_list,double* gauss_l1l2l3l4){
+	
+	const int numgrids=6;
+	double l1l2l3l4l5l6[numgrids];
+
+	GetNodalFunctions(&l1l2l3l4l5l6[0], gauss_l1l2l3l4);
+
+	*pvalue=l1l2l3l4l5l6[0]*v_list[0]+l1l2l3l4l5l6[1]*v_list[1]+l1l2l3l4l5l6[2]*v_list[2]+l1l2l3l4l5l6[3]*v_list[3]+l1l2l3l4l5l6[4]*v_list[4]+l1l2l3l4l5l6[5]*v_list[5];
+}
+
+#undef __FUNCT__ 
+#define __FUNCT__ "Penta::GetParameterDerivativeValue"
+void Penta::GetParameterDerivativeValue(double* p, double* p_list,double* xyz_list, double* gauss_l1l2l3l4){
+				
+	/*From grid values of parameter p (p_list[0], p_list[1], p_list[2], p_list[3], p_list[4] and p_list[4]), return parameter derivative value at gaussian point specified by gauss_l1l2l3l4:
+	 *   dp/dx=p_list[0]*dh1/dx+p_list[1]*dh2/dx+p_list[2]*dh3/dx+p_list[3]*dh4/dx+p_list[4]*dh5/dx+p_list[5]*dh6/dx;
+	 *   dp/dy=p_list[0]*dh1/dy+p_list[1]*dh2/dy+p_list[2]*dh3/dy+p_list[3]*dh4/dy+p_list[4]*dh5/dy+p_list[5]*dh6/dy;
+	 *   dp/dz=p_list[0]*dh1/dz+p_list[1]*dh2/dz+p_list[2]*dh3/dz+p_list[3]*dh4/dz+p_list[4]*dh5/dz+p_list[5]*dh6/dz;
+	 *
+	 *   p is a vector of size 3x1 already allocated.
+	 */
+
+	const int NDOF3=3;
+	const int numgrids=6;
+	double dh1dh2dh3dh4dh5dh6_basic[NDOF3][numgrids];
+
+	/*Get dh1dh2dh3dh4dh5dh6_basic in basic coordinate system: */
+	GetNodalFunctionsDerivativesBasic(&dh1dh2dh3dh4dh5dh6_basic[0][0],xyz_list, gauss_l1l2l3l4);
+	
+	*(p+0)=p_list[0]*dh1dh2dh3dh4dh5dh6_basic[0][0]+p_list[1]*dh1dh2dh3dh4dh5dh6_basic[0][1]+p_list[2]*dh1dh2dh3dh4dh5dh6_basic[0][2]+p_list[3]*dh1dh2dh3dh4dh5dh6_basic[0][3]+p_list[4]*dh1dh2dh3dh4dh5dh6_basic[0][4]+p_list[5]*dh1dh2dh3dh4dh5dh6_basic[0][5];
+;
+	*(p+1)=p_list[0]*dh1dh2dh3dh4dh5dh6_basic[1][0]+p_list[1]*dh1dh2dh3dh4dh5dh6_basic[1][1]+p_list[2]*dh1dh2dh3dh4dh5dh6_basic[1][2]+p_list[3]*dh1dh2dh3dh4dh5dh6_basic[1][3]+p_list[4]*dh1dh2dh3dh4dh5dh6_basic[1][4]+p_list[5]*dh1dh2dh3dh4dh5dh6_basic[1][5];
+
+	*(p+2)=p_list[0]*dh1dh2dh3dh4dh5dh6_basic[2][0]+p_list[1]*dh1dh2dh3dh4dh5dh6_basic[2][1]+p_list[2]*dh1dh2dh3dh4dh5dh6_basic[2][2]+p_list[3]*dh1dh2dh3dh4dh5dh6_basic[2][3]+p_list[4]*dh1dh2dh3dh4dh5dh6_basic[2][4]+p_list[5]*dh1dh2dh3dh4dh5dh6_basic[2][5];
+
+}
+
+#undef __FUNCT__ 
+#define __FUNCT__ "Penta::GetNodalFunctions"
+void Penta::GetNodalFunctions(double* l1l2l3l4l5l6, double* gauss_l1l2l3l4){
+	
+	/*This routine returns the values of the nodal functions  at the gaussian point.*/
+
+	l1l2l3l4l5l6[0]=gauss_l1l2l3l4[0]*(1-gauss_l1l2l3l4[3])/2.0;
+
+	l1l2l3l4l5l6[1]=gauss_l1l2l3l4[1]*(1-gauss_l1l2l3l4[3])/2.0;
+	
+	l1l2l3l4l5l6[2]=gauss_l1l2l3l4[2]*(1-gauss_l1l2l3l4[3])/2.0;
+
+	l1l2l3l4l5l6[3]=gauss_l1l2l3l4[0]*(1+gauss_l1l2l3l4[3])/2.0;
+
+	l1l2l3l4l5l6[4]=gauss_l1l2l3l4[1]*(1+gauss_l1l2l3l4[3])/2.0;
+	
+	l1l2l3l4l5l6[5]=gauss_l1l2l3l4[2]*(1+gauss_l1l2l3l4[3])/2.0;
+
+}
+
+#undef __FUNCT__ 
+#define __FUNCT__ "Penta::VelocityExtrude"
+void  Penta::VelocityExtrude(Vec ug,double* ug_serial){
+
+	/* node data: */
+	const int    numgrids=6;
+	const int    numdofs=2*numgrids;
+	int          doflist[numdofs];
+	int          nodedofs[2];
+	int          numberofdofspernode;
+	
+	Node* node=NULL;
+	int   i;
+	double velocity[2];
+		
+
+	if((collapse==1) && (onbed==1)){
+			
+		GetDofList(&doflist[0],&numberofdofspernode);
+
+		/*this penta is a collapsed macayeal. For each node on the base of this penta, 
+		 * we grab the velocity. Once we know the velocity, we follow the upper nodes, 
+		 * inserting the same velocity value into ug, until we reach the surface: */
+		for(i=0;i<3;i++){
+	
+			node=nodes[i]; //base nodes
+	
+			/*get velocity for this base node: */
+			velocity[0]=ug_serial[doflist[numberofdofspernode*i+0]];
+			velocity[1]=ug_serial[doflist[numberofdofspernode*i+1]];
+
+			//go througn all nodes which sit on top of this node, until we reach the surface, 
+			//and plug  velocity in ug
+			for(;;){
+
+				node->GetDofList(&nodedofs[0],&numberofdofspernode);
+				VecSetValues(ug,1,&nodedofs[0],&velocity[0],INSERT_VALUES);
+				VecSetValues(ug,1,&nodedofs[1],&velocity[1],INSERT_VALUES);
+
+				if (node->IsOnSurface())break;
+				/*get next node: */
+				node=node->GetUpperNode();
+			}
+		}
+
+	}
+
+}