void findmins (SpiceDouble beg, SpiceDouble end) {
SpiceInt i, count;
// SPICEDOUBLE_CELLs are static, so must reinit them each time, sigh
SPICEDOUBLE_CELL(result, 200);
SPICEDOUBLE_CELL(cnfine,2);
scard_c(0,&result);
scard_c(0,&cnfine);
// create interval
wninsd_c(beg,end,&cnfine);
// get results
gfuds_c(gfq,gfdecrx,"LOCMIN",0.,0.,86400.,MAXWIN,&cnfine,&result);
count = wncard_c(&result);
for (i=0; i<count; i++) {
wnfetd_c(&result,i,&beg,&end);
// becuase beg and end are equal, overwriting end with actual value
gfq(beg,&end);
printf("M %f %f %f\n",et2jd(beg),r2d(end),r2d(sunminangle(beg,planetcount,planets)));
}
}
void uiDrawMatrixSkew(uiDrawMatrix *m, double x, double y, double xamount, double yamount)
{
D2D1_MATRIX_3X2_F dm;
D2D1_POINT_2F center;
m2d(m, &dm);
center.x = x;
center.y = y;
dm = dm * D2D1::Matrix3x2F::Skew(r2d(xamount), r2d(yamount), center);
d2m(&dm, m);
}
inline double CvCapture_FFMPEG::get_duration_sec()
{
double sec = (double)ic->duration / (double)AV_TIME_BASE;
if (sec < eps_zero)
{
sec = (double)ic->streams[video_stream]->duration * r2d(ic->streams[video_stream]->time_base);
}
if (sec < eps_zero)
{
sec = (double)ic->streams[video_stream]->duration * r2d(ic->streams[video_stream]->time_base);
}
return sec;
}
int main(){
V6 v6;
DMS dms;
HMS hms;
double ra=0.0, de=0.0, pmra=0.0, pmde=0.0, px=0.0, rv=0.0, C=0.0;
/* Barnard's star from Hipparcos catalog.
ICRS Epoch J1991.25 */
ra = d2r(269.45402305);
de = d2r(4.66828815);
px = 549.01 / 1000.0; /* Arc seconds */
rv = 0.0;
pmra = (-797.84 / 1000.0 ) / cos(de); /* pmra * cos(de) into pmra */
pmra *= 100.0; /* Arcseconds per century. */
pmde = (10326.93 / 1000.0);
pmde *= 100.0; /* Arcseconds per century. */
C = CJ;
printf("RA %f DE %f PMRA %f PMDE %f PX %f RV %f\n",
ra, de, pmra, pmde, px, rv);
v6 = v6init(CARTESIAN);
v6 = cat2v6(ra, de, pmra, pmde, px, rv, C);
v6 = proper_motion(v6, J2000, JYEAR2JD(1991.25));
printf("RAJ2000: %s DEJ2000:%s\n", fmt_alpha(v6GetAlpha(v6c2s(v6))),
fmt_d(r2d(v6GetDelta(v6c2s(v6)))));
hms = hms2hms(r2hms(v6GetAlpha(v6c2s(v6))));
dms = dms2dms(r2dms(v6GetDelta(v6c2s(v6))));
printf("RAJ2000: hh %.1f mm %.1f ss %.4f\n", hms.hh, hms.mm, hms.ss);
printf("DEJ2000: dd %.1f mm %.1f ss %.4f\n", dms.dd, dms.mm, dms.ss);
return 0;
}
AMControlInfoList REIXSXESCalibration::computeSpectrometerPosition(int gratingIndex, double eV, double focusOffsetMm, double tiltOffsetDeg) const
{
qDebug() << "Spectrometer Geometry calculations for: " << gratingNames().at(gratingIndex) << eV << "eV," << focusOffsetMm << "mm defocus," << tiltOffsetDeg << "deg tilt offset:";
AMControlInfoList rv;
double hexU = hexapodU(gratingIndex);
QVector3D hexXYZ = hexapodXYZ(gratingIndex);
QVector3D hexRST = hexapodRST(gratingIndex);
QVector3D detPos = detectorPos(eV, gratingIndex);
double theta = spectrometerTheta(detPos);
double translation = detectorTranslation(detPos, theta, focusOffsetMm);
double spectrometerRotation = spectrometerRotationDrive(theta);
double tilt = tiltStageDrive(eV, gratingIndex, theta, tiltOffsetDeg);
rv.append(AMControlInfo("spectrometerRotationDrive", spectrometerRotation, 0,0, "mm", 0.1, "Linear drive motor for spectrometer angle"));
rv.append(AMControlInfo("detectorTranslation", translation, 0,0, "mm", 0.1, "Detector translation"));
rv.append(AMControlInfo("detectorTiltDrive", tilt, 0.0, 0.0, "mm", 0.1, "Linear drive motor for detector tilt"));
rv.append(AMControlInfo("hexapodX", hexXYZ.x(), 0, 0, "mm", 0.1, "Hexapod X position"));
rv.append(AMControlInfo("hexapodY", hexXYZ.y(), 0, 0, "mm", 0.1, "Hexapod Y position"));
rv.append(AMControlInfo("hexapodZ", hexXYZ.z(), 0, 0, "mm", 0.1, "Hexapod Z position"));
rv.append(AMControlInfo("hexapodU", hexU, 0,0, "deg", 0.1, "Hexapod Tilt around X axis"));
rv.append(AMControlInfo("hexapodR", hexRST.x(), 0,0, "mm", 0.1, "Hexapod rotation point R"));
rv.append(AMControlInfo("hexapodS", hexRST.y(), 0,0, "mm", 0.1, "Hexapod rotation point S"));
rv.append(AMControlInfo("hexapodT", hexRST.z(), 0,0, "mm", 0.1, "Hexapod rotation point T"));
qDebug() << " Alpha required for rowland condition (deg):" << r2d(alpha(gratingIndex));
qDebug() << " Beta using that alpha, at " << eV << "eV:" << r2d(beta(eV, gratingIndex));
qDebug() << " Angle of slit-origin ray above y axis (deg)" << r2d(sTheta(gratingIndex));
qDebug() << " Grating tilt to achieve required alpha (deg):" << hexU;
qDebug() << " r (mm):" << r(gratingIndex);
qDebug() << " r-prime (mm):" << rPrime(eV, gratingIndex);
qDebug() << " Spectrometer dTheta: angle up from y axis to center of detector (deg):" << r2d(dTheta(eV, gratingIndex));
qDebug() << " Detector position:" << detPos;
qDebug() << " Spectrometer theta: (deg)" << r2d(theta);
qDebug() << " Translation: (mm)" << translation;
qDebug() << " Spectrometer rotation stage translation: (mm)" << spectrometerRotation;
qDebug() << " Detector tilt phi (detector angle down to positive y axis) (deg):" << r2d(detectorPhi(eV, gratingIndex));
qDebug() << " Extra tilt on top of spectrometer angle" << r2d(detectorPhi(eV, gratingIndex) - theta);
qDebug() << " Tilt stage translation: (mm)" << tilt;
return rv;
}
double
do_acosd(double x)
{
double temp;
errno = 0;
temp = r2d(acos(x));
MATH_ERROR("acosd");
return (temp);
}
double
do_asind(double x)
{
double temp;
errno = 0;
temp = r2d(asin(x));
MATH_ERROR("asind");
return (temp);
}
double
do_atan2d(double x, double y)
{
double temp;
errno = 0;
temp = r2d(atan2(x, y));
MATH_ERROR("atan2d");
return (temp);
}
/* DO_ANGLE: Calculate angle (degrees) between vector 1 at (0,0; x1,y1) and
* vector 2 at (0,0; x2,y2).
*/
double do_angled(double x1, double y1, double x2, double y2)
{
double temp;
temp = ((x1 * x2) + (y1 * y2)) / (HYPOT(x1, y1) * HYPOT(x2, y2));
errno = 0;
temp = r2d(acos(temp));
MATH_ERROR("angled");
return (temp);
}
inline double CvCapture_FFMPEG::get_fps()
{
double fps = r2d(ic->streams[video_stream]->r_frame_rate);
#if LIBAVFORMAT_BUILD >= CALC_FFMPEG_VERSION(52, 111, 0)
if (fps < eps_zero)
{
fps = r2d(ic->streams[video_stream]->avg_frame_rate);
}
#endif
if (fps < eps_zero)
{
fps = 1.0 / r2d(ic->streams[video_stream]->codec->time_base);
}
return fps;
}
inline void CvCapture_FFMPEG::seek(s64 _frame_number)
{
_frame_number = std::min(_frame_number, get_total_frames());
int delta = 16;
// if we have not grabbed a single frame before first seek, let's read the first frame
// and get some valuable information during the process
if( first_frame_number < 0 && get_total_frames() > 1 )
grabFrame();
for(;;)
{
s64 _frame_number_temp = std::max(_frame_number-delta, (s64)0);
double sec = (double)_frame_number_temp / get_fps();
s64 time_stamp = ic->streams[video_stream]->start_time;
double time_base = r2d(ic->streams[video_stream]->time_base);
time_stamp += (s64)(sec / time_base + 0.5);
if (get_total_frames() > 1) av_seek_frame(ic, video_stream, time_stamp, AVSEEK_FLAG_BACKWARD);
avcodec_flush_buffers(ic->streams[video_stream]->codec);
if( _frame_number > 0 )
{
grabFrame();
if( _frame_number > 1 )
{
frame_number = dts_to_frame_number(picture_pts) - first_frame_number;
//printf("_frame_number = %d, frame_number = %d, delta = %d\n",
// (int)_frame_number, (int)frame_number, delta);
if( frame_number < 0 || frame_number > _frame_number-1 )
{
if( _frame_number_temp == 0 || delta >= INT_MAX/4 )
break;
delta = delta < 16 ? delta*2 : delta*3/2;
continue;
}
while( frame_number < _frame_number-1 )
{
if(!grabFrame())
break;
}
frame_number++;
break;
}
else
{
frame_number = 1;
break;
}
}
else
{
frame_number = 0;
break;
}
}
}
int camera_sphere(double f,double v,double h,double o){
int x,y;
double a_v=r2d(2*atan(v/(2*f)));
double a_h=r2d(2*atan(h/(2*f)));
verbose(L_CAMR,"%s: focal: %6.2f mm",cam->name,f);
verbose(L_CAMR,"%s: υ: sensor: %6.2f • %6.2f mm",cam->name,v,h);
verbose(L_CAMR,"%s: υ: α: %6.2f • %6.2f °",cam->name,a_v,a_h);
a_v=a_v*(1-(o/2));a_h=a_h*(1-(o/2));
verbose(L_CAMR,"%s: υ: α': %6.2f • %6.2f °",cam->name,a_v,a_h);
verbose(L_CAMR,"%s: υ: Ξ: %6.3f",cam->name,o);
for(y=0;y<=ceil(180/a_v);y++){
double pos_theta=(y*(180/ceil(180/a_v)))-90;
for(x=0;x<ceil(360/(a_h/cos(d2r(pos_theta))));x++){
double pos_phi=x*(360/ceil(360/(a_h/cos(d2r(pos_theta)))));
go(0,pos_theta,pos_phi);
trigger_shot(0);
}
}
}
int main(){
double ra = (12+22/60.0+54.899/3600.0) * (2*M_PI/24.0);
double de = (15+49/60.0+20.57/3600.0) * (2*M_PI/360.0);
double ra1, ra1_d, de1, de1_d;
double ep = J2000;
double eq = J2000;
V6 v6;
V6 pvec[N_TPM_STATES];
TPM_TSTATE tstate;
int s1 = TPM_S06; /* Heliocentric mean J2000 FK5 ~~ ICRS */
int s2 = TPM_S00; /* Assign required states. */
for(int i=TPM_S00; i < N_TPM_STATES; i ++){
tpm_data(&tstate, TPM_INIT);
tstate.utc = J2000;
tstate.lon = d2r(-111.598333);
tstate.lat = d2r(31.956389);
tstate.alt = 2093.093;
tstate.delta_ut = delta_UT(tstate.utc);
tpm_data(&tstate, TPM_ALL);
v6 = v6init(SPHERICAL);
v6SetR(v6, 1e9);
v6SetAlpha(v6, ra);
v6SetDelta(v6, de);
pvec[s1] = v6s2c(v6);
s2 = i;
tpm(pvec, s1, s2, ep, eq, &tstate);
v6 = v6c2s(pvec[s2]);
ra1 = v6GetAlpha(v6);
de1 = v6GetDelta(v6);
ra1_d = r2d(ra1);
if (ra1_d < 0.0) ra1_d += 360.0;
de1_d = r2d(de1);
if (de1_d < 0.0) de1_d += 360.0;
printf("%02d-%02d %-17s %s %s %8.4f %8.4f\n", s1, s2, tpm_state(s2), fmt_alpha(ra1), fmt_delta(de1), ra1_d, de1_d);
}
return 0;
}
void uiDrawMatrixRotate(uiDrawMatrix *m, double x, double y, double amount)
{
D2D1_MATRIX_3X2_F dm;
D2D1_POINT_2F center;
m2d(m, &dm);
center.x = x;
center.y = y;
dm = dm * D2D1::Matrix3x2F::Rotation(r2d(amount), center);
d2m(&dm, m);
}
int main(){
V6 v6;
#define L 3
double ha, dec;
double az[L] = {0, 90, 133.30805555555557};
double el[L] = {90, -45, 59.086111111111116};
v6 = v6init(SPHERICAL);
v6SetR(v6, 1e9);
for (int i=0; i < L; i++){
v6SetAlpha(v6, d2r(az[i]));
v6SetDelta(v6, d2r(el[i]));
v6 = v6c2s(azel2hadec(v6s2c(v6), d2r(43.07833)));
ha = r2d(r2r(v6GetAlpha(v6)));
dec = r2d(r2r(v6GetDelta(v6)));
printf("%.9f %.9f\n", ha, dec);
}
return 0;
}
int camera_vshot(double f,double v,double h,double o,double vv,double vh){
int x,y;
double a_v=r2d(2*atan( v/(2*f)));
double a_h=r2d(2*atan( h/(2*f)));
double va_v=r2d(2*atan(vv/(2*f)));
double va_h=r2d(2*atan(vh/(2*f)));
verbose(L_CAMR,"%s: focal: %6.2f mm",cam->name,f);
verbose(L_CAMR,"%s: υ: sensor: %6.2f • %6.2f mm",cam->name,v,h);
verbose(L_CAMR,"%s: υ: α: %6.2f • %6.2f °",cam->name,a_v,a_h);
a_v=a_v*(1-(o/2));a_h=a_h*(1-(o/2));
verbose(L_CAMR,"%s: υ: α': %6.2f • %6.2f °",cam->name,a_v,a_h);
verbose(L_CAMR,"%s: υ: Ξ: %6.3f",cam->name,o);
verbose(L_CAMR,"%s: ν: sensor: %6.2f • %6.2f mm",cam->name,vv,vh);
verbose(L_CAMR,"%s: ν: α: %6.2f • %6.2f °",cam->name,va_v,va_h);
for(y=0;y<=floor(va_v/a_v);y++){
double pos_theta=((a_v*((y*2)-floor(va_v/a_v)))/2);
for(x=0;x<=floor(va_h/(a_h/cos(d2r(pos_theta))));x++){
double pos_phi=(((a_h/cos(d2r(pos_theta)))*((x*2)-floor(va_h/(a_h/cos(d2r(pos_theta))))))/2);
go(0,pos_theta,(y%2?-pos_phi:pos_phi));
trigger_shot(0);
}
}
}
int main (int argc, char **argv) {
SpiceInt i, nres;
SPICEDOUBLE_CELL(cnfine,2);
SPICEDOUBLE_CELL(result,2*MAXWIN);
SpiceDouble beg,end;
furnsh_c("/home/barrycarter/BCGIT/ASTRO/standard.tm");
// this will represent the stars position
planets[0] = -1;
// convert ra and dec to starpos
radrec_c(1,atof(argv[1]),atof(argv[2]),starpos);
// planets array and count
for (i=1; i<argc-2; i++) {planets[i] = atoi(argv[i+2]);}
planetcount = argc-2;
printf("CONJUNCTIONS FOR %f degrees, planets: %s %s %d %d %d %d %d %d\n",r2d(MAXSEP),argv[1],argv[2],(int)planets[1],(int)planets[2],(int)planets[3],(int)planets[4],(int)planets[5],(int)planets[6]);
// TODO: this is for testing only
// wninsd_c(unix2et(0),unix2et(2147483647),&cnfine);
wninsd_c (-479695089600.+86400*468, 479386728000., &cnfine);
// 1 second tolerance (serious overkill, but 1e-6 is default, worse!)
gfstol_c(1.);
// find under MAXSEP degrees...
// TODO: The 90 days below is just for testing, not accurate!!!!
gfuds_c(gfq,gfdecrx,"<",MAXSEP,0.,86400.*90,MAXWIN,&cnfine,&result);
nres = wncard_c(&result);
for (i=0; i<nres; i++) {
wnfetd_c(&result,i,&beg,&end);
// R = range, M = min
printf("R %f %f\n",et2jd(beg),et2jd(end));
findmins(beg,end);
}
// printf("There are %d results\n",wncard_c(&result));
return 0;
}
int main(){
V6 v6;
v6 = v6init(SPHERICAL);
v6SetR(v6, 1e9);
v6SetAlpha(v6, h2r(20.0));
v6SetDelta(v6, d2r(40.0));
v6 = v6s2c(v6);
v6 = v6c2s(ellab(J2000, v6, -1));
printf("X %.9f \tY %.9f \tZ %.9f \nXDOT %.9f \tYDOT %.9f \tZDOT %.9f\n",
v6GetX(v6), r2h(r2r(v6GetY(v6))), r2d(r2r(v6GetZ(v6))), v6GetXDot(v6),
v6GetYDot(v6), v6GetZDot(v6));
return 0;
}
inline double CvCapture_FFMPEG::getProperty( int property_id )
{
if( !video_st ) return 0;
switch( property_id )
{
case CV_FFMPEG_CAP_PROP_POS_MSEC:
return 1000.0*(double)frame_number/get_fps();
case CV_FFMPEG_CAP_PROP_POS_FRAMES:
return (double)frame_number;
case CV_FFMPEG_CAP_PROP_POS_AVI_RATIO:
return r2d(ic->streams[video_stream]->time_base);
case CV_FFMPEG_CAP_PROP_FRAME_COUNT:
return (double)get_total_frames();
case CV_FFMPEG_CAP_PROP_FRAME_WIDTH:
return (double)frame.width;
case CV_FFMPEG_CAP_PROP_FRAME_HEIGHT:
return (double)frame.height;
case CV_FFMPEG_CAP_PROP_FPS:
#if LIBAVCODEC_BUILD > 4753
return av_q2d(video_st->r_frame_rate);
#else
return (double)video_st->codec.frame_rate
/ (double)video_st->codec.frame_rate_base;
#endif
case CV_FFMPEG_CAP_PROP_FOURCC:
#if LIBAVFORMAT_BUILD > 4628
return (double)video_st->codec->codec_tag;
#else
return (double)video_st->codec.codec_tag;
#endif
default:
break;
}
return 0;
}
// update particle position using 4th order Runge-Kutta method
void update_particle_position_rk4( e *E, double* cart_pos, double* vel, double dt, int dim){
int d;
double accumPos[DIM_MAX]; // where the pos[] solution vector is accumulated
double pos[DIM_MAX]; // position in longitude, latitude
double posPred[DIM_MAX]; // position in meters
double k1[DIM_MAX];
int whereILive;
double fac;
fac = 1.0/6.0;
for( d=0; d<dim; d++ ) {
accumPos[d] = cart_pos[d]; // initial step
k1[d] = dt * vel[d];
posPred[d] = cart_pos[d] + 0.5 * k1[d];
accumPos[d] = accumPos[d] + fac * k1[d]; // first step ( + 1/6 k1 )
}
// convert posPred back to lon lat so we can call get_owner_element
pos[1] = r2d(posPred[1]/R); // calc lat first
pos[0] = r2d(posPred[0] / (R*cos(d2r(pos[1])))); // now lon
// find out which element containts this position
whereILive = get_owner_element(E, pos);
// update weights
calculate_interpolation_weights(&E->el[whereILive], E->xi, E->eta, pos);
// interpolate the nodal velocity to the current point
interpolate_point(&E->el[whereILive], vel);
/* Reuse k1. Here k1 = k2 */
for( d=0; d<dim; d++ ) {
k1[d] = dt * vel[d];
posPred[d] = cart_pos[d] + 0.5 * k1[d];
accumPos[d] = accumPos[d] + 2.0*fac*k1[d]; /* second step ( + 1/3 k2 ) */
}
/* Put positions back in posPred */
//for( d=0; d<dim; d++ ) {
// posPred[d] = cart_pos[d] + 0.5 * k1[d];
//}
// convert cart_pos back to lon lat
pos[1] = r2d(posPred[1]/R); // calc lat first
pos[0] = r2d(posPred[0] / (R*cos(d2r(pos[1])))); // now lon
// find out which element containts this position
//printf("\t\t2: posPred[0] = %f, posPred[1] = %f\n", posPred[0], posPred[1]);
//printf("\t\t2: pos[0] = %f, pos[1] = %f\n", pos[0], pos[1]);
whereILive = get_owner_element(E, pos);
calculate_interpolation_weights(&E->el[whereILive], E->xi, E->eta, pos);
// interpolate the nodal velocity to the current point
interpolate_point(&E->el[whereILive], vel);
/* Reuse k1. Here k1 = k3 */
for( d=0; d<dim; d++ ) {
k1[d] = dt * vel[d];
posPred[d] = cart_pos[d] + k1[d];
accumPos[d] = accumPos[d] + 2.0*fac*k1[d]; /* third step ( + 1/3 k3 ) */
}
/* Put positions back in posPred */
//for( d=0; d<dim; d++ ) {
// posPred[d] = cart_pos[d] + k1[d];
//}
// convert cart_pos back to lon lat
pos[1] = r2d(posPred[1]/R); // calc lat first
pos[0] = r2d(posPred[0] / (R*cos(d2r(pos[1])))); // now lon
// find out which element containts this position
whereILive = get_owner_element(E, pos);
calculate_interpolation_weights(&E->el[whereILive], E->xi, E->eta, pos);
// interpolate the nodal velocity to the current point
interpolate_point(&E->el[whereILive], vel);
/* Reuse k1. Here k1 = k4 */
for( d=0; d<dim; d++ ) {
k1[d] = dt * vel[d];
accumPos[d] = accumPos[d] + fac*k1[d]; /* fourth step ( + 1/6 k4 ) */
// get new position
cart_pos[d] = accumPos[d];
}
//printf("done\n"); fflush(stdout);
}
real chisq()
{
printf("Initializing likelihood calculator...");
real chisqval = 0.0;
int i, j;
int count = 0;
int counttest = 1;
FILE* f;
real x[ctx.model.nbody];
real y[ctx.model.nbody];
real z[ctx.model.nbody];
real l[ctx.model.nbody];
real b[ctx.model.nbody];
real lambda[ctx.model.nbody];
real beta[ctx.model.nbody];
bodyptr p;
// Histogram prep
int index1, index2;
int largestbin;
int maxindex1 = (int)end / binsize;
int maxindex2 = (int)abs(beginning) / binsize;
real histodata1[maxindex1 + 1], histodata2[maxindex2 + 1];
// Zero all of the histogram arrays
memset(histodata1, 0, sizeof(real) * maxindex1);
memset(histodata2, 0, sizeof(real) * maxindex2);
printf("done\n");
printf("Importing simulation results...");
for (p = st.bodytab; p < st.bodytab + ctx.model.nbody; p++)
{
x[counttest-1] = Pos(p)[0];
y[counttest-1] = Pos(p)[1];
z[counttest-1] = Pos(p)[2];
counttest++;
}
printf("done\n");
printf("Transforming simulation results...");
for (i = 0; i < counttest - 1; i++)
{
count++;
// Convert to (l,b) (involves convert x to Sun-centered)
real r;
x[count-1] += r0;
r = sqrt(x[count-1] * x[count-1] + y[count-1] * y[count-1] + z[count-1] * z[count-1]);
// Leave in radians to make rotation easier
b[count-1] = atan2(z[count-1], sqrt(x[count-1] * x[count-1] + y[count-1] * y[count-1]));
l[count-1] = atan2(y[count-1], x[count-1]);
// Convert to (lambda, beta) (involves a rotation using the Newberg et al (2009) rotation matrices)
beta[count-1] = r2d(asin( sin(theta) * sin(phi) * cos(b[count-1]) * cos(l[count-1]) - sin(theta) * cos(phi) * cos(b[count-1]) * sin(l[count-1]) + cos(theta) * sin(b[count-1]) ));
lambda[count-1] = r2d(atan2( (-sin(psi) * cos(phi) - cos(theta) * sin(phi) * cos(psi)) * cos(b[count-1]) * cos(l[count-1]) + (-sin(psi) * sin(phi) + cos(theta) * cos(phi) * cos(psi)) * cos(b[count-1]) * sin(l[count-1]) + cos(psi) * sin(theta) * sin(b[count-1]), (cos(psi) * cos(phi) - cos(theta) * sin(phi) * sin(psi)) * cos(b[count-1]) * cos(l[count-1]) + (cos(psi) * sin(phi) + cos(theta) * cos(phi) * sin(psi)) * cos(b[count-1]) * sin(l[count-1]) + sin(psi) * sin(theta) * sin(b[count-1]) ));
// Create the histogram
if (lambda[count-1] > 0 && lambda[count-1] < end)
{
index1 = (int)(lambda[count-1] / binsize);
histodata1[index1]++;
}
else if (lambda[count-1] > beginning)
{
index2 = abs((int)(lambda[count-1] / binsize));
histodata2[abs(index2)]++;
}
}
printf("done\n");
// Find the largest entry
// Get the single largest bin so we can normalize over it
//CHECKME: Why the +1's in the sizes of histodatas?
largestbin = MAX(findmax(histodata1, maxindex1), findmax(histodata2, maxindex2));
printf("Largest bin: %i\n", largestbin);
printf("Outputting to disk...");
f = fopen("histout", "w");
if (f == NULL)
{
printf("There was an error writing to the file\n");
exit(EXIT_FAILURE);
}
printf("...file open...");
// Print out the histogram
real foo;
for (i = 0, foo = -binsize; foo >= beginning; foo -= binsize, ++i)
{
fprintf(f, "%f %f\n", foo + (binsize / 2.0) , histodata2[i] / ((real)largestbin));
}
for (i = 0, foo = 0; foo <= end; foo += binsize, ++i)
{
fprintf(f, "%f %f\n", foo + (binsize / 2.0) , histodata1[i] / ((real)largestbin));
}
fclose(f);
printf("done\n");
// Calculate the chisq value by reading a file called "histogram" (the real data histogram should already be normalized)
f = fopen("histogram.txt", "r");
if (f == NULL)
{
printf("histogram file not found...exiting\n");
exit(EXIT_FAILURE);
}
int pos, fsize;
real* fileLambda = NULL, *fileCount = NULL, *fileCountErr = NULL;
int filecount = 0;
pos = fseek(f, 0L, SEEK_END);
fsize = ceil((double) (ftell(f) + 1) / 3); /* Make sure it's big enough, avoid fun with integer division */
fseek(f, 0L, SEEK_SET);
fileLambda = (real*) calloc(sizeof(real), fsize);
fileCount = (real*) calloc(sizeof(real), fsize);
fileCountErr = (real*) calloc(sizeof(real), fsize);
while (fscanf(f,
"%g %g %g\n",
&fileLambda[filecount],
&fileCount[filecount],
&fileCountErr[filecount]) != EOF)
{
++filecount;
}
fclose(f);
// Calculate the chisq
for (i = 0, foo = -binsize; foo >= beginning; foo -= binsize, ++i)
{
for (j = 0; j < filecount; ++j)
{
// Don't include bins with zero, or zero error, this means there were no counts in that bin to begin with
if (fileLambda[j] == foo + (binsize / 2.0) && histodata2[i] != 0 && fileCountErr[j] != 0)
{
chisqval += ((fileCount[j] - (histodata2[i] / largestbin)) / fileCountErr[j])
* ((fileCount[j] - (histodata2[i] / largestbin)) / fileCountErr[j]);
}
}
}
for (i = 0, foo = 0.0; foo <= end; foo += binsize, ++i)
{
for (j = 0; j < filecount; ++j)
{
// Don't include bins with zero, or zero error, this means there were no counts in that bin to begin with
if (fileLambda[j] == foo + (binsize / 2.0) && histodata1[i] != 0 && fileCountErr[j] != 0)
{
chisqval += ((fileCount[j] - (histodata1[i] / largestbin)) / fileCountErr[j])
* ((fileCount[j] - (histodata1[i] / largestbin)) / fileCountErr[j]);
}
}
}
free(fileLambda);
free(fileCount);
free(fileCountErr);
printf("CHISQ = %f\n", chisqval);
// MAXIMUM likelihood, multiply by -1
real likelihood = -chisqval;
printf("likelihood = %f\n", likelihood);
return likelihood;
}
double SettingDlg::GetShadowDir()
{
double deg = r2d(atan2(m_shadow_dir_y, m_shadow_dir_x));
return deg;
}
inline double CvCapture_FFMPEG::dts_to_sec(s64 dts)
{
return (double)(dts - ic->streams[video_stream]->start_time) *
r2d(ic->streams[video_stream]->time_base);
}
// Main control loop to read current pose and update control values
// return false if no destination available (i.e. routing finished and need a new endpoint)
bool MotionControl::iterate(SLAM::RobotLocation myPose) {
//if (myPose.theta < 0) myPose.theta += 2*M_PI;
// you spin me right round
while (myPose.theta > M_PI)
myPose.theta -= 2*M_PI;
while (myPose.theta < -M_PI)
myPose.theta += 2*M_PI;
lastPose = myPose;
cout << " " << myPose.theta << endl;
cout << "ITERATE" << endl;
cout << midpoints.size() << endl;
if (midpoints.size() == 0) {
cout << "\tNo midpoints" << endl;
ctrl.speed = 0;
ctrl.angle = 0;
return false;
}
SLAM::Location midpdest = midpoints.front();
float dx = midpdest.x - myPose.x;
float dy = midpdest.y - myPose.y;
// TODO: subtract mod2pi somehow so that almost2pi-littleover0=2pi-almost2pi+littleover0 (?)
//if (myPose.theta<0.01)myPose.theta=2*M_PI;
float rho = sqrt(dx * dx + dy * dy);
float ata = atan2(dy, dx);
//if (ata < 0) ata += 2*M_PI;
float alpha = ata - myPose.theta;
alpha = fixangles(alpha);
// When new route is gotten, first rotate to orientation of first waypoint.
static const float eps = 20*M_PI/180;
if (routeStarting) {
if (floateq(fixangles(myPose.theta), fixangles(startPoint.theta), eps)) {
routeStarting = false;
return nextMidpoint();
} else {
std::cout << "Route starting" << std::endl;
ctrl.speed = 0;
float dt = startPoint.theta - myPose.theta;
dt = fixangles(dt);
ctrl.angle = k.a * dt;
return true;
}
}
ctrl.speed = k.p - k.iiris * fabsf(alpha);// * rho;
ctrl.angle = k.a * alpha;
cout<< "\tdx = " << dx << endl;
cout<< "\tdy = " << dy << endl;
cout<< "\tmy theta = " << myPose.theta << " " << r2d(myPose.theta) << endl;
cout<< "\tatan2 dydx = " << ata << " " << r2d(ata) << endl;
cout<< "\trho = " << rho << endl;
cout<< "\talpha = " << alpha << " " << r2d(alpha) << endl;
cout<< "\tv = " << ctrl.speed << endl;
cout<< "\tw = " << ctrl.angle << " " << r2d(ctrl.angle) << endl;
history.push_back(HistPoint{lastPose, Ctrl{ctrl.speed, ctrl.angle}, alpha, ata});
float enough = (midpoints.size() > 1) ? k.closeEnough : k.closeEnoughLast;
if (rho < enough) {
cout << "\tCLOSE ENOUGH!" << endl;
return nextMidpoint();
}
return true;
}
double
do_r2d(double x)
{
return (r2d(x));
}
