C++ SQRT函数代码示例

extern void dops(int ns, const double *azel, double elmin, double *dop)
{
	double H[4 * MAXSAT], Q[16], cosel, sinel;
	int i, n;

	for (i = 0; i < 4; i++) dop[i] = 0.0;
	for (i = n = 0; i < ns&&i < MAXSAT; i++) {
		if (azel[1 + i * 2] < elmin || azel[1 + i * 2] <= 0.0) continue;
		cosel = cos(azel[1 + i * 2]);
		sinel = sin(azel[1 + i * 2]);
		H[4 * n] = cosel*sin(azel[i * 2]);
		H[1 + 4 * n] = cosel*cos(azel[i * 2]);
		H[2 + 4 * n] = sinel;
		H[3 + 4 * n++] = 1.0;
	}
	if (n < 4) return;

	matmul("NT", 4, 4, n, 1.0, H, H, 0.0, Q);
	if (!matinv(Q, 4)) {
		dop[0] = SQRT(Q[0] + Q[5] + Q[10] + Q[15]); /* GDOP */
		dop[1] = SQRT(Q[0] + Q[5] + Q[10]);       /* PDOP */
		dop[2] = SQRT(Q[0] + Q[5]);             /* HDOP */
		dop[3] = SQRT(Q[10]);                 /* VDOP */
	}
}

static void cylinder_normal(const cylinder * cyl, const vector * pnt, const ray * incident, vector * N) {
  vector a, b;
  flt t, invlen, invlen2;

  a.x = pnt->x - cyl->ctr.x;
  a.y = pnt->y - cyl->ctr.y;
  a.z = pnt->z - cyl->ctr.z;

  b=cyl->axis;

  invlen = 1.0 / SQRT(b.x*b.x + b.y*b.y + b.z*b.z);
  b.x *= invlen;
  b.y *= invlen;
  b.z *= invlen;
 
  VDOT(t, a, b);

  N->x = pnt->x - (b.x * t + cyl->ctr.x); 
  N->y = pnt->y - (b.y * t + cyl->ctr.y);
  N->z = pnt->z - (b.z * t + cyl->ctr.z);

  invlen2 = 1.0 / SQRT(N->x*N->x + N->y*N->y + N->z*N->z);
  N->x *= invlen2;
  N->y *= invlen2;
  N->z *= invlen2;

  /* Flip surface normal to point toward the viewer if necessary */
  if (VDot(N, &(incident->d)) > 0.0)  {
    N->x=-N->x;
    N->y=-N->y;
    N->z=-N->z;
  }
}

Polygon2D < double > Game_Engine::Polygon_from(const Point2D < double > &C_p, const Vector2D < double > &V, const double &w) {
	vector < Point2D < double > > Pol;
	if (abs(V.y) < EPS) {
		Pol.push_back(Point2D < double >(C_p.x, C_p.y - w / 2.0));
		Pol.push_back(Point2D < double >(C_p.x, C_p.y + w / 2.0));
		Pol.push_back(Pol[0] + V);
		Pol.push_back(Pol[1] + V);
	}
	else {
		if (abs(V.x) < EPS) {
			Pol.push_back(Point2D < double >(C_p.x - w / 2.0, C_p.y));
			Pol.push_back(Point2D < double >(C_p.x + w / 2.0, C_p.y));
			Pol.push_back(Pol[0] + V);
			Pol.push_back(Pol[1] + V);
		}
		else {
			Pol.push_back(Point2D < double >(C_p.x + w / (2.0*SQRT(1 + SQR(V.x / V.y))), 0));
			Pol[0].y = C_p.y - V.x*(Pol[0].x - C_p.x) / V.y;
			Pol.push_back(Point2D < double >(C_p.x - w / (2.0*SQRT(1 + SQR(V.x / V.y))), 0));
			Pol[1].y = C_p.y - V.x*(Pol[1].x - C_p.x) / V.y;
			Pol.push_back(Pol[1] + V);
			Pol.push_back(Pol[0] + V);
		}
	}
	return Polygon2D < double >(Pol);
}

double SO3_alpha(const int m1, const int m2, const int j)
{
  const int M = MAX(ABS(m1),ABS(m2)), mini = MIN(ABS(m1),ABS(m2));

  if (j < 0)
    return K(0.0);
  else if (j == 0)
  {
    if (m1 == 0 && m2 == 0)
      return K(1.0);
    if (m1 == m2)
      return K(0.5);
    else
      return IF((m1+m2)%2,K(0.0),K(-0.5));
  }
  else if (j < M - mini)
    return IF(j%2,K(0.5),K(-0.5));
  else if (j < M)
    return K(0.5) * SIGNF((R)m1)*SIGNF((R)m2);
  else
    return
      SQRT(((R)(j+1))/((R)(j+1-m1)))
      * SQRT(((R)(2*j+1))/((R)(j+1+m1)))
      * SQRT(((R)(j+1))/((R)(j+1-m2)))
      * SQRT(((R)(2*j+1))/((R)(j+1+m2)));
}

 T BivarStats<T>::sigmaYX(void) const
 {
    if (ns>2)
       return (stdDevY() * SQRT(T(ns-1) / T(ns-2))
               * SQRT(T(1) - correlation() * correlation()) );
    else return T();
 }

static int32_t gausstrig_process_krate(CSOUND* csound, GAUSSTRIG *p)
{
    MYFLT frq, dev;
    p->frq0 = *p->kfrq;
    frq = (*p->kfrq > FL(0.001) ? *p->kfrq : FL(0.001));
    dev = *p->kdev;
    if (p->first > 0) {
      /* values less than FL(0.0) could be used in later versions
         as an offset in samples */
      int32_t     nextsamps;
      MYFLT   nextcount, r1, r2;
      /* this very line of k-time fix. Changed GetSt to GetKr */
      nextsamps = (int32_t)(csound->GetKr(csound) / frq);
      p->rand = csoundRand31(&p->rand);
      r1 = (MYFLT)p->rand * dv2_31;
      p->rand = csoundRand31(&p->rand);
      r2 = (MYFLT)p->rand * dv2_31;
      nextcount = SQRT(FL(-2.0) * LOG(r1)) * SIN(r2 * TWOPI_F);
      if (nextcount < FL(-1.0)) {
        MYFLT diff = FL(-1.0) - nextcount;
        nextcount  = (FL(1.0) < FL(-1.0) + diff ? FL(1.0) : FL(-1.0) + diff);
      }
      else if (nextcount > FL(1.0)) {
        MYFLT diff = nextcount - FL(1.0);
        nextcount  = (FL(-1.0) > FL(1.0) - diff ? FL(-1.0) : FL(1.0) - diff);
      }
      p->count = (int32_t)(nextsamps + nextcount * dev * nextsamps);
      p->first = 0;
    }
    if (p->count <= 0) {
      int32_t     nextsamps;
      MYFLT   nextcount, r1, r2;
/* this very line of k-time fix. Changed GetSt to GetKr */
      nextsamps = (int32_t)(csound->GetKr(csound) / frq);
      p->rand = csoundRand31(&p->rand);
      r1 = (MYFLT)p->rand * dv2_31;
      p->rand = csoundRand31(&p->rand);
      r2 = (MYFLT)p->rand * dv2_31;
      nextcount = SQRT(FL(-2.0) * LOG(r1)) * SIN(r2 * TWOPI_F);
      if (nextcount < FL(-1.0)) {
        MYFLT diff = FL(-1.0) - nextcount;
        nextcount  = (FL(1.0) < FL(-1.0) + diff ? FL(1.0) : FL(-1.0) + diff);
      }
      else if (nextcount > FL(1.0)) {
        MYFLT diff = nextcount - FL(1.0);
        nextcount  = (FL(-1.0) > FL(1.0) - diff ? FL(-1.0) : FL(1.0) - diff);
      }
      p->count = (int32_t)(nextsamps + nextcount * dev * nextsamps);
      *p->out = *p->kamp;
    }
    else {
      if (p->mmode && *p->kfrq != p->frq0)
        p->count = 0;
      *p->out = FL(0.0);
    }
    p->count--;
    return OK;
}

	Int32 Cubic( Real x3, Real x2, Real x, Real k, Real roots[3] )
	{
		if( IsZero( x3 ) )
			return Quadratic( x2, x, k, roots );

		Real a, b, c, d, a2, p, q, p3, s;

		// Normalize
		a = x2 / x3;
		b = x  / x3;
		c = k  / x3;

		// Reduction to a depressed cubic
		a2 = a * a;
		p = 1.0f / 3.0f * ( -1.0f / 3.0f * a2 + b );
		q = 1.0f / 2.0f * ( 2.0f / 27.0f * a * a2 - 1.0f / 3.0f * a * b + c );
		s = 1.0f / 3.0f * a;

		// Cardano's method
		p3 = p * p * p;
		d = q * q + p3;

		if( IsZero( d ) )
		{
			if( IsZero( q ) )
			{
				roots[0] = -s;
				return 1;			
			}
			else
			{
				Real u = CubeRoot( -q );
				roots[0] = 2.0f * u - s;
				roots[1] = -u - s;
				return 2;
			}
		}

		if( d < 0.0f )
		{
			Real phi = 1.0f / 3.0f * ACOS( -q / SQRT( -p3 ) );
			Real t = 2.0f * SQRT( -p );
			roots[0] =  t * COS( phi ) - s;
			roots[1] = -t * COS( phi + PI / 3.0f ) - s;
			roots[2] = -t * COS( phi - PI / 3.0f ) - s;
			return 3;
		}

		Real u = CubeRoot( SQRT( d ) + ABS( q ) );
		roots[0] = ( q > 0.0f ? - u + p / u : u - p / u ) - s;
		return 1;
	}

/*
 * refclock_receive - simulate the receive and packet procedures
 *
 * This routine simulates the NTP receive and packet procedures for a
 * reference clock. This provides a mechanism in which the ordinary NTP
 * filter, selection and combining algorithms can be used to suppress
 * misbehaving radios and to mitigate between them when more than one is
 * available for backup.
 */
void
refclock_receive(
	struct peer *peer	/* peer structure pointer */
	)
{
	struct refclockproc *pp;

#ifdef DEBUG
	if (debug)
		printf("refclock_receive: at %lu %s\n",
		    current_time, ntoa(&peer->srcadr));
#endif

	/*
	 * Do a little sanity dance and update the peer structure. Groom
	 * the median filter samples and give the data to the clock
	 * filter.
	 */
	peer->received++;
	pp = peer->procptr;
	peer->processed++;
	peer->timereceived = current_time;
	peer->leap = pp->leap;
	if (peer->leap == LEAP_NOTINSYNC) {
		refclock_report(peer, CEVNT_FAULT);
		return;
	}
	if (!peer->reach)
		report_event(EVNT_REACH, peer);
	peer->reach |= 1;
	peer->reftime = peer->org = pp->lastrec;
	peer->rootdispersion = pp->disp + SQRT(pp->jitter);
	get_systime(&peer->rec);
	if (!refclock_sample(pp))
		return;
	clock_filter(peer, pp->offset, 0., pp->jitter);
	clock_select();
	record_peer_stats(&peer->srcadr, ctlpeerstatus(peer),
	    peer->offset, peer->delay, clock_phi * (current_time -
	    peer->epoch), SQRT(peer->jitter));
	if (cal_enable && last_offset < MINDISPERSE) {
#ifdef KERNEL_PLL
		if (peer != sys_peer || pll_status & STA_PPSTIME)
#else
		if (peer != sys_peer)
#endif /* KERNEL_PLL */
			pp->fudgetime1 -= pp->offset * FUDGEFAC;
		else
			pp->fudgetime1 -= pp->fudgetime1 * FUDGEFAC;
	}
}

static void cylinder_intersect(const cylinder * cyl, ray * ry) {
  vector rc, n, D, O;  
  flt t, s, tin, tout, ln, d; 

  rc.x = ry->o.x - cyl->ctr.x;
  rc.y = ry->o.y - cyl->ctr.y;
  rc.z = ry->o.z - cyl->ctr.z; 

  VCross(&ry->d, &cyl->axis, &n);

  ln=SQRT(n.x*n.x + n.y*n.y + n.z*n.z);    /* finish length calculation */

  if (ln == 0.0) {  /* ray is parallel to the cylinder.. */
    VDOT(d, rc, cyl->axis);         
    D.x = rc.x - d * cyl->axis.x; 
    D.y = rc.y - d * cyl->axis.y;
    D.z = rc.z - d * cyl->axis.z;
    VDOT(d, D, D);
    d = SQRT(d);
    tin = -FHUGE;
    tout = FHUGE;
    /* if (d <= cyl->rad) then ray is inside cylinder.. else outside */
  }

  n.x /= ln;
  n.y /= ln;
  n.z /= ln;

  VDOT(d, rc, n);
  d = FABS(d); 

  if (d <= cyl->rad) {  /* ray intersects cylinder.. */
    VCross(&rc, &cyl->axis, &O);
    VDOT(t, O, n);
    t = - t / ln;
    VCross(&n, &cyl->axis, &O); 

    ln = SQRT(O.x*O.x + O.y*O.y + O.z*O.z);
    O.x /= ln;
    O.y /= ln;
    O.z /= ln;

    VDOT(s, ry->d, O);
    s = FABS(SQRT(cyl->rad*cyl->rad - d*d) / s);
    tin = t - s;
    ry->add_intersection(tin, (object *) cyl, ry); 
    tout = t + s;
    ry->add_intersection(tout, (object *) cyl, ry);
  }
}

REAL cucompute_beta(REAL temp, REAL unit_number, REAL aexp)
{
  // Collizional ionization rate m**3 s*-1
  // temperature in Kelvin
  REAL beta,T5;
  T5=temp/1e5;
  beta=5.85e-11*SQRT(temp)/(1+SQRT(T5))*exp(-(157809e0/temp)); //cm3/s
#ifdef TESTCOSMO
  beta=beta*1e-6*unit_number;//(aexp*aexp*aexp); // !m3/s
#else
  beta=beta*1e-6*unit_number; // !m3/s
#endif
  return beta;
}

	Int32 Quartic( Real x4, Real x3, Real x2, Real x, Real k, Real roots[4] )
	{
		if( IsZero( x4 ) )
			return Cubic( x3, x2, x, k, roots );
		
		Int32 n;
		Real a, b, c, d, s, a2, p, q, r, z, u, v;

		// Normalize
		a = x3 / x4;
		b = x2 / x4;
		c = x  / x4;
		d = k  / x4;

		// Reduction to a depressed quartic
		a2 = a * a;
		p = -3.0f / 8.0f * a2 + b;
		q = 1.0f / 8.0f * a2 * a - 1.0f / 2.0f * a * b + c;
		r = -3.0f / 256.0f * a2 * a2 + 1.0f / 16.0f * a2 * b - 1.0f / 4.0f * a * c + d;

		if( IsZero( r ) )
		{
			n = Cubic( 1.0f, 0.0f, p, q, roots );
			roots[n++] = 0.0f;
		}
		else
		{
			Cubic( 1.0f, -1.0f / 2.0f * p, -r, 1.0f / 2.0f * r * p - 1.0f / 8.0f * q * q, roots );

			z = roots[0];
			u = z * z - r;
			v = 2.0f * z - p;

			if( u < -CLOSE ) return 0;
			else u = u > CLOSE ? SQRT( u ) : 0.0f;

			if( v < -CLOSE ) return 0;
			else v = v > CLOSE ? SQRT( v ) : 0.0f;

			n  = Quadratic( 1.0f, q < 0.0f ? -v : v, z - u, roots );
			n += Quadratic( 1.0f, q < 0.0f ? v : -v, z + u, roots + n );
		}

		s = 1.0f / 4.0f * a;
		for( Int32 i = 0; i < n; i++ )
			roots[i] -= s;

		return n;
	}

 inline T norm(const ConstVectorBase<T, BaseClass>& v) 
 {
    T mag=T(0);
    if(v.size()==0) return mag;
    mag = ABS(v(0));
    for(size_t i=1; i<v.size(); i++) {
       if(mag > ABS(v(i)))
          mag *= SQRT(T(1)+(v(i)/mag)*(v(i)/mag));
       else if(ABS(v(i)) > mag)
          mag = ABS(v(i))*SQRT(T(1)+(mag/v(i))*(mag/v(i)));
       else
          mag *= SQRT(T(2));
    }
    return mag;
 } 

    //------------------------------------------------------------------------
    void trans_warp_magnifier::inverse_transform(real* x, real* y) const
    {
        // New version by Andrew Skalkin
        //-----------------
        real dx = *x - m_xc;
        real dy = *y - m_yc;
        real r = SQRT(dx * dx + dy * dy);

        if(r < m_radius * m_magn) 
        {
            *x = m_xc + dx / m_magn;
            *y = m_yc + dy / m_magn;
        }
        else
        {
            real rnew = r - m_radius * (m_magn - 1.0f);
            *x = m_xc + rnew * dx / r; 
            *y = m_yc + rnew * dy / r;
        }

        // Old version
        //-----------------
        //trans_warp_magnifier t(*this);
        //t.magnification(1.0f / m_magn);
        //t.radius(m_radius * m_magn);
        //t.transform(x, y);
    }

 T BivarStats<T>::sigmaSlope(void) const
 {
    if (ns>2)
       return sigmaYX() / (stdDevX() * SQRT(T(ns-1)));
    else
       return T();
 }

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
double ros_ErrorNorm ( 
  /*~~~> Input arguments */  
     volatile double Y[], double Ynew[], double Yerr[], 
     volatile double AbsTol[], volatile double RelTol[], 
     char VectorTol )
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
        Computes and returns the "scaled norm" of the error vector Yerr
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/   
{   	 
  /*~~~> Local variables */     
   double Err, Scale, Ymax;   
   int i;
   
   Err = ZERO;
   for (i=0; i<74; i++) {
	Ymax = MAX(ABS(Y[i]),ABS(Ynew[i]));
     if (VectorTol) {
       Scale = AbsTol[i]+RelTol[i]*Ymax;
     } else {
       Scale = AbsTol[0]+RelTol[0]*Ymax;
     } /* end if */
     Err = Err+(Yerr[i]*Yerr[i])/(Scale*Scale);
   } /* for i */
   Err  = SQRT(Err/(double)74);

   return Err;
   
} /* ros_ErrorNorm */