int main()
{
std::cout << std::fixed << std::setprecision(5) ;
double x = 3.00055 ;
std::cout << "x: " << x << " Fx(x): " << Fx(x) << " (root)\n\n" ;
std::cout << "now, iterating from 3.0000 in steps of 0.0001\n" ;
bool found = false ;
for( double x = 3.0000 ; x < 3.0010 ; x += 0.0001 )
{
if( Fx(x) >= 0.0 )
{
std::cout << "found the root!\n" ;
found = true ;
break ;
}
std::cout << "x: " << x << " Fx(x): " << Fx(x) << '\n' ;
}
if( !found ) std::cout << "alas! we missed the root.\n" ;
}
double Polynomial_Root(int n, double c[], double a, double b, double EPS){
double t;
if(a > b){
t = a;
a = b;
b = t;
}
double eps = EPS * 1e-7;
double minError = 999;
int MAX_ITERATION = 1000;
double x, root, f, df, d2f;
int i, j;
for(i = 0; i < 10; i++){
x = a + (b-a)*i/10;
j = 0;
while(j <= MAX_ITERATION){
f = Fx(n, c, x);
df = dFx(n, c, x);
d2f = d2Fx(n, c, x);
j++;
if(fabs(df*df-f*d2f) < eps)
break;
x = x - (f*df)/(df*df - f*d2f);
if(x<a || x>b)
break;
}
double error = fabs(Fx(n, c, x));
if(a<=x && x<=b && error<minError) {
root = x;
minError = error;
}
}
if(fabs(root) < eps)
return fabs(root);
else
return root;
}
Environment::Environment() {
environment = VS::get_singleton()->environment_create();
set_background(BG_DEFAULT_COLOR);
set_background_param(BG_PARAM_COLOR,Color(0,0,0));
set_background_param(BG_PARAM_TEXTURE,Ref<ImageTexture>());
set_background_param(BG_PARAM_CUBEMAP,Ref<CubeMap>());
set_background_param(BG_PARAM_ENERGY,1.0);
set_background_param(BG_PARAM_SCALE,1.0);
set_background_param(BG_PARAM_GLOW,0.0);
for(int i=0;i<FX_MAX;i++)
set_enable_fx(Fx(i),false);
fx_set_param(FX_PARAM_AMBIENT_LIGHT_COLOR,Color(0,0,0));
fx_set_param(FX_PARAM_AMBIENT_LIGHT_ENERGY,1.0);
fx_set_param(FX_PARAM_GLOW_BLUR_PASSES,1);
fx_set_param(FX_PARAM_GLOW_BLUR_SCALE,1);
fx_set_param(FX_PARAM_GLOW_BLUR_STRENGTH,1);
fx_set_param(FX_PARAM_GLOW_BLOOM,0.0);
fx_set_param(FX_PARAM_GLOW_BLOOM_TRESHOLD,0.5);
fx_set_param(FX_PARAM_DOF_BLUR_PASSES,1);
fx_set_param(FX_PARAM_DOF_BLUR_BEGIN,100.0);
fx_set_param(FX_PARAM_DOF_BLUR_RANGE,10.0);
fx_set_param(FX_PARAM_HDR_TONEMAPPER,FX_HDR_TONE_MAPPER_LINEAR);
fx_set_param(FX_PARAM_HDR_EXPOSURE,0.4);
fx_set_param(FX_PARAM_HDR_WHITE,1.0);
fx_set_param(FX_PARAM_HDR_GLOW_TRESHOLD,0.95);
fx_set_param(FX_PARAM_HDR_GLOW_SCALE,0.2);
fx_set_param(FX_PARAM_HDR_MIN_LUMINANCE,0.4);
fx_set_param(FX_PARAM_HDR_MAX_LUMINANCE,8.0);
fx_set_param(FX_PARAM_HDR_EXPOSURE_ADJUST_SPEED,0.5);
fx_set_param(FX_PARAM_FOG_BEGIN,100.0);
fx_set_param(FX_PARAM_FOG_ATTENUATION,1.0);
fx_set_param(FX_PARAM_FOG_BEGIN_COLOR,Color(0,0,0));
fx_set_param(FX_PARAM_FOG_END_COLOR,Color(0,0,0));
fx_set_param(FX_PARAM_FOG_BG,true);
fx_set_param(FX_PARAM_BCS_BRIGHTNESS,1.0);
fx_set_param(FX_PARAM_BCS_CONTRAST,1.0);
fx_set_param(FX_PARAM_BCS_SATURATION,1.0);
}
//---------------------------------------------------------
void TestPoissonIPDG3D::Run()
//---------------------------------------------------------
{
umLOG(1, "TestPoissonIPDG3D::Run()\n");
double wt1=timer.read();
// Initialize solver and construct grid and metric
StartUp3D();
#if (0)
//-------------------------------------
// check the mesh
//-------------------------------------
Output_Mesh();
umLOG(1, "\n*** Exiting after writing mesh\n\n");
return;
#endif
// sparse operators
CSd A("OP"), M("MM");
// build 3D IPDG Poisson matrix (assuming all Dirichlet)
PoissonIPDG3D(A, M);
if (0) {
// NBN: experiment with diagonal strength
int Nz=0;
for (int j=0; j<A.n; ++j) {
for (int p = A.P[j]; p<A.P[j+1]; ++p) {
if (A.I[p] == j) {
A.X[p] += A.X[p]; // augment A(i,j)
}
}
}
}
if (0) {
umLOG(1, "Dumping file Afull.dat for Matlab\n");
umLOG(1, "A(%d,%d), nnz = %d\n", A.m,A.n, A.P[A.n]);
FILE* fp=fopen("Afull.dat", "w");
A.write_ML(fp);
fclose(fp);
umLOG(1, "exiting.\n");
return;
}
// iterative solver
CS_PCG it_sol;
try
{
// Note: operator A is symmetric, with only its
// lower triangule stored. We use this symmetry
// to accelerate operator A*x
int flag=sp_SYMMETRIC; flag|=sp_LOWER; flag|=sp_TRIANGULAR;
A.set_shape(flag);
// drop tolerance for cholinc
double droptol=1e-4;
// Note: ownership of A is transfered to solver object
it_sol.cholinc(A, droptol);
} catch(...) {
umLOG(1, "\nCaught exception from symbolic chol.\n");
}
DVec exact("exact"), f("f"), rhs("rhs"), u("u");
double t1=0.0,t2=0.0;
//-------------------------------------------------------
// set up boundary condition
//-------------------------------------------------------
DVec xbc = Fx(mapB), ybc = Fy(mapB), zbc = Fz(mapB);
DVec ubc(Nfp*Nfaces*K);
ubc(mapB) = sin(pi*xbc) * sin(pi*ybc) * sin(pi*zbc);
//-------------------------------------------------------
// form right hand side contribution from boundary condition
//-------------------------------------------------------
DMat Abc = PoissonIPDGbc3D(ubc);
//-------------------------------------------------------
// evaluate forcing function
//-------------------------------------------------------
exact = sin(pi*x).dm(sin(pi*y).dm(sin(pi*z)));
f = (-3.0*SQ(pi)) * exact;
//-------------------------------------------------------
// set up right hand side for variational Poisson equation
//-------------------------------------------------------
rhs = M*(-f) + (DVec&)(Abc);
//-------------------------------------------------------
// solve using pcg iterative solver
//-------------------------------------------------------
t1 = timer.read();
u = it_sol.solve(rhs, 1e-9, 30);
t2 = timer.read();
//-------------------------------------------------------
// compute nodal error
//-------------------------------------------------------
r = (u-exact);
m_maxAbsError = r.max_val_abs();
umLOG(1, " solve -- done. (%0.4lf sec)\n\n", t2-t1);
umLOG(1, " max error = %g\n\n", m_maxAbsError);
#if (0)
//#######################################################
// plot solution and error
figure(2);
subplot(1,3,2); PlotContour3D(2*N, u, linspace(-1, 1, 10)); title('numerical solution');
subplot(1,3,3); PlotContour3D(2*N, log10(eps+abs(u-exact)), linspace(-4, 0, 10));
title('numerical error');
//#######################################################
#endif
double wt2=timer.read();
umLOG(1, "TestPoissonIPDG3D::Run() complete\n");
umLOG(1, "total time = %0.4lf sec\n\n", wt2-wt1);
}
D(3), D(4), D(5), 0, 0, 0, A(2), A(3), A(4), 0, 0, 0, C(3), C(4), C(5), 0, 0, 0, Ax(2), Ax(3), Ax(4), 0, 0, 0, G(2), G(3), G(4), 0, 0, 0, D(1), D(2), D(3), 0, 0, 0, G(1), G(2), G(3), 0, 0, 0, Ax(1), Ax(2), Ax(3), 0, 0, 0, D(2), D(3), D(4), 0, 0, 0, G(2), G(3), G(4), 0, 0, 0, A(2), A(3), A(4), 0, 0, 0, D(1), D(2), D(3), 0, 0, 0, A(1), A(2), A(3), 0, 0, 0, D(2), D(3), D(4), 0, 0, 0, Fx(2), Fx(3), Fx(4), 0, 0, 0, A(2), A(3), A(4), 0, 0, 0, Ax(2), Ax(3), Ax(4), 0, 0, 0, D(1), D(2), D(3), 0, 0, 0, G(1), G(2), G(3), 0, 0, 0, Ax(1), Ax(2), Ax(3), 0, 0, 0, D(3), D(4), D(5), 0, 0, 0, Cx(3), Cx(4), Cx(5), 0, 0, 0, D(3), D(4), D(5), 0, 0, 0, Cx(3), Cx(4), Cx(5), 0, 0, 0, D(3), D(4), D(5), 0, 0, 0, A(2), A(3), A(4), 0, 0, 0, C(3), C(4), C(5), 0, 0, 0, Ax(2), Ax(3), Ax(4), 0, 0, 0, G(2), G(3), G(4), 0, 0, 0,
void Model :: MD_2D(double le,int BstartID,int beforeN,int newN)
{
//分子動力学によりnewN個の粒子の位置を最適化 IDがBstartIDからBendIDまでのは境界粒子なので動かさない
double region[2][2]; //解析領域
/////////////////////解析領域の決定
region[A_X][0]=100; region[A_X][1]=-100;
region[A_Y][0]=100; region[A_Y][1]=-100;
for(int i=BstartID;i<beforeN;i++)
{
if(PART[i].Get_X()<region[A_X][0]) region[A_X][0]=PART[i].Get_X();
else if(PART[i].Get_X()>region[A_X][1]) region[A_X][1]=PART[i].Get_X();
if(PART[i].Get_Y()<region[A_Y][0]) region[A_Y][0]=PART[i].Get_Y();
else if(PART[i].Get_Y()>region[A_Y][1]) region[A_Y][1]=PART[i].Get_Y();
}
for(int D=0;D<2;D++)
{
region[D][0]-=5*le; //少し領域を広めにとる
region[D][1]+=5*le;
}//////////////////////////
//パラメータ
double k0=1;
double r=1.5;
double dt=0.001;
//力はax^3+bx^2+dの式を採用。文献[Bubble Mesh Automated Triangular Meshing of Non-Manifold Geometry by Sphere Packing]を参照
double a=(r+1)/(2*r*r-r-1)*k0/(le*le);
double b=-0.5*k0/le-1.5*a*le;
double d=-a*le*le*le-b*le*le;
/////////////
int lastN=beforeN+newN;
vector<double> Fx(newN); //各粒子に働くX方向力
vector<double> Fy(newN); //各粒子に働くY方向力
vector<double> Ax(newN,0); //X方向加速度
vector<double> Ay(newN,0); //Y方向加速度
vector<double> U(newN,0); //X方向速度
vector<double> V(newN,0); //Y方向速度
vector<double> visX(newN); //X方向粘性係数
vector<double> visY(newN); //Y方向粘性係数
//計算の高速化のために格子を形成 解析幅がr*leで割り切れるとは限らないので、はみ出したところは切り捨て。なので各軸とも正の方向には余裕を持つこと
double grid_width=le*((int)(r+1)); //格子の幅。rを含む整数*le
int grid_sizeX=(int)((region[A_X][1]-region[A_X][0])/grid_width); //X方向の格子の個数
int grid_sizeY=(int)((region[A_Y][1]-region[A_Y][0])/grid_width);
int plane_SIZE=grid_sizeX*grid_sizeY;
int *index=new int[newN]; //各内部粒子を含む格子番号
// cout<<"ここっぽい"<<endl;
// vector<int> *MESH=new vector<int>[plane_SIZE]; //各メッシュに格納される粒子ID格納
vector<vector<int> > MESH;
MESH.resize(plane_SIZE);
for(int i=BstartID;i<beforeN;i++) //まずは境界粒子を格子に格納
{
int xn=(int)((PART[i].Get_X()-region[A_X][0])/grid_width); //X方向に何個目の格子か
int yn=(int)((PART[i].Get_Y()-region[A_Y][0])/grid_width); //Y方向に何個目の格子か
int number=yn*grid_sizeX+xn; //粒子iを含む格子の番号
MESH[number].push_back(i);
}
for(int k=0;k<newN;k++) //つぎに内部粒子を格納
{
int i=beforeN+k;
int xn=(int)((PART[i].Get_X()-region[A_X][0])/grid_width);//X方向に何個目の格子か
int yn=(int)((PART[i].Get_Y()-region[A_Y][0])/grid_width);//Y方向に何個目の格子か
int number=yn*grid_sizeX+xn; //粒子iを含む格子の番号
MESH[number].push_back(i);
index[k]=number;
}//////////////////////////////////////////
//計算開始
for(int t=0;t<100;t++)
{
if(t%10==0 &&t>0)//MESHを作り直す
{
//まずはMESHを一度破壊する。
for(int n=0;n<plane_SIZE;n++)
{
size_t size=MESH[n].size();
for(int k=0;k<size;k++) MESH[n].pop_back();
}
for(int i=BstartID;i<beforeN;i++) //まずは境界粒子を格子に格納
{
int xn=(int)((PART[i].Get_X()-region[A_X][0])/grid_width); //X方向に何個目の格子か
int yn=(int)((PART[i].Get_Y()-region[A_Y][0])/grid_width); //Y方向に何個目の格子か
int number=yn*grid_sizeX+xn; //粒子iを含む格子の番号
MESH[number].push_back(i);
}
for(int k=0;k<newN;k++) //つぎに内部粒子を格納
{
int i=beforeN+k;
int xn=(int)((PART[i].Get_X()-region[A_X][0])/grid_width);//X方向に何個目の格子か
int yn=(int)((PART[i].Get_Y()-region[A_Y][0])/grid_width);//Y方向に何個目の格子か
int number=yn*grid_sizeX+xn; //粒子iを含む格子の番号
MESH[number].push_back(i);
index[k]=number;
}
}////////////
for(int k=0;k<newN;k++)
{
Fx[k]=0; Fy[k]=0; //初期化
int i=beforeN+k; //対応する粒子番号
double kx=0; //X方向バネ係数
double ky=0;
int G_id=index[k]; //格納する格子番号
for(int II=G_id-1;II<=G_id+1;II++)
{
for(int JJ=-1*grid_sizeX;JJ<=grid_sizeX;JJ+=grid_sizeX)
{
int M_id=II+JJ;
for(int L=0;L<MESH[M_id].size();L++)
{
int j=MESH[M_id][L];
double x=PART[j].Get_X()-PART[i].Get_X();
double y=PART[j].Get_Y()-PART[i].Get_Y();
double dis=sqrt(x*x+y*y);
if(dis<r*le && dis!=0) //このloopは自分自身も通過するから、dis!=0は必要
{
double F=a*dis*dis*dis+b*dis*dis+d;
Fx[k]-=F*x/dis; //Fの値が正のときは斥力なので、-=にする
Fy[k]-=F*y/dis;
double K=3*a*dis*dis+2*b*dis;//バネ係数 力の式の微分に相当
K=sqrt(K*K); //正の値が欲しい。だから負のときに備えて正に変換
kx+=K*x*x/(dis*dis); //kを各方向に分配。ここで、常に正の量が分配されるようにx*x/(dis*dis)となっている
ky+=K*y*y/(dis*dis);
}
}
}
}
visX[k]=1.414*sqrt(kx);//このように各軸方向の粘性係数を決める。文献「物理モデルによる自動メッシュ分割」P6参照。ただし質量は1としている。
visY[k]=1.414*sqrt(ky);
Ax[k]=(Fx[k]-visX[k]*U[k]);
Ay[k]=(Fy[k]-visY[k]*V[k]);
}//各粒子の加速度が求まった。
if(t==0) //最初のｽﾃｯﾌﾟ時にdtを決定
{
double MaxAccel=0;
for(int k=0;k<newN;k++)
{
double accel2=Ax[k]*Ax[k]+Ay[k]*Ay[k];
if(accel2>MaxAccel) MaxAccel=accel2;
}
MaxAccel=sqrt(MaxAccel);//最大加速度が求まった
dt=sqrt(0.02*le/MaxAccel);
}
for(int k=0;k<newN;k++)//速度と位置の更新
{
int i=beforeN+k;
double u=U[k];
double v=V[k];
U[k]+=dt*Ax[k];
V[k]+=dt*Ay[k];
PART[i].Add(dt*(U[k]+u)*0.5, dt*(V[k]+v)*0.5, 0);
}
//再近接距離がle以下の場合はこれを修正
for(int k=0;k<newN;k++)
{
int i=beforeN+k; //対応する粒子番号
int G_id=index[k]; //格納する格子番号
double mindis=le;
int J=k; //最近接距離の相手粒子
for(int II=G_id-1;II<=G_id+1;II++)
{
for(int JJ=-1*grid_sizeX;JJ<=grid_sizeX;JJ+=grid_sizeX)
{
int M_id=II+JJ;
for(int L=0;L<MESH[M_id].size();L++)
{
int j=MESH[M_id][L];
double x=PART[j].Get_X()-PART[i].Get_X();
double y=PART[j].Get_Y()-PART[i].Get_Y();
double dis=sqrt(x*x+y*y);
if(dis<mindis && i!=j)
{
mindis=dis;
J=j;
}
}
}
}
if(J!=i && J<beforeN)//leより近接している相手が境界粒子なら
{
double L=le-mindis;//開くべき距離
double dX=PART[J].Get_X()-PART[i].Get_X();
double dY=PART[J].Get_Y()-PART[i].Get_Y();
PART[i].Add(-(dX/mindis*L), -(dY/mindis*L), 0);
}
else if(J!=i && J>=beforeN)//leより近接している相手が内部粒子なら
{
double L=0.5*(le-mindis);//開くべき距離
double dX=PART[J].Get_X()-PART[i].Get_X();
double dY=PART[J].Get_Y()-PART[i].Get_Y();
PART[i].Add(-(dX/mindis*L), -(dY/mindis*L), 0);
PART[J].Add(dX/mindis*L, dY/mindis*L, 0);
}
}//////////*/
}/////MD終了
delete [] index;
// delete [] MESH;
}
void Model :: MD_3D(double le,int BstartID,int beforeN,int newN,double r,double region[3][2])
{
//分子動力学によりnewN個の粒子の位置を最適化 IDがBstartIDからBendIDまでのは境界粒子なので動かさない
double k0=1;
double dt=0.001;
int BendID=beforeN;
//力はax^3+bx^2+dの式を採用。文献[Bubble Mesh Automated Triangular Meshing of Non-Manifold Geometry by Sphere Packing]を参照
double a=(r+1)/(2*r*r-r-1)*k0/(le*le);
double b=-0.5*k0/le-1.5*a*le;
double d=-a*le*le*le-b*le*le;
/////////////
int lastN=beforeN+newN;
//cout<<"F="<<a*le*le*le+b*le*le+d<<" "<<a*1.5*le*1.5*le*1.5*le+b*1.5*le*1.5*le+d<<endl;
vector<double> Fx(newN); //各粒子に働くX方向力
vector<double> Fy(newN); //各粒子に働くY方向力
vector<double> Fz(newN); //各粒子に働くZ方向力
vector<double> Ax(newN,0); //X方向加速度
vector<double> Ay(newN,0); //Y方向加速度
vector<double> Az(newN,0); //Z方向加速度
vector<double> U(newN,0); //X方向速度
vector<double> V(newN,0); //Y方向速度
vector<double> W(newN,0); //Z方向速度
vector<double> visX(newN); //X方向粘性係数
vector<double> visY(newN); //Y方向粘性係数
vector<double> visZ(newN); //Y方向粘性係数
vector<double> KX(newN); //X方向バネ係数
vector<double> KY(newN); //Y方向バネ係数
vector<double> KZ(newN); //Y方向バネ係数
//計算の高速化のために格子を形成 解析幅がr*leで割り切れるとは限らないので、はみ出したところは切り捨て。なので各軸とも正の方向には余裕を持つこと
double grid_width=le*((int)(r+1)); //格子の幅。rを含む整数*le
int grid_sizeX=(int)((region[A_X][1]-region[A_X][0])/grid_width); //X方向の格子の個数
int grid_sizeY=(int)((region[A_Y][1]-region[A_Y][0])/grid_width);
int grid_sizeZ=(int)((region[A_Z][1]-region[A_Z][0])/grid_width);
int grid_SIZE=grid_sizeX*grid_sizeY*grid_sizeZ;
int plane_SIZE=grid_sizeX*grid_sizeY;
int *index=new int[newN]; //各内部粒子を含む格子番号
vector<int> *MESH=new vector<int>[grid_SIZE]; //各メッシュに格納される粒子ID格納
for(int i=BstartID;i<BendID;i++) //まずは境界粒子を格子に格納
{
int xn=(int)((PART[i].Get_X()-region[A_X][0])/grid_width);//X方向に何個目の格子か
int yn=(int)((PART[i].Get_Y()-region[A_Y][0])/grid_width);//Y方向に何個目の格子か
int zn=(int)((PART[i].Get_Z()-region[A_Z][0])/grid_width);//Z方向に何個目の格子か
int number=zn*grid_sizeX*grid_sizeY+yn*grid_sizeX+xn;//粒子iを含む格子の番号
MESH[number].push_back(i);
}
for(int k=0;k<newN;k++) //つぎに内部粒子を格納
{
int i=beforeN+k;
int xn=(int)((PART[i].Get_X()-region[A_X][0])/grid_width);//X方向に何個目の格子か
int yn=(int)((PART[i].Get_Y()-region[A_Y][0])/grid_width);//Y方向に何個目の格子か
int zn=(int)((PART[i].Get_Z()-region[A_Z][0])/grid_width);//Z方向に何個目の格子か
int number=zn*grid_sizeX*grid_sizeY+yn*grid_sizeX+xn;//粒子iを含む格子の番号
MESH[number].push_back(i);
index[k]=number;
}
//計算開始
for(int t=0;t<100;t++)
{
if(t%10==0 && t>0)
{
//MESHを一度破壊する。
for(int n=0;n<grid_SIZE;n++)
{
size_t size=MESH[n].size();
for(int k=0;k<size;k++) MESH[n].pop_back();
}
for(int i=BstartID;i<BendID;i++) //まずは境界粒子を格子に格納
{
int xn=(int)((PART[i].Get_X()-region[A_X][0])/grid_width);//X方向に何個目の格子か
int yn=(int)((PART[i].Get_Y()-region[A_Y][0])/grid_width);//Y方向に何個目の格子か
int zn=(int)((PART[i].Get_Z()-region[A_Z][0])/grid_width);//Z方向に何個目の格子か
int number=zn*grid_sizeX*grid_sizeY+yn*grid_sizeX+xn;//粒子iを含む格子の番号
MESH[number].push_back(i);
}
for(int k=0;k<newN;k++) //つぎに内部粒子を格納
{
int i=beforeN+k;
int xn=(int)((PART[i].Get_X()-region[A_X][0])/grid_width);//X方向に何個目の格子か
int yn=(int)((PART[i].Get_Y()-region[A_Y][0])/grid_width);//Y方向に何個目の格子か
int zn=(int)((PART[i].Get_Z()-region[A_Z][0])/grid_width);//Z方向に何個目の格子か
int number=zn*grid_sizeX*grid_sizeY+yn*grid_sizeX+xn;//粒子iを含む格子の番号
MESH[number].push_back(i);
index[k]=number;
}
}
for(int k=0;k<newN;k++)
{
Fx[k]=0; Fy[k]=0, Fz[k]=0; //初期化
KX[k]=0;KY[k]=0; KZ[k]=0; //バネ係数
}
for(int k=0;k<newN;k++)
{
int i=beforeN+k; //対応する粒子番号
int G_id=index[k]; //格納する格子番号
for(int II=G_id-1;II<=G_id+1;II++)
{
for(int JJ=-1*grid_sizeX;JJ<=grid_sizeX;JJ+=grid_sizeX)
{
for(int KK=-1*plane_SIZE;KK<=plane_SIZE;KK+=plane_SIZE)
{
int M_id=II+JJ+KK;
for(int L=0;L<MESH[M_id].size();L++)
{
int j=MESH[M_id][L];
if(j>=beforeN && j>i) //同じ領域内でかつiより大きな番号なら
{
int J=j-beforeN; //newN内での番号
double x=PART[j].Get_X()-PART[i].Get_X();
double y=PART[j].Get_Y()-PART[i].Get_Y();
double z=PART[j].Get_Z()-PART[i].Get_Z();
double dis=sqrt(x*x+y*y+z*z);
if(dis<r*le) //このloopは自分自身も通過するから、dis!=0は必要
{
double F=a*dis*dis*dis+b*dis*dis+d;
Fx[k]-=F*x/dis; //Fの値が正のときは斥力なので、-=にする
Fy[k]-=F*y/dis;
Fz[k]-=F*z/dis;
Fx[J]+=F*x/dis; //相手粒子の力もここで計算。符号は反転させる
Fy[J]+=F*y/dis;
Fz[J]+=F*z/dis;
double K=3*a*dis*dis+2*b*dis;//バネ係数 力の式の微分に相当
K=sqrt(K*K); //正の値が欲しい。だから負のときに備えて正に変換
KX[k]+=K*x*x/(dis*dis); //kを各方向に分配。ここで、常に正の量が分配されるようにx*x/(dis*dis)となっている
KY[k]+=K*y*y/(dis*dis);
KZ[k]+=K*z*z/(dis*dis);
KX[J]+=K*x*x/(dis*dis); //kを相手粒子にも分配
KY[J]+=K*y*y/(dis*dis);
KZ[J]+=K*z*z/(dis*dis);
}
}
if(j<BendID && j>=BstartID)
{
double x=PART[j].Get_X()-PART[i].Get_X();
double y=PART[j].Get_Y()-PART[i].Get_Y();
double z=PART[j].Get_Z()-PART[i].Get_Z();
double dis=sqrt(x*x+y*y+z*z);
if(dis<r*le && dis>0) //このloopは自分自身は通過しない、dis!=0は不要
{
double F=a*dis*dis*dis+b*dis*dis+d;
Fx[k]-=F*x/dis; //Fの値が正のときは斥力なので、-=にする
Fy[k]-=F*y/dis;
Fz[k]-=F*z/dis;
double K=3*a*dis*dis+2*b*dis;//バネ係数 力の式の微分に相当
K=sqrt(K*K); //正の値が欲しい。だから負のときに備えて正に変換
KX[k]+=K*x*x/(dis*dis); //kを各方向に分配。ここで、常に正の量が分配されるようにx*x/(dis*dis)となっている
KY[k]+=K*y*y/(dis*dis);
KZ[k]+=K*z*z/(dis*dis);
}
}
}
}
}
}
//visX[k]=1.414*sqrt(KX[k]);//このように各軸方向の粘性係数を決める。文献「物理モデルによる自動メッシュ分割」P6参照。ただし質量は1としている。
//visY[k]=1.414*sqrt(KY[k]);
//visZ[k]=1.414*sqrt(KZ[k]);
visX[k]=1.414*sqrt(KX[k]);//このように各軸方向の粘性係数を決める。文献「物理モデルによる自動メッシュ分割」P6参照。ただし質量は1としている。
visY[k]=1.414*sqrt(KY[k]);
visZ[k]=1.414*sqrt(KZ[k]);
Ax[k]=(Fx[k]-visX[k]*U[k]);
Ay[k]=(Fy[k]-visY[k]*V[k]);
Az[k]=(Fz[k]-visZ[k]*W[k]);
}//各粒子の加速度が求まった。
if(t==0) //最初のｽﾃｯﾌﾟ時にdtを決定
{
double MaxAccel=0;
for(int k=0;k<newN;k++)
{
double accel2=Ax[k]*Ax[k]+Ay[k]*Ay[k]+Az[k]*Az[k];
if(accel2>MaxAccel) MaxAccel=accel2;
}
MaxAccel=sqrt(MaxAccel);//最大加速度が求まった
if(MaxAccel!=0)
{
dt=sqrt(0.02*le/MaxAccel);
}
}
for(int k=0;k<newN;k++)//速度と位置の更新
{
int i=beforeN+k;
double u=U[k];
double v=V[k];
double w=W[k];
U[k]+=dt*Ax[k];
V[k]+=dt*Ay[k];
W[k]+=dt*Az[k];
PART[i].Add(dt*(U[k]+u)*0.5, dt*(V[k]+v)*0.5, dt*(W[k]+w)*0.5);
}
//再近接距離がle以下の場合はこれを修正
for(int k=0;k<newN;k++)
{
int i=beforeN+k; //対応する粒子番号
int G_id=index[k]; //格納する格子番号
double mindis=le;
int J=k; //最近接距離の相手粒子
for(int II=G_id-1;II<=G_id+1;II++)
{
for(int JJ=-1*grid_sizeX;JJ<=grid_sizeX;JJ+=grid_sizeX)
{
for(int KK=-1*plane_SIZE;KK<=plane_SIZE;KK+=plane_SIZE)
{
int M_id=II+JJ+KK;
for(int L=0;L<MESH[M_id].size();L++)
{
int j=MESH[M_id][L];
double x=PART[j].Get_X()-PART[i].Get_X();
double y=PART[j].Get_Y()-PART[i].Get_Y();
double z=PART[j].Get_Z()-PART[i].Get_Z();
double dis=sqrt(x*x+y*y+z*z);
if(dis<mindis && i!=j)
{
mindis=dis;
J=j;
}
}
}
}
}
if(J!=i && J<beforeN)//leより近接している相手が境界粒子なら
{
double L=le-mindis;//開くべき距離
double dX=PART[J].Get_X()-PART[i].Get_X();
double dY=PART[J].Get_Y()-PART[i].Get_Y();
double dZ=PART[J].Get_Z()-PART[i].Get_Z();
PART[i].Add(-dX/mindis*L, -dY/mindis*L, -dZ/mindis*L);
}
else if(J!=i && J>=beforeN)//leより近接している相手が内部粒子なら
{
double L=0.5*(le-mindis);//開くべき距離
double dX=PART[J].Get_X()-PART[i].Get_X();
double dY=PART[J].Get_Y()-PART[i].Get_Y();
double dZ=PART[J].Get_Z()-PART[i].Get_Z();
PART[i].Add(-dX/mindis*L, -dY/mindis*L, -dZ/mindis*L);
PART[J].Add(dX/mindis*L, dY/mindis*L, dZ/mindis*L);
}
}//////////*/
}/////MD終了
delete [] index;
delete [] MESH;
}
void upperBound(double x[],double *pfc,double f[],double g[],DOUBLE **dfdx,
int n,int m,int me,double ko[],double komod[],double teta[])
{
double D, Z;
double *lb, *rb, *Qcmp, Q[_Nq], **b;
double *pE, ETK[_Nq];
int *piE;
double *dF_dQ, *MQi;
int iz, iq;
struct T *pT;
struct TK *pTK;
int indexq, indext, indexz, indexa,/* indexd,*/ icrit, i_komod, total_krit;
//double EF0, ET0, ETW1, EUU, EKR;
//double VKCE, VKCE0;
//double dconvdF0, dconvdT0, dconvdTW1, dconvdUU, dconvdKR;
//double dT2dF0, dT2dT0, dT2dTW1, dT2dUU, dT2dKR;
//double dQHEdF0, dQHEdT0, dQHEdTW1, dQHEdUU, dQHEdKR;
//double dFwdF0, dFwdT0, dFwdTW1, dFwdUU, dFwdKR;
//double dF1dF0, dF1dT0, dF1dTW1, dF1dUU, dF1dKR;
//double dFW, dF1;
//double *a, *ai;
//struct Q *qi,*qroot=NULL;
//struct Q *qapi,*qaproot=NULL;
double d[_Nd];
int *NQcr;
double U;
int i,j,k,s,ij;
//double *numbers;
double koef;
//double *pQap=NULL;
//double *pZap=NULL;
//double gamma;
double y[_Nq],sig[_Nq],kk;
int nqcr,nqapcr;
int N;
double F;
double *pF=NULL;
*pfc=0;
//Nf = (int)komod[0];
//Nd=(int)komod[2];
//Nt=(int)komod[3];
//Nap=(int)komod[4];
//gamma=komod[5];
//Na=(int)komod[6];
pF=malloc(Nap*sizeof(double));
if(pF==NULL) exit(2);
// читаем D из komod - не как поисковые
//i_komod=7;
// заполняем массив числа крит точек по областям
//NQcr=malloc(Nt*sizeof(int));
//if(NQcr==NULL) exit(2);
////for(indext=0;indext<Nt;indext++)
//// NQcr[indext]=0;
//total_krit=0;
//for(indext=0;indext<Nt;indext++)
//{
// NQcr[indext]=(int)komod[i_komod];
// total_krit+=NQcr[indext];
// i_komod++;
//}
//qroot=malloc(sizeof(struct Q));
//if(qroot==NULL) exit(2);
//qroot->next=NULL;
//qi=qroot;
//i=0;
//nqcr=0;
//for(indext=0; indext<Nt; indext++)
//{
// icrit = NQcr[indext];
// for(indexq=0; indexq<icrit; indexq++)
// {
// qi->next = malloc(sizeof(struct Q));
// if(!qi->next) exit(2);
// qi = qi->next;
// qi->next = NULL;
// qi->F0 = komod[i_komod++];
// qi->T0 = komod[i_komod++];
// //qi->TW1 = komod[i_komod++];
// //qi->UU = komod[i_komod++];
// //qi->KR = komod[i_komod++];
// qi->T=indext;
// nqcr++;
// }
//}
//qaproot=malloc(sizeof(struct Q));
//if(qaproot==NULL) exit(2);
//qaproot->next=NULL;
//qapi=qaproot;
//i=0;
//nqapcr=0;
//for(indexq=0;indexq<Nap;indexq++)
//{
// qapi->next=malloc(sizeof(struct Q));
// if(qapi->next==NULL) exit(2);
// qapi=qapi->next;
// qapi->next=NULL;
// qapi->F0 = komod[i_komod++];
// qapi->T0 = komod[i_komod++];
// //qapi->TW1 = komod[i_komod++];
// //qapi->UU = komod[i_komod++];
// //qapi->KR = komod[i_komod++];
// qapi->T = indexq;
// nqapcr++;
//}
kk = 3.9; // Правило трех сигм
for(i=0; i<Nq; i++)
sig[i] = (start_q[i]-start_minq[i])/kk;
ij = Nd;
pT = ROPUD_pT;
while(pT=pT->pnext)
if(pT->isSoftCons)
{
for(iq=0; iq<Nq; iq++,ij++)
{
pT->slbound[iq] = x[ij];
pT->srbound[iq] = x[ij+Nq];
}
ij+=Nq;
};
// Копирование значений b
pTK = ROPUD_pTK;
while(pTK=pTK->pnext)
for(iz=0; iz<Nz; iz++)
for(iq=0; iq<Nq+1; iq++, ij++)
pTK->b[iz][iq] = x[ij];
//ограничения
//ограничения Эф Круглое
for(i=0; i<Na; i++)
f[i] = alpha[i];
pT = ROPUD_pT;
while(pT=pT->pnext)
{
if(pT->isSoftCons)
{
for (j=0, F=1; j<Nq; j++)
F *= (Fx((pT->srbound[j]-start_q[j])/sig[j]) - Fx((pT->slbound[j]-start_q[j])/sig[j]));
f[pT->NCons] -= F;
}
}
//f[0]*=20;
//f[1]*=200;
//f[3]*=200;
//f[4]*=200;
//вероятностные
i = Na;
//qi = qroot;
pT = ROPUD_pT;
for(indext=0; indext<Nt; indext++)
{
pT = pT->pnext;
// Ограничения на области по каждому параметру (правая граница больше левой)
if(pT->isSoftCons)
{
for(j=0; j<Nq; j++,i++)
f[i] = pT->slbound[j] - pT->srbound[j];
//f[i-4] /= 10.0; f[i-3] /= 10.0;
//f[i-2] *= 100.0; f[i-1] *= 1000.0;
}
//while((qi->next)&&(qi->next->T==indext))
for(icrit=0; icrit<pT->NQcr; icrit++)
{
//qi=qi->next;
//Q[2] = qi->TW1;
//Q[3] = qi->UU;
//Q[4] = qi->KR;
//Q = pT->Qcr + icrit*Nq;
Qcmp = pT->Qcr + icrit*Nq;
lb = pT->slbound;
rb = pT->srbound;
//if(pT->isSoftCons)
for(iq=0; iq<Nq; iq++)
Q[iq] = Qcmp[iq]*(rb[iq]-lb[iq]) + lb[iq];
//else
//for(iq=0; iq<Nq; iq++)
// Q[iq] = Q[iq]*(pT->rbound[iq] - pT->lbound[iq]) + pT->lbound[iq];
set_z(pT->relK, Q);
calcModel(x, pT->relK->z, Q);
//D = x[0];
//Z = pT->relK->z[0];
//switch(pT->NCons)
//{
// // Мягкие
//case 0:
// f[i] = -D-Z*Q[0]-2*Q[1]+1; //-D-Z*Q[0]-2*Q[1]+2
// break;
//case 1:
// f[i] = -2*D-2*Q[0]-Z*Q[1]+2; //-2*D-2*Q[0]-4*Z*Q[1]+5
// break;
// // Жесткие
//case 2:
// f[i] = zmin[0] - Z;
// break;
//case 3:
// f[i] = Z - zmax[0];
// break;
//}
////if(pT->isLocked) f[i] = 0;
//i++;
f[i++] = calcConstraint(pT->NCons);
//f[i-1] *= 50;
}
}
j = 0;
pTK = ROPUD_pTK;
while(pTK = pTK->pnext)
{
pE = pTK->E;
piE = pTK->iE;
for(iq=0; iq<Nq; iq++)
Q[iq] = (pTK->lbound[iq] + pTK->rbound[iq]) /2;
set_z(pTK, Q);
calcModel(x, pTK->z, Q);
//D = x[0];
//Z = pTK->z[0];
b = pTK->b;
//dF_dQ = malloc(2*sizeof(double));
//dF_dQ[0] = D + 0.5*Q[1]*b[0][0];
//dF_dQ[1] = 0.5*(Z + b[0][1]*Q[1]);
//**************************************************************
pF[j] = calcCriteria() * pTK->a;
if(is_linearization)
{
dF_dQ = calcDerivative(x, pTK->z, pTK->b, Q);
for(iq=0; iq<Nq; iq++)
pF[j] += dF_dQ[iq]*(*pE++-Q[iq]*pTK->ai[iq]);
}
//pF[j] += dF_dQ[0]*(*pE++-Q[0]*pTK->ai[0]);
//pF[j] += dF_dQ[1]*(*pE++-Q[1]*pTK->ai[1]);
//f[i++] = zmin[0] - *pTK->z;
//f[i++] = *pTK->z - zmax[0];
for(iz=0; iz<Nz*2; iz++)
f[i++] = calcConstraintOnZ(iz);
//f[i-1] *= 100;
//f[i-2] /= 100.0;
//f[i-3] *= 100;
//f[i-4] /= 100.0;
//if(D<0.6)
// D=D;
j++;
}
//if(Nap==1) *pfc = *pF;
//else
{
for(j=0;j<Nap;j++)
*pfc+=pF[j];
}
f[i++] = -*pfc;
//for(i=0; i<Nf; i++)
// //if(f[i]>0) f[i]+=1000;
// while(f[i]<0.1 && f[i]>0.00001) f[i]*=10;
//if(*pfc<0)
// printf("%f\t%f",T1,T2);
//qi=qroot;
//while(qi)
//{
// qi=qi->next;
// free(qroot);
// qroot=NULL;
// qroot=qi;
//}
//qapi=qaproot;
//while(qapi)
//{
// qapi=qapi->next;
// free(qaproot);
// qaproot=NULL;
// qaproot=qapi;
//}
//free(NQcr);
free(pF);
}
void assemble_postvars_rhs (EquationSystems& es,
const std::string& system_name)
{
const Real E = es.parameters.get<Real>("E");
const Real NU = es.parameters.get<Real>("NU");
const Real KPERM = es.parameters.get<Real>("KPERM");
Real sum_jac_postvars=0;
Real av_pressure=0;
Real total_volume=0;
#include "assemble_preamble_postvars.cpp"
for ( ; el != end_el; ++el)
{
const Elem* elem = *el;
dof_map.dof_indices (elem, dof_indices);
dof_map.dof_indices (elem, dof_indices_u, u_var);
dof_map.dof_indices (elem, dof_indices_v, v_var);
dof_map.dof_indices (elem, dof_indices_p, p_var);
dof_map.dof_indices (elem, dof_indices_x, x_var);
dof_map.dof_indices (elem, dof_indices_y, y_var);
#if THREED
dof_map.dof_indices (elem, dof_indices_w, w_var);
dof_map.dof_indices (elem, dof_indices_z, z_var);
#endif
const unsigned int n_dofs = dof_indices.size();
const unsigned int n_u_dofs = dof_indices_u.size();
const unsigned int n_v_dofs = dof_indices_v.size();
const unsigned int n_p_dofs = dof_indices_p.size();
const unsigned int n_x_dofs = dof_indices_x.size();
const unsigned int n_y_dofs = dof_indices_y.size();
#if THREED
const unsigned int n_w_dofs = dof_indices_w.size();
const unsigned int n_z_dofs = dof_indices_z.size();
#endif
fe_disp->reinit (elem);
fe_vel->reinit (elem);
fe_pres->reinit (elem);
Ke.resize (n_dofs, n_dofs);
Fe.resize (n_dofs);
Kuu.reposition (u_var*n_u_dofs, u_var*n_u_dofs, n_u_dofs, n_u_dofs);
Kuv.reposition (u_var*n_u_dofs, v_var*n_u_dofs, n_u_dofs, n_v_dofs);
Kup.reposition (u_var*n_u_dofs, p_var*n_u_dofs, n_u_dofs, n_p_dofs);
Kux.reposition (u_var*n_u_dofs, p_var*n_u_dofs + n_p_dofs , n_u_dofs, n_x_dofs);
Kuy.reposition (u_var*n_u_dofs, p_var*n_u_dofs + n_p_dofs+n_x_dofs , n_u_dofs, n_y_dofs);
#if THREED
Kuw.reposition (u_var*n_u_dofs, w_var*n_u_dofs, n_u_dofs, n_w_dofs);
Kuz.reposition (u_var*n_u_dofs, p_var*n_u_dofs + n_p_dofs+2*n_x_dofs , n_u_dofs, n_z_dofs);
#endif
Kvu.reposition (v_var*n_v_dofs, u_var*n_v_dofs, n_v_dofs, n_u_dofs);
Kvv.reposition (v_var*n_v_dofs, v_var*n_v_dofs, n_v_dofs, n_v_dofs);
Kvp.reposition (v_var*n_v_dofs, p_var*n_v_dofs, n_v_dofs, n_p_dofs);
Kvx.reposition (v_var*n_v_dofs, p_var*n_u_dofs + n_p_dofs , n_v_dofs, n_x_dofs);
Kvy.reposition (v_var*n_v_dofs, p_var*n_u_dofs + n_p_dofs+n_x_dofs , n_v_dofs, n_y_dofs);
#if THREED
Kvw.reposition (v_var*n_u_dofs, w_var*n_u_dofs, n_v_dofs, n_w_dofs);
Kuz.reposition (v_var*n_u_dofs, p_var*n_u_dofs + n_p_dofs+2*n_x_dofs , n_u_dofs, n_z_dofs);
#endif
#if THREED
Kwu.reposition (w_var*n_w_dofs, u_var*n_v_dofs, n_v_dofs, n_u_dofs);
Kwv.reposition (w_var*n_w_dofs, v_var*n_v_dofs, n_v_dofs, n_v_dofs);
Kwp.reposition (w_var*n_w_dofs, p_var*n_v_dofs, n_v_dofs, n_p_dofs);
Kwx.reposition (w_var*n_w_dofs, p_var*n_u_dofs + n_p_dofs , n_v_dofs, n_x_dofs);
Kwy.reposition (w_var*n_w_dofs, p_var*n_u_dofs + n_p_dofs+n_x_dofs , n_v_dofs, n_y_dofs);
Kww.reposition (w_var*n_w_dofs, w_var*n_u_dofs, n_v_dofs, n_w_dofs);
Kwz.reposition (w_var*n_w_dofs, p_var*n_u_dofs + n_p_dofs+2*n_x_dofs , n_u_dofs, n_z_dofs);
#endif
Kpu.reposition (p_var*n_u_dofs, u_var*n_u_dofs, n_p_dofs, n_u_dofs);
Kpv.reposition (p_var*n_u_dofs, v_var*n_u_dofs, n_p_dofs, n_v_dofs);
Kpp.reposition (p_var*n_u_dofs, p_var*n_u_dofs, n_p_dofs, n_p_dofs);
Kpx.reposition (p_var*n_v_dofs, p_var*n_u_dofs + n_p_dofs , n_p_dofs, n_x_dofs);
Kpy.reposition (p_var*n_v_dofs, p_var*n_u_dofs + n_p_dofs+n_x_dofs , n_p_dofs, n_y_dofs);
#if THREED
Kpw.reposition (p_var*n_u_dofs, w_var*n_u_dofs, n_p_dofs, n_w_dofs);
Kpz.reposition (p_var*n_u_dofs, p_var*n_u_dofs + n_p_dofs+2*n_x_dofs , n_p_dofs, n_z_dofs);
#endif
Kxu.reposition (p_var*n_u_dofs + n_p_dofs, u_var*n_u_dofs, n_x_dofs, n_u_dofs);
Kxv.reposition (p_var*n_u_dofs + n_p_dofs, v_var*n_u_dofs, n_x_dofs, n_v_dofs);
Kxp.reposition (p_var*n_u_dofs + n_p_dofs, p_var*n_u_dofs, n_x_dofs, n_p_dofs);
Kxx.reposition (p_var*n_u_dofs + n_p_dofs, p_var*n_u_dofs + n_p_dofs , n_x_dofs, n_x_dofs);
Kxy.reposition (p_var*n_u_dofs + n_p_dofs, p_var*n_u_dofs + n_p_dofs+n_x_dofs , n_x_dofs, n_y_dofs);
#if THREED
Kxw.reposition (p_var*n_u_dofs + n_p_dofs, w_var*n_u_dofs, n_x_dofs, n_w_dofs);
Kxz.reposition (p_var*n_u_dofs + n_p_dofs, p_var*n_u_dofs + n_p_dofs+2*n_x_dofs , n_x_dofs, n_z_dofs);
#endif
Kyu.reposition (p_var*n_u_dofs + n_p_dofs+n_x_dofs, u_var*n_u_dofs, n_y_dofs, n_u_dofs);
Kyv.reposition (p_var*n_u_dofs + n_p_dofs+n_x_dofs, v_var*n_u_dofs, n_y_dofs, n_v_dofs);
Kyp.reposition (p_var*n_u_dofs + n_p_dofs+n_x_dofs, p_var*n_u_dofs, n_y_dofs, n_p_dofs);
Kyx.reposition (p_var*n_u_dofs + n_p_dofs+n_x_dofs, p_var*n_u_dofs + n_p_dofs , n_y_dofs, n_x_dofs);
Kyy.reposition (p_var*n_u_dofs + n_p_dofs+n_x_dofs, p_var*n_u_dofs + n_p_dofs+n_x_dofs , n_y_dofs, n_y_dofs);
#if THREED
Kyw.reposition (p_var*n_u_dofs + n_p_dofs+n_x_dofs, w_var*n_u_dofs, n_x_dofs, n_w_dofs);
Kyz.reposition (p_var*n_u_dofs + n_p_dofs+n_x_dofs, p_var*n_u_dofs + n_p_dofs+2*n_x_dofs , n_x_dofs, n_z_dofs);
#endif
#if THREED
Kzu.reposition (p_var*n_u_dofs + n_p_dofs+2*n_x_dofs, u_var*n_u_dofs, n_y_dofs, n_u_dofs);
Kzv.reposition (p_var*n_u_dofs + n_p_dofs+2*n_x_dofs, v_var*n_u_dofs, n_y_dofs, n_v_dofs);
Kzp.reposition (p_var*n_u_dofs + n_p_dofs+2*n_x_dofs, p_var*n_u_dofs, n_y_dofs, n_p_dofs);
Kzx.reposition (p_var*n_u_dofs + n_p_dofs+2*n_x_dofs, p_var*n_u_dofs + n_p_dofs , n_y_dofs, n_x_dofs);
Kzy.reposition (p_var*n_u_dofs + n_p_dofs+2*n_x_dofs, p_var*n_u_dofs + n_p_dofs+n_x_dofs , n_y_dofs, n_y_dofs);
Kzw.reposition (p_var*n_u_dofs + n_p_dofs+2*n_x_dofs, w_var*n_u_dofs, n_x_dofs, n_w_dofs);
Kzz.reposition (p_var*n_u_dofs + n_p_dofs+2*n_x_dofs, p_var*n_u_dofs + n_p_dofs+2*n_x_dofs , n_x_dofs, n_z_dofs);
#endif
Fu.reposition (u_var*n_u_dofs, n_u_dofs);
Fv.reposition (v_var*n_u_dofs, n_v_dofs);
Fp.reposition (p_var*n_u_dofs, n_p_dofs);
Fx.reposition (p_var*n_u_dofs + n_p_dofs, n_x_dofs);
Fy.reposition (p_var*n_u_dofs + n_p_dofs+n_x_dofs, n_y_dofs);
#if THREED
Fw.reposition (w_var*n_u_dofs, n_w_dofs);
Fz.reposition (p_var*n_u_dofs + n_p_dofs+2*n_x_dofs, n_y_dofs);
#endif
std::vector<unsigned int> undefo_index;
PoroelasticConfig material(dphi,psi);
// Now we will build the element matrix.
for (unsigned int qp=0; qp<qrule.n_points(); qp++)
{
Number p_solid = 0.;
grad_u_mat(0) = grad_u_mat(1) = grad_u_mat(2) = 0;
for (unsigned int d = 0; d < dim; ++d) {
std::vector<Number> u_undefo;
std::vector<Number> u_undefo_ref;
//Fills the vector di with the global degree of freedom indices for the element. :dof_indicies
Last_non_linear_soln.get_dof_map().dof_indices(elem, undefo_index,d);
Last_non_linear_soln.current_local_solution->get(undefo_index, u_undefo);
reference.current_local_solution->get(undefo_index, u_undefo_ref);
for (unsigned int l = 0; l != n_u_dofs; l++){
grad_u_mat(d).add_scaled(dphi[l][qp], u_undefo[l]+u_undefo_ref[l]);
}
}
for (unsigned int l=0; l<n_p_dofs; l++)
{
p_solid += psi[l][qp]*Last_non_linear_soln.current_local_solution->el(dof_indices_p[l]);
}
Point rX;
material.init_for_qp(rX,grad_u_mat, p_solid, qp,0, p_solid,es);
Real J=material.J;
Real I_1=material.I_1;
Real I_2=material.I_2;
Real I_3=material.I_3;
RealTensor sigma=material.sigma;
av_pressure=av_pressure + p_solid*JxW[qp];
/*
std::cout<<"grad_u_mat(0)" << grad_u_mat(0) <<std::endl;
std::cout<<" J " << J <<std::endl;
std::cout<<" sigma " << sigma <<std::endl;
*/
Real sigma_sum_sq=pow(sigma(0,0)*sigma(0,0)+sigma(0,1)*sigma(0,1)+sigma(0,2)*sigma(0,2)+sigma(1,0)*sigma(1,0)+sigma(1,1)*sigma(1,1)+sigma(1,2)*sigma(1,2)+sigma(2,0)*sigma(2,0)+sigma(2,1)*sigma(2,1)+sigma(2,2)*sigma(2,2),0.5);
// std::cout<<" J " << J <<std::endl;
sum_jac_postvars=sum_jac_postvars+JxW[qp];
for (unsigned int i=0; i<n_u_dofs; i++){
Fu(i) += I_1*JxW[qp]*phi[i][qp];
Fv(i) += I_2*JxW[qp]*phi[i][qp];
Fw(i) += I_3*JxW[qp]*phi[i][qp];
Fx(i) += sigma_sum_sq*JxW[qp]*phi[i][qp];
Fy(i) += J*JxW[qp]*phi[i][qp];
Fz(i) += 0*JxW[qp]*phi[i][qp];
}
for (unsigned int i=0; i<n_p_dofs; i++){
Fp(i) += J*JxW[qp]*psi[i][qp];
}
} // end qp
system.rhs->add_vector(Fe, dof_indices);
system.matrix->add_matrix (Ke, dof_indices);
} // end of element loop
system.matrix->close();
system.rhs->close();
std::cout<<"Assemble postvars rhs->l2_norm () "<<system.rhs->l2_norm ()<<std::endl;
std::cout<<"sum_jac "<< sum_jac_postvars <<std::endl;
std::cout<<"av_pressure "<< av_pressure/sum_jac_postvars <<std::endl;
return;
}
void Filter::Kalman(Joint joint, double &dx, double &dy)
{
Kalmans[Kalman_count++] = joint;
Kalman_count = Kalman_count % Kalman_limit;
Kalman_num++;
if (Kalman_num > Kalman_limit)
{
Kalman_num = Kalman_limit;
}
if (Kalman_num < Kalman_limit)
{
dx = joint.Position.X;
dy = joint.Position.Y;
return;
}
else
{
//X, Y
int haha;
haha = 1;
double x[5], y[5];
int pos = Kalman_count;
for (int i = 0; i < 5; i++)
{
x[i] = Kalmans[pos].Position.X;
y[i] = Kalmans[pos].Position.Y;
pos++;
pos = pos%Kalman_limit;
}
//求系数Ax, Ay
double Ax[5] = {
/*a0*/ x[0],
/*a1*/ 4 * (x[1] - x[0]) - 3 * x[2] + 4 * x[3] / 3 - x[4] / 4,
/*a2*/ 11 * x[4] / 24 - 7 * x[3] / 3 + 19 * x[2] / 4 - 13 * (x[1] - x[0]) / 3,
/*a3*/ x[4] / 3 - 7 * x[3] / 6 + x[2] - (x[1] - x[0]) / 2,
/*a4*/ (x[4] - 4 * x[3] + 6 * x[2] + 4 * (x[1] - x[0])) / 24
};
double Ay[5] = {
/*a0*/ y[0],
/*a1*/ 4 * (y[1] - y[0]) - 3 * y[2] + 4 * y[3] / 3 - y[4] / 4,
/*a2*/ 11 * y[4] / 24 - 7 * y[3] / 3 + 19 * y[2] / 4 - 13 * (y[1] - y[0]) / 3,
/*a3*/ y[4] / 3 - 7 * y[3] / 6 + y[2] - (y[1] - y[0]) / 2,
/*a4*/ (y[4] - 4 * y[3] + 6 * y[2] + 4 * (y[1] - y[0])) / 24
};
//求转换矩阵Fx, Fy
Matrix Fx(4, 4,
new double[16]{
1, 1, -0.5, (Ax[1] + 6 * Ax[3] - 4 * Ax[4]) / (24 * Ax[4]),
0, 1, 1, 0.5,
0, 0, 1, 1,
0, 0, 0, 1});
Matrix Fy(4, 4,
new double[16]{
1, 1, -0.5, (Ay[1] + 6 * Ay[3] - 4 * Ay[4]) / (24 * Ay[4]),
0, 1, 1, 0.5,
0, 0, 1, 1,
0, 0, 0, 1});
//求ε(t|t-1)
Matrix ex(4, 4), ey(4, 4);
ex = Fx*Kalman_ex*(!Fx);
ey = Fy*Kalman_ey*(!Fy);
//cout << "ex" << endl; ex.print();
//cout << "ey" << endl; ey.print();
Matrix Bx(4, 1), By(4, 1);
//cout << "!Kalman_C" << endl; (!Kalman_C).print();
//cout << "Kalman_vx" << endl; Kalman_vx.print();
//cout << "Kalman_C" << endl; Kalman_C.print();
//cout << "!Kalman_C" << endl; (!Kalman_C).print();
//cout << "Kalman_C*ex" << endl; (Kalman_C*ex).print();
//cout << "Kalman_C*ex*(!Kalman_C)" << endl; (Kalman_C*ex*(!Kalman_C)).print();
//cout << "(~(Kalman_vx + Kalman_C*ex*(!Kalman_C)))" << endl;
//(~(Kalman_vx + Kalman_C*ex*(!Kalman_C))).print();
Bx = ex*(!Kalman_C)*(~(Kalman_vx + Kalman_C*ex*(!Kalman_C)));
//cout << "Bx" << endl; Bx.print();
By = ey*(!Kalman_C)*(~(Kalman_vy + Kalman_C*ey*(!Kalman_C)));
Matrix I4(4, 4);
I4.SetIdentity();
Kalman_Sx = (I4 - Bx*Kalman_C)*(Fx*Kalman_Sx + Kalman_Gx) +
Bx*Matrix(1, 1, new double[1] {joint.Position.X});
//cout << "Kalman_Sx" << endl; Kalman_Sx.print();
Kalman_Sy = (I4 - By*Kalman_C)*(Fy*Kalman_Sy + Kalman_Gy) +
By*Matrix(1, 1, new double[1] {joint.Position.Y});
Kalman_ex = ex - Bx*Kalman_C*ex;
Kalman_ey = ey - By*Kalman_C*ey;
dx = Kalman_Sx.at(0, 0);
dy = Kalman_Sy.at(0, 0);
}
}
//---------------------------------------------------------
void NDG2D::BuildPeriodicMaps2D(double xperiod, double yperiod)
//---------------------------------------------------------
{
// function [] = BuildPeriodicMaps2D(xperiod, yperiod);
// Purpose: Connectivity and boundary tables for with all
// maps returned in Globals2D assuming periodicity
// Find node to node connectivity
vmapM.resize(Nfp*Nfaces*K); vmapP.resize(Nfp*Nfaces*K);
DVec FxL,FyL,FxR,FyR; DMat x1,x2,y1,y2,D,xF1,yF1,xF2,yF2;
IMat idLR; IVec idL,idR,vidL,vidR,fidL,fidR;
int k1=0,f1=0, k2=0,f2=0; DVec onesNfp=ones(Nfp);
double dx=0.0, dy=0.0, cx1=0.0,cx2=0.0,cy1=0.0,cy2=0.0;
double dNfp=(double)Nfp;
for (k1=1; k1<=K; ++k1) {
for (f1=1; f1<=Nfaces; ++f1) {
k2 = EToE(k1,f1); f2 = EToF(k1,f1);
vidL = Fmask(All,f1); vidL += (k1-1)*Np;
vidR = Fmask(All,f2); vidR += (k2-1)*Np;
fidL = Range(1,Nfp) + (f1-1)*Nfp + (k1-1)*Nfp*Nfaces;
fidR = Range(1,Nfp) + (f2-1)*Nfp + (k2-1)*Nfp*Nfaces;
vmapM(fidL) = vidL; vmapP(fidL) = vidL;
FxL=Fx(fidL); FyL=Fy(fidL); FxR=Fx(fidR); FyR=Fy(fidR);
x1 = outer(FxL, onesNfp); y1 = outer(FyL, onesNfp);
x2 = outer(FxR, onesNfp); y2 = outer(FyR, onesNfp);
// Compute distance matrix
D = sqr(x1-trans(x2)) + sqr(y1-trans(y2));
idLR = find2D(abs(D), '<', NODETOL);
idL=idLR(All,1); idR=idLR(All,2);
vmapP(fidL(idL)) = vidR(idR);
}
}
for (k1=1; k1<=K; ++k1) {
for (f1=1; f1<=Nfaces; ++f1) {
//###################################################
xF1=x(Fmask(All,f1), k1); cx1=xF1.sum()/dNfp;
yF1=y(Fmask(All,f1), k1); cy1=yF1.sum()/dNfp;
//###################################################
k2 = EToE(k1,f1); f2 = EToF(k1,f1);
if (k2==k1) {
for (k2=1; k2<=K; ++k2) {
if (k1!=k2) {
for (f2=1; f2<=Nfaces; ++f2) {
if (EToE(k2,f2)==k2) {
//#########################################
xF2=x(Fmask(All,f2), k2); cx2=xF2.sum()/dNfp;
yF2=y(Fmask(All,f2), k2); cy2=yF2.sum()/dNfp;
//#########################################
dx = sqrt( SQ(abs(cx1-cx2)-xperiod) + SQ(cy1-cy2));
dy = sqrt( SQ(cx1-cx2) + SQ(abs(cy1-cy2)-yperiod));
if (dx<NODETOL || dy<NODETOL) {
EToE(k1,f1) = k2; EToE(k2,f2) = k1;
EToF(k1,f1) = f2; EToF(k2,f2) = f1;
vidL = Fmask(All,f1); vidL += (k1-1)*Np;
vidR = Fmask(All,f2); vidR += (k2-1)*Np;
fidL = Range(1,Nfp) + (f1-1)*Nfp + (k1-1)*Nfp*Nfaces;
fidR = Range(1,Nfp) + (f2-1)*Nfp + (k2-1)*Nfp*Nfaces;
FxL=Fx(fidL); FyL=Fy(fidL); FxR=Fx(fidR); FyR=Fy(fidR);
x1 = outer(FxL, onesNfp); y1 = outer(FyL, onesNfp);
x2 = outer(FxR, onesNfp); y2 = outer(FyR, onesNfp);
// Compute distance matrix
if (dx<NODETOL) {
D = sqr(abs(x1-trans(x2))-xperiod) + sqr(y1-trans(y2));
} else {
D = sqr(x1-trans(x2)) + sqr(abs(y1-trans(y2))-yperiod);
}
idLR = find2D(abs(D), '<', NODETOL);
idL=idLR(All,1); idR=idLR(All,2);
//assert(idL.size() == Nfp);
if (idL.size() != Nfp) {
umERROR("NDG2D::BuildPeriodicMaps2D", "Nfp != idL.size() = %d", idL.size());
}
vmapP(fidL(idL)) = vidR(idR);
vmapP(fidR(idR)) = vidL(idL);
}
}
}
}
}
}
}
}
// Create default list of boundary nodes
mapB = find(vmapP, '=', vmapM); vmapB = vmapM(mapB);
}
//---------------------------------------------------------
void NDG2D::OutputNodes(bool bFaceNodes)
//---------------------------------------------------------
{
static int count = 0;
string output_dir = ".";
string buf = umOFORM("%s/mesh_N%02d_%04d.vtk",
output_dir.c_str(), this->Nfp, ++count);
FILE *fp = fopen(buf.c_str(), "w");
if (!fp) {
umLOG(1, "Could no open %s for output!\n", buf.c_str());
return;
}
// Set flags and totals
int Npoints = this->Np; // volume nodes per element
if (bFaceNodes)
Npoints = Nfp*Nfaces; // face nodes per element
// set totals for Vtk output
#if (1)
// FIXME: no connectivity
int vtkTotalPoints = this->K * Npoints;
int vtkTotalCells = vtkTotalPoints;
int vtkTotalConns = vtkTotalPoints;
int Ncells = Npoints;
#else
int vtkTotalPoints = this->K * Npoints;
int vtkTotalCells = this->K * Ncells;
int vtkTotalConns = (this->EToV.num_cols()+1) * this->K * Ncells;
int Ncells = this->N * this->N;
#endif
//-------------------------------------
// 1. Write the VTK header details
//-------------------------------------
fprintf(fp, "# vtk DataFile Version 2");
fprintf(fp, "\nNDGFem simulation nodes");
fprintf(fp, "\nASCII");
fprintf(fp, "\nDATASET UNSTRUCTURED_GRID\n");
fprintf(fp, "\nPOINTS %d double", vtkTotalPoints);
int newNpts=0;
//-------------------------------------
// 2. Write the vertex data
//-------------------------------------
if (bFaceNodes) {
for (int k=1; k<=this->K; ++k) {
for (int n=1; n<=Npoints; ++n) {
fprintf(fp, "\n%20.12e %20.12e %6.1lf", Fx(n,k), Fy(n,k), 0.0);
}
}
} else {
for (int k=1; k<=this->K; ++k) {
for (int n=1; n<=Npoints; ++n) {
fprintf(fp, "\n%20.12e %20.12e %6.1lf", x(n,k), y(n,k), 0.0);
}
}
}
//-------------------------------------
// 3. Write the element connectivity
//-------------------------------------
// Number of indices required to define connectivity
fprintf(fp, "\n\nCELLS %d %d", vtkTotalCells, 2*vtkTotalConns);
// TODO: write element connectivity to file
// FIXME: out-putting as VTK_VERTEX
int nodesk=0;
for (int k=0; k<this->K; ++k) { // for each element
for (int n=1; n<=Npoints; ++n) {
fprintf(fp, "\n%d", 1); // for each triangle
for (int i=1; i<=1; ++i) { // FIXME: no connectivity
fprintf(fp, " %5d", nodesk); // nodes in nth triangle
nodesk++; // indexed from 0
}
}
}
//-------------------------------------
// 4. Write the cell types
//-------------------------------------
// For each element (cell) write a single integer
// identifying the cell type. The integer should
// correspond to the enumeration in the vtk file:
// /VTK/Filtering/vtkCellType.h
fprintf(fp, "\n\nCELL_TYPES %d", vtkTotalCells);
for (int k=0; k<this->K; ++k) {
fprintf(fp, "\n");
for (int i=1; i<=Ncells; ++i) {
//fprintf(fp, "5 "); // 5:VTK_TRIANGLE
fprintf(fp, "1 "); // 1:VTK_VERTEX
if (! (i%10))
fprintf(fp, "\n");
}
}
// add final newline to output
fprintf(fp, "\n");
fclose(fp);
}
int main(int argc, char *argv[])
{
#ifdef __linux
feenableexcept(2);
feenableexcept(3);
#endif
FILE * matrici_iniziali=fopen("Matrici_Iniziali.txt","w");
FILE * posizionePart=fopen("Posizione_Particelle.txt","w");
FILE * ellissi=fopen("Parametri_Ellissi_Funz_Ottiche.txt","w");
FILE * funzioni_ottiche=fopen("Funzioni_Ottiche.txt","w");
FILE * confronti=fopen("Math_rilevati.txt","w");
#ifdef TEST_OPTICAL_FUNCTIONS
FILE * funzioni_ottiche_t=fopen("Funzioni_Ottiche_T.txt","w");
FILE * ellissi_t=fopen("Parametri_Ellissi_Funz_Ottiche_T.txt","w");
FILE * confronti_t=fopen("Math_rilevati_T.txt","w");
#endif
#ifdef DEBUG
FILE * outputDEBUG=fopen("DEBUG.txt","w");
#endif
bool fallita_lettura_parametri = true;
bool fallita_lettura_inputdistr = true;
bool do_transport = false;
bool do_optics = false;
bool posso_fare_funzioni_ottiche = false;
ifstream parametri;
ifstream inputdistr;
int nstep = 1;
double gnuplot_ymax_opt=0.;
bool calcola_ymax_opt = true;
#ifdef TEST_OPTICAL_FUNCTIONS
double gnuplot_ymax_opt_T=0.;
bool calcola_ymax_opt_T = true;
#endif
double gnuplot_ymax_pos=0.;
bool calcola_ymax_pos = true;
double gnuplot_xmax_opt=0.;
double gnuplot_xmax_pos=0.;
bool calcola_ymax_ell = true;
double gnuplot_ymax_ell=0.;
double percentuale=0.03;
int conto_per_confronto=0;
double *compare_x=new double[2];
double *compare_y=new double[2];
bool confronto_pos_x=false;
bool confronto_pos_y=false;
double *paramIniz_X=new double[2];
double *paramIniz_Y=new double[2];
#ifdef TEST_OPTICAL_FUNCTIONS
int conto_per_confronto_t_x=0;
int conto_per_confronto_t_y=0;
bool confronto_pos_t_y=false;
bool confronto_pos_t_x=false;
bool fai_da_te_x=false;
bool fai_da_te_y=false;
#endif
for (int i = 1; i < argc; i++)
{
if (string(argv[i]) == "-p")
{
parametri.open(argv[i+1]);
fallita_lettura_parametri=parametri.fail();
i++;
}
else if (string(argv[i]) == "-i")
{
inputdistr.open(argv[i+1]);
fallita_lettura_inputdistr=inputdistr.fail();
i++;
}
else if (string(argv[i]) == "-transport")
{
do_transport=true;
}
else if (string(argv[i]) == "-xmax_opt")
{
gnuplot_xmax_opt=atof(argv[i+1]);
i++;
}
else if (string(argv[i]) == "-xmax_pos")
{
gnuplot_xmax_pos=atof(argv[i+1]);
i++;
}
else if (string(argv[i]) == "-ymax_opt")
{
gnuplot_ymax_opt=atof(argv[i+1]);
calcola_ymax_opt = false;
i++;
}
#ifdef TEST_OPTICAL_FUNCTIONS
else if (string(argv[i]) == "-ymax_opt_T")
{
gnuplot_ymax_opt_T=atof(argv[i+1]);
calcola_ymax_opt_T = false;
i++;
}
#endif
else if (string(argv[i]) == "-ymax_pos")
{
gnuplot_ymax_pos=atof(argv[i+1]);
calcola_ymax_pos = false;
i++;
}
else if (string(argv[i]) == "-ymax_ell")
{
gnuplot_ymax_ell=atof(argv[i+1]);
calcola_ymax_ell = false;
i++;
}
else if (string(argv[i]) == "-compare_X")
{
compare_x[0]=atof(argv[i+1]);
compare_x[1]=atof(argv[i+2]);
i+=2;
}
else if (string(argv[i]) == "-compare_Y")
{
compare_y[0]=atof(argv[i+1]);
compare_y[1]=atof(argv[i+2]);
i+=2;
}
else if (string(argv[i]) == "-perc")
{
percentuale=atof(argv[i+1]);
fprintf(matrici_iniziali,"\n%f\n",percentuale);
i++;
}
else if (string(argv[i]) == "-optics")
{
do_optics=true;
}
else if (string(argv[i]) == "-nstep")
{
nstep = atoi(argv[i+1]);
i++;
}
#ifdef TEST_OPTICAL_FUNCTIONS
else if (string(argv[i]) == "-paramIniz_X")
{
paramIniz_X[0] = atoi(argv[i+1]);
paramIniz_X[1] = atoi(argv[i+2]);
fai_da_te_x=true;
i+=2;
}
else if (string(argv[i]) == "-paramIniz_Y")
{
paramIniz_Y[0] = atoi(argv[i+1]);
paramIniz_Y[1] = atoi(argv[i+2]);
fai_da_te_y=true;
i+=2;
}
#endif
else
{
printf("Impossibile riconoscere il parametro %s\n",argv[i]);
}
}
string utile_per_contare;
int conta_righe_parametri = 0;
if (fallita_lettura_parametri || fallita_lettura_inputdistr)
{
printf("Impossibile aprire (o non definito) il file contenente i parametri\no il file contenente la distribuzione/particella iniziale\n");
exit(204);
}
double * dati_iniziali = new double[6]; // emittanza, energia, x, y, px, py
for (int i = 0; i < 6; i++)
{
if(inputdistr.eof())
{
printf("Mancano dei dati iniziali!\n");
exit(123);
}
inputdistr >> dati_iniziali[i];
}
inputdistr.clear();
inputdistr.seekg(0,std::ios::beg);
double emittanza = dati_iniziali[0];
double energia = dati_iniziali[1];
double *vett_i=new double[4];
vett_i[0]=dati_iniziali[2];
vett_i[1]=dati_iniziali[4];
vett_i[2]=dati_iniziali[3];
vett_i[3]=dati_iniziali[5];
do
{
parametri >> utile_per_contare;
if(parametri.eof()) break;
parametri.ignore(1000, '\n');
conta_righe_parametri++;
}
while(!parametri.eof());
parametri.clear();
parametri.seekg(0,std::ios::beg);
// qui di leggono tutti i dati
// char *elemento=new char[conta_righe_parametri];
string *elemento=new string[conta_righe_parametri];
double * lunghezza= new double[conta_righe_parametri];
double * gradiente= new double[conta_righe_parametri];
int contatore=0;
for (int i = 0; i < conta_righe_parametri; i++)
{
parametri >> elemento[i];
parametri >> gradiente[i];
parametri >> lunghezza[i];
#ifdef DEBUG
cout << "Tipo elemento: " << elemento[i] << ", gradiente: " << gradiente[i] << ", lunghezza: " << lunghezza[i] << endl;
#endif
contatore++;
}
#ifdef DEBUG
printf("contatore: %d",contatore);
#endif
vector <vector <double> > I(4,vector<double>(4,0.0));
vector <vector <double> > K(4,vector<double>(4,0.0));
vector <vector <double> > F(4,vector<double>(4,0.0));
vector <vector <vector <double> > > Fx(contatore,vector <vector <double> > (4, vector <double> (4,0.0)));
vector <vector <vector <double> > > Dx(contatore,vector <vector <double> > (4, vector <double> (4,0.0)));
vector <vector <vector <double> > > OI(contatore,vector <vector <double> > (4, vector <double> (4,0.0)));
vector <vector <vector <double> > > FxI(contatore,vector <vector <double> > (4, vector <double> (4,0.0)));
vector <vector <vector <double> > > DxI(contatore,vector <vector <double> > (4, vector <double> (4,0.0)));
vector <vector <vector <double> > > O(contatore,vector <vector <double> > (4, vector <double> (4,0.0)));
double *alpha = new double[2];
double *beta = new double[2];
double *aminmax = new double[2];
double *bminmax = new double[2];
bool alpha_calcolato_con_successo=false;
bool beta_calcolato_con_successo=false;
#ifdef TEST_OPTICAL_FUNCTIONS
double *ottiche_x_t = new double[2];
double *ottiche_y_t = new double[2];
double *aminmax_x_t = new double[2];
double *bminmax_y_t = new double[2];
for (int i = 0; i < 2; i++) ottiche_x_t[i] = ottiche_y_t[i] = aminmax_x_t[i] = bminmax_y_t[i] = 0.;
#endif
double gamma_beta=sqrt(2.0*energia/MP_MEV);
double gamma_v=gamma_beta*SPEED_OF_LIGHT;
double *f1 =new double [contatore];
double *d1 =new double [contatore];
for (int i=0;i<contatore;i++)
{
if (elemento[i]=="F")
f1[i]=sqrt(gradiente[i]*CHARGE/(MP_KG*gamma_v));
if (elemento[i]=="D")
d1[i]=sqrt(gradiente[i]*CHARGE/(MP_KG*gamma_v));
}
#ifdef DEBUG
for (int i=0;i<contatore;i++)
{
fprintf(outputDEBUG,"\ngrad. foc. %+20.10f ", f1[i]*f1[i]);
fprintf(outputDEBUG,"\ngrad. defoc. %+20.10f ", d1[i]*d1[i]);
}
#endif
for (int i=0;i<contatore;i++)
{
#ifdef DEBUG
if (elemento[i]=="F")
Fx[i]=focusing(Fx[i],f1[i],lunghezza[i],matrici_iniziali,i);
else if (elemento[i]=="D")
Dx[i]=defocusing(Dx[i],d1[i],lunghezza[i],matrici_iniziali,i);
else if (elemento[i]=="O")
O[i]=drift(O[i],lunghezza[i],matrici_iniziali,i);
else
fprintf(outputDEBUG,"Elemento[%d] non riconosciuto\n", i);
#else
if (elemento[i]=="F")
Fx[i]=focusing(Fx[i],f1[i],lunghezza[i]);
else if (elemento[i]=="D")
Dx[i]=defocusing(Dx[i],d1[i],lunghezza[i]);
else if (elemento[i]=="O")
O[i]=drift(O[i],lunghezza[i]);
#endif
}
#ifdef DEBUG
for (int i=0;i<contatore;i++)
{
if (elemento[i]=="O")
{
// if (O[i][0][0] == 0.0) continue;
fprintf(matrici_iniziali,"\nMATRICE DRIFT");
scrivimatr2D(O[i],matrici_iniziali);
fprintf(matrici_iniziali,"\n");
}
else if (elemento[i]=="F")
{
// if (Fx[i][0][0] == 0.0) continue;
fprintf(matrici_iniziali,"\nMATRICE FOC.");
scrivimatr2D(Fx[i],matrici_iniziali);
fprintf(matrici_iniziali,"\n");
}
else if (elemento[i]=="D")
{
// if (Dx[i][0][0] == 0.0) continue;
fprintf(matrici_iniziali,"\nMATRICE DEFOC.");
scrivimatr2D(Dx[i],matrici_iniziali);
fprintf(matrici_iniziali,"\n");
}
}
#endif
/************************************************************************/
if (do_optics)
{
vector <vector <double> > compos(4,vector<double>(4,0));
if (elemento[0]=="O")
{
for (int k=0; k<4; k++)
for (int j=0; j<4; j++)
compos[k][j]=O[0][k][j];
}
else if (elemento[0]=="F")
{
for (int k=0; k<4; k++)
for (int j=0; j<4; j++)
compos[k][j]=Fx[0][k][j];
}
else if (elemento[0]=="D")
{
for (int k=0; k<4; k++)
for (int j=0; j<4; j++)
compos[k][j]=Dx[0][k][j];
}
for (int i=1;i<contatore;i++)
{
if (elemento[i]=="O")
compos=prodo(O[i],compos,4);
else if (elemento[i]=="F")
compos=prodo(Fx[i],compos,4);
else if (elemento[i]=="D")
compos=prodo(Dx[i],compos,4);
}
for (int i=0;i<4;i++)
for(int a=0;a<4;a++)
F[i][a]=compos[i][a];
// Calcolo Funzioni OTTICHE
for (int i = 0; i < 2; i++) alpha[i] = beta[i] = aminmax[i] = bminmax[i] = 0.;
if ( (fabs((F[FOC][FOC]+F[FOC+1][FOC+1])*0.5) <= 1.) && (fabs((F[DEFOC][DEFOC]+F[DEFOC+1][DEFOC+1])*0.5) <= 1.))
posso_fare_funzioni_ottiche = true;
else cout << "Impossibile calcolare le funzioni ottiche!" << endl;
//cout << "posso_fare_funzioni_ottiche="<<posso_fare_funzioni_ottiche<<endl;
//if ((((F[FOC][FOC]+F[FOC+1][FOC+1])*(F[FOC][FOC]+F[FOC+1][FOC+1])-4)<=0) && (((F[DEFOC][DEFOC]+F[DEFOC+1][DEFOC+1])*(F[DEFOC][DEFOC]+F[DEFOC+1][DEFOC+1])-4)<=0) )
// posso_fare_funzioni_ottiche = true;
//else cout << "Impossibile calcolare le funzioni ottiche!" << endl;
if (posso_fare_funzioni_ottiche)
{
alpha=optics(F,FOC,&alpha_calcolato_con_successo);
beta=optics(F,DEFOC,&beta_calcolato_con_successo);
aminmax = assi_ellissi(alpha, emittanza);
bminmax = assi_ellissi(beta, emittanza);
if (calcola_ymax_opt) massimo_opt(alpha,beta,&gnuplot_ymax_opt);
if (calcola_ymax_pos) massimo_pos(vett_i,&gnuplot_ymax_pos);
if (calcola_ymax_ell) massimo_opt(aminmax,bminmax,&gnuplot_ymax_ell);
fprintf(funzioni_ottiche,"# alpha_successo %d beta_successo %d\n",(int)(alpha_calcolato_con_successo),(int)(beta_calcolato_con_successo));
if (alpha_calcolato_con_successo&&beta_calcolato_con_successo)
{
fprintf(funzioni_ottiche,"\n#%7c",'S');
fprintf(funzioni_ottiche,"%10.8s","Alpha x");
fprintf(funzioni_ottiche,"%10.7s","Beta x");
fprintf(funzioni_ottiche,"%12.8s","Alpha y");
fprintf(funzioni_ottiche,"%10.7s","Beta y");
fprintf(ellissi,"%10s","x");
fprintf(ellissi,"%11s","p_x");
fprintf(ellissi,"%11s","y");
fprintf(ellissi,"%11s","p_y");
}
scrividati(0.0,alpha,beta,funzioni_ottiche);
scrividati_ellissi(0.0,aminmax,bminmax,ellissi);
#ifdef TEST_OPTICAL_FUNCTIONS
for (int i=0;i<2;i++)
{
if (fai_da_te_x&&fai_da_te_y)
{
ottiche_x_t[i]=paramIniz_X[i];
ottiche_y_t[i]=paramIniz_Y[i];
}
else if (fai_da_te_x)
{
ottiche_x_t[i]=paramIniz_X[i];
ottiche_y_t[i]=paramIniz_X[i];
}
else if (fai_da_te_y)
{
ottiche_x_t[i]=paramIniz_Y[i];
ottiche_y_t[i]=paramIniz_Y[i];
}
else
{
ottiche_x_t[i]=alpha[i];
ottiche_y_t[i]=beta[i];
}
}
aminmax_x_t = assi_ellissi(ottiche_x_t, emittanza);
bminmax_y_t = assi_ellissi(ottiche_y_t, emittanza);
scrividati(0.0,ottiche_x_t,ottiche_y_t,funzioni_ottiche_t);
scrividati_ellissi(0.0,aminmax_x_t,bminmax_y_t,ellissi_t);
if (calcola_ymax_opt_T) massimo_opt(ottiche_x_t,ottiche_y_t,&gnuplot_ymax_opt_T);
#endif
}
}
if(do_transport)
{
fprintf(posizionePart," %+10.5f",0.0);
fprintf(posizionePart," %+10.5f",vett_i[0]);
fprintf(posizionePart," %+10.5f",vett_i[1]);
fprintf(posizionePart," %+10.5f",vett_i[2]);
fprintf(posizionePart," %+10.5f\n",vett_i[3]);
}
#ifdef DEBUG
fprintf(outputDEBUG, "\nFODO:");
scrivimatr2D(F,outputDEBUG);
#endif
/************************************************************************/
//ora primi dell'iterazione mi calcolo le micromappe Li di lunghezza S=L/n
double lunghezzatotale=0.;
for (int i = 0 ;i < contatore;i++)
lunghezzatotale+=lunghezza[i];
#ifdef DEBUG
for (int i=0; i < contatore; i++)
{
fprintf(outputDEBUG,"\n#step in elemento %d = %d",i, dsMap(lunghezza[i],lunghezzatotale,nstep));
}
fprintf(outputDEBUG,"\n");
#endif
double S = 0.;
//Calcolo MICROMAPPE per il Drift
for (int i=0; i < contatore; i++)
{
S=lunghezza[i]/dsMap(lunghezza[i],lunghezzatotale,nstep);
#ifdef DEBUG
if (elemento[i] == "O")
{
O[i]=drift(O[i],S,matrici_iniziali,i);
OI[i]=drift(OI[i],-S,matrici_iniziali,i);
}
else if (elemento[i] == "F")
{
Fx[i]=focusing(Fx[i],f1[i],S,matrici_iniziali,i);
FxI[i]=focusing(FxI[i],f1[i],-S,matrici_iniziali,i);
}
else if (elemento[i] == "D")
{
Dx[i]=defocusing(Dx[i],d1[i],S,matrici_iniziali,i);
DxI[i]=defocusing(DxI[i],d1[i],-S,matrici_iniziali,i);
}
#else
if (elemento[i] == "O")
{
O[i]=drift(O[i],S);
OI[i]=drift(OI[i],-S);
}
else if (elemento[i] == "F")
{
Fx[i]=focusing(Fx[i],f1[i],S);
FxI[i]=focusing(FxI[i],f1[i],-S);
}
else if (elemento[i] == "D")
{
Dx[i]=defocusing(Dx[i],d1[i],S);
DxI[i]=defocusing(DxI[i],d1[i],-S);
}
#endif
}
/***********************************************************************/
double dl=0.;
double lunghezza_accumulata=0.0;
for (int i=0;i<contatore;i++)
{
dl=lunghezza[i]/dsMap(lunghezza[i],lunghezzatotale,nstep);
if (elemento[i]=="O")
{
fprintf(matrici_iniziali,"\n#Drift #%d, dl = %f",i,dl);
fprintf(funzioni_ottiche,"\n#Drift #%d, dl = %f",i,dl);
while(S<=(lunghezza_accumulata+lunghezza[i]))
{
fprintf(matrici_iniziali,"\n\n Num_Step %f", S);
scrivimatr2D(F,matrici_iniziali);
if (do_transport)
{
#ifdef DEBUG
prod(vett_i,O[i],S);
#else
prod(vett_i,O[i]);
#endif
if (calcola_ymax_pos) massimo_pos(vett_i,&gnuplot_ymax_pos);
scrivi_pos_part(posizionePart,vett_i,S);
}
if (do_optics)
{
F=simil(F,OI[i],O[i]);
if (posso_fare_funzioni_ottiche)
{
alpha=optics(F,FOC,&alpha_calcolato_con_successo);
beta=optics(F,DEFOC,&beta_calcolato_con_successo);
aminmax = assi_ellissi(alpha, emittanza);
bminmax = assi_ellissi(beta, emittanza);
confronto(compare_x,aminmax,S,percentuale,confronti,&confronto_pos_x,&conto_per_confronto);
confronto(compare_y,bminmax,S,percentuale,confronti,&confronto_pos_y,&conto_per_confronto);
scrividati(S,alpha,beta,funzioni_ottiche);
scrividati_ellissi(S,aminmax,bminmax,ellissi);
if (calcola_ymax_ell) massimo_opt(aminmax,bminmax,&gnuplot_ymax_ell);
if (calcola_ymax_opt) massimo_opt(alpha,beta,&gnuplot_ymax_opt);
#ifdef TEST_OPTICAL_FUNCTIONS
ottiche_x_t=optics_T(ottiche_x_t,FOC,O[i]);
ottiche_y_t=optics_T(ottiche_y_t,DEFOC,O[i]);
aminmax_x_t = assi_ellissi(ottiche_x_t, emittanza);
bminmax_y_t = assi_ellissi(ottiche_y_t, emittanza);
confronto(paramIniz_X,aminmax_x_t,S,percentuale,confronti_t,&confronto_pos_t_x,&conto_per_confronto_t_x);
confronto(paramIniz_Y,bminmax_y_t,S,percentuale,confronti_t,&confronto_pos_t_y,&conto_per_confronto_t_y);
scrividati(S,ottiche_x_t,ottiche_y_t,funzioni_ottiche_t);
scrividati_ellissi(S,aminmax_x_t,bminmax_y_t,ellissi_t);
if (calcola_ymax_opt_T) massimo_opt(ottiche_x_t,ottiche_y_t,&gnuplot_ymax_opt_T);
#endif
}
}
S+=dl;
}
lunghezza_accumulata+=lunghezza[i];
}
else if (elemento[i]=="F")
{
fprintf(matrici_iniziali,"\n#Foc. #%d, dl = %f",i,dl);
fprintf(funzioni_ottiche,"\n#Foc. #%d, dl = %f",i,dl);
while(S<=(lunghezza_accumulata+lunghezza[i]))
{
fprintf(matrici_iniziali,"\n\n Num_Step %f", S);
scrivimatr2D(F,matrici_iniziali);
if (do_transport)
{
#ifdef DEBUG
prod(vett_i,Fx[i],S);
#else
prod(vett_i,Fx[i]);
#endif
if (calcola_ymax_pos) massimo_pos(vett_i,&gnuplot_ymax_pos);
scrivi_pos_part(posizionePart,vett_i,S);
}
if (do_optics)
{
if (posso_fare_funzioni_ottiche)
{
F=simil(F,FxI[i],Fx[i]);
alpha=optics(F,FOC,&alpha_calcolato_con_successo);
beta=optics(F,DEFOC,&beta_calcolato_con_successo);
aminmax = assi_ellissi(alpha, emittanza);
bminmax = assi_ellissi(beta, emittanza);
confronto(compare_x,aminmax,S,percentuale,confronti,&confronto_pos_x,&conto_per_confronto);
confronto(compare_y,bminmax,S,percentuale,confronti,&confronto_pos_y,&conto_per_confronto);
scrividati(S,alpha,beta,funzioni_ottiche);
scrividati_ellissi(S,aminmax,bminmax,ellissi);
if (calcola_ymax_ell) massimo_opt(aminmax,bminmax,&gnuplot_ymax_ell);
if (calcola_ymax_opt) massimo_opt(alpha,beta,&gnuplot_ymax_opt);
#ifdef TEST_OPTICAL_FUNCTIONS
ottiche_x_t=optics_T(ottiche_x_t,FOC,Fx[i]);
ottiche_y_t=optics_T(ottiche_y_t,DEFOC,Fx[i]);
aminmax_x_t = assi_ellissi(ottiche_x_t, emittanza);
bminmax_y_t = assi_ellissi(ottiche_y_t, emittanza);
confronto(paramIniz_X,aminmax_x_t,S,percentuale,confronti_t,&confronto_pos_t_x,&conto_per_confronto_t_x);
confronto(paramIniz_Y,bminmax_y_t,S,percentuale,confronti_t,&confronto_pos_t_y,&conto_per_confronto_t_y);
scrividati(S,ottiche_x_t,ottiche_y_t,funzioni_ottiche_t);
scrividati_ellissi(S,aminmax_x_t,bminmax_y_t,ellissi_t);
if (calcola_ymax_opt_T) massimo_opt(ottiche_x_t,ottiche_y_t,&gnuplot_ymax_opt_T);
#endif
}
}
S+=dl;
}
lunghezza_accumulata+=lunghezza[i];
}
else if (elemento[i]=="D")
{
fprintf(matrici_iniziali,"\n#Defoc. #%d, dl = %f",i,dl);
fprintf(funzioni_ottiche,"\n#Defoc. #%d, dl = %f",i,dl);
while (S<=(lunghezza_accumulata+lunghezza[i]))
{
fprintf(matrici_iniziali,"\n\n Num_Step %f", S);
scrivimatr2D(F,matrici_iniziali);
if (do_transport)
{
#ifdef DEBUG
prod(vett_i,Dx[i],S);
#else
prod(vett_i,Dx[i]);
#endif
if (calcola_ymax_pos) massimo_pos(vett_i,&gnuplot_ymax_pos);
scrivi_pos_part(posizionePart,vett_i,S);
}
if (do_optics)
{
if (posso_fare_funzioni_ottiche)
{
F=simil(F,DxI[i],Dx[i]);
alpha=optics(F,FOC,&alpha_calcolato_con_successo);
beta=optics(F,DEFOC,&beta_calcolato_con_successo);
aminmax = assi_ellissi(alpha, emittanza);
bminmax = assi_ellissi(beta, emittanza);
confronto(compare_x,aminmax,S,percentuale,confronti,&confronto_pos_x,&conto_per_confronto);
confronto(compare_y,bminmax,S,percentuale,confronti,&confronto_pos_y,&conto_per_confronto);
scrividati(S,alpha,beta,funzioni_ottiche);
scrividati_ellissi(S,aminmax,bminmax,ellissi);
if (calcola_ymax_ell) massimo_opt(aminmax,bminmax,&gnuplot_ymax_ell);
if (calcola_ymax_opt) massimo_opt(alpha,beta,&gnuplot_ymax_opt);
#ifdef TEST_OPTICAL_FUNCTIONS
ottiche_x_t=optics_T(ottiche_x_t,FOC,Dx[i]);
ottiche_y_t=optics_T(ottiche_y_t,DEFOC,Dx[i]);
aminmax_x_t = assi_ellissi(ottiche_x_t, emittanza);
bminmax_y_t = assi_ellissi(ottiche_y_t, emittanza);
confronto(paramIniz_X,aminmax_x_t,S,percentuale,confronti_t,&confronto_pos_t_x,&conto_per_confronto_t_x);
confronto(paramIniz_Y,bminmax_y_t,S,percentuale,confronti_t,&confronto_pos_t_y,&conto_per_confronto_t_y);
scrividati(S,ottiche_x_t,ottiche_y_t,funzioni_ottiche_t);
scrividati_ellissi(S,aminmax_x_t,bminmax_y_t,ellissi_t);
if (calcola_ymax_opt_T) massimo_opt(ottiche_x_t,ottiche_y_t,&gnuplot_ymax_opt_T);
#endif
}
}
S+=dl;
}
lunghezza_accumulata+=lunghezza[i];
}
}
fclose(funzioni_ottiche);
fclose(matrici_iniziali);
fclose(posizionePart);
fclose(ellissi);
fclose(confronti);
parametri.close();
inputdistr.close();
#ifdef TEST_OPTICAL_FUNCTIONS
fclose(funzioni_ottiche_t);
fclose(ellissi_t);
#endif
string *etichette_posizione = new string[8];
string *etichette_ottiche = new string[8];
string *etichette_ellissi = new string[8];
#ifdef TEST_OPTICAL_FUNCTIONS
string *etichette_ottiche_T = new string[8];
string *etichette_ellissi_T = new string[8];
#endif
etichette_posizione[0] = "Posizione_Particelle";
etichette_posizione[1] = "Posizione Particelle";
etichette_posizione[2] = "z (m)";
etichette_posizione[3] = "x/y (m), p_x/p_y";
etichette_posizione[4] = "x";
etichette_posizione[5] = "y";
etichette_posizione[6] = "p_x";
etichette_posizione[7] = "p_y";
etichette_ottiche[0] = "Funzioni_Ottiche";
etichette_ottiche[1] = "Funzioni Ottiche";
etichette_ottiche[2] = "z (m)";
#if defined (CREATE_EPS)
etichette_ottiche[3] = "{/Symbol a}, {/Symbol b}";
etichette_ottiche[4] = "{/Symbol a}_x";
etichette_ottiche[5] = "{/Symbol a}_y";
etichette_ottiche[6] = "{/Symbol b}_x";
etichette_ottiche[7] = "{/Symbol b}_y";
#else
etichette_ottiche[3] = "Alpha, Beta";
etichette_ottiche[4] = "Alpha_x";
etichette_ottiche[5] = "Alpha_y";
etichette_ottiche[6] = "Beta_x";
etichette_ottiche[7] = "Beta_y";
#endif
etichette_ellissi[0] = "Parametri_Ellissi_Funz_Ottiche";
etichette_ellissi[1] = "Parametri Ellissi Funz Ottiche";
etichette_ellissi[2] = "z (m)";
etichette_ellissi[3] = "X , P";
etichette_ellissi[4] = "Xmax";
etichette_ellissi[5] = "Pmax_x";
etichette_ellissi[6] = "Ymax";
etichette_ellissi[7] = "Pmax_y";
#ifdef TEST_OPTICAL_FUNCTIONS
etichette_ottiche_T[0] = "Funzioni_Ottiche_T";
etichette_ottiche_T[1] = "Funzioni ottiche test";
etichette_ottiche_T[2] = "z (m)";
#if defined (CREATE_EPS)
etichette_ottiche_T[3] = "{/Symbol a}, {/Symbol b}";
etichette_ottiche_T[4] = "{/Symbol a}_x";
etichette_ottiche_T[5] = "{/Symbol a}_y";
etichette_ottiche_T[6] = "{/Symbol b}_x";
etichette_ottiche_T[7] = "{/Symbol b}_y";
#else
etichette_ottiche_T[3] = "Alpha, Beta";
etichette_ottiche_T[4] = "Alpha_x";
etichette_ottiche_T[5] = "Alpha_y";
etichette_ottiche_T[6] = "Beta_x";
etichette_ottiche_T[7] = "Beta_y";
#endif
etichette_ellissi_T[0] = "Parametri_Ellissi_Funz_Ottiche_T";
etichette_ellissi_T[1] = "Parametri Ellissi Funz Ottiche_T";
etichette_ellissi_T[2] = "z (m)";
etichette_ellissi_T[3] = "X , P";
etichette_ellissi_T[4] = "Xmax";
etichette_ellissi_T[5] = "Pmax_x";
etichette_ellissi_T[6] = "Ymax";
etichette_ellissi_T[7] = "Pmax_y";
#endif
/***********************************************************/
//cout << "conto_per_confronto= "<<conto_per_confronto;
double *dati_rilevati=new double [conto_per_confronto];
for (int a=0;a<conto_per_confronto;a++)
dati_rilevati[a]=0.;
if (confronto_pos_x||confronto_pos_y)
{
ifstream confro;
confro.open("Math_rilevati.txt");
for (int i=0;i<conto_per_confronto;i++)
{
confro >> dati_rilevati[i];
}
}
/* readonly attribute nsIDOMSVGAnimatedLength fx; */
NS_IMETHODIMP SVGRadialGradientElement::GetFx(nsIDOMSVGAnimatedLength * *aFx)
{
*aFx = Fx().get();
return NS_OK;
}
