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());
}
}
}
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");
}
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;
}
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");
}
}
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;
}
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;
}
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;
}
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 ;
}
/**
* 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);
}
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);
};
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;
}
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;
}
/**
* 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);
}
}
/*!
\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;
}
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
}
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;
}
/*!
* 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;
}
