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 *cw
static void
CalcUnpred (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_hidden
pnchisq_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 0
if(!(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 ;
//.........这里部分代码省略.........