text:
/************************************************************************//**
* @file time_evolution.c
*
* This file contains code to advance the solutions one step forward in time
*
* *****************************************************************************/

#include <stdio.h>
#include <math.h>
#include <gsl/gsl_vector.h>
#include "ChannelsAndJunctions.h"
#include "MeshAttributes.h"
#include "mathfunctions.h"
#include "oneTimeStep.h"

/* @cond GLOBAL_VARIABLES */
extern const double g;
/* @endcond */

/***************************************************************************//**
*
* Function to advance a channel structure one step forward in time using 2-stage
* RKDG scheme
* @param[in] Chan channel structure that we are currently working on
* @param[in] time current time of the simulation
* @param[in] dt size of the time step
* @param[in] channelNumber identity (number) of the channel structure that we
* are currently working on
*
* *********************************************************************************/
void oneTimeStep (struct channel *Chan, double time, double dt, int channelNumber)
{
	int TotalNumNodes = 2*Chan->NumNodes;
	double RHSA[TotalNumNodes];
	double RHSQ[TotalNumNodes];

	// Calculate wet/dry status of elements
	#ifdef WDON
	wetDryStatus1D(Chan);
	#endif

	// store A and Q
	gsl_vector *A = gsl_vector_alloc(TotalNumNodes);
	gsl_vector *Q = gsl_vector_alloc(TotalNumNodes);
	for (int i=0; i < TotalNumNodes; ++i)
	{
		gsl_vector_set(A,i,(*Chan).A[i]);
		gsl_vector_set(Q,i,(*Chan).Q[i]);
	}

	/******** For debugging print out RHSA and RHSQ)************/
	#ifdef DEBUG
	printf("Intial A and Q:\n");
	gsl_vector_fprintf(stdout, A, "%1.16f");
	printf("\n");
	gsl_vector_fprintf(stdout, Q, "%1.16f");
	printf("\n");
	#endif
	/***********************************************************/

	// Intermediate RK step
	computeL(Chan, time, channelNumber, RHSA, RHSQ);

	gsl_vector *gRHSA = gsl_vector_alloc(TotalNumNodes);
	gsl_vector *gRHSQ = gsl_vector_alloc(TotalNumNodes);
	for (int i=0; i<TotalNumNodes; ++i)
	{
		gsl_vector_set(gRHSA,i,RHSA[i]);
		gsl_vector_set(gRHSQ,i,RHSQ[i]);
	}

	/******** For debugging print out RHSA and RHSQ)************/
	#ifdef DEBUG
	printf("RHSA and RHSQ after first computeL:\n");
	gsl_vector_fprintf(stdout, RHSA, "%1.16f");
	printf("\n");
	gsl_vector_fprintf(stdout, RHSQ, "%1.16f");
	printf("\n");
	#endif
	/***********************************************************/

	// w_1 = w + dt*L
	gsl_vector *A_1 = gsl_vector_alloc(TotalNumNodes);
	gsl_vector *Q_1 = gsl_vector_alloc(TotalNumNodes);
	for (int i=0; i<TotalNumNodes; ++i)
	{
		gsl_vector_set(A_1,i,gsl_vector_get(A,i));
		gsl_vector_set(Q_1,i,gsl_vector_get(Q,i));
	}
	gsl_vector_scale(gRHSA,dt);
	gsl_vector_scale(gRHSQ,dt);
	gsl_vector_add(A_1,gRHSA);
	gsl_vector_add(Q_1,gRHSQ);

	/******** For debugging print out A_1 and Q_1************/
	#ifdef DEBUG
	printf("A_1 and Q_1\n");
	gsl_vector_fprintf(stdout, A_1, "%1.16f");
	printf("\n");
	gsl_vector_fprintf(stdout, Q_1, "%1.16f");
	printf("\n");
	#endif
	/***********************************************************/

	for (int i=0; i <TotalNumNodes; ++i)
	{
		Chan->A[i] = gsl_vector_get(A_1,i);
		Chan->Q[i] = gsl_vector_get(Q_1,i);
	}

	// Apply minmod slope limiter on w_1
	minmod(Chan);

	#ifdef WDON
	// apply the positive-depth operaton on w_1
	PDop1D(Chan);

	// Caculate wet/dry status again
	wetDryStatus1D(Chan);

	#endif

//	internal_BC();
	boundary_conditions();

	for (int i = 0; i<TotalNumNodes; ++i)
	{
		gsl_vector_set(A_1,i,Chan->A[i]);
		gsl_vector_set(Q_1,i,Chan->Q[i]);
	}

	/******** For debugging print out A and Q************/
/*	#ifdef DEBUG
	printf("A and Q after first minmod\n");
	gsl_vector_fprintf(stdout, A_1, "%e");
	printf("\n");
	gsl_vector_fprintf(stdout, Q_1, "%e");
	printf("\n");
	#endif
*/	/***********************************************************/

	// Compute w
	computeL(Chan, time, channelNumber, RHSA, RHSQ);

	for (int i=0; i<TotalNumNodes; ++i)
	{
		gsl_vector_set(gRHSA, i, RHSA[i]);
		gsl_vector_set(gRHSQ, i, RHSQ[i]);
	}

	/******** For debugging print out RHSA and RHSQ)************/
	#ifdef DEBUG
	printf("RHSA and RHSQ after second computeL:\n");
	gsl_vector_fprintf(stdout, RHSA, "%1.16f");
	printf("\n");
	gsl_vector_fprintf(stdout, RHSQ, "%1.16f");
	printf("\n");
	#endif
	/***********************************************************/

	// w_1 = w + w_1
	gsl_vector_add(A_1,A);
	gsl_vector_add(Q_1,Q);

	// w = (w1 + dt*L)/2
	gsl_vector_scale(A_1,0.5);
	gsl_vector_scale(Q_1,0.5);
	gsl_vector_scale(gRHSA,dt/2);
	gsl_vector_scale(gRHSQ,dt/2);

	gsl_vector_add(A_1,gRHSA);
	gsl_vector_add(Q_1,gRHSQ);

	for (int i=0; i < TotalNumNodes; ++i)
	{
		Chan->A[i] = gsl_vector_get(A_1,i);
		Chan->Q[i] = gsl_vector_get(Q_1,i);
	}

	/******** For debugging print out A and Q)************/
	#ifdef DEBUG
	printf("A and Q after second computeL:\n");
	gsl_vector_fprintf(stdout, A_1, "%1.16f");
	printf("\n");
	gsl_vector_fprintf(stdout, Q_1, "%1.16f");
	printf("\n");
	#endif
	/***********************************************************/

	// Apply minmod slope limiter to w
	minmod(Chan);

	#ifdef WDON
	PDop1D(Chan);
	#endif

	gsl_vector_free(A);
	gsl_vector_free(Q);
	gsl_vector_free(A_1);
	gsl_vector_free(Q_1);
	gsl_vector_free(gRHSA);
	gsl_vector_free(gRHSQ);

	boundary_conditions();
}

/***************************************************************************//**
*
* Function to advance a junction structure one step forward in time using 2-stage
* RKDG scheme
* @param[in] junc the junction tructure that we are currently working on
* @param[in] time current time of the simulation
* @param[in] dt size of the time step
*
* *********************************************************************************/

void oneTimeStep2D (struct junction *junc, double time, double dt)
{

	int NumEl = junc->NumEl;
	int NumEdges = 3*NumEl;
	double RHSZeta[NumEdges], RHSQx[NumEdges], RHSQy[NumEdges];

	#ifdef WDON
	wetDryStatus2D(junc);
	#endif

	// store H, Qx, Qy
	gsl_vector *zeta = gsl_vector_alloc(NumEdges);
	gsl_vector *Qx = gsl_vector_alloc(NumEdges);
	gsl_vector *Qy = gsl_vector_alloc(NumEdges);

	int j = 0;
	for (int i=0; i < NumEl; ++i) {
		for (int k=0; k < 3; ++k){
			gsl_vector_set(zeta,j,junc->zeta[index(i,k,3)]);
			gsl_vector_set(Qx,j, junc->Qx[index(i,k,3)]);
			gsl_vector_set(Qy,j, junc->Qy[index(i,k,3)]);
			++j;
		}
	}

	/************ print out zeta, Qx and  Qy for debugging ********************/
	#ifdef DEBUG
	printf("Initial values:\n zeta \n");
	gsl_vector_fprintf(stdout,zeta,"%e");
	printf("\n Qx \n");
	gsl_vector_fprintf(stdout,Qx,"%e");
	printf("\n Qy \n");
	gsl_vector_fprintf(stdout,Qy,"%e");
	printf("\n");
	#endif
	/***************************************************************************/

	// Intermediate RK step
	compute2DL(junc, time, RHSZeta, RHSQx, RHSQy);

	gsl_vector *gRHSZeta = gsl_vector_alloc(NumEdges);
	gsl_vector *gRHSQx = gsl_vector_alloc(NumEdges);
	gsl_vector *gRHSQy = gsl_vector_alloc(NumEdges);

	j = 0;
	for (int i=0; i < NumEl; ++i) {
		for (int k=0; k < 3; ++k){
			gsl_vector_set(gRHSZeta,j,RHSZeta[index(i,k,3)]);
			gsl_vector_set(gRHSQx,j,RHSQx[index(i,k,3)]);
			gsl_vector_set(gRHSQy,j,RHSQy[index(i,k,3)]);
			++j;
		}
	}

	/************ print out RHSZeta, RHSQx and RHSQy for debugging ********************/
	#ifdef DEBUG
	printf("After first computeL:\n RHSZeta \n");
	gsl_vector_fprintf(stdout,RHSZeta,"%e");
	printf("\n RHSQx \n");
	gsl_vector_fprintf(stdout,RHSQx,"%e");
	printf("\n RHSQy \n");
	gsl_vector_fprintf(stdout,RHSQy,"%e");
	printf("\n");
	#endif
	/***************************************************************************/

	// w_1 = w + dt*L;
	gsl_vector *zeta_1 = gsl_vector_alloc(NumEdges);
	gsl_vector *Qx_1 = gsl_vector_alloc(NumEdges);
	gsl_vector *Qy_1 = gsl_vector_alloc(NumEdges);
	for (int i=0; i<NumEdges; ++i)
	{
		gsl_vector_set(zeta_1,i,gsl_vector_get(zeta,i));
		gsl_vector_set(Qx_1,i,gsl_vector_get(Qx,i));
		gsl_vector_set(Qy_1,i,gsl_vector_get(Qy,i));
	}

	gsl_vector_scale(gRHSZeta,dt);
	gsl_vector_scale(gRHSQx,dt);
	gsl_vector_scale(gRHSQy,dt);
	gsl_vector_add(zeta_1,gRHSZeta);
	gsl_vector_add(Qx_1, gRHSQx);
	gsl_vector_add(Qy_1, gRHSQy);

	/************ print out zeta, Qx and  Qy for debugging ********************/
	#ifdef DEBUG
	printf("After first computeL:\n Zeta \n");
	gsl_vector_fprintf(stdout,zeta_1,"%e");
	printf("\n Qx \n");
	gsl_vector_fprintf(stdout,Qx_1,"%e");
	printf("\n Qy \n");
	gsl_vector_fprintf(stdout,Qy_1,"%e");
	printf("\n");
	#endif
	/***************************************************************************/

	j = 0;
	for (int i=0; i<NumEl; ++i){
		for (int k =0; k < 3; ++k){
			junc->zeta[index(i,k,3)] = gsl_vector_get(zeta_1,j);
			junc->Qx[index(i,k,3)] = gsl_vector_get(Qx_1,j);
			junc->Qy[index(i,k,3)] = gsl_vector_get(Qy_1,j);
			++j;
		}
	}

	// Apply minmod slope limiter on w_1
	SlopeLimiter(junc);
//	internal_BC();

	#ifdef WDON
	PDop2D(junc);
	wetDryStatus2D(junc);
	#endif

	j = 0;
	for (int i=0; i<NumEl; ++i)
	{
		for(int k=0; k <3; ++k)
		{
			gsl_vector_set(zeta_1,j,junc->zeta[index(i,k,3)]);
			gsl_vector_set(Qx_1,j,junc->Qx[index(i,k,3)]);
			gsl_vector_set(Qy_1,j,junc->Qy[index(i,k,3)]);
			++j;
		}
	}

	/************ print out zeta, Qx and  Qy for debugging ********************/
	#ifdef DEBUG
	printf("After first slope limiting:\n zeta \n");
	gsl_vector_fprintf(stdout,zeta_1,"%e");
	printf("\n Qx \n");
	gsl_vector_fprintf(stdout,Qx_1,"%e");
	printf("\n Qy \n");
	gsl_vector_fprintf(stdout,Qy_1,"%e");
	printf("\n");
	#endif
	/***************************************************************************/

	// w_1 = w_1 + w;
	gsl_vector_add(zeta_1,zeta);
	gsl_vector_add(Qx_1, Qx);
	gsl_vector_add(Qy_1,Qy);

	// Compute w
	compute2DL(junc, time, RHSZeta, RHSQx, RHSQy);

	j = 0;
	for (int i=0; i < NumEl; ++i) {
		for (int k=0; k < 3; ++k){
			gsl_vector_set(gRHSZeta,j,RHSZeta[index(i,k,3)]);
			gsl_vector_set(gRHSQx,j,RHSQx[index(i,k,3)]);
			gsl_vector_set(gRHSQy,j,RHSQy[index(i,k,3)]);
			++j;
		}
	}

	/************ print out zeta, Qx and  Qy for debugging ********************/
	#ifdef DEBUG
	printf("After second computeL:\n RHSZeta \n");
	gsl_vector_fprintf(stdout,RHSZeta,"%e");
	printf("\n RHSQx \n");
	gsl_vector_fprintf(stdout,RHSQx,"%e");
	printf("\n RHSQy \n");
	gsl_vector_fprintf(stdout,RHSQy,"%e");
	printf("\n");
	#endif
	/***************************************************************************/

	gsl_vector_scale(zeta_1,0.5);
	gsl_vector_scale(Qx_1,0.5);
	gsl_vector_scale(Qy_1,0.5);
	gsl_vector_scale(gRHSZeta,dt/2);
	gsl_vector_scale(gRHSQx,dt/2);
	gsl_vector_scale(gRHSQy,dt/2);

	gsl_vector_add(zeta_1,gRHSZeta);
	gsl_vector_add(Qx_1,gRHSQx);
	gsl_vector_add(Qy_1,gRHSQy);

	j = 0;
	for (int i=0; i<NumEl; ++i){
		for (int k =0; k < 3; ++k){
			junc->zeta[index(i,k,3)] = gsl_vector_get(zeta_1,j);
			junc->Qx[index(i,k,3)] = gsl_vector_get(Qx_1,j);
			junc->Qy[index(i,k,3)] = gsl_vector_get(Qy_1,j);
			++j;
		}
	}

	// Apply minmod slope limiter to w
	SlopeLimiter(junc);

	#ifdef WDON
	PDop2D(junc);
	#endif

	gsl_vector_free(zeta);
	gsl_vector_free(Qx);
	gsl_vector_free(Qy);
	gsl_vector_free(zeta_1);
	gsl_vector_free(Qx_1);
	gsl_vector_free(Qy_1);
	gsl_vector_free(gRHSZeta);
	gsl_vector_free(gRHSQx);
	gsl_vector_free(gRHSQy);

}

/*******************************************************************//**
*
* Function to evolve the channels as well as the junctions through time
* until the final simulation time. It also outputs the data at a certain
* time
*
* *********************************************************************/
void time_evolution(double FinalTime)
{

	//Output initial conditions
	FILE* file1;
	FILE* file2;

	for (int i=0; i<NumChannels; ++i)
	{
		char filename1[100];
		char filename2[100];
		sprintf(filename1, "000_Zeta%d.dat", i);
		sprintf(filename2, "000_Q%d.dat", i);
		file1 = fopen(filename1, "w");
		file2 = fopen(filename2, "w");

		fprintf(file1, "Height at time 0 for Channel %d\n", i);
		fprintf(file2, "Q at time 0 for Channel %d\n", i);

		int NumNodes = ChannelList[i]->NumNodes;

		for (int j = 0; j<NumNodes; ++j)
		{
			double B;
			double z;

			B = ChannelList[i]->b[j];
			z = ChannelList[i]->z[j];
			double x_val = ChannelList[i]->x[j];
			double y_val = ChannelList[i]->y[j];
			for (int k=0; k<2; ++k)
			{

				double A = ChannelList[i]->A[index(j,k,2)];
				double Q = ChannelList[i]->Q[index(j,k,2)];
				double H = A/B;
				double zeta = H - z;
				fprintf(file1, "%e\t%e\t%e\t%e\n", x_val, y_val, -z, H);
				fprintf(file2, "%e\t%e\t%e\t%e\n", x_val, y_val, -z, Q);
			}
		}
		fclose(file1);
		fclose(file2);
	}

	// output the file with x y and z information for the elements
/*	FILE *juncMesh;
	for (int i=0; i <NumJunctions; ++i)
	{
		char filename1[100];
		sprintf(filename1, "Junction%dMesh.dat",i);
		juncMesh = fopen(filename1, "w");
		int NumEl = JunctionList[i]->NumEl;
		int NumNodes = JunctionList[i]->NumNodes;
		int NumEdges = JunctionList[i]->TotalNumEdges;

		fprintf(juncMesh, "NodeNum \t x \t\t y \t\t z\n");
		for (int j = 0; j < NumNodes; ++j)
		{
			double xcor = JunctionList[i]->x[j];
			double ycor = JunctionList[i]->y[j];
			double zcor = JunctionList[i]->z[j];
			fprintf(juncMesh, "%d\t%e\t%e\t%e\n",j+1, xcor, ycor, zcor);
		}

		fprintf(juncMesh, "\n EltoVert \n");
		for (int j = 0; j < NumEl; ++j)
		{
			int n1 = JunctionList[i]->EltoVert[j*3]+1;
			int n2 = JunctionList[i]->EltoVert[j*3+1]+1;
			int n3 = JunctionList[i]->EltoVert[j*3+2]+1;
			fprintf(juncMesh, "%d\t3\t%d\t%d\t%d\n", j+1, n1, n2,n3);

		}

		fprintf(juncMesh, "\n EdgtoVert \n");
		for (int j =0; j < NumEdges; ++j)
		{
			int n1 = JunctionList[i]->EdgtoVert[j*2]+1;
			int n2 = JunctionList[i]->EdgtoVert[j*2+1]+1;
			fprintf(juncMesh, "%d\t%d\n",n1,n2);

		}

		fprintf(juncMesh, "\n EdgtoEls \n");
		for (int j =0; j < NumEdges; ++j)
		{
			int n1 = JunctionList[i]->EdgtoEls[j*2];
			int n2 = JunctionList[i]->EdgtoEls[j*2+1];
			fprintf(juncMesh, "%d\t%d\n",n1,n2);

		}

		fclose(juncMesh);

	}
*/
	boundary_conditions();

	double time = 0;
	double dt = 1e-5;
	int Nstep = 0;
	int fileNumber = 1;

	double minLength = 99999999;
	for (int i = 0; i < NumChannels; ++i)
	{
		minLength = fmin(minLength, ChannelList[i]->mindh);
	}

	for (int i = 0; i < NumJunctions; ++i)
	{
		minLength = fmin(minLength, JunctionList[i]->minEdgLength);
	}

	while (time < FinalTime)
	{
		time = time + dt;
		printf("dt = %e\n", dt);
		printf("time = %e\n", time);

		double maxLambda = 0;

		// step forward in time
		for(int i=0; i<NumChannels; ++i)
		{
			oneTimeStep(ChannelList[i],time,dt,i);
			maxLambda = fmax(maxLambda, ChannelList[i]->max_lambda);

		}

		internal_BC();

		for(int i=0; i<NumJunctions; ++i)
		{
			oneTimeStep2D(JunctionList[i],time,dt);
			maxLambda = fmax(maxLambda, JunctionList[i]->max_lambda);
		}
		internal_BC();

		boundary_conditions();

		/********* Output data every twentienth step *************/
		Nstep = Nstep + 1;
		int NstepinOneSec = floor(1./dt);
		if (Nstep%5000== 0 || time == FinalTime)
		//if (Nstep% NstepinOneSec == 0)
		{

			//Files for the channels
			FILE* file1;
			FILE* file2;

			for (int i=0; i<NumChannels; ++i)
			{
				char filename1[100];
				char filename2[100];
				sprintf(filename1, "%3.3d_Zeta%d.dat", fileNumber,i);
				sprintf(filename2, "%3.3d_Q%d.dat", fileNumber,i);
				file1 = fopen(filename1, "w");
				file2 = fopen(filename2, "w");

				fprintf(file1, "Height at time %e for Channel %d\n", time, i);
				fprintf(file2, "Q at time %e for Channel %d\n", time,i);

				int NumNodes = ChannelList[i]->NumNodes;

				for (int j = 0; j<NumNodes; ++j)
				{
					double B;
					double z;

					B = ChannelList[i]->b[j];
					z = ChannelList[i]->z[j];
					double x_val = ChannelList[i]->x[j];
					double y_val = ChannelList[i]->y[j];

					int k;
					for (k=0; k<2; ++k)
					{

						double A = ChannelList[i]->A[index(j,k,2)];
						double Q = ChannelList[i]->Q[index(j,k,2)];
						double H = A/B;
						double zeta = H - z;
						fprintf(file1, "%e\t%e\t%e\t%e\n", x_val, y_val, -z, H);
						fprintf(file2, "%e\t%e\t%e\t%e\n", x_val, y_val, -z, Q);
					}

				}
				fclose(file1);
				fclose(file2);
			}

			FILE* file3;
			FILE* file4;
			FILE* file5;

			for (int i=0; i<NumJunctions;++i)
			{
				char filename1[100];
				char filename2[100];
				char filename3[100];
				sprintf(filename1, "%3.3d_Height_junc%d.dat", fileNumber,i);
				sprintf(filename2, "%3.3d_Qx_junc%d.dat", fileNumber,i);
				sprintf(filename3, "%3.3d_Qy_junc%d.dat", fileNumber,i);
				file3 = fopen(filename1, "w");
				file4 = fopen(filename2, "w");
				file5 = fopen(filename3, "w");

				fprintf(file3, "Zeta at time %e for Junction %d\n", time, i);
				fprintf(file4, "Qx at time %e for Junction %d\n", time, i);
				fprintf(file5, "Qy at time %e for Junction %d\n", time,i);

				int j;
				int k;
				for (j=0; j<JunctionList[i]->NumEl; ++j)
				{
					for(k=0; k<3; ++k)
					{
						double zeta = JunctionList[i]->zeta[index(j,k,3)];
						double Qx = JunctionList[i]->Qx[index(j,k,3)];
						double Qy = JunctionList[i]->Qy[index(j,k,3)];
						double z1 = JunctionList[i]->z[JunctionList[i]->EltoVert[j*3+k]];
						double z2 = JunctionList[i]->z[JunctionList[i]->EltoVert[j*3+(k+1)%3]];
						double z = 0.5*(z1+z2);
						double height = zeta + z;
						fprintf(file3, "%1.15f\t\t",height);
						fprintf(file4, "%1.15f\t\t",Qx);
						fprintf(file5, "%1.15f\t\t",Qy);
					}
					fprintf(file3, "\n");
					fprintf(file4, "\n");
					fprintf(file5, "\n");
				}
				fclose(file3);
				fclose(file4);
				fclose(file5);
			}

			++fileNumber;
		}

		/**************** Set next time step ****************/

		// set next time step
		//printf("max_lambda = %e\n", maxlambda);
		//printf("mindx = %e\n", mindx);
		dt = 0.4*minLength/maxLambda;
		if ((time + dt) > FinalTime)
			dt = FinalTime - time;

	}
}

max_stars_repo_name: evalseth/DG-RAIN

max_issues_repo_name: evalseth/DG-RAIN

max_forks_repo_name: evalseth/DG-RAIN


text:
#ifndef CONVERTERS_H_HVFGCFVK
#define CONVERTERS_H_HVFGCFVK

#include <fmt/core.h>
#include <gsl/gsl>
#include <opencv2/imgcodecs.hpp>
#include <sens_loc/conversion/depth_to_bearing.h>
#include <sens_loc/conversion/depth_to_curvature.h>
#include <sens_loc/conversion/depth_to_flexion.h>
#include <sens_loc/conversion/depth_to_laserscan.h>
#include <sens_loc/conversion/depth_to_max_curve.h>
#include <util/batch_converter.h>

namespace sens_loc::apps {

/// \addtogroup conversion-driver
/// @{

/// Batch conversion to bearing-angle images.
/// \sa conversion::depth_to_bearing
template <typename Intrinsic>
class bearing_converter : public batch_sensor_converter<Intrinsic> {
  public:
    bearing_converter(const file_patterns& files,
                      depth_type           t,
                      Intrinsic            intrinsic)
        : batch_sensor_converter<Intrinsic>(files, t, std::move(intrinsic)) {
        if (files.horizontal.empty() && files.vertical.empty() &&
            files.diagonal.empty() && files.antidiagonal.empty()) {
            throw std::invalid_argument{
                "Missing output pattern for at least one bearing direction"};
        }
    }
    bearing_converter(const bearing_converter&) = default;
    bearing_converter(bearing_converter&&)      = default;
    bearing_converter& operator=(const bearing_converter&) = default;
    bearing_converter& operator=(bearing_converter&&) = default;
    ~bearing_converter() override                     = default;

  private:
    [[nodiscard]] bool process_file(const math::image<float>& depth_image,
                                    int idx) const noexcept override;
};
#include "converter_bearing.h.inl"

/// Convert orthographic depth-images to range (laserscan-like) images.
/// \sa conversion::depth_to_laserscan
template <typename Intrinsic>
class range_converter : public batch_sensor_converter<Intrinsic> {
  public:
    range_converter(const file_patterns& files,
                    depth_type           t,
                    Intrinsic            intrinsic)
        : batch_sensor_converter<Intrinsic>(files, t, std::move(intrinsic)) {}
    range_converter(const range_converter&) = default;
    range_converter(range_converter&&)      = default;
    range_converter& operator=(const range_converter&) = default;
    range_converter& operator=(range_converter&&) = default;
    ~range_converter() override                   = default;

  private:
    [[nodiscard]] bool process_file(const math::image<float>& depth_image,
                                    int idx) const noexcept override;
};
#include "converter_laserscan.h.inl"

/// Convert range-images to gaussian curvature images.
/// \sa conversion::depth_to_gaussian_curvature
template <typename Intrinsic>
class gauss_curv_converter : public batch_sensor_converter<Intrinsic> {
  public:
    /// \param files,t,intrinsic normal parameters for batch conversion
    /// \param lower_bound,upper_bound clamping parameters. Values below/above
    /// will map to these values.
    gauss_curv_converter(const file_patterns& files,
                         depth_type           t,
                         Intrinsic            intrinsic,
                         double               lower_bound,
                         double               upper_bound)
        : batch_sensor_converter<Intrinsic>(files, t, std::move(intrinsic))
        , lower_bound{lower_bound}
        , upper_bound{upper_bound} {}
    gauss_curv_converter(const gauss_curv_converter&) = default;
    gauss_curv_converter(gauss_curv_converter&&)      = default;
    gauss_curv_converter& operator=(const gauss_curv_converter&) = default;
    gauss_curv_converter& operator=(gauss_curv_converter&&) = default;
    ~gauss_curv_converter() override                        = default;

  private:
    [[nodiscard]] bool process_file(const math::image<float>& depth_image,
                                    int idx) const noexcept override;

    double lower_bound;
    double upper_bound;
};

/// Convert range-images to mean curvature images.
/// \sa conversion::depth_to_mean_curvature
template <typename Intrinsic>
class mean_curv_converter : public batch_sensor_converter<Intrinsic> {
  public:
    /// \param files,t,intrinsic normal parameters for batch conversion
    /// \param lower_bound,upper_bound clamping parameters. Values
    /// below/above will map to these values.
    mean_curv_converter(const file_patterns& files,
                        depth_type           t,
                        Intrinsic            intrinsic,
                        double               lower_bound,
                        double               upper_bound)
        : batch_sensor_converter<Intrinsic>(files, t, std::move(intrinsic))
        , lower_bound{lower_bound}
        , upper_bound{upper_bound} {}
    mean_curv_converter(const mean_curv_converter&) = default;
    mean_curv_converter(mean_curv_converter&&)      = default;
    mean_curv_converter& operator=(const mean_curv_converter&) = default;
    mean_curv_converter& operator=(mean_curv_converter&&) = default;
    ~mean_curv_converter() override                       = default;

  private:
    [[nodiscard]] bool process_file(const math::image<float>& depth_image,
                                    int idx) const noexcept override;

    double lower_bound;
    double upper_bound;
};
#include "converter_curvature.h.inl"

/// Convert range-images to max-curve images.
/// \sa conversion::depth_to_max_curve
template <typename Intrinsic>
class max_curve_converter : public batch_sensor_converter<Intrinsic> {
  public:
    max_curve_converter(const file_patterns& files,
                        depth_type           t,
                        Intrinsic            intrinsic)
        : batch_sensor_converter<Intrinsic>(files, t, std::move(intrinsic)) {}
    max_curve_converter(const max_curve_converter&) = default;
    max_curve_converter(max_curve_converter&&)      = default;
    max_curve_converter& operator=(const max_curve_converter&) = default;
    max_curve_converter& operator=(max_curve_converter&&) = default;
    ~max_curve_converter() override                       = default;

  private:
    [[nodiscard]] bool process_file(const math::image<float>& depth_image,
                                    int idx) const noexcept override;
};
#include "converter_max_curve.h.inl"

/// Convert range-images to flexion images.
/// \sa conversion::depth_to_flexion
template <typename Intrinsic>
class flexion_converter : public batch_sensor_converter<Intrinsic> {
  public:
    flexion_converter(const file_patterns& files,
                      depth_type           t,
                      Intrinsic            intrinsic)
        : batch_sensor_converter<Intrinsic>(files, t, std::move(intrinsic)) {}
    flexion_converter(const flexion_converter&) = default;
    flexion_converter(flexion_converter&&)      = default;
    flexion_converter& operator=(const flexion_converter&) = default;
    flexion_converter& operator=(flexion_converter&&) = default;
    ~flexion_converter() override                     = default;

  private:
    [[nodiscard]] bool process_file(const math::image<float>& depth_image,
                                    int idx) const noexcept override;
};
#include "converter_flexion.h.inl"

/// @}

}  // namespace sens_loc::apps

#endif

ext: h

max_stars_repo_name: JonasToth/depth-conversions

max_issues_repo_name: JonasToth/depth-conversions

max_forks_repo_name: JonasToth/depth-conversions


text:
/* roots/brent.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Reid Priedhorsky, Brian Gough
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/* brent.c -- brent root finding algorithm */

#include <config.h>

#include <stddef.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <float.h>

#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_roots.h>

#include "roots.h"

typedef struct
  {
    double a, b, c, d, e;
    double fa, fb, fc;
  }
brent_state_t;

static int brent_init (void * vstate, gsl_function * f, double * root, double x_lower, double x_upper);
static int brent_iterate (void * vstate, gsl_function * f, double * root, double * x_lower, double * x_upper);

static int
brent_init (void * vstate, gsl_function * f, double * root, double x_lower, double x_upper)
{
  brent_state_t * state = (brent_state_t *) vstate;

  double f_lower, f_upper ;

  *root = 0.5 * (x_lower + x_upper) ;

  SAFE_FUNC_CALL (f, x_lower, &f_lower);
  SAFE_FUNC_CALL (f, x_upper, &f_upper);

  state->a = x_lower;
  state->fa = f_lower;

  state->b = x_upper;
  state->fb = f_upper;

  state->c = x_upper;
  state->fc = f_upper;

  state->d = x_upper - x_lower ;
  state->e = x_upper - x_lower ;

  if ((f_lower < 0.0 && f_upper < 0.0) || (f_lower > 0.0 && f_upper > 0.0))
    {
      GSL_ERROR ("endpoints do not straddle y=0", GSL_EINVAL);
    }

  return GSL_SUCCESS;

}

static int
brent_iterate (void * vstate, gsl_function * f, double * root, double * x_lower, double * x_upper)
{
  brent_state_t * state = (brent_state_t *) vstate;

  double tol, m;

  int ac_equal = 0;

  double a = state->a, b = state->b, c = state->c;
  double fa = state->fa, fb = state->fb, fc = state->fc;
  double d = state->d, e = state->e;

  if ((fb < 0 && fc < 0) || (fb > 0 && fc > 0))
    {
      ac_equal = 1;
      c = a;
      fc = fa;
      d = b - a;
      e = b - a;
    }

  if (fabs (fc) < fabs (fb))
    {
      ac_equal = 1;
      a = b;
      b = c;
      c = a;
      fa = fb;
      fb = fc;
      fc = fa;
    }

  tol = 0.5 * GSL_DBL_EPSILON * fabs (b);
  m = 0.5 * (c - b);

  if (fb == 0)
    {
      *root = b;
      *x_lower = b;
      *x_upper = b;

      return GSL_SUCCESS;
    }

  if (fabs (m) <= tol)
    {
      *root = b;

      if (b < c)
        {
          *x_lower = b;
          *x_upper = c;
        }
      else
        {
          *x_lower = c;
          *x_upper = b;
        }

      return GSL_SUCCESS;
    }

  if (fabs (e) < tol || fabs (fa) <= fabs (fb))
    {
      d = m;            /* use bisection */
      e = m;
    }
  else
    {
      double p, q, r;   /* use inverse cubic interpolation */
      double s = fb / fa;

      if (ac_equal)
        {
          p = 2 * m * s;
          q = 1 - s;
        }
      else
        {
          q = fa / fc;
          r = fb / fc;
          p = s * (2 * m * q * (q - r) - (b - a) * (r - 1));
          q = (q - 1) * (r - 1) * (s - 1);
        }

      if (p > 0)
        {
          q = -q;
        }
      else
        {
          p = -p;
        }

      if (2 * p < GSL_MIN (3 * m * q - fabs (tol * q), fabs (e * q)))
        {
          e = d;
          d = p / q;
        }
      else
        {
          /* interpolation failed, fall back to bisection */

          d = m;
          e = m;
        }
    }

  a = b;
  fa = fb;

  if (fabs (d) > tol)
    {
      b += d;
    }
  else
    {
      b += (m > 0 ? +tol : -tol);
    }

  SAFE_FUNC_CALL (f, b, &fb);

  state->a = a ;
  state->b = b ;
  state->c = c ;
  state->d = d ;
  state->e = e ;
  state->fa = fa ;
  state->fb = fb ;
  state->fc = fc ;

  /* Update the best estimate of the root and bounds on each
     iteration */

  *root = b;

  if ((fb < 0 && fc < 0) || (fb > 0 && fc > 0))
    {
      c = a;
    }

  if (b < c)
    {
      *x_lower = b;
      *x_upper = c;
    }
  else
    {
      *x_lower = c;
      *x_upper = b;
    }

  return GSL_SUCCESS ;
}

static const gsl_root_fsolver_type brent_type =
{"brent",                               /* name */
 sizeof (brent_state_t),
 &brent_init,
 &brent_iterate};

const gsl_root_fsolver_type  * gsl_root_fsolver_brent = &brent_type;

ext: c

max_stars_repo_name: manggoguy/parsec-modified

max_issues_repo_name: manggoguy/parsec-modified

max_forks_repo_name: manggoguy/parsec-modified


text:
#ifndef INIT
#define INIT

#include <assert.h>
#include <stdio.h>
#include <time.h>
#include <math.h>
/* #include <gsl/gsl_linalg.h> */

#include "macros.h"
#include "algebra.h"
#include "util.h"

/**
 * Initialization of params and vectors.
 * These functions are not (necessarily) optimized for performance.
 */

void init_random(curandGenerator_t *gen) {
  static unsigned int seed = 1234; // TODO manage externally from this function
  seed++;
  // TODO do this on CPU to avoid data copying
  curandCreateGenerator(gen, CURAND_RNG_PSEUDO_XORWOW);
  /* curandCreateGenerator(gen, CURAND_RNG_PSEUDO_MT19937); */
  curandSetPseudoRandomGeneratorSeed(*gen, seed);
}

void randomize(float *x, float *d_x, size_t len, curandGenerator_t gen) {
  // TODO do this on cpu or at runtime (per batch)
  curandGenerateUniform(gen, d_x, len);
  cuR( curandGenerateUniform(gen, d_x, len * sizeof(float) / 32) );
  cudaMemcpy(x, d_x, len * sizeof(float), cudaMemcpyDeviceToHost);
  cudaDeviceSynchronize();
}

///////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
namespace init {
///////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////

void derive_secondary_geometry(struct Geometry& p) {
  p.gridSize = {p.blockDim.x * p.gridDim.x,
                p.blockDim.y * p.gridDim.y,
                1};
  p.batch_size = {MIN(p.n.x, p.thread_size.x * p.gridSize.x),
                  MIN(p.n.y, p.thread_size.y * p.gridSize.y)};

#ifdef SQUARE_TARGET_BATCHES
  if (p.n.y > p.batch_size.y) {
    // round down to the nearest square
    const size_t sq =  sqrt(p.batch_size.y);
    p.batch_size.y = sq * sq;
  }
#endif

  p.n_batches = {CEIL(p.n.x, p.batch_size.x),
                 CEIL(p.n.y, p.batch_size.y)};
  check_hyper_params(p);
}

Geometry simple_geometry(const size_t x, const size_t y) {
  Geometry p;
  p.blockDim = {1,1,1};
  p.gridDim = {1,1,1};
  p.n = {x, y};
  p.thread_size = {1,1};
  p.n_streams = 1;
  derive_secondary_geometry(p);
  return p;
}

Geometry geometry(struct Params& params, const size_t x, const size_t y) {
  Geometry p;
  p.algorithm = params.algorithm;
  p.blockDim = params.blockDim;
  p.gridDim = params.gridDim;
  p.n = {x, y};
  p.thread_size = params.thread_size;
  p.n_streams = params.n_streams;

  derive_secondary_geometry(p);
  return p;
}

/**
 * Distribute sampling points over a 2D plane in 3D space.
 */
void plane(std::vector<SPACE> &v, const Plane p) {
  // TODO return ptr to device memory, copy pos data to CPU during batches
  const size_t n = v.size() / DIMS;
  printf("offset: %f\n", p.offset.z);
  for (unsigned int i = 0; i < n; ++i)
    v[i*DIMS + 2] = p.offset.z;
  /*
   * Assume HD dimensions, keep remaining pixels for kernel geometry compatibility
   * solve for x:  `n * (ratio) = x * y * (x/y) = x^2   ->   x = sqrt(n * ratio)`
   */
  auto
    x = (size_t) sqrt(n * p.aspect_ratio),
    y = n / x;
  if (p.aspect_ratio != 1.0) {
    printf("Screen dimensions: %i x %i\t", x, y);
    printf("Remaining points: %i/%i\n", n - x*y, n);
  }
  assert(x * y <= n);

  // distribute points over a lattice (each point in the center of a cell in a grid)
  const double
    height = p.width / p.aspect_ratio,
    dx = p.width / (double) x, // excl. boundary; don't divide by (x-1)
    dy = height / (double) y,
    x_half = (p.width - dx) / 2., // subtract dx/2 to position points in center
    y_half = (height - dy) / 2.;

  printf("init::plane, width: %e, n_x: %lu, dx: %e, x_half: %e\n", p.width, x, dx, x_half);
  // properties to generate random offsets
  const double
    rel_margin = p.randomize ? 0.05 : 0.0,
    x_margin = rel_margin * dx, // TODO min space between projector pixels
    y_margin = rel_margin * dy,
    x_random_range = dx - 0.5 * x_margin,
    y_random_range = dy - 0.5 * y_margin;

  const size_t n_random = x + y;
  curandGenerator_t generator;
  float *d_random, random[n_random];

  if (p.randomize) {
    init_random(&generator);
    cu( cudaMalloc( (void **) &d_random, n_random * sizeof(float) ) );
  }

  for (unsigned int i = 0; i < x; ++i) {
    if (p.randomize)
      randomize(random, d_random, n_random, generator);

    for (unsigned int j = 0; j < y; ++j) {
      v[Ix2D(i,j,0,y)] = i * dx - x_half + p.offset.x;
      v[Ix2D(i,j,1,y)] = j * dy - y_half + p.offset.y;
      v[Ix2D(i,j,2,y)] = p.offset.z;

      if (p.randomize) {
        v[Ix2D(i,j,0,y)] += x_random_range * (random[j*2] - 0.5);
        v[Ix2D(i,j,1,y)] += y_random_range * (random[j*2+1] - 0.5);
      }
    }
  }
  if (p.randomize) {
    curandDestroyGenerator(generator);
    cu( cudaFree( d_random ) );
  }

  // fill rest of array with semi-realistic values to minimize incompatibility issues
  for (unsigned int i = x * y; i < n; ++i)
    for (unsigned int j = 0; j < DIMS; ++j)
      v[Ix(i, j)] = v[Ix(0,j)];
}

std::vector<SPACE> sparse_plane(std::vector<SPACE> &u, Shape shape, double width,
                                const Cartesian<double> &offset, double modulate = 0.) {
  const size_t n = u.size() / DIMS;
  for (unsigned int i = 0; i < n; ++i)
    u[Ix(i, 2)] = offset.z;

  assert(n != 0);
  if (n == 1) {
    u[Ix(0,0)] = offset.x;
    u[Ix(0,1)] = offset.y;
    return u;
  }

  // set the x,y dimensions
  switch (shape) {
  case Shape::Line: {
    // Distribute datapoints over a line
    auto
      du = width / (double) (n-1), // TODO use SCALE?
      half_width = width / 2.0;

    for (unsigned int i = 0; i < n; ++i)
      u[Ix(i,0)] = i * du - half_width;

    break;
  }
  case Shape::LogLine: {
    const auto x = geomspace(n, 1, 1 + width);
    for (auto& i : range(n))
      u[Ix(i,0)] = x[i] - 1 + width / 2.;
    break;
  }
  case Shape::Circle: {
    // Distribute datapoints over a circle
    // (using polar coordinates, s.t. $phi_i \equiv i \Delta \phi$)
    auto
      radius = width / 2.0,
      /* circumference = TWO_PI * pow(radius, 2), */
      d_phase = TWO_PI / (double) n,
      arbitrary_offset = 0.1125 + modulate * TWO_PI;

    for (unsigned int i = 0; i < n; ++i) {
      u[Ix(i,0)] = sin(i * d_phase + arbitrary_offset) * radius;
      u[Ix(i,1)] = cos(i * d_phase + arbitrary_offset) * radius;
    }
    break;
  }
  case Shape::DottedCircle: {
    // (using polar coordinates)
    auto
      radius = width / 2.0,
      /* circumference = TWO_PI * pow(radius, 2), */
      /* don't include end; divide by n */
      d_phase = TWO_PI / (double) (n-1), // subtract center point
      arbitrary_offset = 0.1125 + modulate * TWO_PI;

    // TODO randomize slightly?
    for (unsigned int i = 1; i < n; ++i) {
      u[Ix(i,0)] = sin(i * d_phase + arbitrary_offset) * radius;
      u[Ix(i,1)] = cos(i * d_phase + arbitrary_offset) * radius;
    }

    break;
  }
  }

  if (offset.x != 0 || offset.y != 0) {
    for (unsigned int i = 0; i < n; ++i) {
      u[Ix(i,0)] += offset.x;
      u[Ix(i,1)] += offset.y;
    }
  }

  return u;
}

template<typename T>
std::vector<T*> malloc_vectors(T **d_ptr, size_t dim1, size_t dim2) {
  // TODO rename => malloc_arrays
  // Return a host vector of device pointers
  assert(dim1 >= 1); assert(dim2 >= 1);
  cu( cudaMalloc( d_ptr, dim1 * dim2 * sizeof(T) ) );
  auto vec = std::vector<T*>(dim1);
  for (size_t i = 0; i < dim1; ++i)
    vec[i] = *d_ptr + i * dim2;
  return vec;
 }

template<typename T>
std::vector<CUDAVector<T>> malloc_matrix(T **d_ptr, size_t dim1, size_t dim2) {
  // Return a host vector of CUDAVector elements
  assert(dim1 >= 1); assert(dim2 >= 1);
  cu( cudaMalloc( d_ptr, dim1 * dim2 * sizeof(T) ) );
  auto matrix = std::vector<CUDAVector<T>>(dim1);
  // std::vector<T>(*d_ptr + a, *d_ptr + b); has weird side effects
  // note that *ptr+i == &ptr[i], but that ptr[i] cannot be read by host if it's located on device
  for (size_t i = 0; i < dim1; ++i)
    matrix[i] = CUDAVector<T>{data: *d_ptr + i * dim2,
                              size: dim2};
  return matrix;
}

template<typename T>
std::vector<T*> pinned_malloc_vectors(T **d_ptr, size_t dim1, size_t dim2) {
  // Return a host vector of pointers
  assert(dim1 >= 1); assert(dim2 >= 1);
  // TODO consider cudaHostAllocMapped(..., cudaHostAllocMapped)
  cu( cudaMallocHost( d_ptr, dim1 * dim2 * sizeof(T) ) );
  auto vec = std::vector<T*>(dim1);
  for (size_t i = 0; i < dim1; ++i)
    vec[i] = *d_ptr + i * dim2;
  return vec;
}

template<typename T>
std::vector<CUDAVector<T>> pinned_malloc_matrix(T **d_ptr, size_t dim1, size_t dim2) {
  // Return a host vector of DeviceVector elements
  assert(dim1 >= 1); assert(dim2 >= 1);
  cu( cudaMallocHost( d_ptr, dim1 * dim2 * sizeof(T) ) );
  auto matrix = std::vector<CUDAVector<T>>(dim1);
  // std::vector<T>(*d_ptr + a, *d_ptr + b); has weird side effects
  // note that *ptr+i == &ptr[i], but that ptr[i] cannot be read by host if it's located on device
  for (size_t i = 0; i < dim1; ++i)
    matrix[i] = CUDAVector<T>{data: *d_ptr + i * dim2,
                              size: dim2};
  return matrix;
}

///////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
} // end namespace
///////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
#endif


text:
/*
* -----------------------------------------------------------------
*  Ellipsoid Library --- ell_lib.h
*  Version: 1.6180
*  Date: Feb 19, 2010
* -----------------------------------------------------------------
*  Programmer: Americo Barbosa da Cunha Junior
*              americo.cunhajr@gmail.com
* -----------------------------------------------------------------
*  Copyright (c) 2010 by Americo Barbosa da Cunha Junior
*
*  This program is free software: you can redistribute it and/or
*  modify it under the terms of the GNU General Public License as
*  published by the Free Software Foundation, either version 3 of
*  the License, or (at your option) any later version.
*
*  This program is distributed in the hope that it will be useful,
*  but WITHOUT ANY WARRANTY; without even the implied warranty of
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
*  GNU General Public License for more details.
*
*  A copy of the GNU General Public License is available in
*  LICENSE.txt or http://www.gnu.org/licenses/.
* -----------------------------------------------------------------
*  This is the header file for a library that operates an
*  n-dimensional ellipsoid.
* -----------------------------------------------------------------
*/

#ifndef __ELL_LIB_H__
#define __ELL_LIB_H__

#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>

#undef  TOL
#define TOL 1.0e-7

#undef  TRUE
#define TRUE 1

#undef  FALSE
#define FALSE 0

/*
*------------------------------------------------------------
* function prototypes
*------------------------------------------------------------
*/

void ell_psd2eig(gsl_matrix *B,
                 gsl_matrix *V,
                 gsl_vector *lam);

void ell_psd2chol(gsl_matrix *B,
                  gsl_matrix *L);

void ell_eig2chol(gsl_matrix *V,
                  gsl_vector *sig,
                  gsl_matrix *L);

double ell_pt_in(gsl_vector *x,
                 gsl_vector *c,
                 gsl_matrix *L);

void ell_map2uhs(gsl_vector *x,
                 gsl_vector *c,
                 gsl_matrix *L,
                 gsl_vector *y);

void ell_pt_modify(gsl_vector *p,
                   gsl_vector *c,
                   gsl_matrix *L);

#endif /* __ELL_LIB_H__ */

max_stars_repo_name: americocunhajr/CRFlowLib

max_issues_repo_name: americocunhajr/CRFlowLib

max_forks_repo_name: americocunhajr/CRFlowLib


text:
/**
 * @file kjg_gsl.h
 * @brief Augment GSL functions
 */

#ifndef KJG_GSL_H_
#define KJG_GSL_H_

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_rng.h>

/**
 * Prints the matrix tab-delimited
 * @param *stream output file pointer
 * @param *m gsl_matrix to print
 * @param *template character template for fprintf
 */

void kjg_gsl_matrix_fprintf (FILE * stream, gsl_matrix * m,
			     const char *template);

/**
 * Prints the eigenvalues and then eigenvectors below
 * @param *stream output file pointer
 * @param *eval eigenvalues
 * @param *evec eigenvectors
 * @param *template character template for fprintf */

void kjg_gsl_evec_fprintf (FILE * stream,
			   gsl_vector * eval,
			   gsl_matrix * evec, const char *template);

/**
 * Reads a matrix
 * @param *stream input file pointer
 * @param *m matrix to store
 */

void kjg_gsl_matrix_fscanf (FILE * stream, gsl_matrix * m);

/**
 * Reads an evec
 * @param *stream input file pointer
 * @param *eval eigenvalues vector
 * @param *evec eigenvectors matrix
 */

int kjg_gsl_evec_fscanf (FILE * stream, gsl_vector * eval, gsl_matrix * evec);

/**
 * Initializes random number generation.
 */

gsl_rng *kjg_gsl_rng_init ();

/**
 * Initializes the matrix with random unit gaussians
 * @param *m matrix to be set
 * @param *r random number generator
 */

void kjg_gsl_ran_ugaussian_pair (const gsl_rng * r, double x[2]);

/** Fills a matrix with unit Gaussian random variates
 * @param *r random number generator
 * @param *m matrix to be filled
 */

void kjg_gsl_ran_ugaussian_matrix (const gsl_rng * r, gsl_matrix * m);

/**
 * Normalizes the matrix so the Frobenius norm is M*N
 * @param *m matrix to normalize
 * @return if error
 */

int kjg_gsl_matrix_frobenius_normalize (gsl_matrix * m);

/**
 * Calculates the norm of a matrix
 * @param norm type of norm to return, see lapack dlange
 * @param *m matrix to find norm of
 * @return norm
 */

double kjg_gsl_dlange (const char norm, const gsl_matrix * m);

/**
 * Performs the QR decomposition on the matrix and return Q in the matrix
 * @param *m matrix to orthogonalize
 */

void kjg_gsl_matrix_QR (gsl_matrix * m);

/**
 * Calls LAPACK dgeqrf and return R and compacted Q matrix
 * @param *m input matrix
 * @param *tau see LAPACK documentation
 * @return LAPACK return
 */

int kjg_gsl_dgeqrf (gsl_matrix * m, gsl_vector * tau);

/**
 * Calls LAPACK dorgqr to extract Q matrix
 * @param *m matrix with compacted Q and will store unpacked Q
 * @param *tau see LAPACK documentation
 * @return LAPACK return
 */
int kjg_gsl_dorgqr (gsl_matrix * m, gsl_vector * tau);

/**
 * Calls LAPACK dgesvd, keeping u (in m) and s, discarding v^T
 * @param *m input matrix / where u is stored
 * @param *s entries of the diagonal matrix
 * @return LAPACK return
 */
int kjg_gsl_SVD (gsl_matrix * M, gsl_matrix * V, gsl_vector * S);

#endif /* KJG_GSL_H_ */

max_stars_repo_name: nevrome/EIG

max_issues_repo_name: nevrome/EIG

max_forks_repo_name: nevrome/EIG


text:
#ifndef libceed_petsc_examples_bps_problem_data_h
#define libceed_petsc_examples_bps_problem_data_h

#include <ceed.h>
#include <petsc.h>
#include "../include/bcfunctions.h"
#include "../include/structs.h"
#include "../qfunctions/bps/bp1.h"
#include "../qfunctions/bps/bp2.h"
#include "../qfunctions/bps/bp3.h"
#include "../qfunctions/bps/bp4.h"
#include "../qfunctions/bps/common.h"

// -----------------------------------------------------------------------------
// BP Option Data
// -----------------------------------------------------------------------------

// BP options
typedef enum {
  CEED_BP1 = 0, CEED_BP2 = 1, CEED_BP3 = 2,
  CEED_BP4 = 3, CEED_BP5 = 4, CEED_BP6 = 5
} BPType;

BPData bp_options[6] = {
  [CEED_BP1] = {
    .num_comp_u = 1,
    .num_comp_x = 3,
    .topo_dim = 3,
    .q_data_size = 1,
    .q_extra = 1,
    .setup_geo = SetupMassGeo,
    .setup_rhs = SetupMassRhs,
    .apply = Mass,
    .error = Error,
    .setup_geo_loc = SetupMassGeo_loc,
    .setup_rhs_loc = SetupMassRhs_loc,
    .apply_loc = Mass_loc,
    .error_loc = Error_loc,
    .in_mode = CEED_EVAL_INTERP,
    .out_mode = CEED_EVAL_INTERP,
    .q_mode = CEED_GAUSS,
    .enforce_bc = PETSC_FALSE,
    .bc_func = BCsMass
  },
  [CEED_BP2] = {
    .num_comp_u = 3,
    .num_comp_x = 3,
    .topo_dim = 3,
    .q_data_size = 1,
    .q_extra = 1,
    .setup_geo = SetupMassGeo,
    .setup_rhs = SetupMassRhs3,
    .apply = Mass3,
    .error = Error3,
    .setup_geo_loc = SetupMassGeo_loc,
    .setup_rhs_loc = SetupMassRhs3_loc,
    .apply_loc = Mass3_loc,
    .error_loc = Error3_loc,
    .in_mode = CEED_EVAL_INTERP,
    .out_mode = CEED_EVAL_INTERP,
    .q_mode = CEED_GAUSS,
    .enforce_bc = PETSC_FALSE,
    .bc_func = BCsMass
  },
  [CEED_BP3] = {
    .num_comp_u = 1,
    .num_comp_x = 3,
    .topo_dim = 3,
    .q_data_size = 7,
    .q_extra = 1,
    .setup_geo = SetupDiffGeo,
    .setup_rhs = SetupDiffRhs,
    .apply = Diff,
    .error = Error,
    .setup_geo_loc = SetupDiffGeo_loc,
    .setup_rhs_loc = SetupDiffRhs_loc,
    .apply_loc = Diff_loc,
    .error_loc = Error_loc,
    .in_mode = CEED_EVAL_GRAD,
    .out_mode = CEED_EVAL_GRAD,
    .q_mode = CEED_GAUSS,
    .enforce_bc = PETSC_TRUE,
    .bc_func = BCsDiff
  },
  [CEED_BP4] = {
    .num_comp_u = 3,
    .num_comp_x = 3,
    .topo_dim = 3,
    .q_data_size = 7,
    .q_extra = 1,
    .setup_geo = SetupDiffGeo,
    .setup_rhs = SetupDiffRhs3,
    .apply = Diff3,
    .error = Error3,
    .setup_geo_loc = SetupDiffGeo_loc,
    .setup_rhs_loc = SetupDiffRhs3_loc,
    .apply_loc = Diff3_loc,
    .error_loc = Error3_loc,
    .in_mode = CEED_EVAL_GRAD,
    .out_mode = CEED_EVAL_GRAD,
    .q_mode = CEED_GAUSS,
    .enforce_bc = PETSC_TRUE,
    .bc_func = BCsDiff
  },
  [CEED_BP5] = {
    .num_comp_u = 1,
    .num_comp_x = 3,
    .topo_dim = 3,
    .q_data_size = 7,
    .q_extra = 0,
    .setup_geo = SetupDiffGeo,
    .setup_rhs = SetupDiffRhs,
    .apply = Diff,
    .error = Error,
    .setup_geo_loc = SetupDiffGeo_loc,
    .setup_rhs_loc = SetupDiffRhs_loc,
    .apply_loc = Diff_loc,
    .error_loc = Error_loc,
    .in_mode = CEED_EVAL_GRAD,
    .out_mode = CEED_EVAL_GRAD,
    .q_mode = CEED_GAUSS_LOBATTO,
    .enforce_bc = PETSC_TRUE,
    .bc_func = BCsDiff
  },
  [CEED_BP6] = {
    .num_comp_u = 3,
    .num_comp_x = 3,
    .topo_dim = 3,
    .q_data_size = 7,
    .q_extra = 0,
    .setup_geo = SetupDiffGeo,
    .setup_rhs = SetupDiffRhs3,
    .apply = Diff3,
    .error = Error3,
    .setup_geo_loc = SetupDiffGeo_loc,
    .setup_rhs_loc = SetupDiffRhs3_loc,
    .apply_loc = Diff3_loc,
    .error_loc = Error3_loc,
    .in_mode = CEED_EVAL_GRAD,
    .out_mode = CEED_EVAL_GRAD,
    .q_mode = CEED_GAUSS_LOBATTO,
    .enforce_bc = PETSC_TRUE,
    .bc_func = BCsDiff
  }
};

#endif // libceed_petsc_examples_bps_problem_data_h

max_stars_repo_name: wence-/libCEED

max_issues_repo_name: wence-/libCEED

max_forks_repo_name: wence-/libCEED


text:
#ifndef _params_h_
#define _params_h_

#include <stdio.h>
#include <time.h>
#include <unistd.h> //For getpid() function
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <fftw3.h>

#include <gsl/gsl_spline.h>
#include <gsl/gsl_interp.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_matrix.h>

//****Simulation Parameters:
#define N_ps (int) 64 //Number of points in PS file
#define N_gen (int) 4 //To generate the 21cm field
#define LEN (double) 8.0 //Length of simulation box (in Mpc h^{-1})
#define freq_width (double) 2.0 //Width of frequency channel (in MHz)

//****Foreground Removal Parameters:
double *expo;
int expo_array;
int count(int);

//****Function declarations:
double interp_ps_lin(double);
double read_21cm_cube(char *);
double sim_21cm_cube();
double generate_full_signal();
double count_freq_channels();
double noise_removal();

//****Multifrequency fitting declarations:
double *y, *x, *N, *Ni, *xtNi, *xtNix, *xtNix_i, *big_matrix, *a, *xa;

//****GSL rng declarations:
const gsl_rng_type *T;
gsl_rng *rand1, *rand2;
long seed;
double a_q, b_q;

//****Others:
int SimON, XY_pix, Z_pix, Npol; //XY_pix and Z_pix are the box dimensions in pixels
double frequency, original_21cm_mean, total_21cm_mean;
double *original_21cm_field, *total_21cm_field, *detector_noise_field, *total_noise_field , *recovered_21cm_field, *parameter_a_cube;
double k[N_ps], Pk[N_ps];
double *freq_array;
double sigma, nu, tau, offset;
double XY_LEN, Z_LEN; //Length of the box

FILE *file;
char cmnd[1000], header[100], polystr[100];
//****polystr is to store the exponents of variables of the fitting polynomial

#endif


text:
/* linalg/balance.c
 *
 * Copyright (C) 2006 Patrick Alken
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/* Balance a general matrix by scaling the rows and columns, so the
 * new row and column norms are the same order of magnitude.
 *
 * B =  D^-1 A D
 *
 * where D is a diagonal matrix
 *
 * This is necessary for the unsymmetric eigenvalue problem since the
 * calculation can become numerically unstable for unbalanced
 * matrices.
 *
 * See Golub & Van Loan, "Matrix Computations" (3rd ed), Section 7.5.7
 * and Wilkinson & Reinsch, "Handbook for Automatic Computation", II/11 p320.
 */

#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>

#include <gsl/gsl_linalg.h>

#define FLOAT_RADIX       2.0
#define FLOAT_RADIX_SQ    (FLOAT_RADIX * FLOAT_RADIX)

int
gsl_linalg_balance_matrix(gsl_matrix * A, gsl_vector * D)
{
  const size_t N = A->size1;

  if (N != D->size)
    {
      GSL_ERROR ("vector must match matrix size", GSL_EBADLEN);
    }
  else
    {
      double row_norm,
             col_norm;
      int not_converged;
      gsl_vector_view v;

      /* initialize D to the identity matrix */
      gsl_vector_set_all(D, 1.0);

      not_converged = 1;

      while (not_converged)
        {
          size_t i, j;
          double g, f, s;

          not_converged = 0;

          for (i = 0; i < N; ++i)
            {
              row_norm = 0.0;
              col_norm = 0.0;

              for (j = 0; j < N; ++j)
                {
                  if (j != i)
                    {
                      col_norm += fabs(gsl_matrix_get(A, j, i));
                      row_norm += fabs(gsl_matrix_get(A, i, j));
                    }
                }

              if ((col_norm == 0.0) || (row_norm == 0.0))
                {
                  continue;
                }

              g = row_norm / FLOAT_RADIX;
              f = 1.0;
              s = col_norm + row_norm;

              /*
               * find the integer power of the machine radix which
               * comes closest to balancing the matrix
               */
              while (col_norm < g)
                {
                  f *= FLOAT_RADIX;
                  col_norm *= FLOAT_RADIX_SQ;
                }

              g = row_norm * FLOAT_RADIX;

              while (col_norm > g)
                {
                  f /= FLOAT_RADIX;
                  col_norm /= FLOAT_RADIX_SQ;
                }

              if ((row_norm + col_norm) < 0.95 * s * f)
                {
                  not_converged = 1;

                  g = 1.0 / f;

                  /*
                   * apply similarity transformation D, where
                   * D_{ij} = f_i * delta_{ij}
                   */

                  /* multiply by D^{-1} on the left */
                  v = gsl_matrix_row(A, i);
                  gsl_blas_dscal(g, &v.vector);

                  /* multiply by D on the right */
                  v = gsl_matrix_column(A, i);
                  gsl_blas_dscal(f, &v.vector);

                  /* keep track of transformation */
                  gsl_vector_set(D, i, gsl_vector_get(D, i) * f);
                }
            }
        }

      return GSL_SUCCESS;
    }
} /* gsl_linalg_balance_matrix() */

/*
gsl_linalg_balance_accum()
  Accumulate a balancing transformation into a matrix.
This is used during the computation of Schur vectors since the
Schur vectors computed are the vectors for the balanced matrix.
We must at some point accumulate the balancing transformation into
the Schur vector matrix to get the vectors for the original matrix.

A -> D A

where D is the diagonal matrix

Inputs: A - matrix to transform
        D - vector containing diagonal elements of D
*/

int
gsl_linalg_balance_accum(gsl_matrix *A, gsl_vector *D)
{
  const size_t N = A->size1;

  if (N != D->size)
    {
      GSL_ERROR ("vector must match matrix size", GSL_EBADLEN);
    }
  else
    {
      size_t i;
      double s;
      gsl_vector_view r;

      for (i = 0; i < N; ++i)
        {
          s = gsl_vector_get(D, i);
          r = gsl_matrix_row(A, i);

          gsl_blas_dscal(s, &r.vector);
        }

      return GSL_SUCCESS;
    }
} /* gsl_linalg_balance_accum() */

max_stars_repo_name: ielomariala/Hex-Game

max_issues_repo_name: Brian-ning/HMNE

max_forks_repo_name: Brian-ning/HMNE


text:
/*
** Copyright (c) 2008, 2009   Gerald I. Evenden
*/
static const char
RCS_ID[] = "$Id: proj_gdinverse.c,v 5.6 2009/05/17 19:41:47 gie Exp gie $";
/*
** Permission is hereby granted, free of charge, to any person obtaining
** a copy of this software and associated documentation files (the
** "Software"), to deal in the Software without restriction, including
** without limitation the rights to use, copy, modify, merge, publish,
** distribute, sublicense, and/or sell copies of the Software, and to
** permit persons to whom the Software is furnished to do so, subject to
** the following conditions:
**
** The above copyright notice and this permission notice shall be
** included in all copies or substantial portions of the Software.
**
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
** EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
** MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
** IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
** CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
** TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
** SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
/** @file proj_gdinverse.c
 * @brief general inverse projection support
 * @author Gerald I. Evenden
 */
#include "config.h"
#include <stdlib.h>

#if HAVE_LIBGSL

#include <gsl/gsl_vector.h>
#include <gsl/gsl_multiroots.h>
#include <project.h>
#define LOOP_LIMIT 10

struct p_gsl { PROJ *P; double x, y; };
typedef struct p_gsl GP;
	static int
func_f(const gsl_vector *x, void *p, gsl_vector *f) {
//func_f(const gsl_vector *x, GP *p, gsl_vector *f) {
	PROJ_LP lp;

	lp.lam = gsl_vector_get(x,0);
	lp.phi = gsl_vector_get(x,1);
	PROJ_XY xy = (*((GP *)p)->P->fwd)(lp, ((GP *)p)->P);
	gsl_vector_set(f, 0, xy.x - ((GP *)p)->x);
	gsl_vector_set(f, 1, xy.y - ((GP *)p)->y);
	return GSL_SUCCESS;
}
	static int
func_df(const gsl_vector * x, void *p, gsl_matrix *J) {
	struct PROJ_DERIVS der;
	PROJ_LP lp;

	lp.lam = gsl_vector_get(x,0);
	lp.phi = gsl_vector_get(x,1);
	(*((GP *)p)->P->derivs)(((GP *)p)->P, lp, &der);
	gsl_matrix_set(J, 0, 0, der.x_l);
	gsl_matrix_set(J, 0, 1, der.x_p);
	gsl_matrix_set(J, 1, 0, der.y_l);
	gsl_matrix_set(J, 1, 1, der.y_p);
	return GSL_SUCCESS;
}
	static int
func_fdf(const gsl_vector * x, void *p, gsl_vector *f,  gsl_matrix *J) {
	func_f(x, p, f);
	func_df(x, p, J);
	return GSL_SUCCESS;
}
/* @brief determine projection inverse
 * @param[in] P projection control structure
 * @param est estimated geographic answer
 * @param[in] xy Cartesian location
 * @param[in] tol tolerance
 * @return ==0 if successful
 */
	int
proj_gdinverse(PROJ *P, PROJ_LP *est, PROJ_XY xy, double tol) {
	const gsl_multiroot_fdfsolver_type *T2;
	const gsl_multiroot_fsolver_type *T1;
	gsl_multiroot_fdfsolver *s2;
	gsl_multiroot_fsolver *s1;
	int status;
	size_t i, iter = 0;
	const size_t n = 2;
	GP p;
	int ders = P->derivs != NULL;
	gsl_multiroot_function_fdf f2 = {&func_f, &func_df, &func_fdf, n, &p};
	gsl_multiroot_function f1 = {&func_f, n, &p};

	p.P = P;
	p.x = xy.x;
	p.y = xy.y;
	gsl_vector *x = gsl_vector_alloc(n);
	gsl_vector_set(x, 0, est->lam);
	gsl_vector_set(x, 1, est->phi);
	if (ders) {
		T2 = gsl_multiroot_fdfsolver_gnewton;
		s2 = gsl_multiroot_fdfsolver_alloc(T2, n);
		gsl_multiroot_fdfsolver_set(s2, &f2, x);
	} else {
		T1 = gsl_multiroot_fsolver_hybrids;
		s1 = gsl_multiroot_fsolver_alloc(T1, n);
		gsl_multiroot_fsolver_set(s1, &f1, x);
	}
	do {
		++iter;
		status = ders ? gsl_multiroot_fdfsolver_iterate(s2):
						gsl_multiroot_fsolver_iterate(s1);
		if (status)
			break;
		status = gsl_multiroot_test_residual( ders?s2->f : s1->f, tol);
	} while (status == GSL_CONTINUE && iter < LOOP_LIMIT);
	if (ders) {
		est->lam = gsl_vector_get(s2->x,0);
		est->phi = gsl_vector_get(s2->x,1);
		gsl_multiroot_fdfsolver_free(s2);
	} else {
		est->lam = gsl_vector_get(s1->x,0);
		est->phi = gsl_vector_get(s1->x,1);
		gsl_multiroot_fsolver_free(s1);
	}
	gsl_vector_free(x);
	return 0;
}
#else // end LIBGSL true
/* the following is present to resolve any funky linking and to
 * ensure the system fails if gdinverse is called when the GSL flag
 * is not set.
*/
#include <stdio.h>
#include <project.h>
	int
proj_gdinverse(PROJ *P, PROJ_LP *est, PROJ_XY xy, double tol) {
	fprintf(stderr,"libproj4: entered gdinverse---no GSL avail. See admin\n");
	exit(-1);
	return -1;
}
#endif // end LIBGSL condition
/*
 * $Log:
 */


text:
/*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
**                                                                                **
** This file forms part of the Underworld geophysics modelling application.         **
**                                                                                **
** For full license and copyright information, please refer to the LICENSE.md file  **
** located at the project root, or contact the authors.                             **
**                                                                                **
**~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*/
/*

Given a block system
A x = b,
or
( K G )(u) = (f)
( D C )(p)   (h)

We define a symmetrically scaled system,
L A R R^-1 x = L b,		A'x' = b'
where L = diag(L1, L2), R = diag(R1,R2) are block diagonal
and L_i and R_i are both diagonal matrirces.

The scaling produces
( L1   )( K G )( R1   )( R1   )^-1(u) = ( L1   )(f)
(   L2 )( D C )(   R2 )(   R2 )   (p)   (   L2 )(h)
and we solve
( L1   )( K G )( R1   )(v) = ( L1   )(f)
(   L2 )( D C )(   R2 )(q)   (   L2 )(h)
or
( L1KR1 L1GR2 )(v) = (L1f)
( L2DR1 L2CR2 )(q)   (L2h)

The solution u,p is recovered via
(u) = ( R1   )(v)
(p)   (   R2 )(q)

*/

#include <petsc.h>
#include <petscmat.h>
#include <petscvec.h>

#include <StGermain/StGermain.h>
#include <StgDomain/StgDomain.h>

#include "common-driver-utils.h"
#include "stokes_block_scaling.h"

/* private prototypes */
PetscErrorCode BSSCR_MatStokesBlock_ApplyScaling( MatStokesBlockScaling BA, Mat A, Vec b, Vec x, Mat S, PetscTruth sym );

/* A x = b -> A'x' = b' */
/*
Note this routine actually modifies the matrix and rhs b.
*/

// updated
PetscErrorCode BSSCR_MatStokesBlock_ApplyScaling( MatStokesBlockScaling BA, Mat A, Vec b, Vec x, Mat S, PetscTruth sym )
{
	Mat K,G,D,C;
	Vec L1,L2, R1,R2;
	Vec f,h, u,p;

	/* Get the scalings out the block mat data */
	VecNestGetSubVec( BA->Lz, 0, &L1 );
	VecNestGetSubVec( BA->Lz, 1, &L2 );
	VecNestGetSubVec( BA->Rz, 0, &R1 );
	VecNestGetSubVec( BA->Rz, 1, &R2 );

	/* get the subblock solution and rhs */
	if( x != PETSC_NULL ) {
		VecNestGetSubVec( x, 0, &u );
		VecNestGetSubVec( x, 1, &p );

		VecPointwiseDivide( u, u,R1); /* x <- x * 1/R1 */
		VecPointwiseDivide( p, p,R2);

	}
	if( b != PETSC_NULL ) {
		VecNestGetSubVec( b, 0, &f );
		VecNestGetSubVec( b, 1, &h );

		VecPointwiseMult( f, f,L1); /* f <- f * L1 */
		VecPointwiseMult( h, h,L2);

	}

	/* Scale matrices */
	MatNestGetSubMat( A, 0,0, &K );
	MatNestGetSubMat( A, 0,1, &G );
	MatNestGetSubMat( A, 1,0, &D );
	MatNestGetSubMat( A, 1,1, &C );

	if( K != PETSC_NULL ) {		MatDiagonalScale( K, L1,R1 );		}
	if( G != PETSC_NULL ) {		MatDiagonalScale( G, L1,R2 );		}
	if( D != PETSC_NULL && !sym ) {	MatDiagonalScale( D, L2,R1 );		}
	if( C != PETSC_NULL ) {		MatDiagonalScale( C, L2,R2 );		}
	if( S != PETSC_NULL ) {		MatDiagonalScale( S, L2,R2 );		}

	PetscFunctionReturn(0);
}
PetscErrorCode BSSCR_MatStokesBlock_ApplyScaling2( MatStokesBlockScaling BA, Mat A, Vec b, Vec x, Mat S, Mat M, PetscTruth sym )
{
	Mat K,G,D,C;
	Vec L1,L2, R1,R2;
	Vec f,h, u,p;

	/* Get the scalings out the block mat data */
	VecNestGetSubVec( BA->Lz, 0, &L1 );
	VecNestGetSubVec( BA->Lz, 1, &L2 );
	VecNestGetSubVec( BA->Rz, 0, &R1 );
	VecNestGetSubVec( BA->Rz, 1, &R2 );

	/* get the subblock solution and rhs */
	if( x != PETSC_NULL ) {
		VecNestGetSubVec( x, 0, &u );
		VecNestGetSubVec( x, 1, &p );

		VecPointwiseDivide( u, u,R1); /* x <- x * 1/R1 */
		VecPointwiseDivide( p, p,R2);

	}
	if( b != PETSC_NULL ) {
		VecNestGetSubVec( b, 0, &f );
		VecNestGetSubVec( b, 1, &h );

		VecPointwiseMult( f, f,L1); /* f <- f * L1 */
		VecPointwiseMult( h, h,L2);

	}

	/* Scale matrices */
	MatNestGetSubMat( A, 0,0, &K );
	MatNestGetSubMat( A, 0,1, &G );
	MatNestGetSubMat( A, 1,0, &D );
	MatNestGetSubMat( A, 1,1, &C );

	if( K != PETSC_NULL ) {		MatDiagonalScale( K, L1,R1 );		}
	if( G != PETSC_NULL ) {		MatDiagonalScale( G, L1,R2 );		}
	if( D != PETSC_NULL && !sym ) {	MatDiagonalScale( D, L2,R1 );		}
	if( C != PETSC_NULL ) {		MatDiagonalScale( C, L2,R2 );		}
	if( S != PETSC_NULL ) {		MatDiagonalScale( S, L2,R2 );		}
	if( M != PETSC_NULL ) {		MatDiagonalScale( M, L2,R2 );		}

	PetscFunctionReturn(0);
}

// updated
PetscErrorCode BSSCR_MatStokesBlockScalingCreate( MatStokesBlockScaling *_BA )
{
	MatStokesBlockScaling BA;

	PetscMalloc( sizeof(struct _p_MatStokesBlockScaling), &BA );

	BA->Lz = PETSC_NULL;
	BA->Rz = PETSC_NULL;

	BA->scaling_exists              = PETSC_FALSE;
	BA->scalings_have_been_inverted = PETSC_FALSE;
	BA->system_has_been_scaled      = PETSC_FALSE;

	*_BA = BA;

	PetscFunctionReturn(0);
}

// updated
PetscErrorCode BSSCR_MatStokesBlockScalingDestroy( MatStokesBlockScaling BA )
{

	if( BA->scaling_exists == PETSC_FALSE ) PetscFunctionReturn(0);

	if( BA->Lz != PETSC_NULL ) {
		Stg_VecDestroy(&BA->Lz );
		BA->Lz = PETSC_NULL;
	}

	if( BA->Rz != PETSC_NULL ) {
		Stg_VecDestroy(&BA->Rz );
		BA->Rz = PETSC_NULL;
	}

	PetscFree( BA );

	PetscFunctionReturn(0);
}

/*
A is 2x2 block matrix
b and x are 2x1 block vectors
*/
// updated
PetscErrorCode BSSCR_MatBlock_ConstructScaling( MatStokesBlockScaling BA, Mat A, Vec b, Vec x )
{
    if( BA->scaling_exists == PETSC_FALSE ) {

	VecDuplicate( x, &BA->Lz );
	VecDuplicate( x, &BA->Rz );

	BA->scaling_exists = PETSC_TRUE;
	BSSCR_MatStokesBlockDefaultBuildScaling( BA,A );
	BA->scalings_have_been_inverted = PETSC_FALSE;
    }

    PetscFunctionReturn(0);
}

// updated
PetscErrorCode BSSCR_mat_block_invert_scalings( MatStokesBlockScaling BA )
{
	Vec L1,L2, R1,R2;

	VecNestGetSubVec( BA->Lz, 0, &L1 );
	VecNestGetSubVec( BA->Lz, 1, &L2 );
	VecNestGetSubVec( BA->Rz, 0, &R1 );
	VecNestGetSubVec( BA->Rz, 1, &R2 );

	VecReciprocal(L1);
	VecReciprocal(L2);
	VecReciprocal(R1);
	VecReciprocal(R2);

	/* toggle inversion flag */
	if( BA->scalings_have_been_inverted == PETSC_TRUE ) {   BA->scalings_have_been_inverted = PETSC_FALSE;  }
	if( BA->scalings_have_been_inverted == PETSC_FALSE ) {  BA->scalings_have_been_inverted = PETSC_TRUE;   }

	PetscFunctionReturn(0);
}

/* updated */
PetscErrorCode BSSCR_MatStokesBlockScaleSystem( MatStokesBlockScaling BA, Mat A, Vec b, Vec x, Mat S, PetscTruth sym )
{
	if( BA->scaling_exists == PETSC_FALSE ) {
		BSSCR_MatBlock_ConstructScaling( BA,A,b,x );
	}

	if( BA->scalings_have_been_inverted == PETSC_TRUE ) {
		BSSCR_mat_block_invert_scalings(BA); /* to undo inversion */
	}

	BSSCR_MatStokesBlock_ApplyScaling(BA,A,b,x,S,sym);
	BA->system_has_been_scaled = PETSC_TRUE;

	/*
	PetscPrintf( PETSC_COMM_WORLD, "Post Scaling \n");
	MatBlock_ReportOperatorScales(A);
	*/

	PetscFunctionReturn(0);
}
PetscErrorCode BSSCR_MatStokesBlockScaleSystem2( MatStokesBlockScaling BA, Mat A, Vec b, Vec x, Mat S, Mat M, PetscTruth sym )
{
	if( BA->scaling_exists == PETSC_FALSE ) {
		BSSCR_MatBlock_ConstructScaling( BA,A,b,x );
	}

	if( BA->scalings_have_been_inverted == PETSC_TRUE ) {
		BSSCR_mat_block_invert_scalings(BA); /* to undo inversion */
	}

	BSSCR_MatStokesBlock_ApplyScaling2(BA,A,b,x,S,M,sym);
	BA->system_has_been_scaled = PETSC_TRUE;

	/*
	PetscPrintf( PETSC_COMM_WORLD, "Post Scaling \n");
	MatBlock_ReportOperatorScales(A);
	*/

	PetscFunctionReturn(0);
}

// updated
PetscErrorCode BSSCR_MatStokesBlockUnScaleSystem( MatStokesBlockScaling BA, Mat A, Vec b, Vec x, Mat S, PetscTruth sym )
{

	if( BA->system_has_been_scaled == PETSC_FALSE ) {
		printf("Warning: MatBlock has not been scaled !! \n");
		PetscFunctionReturn(0);
	}

	if( BA->scalings_have_been_inverted == PETSC_FALSE ) {
		BSSCR_mat_block_invert_scalings(BA);
	}

	BSSCR_MatStokesBlock_ApplyScaling(BA,A,b,x,S,sym);
	BA->system_has_been_scaled = PETSC_FALSE;

	PetscFunctionReturn(0);
}

PetscErrorCode BSSCR_MatStokesBlockReportOperatorScales( Mat A, PetscTruth sym )
{
	Vec rA, rG;
	PetscInt loc;
	PetscReal min, max;
	Mat K,G,D,C;

	MatNestGetSubMat( A, 0,0, &K );
	MatNestGetSubMat( A, 0,1, &G );
	MatNestGetSubMat( A, 1,0, &D );
	MatNestGetSubMat( A, 1,1, &C );

	MatGetVecs( K, PETSC_NULL, &rA );
	VecDuplicate( rA, &rG );

	/* Report the row max and mins */
	if (K!=PETSC_NULL) {
                PetscReal KNorm;
		MatGetRowMax( K, rA, PETSC_NULL );
		VecMax( rA, &loc, &max );
		PetscPrintf( PETSC_COMM_WORLD, "Sup_max(K) = %g \n", max );

		MatGetRowMinAbs( K, rA, PETSC_NULL );
		VecMin( rA, &loc, &min );
		PetscPrintf( PETSC_COMM_WORLD, "Sup_min(K) = %g \n", min );

		MatNorm(K,NORM_1,&KNorm);
		PetscPrintf( PETSC_COMM_WORLD, "norm_1(K)  = %g \n\n",KNorm);
	}

	if( G != PETSC_NULL ) {
		MatGetRowMax( G, rG, PETSC_NULL );
		VecMax( rG, &loc, &max );
		PetscPrintf( PETSC_COMM_WORLD, "Sup_max(G) = %g \n", max );

		MatGetRowMinAbs( G, rG, PETSC_NULL );
		VecMin( rG, &loc, &min );
		PetscPrintf( PETSC_COMM_WORLD, "Sup_min(G) = %g \n", min );
	}

	if( D != PETSC_NULL && !sym ) {
                Vec rD;

                MatGetVecs( D, PETSC_NULL, &rD );
		MatGetRowMax( D, rD, PETSC_NULL );
		VecMax( rD, &loc, &max );
		PetscPrintf( PETSC_COMM_WORLD, "Sup_max(D) = %g \n", max );

		MatGetRowMinAbs( D, rD, PETSC_NULL );
		VecMin( rD, &loc, &min );
		PetscPrintf( PETSC_COMM_WORLD, "Sup_min(D) = %g \n", min );

                Stg_VecDestroy(&rD );
	}

	if( C != PETSC_NULL ) {
		Vec cG;

		MatGetVecs( G, &cG, PETSC_NULL );
		MatGetRowMax( C, cG, PETSC_NULL );
		VecMax( cG, &loc, &max );
		PetscPrintf( PETSC_COMM_WORLD, "Sup_max(C) = %g \n", max );

		MatGetRowMin( C, cG, PETSC_NULL );
		VecMin( cG, &loc, &min );
		PetscPrintf( PETSC_COMM_WORLD, "Sup_min(C) = %g \n\n", min );

		Stg_VecDestroy(&cG);
	}

	Stg_VecDestroy(&rA );
	Stg_VecDestroy(&rG );

	PetscFunctionReturn(0);
}

// updated
PetscErrorCode BSSCR_MatStokesBlockDefaultBuildScaling( MatStokesBlockScaling BA, Mat A )
{
	Mat K,G,D,C;
	Vec rG;
	PetscScalar rg2, rg, ra;
	PetscInt N;
	Vec rA, rC;
	Vec L1,L2, R1,R2;

	VecNestGetSubVec( BA->Lz, 0, &L1 );
	VecNestGetSubVec( BA->Lz, 1, &L2 );

	VecNestGetSubVec( BA->Rz, 0, &R1 );
	VecNestGetSubVec( BA->Rz, 1, &R2 );

	rA = L1;
	rC = L2;

	MatNestGetSubMat( A, 0,0, &K );
	MatNestGetSubMat( A, 0,1, &G );
	MatNestGetSubMat( A, 1,0, &D );
	MatNestGetSubMat( A, 1,1, &C );

	VecDuplicate( rA, &rG );

	/* Get magnitude of K */
	//px_MatGetAbsRowSum( K, rA );
	MatGetRowMax( K, rA, PETSC_NULL );

	VecSqrt( rA );
	VecReciprocal( rA );

	VecDot( rA,rA, &ra );
	VecGetSize( rA, &N );
	ra = PetscSqrtScalar( ra/N );

	/* Get magnitude of G */
	//px_MatGetAbsRowSum( G, rG );
	MatGetRowMax( G, rG, PETSC_NULL );

	VecDot( rG, rG, &rg2 );
	VecGetSize( rG, &N );
	rg = PetscSqrtScalar(rg2/N);
	//      printf("rg = %f \n", rg );

	VecSet( rC, 1.0/(rg*ra) );

	Stg_VecDestroy(&rG );

	VecCopy( L1, R1 );
	VecCopy( L2, R2 );

	PetscFunctionReturn(0);
}


text:
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "../mersenne.h"
#include <lapacke.h>
#include <time.h>
#include <sys/time.h>

#ifndef M_PI
#define M_PI 3.14159265358979323846264338327
#endif

/* Lattice size and dimensions */
#define NT 4
#define NX 4

#define ND 2
#define NDIRS (2*ND)

#define TUP 0
#define XUP 1
#define TDN 2
#define XDN 3

#define VOLUME (NT*NX)

//#define ANTISYMMETRIC //Antisymmetric boundaries
#define OPENX       //Antisymmetric in time, open in space

/* Enumerate possible values for a field */
#define MONOMER 1
#define LINK_TUP (2+TUP)   // Links enumerated as 2+direction
#define LINK_XUP (2+XUP)
#define LINK_TDN (2+TDN)
#define LINK_XDN (2+XDN)
#define SOURCE_MONOMER (2+NDIRS)
#define EMPTY -100   //A meta value for sites that don't exist

#define CG_ACCURACY 1e-20
#define CG_MAX_ITER 10000

/* Propability of exiting in the monomer moving worm update */
#define flip_exit_propability 0.2

#define FLUCTUATION_MATRIX
#define WITH_MASS_MONOMERS
#define PROPAGATOR_MATRIX

/* Functions in vec_ops.c */
void vec_zero( double **a );
void vec_one(double **a);
void vec_add(double **a, double **b);
void vec_d_mul(double **a, double d);
void vec_zero_occupied(double **a);
void free_vector(double ** a);
double ** alloc_vector();
void cg_propagator( double **propagator, double **source );

void fM( double **chi, double **psi );
void fM_transpose(double **chi, double **psi );

int cg_MdM_occupied( double *chi, double *psi );
void fM_occupied( double *chi, double *psi );
double action(double *psi);
void vec_gaussian(double *a);
double * alloc_field();


text:
#ifndef CONSTITUTIVE_DF_H
#define CONSTITUTIVE_DF_H

#include <algorithm>
#include <numeric>
#include "SIMD_BDB_Integrator.h"

#ifdef HAS_MKL
#include <mkl.h>
#else
#include <cblas.h>
#endif

inline void GetTotalTraction_(Real *TotalTraction, const Real *CauchyStressTensor, int ndim) {
    if (ndim==3) {
        TotalTraction[0] = CauchyStressTensor[0];
        TotalTraction[1] = CauchyStressTensor[4];
        TotalTraction[2] = CauchyStressTensor[8];
        TotalTraction[3] = CauchyStressTensor[1];
        TotalTraction[4] = CauchyStressTensor[2];
        TotalTraction[5] = CauchyStressTensor[5];
    }
    else if (ndim == 2) {
        TotalTraction[0] = CauchyStressTensor[0];
        TotalTraction[1] = CauchyStressTensor[3];
        TotalTraction[2] = CauchyStressTensor[1];
    }
}

inline void FillConstitutiveB_(Real *B, const Real* SpatialGradient,
                     int ndim, int nvar, int rows, int cols) {
    int i = 0;

    if (ndim == 3) {

        for (; i<rows; ++i) {

            // Store in registers
            const Real a0 = SpatialGradient[i*ndim];
            const Real a1 = SpatialGradient[i*ndim+1];
            const Real a2 = SpatialGradient[i*ndim+2];

            // MECHANICAL TERMS
            B[i*cols*nvar]              = a0;
            B[i*cols*nvar+cols+1]       = a1;
            B[i*cols*nvar+2*(cols+1)]   = a2;

            B[i*cols*nvar+cols+5]       = a2;
            B[i*cols*nvar+2*cols+5]     = a1;

            B[i*cols*nvar+4]            = a2;
            B[i*cols*nvar+2*cols+4]     = a0;

            B[i*cols*nvar+3]            = a1;
            B[i*cols*nvar+cols+3]       = a0;
        }
    }

    else if (ndim == 2) {

        for (; i<rows; ++i) {

            // Store in registers
            const Real a0 = SpatialGradient[i*ndim];
            const Real a1 = SpatialGradient[i*ndim+1];

            // MECHANICAL TERMS
            B[i*cols*nvar]              = a0;
            B[i*cols*nvar+cols+1]       = a1;

            B[i*cols*nvar+2]            = a1;
            B[i*cols*nvar+cols+2]       = a0;
        }
    }
}

inline void _ConstitutiveStiffnessIntegrandDF_Filler_(
    Real *stiffness,
    Real *traction,
    const Real* SpatialGradient,
    const Real* CauchyStressTensor,
    const Real* H_Voigt,
    const Real* detJ,
    int ngauss,
    int noderpelem,
    int ndim,
    int nvar,
    int H_VoigtSize,
    int requires_geometry_update) {

    int local_size = nvar*noderpelem;

    Real *t;
    if (ndim==3) { t = allocate<Real>(6);}
    else if (ndim==2) { t =  allocate<Real>(3);}

    Real *B = allocate<Real>(H_VoigtSize*local_size);
    Real *HBT = allocate<Real>(H_VoigtSize*local_size);
    Real *BDB_1 = allocate<Real>(local_size*local_size);

    std::fill(B,B+H_VoigtSize*local_size,0.);

    for (int igauss = 0; igauss < ngauss; ++igauss) {

        FillConstitutiveB_(B,&SpatialGradient[igauss*ndim*noderpelem],ndim,nvar,noderpelem,H_VoigtSize);
        // if (ndim == 3) {
            cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasTrans,
                H_VoigtSize, local_size, H_VoigtSize, 1.0, &H_Voigt[igauss*H_VoigtSize*H_VoigtSize], H_VoigtSize, B, H_VoigtSize, 0.0, HBT, local_size);

            cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans,
                local_size, local_size, H_VoigtSize, 1.0, B, H_VoigtSize, HBT, local_size, 0.0, BDB_1, local_size);
        // }
        // else
        // {
        //     _SIMD_BDB_Integrator_DF_2D_(
        //         HBT,
        //         BDB_1,
        //         &SpatialGradient[igauss*ndim*noderpelem],
        //         &H_Voigt[igauss*H_VoigtSize*H_VoigtSize],
        //         noderpelem
        //         );
        // }

        // Multiply stiffness with detJ
        const Real detJ_igauss = detJ[igauss];
        for (int i=0; i<local_size*local_size; ++i) {
            stiffness[i] += BDB_1[i]*detJ_igauss;
        }

        if (requires_geometry_update==1) {
            // Compute tractions
            GetTotalTraction_(t, &CauchyStressTensor[igauss*ndim*ndim], ndim);

            // Multiply B with traction - for loop is okay
            for (int i=0; i<local_size; ++i) {
                Real tmp = 0;
                for (int j=0; j<H_VoigtSize; ++j) {
                    tmp += B[i*H_VoigtSize+j]*t[j];
                }
                // local_traction[i] = tmp;
                traction[i] += tmp*detJ_igauss;
            }
        }
    }

    deallocate(t);
    deallocate(B);
    deallocate(HBT);
    deallocate(BDB_1);
}

#endif

max_stars_repo_name: jdlaubrie/florence

max_issues_repo_name: jdlaubrie/florence

max_forks_repo_name: jdlaubrie/florence