int SegmentsIntersect(const double a[2], const double b[2], const double c[2], const double d[2], double *x, double *t){
double c0, c1, c2, c3;
c0 = LeftTurn(a,b,c);
c1 = LeftTurn(a,b,d);
if(0 == c0 && 0 == c1){ return 0; } // collinear -> no intersection
if(dsign(c0) != dsign(c1)){
c2 = LeftTurn(c, d, a);
c3 = LeftTurn(c, d, b);
if(dsign(c2) != dsign(c3)){
if(NULL != x){
double r;
c1 = c0-c1; c3 = c2-c3;
if(fabs(c1) > fabs(c3)){
r = c0/c1;
x[0] = c[0] + r*(d[0]-c[0]);
x[1] = c[1] + r*(d[1]-c[1]);
}else{
r = c2/c3;
x[0] = a[0] + r*(b[0]-a[0]);
x[1] = a[1] + r*(b[1]-a[1]);
}
if(NULL != t){ *t = r; }
}
return 1;
}else{ return 0; }
}else{ return 0; }
}
main() {
double ans, a, b, c;
a = 2.0;
b = -5.0;
c = 7.0;
printf( "\n\n Test of utility routines." );
printf( "\n\n" );
ans = dsqr( a );
printf( "\n a = %lf ans = dsqr( a ) = %lf", a, ans );
ans = dcub( a );
printf( "\n a = %lf ans = dcub( a ) = %lf", a, ans );
ans = dpow4( a );
printf( "\n a = %lf ans = dpow4( a ) = %lf", a, ans );
ans = dpow5( a );
printf( "\n a = %lf ans = dpow5( a ) = %lf", a, ans );
ans = dsgn( a );
printf( "\n a = %lf ans = dsgn( a ) = %lf", a, ans );
ans = dsgn( b );
printf( "\n b = %lf ans = dsgn( b ) = %lf", b, ans );
ans = dsign( a, b );
printf( "\n a = %lf, b = %lf, ans = sign( a, b ) = %lf", a, b, ans );
ans = dpythag( a, b );
printf( "\n a = %lf, b = %lf, ans = pythag( a, b ) = %lf", a, b, ans );
ans = dmin( a, b );
printf( "\n a = %lf, b = %lf, ans = dmin( a, b ) = %lf", a, b, ans );
ans = dmax( a, b );
printf( "\n a = %lf, b = %lf, ans = dmax( a, b ) = %lf", a, b, ans );
ans = dmin3( a, b, c );
printf( "\n a = %lf, b = %lf, c = %lf, ans = dmin3( a, b, c ) = %lf", a, b, c, ans );
ans = dmax3( a, b, c );
printf( "\n a = %lf, b = %lf, c = %lf, ans = dmax3( a, b, c ) = %lf", a, b, c, ans );
printf( "\n\n" );
}
static int
pdf_coord__sort_compar_X (const void *pp1, const void *pp2)
{
pdf_coord *p1 = (pdf_coord *)pp1;
pdf_coord *p2 = (pdf_coord *)pp2;
if (pdf_coord__equal(p1, p2))
return 0;
else
return (int) dsign(p1->x - p2->x);
return 1;
}
/*
* This routine solves a quadratic equation:
* 0 = a0 + a1.x + a2.x^2
* placing the (real) roots in r1 and r2
* or the real and imaginary parts of a complex root in r1 and r2 respectively.
* The number of unique real roots is returned.
*/
int quadratic( double a0, double a1, double a2, double *r1, double *r2 ) {
double d, q, sign;
d = a1*a1 - 4.0*a2*a0;
if ( d < 0.0 ) {
/*
* Set the first root to the real part of the true complex root
* and the second root to the imaginary part of the true complex root.
*/
*r1 = -a1/(2.0*a2);
*r2 = -dsign(sqrt(-d),a1)/(2.0*a2);
return 0;
} else if ( d == 0.0 ) {
*r1 = *r2 = -a1/(2.0*a2);
return 1;
} else {
q = -0.5*(a1 + dsign(sqrt(d),a1));
*r1 = q/a2;
*r2 = a0/q;
return 2;
}
}
gboolean
upd_adj (GtkWidget * widget, progdata * pdat)
{
bardat *barra;
double temp;
if (GTK_OBJECT (widget) == pdat->barl.adj)
barra = &pdat->barl;
else
barra = &pdat->barr;
if (!pdat->flg.lock)
{
if (pdat->flg.dist)
{
temp = (GTK_ADJUSTMENT (barra->adj))->value + pdat->phys.ldist *
dsign (-barra->save + barra->alt->save);
/*
calcula onde por a outra lente quando "dist" está set
o dsign serve para ver a posições relativas
ie se soma ou subtrai uso temp pois só quero fazer aquilo^ uma vez
*/
if (temp > TOL &&
temp < pdat->drawbox->allocation.width * *pdat->phys.scl - TOL)
{
GTK_ADJUSTMENT (barra->alt->adj)->value = temp;
upd_mod (barra->alt, OFFPOS);
}
else
GTK_ADJUSTMENT (barra->adj)->value = barra->save;
}
upd_mod (barra, OFFPOS);
gtk_widget_queue_draw (pdat->window);
}
else
(GTK_ADJUSTMENT (barra->adj))->value = barra->save;
upd_phys (pdat);
return TRUE;
}
double dawson( double x ) {
int i, n0;
double d1, d2, e1, e2, sum, x2, xp, xx, ans;
static double c[NMAX+1];
static int init = 0;
if ( init == 0 ) {
init = 1;
for ( i = 1; i <= NMAX; ++i ) {
c[i] = exp( -dsqr( ( 2.0*i - 1.0 )*H ) );
}
}
if ( fabs(x) < XMIN ) {
/*
* Use series expansion.
*/
x2 = x*x;
ans = x*( 1.0 - (2.0/3.0)*x2*( 1.0 - (2.0/5.0)*x2*( 1.0 - (2.0/7.0)*x2 ) ) );
} else {
/*
* Use sampling theorem representation.
*/
xx = fabs(x);
n0 = 2*( (int)(0.5 + 0.5*xx/H) );
xp = xx - n0*H;
e1 = exp(2.0*xp*H);
e2 = e1*e1;
d1 = (double) n0 + 1.0;
d2 = d1 - 2.0;
sum = 0.0;
for ( i = 1; i <= NMAX; ++i ) {
sum += c[i]*( e1/d1 + 1.0/(d2*e1) );
d1 += 2.0;
d2 -= 2.0;
e1 *= e2;
}
ans = dsign( exp( -xp*xp ), x )*sum/SQRTPI;
}
return ans;
}
FLA_Error FLA_Househ3UD_UT_opd( int m_x1,
int m_y2,
double* chi_0,
double* x1, int inc_x1,
double* y2, int inc_y2,
double* tau )
{
double one_half = *FLA_DOUBLE_PTR( FLA_ONE_HALF );
double alpha;
double chi_0_minus_alpha;
double neg_chi_0_minus_alpha;
double abs_chi_0;
double norm_x_1;
double norm_y_2;
double lambda;
double abs_sq_chi_0_minus_alpha;
int i_one = 1;
//
// Compute the 2-norms of x_1 and y_2:
//
// norm_x_1 := || x_1 ||_2
// norm_y_2 := || y_2 ||_2
//
bl1_dnrm2( m_x1,
x1, inc_x1,
&norm_x_1 );
bl1_dnrm2( m_y2,
y2, inc_y2,
&norm_y_2 );
//
// If 2-norms of x_1, y_2 are zero, then return with trivial tau, chi_0 values.
//
if ( norm_x_1 == 0.0 &&
norm_y_2 == 0.0 )
{
*chi_0 = -(*chi_0);
*tau = one_half;
return FLA_SUCCESS;
}
//
// Compute the absolute value (magnitude) of chi_0, which equals the 2-norm
// of chi_0:
//
// abs_chi_0 := | chi_0 | = || chi_0 ||_2
//
bl1_dnrm2( i_one,
chi_0, i_one,
&abs_chi_0 );
//
// Compute lambda:
//
// lambda := sqrt( conj(chi0) chi0 + x1' x1 - y2' y2 )
//
lambda = sqrt( abs_chi_0 * abs_chi_0 +
norm_x_1 * norm_x_1 -
norm_y_2 * norm_y_2 );
// Compute alpha:
//
// alpha := - lambda * chi_0 / | chi_0 |
// = -sign( chi_0 ) * lambda
//
alpha = -dsign( *chi_0 ) * lambda;
//
// Overwrite x_1 and y_2 with u_1 and v_2, respectively:
//
// x_1 := x_1 / ( chi_0 - alpha )
// y_2 := y_2 / -( chi_0 - alpha )
//
chi_0_minus_alpha = (*chi_0) - alpha;
bl1_dinvscalv( BLIS1_NO_CONJUGATE,
m_x1,
&chi_0_minus_alpha,
x1, inc_x1 );
neg_chi_0_minus_alpha = -chi_0_minus_alpha;
bl1_dinvscalv( BLIS1_NO_CONJUGATE,
m_y2,
&neg_chi_0_minus_alpha,
y2, inc_y2 );
//
// Compute tau:
//
// tau := ( 1 + u_1' * u_1 - v_2' * v_2 ) / 2
// = ( ( chi_1 - alpha ) * conj( chi_1 - alpha ) + x_1' * x_1 - y_2' * y_2 ) /
// ( 2 * ( chi_1 - alpha ) * conj( chi_1 - alpha ) )
// = ( | chi_1 - alpha |^2 + || x_2 ||_2^2 - || y_2 ||_2^2 ) /
// ( 2 * | chi_1 - alpha |^2 )
//
abs_sq_chi_0_minus_alpha = chi_0_minus_alpha * chi_0_minus_alpha;
*tau = ( abs_sq_chi_0_minus_alpha +
norm_x_1 * norm_x_1 -
norm_y_2 * norm_y_2 ) /
( 2.0 * abs_sq_chi_0_minus_alpha );
//
// Overwrite chi_0 with alpha:
//
// chi_0 := alpha
//
*chi_0 = alpha;
return FLA_SUCCESS;
}
void iauPb06(double date1, double date2,
double *bzeta, double *bz, double *btheta)
/*
** - - - - - - - -
** i a u P b 0 6
** - - - - - - - -
**
** This function forms three Euler angles which implement general
** precession from epoch J2000.0, using the IAU 2006 model. Frame
** bias (the offset between ICRS and mean J2000.0) is included.
**
** This function is part of the International Astronomical Union's
** SOFA (Standards Of Fundamental Astronomy) software collection.
**
** Status: support function.
**
** Given:
** date1,date2 double TT as a 2-part Julian Date (Note 1)
**
** Returned:
** bzeta double 1st rotation: radians cw around z
** bz double 3rd rotation: radians cw around z
** btheta double 2nd rotation: radians ccw around y
**
** Notes:
**
** 1) The TT date date1+date2 is a Julian Date, apportioned in any
** convenient way between the two arguments. For example,
** JD(TT)=2450123.7 could be expressed in any of these ways,
** among others:
**
** date1 date2
**
** 2450123.7 0.0 (JD method)
** 2451545.0 -1421.3 (J2000 method)
** 2400000.5 50123.2 (MJD method)
** 2450123.5 0.2 (date & time method)
**
** The JD method is the most natural and convenient to use in
** cases where the loss of several decimal digits of resolution
** is acceptable. The J2000 method is best matched to the way
** the argument is handled internally and will deliver the
** optimum resolution. The MJD method and the date & time methods
** are both good compromises between resolution and convenience.
**
** 2) The traditional accumulated precession angles zeta_A, z_A,
** theta_A cannot be obtained in the usual way, namely through
** polynomial expressions, because of the frame bias. The latter
** means that two of the angles undergo rapid changes near this
** date. They are instead the results of decomposing the
** precession-bias matrix obtained by using the Fukushima-Williams
** method, which does not suffer from the problem. The
** decomposition returns values which can be used in the
** conventional formulation and which include frame bias.
**
** 3) The three angles are returned in the conventional order, which
** is not the same as the order of the corresponding Euler
** rotations. The precession-bias matrix is
** R_3(-z) x R_2(+theta) x R_3(-zeta).
**
** 4) Should zeta_A, z_A, theta_A angles be required that do not
** contain frame bias, they are available by calling the SOFA
** function iauP06e.
**
** Called:
** iauPmat06 PB matrix, IAU 2006
** iauRz rotate around Z-axis
**
** This revision: 2013 June 18
**
** SOFA release 2015-02-09
**
** Copyright (C) 2015 IAU SOFA Board. See notes at end.
*/
{
double r[3][3], r31, r32;
/* Precession matrix via Fukushima-Williams angles. */
iauPmat06(date1, date2, r);
/* Solve for z. */
*bz = atan2(r[1][2], r[0][2]);
/* Remove it from the matrix. */
iauRz(*bz, r);
/* Solve for the remaining two angles. */
*bzeta = atan2 (r[1][0], r[1][1]);
r31 = r[2][0];
r32 = r[2][1];
*btheta = atan2(-dsign(sqrt(r31 * r31 + r32 * r32), r[0][2]),
r[2][2]);
return;
/*----------------------------------------------------------------------
**
** Copyright (C) 2015
** Standards Of Fundamental Astronomy Board
** of the International Astronomical Union.
**
** =====================
** SOFA Software License
** =====================
**
** NOTICE TO USER:
**
** BY USING THIS SOFTWARE YOU ACCEPT THE FOLLOWING SIX TERMS AND
** CONDITIONS WHICH APPLY TO ITS USE.
**
** 1. The Software is owned by the IAU SOFA Board ("SOFA").
**
** 2. Permission is granted to anyone to use the SOFA software for any
** purpose, including commercial applications, free of charge and
** without payment of royalties, subject to the conditions and
** restrictions listed below.
**
** 3. You (the user) may copy and distribute SOFA source code to others,
** and use and adapt its code and algorithms in your own software,
** on a world-wide, royalty-free basis. That portion of your
** distribution that does not consist of intact and unchanged copies
** of SOFA source code files is a "derived work" that must comply
** with the following requirements:
**
** a) Your work shall be marked or carry a statement that it
** (i) uses routines and computations derived by you from
** software provided by SOFA under license to you; and
** (ii) does not itself constitute software provided by and/or
** endorsed by SOFA.
**
** b) The source code of your derived work must contain descriptions
** of how the derived work is based upon, contains and/or differs
** from the original SOFA software.
**
** c) The names of all routines in your derived work shall not
** include the prefix "iau" or "sofa" or trivial modifications
** thereof such as changes of case.
**
** d) The origin of the SOFA components of your derived work must
** not be misrepresented; you must not claim that you wrote the
** original software, nor file a patent application for SOFA
** software or algorithms embedded in the SOFA software.
**
** e) These requirements must be reproduced intact in any source
** distribution and shall apply to anyone to whom you have
** granted a further right to modify the source code of your
** derived work.
**
** Note that, as originally distributed, the SOFA software is
** intended to be a definitive implementation of the IAU standards,
** and consequently third-party modifications are discouraged. All
** variations, no matter how minor, must be explicitly marked as
** such, as explained above.
**
** 4. You shall not cause the SOFA software to be brought into
** disrepute, either by misuse, or use for inappropriate tasks, or
** by inappropriate modification.
**
** 5. The SOFA software is provided "as is" and SOFA makes no warranty
** as to its use or performance. SOFA does not and cannot warrant
** the performance or results which the user may obtain by using the
** SOFA software. SOFA makes no warranties, express or implied, as
** to non-infringement of third party rights, merchantability, or
** fitness for any particular purpose. In no event will SOFA be
** liable to the user for any consequential, incidental, or special
** damages, including any lost profits or lost savings, even if a
** SOFA representative has been advised of such damages, or for any
** claim by any third party.
**
** 6. The provision of any version of the SOFA software under the terms
** and conditions specified herein does not imply that future
** versions will also be made available under the same terms and
** conditions.
*
** In any published work or commercial product which uses the SOFA
** software directly, acknowledgement (see www.iausofa.org) is
** appreciated.
**
** Correspondence concerning SOFA software should be addressed as
** follows:
**
** By email: [email protected]
** By post: IAU SOFA Center
** HM Nautical Almanac Office
** UK Hydrographic Office
** Admiralty Way, Taunton
** Somerset, TA1 2DN
** United Kingdom
**
**--------------------------------------------------------------------*/
}
double iauAnpm(double a)
/*
** - - - - - - - -
** i a u A n p m
** - - - - - - - -
**
** Normalize angle into the range -pi <= a < +pi.
**
** This function is part of the International Astronomical Union's
** SOFA (Standards Of Fundamental Astronomy) software collection.
**
** Status: vector/matrix support function.
**
** Given:
** a double angle (radians)
**
** Returned (function value):
** double angle in range +/-pi
**
** This revision: 2013 June 18
**
** SOFA release 2013-12-02
**
** Copyright (C) 2013 IAU SOFA Board. See notes at end.
*/
{
double w;
w = fmod(a, D2PI);
if (fabs(w) >= DPI) w -= dsign(D2PI, a);
return w;
/*----------------------------------------------------------------------
**
** Copyright (C) 2013
** Standards Of Fundamental Astronomy Board
** of the International Astronomical Union.
**
** =====================
** SOFA Software License
** =====================
**
** NOTICE TO USER:
**
** BY USING THIS SOFTWARE YOU ACCEPT THE FOLLOWING SIX TERMS AND
** CONDITIONS WHICH APPLY TO ITS USE.
**
** 1. The Software is owned by the IAU SOFA Board ("SOFA").
**
** 2. Permission is granted to anyone to use the SOFA software for any
** purpose, including commercial applications, free of charge and
** without payment of royalties, subject to the conditions and
** restrictions listed below.
**
** 3. You (the user) may copy and distribute SOFA source code to others,
** and use and adapt its code and algorithms in your own software,
** on a world-wide, royalty-free basis. That portion of your
** distribution that does not consist of intact and unchanged copies
** of SOFA source code files is a "derived work" that must comply
** with the following requirements:
**
** a) Your work shall be marked or carry a statement that it
** (i) uses routines and computations derived by you from
** software provided by SOFA under license to you; and
** (ii) does not itself constitute software provided by and/or
** endorsed by SOFA.
**
** b) The source code of your derived work must contain descriptions
** of how the derived work is based upon, contains and/or differs
** from the original SOFA software.
**
** c) The names of all routines in your derived work shall not
** include the prefix "iau" or "sofa" or trivial modifications
** thereof such as changes of case.
**
** d) The origin of the SOFA components of your derived work must
** not be misrepresented; you must not claim that you wrote the
** original software, nor file a patent application for SOFA
** software or algorithms embedded in the SOFA software.
**
** e) These requirements must be reproduced intact in any source
** distribution and shall apply to anyone to whom you have
** granted a further right to modify the source code of your
** derived work.
**
** Note that, as originally distributed, the SOFA software is
** intended to be a definitive implementation of the IAU standards,
** and consequently third-party modifications are discouraged. All
** variations, no matter how minor, must be explicitly marked as
** such, as explained above.
**
** 4. You shall not cause the SOFA software to be brought into
** disrepute, either by misuse, or use for inappropriate tasks, or
** by inappropriate modification.
**
** 5. The SOFA software is provided "as is" and SOFA makes no warranty
** as to its use or performance. SOFA does not and cannot warrant
** the performance or results which the user may obtain by using the
** SOFA software. SOFA makes no warranties, express or implied, as
** to non-infringement of third party rights, merchantability, or
** fitness for any particular purpose. In no event will SOFA be
** liable to the user for any consequential, incidental, or special
** damages, including any lost profits or lost savings, even if a
** SOFA representative has been advised of such damages, or for any
** claim by any third party.
**
** 6. The provision of any version of the SOFA software under the terms
** and conditions specified herein does not imply that future
** versions will also be made available under the same terms and
** conditions.
*
** In any published work or commercial product which uses the SOFA
** software directly, acknowledgement (see www.iausofa.org) is
** appreciated.
**
** Correspondence concerning SOFA software should be addressed as
** follows:
**
** By email: [email protected]
** By post: IAU SOFA Center
** HM Nautical Almanac Office
** UK Hydrographic Office
** Admiralty Way, Taunton
** Somerset, TA1 2DN
** United Kingdom
**
**--------------------------------------------------------------------*/
}
static void
tqli(int n, double* d, double** z, double* e, int matz, double toler)
{
register int k;
int i,l,m,iter;
double g,r,s,c,p,f,b;
double azi;
f=0.0;
if (n == 1) {
d[0]=z[0][0];
z[0][0] = 1.0;
return;
}
for (i=1; i < n ; i++) e[i-1] = e[i];
e[n-1] = 0.0;
for (l=0; l < n; l++) {
iter = 0;
L1:
for (m=l; m < n-1;m++) if (fabs(e[m]) < toler) goto L2;
m=n-1;
L2:
if (m != l) {
if (iter++ == 30) {
fprintf (stderr,"tqli not converging %d %g\n",l,e[l]);
continue;
}
g = (d[l+1]-d[l])/(2.0*e[l]);
r = sqrt(g*g + 1.0);
g = d[m] - d[l] + e[l]/((g + dsign(r,g)));
s=1.0;
c=1.0;
p=0.0;
for (i=m-1; i >= l; i--) {
f = s*e[i];
b = c*e[i];
if (fabs(f) >= fabs(g)) {
c = g/f;
r = sqrt(c*c + 1.0);
e[i+1] = f*r;
s=1.0/r;
c *= s;
}
else {
s = f/g;
r = sqrt(s*s + 1.0);
e[i+1] = g*r;
c = 1.0/r;
s *= c;
}
g = d[i+1] - p;
r = (d[i]-g)*s + 2.0*c*b;
p = s*r;
d[i+1] = g+p;
g = c*r-b;
if (matz) {
double *zi = z[i];
double *zi1 = z[i+1];
for (k=n; k ; k--,zi++,zi1++) {
azi = *zi;
f = *zi1;
*zi1 = azi*s + c*f;
*zi = azi*c - s*f;
}
}
}
d[l] -= p;
e[l] = g;
e[m] = 0.0;
goto L1;
}
}
}
static void
tred2(int n,double** a,double* d,double* e,int matz)
{
int i,j,k,l;
double f,g,h,hh,scale,scale_inv,h_inv;
if (n == 1) return;
for(i=n-1; i > 0; i--) {
l = i-1;
h = 0.0;
scale = 0.0;
if(l) {
for(k=0; k <= l; k++) scale += fabs(a[i][k]);
if (scale == 0.0) e[i] = a[i][l];
else {
scale_inv=1.0/scale;
for (k=0; k <= l; k++) {
a[i][k] *= scale_inv;
h += a[i][k]*a[i][k];
}
f=a[i][l];
g= -(dsign(sqrt(h),f));
e[i] = scale*g;
h -= f*g;
a[i][l] = f-g;
f = 0.0;
h_inv=1.0/h;
for (j=0; j <= l; j++) {
if (matz) a[j][i] = a[i][j]*h_inv;
g = 0.0;
for (k=0; k <= j; k++) g += a[j][k]*a[i][k];
if (l > j) for (k=j+1; k <= l; k++) g += a[k][j]*a[i][k];
e[j] = g*h_inv;
f += e[j]*a[i][j];
}
hh = f/(h+h);
for (j=0; j <= l; j++) {
f = a[i][j];
g = e[j] - hh*f;
e[j] = g;
for (k=0; k <= j; k++) a[j][k] -= (f*e[k] + g*a[i][k]);
}
}
}
else {
e[i] = a[i][l];
}
d[i] = h;
}
if(matz) d[0] = 0.0;
e[0] = 0.0;
for(i=0; i < n; i++) {
l = i-1;
if (matz) {
if(d[i]) {
for(j=0; j <= l; j++) {
g = 0.0;
for(k=0; k <= l; k++) g += a[i][k]*a[k][j];
for(k=0; k <= l; k++) a[k][j] -= g*a[k][i];
}
}
}
d[i] = a[i][i];
if(matz) {
a[i][i] = 1.0;
if(l >= 0) for (j=0; j<= l; j++) a[i][j] = a[j][i] = 0.0;
}
}
}
void bessel_jy( double xnu, double x, double *rj, double *ry, double *rjp, double *ryp ) {
const double EPS = 1.0e-16;
const double FPMIN = 1.0e-30;
const int MAXIT = 10000;
const double XMIN = 2.0;
int i, isign, l, nl;
double a, b, br, bi, c, cr, ci, d, del, del1, den, di, dlr, dli, dr, e, f, fact, fact2,
fact3, ff, gam, gam1, gam2, gammi, gampl, h, p, pimu, pimu2, q, r, rjl,
rjl1, rjmu, rjp1, rjpl, rjtemp, ry1, rymu, rymup, rytemp, sum, sum1,
temp, w, x2, xi, xi2, xmu, xmu2;
if ( x <= 0.0 || xnu < 0.0 ) nrerror( "Bad arguments in bessel_jy." );
nl = ( x < XMIN ? (int)(xnu + 0.5) : imax( 0, (int)(xnu - x + 1.5) ) );
xmu = xnu - nl;
xmu2 = xmu*xmu;
xi = 1.0/x;
xi2 = 2.0*xi;
w = xi2/PI;
isign = 1;
h = xnu*xi;
if ( h < FPMIN ) h = FPMIN;
b = xi2*xnu;
d = 0.0;
c = h;
for ( i = 1; i <= MAXIT; ++i ) {
b += xi2;
d = b - d;
if ( fabs(d) < FPMIN ) d = FPMIN;
c = b - 1.0/c;
if ( fabs(c) < FPMIN ) c = FPMIN;
d = 1.0/d;
del = c*d;
h = del*h;
if ( d < 0.0 ) isign = -isign;
if ( fabs(del - 1.0) < EPS ) break;
}
if ( i > MAXIT ) nrerror( "Argument x too large in bessel_jy; try asymptotic expansion." );
rjl = isign*FPMIN;
rjpl = h*rjl;
rjl1 = rjl;
rjp1 = rjpl;
fact = xnu*xi;
for ( l = nl; l >= 1; --l ) {
rjtemp = fact*rjl + rjpl;
fact -= xi;
rjpl = fact*rjtemp - rjl;
rjl = rjtemp;
}
if ( rjl == 0.0 ) rjl = EPS;
f= rjpl/rjl;
if ( x < XMIN ) {
x2 = 0.5*x;
pimu = PI*xmu;
fact = ( fabs(pimu) < EPS ? 1.0 : pimu/sin(pimu) );
d = -log(x2);
e = xmu*d;
fact2 = ( fabs(e) < EPS ? 1.0 : sinh(e)/e );
bessel_cheb( xmu, &gam1, &gam2, &gampl, &gammi );
ff = (2.0/PI)*fact*(gam1*cosh(e) + gam2*fact2*d);
e = exp(e);
p = e/(PI*gampl);
q = 1.0/(e*PI*gammi);
pimu2 = 0.5*pimu;
fact3 = (fabs(pimu2) < EPS ? 1.0 : sin(pimu2)/pimu2 );
r = PI*pimu2*fact3*fact3;
c = 1.0;
d = -x2*x2;
sum = ff + r*q;
sum1 = p;
for ( i = 1; i <= MAXIT; ++i ) {
ff = (i*ff + p + q)/(i*i - xmu2);
c *= d/i;
p /= i - xmu;
q /= i + xmu;
del = c*(ff + r*q);
sum += del;
del1 = c*p - i*del;
sum1 += del1;
if ( fabs(del) < EPS*(1.0 + fabs(sum)) ) break;
}
if ( i > MAXIT ) nrerror("Bessel y series failed to converge in bessel_jy." );
rymu = -sum;
ry1 = -sum1*xi2;
rymup = xmu*xi*rymu - ry1;
rjmu = w/(rymup - f*rymu);
} else {
a = 0.25 - xmu2;
q = 1.0;
p = -0.5*xi;
br = 2.0*x;
bi = 2.0;
fact = a*xi/(p*p + q*q);
cr = br + q*fact;
ci = bi + p*fact;
den = br*br + bi*bi;
dr = br/den;
di = -bi/den;
dlr = cr*dr - ci*di;
dli = cr*di + ci*dr;
temp = p*dlr - q*dli;
q = p*dli + q*dlr;
p = temp;
for ( i = 2; i <= MAXIT; ++i ) {
a += 2*(i - 1);
bi += 2.0;
dr = a*dr + br;
di = a*di + bi;
if ( fabs(dr) + fabs(di) < FPMIN ) dr = FPMIN;
fact = a/(cr*cr + ci*ci);
cr = br + cr*fact;
ci = bi - ci*fact;
if ( fabs(cr) + fabs(ci) < FPMIN ) cr = FPMIN;
den = dr*dr + di*di;
dr /= den;
di /= -den;
dlr = cr*dr - ci*di;
dli = cr*di + ci*dr;
temp = p*dlr - q*dli;
q = p*dli + q*dlr;
p = temp;
if ( fabs(dlr - 1.0) + fabs(dli) < EPS ) break;
}
if ( i > MAXIT ) nrerror( "Lentz's method failed in bessel_jy." );
gam = (p - f)/q;
rjmu = sqrt(w/((p - f)*gam + q));
rjmu = dsign( rjmu, rjl );
rymu = rjmu*gam;
rymup = rymu*(p + q/gam);
ry1 = xmu*xi*rymu - rymup;
}
fact = rjmu/rjl;
*rj = rjl1*fact;
*rjp = rjp1*fact;
for ( i = 1; i <= nl; ++i ) {
rytemp = (xmu + i)*xi2*ry1 - rymu;
rymu = ry1;
ry1 = rytemp;
}
*ry = rymu;
*ryp = xnu*xi*rymu - ry1;
}
/** Update the current poses of the robot
*/
static void conscious_thought(void) {
char *scmd;
int execute;
struct timeval currtime;
// get a speech command, and place that as
// the priority task
/*if ((scmd = get_speech_command())) {
if (strstr(scmd, "fetch")) {
task = "fetch";
} else if (strstr(scmd, "stop")) {
task = "stop";
}
}*/
// FSM (taskgraph)
execute = 0;
printf("Updating objects...\n");
update_object_positions();
gettimeofday(&currtime, NULL);
printf("Object positions:\n");
printf("<ball>\n");
for (int n = 0; n < (int)ballpos.size(); n++) {
printf("Ball: %f %f %f\n", ballpos[n].x, ballpos[n].y, ballpos[n].z);
}
printf("</ball>\n");
printf("<basket>\n");
for (int n = 0; n < (int)basketpos.size(); n++) {
printf("Basket: %f %f %f\n", basketpos[n].x, basketpos[n].y, basketpos[n].z);
}
printf("</basket>\n");
printf("Updating states...\n");
while (!execute) {
if (strcmp(task, "fetch") == 0) {
// transitions
// TODO: create a counter resolution for infinite loops
// TODO: pick up more than 1 ball
switch (subtask) {
case S_IDLE:
printf("State: IDLE\n");
if (num_balls_in_basket() == 0) {
subtask = S_FINDBALL;
} else {
subtask = S_FINDBASKET;
// execute = 1;
}
break;
case S_FINDBALL:
printf("State: FIND BALL\n");
if (num_balls_in_basket() != 0) {
subtask = S_IDLE;
} else if (!ballfound()) {
execute = 1;
} else {
subtask = S_GOTOBALL;
}
break;
case S_GOTOBALL:
printf("State: GOTO BALL\n");
if (num_balls_in_basket() != 0) {
subtask = S_IDLE;
} else if (!ballfound()) {
subtask = S_FINDBALL;
} else if (can_pickup(closest_object(ballpos))) {
subtask = S_PICKBALL;
} else {
execute = 1;
}
break;
case S_PICKBALL:
printf("State: PICK BALL\n");
if (num_balls_in_basket() != 0) {
subtask = S_IDLE;
} else if (!ballfound()) {
subtask = S_FINDBALL;
} else if (!can_pickup(closest_object(ballpos))) {
subtask = S_GOTOBALL;
} else {
execute = 1;
}
break;
case S_FINDBASKET:
printf("State: FIND BASKET\n");
if (!basketfound()) {
execute = 1;
} else {
subtask = S_GOTOBASKET;
}
break;
case S_GOTOBASKET:
printf("State: GOTO BASKET\n");
if (!basketfound()) {
subtask = S_FINDBASKET;
} else if (can_drop(closest_object(basketpos))) {
// have to do a strange thing here with timers
gettimeofday(&actiontime, NULL);
subtask = S_DROPBASKET_PHASE1;
} else {
execute = 1;
}
break;
case S_DROPBASKET_PHASE1:
printf("State: DROP BASKET P1\n");
if (difftime(currtime, actiontime) < 1.2) {
execute = 1;
} else {
gettimeofday(&actiontime, NULL);
subtask = S_DROPBASKET_PHASE2;
}
break;
case S_DROPBASKET_PHASE2:
printf("State: DROP BASKET P2\n");
if (difftime(currtime, actiontime) < 0.5) {
execute = 1;
} else {
gettimeofday(&actiontime, NULL);
subtask = S_DROPBASKET_PHASE3;
}
break;
case S_DROPBASKET_PHASE3:
printf("State: DROP BASKET P3\n");
if (difftime(currtime, actiontime) < 1.0) {
execute = 1;
} else {
gettimeofday(&actiontime, NULL);
subtask = S_DROPBASKET_PHASE4;
}
break;
case S_DROPBASKET_PHASE4:
printf("State: DROP BASKET P4\n");
if (difftime(currtime, actiontime) < 1.0) {
execute = 1;
} else {
gettimeofday(&actiontime, NULL);
subtask = S_DROPBASKET_PHASE5;
}
break;
case S_DROPBASKET_PHASE5:
printf("State: DROP BASKET P5\n");
if (difftime(currtime, actiontime) < 1.2) {
execute = 1;
} else {
subtask = S_IDLE;
}
break;
default:
subtask = S_IDLE;
}
} else if (strcmp(task, "stop") == 0) {
subtask = S_IDLE;
printf("State: IDLE\n");
execute = 1;
}
}
printf("Executing...\n");
// move accordingly to the state
// TODO: do collision detection, stop the robot from moving
switch (subtask) {
case S_IDLE:
set_robot(0.0, 0.0, 0.0, 0.0);
break;
case S_FINDBALL:
set_robot(0.0, 0.30 * find_coeff, 0.0, 0.0);
break;
case S_GOTOBALL:
set_robot(0.32, -ballpos[0].x / 1000.0, -1.0, 0.0);
find_coeff = dsign(-ballpos[0].x);
break;
case S_PICKBALL:
set_robot(0.30, -ballpos[0].x / 1000.0, -1.0, 1.0);
find_coeff = dsign(-ballpos[0].x);
break;
case S_FINDBASKET:
set_robot(0.0, -0.30 * find_coeff, 0.0, 0.0);
break;
case S_GOTOBASKET:
set_robot(0.32, -basketpos[0].x / 1200.0, 0.0, 0.0);
find_coeff = dsign(-basketpos[0].x);
break;
case S_DROPBASKET_PHASE1:
set_robot(0.0, 0.0, 1.0, 0.0);
break;
case S_DROPBASKET_PHASE2:
set_robot(0.40, -basketpos[0].x / 1200.0, 0.0, 0.0);
find_coeff = dsign(-basketpos[0].x);
break;
case S_DROPBASKET_PHASE3:
set_robot(0.0, 0.0, 0.0, -1.0);
break;
case S_DROPBASKET_PHASE4:
set_robot(-0.40, 0.0, 0.0, 0.0);
break;
case S_DROPBASKET_PHASE5:
set_robot(0.0, 0.0, -1.0, 0.0);
break;
default:
set_robot(0.0, 0.0, 0.0, 0.0);
}
}
