自学教程：C++ FABS函数代码示例

/* Absolute value versions of the above */static int32_t MaxAbsAccumulate(CSOUND *csound, MINMAXACCUM *p){    IGN(csound);    uint32_t offset = p->h.insdshead->ksmps_offset;    uint32_t early  = p->h.insdshead->ksmps_no_end;    uint32_t n, nsmps = CS_KSMPS;    MYFLT   *out = p->accum;    MYFLT   *in = p->ain;    MYFLT   inabs;    if (UNLIKELY(offset)) memset(out, '/0', offset*sizeof(MYFLT));    if (UNLIKELY(early)) {      nsmps -= early;      memset(&out[nsmps], '/0', early*sizeof(MYFLT));    }    for (n=offset; n<nsmps; n++) {      inabs = FABS(in[n]);      if (UNLIKELY(inabs > out[n]))        out[n] = inabs;    }    return OK;}

// input : current spectrum in the form of power *spec and phase *phase,//         the last two earlier spectrums are at position//         512 and 1024 of the corresponding Input-Arrays.//         Array *vocal, which can mark an FFT_Linie as harmonic// output: current amplitude *amp and unpredictability *cwstatic voidCalcUnpred (PsyModel* m,			const int     MaxLine,			const float*  spec,			const float*  phase,			const int*    vocal,			float*        amp0,			float*        phs0,			float*        cw ){    int     n;    float   amp;    float   tmp;#define amp1  ((amp0) +  512)           // amp[ 512...1023] contains data of frame-1#define amp2  ((amp0) + 1024)           // amp[1024...1535] contains data of frame-2#define phs1  ((phs0) +  512)           // phs[ 512...1023] contains data of frame-1#define phs2  ((phs0) + 1024)           // phs[1024...1535] contains data of frame-2    for ( n = 0; n < MaxLine; n++ ) {        tmp     = COSF  ((phs0[n] = phase[n]) - 2*phs1[n] + phs2[n]);   // copy phase to output-array, predict phase and calculate predictive error        amp0[n] = SQRTF (spec[n]);                                      // calculate and set amplitude        amp     = 2*amp1[n] - amp2[n];                                  // predict amplitude        // calculate unpredictability        cw[n] = SQRTF (spec[n] + amp * (amp - 2*amp0[n] * tmp)) / (amp0[n] + FABS(amp));    }    // postprocessing of harmonic FFT-lines (*cw is set to CVD_UNPRED)	if ( m->CVD_used  &&  vocal != NULL ) {        for ( n = 0; n < MAX_CVD_LINE; n++, cw++, vocal++ )            if ( *vocal != 0  &&  *cw > CVD_UNPRED * 0.01 * *vocal )                *cw = CVD_UNPRED * 0.01 * *vocal;    }    return;}

/* central finite difference approximation to the jacobian of func */void FDIF_CENT_JAC_APPROX(    void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata),													   /* function to differentiate */    LM_REAL *p,              /* I: current parameter estimate, mx1 */    LM_REAL *hxm,            /* W/O: work array for evaluating func(p-delta), nx1 */    LM_REAL *hxp,            /* W/O: work array for evaluating func(p+delta), nx1 */    LM_REAL delta,           /* increment for computing the jacobian */    LM_REAL *jac,            /* O: array for storing approximated jacobian, nxm */    int m,    int n,    void *adata){register int i, j;LM_REAL tmp;register LM_REAL d;  for(j=0; j<m; ++j){    /* determine d=max(1E-04*|p[j]|, delta), see HZ */    d=CNST(1E-04)*p[j]; // force evaluation    d=FABS(d);    if(d<delta)      d=delta;    tmp=p[j];    p[j]-=d;    (*func)(p, hxm, m, n, adata);    p[j]=tmp+d;    (*func)(p, hxp, m, n, adata);    p[j]=tmp; /* restore */    d=CNST(0.5)/d; /* invert so that divisions can be carried out faster as multiplications */    for(i=0; i<n; ++i){      jac[i*m+j]=(hxp[i]-hxm[i])*d;    }  }}

static int get_absinsno(CSOUND *csound, TRIGINSTR *p, int stringname){    int insno;    /* Get absolute instr num */    /* IV - Oct 31 2002: allow string argument for named instruments */    if (stringname)      insno = (int)strarg2insno_p(csound, ((STRINGDAT*)p->args[0])->data);    else if (ISSTRCOD(*p->args[0])) {      char *ss = get_arg_string(csound, *p->args[0]);      insno = (int)strarg2insno_p(csound, ss);    }    else      insno = (int)FABS(*p->args[0]);    /* Check that instrument is defined */    if (UNLIKELY(insno < 1 || insno > csound->engineState.maxinsno ||                 csound->engineState.instrtxtp[insno] == NULL)) {      csound->Warning(csound, Str("schedkwhen ignored. "                                  "Instrument %d undefined/n"), insno);      csound->perferrcnt++;      return -1;    }    return insno;}

static double evaluateGTRGAMMAPROT (int *wptr,				    double *x1, double *x2,  				    double *tipVector, 				    unsigned char *tipX1, int n, double *diagptable){  double   sum = 0.0, term;          int     i, j, l;     double  *left, *right;                  if(tipX1)    {                     for (i = 0; i < n; i++) 	{	  __m128d tv = _mm_setzero_pd();	  left = &(tipVector[20 * tipX1[i]]);	  	  	  	  for(j = 0, term = 0.0; j < 4; j++)	    {	      double *d = &diagptable[j * 20];	      right = &(x2[80 * i + 20 * j]);	      for(l = 0; l < 20; l+=2)		{		  __m128d mul = _mm_mul_pd(_mm_load_pd(&left[l]), _mm_load_pd(&right[l]));		  tv = _mm_add_pd(tv, _mm_mul_pd(mul, _mm_load_pd(&d[l])));		   		}		 			    }	  tv = _mm_hadd_pd(tv, tv);	  _mm_storel_pd(&term, tv);	  	  	 	  term = LOG(0.25 * FABS(term));		 	  	  sum += wptr[i] * term;	}    	            }                else    {      for (i = 0; i < n; i++) 	{	  	 	             	  __m128d tv = _mm_setzero_pd();	 	  	  	      	  for(j = 0, term = 0.0; j < 4; j++)	    {	      double *d = &diagptable[j * 20];	      left  = &(x1[80 * i + 20 * j]);	      right = &(x2[80 * i + 20 * j]);	      	      for(l = 0; l < 20; l+=2)		{		  __m128d mul = _mm_mul_pd(_mm_load_pd(&left[l]), _mm_load_pd(&right[l]));		  tv = _mm_add_pd(tv, _mm_mul_pd(mul, _mm_load_pd(&d[l])));		   		}		 			    }	  tv = _mm_hadd_pd(tv, tv);	  _mm_storel_pd(&term, tv);	  	  		  term = LOG(0.25 * FABS(term));	  	  	  sum += wptr[i] * term;	}    }         return  sum;}

static double evaluateCAT_FLEX (int *cptr, int *wptr,				double *x1, double *x2, double *tipVector,				unsigned char *tipX1, int n, double *diagptable_start, const int states){  double       sum = 0.0,     term,    *diagptable,      *left,     *right;    int         i,     l;                               /* chosing between tip vectors and non tip vectors is identical in all flavors of this function ,regardless      of whether we are using CAT, GAMMA, DNA or protein data etc */  if(tipX1)    {                       for (i = 0; i < n; i++) 	{	  /* same as in the GAMMA implementation */	  left = &(tipVector[states * tipX1[i]]);	  right = &(x2[states * i]);	  	  /* important difference here, we do not have, as for GAMMA 	     4 P matrices assigned to each site, but just one. However those 	     P-Matrices can be different for the sites.	     Hence we index into the precalculated P-matrices for individual sites 	     via the category pointer cptr[i]	  */	  diagptable = &diagptable_start[states * cptr[i]];	           	 	  /* similar to gamma, with the only difference that we do not integrate (sum)	     over the discrete gamma rates, but simply compute the likelihood of the 	     site and the given P-matrix */	  for(l = 0, term = 0.0; l < states; l++)	    term += left[l] * right[l] * diagptable[l];	 	  	   	  	  /* take the log */	  term = LOG(FABS(term));	  	  	  /* 	     multiply the log with the pattern weight of this site. 	     The site pattern for which we just computed the likelihood may 	     represent several alignment columns sites that have been compressed 	     into one site pattern if they are exactly identical AND evolve under the same model,	     i.e., form part of the same partition.	  */	   	     	  sum += wptr[i] * term;	}          }      else    {          for (i = 0; i < n; i++) 	{		  /* as before we now access the likelihood arrayes of two inner nodes */	  left  = &x1[states * i];	  right = &x2[states * i];	  	  diagptable = &diagptable_start[states * cptr[i]];	  		  for(l = 0, term = 0.0; l < states; l++)	    term += left[l] * right[l] * diagptable[l];		  	  term = LOG(FABS(term));	 	  	  sum += wptr[i] * term;      	}    }               return  sum;         } 

static double evaluateGTRCATPROT_SAVE (int *cptr, int *wptr,				       double *x1, double *x2, double *tipVector,				       unsigned char *tipX1, int n, double *diagptable_start, 				       double *x1_gapColumn, double *x2_gapColumn, unsigned int *x1_gap, unsigned int *x2_gap){  double       sum = 0.0,     term,    *diagptable,      *left,     *right,    *left_ptr = x1,    *right_ptr = x2;    int         i,     l;                               if(tipX1)    {                       for (i = 0; i < n; i++) 	{	       		  left = &(tipVector[20 * tipX1[i]]);	  if(isGap(x2_gap, i))	    right = x2_gapColumn;	  else	    {	      right = right_ptr;	      right_ptr += 20;	    }	  	 	  	  diagptable = &diagptable_start[20 * cptr[i]];	           	 	  __m128d tv = _mm_setzero_pd();	    	  	  for(l = 0; l < 20; l+=2)	    {	      __m128d lv = _mm_load_pd(&left[l]);	      __m128d rv = _mm_load_pd(&right[l]);	      __m128d mul = _mm_mul_pd(lv, rv);	      __m128d dv = _mm_load_pd(&diagptable[l]);	      	      tv = _mm_add_pd(tv, _mm_mul_pd(mul, dv));		   	    }		 			  	  tv = _mm_hadd_pd(tv, tv);	  _mm_storel_pd(&term, tv);    	  	  term = LOG(FABS(term));	  	  	  sum += wptr[i] * term;	}          }      else    {          for (i = 0; i < n; i++) 	{		       	      	      	  	  if(isGap(x1_gap, i))	    left = x1_gapColumn;	  else	    {	      left = left_ptr;	      left_ptr += 20;	    }	  	  if(isGap(x2_gap, i))	    right = x2_gapColumn;	  else	    {	      right = right_ptr;	      right_ptr += 20;	    }	  	  diagptable = &diagptable_start[20 * cptr[i]];	  		  __m128d tv = _mm_setzero_pd();	    	  	  for(l = 0; l < 20; l+=2)	    {	      __m128d lv = _mm_load_pd(&left[l]);	      __m128d rv = _mm_load_pd(&right[l]);	      __m128d mul = _mm_mul_pd(lv, rv);	      __m128d dv = _mm_load_pd(&diagptable[l]);	      	      tv = _mm_add_pd(tv, _mm_mul_pd(mul, dv));		   	    }		 			  	  tv = _mm_hadd_pd(tv, tv);	  _mm_storel_pd(&term, tv);	  	  	  term = LOG(FABS(term));	 	  	  sum += wptr[i] * term;      	}    }               return  sum;         //.........这里部分代码省略.........

double attribute_hiddenpnchisq_raw(double x, double f, double theta,	    double errmax, double reltol, int itrmax, Rboolean lower_tail){    double lam, x2, f2, term, bound, f_x_2n, f_2n;    double l_lam = -1., l_x = -1.; /* initialized for -Wall */    int n;    Rboolean lamSml, tSml, is_r, is_b, is_it;    LDOUBLE ans, u, v, t, lt, lu =-1;    static const double _dbl_min_exp = M_LN2 * DBL_MIN_EXP;    /*= -708.3964 for IEEE double precision */    if (x <= 0.) {	if(x == 0. && f == 0.)	    return lower_tail ? exp(-0.5*theta) : -expm1(-0.5*theta);	/* x < 0  or {x==0, f > 0} */	return lower_tail ? 0. : 1.;    }    if(!R_FINITE(x))	return lower_tail ? 1. : 0.;    /* This is principally for use from qnchisq */#ifndef MATHLIB_STANDALONE    R_CheckUserInterrupt();#endif    if(theta < 80) { /* use 110 for Inf, as ppois(110, 80/2, lower.tail=FALSE) is 2e-20 */	LDOUBLE sum = 0, sum2 = 0, lambda = 0.5*theta, 	    pr = EXP(-lambda); // does this need a feature test?	double ans;	int i;	/* we need to renormalize here: the result could be very close to 1 */	for(i = 0; i < 110;  pr *= lambda/++i) {	    sum2 += pr;	    sum += pr * pchisq(x, f+2*i, lower_tail, FALSE);	    if (sum2 >= 1-1e-15) break;	}	ans = (double) (sum/sum2);	return ans;    }#ifdef DEBUG_pnch    REprintf("pnchisq(x=%g, f=%g, theta=%g): ",x,f,theta);#endif    lam = .5 * theta;    lamSml = (-lam < _dbl_min_exp);    if(lamSml) {	/* MATHLIB_ERROR(	   "non centrality parameter (= %g) too large for current algorithm",	   theta) */        u = 0;        lu = -lam;/* == ln(u) */        l_lam = log(lam);    } else {	u = exp(-lam);    }    /* evaluate the first term */    v = u;    x2 = .5 * x;    f2 = .5 * f;    f_x_2n = f - x;#ifdef DEBUG_pnch    REprintf("-- v=exp(-th/2)=%g, x/2= %g, f/2= %g/n",v,x2,f2);#endif    if(f2 * DBL_EPSILON > 0.125 && /* very large f and x ~= f: probably needs */       FABS(t = x2 - f2) <         /* another algorithm anyway */       sqrt(DBL_EPSILON) * f2) {	/* evade cancellation error */	/* t = exp((1 - t)*(2 - t/(f2 + 1))) / sqrt(2*M_PI*(f2 + 1));*/        lt = (1 - t)*(2 - t/(f2 + 1)) - 0.5 * log(2*M_PI*(f2 + 1));#ifdef DEBUG_pnch	REprintf(" (case I) ==> ");#endif    }    else {	/* Usual case 2: careful not to overflow .. : */	lt = f2*log(x2) -x2 - lgammafn(f2 + 1);    }#ifdef DEBUG_pnch    REprintf(" lt= %g", lt);#endif    tSml = (lt < _dbl_min_exp);    if(tSml) {	if (x > f + theta +  5* sqrt( 2*(f + 2*theta))) {	    /* x > E[X] + 5* sigma(X) */	    return lower_tail ? 1. : 0.; /* FIXME: We could be more accurate than 0. */	} /* else */	l_x = log(x);	ans = term = 0.; t = 0;    }    else {	t = EXP(lt);#ifdef DEBUG_pnch 	REprintf(", t=exp(lt)= %g/n", t);#endif//.........这里部分代码省略.........

    //------------------------------------------------------------------------    void bezier_arc::init(real x,  real y,                           real rx, real ry,                           real start_angle,                           real sweep_angle)    {        start_angle = FMOD(start_angle, 2.0f * pi);        if(sweep_angle >=  2.0f * pi) sweep_angle =  2.0f * pi;        if(sweep_angle <= -2.0f * pi) sweep_angle = -2.0f * pi;        if(FABS(sweep_angle) < 1e-10)        {            m_num_vertices = 4;            m_cmd = path_cmd_line_to;            m_vertices[0] = x + rx * (real)cos(start_angle);            m_vertices[1] = y + ry * (real)sin(start_angle);            m_vertices[2] = x + rx * (real)cos(start_angle + sweep_angle);            m_vertices[3] = y + ry * (real)sin(start_angle + sweep_angle);            return;        }        real total_sweep = 0.0f;        real local_sweep = 0.0f;        real prev_sweep;        m_num_vertices = 2;        m_cmd = path_cmd_curve4;        bool done = false;        do        {            if(sweep_angle < 0.0f)            {                prev_sweep  = total_sweep;                local_sweep = -pi * 0.5f;                total_sweep -= pi * 0.5f;                if(total_sweep <= sweep_angle + bezier_arc_angle_epsilon)                {                    local_sweep = sweep_angle - prev_sweep;                    done = true;                }            }            else            {                prev_sweep  = total_sweep;                local_sweep =  pi * 0.5f;                total_sweep += pi * 0.5f;                if(total_sweep >= sweep_angle - bezier_arc_angle_epsilon)                {                    local_sweep = sweep_angle - prev_sweep;                    done = true;                }            }            arc_to_bezier(x, y, rx, ry,                           start_angle,                           local_sweep,                           m_vertices + m_num_vertices - 2);            m_num_vertices += 6;            start_angle += local_sweep;        }        while(!done && m_num_vertices < 26);    }

//.........这里部分代码省略.........            lm=l*m;            tmp+=jac[lm+i]*jac[lm+j];          }		      /* store tmp in the corresponding upper and lower part elements */          jacTjac[i*m+j]=jacTjac[j*m+i]=tmp;        }        /* J^T e */        for(l=0, tmp=0.0; l<n; ++l)          tmp+=jac[l*m+i]*e[l];        jacTe[i]=tmp;      }    }    else{ // this is a large problem      /* Cache efficient computation of J^T J based on blocking       */      TRANS_MAT_MAT_MULT(jac, jacTjac, n, m);      /* cache efficient computation of J^T e */      for(i=0; i<m; ++i)        jacTe[i]=0.0;      for(i=0; i<n; ++i){        register LM_REAL *jacrow;        for(l=0, jacrow=jac+i*m, tmp=e[i]; l<m; ++l)          jacTe[l]+=jacrow[l]*tmp;      }    }	  /* Compute ||J^T e||_inf and ||p||^2 */    for(i=0, p_L2=jacTe_inf=0.0; i<m; ++i){      if(jacTe_inf < (tmp=FABS(jacTe[i]))) jacTe_inf=tmp;      diag_jacTjac[i]=jacTjac[i*m+i]; /* save diagonal entries so that augmentation can be later canceled */      p_L2+=p[i]*p[i];    }    //p_L2=sqrt(p_L2);#if 0if(!(k%10)){  printf("Iter: %d, estimate: ", k);  for(i=0; i<m; ++i)    printf("%.9g ", p[i]);  printf("-- errors %.9g %0.9g/n", jacTe_inf, p_eL2);}#endif    /* check for convergence */    if((jacTe_inf <= eps1)){      Dp_L2=0.0; /* no increment for p in this case */      stop=1;      break;    }   /* compute initial damping factor */    if(k==0){      for(i=0, tmp=LM_REAL_MIN; i<m; ++i)        if(diag_jacTjac[i]>tmp) tmp=diag_jacTjac[i]; /* find max diagonal element */      mu=tau*tmp;    }    /* determine increment using adaptive damping */    while(1){      /* augment normal equations */

//**********************************************************************************//**********************************************************************************void cuCompCooling(REAL temp, REAL x, REAL nH, REAL *lambda, REAL *tcool, REAL aexp,REAL CLUMPF){  REAL c1,c2,c3,c4,c5,c6;  REAL unsurtc;  REAL nh2;  nh2=nH*1e-6;// ! m-3 ==> cm-3  // Collisional Ionization Cooling  c1=EXP(-157809.1e0/temp)*1.27e-21*SQRT(temp)/(1.+SQRT(temp/1e5))*x*(1.-x)*nh2*nh2*CLUMPF;  // Case A Recombination Cooling  c2=1.778e-29*temp*POW(2e0*157807e0/temp,1.965e0)/POW(1.+POW(2e0*157807e0/temp/0.541e0,0.502e0),2.697e0)*x*x*nh2*nh2*CLUMPF;  // Case B Recombination Cooling  c6=3.435e-30*temp*POW(2e0*157807e0/temp,1.970e0)/POW(1.+(POW(2e0*157807e0/temp/2.250e0,0.376e0)),3.720e0)*x*x*nh2*nh2*CLUMPF;  c6=0.;  // Collisional excitation cooling  c3=EXP(-118348e0/temp)*7.5e-19/(1+SQRT(temp/1e5))*x*(1.-x)*nh2*nh2*CLUMPF;  // Bremmsstrahlung  c4=1.42e-27*1.5e0*SQRT(temp)*x*x*nh2*nh2*CLUMPF;  // Compton Cooling  c5=0;  /* c5=1.017e-37*POW(2.727/aexp,4)*(temp-2.727/aexp)*nh2*x; */  /* c5=0.; */#ifndef WRADTEST  c5=5.406e-24*(temp-2.727/aexp)/POW(aexp/0.001,4)*x*nh2;  REAL Ta=2.727/aexp; c5=5.406e-36*(temp-Ta)/(aexp*aexp*aexp*aexp)*x*nh2;#endif  // Overall Cooling  *lambda=c1+c2+c3+c4+c5+c6;// ! erg*cm-3*s-1  // Unit Conversion  *lambda=(*lambda)*1e-7*1e6;// ! J*m-3*s-1  // cooling times  unsurtc=FMAX(c1,c2);  unsurtc=FMAX(unsurtc,c3);  unsurtc=FMAX(unsurtc,c4);  unsurtc=FMAX(unsurtc,FABS(c5));  unsurtc=FMAX(unsurtc,c6)*1e-7;// ==> J/cm3/s  *tcool=1.5e0*nh2*(1.+x)*KBOLTZ*temp/unsurtc; //Myr}

//.........这里部分代码省略.........      currentcool_t=0.;      nitcool=0.;      REAL da;      //printf("cpu=%d fudge=%e ncv=%d currentcool_t=%e dt=%e/n",cpu->rank,param->fudgecool,ncvgcool,currentcool_t,dt);      // local cooling loop -------------------------------      while(currentcool_t<dt)	{	  /// Cosmological Adiabatic expansion effects ==============#ifdef TESTCOSMO	  REAL hubblet=param->cosmo->H0*SQRT(param->cosmo->om/aexp+param->cosmo->ov*(aexp*aexp))/aexp*(1e3/(1e6*PARSEC)); // s-1 // SOMETHING TO CHECK HERE#else	  REAL hubblet=0.;#endif	  //if(eint[idloc]!=E0) printf("2!/n");	  tloc=eint[idloc]/(1.5*nH[idloc]*KBOLTZ*(1.+x0[idloc]));	  //== Getting a timestep	  cuCompCooling(tloc,x0[idloc],nH[idloc],&Cool,&tcool1,aexp,CLUMPF2);	  ai_tmp1=0.;	  //if(eint[idloc]!=E0) printf("3!/n");	  if(fudgecool<1e-20){	    printf("eint=%e(%e<%e) nH=%e x0=%e(%e) T=%e(%e) N=%e(%e)/n",eint[idloc],eorg,emin,nH[idloc],x0[idloc],xorg,tloc,torg,et[0],etorg);	    if(fudgecool<1e-20) abort();	  }	  for (igrp=0;igrp<NGRP;igrp++) ai_tmp1 += ((alphae[igrp])*hnu[igrp]-(alphai[igrp])*hnu0)*egyloc[idloc+igrp*BLOCKCOOL];	  tcool=FABS(eint[idloc]/(nH[idloc]*(1.0-x0[idloc])*ai_tmp1*(!chemonly)-Cool));	  ai_tmp1=0.;	  dtcool=FMIN(fudgecool*tcool,dt-currentcool_t);	  alpha=cucompute_alpha_a(tloc,1.,1.)*CLUMPF2;	  alphab=cucompute_alpha_b(tloc,1.,1.)*CLUMPF2;	  beta=cucompute_beta(tloc,1.,1.)*CLUMPF2;	  //== Update	  // ABSORPTION	  int test = 0;	  REAL factotsa[NGRP];	  for(igrp=0;igrp<NGRP;igrp++)	      {#ifdef OTSA		factotsa[igrp]=0;		alpha=alphab; // recombination is limited to non ground state levels#else		factotsa[igrp]=(igrp==0);#endif		ai_tmp1 = alphai[igrp];		if(chemonly){		  et[igrp]=egyloc[idloc+igrp*BLOCKCOOL];		}		else{		  et[igrp]=((alpha-alphab)*x0[idloc]*x0[idloc]*nH[idloc]*nH[idloc]*dtcool*factotsa[igrp]+egyloc[idloc+igrp*BLOCKCOOL]+srcloc[idloc+igrp*BLOCKCOOL]*dtcool*factgrp[igrp])/(1.+dtcool*(ai_tmp1*(1.-x0[idloc])*nH[idloc]));		}		if((et[igrp]<0)||(isnan(et[igrp]))){		  test=1;		  //printf("eint=%e nH=%e x0=%e T=%e N=%e/n",eint[idloc],nH[idloc],x0[idloc],tloc,et[0]);

//.........这里部分代码省略.........		      if (pos_j==0 && M->model_properties[state][M->LEN_J])continue;		      if (pos_i==0 && M->model_properties[state][M->LEN_I])continue;		     		     		     t=M->model[M->START][state];		     e=M->model_properties[state][M->TERM_EMISSION];		     Mat   [state*mI+i*mJ+j]=t+e*l;		     LMat  [state*mI+i*mJ+j]=l;		     trace [state*mI+i*mJ+j]=M->START;		    }		}	    }/*DYNAMIC PROGRAMMING: Forward Pass*/		for (i=1; i<=l0;i++)	  {							    for (j=1; j<=ndiag;j++)	      {		pos_j=M->diag[j]-l0+i;		pos_i=i;				if (pos_j<=0 || pos_j>l1 )continue;		last_i=i;		last_j=j;				for (cur_state=0; cur_state<M->START; cur_state++)		  {		    if (M->model_properties[cur_state][M->DELTA_J])		      {			prev_j=j+M->model_properties[cur_state][M->DELTA_J];			prev_i=i+M->model_properties[cur_state][M->DELTA_I]*FABS((M->diag[j]-M->diag[prev_j]));					      }		    else		      {			prev_j=j;			prev_i=i+M->model_properties[cur_state][M->DELTA_I];		      }		    len_i=FABS((i-prev_i));		    len_j=FABS((M->diag[prev_j]-M->diag[j]));		    len=MAX(len_i, len_j);		    a1=A->seq_al[M->model_properties[cur_state][M->F0]  ][pos_i-1];		    a2=A->seq_al[M->model_properties[cur_state][M->F1]+3][pos_j-1];				    if (M->model_properties[cur_state][M->TYPE]==M->CODING0)		      {			if ( a1=='o' || a2=='o')em=-(mat['w'-'A']['w'-'A'])*SCORE_K;			else if (a1=='x' || a2=='x')em=UNDEFINED;			else if ( a1==0 || a2==0)exit (0);			else 			  {			    em=(mat[a1-'A'][a2-'A'])*SCORE_K;			  }		      }		    else		      {			em=M->model_properties[cur_state][M->EMISSION];		      }		    		    		   		    for (pc=best_pc=UNDEFINED, model_index=1; model_index<=M->bounded_model[cur_state][0]; model_index++)		      {			prev_state=M->bounded_model[cur_state][model_index];

/* * This function returns the solution of Ax = b * * The function employs LU decomposition followed by forward/back substitution (see  * also the LAPACK-based LU solver above) * * A is mxm, b is mx1 * * The function returns 0 in case of error, 1 if successful * * This function is often called repetitively to solve problems of identical * dimensions. To avoid repetitive malloc's and free's, allocated memory is * retained between calls and free'd-malloc'ed when not of the appropriate size. * A call with NULL as the first argument forces this memory to be released. */int AX_EQ_B_LU(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m){__STATIC__ void *buf=NULL;__STATIC__ int buf_sz=0;register int i, j, k;int *idx, maxi=-1, idx_sz, a_sz, work_sz, tot_sz;LM_REAL *a, *work, max, sum, tmp;    if(!A)#ifdef LINSOLVERS_RETAIN_MEMORY    {      if(buf) free(buf);      buf=NULL;      buf_sz=0;      return 1;    }#else    return 1; /* NOP */#endif /* LINSOLVERS_RETAIN_MEMORY */     /* calculate required memory size */  idx_sz=m;  a_sz=m*m;  work_sz=m;  tot_sz=(a_sz+work_sz)*sizeof(LM_REAL) + idx_sz*sizeof(int); /* should be arranged in that order for proper doubles alignment */  // Check inputs for validity  for(i=0; i<a_sz; i++)     if (!LM_FINITE(A[i]))        return 0;#ifdef LINSOLVERS_RETAIN_MEMORY  if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */    if(buf) free(buf); /* free previously allocated memory */    buf_sz=tot_sz;    buf=(void *)malloc(tot_sz);    if(!buf){      fprintf(stderr, RCAT("memory allocation in ", AX_EQ_B_LU) "() failed!/n");      exit(1);    }  }#else    buf_sz=tot_sz;    buf=(void *)malloc(tot_sz);    if(!buf){      fprintf(stderr, RCAT("memory allocation in ", AX_EQ_B_LU) "() failed!/n");      exit(1);    }#endif /* LINSOLVERS_RETAIN_MEMORY */  a=(LM_REAL*)buf;  work=a+a_sz;  idx=(int *)(work+work_sz);  /* avoid destroying A, B by copying them to a, x resp. */  memcpy(a, A, a_sz*sizeof(LM_REAL));  memcpy(x, B, m*sizeof(LM_REAL));  /* compute the LU decomposition of a row permutation of matrix a; the permutation itself is saved in idx[] */	for(i=0; i<m; ++i){		max=0.0;		for(j=0; j<m; ++j)			if((tmp=FABS(a[i*m+j]))>max)        max=tmp;		  if(max==0.0){        fprintf(stderr, RCAT("Singular matrix A in ", AX_EQ_B_LU) "()!/n");#ifndef LINSOLVERS_RETAIN_MEMORY        free(buf);#endif        return 0;      }		  work[i]=LM_CNST(1.0)/max;	}	for(j=0; j<m; ++j){		for(i=0; i<j; ++i){			sum=a[i*m+j];			for(k=0; k<i; ++k)        sum-=a[i*m+k]*a[k*m+j];			a[i*m+j]=sum;//.........这里部分代码省略.........

static MRI *MRIcomputeSurfaceDistanceIntensities(MRI_SURFACE *mris,  MRI *mri_ribbon, MRI *mri_aparc, MRI *mri, MRI *mri_aseg, int whalf) {  MRI          *mri_features, *mri_binary, *mri_white_dist, *mri_pial_dist ;  int          vno, ngm, outside_of_ribbon, label0, label, ohemi_label, xi, yi, zi, xk, yk, zk, x0, y0, z0, hemi_label, assignable ;  double       xv, yv, zv, step_size, dist, thickness, wdist, pdist, snx, sny, snz, nx, ny, nz, xl, yl, zl, x, y, z, dot, angle ;  VERTEX       *v ;  mri_features = MRIallocSequence(mris->nvertices, 1, 1, MRI_FLOAT, 1) ;  // one samples inwards, one in ribbon, and one outside  MRIcopyHeader(mri, mri_features) ;  mri_binary = MRIcopy(mri_ribbon, NULL) ;  mri_binary = MRIbinarize(mri_ribbon, NULL, 1, 0, 1) ;  mri_pial_dist = MRIdistanceTransform(mri_binary, NULL, 1, max_pial_dist+1, DTRANS_MODE_SIGNED,NULL);  if (Gdiag & DIAG_WRITE && DIAG_VERBOSE_ON)    MRIwrite(mri_pial_dist, "pd.mgz") ;  MRIclear(mri_binary) ;  MRIcopyLabel(mri_ribbon, mri_binary, Left_Cerebral_White_Matter) ;  MRIcopyLabel(mri_ribbon, mri_binary, Right_Cerebral_White_Matter) ;  MRIbinarize(mri_binary, mri_binary, 1, 0, 1) ;  mri_white_dist = MRIdistanceTransform(mri_binary, NULL, 1, max_white_dist+1, DTRANS_MODE_SIGNED,NULL);  if (Gdiag & DIAG_WRITE && DIAG_VERBOSE_ON)    MRIwrite(mri_white_dist, "wd.mgz") ;  if (mris->hemisphere == LEFT_HEMISPHERE)  {    ohemi_label = Right_Cerebral_Cortex ;    hemi_label = Left_Cerebral_Cortex ;  }  else  {    hemi_label = Right_Cerebral_Cortex ;    ohemi_label = Left_Cerebral_Cortex ;  }  step_size = mri->xsize/2 ;  for (vno = 0 ; vno < mris->nvertices ; vno++)  {    v = &mris->vertices[vno] ;    if (vno == Gdiag_no)      DiagBreak() ;    if (v->ripflag)      continue ;  // not cortex    nx = v->pialx - v->whitex ; ny = v->pialy - v->whitey ; nz = v->pialz - v->whitez ;    thickness = sqrt(nx*nx + ny*ny + nz*nz) ;    if (FZERO(thickness))      continue ;   // no  cortex here    x = (v->pialx + v->whitex)/2 ; y = (v->pialy + v->whitey)/2 ; z = (v->pialz + v->whitez)/2 ;  // halfway between white and pial is x0    MRISsurfaceRASToVoxelCached(mris, mri_aseg, x, y, z, &xl, &yl, &zl) ;    x0 = nint(xl); y0 = nint(yl) ; z0 = nint(zl) ;    label0 = MRIgetVoxVal(mri_aparc, x0, y0, z0,0) ;    // compute surface normal in voxel coords    MRISsurfaceRASToVoxelCached(mris, mri_aseg, x+v->nx, y+v->ny, z+v->nz, &snx, &sny, &snz) ;    snx -= xl ; sny -= yl ; snz -= zl ;    for (ngm = 0, xk = -whalf ; xk <= whalf ; xk++)    {      xi = mri_aseg->xi[x0+xk] ;      for (yk = -whalf ; yk <= whalf ; yk++)      {	yi = mri_aseg->yi[y0+yk] ;	for (zk = -whalf ; zk <= whalf ; zk++)	{	  zi = mri_aseg->zi[z0+zk] ;	  label = MRIgetVoxVal(mri_aseg, xi, yi, zi,0) ;	  if (xi == Gx && yi == Gy && zi == Gz)	    DiagBreak() ;	  if (label != hemi_label)	    continue ;	  label = MRIgetVoxVal(mri_aparc, xi, yi, zi,0) ;	  if (label && label != label0)  // if  outside the ribbon it won't be assigned to a parcel	    continue ;  // constrain it to be in the same cortical parcel	  // search along vector connecting x0 to this point to make sure it is we don't perforate wm or leave and re-enter cortex	  nx = xi-x0 ; ny = yi-y0 ; nz = zi-z0 ;	  thickness = sqrt(nx*nx + ny*ny + nz*nz) ;	  assignable = 1 ;  // assume this point should be counted	  if (thickness > 0)	  {	    nx /= thickness ; ny /= thickness ; nz /= thickness ;	    dot = nx*snx + ny*sny + nz*snz ; angle = acos(dot) ;	    if (FABS(angle) > angle_threshold)	      assignable = 0 ;	    outside_of_ribbon = 0 ;	    for (dist = 0 ; assignable && dist <= thickness ; dist += step_size) 	    {	      xv = x0+nx*dist ;  yv = y0+ny*dist ;  zv = z0+nz*dist ; 	      if (nint(xv) == Gx && nint(yv) == Gy && nint(zv) == Gz)		DiagBreak() ;	      MRIsampleVolume(mri_pial_dist, xv, yv, zv, &pdist) ;	      MRIsampleVolume(mri_white_dist, xv, yv, zv, &wdist) ;	      label = MRIgetVoxVal(mri_aseg, xi, yi, zi,0) ;	      if (SKIP_LABEL(label) || label == ohemi_label)		assignable = 0 ;	      if (wdist < 0)  // entered wm - not assignable		assignable = 0 ;//.........这里部分代码省略.........

//.........这里部分代码省略.........        }        /* J^T e */        for(l=0, tmp=0.0; l<n; ++l)          tmp+=jac[l*m+i]*e[l];        jacTe[i]=tmp;      }    }    else{ // this is a large problem      /* Cache efficient computation of J^T J based on blocking       */      TRANS_MAT_MAT_MULT(jac, jacTjac, n, m);      /* cache efficient computation of J^T e */      for(i=0; i<m; ++i)        jacTe[i]=0.0;      for(i=0; i<n; ++i){        register LM_REAL *jacrow;        for(l=0, jacrow=jac+i*m, tmp=e[i]; l<m; ++l)          jacTe[l]+=jacrow[l]*tmp;      }    }	  /* Compute ||J^T e||_inf and ||p||^2. Note that ||J^T e||_inf     * is computed for free (i.e. inactive) variables only.      * At a local minimum, if p[i]==ub[i] then g[i]>0;     * if p[i]==lb[i] g[i]<0; otherwise g[i]=0      */    for(i=j=numactive=0, p_L2=jacTe_inf=0.0; i<m; ++i){      if(ub && p[i]==ub[i]){ ++numactive; if(jacTe[i]>0.0) ++j; }      else if(lb && p[i]==lb[i]){ ++numactive; if(jacTe[i]<0.0) ++j; }      else if(jacTe_inf < (tmp=FABS(jacTe[i]))) jacTe_inf=tmp;      diag_jacTjac[i]=jacTjac[i*m+i]; /* save diagonal entries so that augmentation can be later canceled */      p_L2+=p[i]*p[i];    }    //p_L2=sqrt(p_L2);#if 0if(!(k%100)){  printf("Current estimate: ");  for(i=0; i<m; ++i)    printf("%.9g ", p[i]);  printf("-- errors %.9g %0.9g, #active %d [%d]/n", jacTe_inf, p_eL2, numactive, j);}#endif    /* check for convergence */    if(j==numactive && (jacTe_inf <= eps1)){      Dp_L2=0.0; /* no increment for p in this case */      stop=1;      break;    }   /* compute initial damping factor */    if(k==0){      if(!lb && !ub){ /* no bounds */        for(i=0, tmp=LM_REAL_MIN; i<m; ++i)          if(diag_jacTjac[i]>tmp) tmp=diag_jacTjac[i]; /* find max diagonal element */        mu=tau*tmp;      }      else         mu=CNST(0.5)*tau*p_eL2; /* use Kanzow's starting mu */    }

static voidLNSRCH(int m, LM_REAL *x, LM_REAL f, LM_REAL *g, LM_REAL *p, LM_REAL alpha, LM_REAL *xpls,       LM_REAL *ffpls, void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata), struct FUNC_STATE state,       int *mxtake, int *iretcd, LM_REAL stepmx, LM_REAL steptl, LM_REAL *sx){/* Find a next newton iterate by backtracking line search. * Specifically, finds a /lambda such that for a fixed alpha<0.5 (usually 1e-4), * f(x + /lambda*p) <= f(x) + alpha * /lambda * g^T*p * * Translated (with minor changes) from Schnabel, Koontz & Weiss uncmin.f,  v1.3 * PARAMETERS : *	m       --> dimension of problem (i.e. number of variables) *	x(m)    --> old iterate:	x[k-1] *	f       --> function value at old iterate, f(x) *	g(m)    --> gradient at old iterate, g(x), or approximate *	p(m)    --> non-zero newton step *	alpha   --> fixed constant < 0.5 for line search (see above) *	xpls(m) <--	 new iterate x[k] *	ffpls   <--	 function value at new iterate, f(xpls) *	func    --> name of subroutine to evaluate function *	state   <--> information other than x and m that func requires. *			    state is not modified in xlnsrch (but can be modified by func). *	iretcd  <--	 return code *	mxtake  <--	 boolean flag indicating step of maximum length used *	stepmx  --> maximum allowable step size *	steptl  --> relative step size at which successive iterates *			    considered close enough to terminate algorithm *	sx(m)	  --> diagonal scaling matrix for x, can be NULL *	internal variables *	sln		 newton length *	rln		 relative length of newton step*/    register int i;    int firstback = 1;    LM_REAL disc;    LM_REAL a3, b;    LM_REAL t1, t2, t3, lambda, tlmbda, rmnlmb;    LM_REAL scl, rln, sln, slp;    LM_REAL tmp1, tmp2;    LM_REAL fpls, pfpls = 0., plmbda = 0.; /* -Wall */    f*=CNST(0.5);    *mxtake = 0;    *iretcd = 2;    tmp1 = 0.;    if(!sx) /* no scaling */      for (i = 0; i < m; ++i)        tmp1 += p[i] * p[i];    else      for (i = 0; i < m; ++i)        tmp1 += sx[i] * sx[i] * p[i] * p[i];    sln = (LM_REAL)sqrt(tmp1);    if (sln > stepmx) {	  /*	newton step longer than maximum allowed */	    scl = stepmx / sln;      for(i=0; i<m; ++i) /* p * scl */        p[i]*=scl;	    sln = stepmx;    }    for(i=0, slp=0.; i<m; ++i) /* g^T * p */      slp+=g[i]*p[i];    rln = 0.;    if(!sx) /* no scaling */      for (i = 0; i < m; ++i) {	      tmp1 = (FABS(x[i])>=CNST(1.))? FABS(x[i]) : CNST(1.);	      tmp2 = FABS(p[i])/tmp1;	      if(rln < tmp2) rln = tmp2;      }    else      for (i = 0; i < m; ++i) {	      tmp1 = (FABS(x[i])>=CNST(1.)/sx[i])? FABS(x[i]) : CNST(1.)/sx[i];	      tmp2 = FABS(p[i])/tmp1;	      if(rln < tmp2) rln = tmp2;      }    rmnlmb = steptl / rln;    lambda = CNST(1.0);    /*	check if new iterate satisfactory.  generate new lambda if necessary. */    while(*iretcd > 1) {	    for (i = 0; i < m; ++i)	      xpls[i] = x[i] + lambda * p[i];      /* evaluate function at new point */      (*func)(xpls, state.hx, m, state.n, state.adata);      for(i=0, tmp1=0.0; i<state.n; ++i){        state.hx[i]=tmp2=state.x[i]-state.hx[i];        tmp1+=tmp2*tmp2;      }      fpls=CNST(0.5)*tmp1; *ffpls=tmp1; ++(*(state.nfev));	    if (fpls <= f + slp * alpha * lambda) { /* solution found */	      *iretcd = 0;	      if (lambda == CNST(1.) && sln > stepmx * CNST(.99)) *mxtake = 1;	      return;//.........这里部分代码省略.........

int NoDivTriTriIsect(double V0[3],double V1[3],double V2[3],                     double U0[3],double U1[3],double U2[3]){  double E1[3],E2[3];  double N1[3],N2[3],d1,d2;  double du0,du1,du2,dv0,dv1,dv2;  double D[3];  double isect1[2], isect2[2];  double du0du1,du0du2,dv0dv1,dv0dv2;  short index;  double vp0,vp1,vp2;  double up0,up1,up2;  double bb,cc,max;  double a,b,c,x0,x1;  double d,e,f,y0,y1;  double xx,yy,xxyy,tmp;  /* compute plane equation of triangle(V0,V1,V2) */  SUB(E1,V1,V0);  SUB(E2,V2,V0);  CROSS(N1,E1,E2);  d1=-DOT(N1,V0);  /* plane equation 1: N1.X+d1=0 */  /* put U0,U1,U2 into plane equation 1 to compute signed distances to the plane*/  du0=DOT(N1,U0)+d1;  du1=DOT(N1,U1)+d1;  du2=DOT(N1,U2)+d1;  /* coplanarity robustness check */#if USE_EPSILON_TEST==TRUE  if(FABS(du0)<EPSILON) du0=0.0;  if(FABS(du1)<EPSILON) du1=0.0;  if(FABS(du2)<EPSILON) du2=0.0;#endif  du0du1=du0*du1;  du0du2=du0*du2;  if(du0du1>0.0f && du0du2>0.0f) /* same sign on all of them + not equal 0 ? */    return 0;                    /* no intersection occurs */  /* compute plane of triangle (U0,U1,U2) */  SUB(E1,U1,U0);  SUB(E2,U2,U0);  CROSS(N2,E1,E2);  d2=-DOT(N2,U0);  /* plane equation 2: N2.X+d2=0 */  /* put V0,V1,V2 into plane equation 2 */  dv0=DOT(N2,V0)+d2;  dv1=DOT(N2,V1)+d2;  dv2=DOT(N2,V2)+d2;#if USE_EPSILON_TEST==TRUE  if(FABS(dv0)<EPSILON) dv0=0.0;  if(FABS(dv1)<EPSILON) dv1=0.0;  if(FABS(dv2)<EPSILON) dv2=0.0;#endif  dv0dv1=dv0*dv1;  dv0dv2=dv0*dv2;  if(dv0dv1>0.0f && dv0dv2>0.0f) /* same sign on all of them + not equal 0 ? */    return 0;                    /* no intersection occurs */  /* compute direction of intersection line */  CROSS(D,N1,N2);  /* compute and index to the largest component of D */  max=(double)FABS(D[0]);  index=0;  bb=(double)FABS(D[1]);  cc=(double)FABS(D[2]);  if(bb>max) max=bb,index=1;  if(cc>max) max=cc,index=2;  /* this is the simplified projection onto L*/  vp0=V0[index];  vp1=V1[index];  vp2=V2[index];  up0=U0[index];  up1=U1[index];  up2=U2[index];  /* compute interval for triangle 1 */  NEWCOMPUTE_INTERVALS(vp0,vp1,vp2,dv0,dv1,dv2,dv0dv1,dv0dv2,a,b,c,x0,x1);  /* compute interval for triangle 2 */  NEWCOMPUTE_INTERVALS(up0,up1,up2,du0,du1,du2,du0du1,du0du2,d,e,f,y0,y1);  xx=x0*x1;  yy=y0*y1;  xxyy=xx*yy;  tmp=a*xxyy;  isect1[0]=tmp+b*x1*yy;  isect1[1]=tmp+c*x0*yy;  tmp=d*xxyy;//.........这里部分代码省略.........

//.........这里部分代码省略.........    /* Iterate through the play history and calculate the signal correlation     * for every tail position in the play_hist. Save the result in temporary     * array since we may bail out early if the conversation state is not good     * to detect echo.     */    /*      * First phase: do full calculation for the first position      */    if (ec->sum_play_level0 == 0) {	/* Buffer has just been filled up, do full calculation */	sum_play_level = 0;	play_corr = 0;	for (j=0; j<ec->templ_cnt-1; ++j) {	    float corr;	    corr = (float)ec->play_hist[j+1] / ec->play_hist[j];	    play_corr += corr;	    sum_play_level += ec->play_hist[j];	}	sum_play_level += ec->play_hist[j];	ec->sum_play_level0 = sum_play_level;	ec->play_corr0 = play_corr;    } else {	/* Update from previous calculation */	ec->sum_play_level0 = ec->sum_play_level0 - old_play_frm_level + 			      ec->play_hist[ec->templ_cnt-1];	ec->play_corr0 = ec->play_corr0 - ((float)ec->play_hist[0] / 					          old_play_frm_level) +		         ((float)ec->play_hist[ec->templ_cnt-1] /			         ec->play_hist[ec->templ_cnt-2]);	sum_play_level = ec->sum_play_level0;	play_corr = ec->play_corr0;    }    ec->tmp_corr[0] = FABS(play_corr - ec->rec_corr);    ec->tmp_factor[0] = (float)ec->sum_rec_level / sum_play_level;    /* Bail out if remote isn't talking */    ulaw = pjmedia_linear2ulaw(sum_play_level/ec->templ_cnt) ^ 0xFF;    if (ulaw < MIN_SIGNAL_ULAW) {	echo_supp_set_state(ec, ST_REM_SILENT, ulaw);	return;    }    /* Bail out if local user is talking */    if (ec->sum_rec_level >= sum_play_level) {	echo_supp_set_state(ec, ST_LOCAL_TALK, ulaw);	return;    }    /*     * Second phase: do incremental calculation for the rest of positions     */    for (i=1; i < ec->tail_cnt; ++i) {	unsigned end;	end = i + ec->templ_cnt;	sum_play_level = sum_play_level - ec->play_hist[i-1] +			 ec->play_hist[end-1];	play_corr = play_corr - ((float)ec->play_hist[i]/ec->play_hist[i-1]) +		    ((float)ec->play_hist[end-1]/ec->play_hist[end-2]);	/* Bail out if remote isn't talking */	ulaw = pjmedia_linear2ulaw(sum_play_level/ec->templ_cnt) ^ 0xFF;	if (ulaw < MIN_SIGNAL_ULAW) {	    echo_supp_set_state(ec, ST_REM_SILENT, ulaw);	    return;

static void Loc_cbInfo( LocState *pts ) {		if( pts->theInfo.status == AEEGPS_ERR_NO_ERR 		|| (pts->theInfo.status == AEEGPS_ERR_INFO_UNAVAIL && pts->theInfo.fValid) ) {		#if MIN_BREW_VERSION(2,1)		pts->pResp->lat = WGS84_TO_DEGREES( pts->theInfo.dwLat );#ifdef AEE_SIMULATOR		//FOR TEST		pts->pResp->lon = -WGS84_TO_DEGREES( pts->theInfo.dwLon );#else		pts->pResp->lon = WGS84_TO_DEGREES( pts->theInfo.dwLon );#endif#else		double    wgsFactor;		wgsFactor = FASSIGN_STR("186413.5111");		pts->pResp->lat = FASSIGN_INT(pts->theInfo.dwLat);		pts->pResp->lat = FDIV(pts->pResp->lat, wgsFactor);				pts->pResp->lon = FASSIGN_INT(pts->theInfo.dwLon);		pts->pResp->lon = FDIV(pts->pResp->lon, wgsFactor);#endif /* MIN_BREW_VERSION 2.1 */				pts->pResp->height = pts->theInfo.wAltitude - 500;		pts->pResp->velocityHor = FMUL( pts->theInfo.wVelocityHor,0.25);				//当前夹角		if (FCMP_G(FABS(pts->lastCoordinate.lat), 0))		{			pts->pResp->heading = Loc_Calc_Azimuth(pts->lastCoordinate.lat, pts->lastCoordinate.lon, pts->pResp->lat, pts->pResp->lon);		}		else		{			pts->pResp->heading = 0;		}		//For Test Hack#ifdef AEE_SIMULATOR		pts->pResp->lat = 38.0422378880;		pts->pResp->lon = 114.4925141047;#endif		if (pts->pResp->bSetDestPos)		{			//计算距离和方位角			pts->pResp->distance = Loc_Calc_Distance(pts->pResp->lat, pts->pResp->lon, pts->pResp->destPos.lat, pts->pResp->destPos.lon);			pts->pResp->destHeading = Loc_Calc_Azimuth(pts->pResp->lat, pts->pResp->lon, pts->pResp->destPos.lat, pts->pResp->destPos.lon);		}				//记录历史定位信息		pts->lastCoordinate.lat = pts->pResp->lat;		pts->lastCoordinate.lon = pts->pResp->lon;				pts->pResp->dwFixNum++;				pts->pResp->nErr = SUCCESS;				Loc_Notify( pts );				if( FALSE == pts->bSetForCancellation ) {						ISHELL_SetTimerEx( pts->pShell, pts->nLocInterval * 1000, &pts->cbIntervalTimer );		}		else {						Loc_Stop( pts );		}	}}

//.........这里部分代码省略.........        ASSERT (errno == 0 || errno == EINVAL);    }    /* Simple floating point values.  */    {        const char input[] = "1";        char *ptr;        double result;        errno = 0;        result = strtod (input, &ptr);        ASSERT (result == 1.0);        ASSERT (ptr == input + 1);        ASSERT (errno == 0);    }    {        const char input[] = "1.";        char *ptr;        double result;        errno = 0;        result = strtod (input, &ptr);        ASSERT (result == 1.0);        ASSERT (ptr == input + 2);        ASSERT (errno == 0);    }    {        const char input[] = ".5";        char *ptr;        double result;        errno = 0;        result = strtod (input, &ptr);        /* FIXME - gnulib's version is rather inaccurate.  It would be           nice to guarantee an exact result, but for now, we settle for a           1-ulp error.  */        ASSERT (FABS (result - 0.5) < DBL_EPSILON);        ASSERT (ptr == input + 2);        ASSERT (errno == 0);    }    {        const char input[] = " 1";        char *ptr;        double result;        errno = 0;        result = strtod (input, &ptr);        ASSERT (result == 1.0);        ASSERT (ptr == input + 2);        ASSERT (errno == 0);    }    {        const char input[] = "+1";        char *ptr;        double result;        errno = 0;        result = strtod (input, &ptr);        ASSERT (result == 1.0);        ASSERT (ptr == input + 2);        ASSERT (errno == 0);    }    {        const char input[] = "-1";        char *ptr;        double result;        errno = 0;        result = strtod (input, &ptr);        ASSERT (result == -1.0);        ASSERT (ptr == input + 2);        ASSERT (errno == 0);

//.........这里部分代码省略.........		if (!insert_ob_in_map(throw_ob, op->map, op, 0))		{			return;		}		if (op->type == PLAYER)		{			if (!dir)			{				new_draw_info_format(NDI_UNIQUE, op, "You drop %s at the ground.", query_name(throw_ob, NULL));			}			else			{				new_draw_info(NDI_UNIQUE, op, "Something is in the way.");			}		}		return;	}	set_owner(throw_ob, op);	set_owner(throw_ob->inv, op);	throw_ob->direction = dir;	throw_ob->x = op->x;	throw_ob->y = op->y;	/* Save original wc and dam */	throw_ob->last_heal = throw_ob->stats.wc;	throw_ob->stats.hp = throw_ob->stats.dam;	/* Speed */	throw_ob->speed = MIN(1.0f, (speed_bonus[op->stats.Str] + 1.0f) / 1.5f);	/* Now we get the wc from the used skill. */	if ((tmp_op = SK_skill(op)))	{		throw_ob->stats.wc += tmp_op->last_heal;	}	/* Monsters */	else	{		throw_ob->stats.wc += 10;	}	throw_ob->stats.wc_range = op->stats.wc_range;	if (QUERY_FLAG(throw_ob, FLAG_IS_THROWN))	{		throw_ob->stats.dam += throw_ob->magic;		throw_ob->stats.wc += throw_ob->magic;		/* Adjust for players */		if (op->type == PLAYER)		{			op->chosen_skill->stats.maxsp = throw_ob->last_grace;			throw_ob->stats.dam = FABS((int) ((float) (throw_ob->stats.dam + dam_bonus[op->stats.Str] / 2) * LEVEL_DAMAGE(SK_level(op))));			throw_ob->stats.wc += thaco_bonus[op->stats.Dex] + SK_level(op);		}		else		{			throw_ob->stats.dam = FABS((int) ((float) (throw_ob->stats.dam) * LEVEL_DAMAGE(op->level)));			throw_ob->stats.wc += 10 + op->level;		}		throw_ob->stats.grace = throw_ob->last_sp;		throw_ob->stats.maxgrace = 60 + (RANDOM() % 12);		/* Only throw objects get directional faces */		if (GET_ANIM_ID(throw_ob) && NUM_ANIMATIONS(throw_ob))		{			SET_ANIMATION(throw_ob, (NUM_ANIMATIONS(throw_ob) / NUM_FACINGS(throw_ob)) * dir);		}		/* Adjust damage with item condition */		throw_ob->stats.dam = (sint16) (((float) throw_ob->stats.dam / 100.0f) * (float) throw_ob->item_condition);	}	if (throw_ob->stats.dam < 0)	{		throw_ob->stats.dam = 0;	}	update_ob_speed(throw_ob);	throw_ob->speed_left = 0;	SET_MULTI_FLAG(throw_ob, FLAG_FLYING);	SET_FLAG(throw_ob, FLAG_FLY_ON);	SET_FLAG(throw_ob, FLAG_WALK_ON);	play_sound_map(op->map, CMD_SOUND_EFFECT, "throw.ogg", op->x, op->y, 0, 0);	/* Trigger the THROW event */	trigger_event(EVENT_THROW, op, throw_ob, NULL, NULL, 0, 0, 0, SCRIPT_FIX_ACTIVATOR);	if (insert_ob_in_map(throw_ob, op->map, op, 0))	{		move_arrow(throw_ob);	}}

/* Given a parameter vector p made up of the 3D coordinates of n points, compute in * jac the jacobian of the predicted measurements, i.e. the jacobian of the projections of 3D points in the m images. * The jacobian is approximated with the aid of finite differences and is returned in the order * (B_11, ..., B_1m, ..., B_n1, ..., B_nm), where B_ij=dx_ij/db_i (see HZ). * Notice that depending on idxij, some of the B_ij might be missing * * Problem-specific information is assumed to be stored in a structure pointed to by "dat". * * NOTE: This function is provided mainly for illustration purposes; in case that execution time is a concern, * the jacobian should be computed analytically */static void sba_str_Qs_fdjac(    double *p,                /* I: current parameter estimate, (n*pnp)x1 */    struct sba_crsm *idxij,   /* I: sparse matrix containing the location of x_ij in hx */    int    *rcidxs,           /* work array for the indexes of nonzero elements of a single sparse matrix row/column */    int    *rcsubs,           /* work array for the subscripts of nonzero elements in a single sparse matrix row/column */    double *jac,              /* O: array for storing the approximated jacobian */    void   *dat)              /* I: points to a "wrap_str_data_" structure */{  register int i, j, ii, jj;  double *pbi;  register double *pB;  //int m;  int n, nnz, Bsz;  double tmp;  register double d, d1;  struct wrap_str_data_ *fdjd;  void (*proj)(int j, int i, double *bi, double *xij, void *adata);  double *hxij, *hxxij;  int pnp, mnp;  void *adata;  /* retrieve problem-specific information passed in *dat */  fdjd=(struct wrap_str_data_ *)dat;  proj=fdjd->proj;  pnp=fdjd->pnp; mnp=fdjd->mnp;  adata=fdjd->adata;  n=idxij->nr;  //m=idxij->nc;  Bsz=mnp*pnp;  /* allocate memory for hxij, hxxij */  if((hxij=malloc(2*mnp*sizeof(double)))==NULL){    fprintf(stderr, "memory allocation request failed in sba_str_Qs_fdjac()!/n");    exit(1);  }  hxxij=hxij+mnp;  /* compute B_ij */  for(i=0; i<n; ++i){    pbi=p+i*pnp; // i-th point parameters    nnz=sba_crsm_row_elmidxs(idxij, i, rcidxs, rcsubs); /* find nonzero B_ij, j=0...m-1 */    for(jj=0; jj<pnp; ++jj){      /* determine d=max(SBA_DELTA_SCALE*|pbi[jj]|, SBA_MIN_DELTA), see HZ */      d=(double)(SBA_DELTA_SCALE)*pbi[jj]; // force evaluation      d=FABS(d);      if(d<SBA_MIN_DELTA) d=SBA_MIN_DELTA;      d1=1.0/d; /* invert so that divisions can be carried out faster as multiplications */      for(j=0; j<nnz; ++j){        (*proj)(rcsubs[j], i, pbi, hxij, adata); // evaluate supplied function on current solution        tmp=pbi[jj];        pbi[jj]+=d;        (*proj)(rcsubs[j], i, pbi, hxxij, adata);        pbi[jj]=tmp; /* restore */        pB=jac + idxij->val[rcidxs[j]]*Bsz; // set pB to point to B_ij        for(ii=0; ii<mnp; ++ii)          pB[ii*pnp+jj]=(hxxij[ii]-hxij[ii])*d1;      }    }  }  free(hxij);}

/** * Process objects with speed, like teleporters, players, etc. */voidprocess_events (void){    object *op;    tag_t tag;    /* Put marker object at beginning of active list */    marker.active_next = active_objects;    if (marker.active_next) {        marker.active_next->active_prev = &marker;    }    marker.active_prev = NULL;    active_objects = &marker;    while (marker.active_next) {        op = marker.active_next;        tag = op->count;        /* Move marker forward - swap op and marker */        op->active_prev = marker.active_prev;        if (op->active_prev) {            op->active_prev->active_next = op;        } else {            active_objects = op;        }        marker.active_next = op->active_next;        if (marker.active_next) {            marker.active_next->active_prev = &marker;        }        marker.active_prev = op;        op->active_next = &marker;        /* Now process op */        if (unlikely(OBJECT_FREE(op))) {            LOG(ERROR, "Free object on active list");            op->speed = 0;            object_update_speed(op);            continue;        }        if (unlikely(QUERY_FLAG(op, FLAG_REMOVED))) {            /*             * This is not actually an error; object_remove() doesn't remove             * active objects from the active list, since the two most common             * next steps are to either: re-insert the object elsewhere (for             * which we would have to re-add it to the active list), or destroy             * the object altogether (which does remove it from the active             * list).             *             * For now, just drop a DEVEL message about this case, so we can             * get a better idea of the objects that rely on this behavior.             */            LOG(DEVEL, "Removed object on active list: %s", object_get_str(op));            op->speed = 0;            object_update_speed(op);            continue;        }        if (unlikely(DBL_EQUAL(op->speed, 0.0))) {            LOG(ERROR, "Object has no speed, but is on active list: %s",                object_get_str(op));            object_update_speed(op);            continue;        }        if (unlikely(op->map == NULL && op->env == NULL)) {            LOG(ERROR, "Object without map or inventory is on active list: %s",                object_get_str(op));            op->speed = 0;            object_update_speed(op);            continue;        }        /* As long we are > 0, we are not ready to swing. */        if (op->weapon_speed_left > 0) {            op->weapon_speed_left -= op->weapon_speed;        }        if (op->speed_left <= 0) {            op->speed_left += FABS(op->speed);        }        if (op->type == PLAYER && op->speed_left > op->speed) {            op->speed_left = op->speed;        }        if (op->speed_left >= 0 || op->type == PLAYER) {            if (op->type != PLAYER) {                --op->speed_left;            }//.........这里部分代码省略.........

static double evaluateGAMMA_FLEX(int *wptr,				 double *x1_start, double *x2_start, 				 double *tipVector, 				 unsigned char *tipX1, const int n, double *diagptable, const int states){  double       sum = 0.0,     term,    *x1,    *x2;  int         i,     j,    k;  /* span is the offset within the likelihood array at an inner node that gets us from the values      of site i to the values of site i + 1 */  const int     span = states * 4;  /* we distingusih between two cases here: one node of the two nodes defining the branch at which we put the virtual root is      a tip. Both nodes can not be tips because we do not allow for two-taxon trees ;-)      Nota that, if a node is a tip, this will always be tipX1. This is done for code simplicity and the flipping of the nodes     is done before when we compute the traversal descriptor.       */  /* the left node is a tip */  if(tipX1)    {          	      /* loop over the sites of this partition */      for (i = 0; i < n; i++)	{	  /* access pre-computed tip vector values via a lookup table */	  x1 = &(tipVector[states * tipX1[i]]);	 	  /* access the other(inner) node at the other end of the branch */	  x2 = &(x2_start[span * i]);	 	  	  /* loop over GAMMA rate categories, hard-coded as 4 in RAxML */	  for(j = 0, term = 0.0; j < 4; j++)	    /* loop over states and multiply them with the P matrix */	    for(k = 0; k < states; k++)	      term += x1[k] * x2[j * states + k] * diagptable[j * states + k];	          	  	  	    	    	  	  	 	  /* take the log of the likelihood and multiply the per-gamma rate likelihood by 1/4.	     Under the GAMMA model the 4 discrete GAMMA rates all have the same probability 	     of 0.25 */	  term = LOG(0.25 * FABS(term));	 	 	  	  sum += wptr[i] * term;	}         }  else    {              for (i = 0; i < n; i++) 	{	  /* same as before, only that now we access two inner likelihood vectors x1 and x2 */	  	 	  	  	  x1 = &(x1_start[span * i]);	  x2 = &(x2_start[span * i]);	  	  		  for(j = 0, term = 0.0; j < 4; j++)	    for(k = 0; k < states; k++)	      term += x1[j * states + k] * x2[j * states + k] * diagptable[j * states + k];	          	  	  	      	  	  term = LOG(0.25 * FABS(term));	  	  	  sum += wptr[i] * term;	}                      	    }  return sum;} 

/* Similar to the LEVMAR_LEC_DER() function above, except that the jacobian is approximated * with the aid of finite differences (forward or central, see the comment for the opts argument) */int LEVMAR_LEC_DIF(  void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata), /* functional relation describing measurements. A p /in R^m yields a /hat{x} /in  R^n */  LM_REAL *p,         /* I/O: initial parameter estimates. On output has the estimated solution */  LM_REAL *x,         /* I: measurement vector */  int m,              /* I: parameter vector dimension (i.e. #unknowns) */  int n,              /* I: measurement vector dimension */  LM_REAL *A,         /* I: constraints matrix, kxm */  LM_REAL *b,         /* I: right hand constraints vector, kx1 */  int k,              /* I: number of contraints (i.e. A's #rows) */  int itmax,          /* I: maximum number of iterations */  LM_REAL opts[5],    /* I: opts[0-3] = minim. options [/mu, /epsilon1, /epsilon2, /epsilon3, /delta]. Respectively the                       * scale factor for initial /mu, stopping thresholds for ||J^T e||_inf, ||Dp||_2 and ||e||_2 and                       * the step used in difference approximation to the jacobian. Set to NULL for defaults to be used.                       * If /delta<0, the jacobian is approximated with central differences which are more accurate                       * (but slower!) compared to the forward differences employed by default.                        */  LM_REAL info[LM_INFO_SZ],					           /* O: information regarding the minimization. Set to NULL if don't care                      * info[0]= ||e||_2 at initial p.                      * info[1-4]=[ ||e||_2, ||J^T e||_inf,  ||Dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.                      * info[5]= # iterations,                      * info[6]=reason for terminating: 1 - stopped by small gradient J^T e                      *                                 2 - stopped by small Dp                      *                                 3 - stopped by itmax                      *                                 4 - singular matrix. Restart from current p with increased mu                       *                                 5 - no further error reduction is possible. Restart with increased mu                      *                                 6 - stopped by small ||e||_2                      * info[7]= # function evaluations                      * info[8]= # jacobian evaluations                      */  LM_REAL *work,     /* working memory, allocate if NULL */  LM_REAL *covar,    /* O: Covariance matrix corresponding to LS solution; mxm. Set to NULL if not needed. */  void *adata)       /* pointer to possibly additional data, passed uninterpreted to func.                      * Set to NULL if not needed                      */{  struct LMLEC_DATA data;  LM_REAL *ptr, *Z, *pp, *p0, *Zimm; /* Z is mxmm */  int mm, ret;  register int i, j;  register LM_REAL tmp;  LM_REAL locinfo[LM_INFO_SZ];  mm=m-k;  ptr=(LM_REAL *)malloc((2*m + m*mm + mm)*sizeof(LM_REAL));  if(!ptr){    fprintf(stderr, LCAT(LEVMAR_LEC_DIF, "(): memory allocation request failed/n"));    exit(1);  }  data.p=p;  p0=ptr;  data.c=p0+m;  data.Z=Z=data.c+m;  data.jac=NULL;  pp=data.Z+m*mm;  data.ncnstr=k;  data.func=func;  data.jacf=NULL;  data.adata=adata;  LMLEC_ELIM(A, b, data.c, NULL, Z, k, m); // compute c, Z  /* compute pp s.t. p = c + Z*pp or (Z^T Z)*pp=Z^T*(p-c)   * Due to orthogonality, Z^T Z = I and the last equation   * becomes pp=Z^T*(p-c). Also, save the starting p in p0   */  for(i=0; i<m; ++i){    p0[i]=p[i];    p[i]-=data.c[i];  }  /* Z^T*(p-c) */  for(i=0; i<mm; ++i){    for(j=0, tmp=0.0; j<m; ++j)      tmp+=Z[j*mm+i]*p[j];    pp[i]=tmp;  }  /* compute the p corresponding to pp (i.e. c + Z*pp) and compare with p0 */  for(i=0; i<m; ++i){    Zimm=Z+i*mm;    for(j=0, tmp=data.c[i]; j<mm; ++j)      tmp+=Zimm[j]*pp[j]; // tmp+=Z[i*mm+j]*pp[j];    if(FABS(tmp-p0[i])>CNST(1E-03))      fprintf(stderr, RCAT("Warning: component %d of starting point not feasible in ", LEVMAR_LEC_DIF) "()! [%.10g reset to %.10g]/n",                      i, p0[i], tmp);  }  if(!info) info=locinfo; /* make sure that LEVMAR_DIF() is called with non-null info */  /* note that covariance computation is not requested from LEVMAR_DIF() */  ret=LEVMAR_DIF(LMLEC_FUNC, pp, x, mm, n, itmax, opts, info, work, NULL, (void *)&data);  /* p=c + Z*pp */  for(i=0; i<m; ++i){    Zimm=Z+i*mm;    for(j=0, tmp=data.c[i]; j<mm; ++j)//.........这里部分代码省略.........

static double evaluateGTRGAMMAPROT_GAPPED_SAVE (int *wptr,						double *x1, double *x2,  						double *tipVector, 						unsigned char *tipX1, int n, double *diagptable, 						double *x1_gapColumn, double *x2_gapColumn, unsigned int *x1_gap, unsigned int *x2_gap)					   {  double   sum = 0.0, term;          int     i, j, l;     double      *left,     *right,    *x1_ptr = x1,    *x2_ptr = x2,    *x1v,    *x2v;                  if(tipX1)    {                     for (i = 0; i < n; i++) 	{	  if(x2_gap[i / 32] & mask32[i % 32])	    x2v = x2_gapColumn;	  else	    {	      x2v = x2_ptr;	      x2_ptr += 80;	    }	  __m128d tv = _mm_setzero_pd();	  left = &(tipVector[20 * tipX1[i]]);	  	  	  	  for(j = 0, term = 0.0; j < 4; j++)	    {	      double *d = &diagptable[j * 20];	      right = &(x2v[20 * j]);	      for(l = 0; l < 20; l+=2)		{		  __m128d mul = _mm_mul_pd(_mm_load_pd(&left[l]), _mm_load_pd(&right[l]));		  tv = _mm_add_pd(tv, _mm_mul_pd(mul, _mm_load_pd(&d[l])));		   		}		 			    }	  tv = _mm_hadd_pd(tv, tv);	  _mm_storel_pd(&term, tv);	  	  	  term = LOG(0.25 * FABS(term));	  	  	  sum += wptr[i] * term;	}    	            }                else    {      for (i = 0; i < n; i++) 	{	  if(x1_gap[i / 32] & mask32[i % 32])	    x1v = x1_gapColumn;	  else	    {	      x1v = x1_ptr;	      x1_ptr += 80;	    }	  if(x2_gap[i / 32] & mask32[i % 32])	    x2v = x2_gapColumn;	  else	    {	      x2v = x2_ptr;	      x2_ptr += 80;	    }	  	 	             	  __m128d tv = _mm_setzero_pd();	 	  	  	      	  for(j = 0, term = 0.0; j < 4; j++)	    {	      double *d = &diagptable[j * 20];	      left  = &(x1v[20 * j]);	      right = &(x2v[20 * j]);	      	      for(l = 0; l < 20; l+=2)		{		  __m128d mul = _mm_mul_pd(_mm_load_pd(&left[l]), _mm_load_pd(&right[l]));		  tv = _mm_add_pd(tv, _mm_mul_pd(mul, _mm_load_pd(&d[l])));		   		}		 			    }	  tv = _mm_hadd_pd(tv, tv);	  _mm_storel_pd(&term, tv);	  	  	 	  term = LOG(0.25 * FABS(term));		  	  sum += wptr[i] * term;	}             }         return  sum;}

/* * This function implements an elimination strategy for linearly constrained * optimization problems. The strategy relies on QR decomposition to transform * an optimization problem constrained by Ax=b to an equivalent, unconstrained * one. Also referred to as "null space" or "reduced Hessian" method. * See pp. 430-433 (chap. 15) of "Numerical Optimization" by Nocedal-Wright * for details. * * A is mxn with m<=n and rank(A)=m * Two matrices Y and Z of dimensions nxm and nx(n-m) are computed from A^T so that * their columns are orthonormal and every x can be written as x=Y*b + Z*x_z= * c + Z*x_z, where c=Y*b is a fixed vector of dimension n and x_z is an * arbitrary vector of dimension n-m. Then, the problem of minimizing f(x) * subject to Ax=b is equivalent to minimizing f(c + Z*x_z) with no constraints. * The computed Y and Z are such that any solution of Ax=b can be written as * x=Y*x_y + Z*x_z for some x_y, x_z. Furthermore, A*Y is nonsingular, A*Z=0 * and Z spans the null space of A. * * The function accepts A, b and computes c, Y, Z. If b or c is NULL, c is not * computed. Also, Y can be NULL in which case it is not referenced. * The function returns 0 in case of error, A's computed rank if successfull * */static int LMLEC_ELIM(LM_REAL *A, LM_REAL *b, LM_REAL *c, LM_REAL *Y, LM_REAL *Z, int m, int n){static LM_REAL eps=CNST(-1.0);LM_REAL *buf=NULL;LM_REAL *a, *tau, *work, *r;register LM_REAL tmp;int a_sz, jpvt_sz, tau_sz, r_sz, Y_sz, worksz;int info, rank, *jpvt, tot_sz, mintmn, tm, tn;register int i, j, k;  if(m>n){    fprintf(stderr, RCAT("matrix of constraints cannot have more rows than columns in", LMLEC_ELIM) "()!/n");    exit(1);  }  tm=n; tn=m; // transpose dimensions  mintmn=m;  /* calculate required memory size */  a_sz=tm*tm; // tm*tn is enough for xgeqp3()  jpvt_sz=tn;  tau_sz=mintmn;  r_sz=mintmn*mintmn; // actually smaller if a is not of full row rank  worksz=2*tn+(tn+1)*32; // more than needed  Y_sz=(Y)? 0 : tm*tn;  tot_sz=jpvt_sz*sizeof(int) + (a_sz + tau_sz + r_sz + worksz + Y_sz)*sizeof(LM_REAL);  buf=(LM_REAL *)malloc(tot_sz); /* allocate a "big" memory chunk at once */  if(!buf){    fprintf(stderr, RCAT("Memory allocation request failed in ", LMLEC_ELIM) "()/n");    exit(1);  }  a=(LM_REAL *)buf;  jpvt=(int *)(a+a_sz);  tau=(LM_REAL *)(jpvt + jpvt_sz);  r=tau+tau_sz;  work=r+r_sz;  if(!Y) Y=work+worksz;  /* copy input array so that LAPACK won't destroy it. Note that copying is   * done in row-major order, which equals A^T in column-major   */  for(i=0; i<tm*tn; ++i)      a[i]=A[i];  /* clear jpvt */  for(i=0; i<jpvt_sz; ++i) jpvt[i]=0;  /* rank revealing QR decomposition of A^T*/  GEQP3((int *)&tm, (int *)&tn, a, (int *)&tm, jpvt, tau, work, (int *)&worksz, &info);  //dgeqpf_((int *)&tm, (int *)&tn, a, (int *)&tm, jpvt, tau, work, &info);  /* error checking */  if(info!=0){    if(info<0){      fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", GEQP3) " in ", LMLEC_ELIM) "()/n", -info);      exit(1);    }    else if(info>0){      fprintf(stderr, RCAT(RCAT("unknown LAPACK error (%d) for ", GEQP3) " in ", LMLEC_ELIM) "()/n", info);      free(buf);      return 0;    }  }  /* the upper triangular part of a now contains the upper triangle of the unpermuted R */  if(eps<0.0){    LM_REAL aux;    /* compute machine epsilon. DBL_EPSILON should do also */    for(eps=CNST(1.0); aux=eps+CNST(1.0), aux-CNST(1.0)>0.0; eps*=CNST(0.5))                              ;    eps*=CNST(2.0);  }  tmp=tm*CNST(10.0)*eps*FABS(a[0]); // threshold. tm is max(tm, tn)//.........这里部分代码省略.........

static double evaluateGTRCATPROT (int *cptr, int *wptr,				  double *x1, double *x2, double *tipVector,				  unsigned char *tipX1, int n, double *diagptable_start){  double   sum = 0.0, term;  double  *diagptable,  *left, *right;  int     i, l;                               if(tipX1)    {                       for (i = 0; i < n; i++) 	{	       		  left = &(tipVector[20 * tipX1[i]]);	  right = &(x2[20 * i]);	  	  diagptable = &diagptable_start[20 * cptr[i]];	           	 	  __m128d tv = _mm_setzero_pd();	    	  	  for(l = 0; l < 20; l+=2)	    {	      __m128d lv = _mm_load_pd(&left[l]);	      __m128d rv = _mm_load_pd(&right[l]);	      __m128d mul = _mm_mul_pd(lv, rv);	      __m128d dv = _mm_load_pd(&diagptable[l]);	      	      tv = _mm_add_pd(tv, _mm_mul_pd(mul, dv));		   	    }		 			  	  tv = _mm_hadd_pd(tv, tv);	  _mm_storel_pd(&term, tv);  	  	  term = LOG(FABS(term));	  	  	  sum += wptr[i] * term;	}          }      else    {          for (i = 0; i < n; i++) 	{		       	      	      	  left  = &x1[20 * i];	  right = &x2[20 * i];	  	  diagptable = &diagptable_start[20 * cptr[i]];	  		  __m128d tv = _mm_setzero_pd();	    	      	    	  for(l = 0; l < 20; l+=2)	    {	      __m128d lv = _mm_load_pd(&left[l]);	      __m128d rv = _mm_load_pd(&right[l]);	      __m128d mul = _mm_mul_pd(lv, rv);	      __m128d dv = _mm_load_pd(&diagptable[l]);	      	      tv = _mm_add_pd(tv, _mm_mul_pd(mul, dv));		   	    }		 			  	  tv = _mm_hadd_pd(tv, tv);	  _mm_storel_pd(&term, tv);	  	  	  term = LOG(FABS(term));	 	  	  sum += wptr[i] * term;      	}    }               return  sum;         } 

intAPPEND (FUNC_PREFIX, ecvt_r) (FLOAT_TYPE value,                               int ndigit,                               int *decpt,                               int *sign,                               char *buf,                               size_t len){  int exponent = 0;  if (!ISNAN (value) && !ISINF (value) && value != 0.0) {      FLOAT_TYPE (*log10_function) (FLOAT_TYPE) = &LOG10;      if (log10_function) {         /* Use the reasonable code if -lm is included.  */         FLOAT_TYPE dexponent;         dexponent = FLOOR (LOG10 (FABS (value)));         value *= EXP (dexponent * -M_LN10);         exponent = (int) dexponent;      } else {         /* Slow code that doesn't require -lm functions.  */         FLOAT_TYPE d;         if (value < 0.0)            d = -value;         else            d = value;         if (d < 1.0) {            do {               d *= 10.0;               --exponent;            } while (d < 1.0);         } else if (d >= 10.0) {            do {               d *= 0.1;               ++exponent;            } while (d >= 10.0);         }         if (value < 0.0)            value = -d;         else            value = d;       }    } else if (value == 0.0)       /* SUSv2 leaves it unspecified whether *DECPT is 0 or 1 for 0.0.        * This could be changed to -1 if we want to return 0.  */        exponent = 0;    if (ndigit <= 0 && len > 0) {       buf[0] = '/0';       *decpt = 1;       if (!ISINF (value) && !ISNAN (value))          *sign = value < 0.0;       else          *sign = 0;    } else       if (APPEND (FUNC_PREFIX, fcvt_r) (value, ndigit - 1, decpt, sign,                      buf, len))          return -1;    *decpt += exponent;    return 0;}