C++ RSqrt函数代码示例

void RayGenerator::Generate(int level,int pw,int ph,int x,int y, Vec3q *out) const {
	{
		DASSERT(level == 3);

		GridSampler sampler;
		Vec3q right(tright), up(tup), tadd = txyz;
		floatq xoff(x + 0, x + 0, x + 2, x + 2);
		floatq yoff(y + 0, y + 0, y - 1, y - 1);

		for(int ty = 0; ty < 16; ty++) {
			floatq tposx[4] = { floatq(0.0f) + xoff, floatq(4.0f) + xoff,
								floatq(8.0f) + xoff, floatq(12.0f) + xoff };
			floatq tposy = floatq(ty) + yoff;

			Vec3q tpoint = up * tposy + tadd;
			Vec3q points[4] = { right * tposx[0] + tpoint, right * tposx[1] + tpoint,
								right * tposx[2] + tpoint, right * tposx[3] + tpoint };

			out[ty * 4 + 0] = points[0] * RSqrt(points[0] | points[0]);
			out[ty * 4 + 1] = points[1] * RSqrt(points[1] | points[1]);
			out[ty * 4 + 2] = points[2] * RSqrt(points[2] | points[2]);
			out[ty * 4 + 3] = points[3] * RSqrt(points[3] | points[3]);
		}
		return;
	}

	if(level > 2) {
		int npw = pw >> 1, nph = ph >> 1, nl = level - 1;

		Generate(nl, npw, nph, x      , y      , out + 0);
		Generate(nl, npw, nph, x + npw, y      , out + (1 << nl*2));
		Generate(nl, npw, nph, x      , y + nph, out + (2 << nl*2));
		Generate(nl, npw, nph, x + npw, y + nph, out + (3 << nl*2));
		return;
	}

int KINSetRelErrFunc(void *kinmem, realtype relfunc)
{
  KINMem kin_mem;
  realtype uround;

  if (kinmem == NULL) {
    KINProcessError(NULL, KIN_MEM_NULL, "KINSOL", "KINSetRelErrFunc", MSG_NO_MEM);
    return(KIN_MEM_NULL);
  }

  kin_mem = (KINMem) kinmem;

  if (relfunc < ZERO) {
    KINProcessError(NULL, KIN_ILL_INPUT, "KINSOL", "KINSetRelErrFunc", MSG_BAD_RELFUNC);
    return(KIN_ILL_INPUT);
  }

  if (relfunc == ZERO) {
    uround = kin_mem->kin_uround;
    kin_mem->kin_sqrt_relfunc = RSqrt(uround);
  } else {
    kin_mem->kin_sqrt_relfunc = RSqrt(relfunc);
  }

  return(KIN_SUCCESS);
}

realtype N_VWrmsNormMask_NrnThread(N_Vector x, N_Vector w, N_Vector id)
{
  long int N;
  N  = NV_LENGTH_NT(x);
  retval = ZERO;
  xpass wpass idpass
  nrn_multithread_job(vwrmsnormmask);
mydebug2("vwrmsnormmask %.20g\n", RSqrt(retval / N));
  return(RSqrt(retval / N));
}

realtype N_VWL2Norm_NrnThread(N_Vector x, N_Vector w)
{
  long int N;
  retval = ZERO;
  xpass wpass
  nrn_multithread_job(vwl2norm);
  N  = NV_LENGTH_NT(x);
mydebug2("vwl2norm %.20g\n", RSqrt(retval));
  return(RSqrt(retval));
}

static int res(realtype t, N_Vector yy, N_Vector yd, N_Vector res, void *userdata)
{
  UserData data;
  realtype k1, k2, k3, k4;
  realtype K, klA, Ks, pCO2, H;

  realtype y1, y2, y3, y4, y5, y6;
  realtype yd1, yd2, yd3, yd4, yd5;

  realtype r1, r2, r3, r4, r5, Fin;

  data = (UserData) userdata;
  k1 = data->k1;
  k2 = data->k2;
  k3 = data->k3;
  k4 = data->k4;
  K = data->K;
  klA = data->klA;
  Ks = data->Ks;
  pCO2 = data->pCO2;
  H = data->H;

  y1 = Ith(yy,1);
  y2 = Ith(yy,2);
  y3 = Ith(yy,3);
  y4 = Ith(yy,4);
  y5 = Ith(yy,5);
  y6 = Ith(yy,6);

  yd1 = Ith(yd,1);
  yd2 = Ith(yd,2);
  yd3 = Ith(yd,3);
  yd4 = Ith(yd,4);
  yd5 = Ith(yd,5);

  r1 = k1 * RPowerI(y1,4) * RSqrt(y2);
  r2 = k2 * y3 * y4;
  r3 = k2/K * y1 * y5;
  r4 = k3 * y1 * y4 * y4;
  r5 = k4 * y6 * y6 * RSqrt(y2);
  Fin = klA * ( pCO2/H - y2 );

  Ith(res,1) = yd1 + TWO*r1 - r2 + r3 + r4;
  Ith(res,2) = yd2 + HALF*r1 + r4 + HALF*r5 - Fin;
  Ith(res,3) = yd3 - r1 + r2 - r3;
  Ith(res,4) = yd4 + r2 - r3 + TWO*r4;
  Ith(res,5) = yd5 - r2 + r3 - r5;
  Ith(res,6) = Ks*y1*y4 - y6;

  return(0);
}

int ModifiedGS(N_Vector *v, realtype **h, int k, int p, 
               realtype *new_vk_norm)
{
  int  i, k_minus_1, i0;
  realtype new_norm_2, new_product, vk_norm, temp;
  
  vk_norm = RSqrt(N_VDotProd(v[k],v[k]));
  k_minus_1 = k - 1;
  i0 = MAX(k-p, 0);
  
  /* Perform modified Gram-Schmidt */
  
  for (i=i0; i < k; i++) {
    h[i][k_minus_1] = N_VDotProd(v[i], v[k]);
    N_VLinearSum(ONE, v[k], -h[i][k_minus_1], v[i], v[k]);
  }

  /* Compute the norm of the new vector at v[k] */

  *new_vk_norm = RSqrt(N_VDotProd(v[k], v[k]));

  /* If the norm of the new vector at v[k] is less than
     FACTOR (== 1000) times unit roundoff times the norm of the
     input vector v[k], then the vector will be reorthogonalized
     in order to ensure that nonorthogonality is not being masked
     by a very small vector length. */

  temp = FACTOR * vk_norm;
  if ((temp + (*new_vk_norm)) != temp) return(0);
  
  new_norm_2 = ZERO;

  for (i=i0; i < k; i++) {
    new_product = N_VDotProd(v[i], v[k]);
    temp = FACTOR * h[i][k_minus_1];
    if ((temp + new_product) == temp) continue;
    h[i][k_minus_1] += new_product;
    N_VLinearSum(ONE, v[k],-new_product, v[i], v[k]);
    new_norm_2 += SQR(new_product);
  }

  if (new_norm_2 != ZERO) {
    new_product = SQR(*new_vk_norm) - new_norm_2;
    *new_vk_norm = (new_product > ZERO) ? RSqrt(new_product) : ZERO;
  }

  return(0);
}

int ClassicalGS(N_Vector *v, realtype **h, int k, int p, 
                realtype *new_vk_norm, N_Vector temp, realtype *s)
{
  int  i, k_minus_1, i0;
  realtype vk_norm;

  k_minus_1 = k - 1;
  
  /* Perform Classical Gram-Schmidt */

  vk_norm = RSqrt(N_VDotProd(v[k], v[k]));

  i0 = MAX(k-p, 0);
  for (i=i0; i < k; i++) {
    h[i][k_minus_1] = N_VDotProd(v[i], v[k]);
  }

  for (i=i0; i < k; i++) {
    N_VLinearSum(ONE, v[k], -h[i][k_minus_1], v[i], v[k]);
  }

  /* Compute the norm of the new vector at v[k] */

  *new_vk_norm = RSqrt(N_VDotProd(v[k], v[k]));

  /* Reorthogonalize if necessary */

  if ((FACTOR * (*new_vk_norm)) < vk_norm) {

    for (i=i0; i < k; i++) {
      s[i] = N_VDotProd(v[i], v[k]);
    }

    if (i0 < k) {
      N_VScale(s[i0], v[i0], temp);
      h[i0][k_minus_1] += s[i0];
    }
    for (i=i0+1; i < k; i++) {
      N_VLinearSum(s[i], v[i], ONE, temp, temp);
      h[i][k_minus_1] += s[i];
    }
    N_VLinearSum(ONE, v[k], -ONE, temp, v[k]);

    *new_vk_norm = RSqrt(N_VDotProd(v[k],v[k]));
  }

  return(0);
}

int IDABBDPrecReInit(void *bbd_data,
		     long int mudq, long int mldq, 
		     realtype dq_rel_yy,  
		     IDABBDLocalFn Gres, IDABBDCommFn Gcomm)
{
  IBBDPrecData pdata;
  IDAMem IDA_mem;
  long int Nlocal;

  if (bbd_data == NULL) {
    IDAProcessError(NULL, IDABBDPRE_PDATA_NULL, "IDABBDPRE", "IDABBDPrecReInit", MSGBBD_PDATA_NULL);
    return(IDABBDPRE_PDATA_NULL);
  } 

  pdata =(IBBDPrecData) bbd_data;
  IDA_mem = (IDAMem) pdata->ida_mem;

  Nlocal = pdata->n_local;

  /* Set pointers to res_data, glocal, and gcomm; load half-bandwidths. */
  pdata->mudq = MIN(Nlocal-1, MAX(0, mudq));
  pdata->mldq = MIN(Nlocal-1, MAX(0, mldq));
  pdata->glocal = Gres;
  pdata->gcomm = Gcomm;

  /* Set rel_yy based on input value dq_rel_yy (0 implies default). */
  pdata->rel_yy = (dq_rel_yy > ZERO) ? dq_rel_yy : RSqrt(uround); 

  /* Re-initialize nge */
  pdata->nge = 0;

  return(IDABBDPRE_SUCCESS);
}

/*
 * Cholesky decomposition of a symmetric positive-definite matrix
 * A = C^T*C: gaxpy version.
 * Only the lower triangle of A is accessed and it is overwritten with
 * the lower triangle of C.
 */
long int densePOTRF(realtype **a, long int m)
{
  realtype *a_col_j, *a_col_k;
  realtype a_diag;
  long int i, j, k;

  for (j=0; j<m; j++) {

    a_col_j = a[j];
   
    if (j>0) {
      for(i=j; i<m; i++) {
        for(k=0;k<j;k++) {
          a_col_k = a[k];
          a_col_j[i] -= a_col_k[i]*a_col_k[j];
        }
      }
    }

    a_diag = a_col_j[j];
    if (a_diag <= ZERO) return(j+1);
    a_diag = RSqrt(a_diag);

    for(i=j; i<m; i++) a_col_j[i] /= a_diag;
    
  }

  return(0);
}

static void KINForcingTerm(KINMem kin_mem, real fnormp)
{
  real eta_max = POINT9, eta_min = POINTOHOHOHONE,
    eta_safe = 0.5, linmodel_norm;

  if (etaflag == ETACHOICE1)        /* Choice 1 forcing terms. */

  { /* compute the norm of f + J p , scaled L2 norm  */
    linmodel_norm = RSqrt(fnorm * fnorm + TWO * sfdotJp + sJpnorm * sJpnorm);

    /* Form the safeguarded Choice 1 eta. */

    eta_safe = RPowerR(eta, ealpha);
    eta = ABS(fnormp - linmodel_norm) / fnorm;
  }

  if (etaflag == ETACHOICE2)        /* Choice 2 forcing terms. */

  {
    eta_safe = egamma * RPowerR(eta, ealpha);
    eta = egamma * RPowerR(fnormp / fnorm, ealpha);
  }

  /* Apply the safeguards. */
  if (eta_safe < POINT1)
    eta_safe = ZERO;
  eta = MAX(eta, eta_safe);
  eta = MAX(eta, eta_min);
  eta = MIN(eta, eta_max);

  return;
}

int CVBBDPrecReInit(void *bbd_data, 
                    long int mudq, long int mldq, 
                    realtype dqrely, 
                    CVLocalFn gloc, CVCommFn cfn)
{
  CVBBDPrecData pdata;
  CVodeMem cv_mem;
  long int Nlocal;

  if ( bbd_data == NULL ) {
    fprintf(stderr, MSGBBDP_NO_PDATA);
    return(CV_PDATA_NULL);
  } 

  pdata = (CVBBDPrecData) bbd_data;
  cv_mem = (CVodeMem) pdata->cvode_mem;

  /* Set pointers to gloc and cfn; load half-bandwidths */
  pdata->gloc = gloc;
  pdata->cfn = cfn;
  Nlocal = pdata->n_local;
  pdata->mudq = MIN( Nlocal-1, MAX(0,mudq) );
  pdata->mldq = MIN( Nlocal-1, MAX(0,mldq) );

  /* Set pdata->dqrely based on input dqrely (0 implies default). */
  pdata->dqrely = (dqrely > ZERO) ? dqrely : RSqrt(uround);

  /* Re-initialize nge */
  pdata->nge = 0;

  return(CV_SUCCESS);
}

static void CVDenseDQJac(long int N, DenseMat J, realtype t,
                         N_Vector y, N_Vector fy, void *jac_data,
                         N_Vector tmp1, N_Vector tmp2, N_Vector tmp3)
{
    realtype fnorm, minInc, inc, inc_inv, yjsaved, srur;
    realtype *tmp2_data, *y_data, *ewt_data;
    N_Vector ftemp, jthCol;
    long int j;

    CVodeMem cv_mem;
    CVDenseMem  cvdense_mem;

    /* jac_data points to cvode_mem */
    cv_mem = (CVodeMem) jac_data;
    cvdense_mem = (CVDenseMem) lmem;

    /* Save pointer to the array in tmp2 */
    tmp2_data = N_VGetArrayPointer(tmp2);

    /* Rename work vectors for readibility */
    ftemp = tmp1;
    jthCol = tmp2;

    /* Obtain pointers to the data for ewt, y */
    ewt_data = N_VGetArrayPointer(ewt);
    y_data   = N_VGetArrayPointer(y);

    /* Set minimum increment based on uround and norm of f */
    srur = RSqrt(uround);
    fnorm = N_VWrmsNorm(fy, ewt);
    minInc = (fnorm != ZERO) ?
             (MIN_INC_MULT * ABS(h) * uround * N * fnorm) : ONE;

    /* This is the only for loop for 0..N-1 in CVODE */

    for (j = 0; j < N; j++) {

        /* Generate the jth col of J(tn,y) */

        N_VSetArrayPointer(DENSE_COL(J,j), jthCol);

        yjsaved = y_data[j];
        inc = MAX(srur*ABS(yjsaved), minInc/ewt_data[j]);
        y_data[j] += inc;
        f(tn, y, ftemp, f_data);
        y_data[j] = yjsaved;

        inc_inv = ONE/inc;
        N_VLinearSum(inc_inv, ftemp, -inc_inv, fy, jthCol);

        DENSE_COL(J,j) = N_VGetArrayPointer(jthCol);
    }

    /* Restore original array pointer in tmp2 */
    N_VSetArrayPointer(tmp2_data, tmp2);

    /* Increment counter nfeD */
    nfeD += N;
}

AABB Sphere::MaximalContainedAABB() const
{
	AABB aabb;
	static const float recipSqrt3 = RSqrt(3);
	float halfSideLength = r * recipSqrt3;
	aabb.SetFromCenterAndSize(pos, DIR_VEC(halfSideLength,halfSideLength,halfSideLength));
	return aabb;
}

realtype N_VWrmsNorm_NrnThread(N_Vector x, N_Vector w)
{
  long int N;
  N  = NV_LENGTH_NT(x);
#if USELONGDOUBLE
  longdretval = ZERO;
#else
  retval = ZERO;
#endif
  xpass  wpass
  nrn_multithread_job(vwrmsnorm);
#if USELONGDOUBLE
  retval = longdretval;
#endif
mydebug2("vwrmsnorm %.20g\n", RSqrt(retval / N));
  return(RSqrt(retval / N));
}

static void CVBandPDQJac(CVBandPrecData pdata, 
                         realtype t, N_Vector y, N_Vector fy, 
                         N_Vector ftemp, N_Vector ytemp)
{
  CVodeMem cv_mem;
  realtype    fnorm, minInc, inc, inc_inv, srur;
  long int group, i, j, width, ngroups, i1, i2;
  realtype *col_j, *ewt_data, *fy_data, *ftemp_data, *y_data, *ytemp_data;

  cv_mem = (CVodeMem) pdata->cvode_mem;

  /* Obtain pointers to the data for ewt, fy, ftemp, y, ytemp. */
  ewt_data   = N_VGetArrayPointer(ewt);
  fy_data    = N_VGetArrayPointer(fy);
  ftemp_data = N_VGetArrayPointer(ftemp);
  y_data     = N_VGetArrayPointer(y);
  ytemp_data = N_VGetArrayPointer(ytemp);

  /* Load ytemp with y = predicted y vector. */
  N_VScale(ONE, y, ytemp);

  /* Set minimum increment based on uround and norm of f. */
  srur = RSqrt(uround);
  fnorm = N_VWrmsNorm(fy, ewt);
  minInc = (fnorm != ZERO) ?
           (MIN_INC_MULT * ABS(h) * uround * N * fnorm) : ONE;

  /* Set bandwidth and number of column groups for band differencing. */
  width = ml + mu + 1;
  ngroups = MIN(width, N);
  
  for (group = 1; group <= ngroups; group++) {
    
    /* Increment all y_j in group. */
    for(j = group-1; j < N; j += width) {
      inc = MAX(srur*ABS(y_data[j]), minInc/ewt_data[j]);
      ytemp_data[j] += inc;
    }

    /* Evaluate f with incremented y. */

    f(t, ytemp, ftemp, f_data);
    nfeBP++;

    /* Restore ytemp, then form and load difference quotients. */
    for (j = group-1; j < N; j += width) {
      ytemp_data[j] = y_data[j];
      col_j = BAND_COL(savedJ,j);
      inc = MAX(srur*ABS(y_data[j]), minInc/ewt_data[j]);
      inc_inv = ONE/inc;
      i1 = MAX(0, j-mu);
      i2 = MIN(j+ml, N-1);
      for (i=i1; i <= i2; i++)
        BAND_COL_ELEM(col_j,i,j) =
          inc_inv * (ftemp_data[i] - fy_data[i]);
    }
  }
}