Calculate_HF.c

#include"utils.h"
void Calculate_HF(int *tlist,double *vlist,int nfac,int nvert,double *angles,double *Eo,double *E0o,double *up,double TIME,double dist,double Gamma,double A,double Hdist,int N,double WL,double *freqx,double *freqy,int nfreq,double *offset,double *Fr,double *Fi)
{
    double complex *F=calloc(nfreq,sizeof(double complex)); 
 double complex *F0;
 F0=(double complex*)calloc(nfreq,sizeof(double complex));
 double *Flux,*Fldx,*Fldy,*Fldz,*FldA;
 Flux=(double*)calloc(nfac,sizeof(double));
 double M[3][3],dMb[3][3],dMo[3][3],dMl[3][3],Mt[3][3];
 double R[3][3],Rdb[3][3],Rdl[3][3],Rdo[3][3],RT[3][3];
 double E[3],E0[3];
 double normalr[3],side1[3],side2[3];
 double dechdx[3],dechdy[3],dechdz[3],dechdA[3];
 double n[3],*nb,*cent;
 double *vb1,*vb2,*vb3;
 double vr1[3],vr2[3],vr3[3];
 double *v1,*v2,*v3;
 double scale;
 double complex tscale,FTC;
 double dp;
 double B,TB=0;
 double norm;
 int t1,t2,t3,blocker,sign;
int j1,j2,j3;
 double mu,mu0,area,mub,ech,rexp;
 double *normal,*centroid;
 int *visible;
 int tb1,tb2,tb3; //Indices to the vertices of possible blocker facet
 int blocked=0;
 //Distance km->arcsec
 dp=1/(dist*149597871.0)*180.0/PI*3600.0;
 visible=calloc(nfac,sizeof(int));
  //Allocate for memory
// normal=(double*)malloc(3*nfac*sizeof(double));
 // centroid=(double*)mxCalloc(3*nfac,sizeof(double));
 // IndexofBlocks=(int*)mxCalloc(nfac,sizeof(int));
// NumofBlocks=(int*)mxCalloc(nfac,sizeof(int));
  //Calculate frame change matrix
  Calculate_Frame_Matrix(Eo,up,R);
 
 //FacetsOverHorizon(tlist,vlist,nfac,nvert,normal,centroid,NumofBlocks,IndexofBlocks);
 
 rotate(angles[0],angles[1],angles[2],0.0,TIME,M,dMb,dMl,dMo);
 //Construct asteroid->Camera frame matrix, which is
 //asteroid->world frame->camera frame
 transpose(M,Mt); //Transpose, since we rotate the model, not view directions
 mult_mat(R,Mt,RT); 
 mult_vector(M,Eo,E);
 mult_vector(M,E0o,E0);

 /*For each facet,
  * 1)Check if facet is visible
  * 2) Calculate echo
  * 3) Convert triangle to range-Doppler frame
  * 4) Calculate FT
  */
//Find actual blockers
FindActualBlockers(tlist,vlist,nfac,nvert,E,E,1,visible);
Calculate_Radiance(tlist,vlist,nfac,nvert,angles,Eo,E0o,TIME,Gamma, A,Hdist,WL,N,Flux,Fldx,Fldy,Fldz,FldA,0);
//for(int i=27;i<nfac;i++)
 // mexPrintf("fl%d: %.10e\n",i+1, Flux[i]);

 //visible is nfac vector, visible[j]=1 if facet (j+1)th facet is visible
//NOTE INDEXING
//mexPrintf("%f %f %f\n",vlist[0],vlist[1],vlist[2]);
 for(int j=0;j<nfac;j++)
 {
   if(visible[j]==0)
     continue;
  //Calculate normal from facet vertices
   //Vertex indices of the current facet
   //Note that C indices from 0, matlab from 1
   j1=tlist[j*3]-1;
   j2=tlist[j*3+1]-1;
   j3=tlist[j*3+2]-1;
   //Current vertices
   
   v1=vlist+j1*3;
   v2=vlist+j2*3;
   v3=vlist+j3*3;
   
   
   //Calculate normals and centroids
   for(int i=0;i<3;i++)
   {
     
     side1[i]=dp*(v2[i]-v1[i]); //Convert km->arcsec
     side2[i]=dp*(v3[i]-v1[i]);
    
   }
   cross(side1,side2,n);
   norm=NORM(n);
   n[0]=n[0]/norm;
   n[1]=n[1]/norm;
   n[2]=n[2]/norm;
   
   mu=DOT(E,n);
   mu0=DOT(E0,n);
  //Convert to camera frame
   mult_vector(RT,v1,vr1);
   mult_vector(RT,v2,vr2);
   mult_vector(RT,v3,vr3);
   for(int i=0;i<3;i++)
   {
     vr1[i]=dp*vr1[i];
     vr2[i]=dp*vr2[i];
     vr3[i]=dp*vr3[i];
   }
    
     //Now we should convert to frequency domain, ie calculate the contribution of each facet
     Calc_FTC(freqx,freqy,nfreq,vr1[0],vr1[1],vr2[0],vr2[1],vr3[0],vr3[1],F0);
    // printf("Fdd: %f %f\n",creal(FTdd[0]),cimag(FTdd[0]));
     //Note that we sum to F at each round, does not work, we need to multiply with the echo
     //Derivatives wrt angles
   area=0.5*norm;
  //if(j==0)
   //{
    // mexPrintf("area: %f mu: %f F0: %f\n",area,mu,F0[0]);
   //}
   //mexPrintf("area: %f normal: %f %f %f mu: %f mu0: %f\n",area,n[0],n[1],n[2],mu,mu0); 
     
   B=Flux[j];
     for(int jf=0;jf<nfreq;jf++)
     {
       //This should be taken outside of the loop
       
    //   mexPrintf("scale:%f offset: %f %f\n",scale,creal(cexp(2*PI*I*(offset[0]*freqx[jf]+offset[1]*freqy[jf]))),cimag(cexp(2*PI*I*(offset[0]*freqx[jf]+offset[1]*freqy[jf]))));
       //FTC=tscale*F0[jf];
       F[jf]+=B*F0[jf];
      }
      TB=TB+B*area*mu;
//  printf("Flux: %f area: %f mu: %f\n",B,area,mu);

}
//printf("Total brightness: %f\n",TB);  
//Normalize with total brightness
double complex temp;
for(int j=0;j<nfreq;j++)
{
    temp=cexp(2.0*PI*I*(offset[0]*freqx[j]+offset[1]*freqy[j]))*F[j]/TB;
  Fr[j]=creal(temp);
  Fi[j]=cimag(temp);
}

free(visible);
free(Flux);
free(F0);
free(F);
}

void Calculate_HF_deriv(int *tlist,double *vlist,int nfac,int nvert,double *angles,double *Eo,double *E0o,double *up,double TIME,double dist,double Gamma,double A,double Hdist,int N,double WL,double *freqx,double *freqy,int nfreq,double *offset,double *Fr,double *Fi,double *dFdxr,double *dFdxi,double  *dFdyr,double  *dFdyi,double *dFdzr,double *dFdzi,double *dFdAr,double *dFdAi,double *dFdoffr,double *dFdoffi)
{
    double complex *F=calloc(nfreq,sizeof(double complex)); 
 double complex *F0,*FTda,*FTdb,*FTdc,*FTdd,*FTdh,*FTdg,*FTdx,*FTdy,*FTdz,*FTdA;
 FTdx=calloc(nfreq*nvert,sizeof(double complex));
 FTdy=calloc(nfreq*nvert,sizeof(double complex));
 FTdz=calloc(nfreq*nvert,sizeof(double complex));
 FTdA=calloc(nfreq*3,sizeof(double complex));
 F0=calloc(nfreq,sizeof(double complex));
 FTda=malloc(nfreq*sizeof(double complex));
 FTdb=malloc(nfreq*sizeof(double complex));
 FTdc=malloc(nfreq*sizeof(double complex));
 FTdd=malloc(nfreq*sizeof(double complex));
 FTdh=malloc(nfreq*sizeof(double complex));
 FTdg=malloc(nfreq*sizeof(double complex));
 
 double M[3][3],dMb[3][3],dMo[3][3],dMl[3][3],Mt[3][3],dMbT[3][3],dMoT[3][3],dMlT[3][3]; //Rotation matrices and their derivatives and transposes
 double R[3][3],Rdb[3][3],Rdl[3][3],Rdo[3][3],RT[3][3]; //Projection matrix, and derivatives of combined projection+rotation matrix
 double dEdb[3],dEdl[3],dEdo[3],dE0db[3],dE0dl[3],dE0do[3];
 double dndx1[3],dndx2[3],dndx3[3],dndy1[3],dndy2[3],dndy3[3],dndz1[3],dndz2[3],dndz3[3]; //Derivatives of the facet normal vector
 double dBdx1,dBdy1,dBdz1,dBdx2,dBdy2,dBdz2,dBdx3,dBdy3,dBdz3,dBdb,dBdl,dBdo; //Derivatives of facet brightness
 double *dTBdx,*dTBdy,*dTBdz; //Derivatives of total brightness, allocating memory
 double *Flux,*Fldx,*Fldy,*Fldz,*FldA;
 Flux=calloc(nfac,sizeof(double));
 Fldx=calloc(nfac*nvert,sizeof(double));
 Fldy=calloc(nfac*nvert,sizeof(double));
 Fldz=calloc(nfac*nvert,sizeof(double));
 FldA=calloc(nfac*3,sizeof(double));
 double dTBdA[3]={0};
 dTBdx=calloc(nvert,sizeof(double));
 dTBdy=calloc(nvert,sizeof(double));
 dTBdz=calloc(nvert,sizeof(double));
 double dmudx1,dmudy1,dmudz1,dmudx2,dmudy2,dmudz2,dmudx3,dmudy3,dmudz3;
 double dmu0dx1,dmu0dy1,dmu0dz1,dmu0dx2,dmu0dy2,dmu0dz2,dmu0dx3,dmu0dy3,dmu0dz3;
 double dmudl,dmudb,dmudo,dmu0dl,dmu0db,dmu0do; //Derivatives of mu and mu0
 double dAdx[3],dAdy[3],dAdz[3]; //Facet area derivatives
 double dadx,dady,dadz,dbdx,dbdy,dbdz; //Derivatives of projected vertices
 double E[3],E0[3]; //Earth and Sun direction, rotated
 double side1[3],side2[3];
 double n[3];
 double v1db[3],v2db[3],v3db[3],v1dl[3],v2dl[3],v3dl[3],v1do[3],v2do[3],v3do[3]; //Derivatives of 2d vertices wrt angles
 double vr1[3],vr2[3],vr3[3];
 double v1[3],v2[3],v3[3];
 double complex scale;

 double dp;
 double B,TB=0.0;
 double norm;
 double mut,mu0t;
 int t1,t2,t3;
int j1,j2,j3;
 double mu,mu0,area;
 double *normal;
 int *visible;
 int tb1,tb2,tb3; //Indices to the vertices of possible blocker facet
 int blocked=0;
 //Distance km->arcsec
 dp=1/(dist*149597871.0)*180.0/PI*3600.0;
 visible=calloc(nfac,sizeof(int));
 //Calculate_Frame_Matrix_Derivatives(Eo,angles,TIME,rfreq,R,Rdb,Rdl,Rdo);
  Calculate_Frame_Matrix(Eo,up,R);
  
  
  //Calculate frame change matrix
 
 
 //FacetsOverHorizon(tlist,vlist,nfac,nvert,normal,centroid,NumofBlocks,IndexofBlocks);
 
 rotate(angles[0],angles[1],angles[2],0.0,TIME,M,dMb,dMl,dMo);
 //Construct asteroid->Camera frame matrix, which is
 //asteroid->world frame->camera frame
 transpose(M,Mt); //Transpose, since we rotate the model, not view directions
 transpose(dMb,dMbT);
 transpose(dMl,dMlT);
 transpose(dMo,dMoT);
 mult_mat(R,Mt,RT); 
 mult_vector(M,Eo,E);
 mult_vector(M,E0o,E0);
 
 mult_mat(R,dMbT,Rdb);
 mult_mat(R,dMlT,Rdl);
 mult_mat(R,dMoT,Rdo);
//Derivatives of E,E0 wrt beta,lambda,omega
 mult_vector(dMb,Eo,dEdb);
 mult_vector(dMl,Eo,dEdl);
 mult_vector(dMo,Eo,dEdo);
 mult_vector(dMb,E0o,dE0db);
 mult_vector(dMl,E0o,dE0dl);
 mult_vector(dMo,E0o,dE0do);
 dadx=RT[0][0];
  dady=RT[0][1];
  dadz=RT[0][2];
  dbdx=RT[1][0];
  dbdy=RT[1][1];
  dbdz=RT[1][2];
 /*For each facet,
  * 1)Check if facet is visible
  * 2) Calculate echo
  * 3) Convert triangle to range-Doppler frame
  * 4) Calculate FT
  */
//Find actual blockers
FindActualBlockers(tlist,vlist,nfac,nvert,E,E,1,visible);
 //visible is nfac vector, visible[j]=1 if facet (j+1)th facet is visible
//NOTE INDEXING
Calculate_Radiance(tlist,vlist,nfac,nvert,angles,Eo,E0o,TIME,Gamma, A,Hdist,WL,N,Flux,Fldx,Fldy,Fldz,FldA,1);

 //for(int j=0;j<nfac;j++)
for(int j=0;j<nfac;j++) 
{
   if(visible[j]==0)
     continue;
  //Calculate normal from facet vertices
   //Vertex indices of the current facet
   //Note that C indices from 0, matlab from 1
   j1=tlist[j*3]-1;
   j2=tlist[j*3+1]-1;
   j3=tlist[j*3+2]-1;
   //Current vertices
  
   
   for(int i=0;i<3;i++)
   {
   v1[i]=*(vlist+j1*3+i)*dp; //convert km->arcsec
   v2[i]=*(vlist+j2*3+i)*dp;
   v3[i]=*(vlist+j3*3+i)*dp;
   }
   
    //Calculate Normal derivatives (in the original frame)
    Calculate_Area_and_Normal_Derivative(v1,v2,v3,n,dndx1,dndx2,dndx3,dndy1,dndy2,dndy3,dndz1,dndz2,dndz3,&area,dAdx,dAdy,dAdz);
    
   //Calculate normals and centroids
   
   mu=DOT(E,n);
   
  //Convert to camera frame
   mult_vector(RT,v1,vr1);
   mult_vector(RT,v2,vr2);
   mult_vector(RT,v3,vr3);
  
    
     //Now we should convert to frequency domain, ie calculate the contribution of each facet
  //   Calc_FTC(freqx,freqy,nfreq,vr1[0],vr1[1],vr2[0],vr2[1],vr3[0],vr3[1],F0);
     Calc_FTC_deriv(freqx,freqy,nfreq,vr1[0],vr1[1],vr2[0],vr2[1],vr3[0],vr3[1],F0,FTda,FTdb,FTdc,FTdd,FTdg,FTdh);
  
    //Derivatives wrt angles
     mult_vector(Rdb,v1,v1db);
     mult_vector(Rdb,v2,v2db);
     mult_vector(Rdb,v3,v3db);
     mult_vector(Rdl,v1,v1dl);
     mult_vector(Rdl,v2,v2dl);
     mult_vector(Rdl,v3,v3dl);
     mult_vector(Rdo,v1,v1do);
     mult_vector(Rdo,v2,v2do);
     mult_vector(Rdo,v3,v3do);
     //Derivatives of mu,mu0
     dmudx1=DOT(E,dndx1);
     dmudx2=DOT(E,dndx2);
     dmudx3=DOT(E,dndx3);
     dmudy1=DOT(E,dndy1);
     dmudy2=DOT(E,dndy2);
     dmudy3=DOT(E,dndy3);
     dmudz1=DOT(E,dndz1);
     dmudz2=DOT(E,dndz2);
     dmudz3=DOT(E,dndz3);
     dmudb=DOT(dEdb,n);
     dmudl=DOT(dEdl,n);
     dmudo=DOT(dEdo,n);
     B=Flux[j];
   //Derivatives of B
  
   dBdx1=Fldx[j*nvert+j1]/dp;
   dBdx2=Fldx[j*nvert+j2]/dp;
   dBdx3=Fldx[j*nvert+j3]/dp;
   
   dBdy1=Fldy[j*nvert+j1]/dp;
   dBdy2=Fldy[j*nvert+j2]/dp;
   dBdy3=Fldy[j*nvert+j3]/dp;
   
   dBdz1=Fldz[j*nvert+j1]/dp;
   dBdz2=Fldz[j*nvert+j2]/dp;
   dBdz3=Fldz[j*nvert+j3]/dp;
   dBdb=FldA[3*j];
   dBdl=FldA[3*j+1];
   dBdo=FldA[3*j+2];
   //Derivative of total brightness
   dTBdx[j1]+=dBdx1*area*mu+B*dAdx[0]*mu+B*area*dmudx1;
   dTBdx[j2]+=dBdx2*area*mu+B*dAdx[1]*mu+B*area*dmudx2;
   dTBdx[j3]+=dBdx3*area*mu+B*dAdx[2]*mu+B*area*dmudx3;
   dTBdy[j1]+=dBdy1*area*mu+B*dAdy[0]*mu+B*area*dmudy1;
   dTBdy[j2]+=dBdy2*area*mu+B*dAdy[1]*mu+B*area*dmudy2;
   dTBdy[j3]+=dBdy3*area*mu+B*dAdy[2]*mu+B*area*dmudy3;
   dTBdz[j1]+=dBdz1*area*mu+B*dAdz[0]*mu+B*area*dmudz1;
   dTBdz[j2]+=dBdz2*area*mu+B*dAdz[1]*mu+B*area*dmudz2;
   dTBdz[j3]+=dBdz3*area*mu+B*dAdz[2]*mu+B*area*dmudz3;
   dTBdA[0]+=dBdb*area*mu+B*area*dmudb;
   dTBdA[1]+=dBdl*area*mu+B*area*dmudl;
   dTBdA[2]+=dBdo*area*mu+B*area*dmudo;
   
     for(int jf=0;jf<nfreq;jf++)
     {
     F[jf]+=B*F0[jf];
     
       
       FTdx[jf*nvert+j1]+=dBdx1*F0[jf]+B*(FTda[jf]*dadx+FTdb[jf]*dbdx);
       FTdx[jf*nvert+j2]+=dBdx2*F0[jf]+B*(FTdc[jf]*dadx+FTdd[jf]*dbdx);
       FTdx[jf*nvert+j3]+=dBdx3*F0[jf]+B*(FTdg[jf]*dadx+FTdh[jf]*dbdx);
       
       FTdy[jf*nvert+j1]+=dBdy1*F0[jf]+B*(FTda[jf]*dady+FTdb[jf]*dbdy);
       FTdy[jf*nvert+j2]+=dBdy2*F0[jf]+B*(FTdc[jf]*dady+FTdd[jf]*dbdy);
       FTdy[jf*nvert+j3]+=dBdy3*F0[jf]+B*(FTdg[jf]*dady+FTdh[jf]*dbdy);
       
       FTdz[jf*nvert+j1]+=dBdz1*F0[jf]+B*(FTda[jf]*dadz+FTdb[jf]*dbdz);
       FTdz[jf*nvert+j2]+=dBdz2*F0[jf]+B*(FTdc[jf]*dadz+FTdd[jf]*dbdz);
       FTdz[jf*nvert+j3]+=dBdz3*F0[jf]+B*(FTdg[jf]*dadz+FTdh[jf]*dbdz);
        //angle derivatives
       
       FTdA[jf*3+0]+=dBdb*F0[jf]+B*(FTda[jf]*v1db[0]+FTdb[jf]*v1db[1]+FTdc[jf]*v2db[0]+FTdd[jf]*v2db[1]+FTdg[jf]*v3db[0]+FTdh[jf]*v3db[1]);
       FTdA[jf*3+1]+=dBdl*F0[jf]+B*(FTda[jf]*v1dl[0]+FTdb[jf]*v1dl[1]+FTdc[jf]*v2dl[0]+FTdd[jf]*v2dl[1]+FTdg[jf]*v3dl[0]+FTdh[jf]*v3dl[1]);
       FTdA[jf*3+2]+=dBdo*F0[jf]+B*(FTda[jf]*v1do[0]+FTdb[jf]*v1do[1]+FTdc[jf]*v2do[0]+FTdd[jf]*v2do[1]+FTdg[jf]*v3do[0]+FTdh[jf]*v3do[1]);
     }
    
      TB=TB+B*area*mu;
 
}

//Normalize with total brightness
double complex temp;
for(int j=0;j<nfreq;j++)
{
  scale=cexp(2.0*PI*I*(offset[0]*freqx[j]+offset[1]*freqy[j]));
  for(int k=0;k<nvert;k++)
  {
      temp=dp*scale*(FTdx[j*nvert+k]*TB-F[j]*dTBdx[k])/pow(TB,2);
    dFdxr[j*nvert+k]=creal(temp);
    dFdxi[j*nvert+k]=cimag(temp);
    temp=dp*scale*(FTdy[j*nvert+k]*TB-F[j]*dTBdy[k])/pow(TB,2);
    dFdyr[j*nvert+k]=creal(temp);
    dFdyi[j*nvert+k]=cimag(temp);
    temp=dp*scale*(FTdz[j*nvert+k]*TB-F[j]*dTBdz[k])/pow(TB,2);
    dFdzr[j*nvert+k]=creal(temp);
    dFdzi[j*nvert+k]=cimag(temp);
  }
  temp=scale*(FTdA[j*3+0]*TB-F[j]*dTBdA[0])/pow(TB,2);
  dFdAr[j*3+0]=creal(temp);
  dFdAi[j*3+0]=cimag(temp);
  temp=scale*(FTdA[j*3+1]*TB-F[j]*dTBdA[1])/pow(TB,2);
  dFdAr[j*3+1]=creal(temp);
  dFdAi[j*3+1]=cimag(temp);
  temp=scale*(FTdA[j*3+2]*TB-F[j]*dTBdA[2])/pow(TB,2);
  dFdAr[j*3+2]=creal(temp);
  dFdAi[j*3+2]=cimag(temp);
  temp=cexp(2.0*PI*I*(offset[0]*freqx[j]+offset[1]*freqy[j]))*F[j]/TB;
  Fr[j]=creal(temp);
  Fi[j]=cimag(temp);
  temp=2.0*PI*I*freqx[j]*F[j];
  dFdoffr[j*2+0]=creal(temp);
  dFdoffi[j*2+0]=cimag(temp);
  temp=2.0*PI*I*freqy[j]*F[j];
  dFdoffr[j*2+1]=creal(temp);
  dFdoffi[j*2+1]=cimag(temp);
}


free(FTdx);
free(FTdy);
free(FTdz);
free(FTdA);
free(dTBdx);
free(dTBdy);
free(dTBdz);

free(FTda);
free(FTdb);
free(FTdc);
free(FTdd);
free(FTdg);
free(FTdh);
free(F0);
free(visible);
free(Flux);
free(Fldx);
free(Fldy);
free(Fldz);
free(FldA);
free(F);
}