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 */
bool Sphere::checkRay (const Ray& ray, worldUnit& range, Vector& dist) const
{
dist = position.diff(ray.getStart());
worldUnit a = ray.getDir().dotProduct(dist);
auto squareLength = dist.dotProduct();
worldUnit D = squareRadius - squareLength + a * a;
//There is no intersection with sphere if D < 0
if (D < 0.f)
{
return false;
}
worldUnit t = SQRT(D);
if (squareLength >= squareRadius)
{ //We are outside sphere
a -= t;
}
else
{ //We are inside sphere
a += t;
}
if ( (a > 0.0f) && (a < range))
{
range = a;
return true;
}
return false;
}
static int32_t ctrl7a_set(CSOUND *csound, CTRL7a *p)
{
int32_t ctlno, chan;
MYFLT cutoff, b;
if ((ctlno = (int32_t) *p->ictlno) < 0 || ctlno > 127)
return csound->InitError(csound, Str("illegal controller number"));
else if ((chan=(int32_t) *p->ichan-1) < 0 || chan > 15)
return csound->InitError(csound, Str("illegal midi channel"));
else p->ctlno = ctlno;
if (*p->ifn > 0) {
if (UNLIKELY(((p->ftp = csound->FTnp2Find(csound, p->ifn)) == NULL)))
p->flag = 0; /* invalid ftable */
else p->flag= 1;
}
else p->flag= 0;
p->yt1 = FL(0.0);
if (*p->icutoff <= 0) cutoff = 5;
else cutoff = *p->icutoff;
b = FL(2.0) - COS(cutoff * csound->tpidsr * CS_KSMPS);
p->c2 = b - SQRT(b * b - 1.0);
p->c1 = FL(1.0) - p->c2;
p->prev = 0;
return OK;
}
static void *draw_worker(void *p)
{
t_data_thread *dt;
dt = (t_data_thread *)p;
pthread_mutex_lock(&dt->frac_mutex);
dt->f->p.y = 0;
while (dt->f->p.x < dt->end)
{
while (dt->f->p.y < HEIGHT)
{
chose_frac(dt->f);
while ((dt->f->z_r * dt->f->z_r) + (SQRT(dt->f->z_i)) <
4 && dt->f->i < dt->f->iter_max)
{
dt->f->tmp = dt->f->z_r;
choose_z(dt->f);
dt->f->i++;
}
put_color(dt->f, dt->img);
dt->f->p.y++;
}
dt->f->p.y = 0;
dt->f->p.x++;
}
pthread_mutex_unlock(&dt->frac_mutex);
return (NULL);
}
void matrix::GrowthCalc()
{
if (GrowthDone) return;
if (! IsIrreducible()) return;
// Ensure convergence by adding identity matrix
uint i;
for (i=0; i<n; i++) p[i][i]+=1;
GrowthDone = true;
decimal* Temp = new decimal[n];
for (i=0; i<n; i++) Temp[i] = 1.0/SQRT(static_cast<long double>(n));
decimal Eval = 1.0;
bool Finished = false;
while (!Finished)
{
Growth = Eval;
for (i=0; i<n; i++)
{
Evec[i] = 0.0;
for (uint j=0; j<n; j++) Evec[i] += decimal(p[i][j])*Temp[j];
}
Eval = EvecModulus();
Finished = true;
for (i=0; i<n; i++)
{
decimal Test = Evec[i]/Eval;
if (FABS(Test-Temp[i])>TOL/10.0) Finished = false;
Temp[i] = Test;
}
}
for (i=0; i<n; i++) Evec[i] = Temp[i];
Growth = Eval-1.0; //Compensate for added identity
for (i=0; i<n; i++) p[i][i]-=1;
delete [] Temp;
}
void thermalFeedbackCell(struct RUNPARAMS *param, struct CELL *cell,struct PART *curp, int level, REAL E){
// ----------------------------------------------------------
/// Inject an energy "E" in the cell "cell" on thermal form.
//----------------------------------------------------------
#ifdef SNTEST
#endif // SNTEST
printf("injecting Energy in thermal form within a cell\n");
//REAL mtot_feedback = curp->mass* param->sn->ejecta_proportion;
//REAL dv = POW(0.5,3.*level);
//REAL d_e = mtot_feedback/dv ;
//curp->mass -= mtot_feedback;
//cell->field.d += d_e;
cell->field.E += E;
cell->field.p += E*(GAMMA-1.);
//Energy conservation
#ifdef DUAL_E
// struct Utype U; // conservative field structure
// W2U(&cell->field, &U); // primitive to conservative
// U.eint*=1.+d_e/cell->field.d; // compute new internal energy
// U2W(&U, &cell->field); // back to primitive
#else
// cell->field.p*=1.+d_e/cell->field.d; // compute new internal energy
#endif
getE(&cell->field); //compute new total energy
cell->field.p=FMAX(cell->field.p,PMIN);
cell->field.a=SQRT(GAMMA*cell->field.p/cell->field.d); // compute new sound speed
}
void Calculate_Droptol( sparse_matrix *A, real *droptol, real dtol )
{
int i, j, k;
real val;
/* init droptol to 0 */
for( i = 0; i < A->n; ++i )
droptol[i] = 0;
/* calculate sqaure of the norm of each row */
for( i = 0; i < A->n; ++i ) {
for( k = A->start[i]; k < A->start[i+1]-1; ++k ) {
j = A->entries[k].j;
val = A->entries[k].val;
droptol[i] += val*val;
droptol[j] += val*val;
}
val = A->entries[k].val; // diagonal entry
droptol[i] += val*val;
}
/* calculate local droptol for each row */
//fprintf( stderr, "droptol: " );
for( i = 0; i < A->n; ++i ) {
//fprintf( stderr, "%f-->", droptol[i] );
droptol[i] = SQRT( droptol[i] ) * dtol;
//fprintf( stderr, "%f ", droptol[i] );
}
//fprintf( stderr, "\n" );
}
FLOAT_TYPE Error_ad( system_t *s, parameters_t *p ) {
FLOAT_TYPE real = Realspace_error( s, p);
FLOAT_TYPE recp = p3m_k_space_error_ad( s, p );
// printf("p3m ad error for mesh %d rcut %lf cao %d alpha %lf : real %e recp %e\n", p->mesh, p->rcut, p->cao, p->alpha, real, recp);
// printf("system size %d box %lf\n", s->nparticles, s->length);
return SQRT( SQR ( real ) + SQR ( recp ) );
}
CALCAMNT KStats::std(){
CALCAMNT result = 0.0;
if(data.count() == 0){
error_flag = TRUE;
#ifdef DEBUG_STATS
printf("set stats error\n");
#endif
return 0.0;
}
result = SQRT(std_kernel() / data.count());
#ifdef DEBUG_STATS
printf ("data.count %d\n",data.count());
#endif
#ifdef DEBUG_STATS
printf("std: %Lg\n",result);
#endif
return result;
}
int ComputeAvgVel( void* arg, int id ) {
double vaverh, velocity, counter, sq;
int i;
vaverh = vaver * timeStep ;
velocity = 0.0 ;
counter = 0.0 ;
int finish;
if (id == NTHREADS - 1) {
finish = numMoles;
}else {
finish = (numMoles / NTHREADS) * (id + 1);
}
for (i = (numMoles / NTHREADS) * id; i < finish; i++) {
sq = SQRT(SQR(vh[IND(0,i)]) + SQR(vh[IND(1,i)]) + SQR(vh[IND(2,i)]));
if (sq > vaverh) {
counter += 1.0;
}
velocity += sq ;
}
BEGIN_TRANSACTION();
vel += (velocity/timeStep);
count += counter;
COMMIT_TRANSACTION();
return 0;
}
/*
* This routine returns a normally distributed deviate with zero mean
* and unit variance. This uses wiprand() as the source of uniform
* deviates. Set the seed to any negative value to initialize or
* reinitialize the sequence.
*/
float wipgaussdev(long int *seed)
{
static int iset = 0;
static float gset;
float fac, rsq, v1, v2;
if (iset == 0) { /* There is no extra deviate handy. */
/*
* Pick two uniform numbers in the square extending from -1
* to +1 in each direction. If they are not inside the unit
* circle, then try again.
*/
do {
v1 = (2.0 * wiprand(seed)) - 1.0;
v2 = (2.0 * wiprand(seed)) - 1.0;
rsq = (v1 * v1) + (v2 * v2);
} while ((rsq >= 1.0) || (rsq == 0.0));
/*
* Make the Box-Muller transformation to get two normal deviates.
* Return one and save the other for the next time.
*/
fac = SQRT(-2.0 * LOG(rsq) / rsq);
gset = v1 * fac;
iset = 1; /* Set the flag. */
return(v2 * fac);
} else { /* There is an extra deviate available. */
iset = 0; /* Reset the flag. */
return(gset);
}
}
// Private helper routine for constructors from OrbitEph-based Ephemerides
void Rinex3NavData::loadFrom(const OrbitEph *oeptr)
{
time = oeptr->ctToc;
sat = RinexSatID(oeptr->satID);
satSys = string(1,sat.systemChar());
PRNID = sat.id;
//Toc = static_cast<GPSWeekSecond>(oeptr->ctToe).getSOW();
af0 = oeptr->af0;
af1 = oeptr->af1;
af2 = oeptr->af2;
//Toe = static_cast<GPSWeekSecond(oeptr->ctToe).getSOW();
M0 = oeptr->M0;
dn = oeptr->dn;
ecc = oeptr->ecc;
Ahalf = SQRT(oeptr->A);
OMEGA0 = oeptr->OMEGA0;
i0 = oeptr->i0;
w = oeptr->w;
OMEGAdot = oeptr->OMEGAdot;
idot = oeptr->idot;
Cuc = oeptr->Cuc;
Cus = oeptr->Cus;
Crc = oeptr->Crc;
Crs = oeptr->Crs;
Cic = oeptr->Cic;
Cis = oeptr->Cis;
}
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
KPP_REAL ros_ErrorNorm (
/*~~~> Input arguments */
KPP_REAL Y[], KPP_REAL Ynew[], KPP_REAL Yerr[],
KPP_REAL AbsTol[], KPP_REAL RelTol[],
char VectorTol )
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Computes and returns the "scaled norm" of the error vector Yerr
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
{
/*~~~> Local variables */
KPP_REAL Err, Scale, Ymax;
int i;
Err = ZERO;
for (i=0; i<KPP_NVAR; 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/(KPP_REAL)KPP_NVAR);
return Err;
} /* ros_ErrorNorm */
CA_FLOAT nuc_func_norm (CA_FLOAT Unuc, CA_FLOAT p1, CA_FLOAT p2, CA_FLOAT p3)
{
CA_FLOAT result;
result = p2 * 1 / (p1 * SQRT (2.0 * 3.14159)) * EXP (-((Unuc - p3) * (Unuc - p3)) / (2 * p1 * p1));
return (result);
}
FLOAT_TYPE p3m_k_space_error_ik_i ( system_t *s, parameters_t *p ) {
int nx, ny, nz;
FLOAT_TYPE he_q = 0.0;
FLOAT_TYPE alias1, alias2, alias3, alias4, n2;
int mesh = p->mesh;
he_q = p3m_find_error(p->alpha*s->length, mesh, p->cao, 1);
if(he_q < 0) {
he_q = 0.0;
for ( nx=-mesh/2; nx<mesh/2; nx++ ) {
for ( ny=-mesh/2; ny<mesh/2; ny++ ) {
for ( nz=-mesh/2; nz<mesh/2; nz++ ) {
if ( ( nx!=0 ) || ( ny!=0 ) || ( nz!=0 ) ) {
n2 = SQR ( nx ) + SQR ( ny ) + SQR ( nz );
p3m_tune_aliasing_sums_ik_i ( nx,ny,nz, s, p, &alias1,&alias2,&alias3, &alias4 );
he_q += alias1 - SQR ( alias2 ) / (0.5*n2*(SQR(alias3)+SQR(alias4)) );
}
}
}
}
he_q = FLOAT_ABS(he_q);
}
return 2.0*s->q2*SQRT ( he_q/ ( FLOAT_TYPE ) s->nparticles ) / ( SQR ( s->length ) );
}
/*
* refclock_sample - process a pile of samples from the clock
*
* This routine implements a recursive median filter to suppress spikes
* in the data, as well as determine a performance statistic. It
* calculates the mean offset and RMS jitter. A time adjustment
* fudgetime1 can be added to the final offset to compensate for various
* systematic errors. The routine returns the number of samples
* processed, which could be zero.
*/
static int
refclock_sample(
struct refclockproc *pp /* refclock structure pointer */
)
{
size_t i, j, k, m, n;
double off[MAXSTAGE];
double offset;
/*
* Copy the raw offsets and sort into ascending order. Don't do
* anything if the buffer is empty.
*/
n = 0;
while (pp->codeproc != pp->coderecv) {
pp->codeproc = (pp->codeproc + 1) % MAXSTAGE;
off[n] = pp->filter[pp->codeproc];
n++;
}
if (n == 0)
return (0);
if (n > 1)
qsort((void *)off, n, sizeof(off[0]), refclock_cmpl_fp);
/*
* Reject the furthest from the median of the samples until
* approximately 60 percent of the samples remain.
*/
i = 0; j = n;
m = n - (n * 4) / 10;
while ((j - i) > m) {
offset = off[(j + i) / 2];
if (off[j - 1] - offset < offset - off[i])
i++; /* reject low end */
else
j--; /* reject high end */
}
/*
* Determine the offset and jitter.
*/
pp->offset = 0;
pp->jitter = 0;
for (k = i; k < j; k++) {
pp->offset += off[k];
if (k > i)
pp->jitter += SQUARE(off[k] - off[k - 1]);
}
pp->offset /= m;
pp->jitter = max(SQRT(pp->jitter / m), LOGTOD(sys_precision));
#ifdef DEBUG
if (debug)
printf(
"refclock_sample: n %d offset %.6f disp %.6f jitter %.6f\n",
n, pp->offset, pp->disp, pp->jitter);
#endif
return (int)n;
}
