示例#1
0
void HistoryInteraction::build_coefficient_table()
{
  Interpolation::UniformLagrangeSet lagrange(interp_order);

  for(int pair_idx = 0; pair_idx < num_interactions; ++pair_idx) {
    int src, obs;
    std::tie(src, obs) = idx2coord(pair_idx);

    Vec3d dr(separation((*dots)[src], (*dots)[obs]));

    std::pair<int, double> delay(
        split_double(dr.norm() / (config.c0 * config.dt)));

    floor_delays[pair_idx] = delay.first;
    lagrange.calculate_weights(delay.second, config.dt);

    std::vector<Eigen::Matrix3cd> interp_dyads(
        dyadic->coefficients(dr, lagrange));

    for(int i = 0; i <= interp_order; ++i) {
      coefficients[pair_idx][i] =
          (*dots)[obs].dipole().dot(interp_dyads[i] * (*dots)[src].dipole());
    }
  }
}
示例#2
0
void save_field(double *xx, double *qq, int elem_num, double *roots, int eres)
{
	double dx, plot_coords[eres], ll[np], xcoord, ycoord;
	int ii, ee;

	dx = 2. / (eres - 1);
	plot_coords[0] = -1;
	plot_coords[eres - 1] = 1;
	for (ii = 1; ii < eres - 1; ++ii)
	{
		plot_coords[ii] = -1 + ii * dx;
	}

	FILE *ff;
	ff = fopen("data2.txt", "w");

	for (ee = 0; ee < elem_num; ++ee)
	{
		for (ii = 0; ii < eres; ++ii)
		{
			lagrange(plot_coords[ii], ll, roots);
			xcoord = dot_product(ll, xx + ee * np, np);
			ycoord = dot_product(ll, qq + ee * np, np);
			fprintf(ff, "%21.6f   %21.6f\n", xcoord, ycoord);
		}
	}

	fclose(ff);
	printf("output file saved!\n");
}
示例#3
0
int main(void){
	linear();
	newton2();
	newton3();
	lagrange();
	return 0;
}
int main()
{
    double pvalueX=0.0;
    for(i=0;i<n;i++)
    {
            pvalueX += fx[i] * lagrange(i);
    }
    printf("\nthe required value is %lf",pvalueX);            
    getch();
    return 0;
}
示例#5
0
int main(int argc, const char *argv[])
{
    float x[MAX],y[MAX],result,xx;
    int i,n;
    scanf("%d",&n);
    for(i=0;i<n;i++)
        scanf("%f %f",&x[i],&y[i]);
    scanf("%f",&xx);
    result = lagrange(x,y,n,xx);
    printf("x=%f, y=%f\n",xx,result);
    return 0;
}
//metodo que inicia o metodo lagrange
void initLagrange(){
     int retorno;
     retorno = lagrange();
     
     if ( retorno == -3){
        printf("\n\n\tFalta de memoria!!!\n\n");
     } else if ( retorno == -2){
        printf("\n\n\tERRO: a0 nao pode ser igual a zero!!!\n\n");
     } else if ( retorno == -1){
        printf("\n\n\tERRO: an nao pode ser menor ou igual a zero!!!\n\n");
     }
}
示例#7
0
 float2 getPoint(float t) //calculates a point from the control points
 {
     float2 r(0.0, 0.0);
     double weight;
     // for every control point
     for (float i = 0; i < controlPoints.size(); i++) {
         // compute weight using the Bernstein formula
         weight = lagrange(i, controlPoints.size()-1, t);
         r += controlPoints.at(i)*weight;
     }
     // add control point to r, weighted
     return r;
 }
示例#8
0
float2 Lagrange::getPoint(float t)
{
    float2 r(0.0, 0.0);
    // for every control point
    // compute weight using the Lagrange summation formula
    // add control point to r, weighted
    int siz = (int)controlPoints.size();
    for (int i = 0; i < siz; i++) {
        float2 ri = controlPoints.at(i);
        ri *= lagrange(i, t);
        r += ri;
    }
    return r;
}
示例#9
0
void main(void)
   {
   int gd = DETECT, gm;
   double points[6][2] = {-3, 2, -2, -1.5, -1, 0, 0, 0, 1, 1, 2, -1.5};
   node *poly;
   if ((poly = lagrange(points, 6)) != NULL)
	print_polynomial(poly, "poly = ");
   else printf("Impossible !!!!");
   getch();
   initgraph(&gd, &gm, "c:\\tcpp\\bgi");
   draw_points(points, 6, 0, 0, 100);
   draw(poly, 0, 0, -5, 5, 100, 0.05);
   getch();
   closegraph();
   }
int main(void)
{
  
   int i;
   
   float point_x=2.5;
   float value_fx = 0.0;
   for( i=0;i<n;i++)
   {
          value_fx += fx[i]*lagrange(i,point_x);
           
   }
   printf("The value of Point_x : %f",value_fx);
   getch();
   return 0;
}
示例#11
0
void
vpPose::poseLagrangeNonPlan(vpHomogeneousMatrix &cMo)
{

  if (DEBUG_LEVEL1)
    std::cout << "begin CPose::PoseLagrange(...) " << std::endl ;

  try{
    double s;
    int i;

    int k=0;
    int nl=npt*2;


    vpMatrix a(nl,3)  ;
    vpMatrix b(nl,9);
    b =0 ;

    vpPoint P ;
    listP.front() ;
    i=0 ;
    while (!listP.outside())
    {
      P= listP.value() ;
      a[k][0]   = -P.get_oX();
      a[k][1]   = 0.0;
      a[k][2]   = P.get_oX()*P.get_x();

      a[k+1][0] = 0.0;
      a[k+1][1] = -P.get_oX();
      a[k+1][2] = P.get_oX()*P.get_y();

      b[k][0]   = -P.get_oY();
      b[k][1]   = 0.0;
      b[k][2]   = P.get_oY()*P.get_x();

      b[k][3]   = -P.get_oZ();
      b[k][4]   =  0.0;
      b[k][5]   =  P.get_oZ()*P.get_x();

      b[k][6]   =  -1.0;
      b[k][7]   =  0.0;
      b[k][8]   =  P.get_x();

      b[k+1][0] =  0.0;
      b[k+1][1] = -P.get_oY();
      b[k+1][2] =  P.get_oY()*P.get_y();

      b[k+1][3] =  0.0;
      b[k+1][4] = -P.get_oZ();
      b[k+1][5] =  P.get_oZ()*P.get_y();


      b[k+1][6] =  0.0;
      b[k+1][7] = -1.0;
      b[k+1][8] =  P.get_y();

      k += 2;
      listP.next() ;
    }
    vpColVector X1(3) ;
    vpColVector X2(9) ;

    if (DEBUG_LEVEL2)
    {
      std::cout <<"a " << a << std::endl ;
      std::cout <<"b " << b << std::endl ;
    }

    lagrange(a,b,X1,X2);
    //  if (err != OK)
    {
      //      std::cout << "in (CLagrange.cc)Lagrange returns " ;
      //    PrintError(err) ;
      //    return err ;
    }


    if (DEBUG_LEVEL2)
    {
      std::cout << "ax1+bx2 (devrait etre 0) " << (a*X1 + b*X2).t() << std::endl ;
      std::cout << "norme X1 " << X1.sumSquare() << std::endl ;;
    }

    if (X2[8] < 0.0)
    {		/* car Zo > 0	*/
      X1 *= -1 ;
      X2 *= -1 ;
    }
    s = 0.0;
    for (i=0;i<3;i++) {s += (X1[i]*X2[i]);}
    for (i=0;i<3;i++)  {X2[i] -= (s*X1[i]);} /* X1^T X2 = 0	*/

    s = 0.0;
    for (i=0;i<3;i++)  {s += (X2[i]*X2[i]);}

    if (s<1e-10)
    {
      vpERROR_TRACE(" division par zero " ) ;
      throw(vpException(vpException::divideByZeroError,
			"division by zero  ")) ;

    }

    s = 1.0/sqrt(s);
    for (i=0;i<3;i++)  {X2[i] *= s;}		/* X2^T X2 = 1	*/

    X2[3] = (X1[1]*X2[2])-(X1[2]*X2[1]);
    X2[4] = (X1[2]*X2[0])-(X1[0]*X2[2]);
    X2[5] = (X1[0]*X2[1])-(X1[1]*X2[0]);

    calculTranslation (a, b, nl, 3, 6, X1, X2) ;


    for (i=0 ; i<3 ; i++)
    {
      cMo[i][0] = X1[i];
      cMo[i][1] = X2[i];
      cMo[i][2] = X2[i+3];
      cMo[i][3] = X2[i+6];
    }

  }
  catch(...)
  {
    vpERROR_TRACE(" ") ;
    throw ;
  }

  if (DEBUG_LEVEL1)
    std::cout << "end vpCalculPose::PoseLagrange(...) " << std::endl ;
}
示例#12
0
/**
 * Build a sbox on polynomial form from a tabuled sbox
 * sbox : input
 * polySbox : result
 */
void buildPolySbox(byte sbox[256], byte polySbox[256]) {
  lagrange(sbox, polySbox, 255);
  revertTab(polySbox, 256);
}
示例#13
0
float
delayline::delay(float smps, float time_, int tap_, int touch,
                 int reverse)
{
    int dlytime = 0;
    int bufptr = 0;

    tap = fabs(tap_);
    if (tap >= maxtaps)
        tap = 0;

    if (reverse)  avgtime[tap] = alpha * 2.0*time_ + beta * avgtime[tap];	//smoothing the rate of time change
    else avgtime[tap] = alpha * time_ + beta * avgtime[tap];	//smoothing the rate of time change
    time[tap] = 1.0f + fSAMPLE_RATE * avgtime[tap];	//convert to something that can be used as a delay line index

//Do some checks to keep things in bounds
    if (time[tap] > maxtime)
        time[tap] = maxtime;
    if (time[tap] < 0.0f)
        time[tap] = 0.0f;

    float fract = (time[tap] - floorf(time[tap]));	//compute fractional delay
    dlytime = lrintf(floorf(time[tap]));

//now put in the sample
    if (touch) {		//make touch zero if you only want to pull samples off the delay line
        cur_smps[tap] = ringbuffer[zero_index] = smps;
        if (--zero_index < 0)
            zero_index = maxdelaysmps - 1;
    }
//if we want reverse delay
//you need to call this every time to keep the buffers up to date, and it's on a different tap
    if (reverse) {

        bufptr = (dlytime + zero_index);	//this points to the sample we want to get
        if (bufptr >= maxdelaysmps)
            bufptr -= maxdelaysmps;
        if (++rvptr > maxdelaysmps)
            rvptr = 0;

        if (bufptr > zero_index) {
            if (rvptr > bufptr) {
                rvptr = zero_index;
                distance = 0;
            } else
                distance = rvptr - zero_index;
        } else if ((bufptr < zero_index) && (rvptr < zero_index)) {
            if (rvptr > bufptr) {
                rvptr = zero_index;
                distance = 0;
            } else
                distance =
                    rvptr + maxdelaysmps - zero_index;
        } else
            distance = rvptr - zero_index;

        bufptr = rvptr;	//this points to the sample we want to get

    } else {
        bufptr = (dlytime + zero_index);	//this points to the sample we want to get
        if (bufptr >= maxdelaysmps)
            bufptr -= maxdelaysmps;
    }

    tapstruct[tap].lvars[3] = tapstruct[tap].lvars[2];
    tapstruct[tap].lvars[2] = tapstruct[tap].lvars[1];
    tapstruct[tap].lvars[1] = tapstruct[tap].lvars[0];
    tapstruct[tap].lvars[0] = ringbuffer[bufptr];

    tapstruct[tap].ivars[3] = tapstruct[tap].ivars[2];
    tapstruct[tap].ivars[2] = tapstruct[tap].ivars[1];
    tapstruct[tap].ivars[1] = tapstruct[tap].ivars[0];
    tapstruct[tap].ivars[0] = cur_smps[tap];

    tapstruct[tap].fracts[3] = tapstruct[tap].fracts[2];
    tapstruct[tap].fracts[2] = tapstruct[tap].fracts[1];
    tapstruct[tap].fracts[1] = tapstruct[tap].fracts[0];
    tapstruct[tap].fracts[0] = fract;

    float tmpfrac =
        0.5f * (tapstruct[tap].fracts[1] + tapstruct[tap].fracts[2]);
    //float itmpfrac = 1.0f - tmpfrac;
    float itmpfrac = 0.5f;  //it was the original approximation 

    float output =
        mix * lagrange(tapstruct[tap].ivars[0],
                       tapstruct[tap].ivars[1],
                       tapstruct[tap].ivars[2],
                       tapstruct[tap].ivars[3],
                       itmpfrac) + imix * lagrange(tapstruct[tap].lvars[0],
                               tapstruct[tap].lvars[1],
                               tapstruct[tap].lvars[2],
                               tapstruct[tap].lvars[3],
                               tmpfrac);

    return (output);

};
示例#14
0
文件: zadanie.c 项目: shnek/MOwNiT
int main(){
	FILE *input, *output, *output2, *output3, *time1, *time2, *time3;
  	time1=fopen("time_lagrange.txt", "w");
  	time2=fopen("time_newton.txt", "w");
  	time3=fopen("time_gsl.txt", "w");

	int N;
	clock_t startTime = clock();
	for(N = min; N < max; N++){
	  	input=fopen("dane.txt","w");
	  	output=fopen("custom_lagrange.txt","w");
	  	output2=fopen("gsl_polynomial.txt","w");
	  	output3=fopen("custom_newton.txt", "w");
		fprintf(time1, "%i, ", N);
		fprintf(time2, "%i, ", N);
		fprintf(time3, "%i, ", N);
		double x[N];
		double y[N];
		int i;

		generateData(N, x, y);

		for(i = 0; i < N; i ++){
			fprintf (input,"%g %g\n", x[i], y[i]);
		}

		double Lg[N];
		startTime = clock();
		lagrange(N, Lg, x, y);
		fprintf(time1, "%f\n", (double)(clock() - startTime)/(double)(CLOCKS_PER_SEC));

		double Nt[N];
		
		startTime = clock();
		newton(N, Nt, x, y);
		fprintf(time2, "%f\n", (double)(clock() - startTime)/(double)(CLOCKS_PER_SEC));
		// for(i = 0; i < N; i++){
		// 	printf("%f\n", Nt[i]);
		// }

		double xi, yi;
		{
	    startTime = clock();
	    gsl_interp_accel *acc = gsl_interp_accel_alloc ();
	    gsl_spline *spline = gsl_spline_alloc (gsl_interp_polynomial,N);
	    gsl_spline_init (spline, x, y, N);
	    fprintf(time3, "%f\n", (double)(clock() - startTime)/(double)(CLOCKS_PER_SEC));

	    for (xi = x[0]; xi < x[N-1]; xi += 0.01)
	      {
	        yi = gsl_spline_eval (spline, xi, acc);
	        fprintf (output2,"%g %g\n", xi, yi);
	      }
	    gsl_spline_free (spline);
	    gsl_interp_accel_free(acc);
	  }

	  for(xi = x[0]; xi < x[N-1]; xi += 0.01){
	  	yi = Polynomial(N, Lg, xi);
	  	fprintf(output, "%g %g\n", xi, yi);
	  }

	  for(xi = x[0]; xi < x[N-1]; xi += 0.01){
	  	yi = Polynomial(N, Nt, xi);
	  	fprintf(output3, "%g %g\n", xi, yi);
	  }		
	  fclose(input);
	  fclose(output);
	  fclose(output2);
	  fclose(output3);
	}
	return 0;
}
示例#15
0
int main(int argc, char **argv)
{

	//double tend = 1E2, speed = 1.;
	double tend = 1E-1, speed = 1.;
	char *init_type = "mixed2";
	double *roots, *weights, *ll, *dl, xmin, xmax, lxmin, lxmax,
		deltax, jac, xr, xl, cfl, dt, rtime, min_dx;
	int ii, jj, kk, ee, idx, eres;
	long nstep;
	double *dx, *mesh;
	double *smat, *xx, *qq, *qtemp, *k1, *k2, *k3, *k4, *minv_vec, *mmat, *dv,
		*mf, *ib, *df, *fstar;

	MPI_Init(&argc, &argv);
	MPI_Comm_size(MPI_COMM_WORLD, &nprocs);
	MPI_Comm_rank(MPI_COMM_WORLD, &rank);
	para_range(0, tne, nprocs, rank, &ista, &iend);
	ne = iend - ista;

	// initialize
	// fortran index structure array[ii,jj,ee] where size(array) = (np, np, ne)
	// c 1d index structure array = [ee*np*np + jj*np + ii]
	roots = (double *)malloc(np * sizeof(double));
	weights = (double *)malloc(np * sizeof(double));
	ll = (double *)malloc(np * sizeof(double));
	dl = (double *)malloc(np * sizeof(double));
	dx = (double *)malloc(ne * sizeof(double));
	mesh = (double *)malloc((ne + 1) * sizeof(double));

	smat = (double *)malloc(np * np * sizeof(double));		// [jj np, ii np]
	xx = (double *)malloc(ne * np * sizeof(double));		// [ee ne, ii np]
	qq = (double *)malloc(ne * np * sizeof(double));		// [ee ne, ii np]
	qtemp = (double *)malloc(ne * np * sizeof(double));		// [ee ne, ii np]
	k1 = (double *)malloc(ne * np * sizeof(double));		// [ee ne, ii np]
	k2 = (double *)malloc(ne * np * sizeof(double));		// [ee ne, ii np]
	k3 = (double *)malloc(ne * np * sizeof(double));		// [ee ne, ii np]
	k4 = (double *)malloc(ne * np * sizeof(double));		// [ee ne, ii np]
	minv_vec = (double *)malloc(ne * np * sizeof(double));  // [ee ne, ii np]
	mmat = (double *)malloc(ne * np * np * sizeof(double)); // [ee ne, jj np, ii np]
	dv = (double *)malloc(ne * np * np * sizeof(double));   // [ee ne, jj np, ii np]
	mf = (double *)malloc(2 * np * sizeof(double));			// [jj 2,  ii np]
	ib = (double *)malloc(2 * np * sizeof(double));			// [jj 2,  ii np]
	fstar = (double *)malloc(2 * ne * sizeof(double));		// [jj 2,  ii ne]
	df = (double *)malloc(ne * 2 * np * sizeof(double));	// [ee ne, jj 2, ii np]

	for (ii = 0; ii < np; ++ii)
	{
		roots[ii] = 0;
		weights[ii] = 0;
		ll[ii] = 0;
		dl[ii] = 0;
	}
	for (ii = 0; ii < ne; ++ii)
	{
		dx[ii] = 0;
		mesh[ii] = 0;
	}
	mesh[ne] = 0;

	for (ii = 0; ii < np * np; ++ii)
	{
		smat[ii] = 0;
	}
	for (ii = 0; ii < ne * np; ++ii)
	{
		xx[ii] = 0;
		qq[ii] = 0;
		k1[ii] = 0;
		k2[ii] = 0;
		k3[ii] = 0;
		k4[ii] = 0;
		minv_vec[ii] = 0;
	}
	for (ii = 0; ii < ne * np * np; ++ii)
	{
		mmat[ii] = 0;
		dv[ii] = 0;
	}
	for (ii = 0; ii < np * 2; ++ii)
	{
		mf[ii] = 0;
		ib[ii] = 0;
	}
	for (ii = 0; ii < ne * 2; ++ii)
	{
		fstar[ii] = 0;
	}
	for (ii = 0; ii < ne * 2 * np; ++ii)
	{
		df[ii] = 0;
	}

	// mesh setup
	xmin = 0.;
	xmax = 10.;
	deltax = (xmax-xmin)/(double)tne;
	/**
	 * lxim, lxmax를 이용하여 각 구간의 mesh[ee]를 구한다
	 * ne의 크기가 tne / process의 개수이기 때문에, 
	 * 각 구간에 맞는 mesh[ee]를 구해야 한다.
	 * 그리고 mesh[ee]를 이용하여 각 변수들을 초기화 한다.
	 */
	lxmin = xmin + (ista)*deltax;
	lxmax = xmin + (iend)*deltax;
	/**
	 * mesh[ne]은 마지막 원소가 아니라는점에 유의한다.
	 */ 
	mesh[ne] = lxmax; 
	for(ee=0;ee<ne;++ee){
		mesh[ee] = lxmin+ee*deltax;
	}

	// gauss lobatto quadrature point, weight setup
	gausslobatto_quadrature(np, roots, weights);

	// coordinates and element size
	min_dx = xmax - xmin; // initial guess
	for (ee = 0; ee < ne; ee++)
	{
		xl = mesh[ee];
		xr = mesh[ee + 1];
		dx[ee] = xr - xl; // size of each element
		if (dx[ee] < min_dx)
		{
			min_dx = dx[ee]; // finding minimum dx
		}
		for (ii = 0; ii < np; ii++)
		{
			idx = ee * np + ii;
			xx[idx] = xl + 0.5 * (1 + roots[ii]) * dx[ee];
		}
	}

	// mass matrix
	for (ii = 0; ii < ne * np * np; ii++)
	{
		mmat[ii] = 0;
	}
	for (ee = 0; ee < ne; ee++)
	{
		jac = fabs(dx[ee]) / 2;
		for (kk = 0; kk < np; kk++)
		{
			lagrange(roots[kk], ll, roots);
			for (jj = 0; jj < np; jj++)
			{
				for (ii = 0; ii < np; ii++)
				{
					idx = ee * np * np + jj * np + ii;
					// mass matrix mmat[ne][np][np] in 1d index representation
					mmat[idx] += jac * weights[kk] * ll[ii] * ll[jj];
				}
			}
		}
	}

	// stiffness matrix
	for (ii = 0; ii < np * np; ii++)
	{
		smat[ii] = 0;
	}
	for (kk = 0; kk < np; kk++)
	{
		lagrange(roots[kk], ll, roots);
		lagrange_deriv(roots[kk], dl, roots);
		for (jj = 0; jj < np; jj++)
		{
			for (ii = 0; ii < np; ii++)
			{
				idx = jj * np + ii;
				// stiffness matrix smat[np][np] in 1d index representation
				smat[idx] += weights[kk] * ll[jj] * dl[ii];
			}
		}
	}

	// face integration
	for (ii = 0; ii < np * 2; ii++)
	{
		mf[ii] = 0;
	}
	lagrange(-1, mf, roots);	 // mf[ii] for(ii=0, ii<np,ii++) represents element left face integration
	lagrange(1, mf + np, roots); // mf[ii] for ii=np, ii<2*np, ii++) reresents element right face integration

	// boundary interpolation
	for (ii = 0; ii < np * 2; ii++)
	{
		ib[ii] = 0;
	}
	lagrange(-1, ib, roots);	 // element left edge interpolation
	lagrange(1, ib + np, roots); // element right edge interpolation

	// divergence operators
	for (ii = 0; ii < ne * np * np; ii++)
	{
		dv[ii] = 0;
	}
	for (ii = 0; ii < ne * np * 2; ii++)
	{
		dv[ii] = 0;
	}
	for (ee = 0; ee < ne; ee++)
	{
		for (jj = 0; jj < np; jj++)
		{
			// it turn out that mmat is diagonal. i.e., ii != jj, mmat[ee][jj][ii] = 0
			// the inverse of mmat is just the inverse of the diagonal components
			// here, we are extracting the inverse diagonal components only
			minv_vec[ee * np + jj] = 1. / mmat[ee * np * np + jj * np + jj];
		}
		for (jj = 0; jj < np; jj++)
		{
			for (ii = 0; ii < np; ii++)
			{
				dv[ee * np * np + jj * np + ii] = minv_vec[ee * np + ii] * smat[jj * np + ii];
			}
		}
		for (jj = 0; jj < 2; jj++)
		{
			for (ii = 0; ii < np; ii++)
			{
				df[ee * np * 2 + jj * np + ii] = minv_vec[ee * np + ii] * mf[jj * np + ii];
			}
		}
	}

	// initialize qq field
	initialize(qq, xx, xmax, xmin, init_type);
	cfl = 1. / (np * np);
	dt = cfl * min_dx / fabs(speed);
	rtime = 0.;
	nstep = 0;

	printf("Start Time Integration\n");

	// Runge-Kutta 4th order Time integration loop

	t_sta = clock();

	while (rtime < tend)
	{
		dt = fmin(dt, tend - rtime);
		rhs(qq, k1, dv, df, ib, speed);
		for (ii = 0; ii < ne * np; ii++)
			qtemp[ii] = qq[ii] + 0.5 * dt * k1[ii];

		rhs(qtemp, k2, dv, df, ib, speed);
		for (ii = 0; ii < ne * np; ii++)
			qtemp[ii] = qq[ii] + 0.5 * dt * k2[ii];
		rhs(qtemp, k3, dv, df, ib, speed);


		for (ii = 0; ii < ne * np; ii++)
			qtemp[ii] = qq[ii] + dt * k3[ii];
		rhs(qtemp, k4, dv, df, ib, speed);

		for (ii = 0; ii < ne * np; ii++)
			qq[ii] += 1. / 6. * dt * (k1[ii] + 2 * k2[ii] + 2 * k3[ii] + k4[ii]);

		rtime += dt;
		nstep += 1;
		if (nstep % 10000 == 0 && rank == 0)
			printf("nstep = %10ld, %5.1f%% complete\n", nstep, rtime / tend * 100);
	}

	// timeloop ends here;

	if (rank != 0)
	{
		int nne = iend - ista;
		MPI_Isend(&nne, 1, MPI_INT, 0, 11, MPI_COMM_WORLD, &ser1);
		MPI_Isend(xx, ne * np, MPI_DOUBLE, 0, 22, MPI_COMM_WORLD, &ser2);
		MPI_Isend(qq, ne * np, MPI_DOUBLE, 0, 33, MPI_COMM_WORLD, &ser3);
		MPI_Wait(&ser1, &st);
		MPI_Wait(&ser2, &st);
		MPI_Wait(&ser3, &st);
	}

	double *bufx;
	double *bufq;
	int *istart;
	int *idisp;

	if (rank == 0)
	{

		printf("Integration complete\n");

		if (tne > 200)
		{
			eres = 2;
		}
		else if (tne > 60)
		{
			eres = 3;
		}
		else if (tne > 30)
		{
			eres = 6;
		}
		else
		{
			eres = 10;
		}

		// final report
		printf("-----------------------------------------------\n");
		printf("code type   : c serial\n");
		printf("Final time  : %13.5e\n", rtime);
		printf("CFL         : %13.5e\n", cfl);
		printf("DOF         : %13d\n", tne * np);
		printf("No. of Elem : %13d\n", tne);
		printf("Order       : %13d\n", np);
		printf("eres        : %13d\n", eres);
		printf("time steps  : %13ld\n", nstep);
		printf("-----------------------------------------------\n");

		bufx = (double *)malloc(sizeof(double) * tne * np);
		bufq = (double *)malloc(sizeof(double) * tne * np);

		for (int i = 0; i < ne * np; i++)
		{
			bufx[i] = xx[i];
			bufq[i] = qq[i];
		}
	}
	if (rank == 0)
	{
		int index[nprocs];
		index[0] = ne * np;
		int idx = index[0];
		for (int i = 1; i < nprocs; i++)
		{
			MPI_Irecv(index + i, 1, MPI_INT, i, 11, MPI_COMM_WORLD, &rer1);
			MPI_Wait(&rer1, &st);
			index[i] *= np;
			MPI_Irecv(bufx + idx, index[i], MPI_DOUBLE, i, 22, MPI_COMM_WORLD, &rer2);
			MPI_Irecv(bufq + idx, index[i], MPI_DOUBLE, i, 33, MPI_COMM_WORLD, &rer3);
			MPI_Wait(&rer2, &st);
			MPI_Wait(&rer3, &st);
			idx += index[i];
		}

		for(int i = 0; i < tne*np; i++){
			printf("%f ", bufx[i]);
		}
		printf("\n");
		for(int i = 0; i < tne*np; i++){
			printf("%f ", bufq[i]);
		}
		printf("\n");

		save_field(bufx, bufq, tne, roots, eres);
		t_end = clock();
		printf("Motion time = %f msec\n", (double)(t_end - t_sta) / 1000.0);
	}
	free(roots);
	free(weights);
	free(ll);
	free(dl);
	free(dx);
	free(mesh);
	free(smat);
	free(xx);
	free(qq);
	free(qtemp);
	free(k1);
	free(k2);
	free(k3);
	free(k4);
	free(minv_vec);
	free(mmat);
	free(dv);
	free(mf);
	free(ib);
	free(fstar);
	free(df);

	MPI_Finalize();
	return 0;
}
示例#16
0
/**
 * Calcule le resultant des polynomes bivaries PY et QY
 * Appeler nb_zeros et del_zeros en sortie pour avoir le resultant sans les premiers coeffs nuls
 * @param resultant le resultant de PY et QY de taille 2*deg_P*deg_Q+1
 * @param deg_P le degre du polynome PY en Y (nb de colonnes)
 * @param deg_Q degre de QY en Y
 * @param degres_PY liste des degres en X des coefficients de PY
 * @param degres_QY liste des degres en X des corfficients de QY
 */
void resultant(mpz_t *resultant, mpz_t **PY, mpz_t **QY, int deg_P, int deg_Q, int *degres_PY, int *degres_QY, mpz_t mod){
  printf("resultant\n");
  int i,j, borne;
  int matrix_length=deg_P+deg_Q;
  int matrix_size= matrix_length*matrix_length;
  
  /* allocation de la matrice de sylvester */
  mpz_t M[matrix_size];
  for(i=0; i<matrix_size; i++)
    mpz_init(M[i]);

  /* Calcul de la borne superieure sur le degre du resultant */
  borne=2*deg_P*deg_Q+1;

  /* si le modulo est trop petit, on ne peut pas choisir assez de valeurs pour l interpolation */
  if(mpz_cmp_si(mod, borne)<=0){
    printf("Modulo trop petit !\n");
    exit(0);
  }
  printf("borne = %d\n", borne);

  /* Choix des valeurs d interpolation*/
  mpz_t values[borne];
  for(i=0; i<borne; i++){
    mpz_init_set_si(values[i], i);
    /*printf("values[%d] = %ld", i, mpz_get_si(values[i]));*/
  }
  /* initialisation de P, Q et du tableau des determinant */
  /*printf("\nfin init values\n");*/
  mpz_t P[deg_P+1], Q[deg_Q+1];
  for(i=0; i<deg_P+1; i++){
    mpz_init(P[i]);
  }
  for(i=0; i<deg_Q+1; i++){
    mpz_init(Q[i]);
  }
  /* determinant contiendra les images des points de values pour l interpolation */
  mpz_t determinant[borne];
  for(i=0;i<borne;i++){
    mpz_init(determinant[i]);
  }
  /* A chaque iteration, on traite une valeur de values et on calcule le determinant associe */
  for(i=0; i<borne; i++){
    /* evaluation du polynome en values[i] */
    eval_biv(values[i], PY, QY, degres_PY, degres_QY, P, Q, deg_P, deg_Q, mod );
    /*print_P(P, deg_P);
    print_P(Q, deg_Q);
    
    printf("appel sylvester\n");
      */
    /* Calcul de la matrice de Sylvester dans M*/
    sylvester(P, Q, deg_P, deg_Q, M);
    /*printf("matrice de sylvester:\n");
    print_M(M, deg_P+deg_Q);

    printf("appel Gauss\n");
      */
    /* Appel de Gauss */
    /* remplit le determinant associe a values[i] */
    gauss(&determinant[i], M, matrix_length, mod);

    /* on remet M a 0 pour la prochaine iteration */
    for(j=0; j<matrix_size; j++)
      mpz_set_si(M[j], 0);
  }
  /* affichage des determinants (avec la fonction d affichage de ploynomes) */
  /*printf("determinants : ");
  print_P(determinant, borne-1);*/

  /* appel a Lagrange */
  
  mpz_t res_mod[borne+1];
  for(j=0; j<borne+1; j++){
    mpz_init(res_mod[j]);
  }
  lagrange(resultant, values, determinant, borne-1, mod, res_mod);
  /*printf("fin lagrange\n");*/

  for(j=0; j<borne; j++){
    mpz_mod(resultant[j],resultant[j], mod);
  }
  
}
示例#17
0
/*!
\brief  Compute the pose of a planar object using Lagrange approach.

\param cMo : Estimated pose. No initialisation is requested to estimate cMo.
\param coplanar_plane_type : Type of coplanar plane:
   1: if plane x=cst
   2: if plane y=cst
   3: if plane z=cst
   0: any other plane
*/
void
vpPose::poseLagrangePlan(vpHomogeneousMatrix &cMo, const int coplanar_plane_type)
{

#if (DEBUG_LEVEL1)
  std::cout << "begin vpPose::PoseLagrange(...) " << std::endl ;
#endif
  try
  {
    double s;
    unsigned int i;

    unsigned int k=0;
    unsigned int nl=npt*2;


    vpMatrix a(nl,3)  ;
    vpMatrix b(nl,6);
    vpPoint P ;
    i=0 ;

    if (coplanar_plane_type == 1) { // plane ax=d
      for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
      {
        P = *it ;
        a[k][0]   = -P.get_oY();
        a[k][1]   = 0.0;
        a[k][2]   = P.get_oY()*P.get_x();

        a[k+1][0] = 0.0;
        a[k+1][1] = -P.get_oY();
        a[k+1][2] = P.get_oY()*P.get_y();

        b[k][0]   = -P.get_oZ();
        b[k][1]   = 0.0;
        b[k][2]   = P.get_oZ()*P.get_x();
        b[k][3]   =  -1.0;
        b[k][4]   =  0.0;
        b[k][5]   =  P.get_x();

        b[k+1][0] =  0.0;
        b[k+1][1] = -P.get_oZ();
        b[k+1][2] =  P.get_oZ()*P.get_y();
        b[k+1][3] =  0.0;
        b[k+1][4] = -1.0;
        b[k+1][5] =  P.get_y();

        k += 2;
      }

    }
    else if (coplanar_plane_type == 2) {  // plane by=d
      for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
      {
        P = *it ;
        a[k][0]   = -P.get_oX();
        a[k][1]   = 0.0;
        a[k][2]   = P.get_oX()*P.get_x();

        a[k+1][0] = 0.0;
        a[k+1][1] = -P.get_oX();
        a[k+1][2] = P.get_oX()*P.get_y();

        b[k][0]   = -P.get_oZ();
        b[k][1]   = 0.0;
        b[k][2]   = P.get_oZ()*P.get_x();
        b[k][3]   =  -1.0;
        b[k][4]   =  0.0;
        b[k][5]   =  P.get_x();

        b[k+1][0] =  0.0;
        b[k+1][1] = -P.get_oZ();
        b[k+1][2] =  P.get_oZ()*P.get_y();
        b[k+1][3] =  0.0;
        b[k+1][4] = -1.0;
        b[k+1][5] =  P.get_y();

        k += 2;
      }

    }
    else { // plane cz=d or any other

      for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
      {
        P = *it ;
        a[k][0]   = -P.get_oX();
        a[k][1]   = 0.0;
        a[k][2]   = P.get_oX()*P.get_x();

        a[k+1][0] = 0.0;
        a[k+1][1] = -P.get_oX();
        a[k+1][2] = P.get_oX()*P.get_y();

        b[k][0]   = -P.get_oY();
        b[k][1]   = 0.0;
        b[k][2]   = P.get_oY()*P.get_x();
        b[k][3]   =  -1.0;
        b[k][4]   =  0.0;
        b[k][5]   =  P.get_x();

        b[k+1][0] =  0.0;
        b[k+1][1] = -P.get_oY();
        b[k+1][2] =  P.get_oY()*P.get_y();
        b[k+1][3] =  0.0;
        b[k+1][4] = -1.0;
        b[k+1][5] =  P.get_y();

        k += 2;
      }
    }
    vpColVector X1(3) ;
    vpColVector X2(6) ;

#if (DEBUG_LEVEL2)
    {
      std::cout <<"a " << a << std::endl ;
      std::cout <<"b " << b << std::endl ;
    }
#endif

    lagrange(a,b,X1,X2);

#if (DEBUG_LEVEL2)
    {
      std::cout << "ax1+bx2 (devrait etre 0) " << (a*X1 + b*X2).t() << std::endl ;
      std::cout << "norme X1 " << X1.sumSquare() << std::endl ;;
    }
#endif

    if (X2[5] < 0.0)
    {		/* car Zo > 0	*/
      for (i=0;i<3;i++) X1[i] = -X1[i];
      for (i=0;i<6;i++) X2[i] = -X2[i];
    }
    s = 0.0;
    for (i=0;i<3;i++) {s += (X1[i]*X2[i]);}
    for (i=0;i<3;i++)  {X2[i] -= (s*X1[i]);} /* X1^T X2 = 0	*/

    s = 0.0;
    for (i=0;i<3;i++)  {s += (X2[i]*X2[i]);}

    if (s<1e-10)
    {
      std::cout << "Points that produce an error: " << std::endl;
      for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
      {
        std::cout << "P: " << (*it).get_x() << " " << (*it).get_y() << " "
                  << (*it).get_oX() << " " << (*it).get_oY() << " " << (*it).get_oZ() << std::endl;
      }
      vpERROR_TRACE( "division par zero ") ;
      throw(vpException(vpException::divideByZeroError,
        "division by zero  ")) ;
    }

    s = 1.0/sqrt(s);
    for (i=0;i<3;i++)  {X2[i] *= s;}		/* X2^T X2 = 1	*/


    calculTranslation (a, b, nl, 3, 3, X1, X2) ;

    // if (err != OK)
    {
      // std::cout << "in (vpCalculPose_plan.cc)CalculTranslation returns " ;
      // PrintError(err) ;
      //    return err ;
    }

    if (coplanar_plane_type == 1) { // plane ax=d
      cMo[0][0] = (X1[1]*X2[2])-(X1[2]*X2[1]);
      cMo[1][0] = (X1[2]*X2[0])-(X1[0]*X2[2]);
      cMo[2][0] = (X1[0]*X2[1])-(X1[1]*X2[0]);

      for (i=0;i<3;i++)
      { /* calcul de la matrice de passage	*/
        cMo[i][1] = X1[i];
        cMo[i][2] = X2[i];
        cMo[i][3] = X2[i+3];
      }

    }
    else if (coplanar_plane_type == 2) {  // plane by=d
      cMo[0][1] = (X1[1]*X2[2])-(X1[2]*X2[1]);
      cMo[1][1] = (X1[2]*X2[0])-(X1[0]*X2[2]);
      cMo[2][1] = (X1[0]*X2[1])-(X1[1]*X2[0]);

      for (i=0;i<3;i++)
      { /* calcul de la matrice de passage	*/
        cMo[i][0] = X1[i];
        cMo[i][2] = X2[i];
        cMo[i][3] = X2[i+3];
      }
    }
    else { // plane cz=d or any other

      cMo[0][2] = (X1[1]*X2[2])-(X1[2]*X2[1]);
      cMo[1][2] = (X1[2]*X2[0])-(X1[0]*X2[2]);
      cMo[2][2] = (X1[0]*X2[1])-(X1[1]*X2[0]);

      for (i=0;i<3;i++)
      { /* calcul de la matrice de passage	*/
        cMo[i][0] = X1[i];
        cMo[i][1] = X2[i];
        cMo[i][3] = X2[i+3];
      }
    }
  }
  catch(...)
  {
    vpERROR_TRACE(" ") ;
    throw ;
  }


#if (DEBUG_LEVEL1)
  std::cout << "end vpCalculPose::PoseLagrange(...) " << std::endl ;
#endif
  //  return(OK);
}
int main(int argc, char **argv){

	double tend = 1E2, speed = 1.;
	// double tend = 1E-1, speed = 1.;
	char *init_type="mixed2";
	double *roots, *weights, *ll, *dl, xmin, xmax, 
		   deltax, jac, xr, xl, cfl, dt, rtime, min_dx;
	int ii, jj, kk, ee, idx, eres;
	long nstep;
	double *dx, *mesh; 
	double *smat, *xx, *qq, *qtemp, *k1, *k2, *k3, *k4, *minv_vec, *mmat, *dv, 
		   *mf, *ib, *df, *fstar;

	// initialize 
	// fortran index structure array[ii,jj,ee] where size(array) = (np, np, ne)
	// c 1d index structure array = [ee*np*np + jj*np + ii]
	roots   = (double*)malloc(np*   sizeof(double));
	weights = (double*)malloc(np*   sizeof(double));
	ll      = (double*)malloc(np*   sizeof(double));
	dl      = (double*)malloc(np*   sizeof(double));
	dx      = (double*)malloc(ne*   sizeof(double));
	mesh    = (double*)malloc((ne+1)*sizeof(double));

	smat	= (double*)malloc(np*np*sizeof(double));    // [jj np, ii np]
	xx		= (double*)malloc(ne*np*sizeof(double));    // [ee ne, ii np]
	qq		= (double*)malloc(ne*np*sizeof(double));    // [ee ne, ii np]	
	qtemp	= (double*)malloc(ne*np*sizeof(double));    // [ee ne, ii np]	
	k1		= (double*)malloc(ne*np*sizeof(double));    // [ee ne, ii np]	
	k2		= (double*)malloc(ne*np*sizeof(double));    // [ee ne, ii np]
	k3		= (double*)malloc(ne*np*sizeof(double));    // [ee ne, ii np]
	k4		= (double*)malloc(ne*np*sizeof(double));    // [ee ne, ii np]
	minv_vec= (double*)malloc(ne*np*sizeof(double));    // [ee ne, ii np]
	mmat	= (double*)malloc(ne*np*np*sizeof(double)); // [ee ne, jj np, ii np]
	dv		= (double*)malloc(ne*np*np*sizeof(double)); // [ee ne, jj np, ii np]
	mf		= (double*)malloc(2*np*sizeof(double));     // [jj 2,  ii np]
	ib		= (double*)malloc(2*np*sizeof(double));     // [jj 2,  ii np]
	fstar	= (double*)malloc(2*ne*sizeof(double));     // [jj 2,  ii ne]
	df		= (double*)malloc(ne*2*np*sizeof(double));  // [ee ne, jj 2, ii np]

	for (ii=0; ii<np; ++ii){
		roots[ii] = 0;
		weights[ii] = 0;
		ll[ii] = 0;
		dl[ii] = 0;
	}
	for (ii=0; ii<ne; ++ii){
		dx[ii] = 0;
		mesh[ii] = 0;
	}
	mesh[ne] = 0;

	
	for (ii=0; ii<np*np; ++ii){
		smat[ii] = 0;
	}
	for (ii=0; ii<ne*np; ++ii){
		xx[ii]	= 0;		 	
		qq[ii]	= 0;		 	
		k1[ii]	= 0;		 	
		k2[ii]	= 0;		 	
		k3[ii]	= 0;		 	
		k4[ii]	= 0;		 	
		minv_vec[ii]	= 0; 
	}
	for (ii=0; ii<ne*np*np; ++ii){
		mmat[ii] = 0;    	
		dv[ii]	 = 0;
	}
	for (ii=0; ii<np*2; ++ii){
		mf[ii] = 0;
		ib[ii] = 0;
	}
	for (ii=0; ii<ne*2; ++ii){
		fstar[ii] = 0;
	}
	for (ii=0; ii<ne*2*np; ++ii){
		df[ii] = 0;
	}

	// mesh setup
	xmin = 0.;
	xmax = 10.;
	deltax = (xmax-xmin)/(double)ne;
	mesh[ne] = xmax;
	for(ee=0;ee<ne;++ee) {
		mesh[ee] = xmin+ee*deltax;
	}
	
	// gauss lobatto quadrature point, weight setup
	gausslobatto_quadrature(np, roots, weights);

	// coordinates and element size
	min_dx = xmax - xmin; // initial guess
	for(ee=0;ee<ne;ee++){
		xl = mesh[ee];
		xr = mesh[ee+1];
		dx[ee] = xr-xl; // size of each element
		if(dx[ee] < min_dx){
			min_dx = dx[ee]; // finding minimum dx
		}
		for(ii=0;ii<np;ii++){
			idx = ee*np+ii;
			xx[idx] = xl + 0.5*(1+roots[ii])*dx[ee];
		}
	}

	// mass matrix
	for(ii=0;ii<ne*np*np;ii++){
		mmat[ii] = 0;
	}

	for(ee=0;ee<ne;ee++){
		jac = fabs(dx[ee])/2;
		for(kk=0;kk<np;kk++){
			lagrange(roots[kk], ll, roots);
			for(jj=0;jj<np;jj++){
				for(ii=0;ii<np;ii++){
					idx = ee*np*np+jj*np+ii;
					// mass matrix mmat[ne][np][np] in 1d index representation
					mmat[idx] += jac*weights[kk]*ll[ii]*ll[jj];
				}
			}
		}
	}

	// stiffness matrix
	for(ii=0;ii<np*np;ii++){
		smat[ii] = 0;
	}
	for(kk=0;kk<np;kk++){
		lagrange(roots[kk], ll, roots);
		lagrange_deriv(roots[kk], dl, roots);
		for(jj=0;jj<np;jj++){
			for(ii=0;ii<np;ii++){
				idx = jj*np+ii;
				// stiffness matrix smat[np][np] in 1d index representation
				smat[idx] += weights[kk]*ll[jj]*dl[ii];
			}
		}
	}

	// face integration
	for(ii=0;ii<np*2;ii++){
		mf[ii] = 0;
	}
	lagrange(-1,mf,   roots); // mf[ii] for(ii=0, ii<np,ii++) represents element left face integration
	lagrange( 1,mf+np,roots); // mf[ii] for ii=np, ii<2*np, ii++) reresents element right face integration

	// boundary interpolation
	for(ii=0;ii<np*2;ii++){
		ib[ii] = 0;
	}
	lagrange(-1,ib,   roots); // element left edge interpolation
	lagrange( 1,ib+np,roots); // element right edge interpolation

	
	// divergence operators
	for(ii=0;ii<ne*np*np;ii++){
		dv[ii] = 0;
	}
	for(ii=0;ii<ne*np*2;ii++){
		dv[ii] = 0;
	}

	for(ee=0;ee<ne;ee++){
		for(jj=0;jj<np;jj++){
			// it turn out that mmat is diagonal. i.e., ii != jj, mmat[ee][jj][ii] = 0
			// the inverse of mmat is just the inverse of the diagonal components
			// here, we are extracting the inverse diagonal components only
			minv_vec[ee*np+jj] = 1./mmat[ee*np*np+jj*np+jj];
		}
		for(jj=0;jj<np;jj++){
			for(ii=0;ii<np;ii++){
				dv[ee*np*np+jj*np+ii] = minv_vec[ee*np+ii]*smat[jj*np+ii];
			}
		}
		for(jj=0;jj<2;jj++){
			for(ii=0;ii<np;ii++){
				df[ee*np*2+jj*np+ii]  = minv_vec[ee*np+ii]*mf[jj*np+ii];
			}
		}

	}
	
	// initialize qq field
	initialize(qq, xx, xmax, xmin, init_type);
	cfl = 1./(np*np);
	dt = cfl * min_dx / fabs(speed);
	rtime = 0.;
	nstep = 0;

	printf("Start Time Integration\n");

	// Runge-Kutta 4th order Time integration loop
	
	t_sta = clock();

	while(rtime < tend){
		dt = fmin(dt, tend-rtime);

		rhs(qq,	   k1, dv, df, ib, speed);

		for(ii=0;ii<ne*np;ii++)
			qtemp[ii] = qq[ii]+0.5*dt*k1[ii];
		rhs(qtemp, k2, dv, df, ib, speed);

		for(ii=0;ii<ne*np;ii++)
			qtemp[ii] = qq[ii]+0.5*dt*k2[ii];
		rhs(qtemp, k3, dv, df, ib, speed);
		
		for(ii=0;ii<ne*np;ii++)
			qtemp[ii] = qq[ii]+dt*k3[ii];
		rhs(qtemp, k4, dv, df, ib, speed);

		for(ii=0;ii<ne*np;ii++)
			qq[ii] += 1./6.*dt*(k1[ii]+2*k2[ii]+2*k3[ii]+k4[ii]);

		rtime += dt;
		nstep += 1;
		if(nstep%10000 == 0) 
			printf("nstep = %10ld, %5.1f%% complete\n", nstep, rtime/tend*100);
	}

	// timeloop ends here;

	printf("Integration complete\n");

	if(ne > 200){
		eres = 2;
	}
	else if (ne > 60){
		eres = 3;
	}
	else if (ne > 30){
		eres = 6;
	}
	else {
		eres = 10;
	}

	// final report
	printf("-----------------------------------------------\n");
	printf("code type   : c serial\n");
	printf("Final time  : %13.5e\n", rtime);
	printf("CFL         : %13.5e\n", cfl);
	printf("DOF         : %13d\n", ne*np);
	printf("No. of Elem : %13d\n", ne);
	printf("Order       : %13d\n", np);
	printf("eres        : %13d\n", eres);
	printf("time steps  : %13ld\n", nstep);
	printf("-----------------------------------------------\n");

	save_field(xx, qq, ne, roots, eres);

	t_end = clock();
	printf("Motion time = %f msec\n", (double)(t_end - t_sta)/1000.0);

	free(roots);   
	free(weights); 
	free(ll);      
	free(dl);      
	free(dx);      
	free(mesh);    
	free(smat);	
	free(xx);		
	free(qq);		
	free(qtemp);	
	free(k1);		
	free(k2);		
	free(k3);		
	free(k4);		
	free(minv_vec);
	free(mmat);	
	free(dv);		
	free(mf);		
	free(ib);		
	free(fstar);	
	free(df);		

	return 0;
}
示例#19
0
void
vpPose::poseLagrangeNonPlan(vpHomogeneousMatrix &cMo)
{

#if (DEBUG_LEVEL1)
  std::cout << "begin CPose::PoseLagrange(...) " << std::endl ;
#endif
  try{
    double s;
    unsigned int i;

    unsigned int k=0;
    unsigned int nl=npt*2;

    vpMatrix a(nl,3)  ;
    vpMatrix b(nl,9);
    b =0 ;

    vpPoint P ;
    i=0 ;
    for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
    {
      P = *it;
      a[k][0]   = -P.get_oX();
      a[k][1]   = 0.0;
      a[k][2]   = P.get_oX()*P.get_x();

      a[k+1][0] = 0.0;
      a[k+1][1] = -P.get_oX();
      a[k+1][2] = P.get_oX()*P.get_y();

      b[k][0]   = -P.get_oY();
      b[k][1]   = 0.0;
      b[k][2]   = P.get_oY()*P.get_x();

      b[k][3]   = -P.get_oZ();
      b[k][4]   =  0.0;
      b[k][5]   =  P.get_oZ()*P.get_x();

      b[k][6]   =  -1.0;
      b[k][7]   =  0.0;
      b[k][8]   =  P.get_x();

      b[k+1][0] =  0.0;
      b[k+1][1] = -P.get_oY();
      b[k+1][2] =  P.get_oY()*P.get_y();

      b[k+1][3] =  0.0;
      b[k+1][4] = -P.get_oZ();
      b[k+1][5] =  P.get_oZ()*P.get_y();

      b[k+1][6] =  0.0;
      b[k+1][7] = -1.0;
      b[k+1][8] =  P.get_y();

      k += 2;
    }
    vpColVector X1(3) ;
    vpColVector X2(9) ;

#if (DEBUG_LEVEL2)
    {
      std::cout <<"a " << a << std::endl ;
      std::cout <<"b " << b << std::endl ;
    }
#endif

    lagrange(a,b,X1,X2);
    //  if (err != OK)
    {
      //      std::cout << "in (CLagrange.cc)Lagrange returns " ;
      //    PrintError(err) ;
      //    return err ;
    }


#if (DEBUG_LEVEL2)
    {
      std::cout << "ax1+bx2 (devrait etre 0) " << (a*X1 + b*X2).t() << std::endl ;
      std::cout << "norme X1 " << X1.sumSquare() << std::endl ;;
    }
#endif

    if (X2[8] < 0.0)
    {		/* car Zo > 0	*/
      X1 *= -1 ;
      X2 *= -1 ;
    }
    s = 0.0;
    for (i=0;i<3;i++) {s += (X1[i]*X2[i]);}
    for (i=0;i<3;i++)  {X2[i] -= (s*X1[i]);} /* X1^T X2 = 0	*/

    //s = 0.0;
    //for (i=0;i<3;i++)  {s += (X2[i]*X2[i]);}
    s = X2[0]*X2[0] + X2[1]*X2[1] + X2[2]*X2[2]; // To avoid a Coverity copy/past error

    if (s<1e-10)
    {
//      std::cout << "Points that produce an error: " << std::endl;
//      for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
//      {
//        std::cout << "P: " << (*it).get_x() << " " << (*it).get_y() << " "
//                  << (*it).get_oX() << " " << (*it).get_oY() << " " << (*it).get_oZ() << std::endl;
//      }
      //vpERROR_TRACE(" division par zero " ) ;
      throw(vpException(vpException::divideByZeroError,
                        "Division by zero in Lagrange pose computation (non planar plane case)")) ;
    }

    s = 1.0/sqrt(s);
    for (i=0;i<3;i++)  {X2[i] *= s;}		/* X2^T X2 = 1	*/

    X2[3] = (X1[1]*X2[2])-(X1[2]*X2[1]);
    X2[4] = (X1[2]*X2[0])-(X1[0]*X2[2]);
    X2[5] = (X1[0]*X2[1])-(X1[1]*X2[0]);

    calculTranslation (a, b, nl, 3, 6, X1, X2) ;

    for (i=0 ; i<3 ; i++)
    {
      cMo[i][0] = X1[i];
      cMo[i][1] = X2[i];
      cMo[i][2] = X2[i+3];
      cMo[i][3] = X2[i+6];
    }

  }
  catch(vpException &e)
  {
    throw e;
  }

#if (DEBUG_LEVEL1)
  std::cout << "end vpCalculPose::PoseLagrange(...) " << std::endl ;
#endif
}
示例#20
0
int main()
{
	PFC pfc(AES_SECURITY);  // initialise pairing-friendly curve
	miracl *mip=get_mip();  // get handle on mip (Miracl Instance Pointer)
	Big order=pfc.order();  // get pairing-friendly group order

	time_t seed;            // crude randomisation
	time(&seed);
    irand((long)seed);

// setup - for 20 attributes 1-20
	int i,j,k,n,ik,S[NBOB];
	Big s,y,qi,M,ED,t[NATTR];
	Big poly[Nd];
	G1 P,T[NATTR],E[Nd],AE[NALICE];
	G2 Q,AD[NALICE],BD[NBOB];
	GT Y,DB;

	pfc.random(P);
	pfc.random(Q);
	pfc.precomp_for_mult(Q);  // Q is fixed, so precompute on it

	pfc.random(y);
	Y=pfc.power(pfc.pairing(Q,P),y);
	for (i=0;i<NATTR;i++)
	{
		pfc.random(t[i]);                 // Note t[i] will be 2*AES_SECURITY bits long
		T[i]=pfc.mult(P,t[i]);            // which may be less than the group order.
		pfc.precomp_for_mult(T[i],TRUE);  // T[i] are system params, so precompute on them
                                          // Note second parameter indicates that all multipliers
										  // must  be <=2*AES_SECURITY bits, which may be shorter
										  // than the full group size.
	}

// key generation for Alice

	// A d-1 degree polynomial is randomly chosen such that q(0)=y
	poly[0]=y;
	for (i=1;i<Nd;i++)
		pfc.random(poly[i]);

	// Private key consists of components D_i where D_i=g^(q(i)/t_i)
	for (j=0;j<NALICE;j++)
	{
		i=Alice[j];
		qi=y; ik=i;
		for (k=1;k<Nd;k++)
		{ // evaluate polynomial a0+a1*x+a2*x^2... for x=i; => result is  q(i)
			qi+=modmult(poly[k],(Big)ik,order);
			ik*=i;
			qi%=order;
		}
		// D_i=g^(q(i)/t_i)
		AD[j]=pfc.mult(Q,moddiv(qi,t[i],order));  // exploits precomputation
	}

// key generation for Bob

	poly[0]=y;
	for (i=1;i<Nd;i++)
		pfc.random(poly[i]);

	for (j=0;j<NBOB;j++)
	{
		i=Bob[j];
		qi=y; ik=i;
		for (k=1;k<Nd;k++)
		{ // evaluate polynomial a0+a1*x+a2*x^2... for x=i;
			qi+=modmult(poly[k],(Big)ik,order);
			ik*=i;
			qi%=order;
		}

		BD[j]=pfc.mult(Q,moddiv(qi,t[i],order));
		pfc.precomp_for_pairing(BD[j]);   // Bob precomputes on his private key
	}

// Encryption to Alice
	mip->IOBASE=256;
	M=(char *)"test message";
	cout << "Message to be encrypted=   " << M << endl;
	mip->IOBASE=16;
	pfc.random(s);
	ED=lxor(M,pfc.hash_to_aes_key(pfc.power(Y,s)));
	for (j=0;j<NALICE;j++)
	{
		i=Alice[j];
		// calculate T_i^s
		AE[j]=pfc.mult(T[i],s);   // exploit precomputation
	}

// Decryption by Bob

// set up to exploit multi-pairing
	G1 *g1[5];
	G2 *g2[5];

	k=0;
	for (j=0;j<NBOB;j++)
	{ // check for common attributes
		i=Bob[j];
		n=has_attribute(NALICE,Alice,i);
		if (n<0) continue;  // Alice doesn't have it
		S[k]=i;
		E[k]=AE[n];
		g2[k]=&BD[j];
		k++;
	}
	if (k<Nd)
	{
		cout << "Bob does not have enough attributes in common with Alice to decrypt successfully" << endl;
		exit(0);
	}

// faster to multiply in G1 than to exponentiate in GT

	for (j=0;j<Nd;j++)
	{
		i=S[j];
		E[j]=pfc.mult(E[j],lagrange(i,S,Nd,order));
		g1[j]=&E[j];
	}

	DB=pfc.multi_pairing(Nd,g2,g1);

	M=lxor(ED,pfc.hash_to_aes_key(DB));
	mip->IOBASE=256;
	cout << "Decrypted message=         " << M << endl;

    return 0;
}
示例#21
0
/*!
 *  start main optimization step
 *  author: Vitalij Ruge
 **/
int startIpopt(DATA* data, SOLVER_INFO* solverInfo, int flag)
{
  int i;
  int j,k,l;
  double obj;
  int res;
  char *cflags;

  IpoptProblem nlp = NULL;
  IPOPT_DATA_ *iData = ((IPOPT_DATA_*)solverInfo->solverData);
  iData->current_var = 0;
  iData->current_time = 0;
  iData->data = data;
  iData->mayer = mayer(data, &obj);
  iData->lagrange = lagrange(data, &obj);
  iData->numObject = 2 - iData->mayer -iData->lagrange;

  iData->matrixA = initialAnalyticJacobianA((void*) iData->data);
  iData->matrixB = initialAnalyticJacobianB((void*) iData->data);
  /*
  iData->matrixC = initialAnalyticJacobianC((void*) iData->data);
  iData->matrixD = initialAnalyticJacobianD((void*) iData->data);
  */

  loadDAEmodel(data, iData);
  iData->index_debug_iter=0;
  iData->degub_step =  10;
  iData->index_debug_next=0;

    cflags = omc_flagValue[FLAG_LS_IPOPT];
    if(!cflags)
      cflags = "mumps";

  /*ToDo*/
  for(i=0; i<(*iData).nx; i++)
  {
    iData->Vmin[i] = (*iData).Vmax[i] = (*iData).x0[i]*iData->scalVar[i];
    iData->v[i] = iData->Vmin[i];

    if(ACTIVE_STREAM(LOG_IPOPT))
    {
      printf("\nx[%i] = %s = %g",i, iData->data->modelData.realVarsData[i].info.name,iData->v[i]);
    }
  }
  initial_guess_ipopt(iData,solverInfo);

  if(ACTIVE_STREAM(LOG_IPOPT))
  {
    for(; i<iData->nv; ++i)
      printf("\nu[%i] = %s = %g",i, iData->data->modelData.realVarsData[iData->index_u + i-iData->nx].info.name,iData->v[i]);
  }

  ipoptDebuge(iData,iData->v);

  if(flag == 5)
  {
     nlp = CreateIpoptProblem((*iData).NV, (*iData).Vmin, (*iData).Vmax,
         (*iData).NRes, (*iData).gmin, (*iData).gmax, (*iData).njac, NULL, 0, &evalfF,
                  &evalfG, &evalfDiffF, &evalfDiffG, &ipopt_h);

    AddIpoptNumOption(nlp, "tol", iData->data->simulationInfo.tolerance);

    if(ACTIVE_STREAM(LOG_IPOPT))
    {
      AddIpoptIntOption(nlp, "print_level", 5);
      AddIpoptIntOption(nlp, "file_print_level", 0);
    }
    else if(ACTIVE_STREAM(LOG_STATS))
    {
      AddIpoptIntOption(nlp, "print_level", 3);
      AddIpoptIntOption(nlp, "file_print_level", 0);
    }
    else
    {
      AddIpoptIntOption(nlp, "print_level", 2);
      AddIpoptIntOption(nlp, "file_print_level", 0);
    }

    AddIpoptStrOption(nlp, "mu_strategy", "adaptive");
    AddIpoptStrOption(nlp, "hessian_approximation", "limited-memory");

    if(cflags)
      AddIpoptStrOption(nlp, "linear_solver", cflags);
    else
      AddIpoptStrOption(nlp, "linear_solver", "mumps");

    /* AddIpoptStrOption(nlp, "derivative_test", "second-order"); */
    /* AddIpoptStrOption(nlp, "derivative_test_print_all", "yes"); */
    /* AddIpoptNumOption(nlp,"derivative_test_perturbation",1e-6); */
    AddIpoptIntOption(nlp, "max_iter", 5000);

    res = IpoptSolve(nlp, (*iData).v, NULL, &obj, (*iData).mult_g, (*iData).mult_x_L, (*iData).mult_x_U, (void*)iData);
    FreeIpoptProblem(nlp);

    if(ACTIVE_STREAM(LOG_IPOPT))
    {
      for(i =0; i<iData->nv;i++)
        if(iData->pFile[i])
          fclose(iData->pFile[i]);
      if(iData->pFile)
        free(iData->pFile);
    }

    iData->current_var = 0;
    res2file(iData,solverInfo);
  }
  return 0;
}