/**
* Find the smallest angle between two headings
* @param theta1 heading at time t
* @param theta2 heading at time t+1
* @return smallest angle to get from theta1 to theta2
*/
double heading_diff(double theta1, double theta2) {
theta1 = fix_angle(theta1);
theta2 = fix_angle(theta2);
double naive_diff = theta2 - theta1;
// enumerate possible smallest diffs and select the lowest magnitude
auto diff_options = {naive_diff, naive_diff - 2*M_PI, naive_diff + 2*M_PI};
auto abs_compare = [](double x, double y) {
return std::abs(x) < std::abs(y);
};
return std::min(diff_options, abs_compare);
}
/* compute declination angle of sun and equation of time */
DoublePair sun_position(double jd)
{
double d = jd - 2451545.0;
double g = fix_angle(357.529 + 0.98560028 * d);
double q = fix_angle(280.459 + 0.98564736 * d);
double l = fix_angle(q + 1.915 * dsin(g) + 0.020 * dsin(2 * g));
// double r = 1.00014 - 0.01671 * dcos(g) - 0.00014 * dcos(2 * g);
double e = 23.439 - 0.00000036 * d;
double dd = darcsin(dsin(e) * dsin(l));
double ra = darctan2(dcos(e) * dsin(l), dcos(l)) / 15.0;
ra = fix_hour(ra);
double eq_t = q / 15.0 - ra;
return DoublePair(dd, eq_t);
}
// Time accessing lbn li immediately after accessing lbn l0 and convert the
// time into an angle in [0,1) according to the rotational period of
// the disk.
double
measure_one_skew(struct dm_disk_if *d,
struct dsstuff *ds,
struct trace *trace,
int l0,
int li) {
double t, tt, a = 0.0;
double period = dm_time_itod(d->mech->dm_period(d));
adjust_lbns(d, &l0, &li);
t = time_second_request(l0, li, ds, trace, d);
if(mode == CALIB) {
tt = get_tracetime(trace, l0, li);
a = fix_angle(-(t - tt) / period);
}
return a;
}
CMUK_ERROR_CODE cmuk::computeFootIK( LegIndex leg,
const vec3f& pos,
vec3f* q_bent_forward,
vec3f* q_bent_rearward ) const {
if ((int)leg < 0 || (int)leg >= NUM_LEGS) {
return CMUK_BAD_LEG_INDEX;
} else if (!q_bent_forward || !q_bent_rearward) {
return CMUK_INSUFFICIENT_ARGUMENTS;
}
debug << "*** computing IK...\n";
int hipflags = 0;
// subtract off hip position
vec3f p = pos - jo(_kc, leg, HIP_RX_OFFSET, _centeredFootIK);
vec3f orig = pos;
// get dist from hip rx joint to y rotation plane
const float& d = jo(_kc, leg, HIP_RY_OFFSET, _centeredFootIK)[1];
// get the squared length of the distance on the plane
float yz = p[1]*p[1] + p[2]*p[2];
// alpha is the angle of the foot in the YZ plane with respect to the Y axis
float alpha = atan2(p[2], p[1]);
// h is the distance of foot from hip in YZ plane
float h = sqrt(yz);
// beta is the angle between the foot-hip vector (projected in YZ
// plane) and the top hip link.
float cosbeta = d / h;
debug << "p = " << p << ", d = " << d << ", yz = " << yz << "\nalpha = " << alpha << ", h = " << h << ", cosbeta=" << cosbeta << "\n";
if (fabs(cosbeta) > 1) {
debug << "violated triangle inequality when calculating hip_rx_angle!\n" ;
if (fabs(cosbeta) - 1 > 1e-4) {
hipflags = hipflags | IK_UPPER_DISTANCE;
}
cosbeta = (cosbeta < 0) ? -1 : 1;
if (yz < 1e-4) {
p[1] = d;
p[2] = 0;
} else {
float scl = fabs(d) / h;
p[1] *= scl;
p[2] *= scl;
orig = p + jo(_kc, leg, HIP_RX_OFFSET, _centeredFootIK);
}
}
float beta = acos(cosbeta);
// Now compute the two possible hip angles
float hip_rx_angles[2], badness[2];
int flags[2];
flags[0] = hipflags;
flags[1] = hipflags;
hip_rx_angles[0] = fix_angle(alpha - beta, -M_PI, M_PI);
hip_rx_angles[1] = fix_angle(alpha + beta, -M_PI, M_PI);
const float& min = jl(_kc, leg, HIP_RX, 0);
const float& max = jl(_kc, leg, HIP_RX, 1);
// See how badly we violate the joint limits for this hip angles
for (int i=0; i<2; ++i) {
float& angle = hip_rx_angles[i];
badness[i] = fabs(compute_badness(angle, min, max));
if (badness[i]) { flags[i] = flags[i] | IK_UPPER_ANGLE_RANGE; }
}
// Put the least bad (and smallest) hip angle first
bool swap = false;
if ( badness[1] <= badness[0] ) {
// We want the less bad solution for hip angle
swap = true;
} else if (badness[0] == 0 && badness[1] == 0) {
// We want the solution for hip angle that leaves the hip up.
if ((leg == FL || leg == HL) && hip_rx_angles[0] > hip_rx_angles[1]) {
swap = true;
} else if ((leg == FR || leg == HR) && hip_rx_angles[0] < hip_rx_angles[1]) {
swap = true;
}
}
if (swap) {
std::swap(hip_rx_angles[0], hip_rx_angles[1]);
std::swap(badness[0], badness[1]);
std::swap(flags[0], flags[1]);
}
int hip_solution_cnt = 2;
if (badness[0] == 0 && badness[1] != 0) {
hip_solution_cnt = 1;
}
debug << "hip_rx_angles[0]=" << hip_rx_angles[0]
<< ", badness=" << badness[0]
<< ", flags=" << flags[0] << "\n";
debug << "hip_rx_angles[1]=" << hip_rx_angles[1]
<< ", badness=" << badness[1]
<< ", flags=" << flags[1] << "\n";
debug << "hip_solution_cnt = " << hip_solution_cnt << "\n";
vec3f qfwd[2], qrear[2];
for (int i=0; i<hip_solution_cnt; ++i) {
debug << "** computing ll solution " << (i+1) << " of " << (hip_solution_cnt) << "\n";
float hip_rx = hip_rx_angles[i];
// now make inv. transform to get rid of hip rotation
Transform3f tx = Transform3f::rx(hip_rx, jo(_kc, leg, HIP_RX_OFFSET, _centeredFootIK));
vec3f ptx = tx.transformInv(orig);
debug << "tx=[" << tx.translation() << ", " << tx.rotation() << "], ptx = " << ptx << "\n";
// calculate lengths for cosine law
float l1sqr = ol2(_kc, leg, KNEE_RY_OFFSET, _centeredFootIK);
float l2sqr = ol2(_kc, leg, FOOT_OFFSET, _centeredFootIK);
float l1 = ol(_kc, leg, KNEE_RY_OFFSET, _centeredFootIK);
float l2 = ol(_kc, leg, FOOT_OFFSET, _centeredFootIK);
float ksqr = ptx[0]*ptx[0] + ptx[2]*ptx[2];
float k = sqrt(ksqr);
debug << "l1=" << l1 << ", l2=" << l2 << ", k=" << k << "\n";
// check triangle inequality
if (k > l1 + l2) {
debug << "oops, violated the triangle inequality for lower segments: "
<< "k = " << k << ", "
<< "l1 + l2 = " << l1 + l2 << "\n";
if (k - (l1 + l2) > 1e-4) {
flags[i] = flags[i] | IK_LOWER_DISTANCE;
}
k = l1 + l2;
ksqr = k * k;
}
// 2*theta is the acute angle formed by the spread
// of the two hip rotations...
float costheta = (l1sqr + ksqr - l2sqr) / (2 * l1 * k);
if (fabs(costheta) > 1) {
debug << "costheta = " << costheta << " > 1\n";
if (fabs(costheta) - 1 > 1e-4) {
flags[i] = flags[i] | IK_LOWER_DISTANCE;
}
costheta = (costheta < 0) ? -1 : 1;
}
float theta = acos(costheta);
// gamma is the angle of the foot with respect to the z axis
float gamma = atan2(-ptx[0], -ptx[2]);
// hip angles are just offsets off of gamma now
float hip_ry_1 = gamma - theta;
float hip_ry_2 = gamma + theta;
// phi is the obtuse angle of the parallelogram
float cosphi = (l1sqr + l2sqr - ksqr) / (2 * l1 * l2);
if (fabs(cosphi) > 1) {
debug << "cosphi = " << cosphi << " > 1\n";
if (fabs(cosphi) - 1 > 1e-4) {
flags[i] = flags[i] | IK_LOWER_DISTANCE;
}
cosphi = (cosphi < 0) ? -1 : 1;
}
float phi = acos(cosphi);
// epsilon is the "error" caused by not having feet offset directly
// along the z-axis (if they were, epsilon would equal zero)
float epsilon = le(_kc, leg, _centeredFootIK);
// now we can directly solve for knee angles
float knee_ry_1 = M_PI - phi - epsilon;
float knee_ry_2 = -M_PI + phi - epsilon;
// now fill out angle structs and check limits
qfwd[i] = vec3f(hip_rx, hip_ry_1, knee_ry_1);
qrear[i] = vec3f(hip_rx, hip_ry_2, knee_ry_2);
debug << "before wrap, qfwd = " << qfwd[i] << "\n";
debug << "before wrap, qrear = " << qrear[i] << "\n";
check_wrap(_kc, qfwd[i], leg);
check_wrap(_kc, qrear[i], leg);
debug << "after wrap, qfwd = " << qfwd[i] << "\n";
debug << "after wrap, qrear = " << qrear[i] << "\n";
if (!check_limits(_kc, qfwd[i], leg)) {
debug << "violated limits forward!\n";
flags[i] = flags[i] | IK_LOWER_ANGLE_RANGE_FWD;
}
if (!check_limits(_kc, qrear[i], leg)) {
debug << "violated limits rearward!\n";
flags[i] = flags[i] | IK_LOWER_ANGLE_RANGE_REAR;
}
} // for each viable hip solution
int best = 0;
if (hip_solution_cnt == 2) {
if (howbad(flags[0]) > howbad(flags[1])) {
best = 1;
}
debug << "best overall solution is " << (best+1) << "\n";
}
*q_bent_forward = qfwd[best];
*q_bent_rearward = qrear[best];
return flags_to_errcode(flags[best]);
}
// do_idx_ent() does the hard work to measure the skews for a single
// index entry. We need to determine e->off, the angular offset of
// the first lbn of this instance taking the first lbn of parent as
// the 0 point and e->alen, the angular offset of the i+1st instance
// of e from the ith.
void
do_idx_ent(struct dm_disk_if *d, // diskmodel
struct dm_layout_g4 *l, // root of g4 layout
struct idx_ent *e, // the entry in question
struct idx *parent, // The index containing e
struct idx_ent *parent_e, // The entry for parent in its parent
int parent_off, // Which entry we are in parent
struct dsstuff *ds, // Disksim instance
struct trace *t, // io trace
int lbn) { // The first lbn of parent
int i;
int l0;
int dist;
double yi;
double *times;
double *tracetimes;
// for bootstrapping off and len
double off0time, len0time;
int off0lbn[2], len0lbn[2];
// Work internally in floating-point, then convert back to the
// integer representation at the end.
double aoff = dm_angle_itod(e->off);
double alen = dm_angle_itod(e->alen);
double period = dm_time_itod(d->mech->dm_period(d));
// Number of instances of this entry.
int quot = e->runlen / e->len;
// times[i] is disksim's prediction of the amount of time to access the
// first lbn of the ith instance after accessing the first instance.
times = calloc(quot, sizeof(double));
// Actual times from the trace replay against the real disk.
tracetimes = calloc(quot, sizeof(double));
// li[i] contains the first lbn of the ith instance of e.
int *li = calloc(quot, sizeof(*li));
printf("%s() lbn %d e->lbn %d alen %f off %f quot %d\n",
__func__, lbn, e->lbn, alen, aoff, quot);
if(e->alen != 0 || e->off != 0) {
printf("%s() already done, apparently\n", __func__);
return;
}
// Bootstrap offset by measuring the first instance.
if(e->lbn != 0) {
aoff = measure_one_skew(d, ds, t, lbn, lbn + e->lbn);
e->off = dm_angle_dtoi(aoff);
}
// Check for the end of the lbn space.
if(lbn + e->lbn + e->len >= d->dm_sectors) {
e->alen = dm_angle_dtoi(0.0);
return;
}
// Bootstrap alen by measuring the skew from the first to the second
// instance.
alen = measure_one_skew(d, ds, t, lbn + e->lbn,
lbn + e->lbn + e->len);
e->alen = dm_angle_dtoi(alen);
printf("first off %f len %f\n", aoff, alen);
// Expand out the lbns of all of the instances. For calibration
// (second pass), read in the values from the trace. To generate
// the trace (first pass), time how long it takes to read the first
// lbn of the ith instance after reading the last lbn on the first
// track of the first instance.
for(i = 1; i < quot; i++) {
l0 = lbn;
li[i] = l0 + e->lbn + e->len * i;
adjust_lbns(d, &l0, &li[i]);
if(mode == CALIB) {
tracetimes[i] = get_tracetime(t, l0, li[i]);
printf("%d (%d,%d) trace %f pred %f\n", i,
l0, li[i],
tracetimes[i], times[i]);
}
else {
time_second_request(l0, li[i], ds, t, d);
}
}
if(mode == GENTRACE) {
return;
}
// Now calibrate the value. We first look at the first 2 instances,
// then 4, then 8, etc, refining our estimate at each step.
for(dist = 2; dist < quot ; ) {
// y = a + bx; r is the correlation coeffecient.
double a, b;
for(i = 1; i < dist; i++) {
times[i] = time_second_request(l0, li[i], ds, t, d);
printf("%d (%d,%d) trace %f pred %f\n", i,
l0, li[i],
tracetimes[i], times[i]);
}
// Do a linear least squares fit of the difference between the
// predicted and actual service times. The slope (b) of that line
// corresponds to the error in our estimate of alen and the y
// intercept corresponds to the error in our estimate of off.
b = find_slope(times, tracetimes, dist, &a, period);
// Adjust the instance-to-instance skew according to the fit.
alen = fix_angle(alen - b / period);
e->alen = dm_angle_dtoi(alen);
// If e is the first instance in its parent index, its offset is
// defined to be 0. Otherwise, correct according to the fit.
if(e->lbn > lbn) {
printf("fix aoff (%d, %d)\n", lbn, e->lbn);
aoff = fix_angle(aoff - a / period);
e->off = dm_angle_dtoi(aoff);
}
printf("alen -> %f, off -> %f (dist %d)\n", alen, aoff, dist);
if(dist == quot) {
break;
} else if(dist * 2 >= quot) {
dist = quot;
} else {
dist *= 2;
}
}
free(li);
free(tracetimes);
free(times);
return;
}
void lzs_renderer_draw(lzs_renderer_t* self)
{
assert(self);
LOGD("debug");
double t0 = a3d_utime();
// stretch screen to 800x480
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
glOrthof(0.0f, SCREEN_W, SCREEN_H, 0.0f, 0.0f, 2.0f);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
// draw camera
glEnable(GL_TEXTURE_EXTERNAL_OES);
glEnableClientState(GL_TEXTURE_COORD_ARRAY);
glBindTexture(GL_TEXTURE_EXTERNAL_OES, self->texid);
glVertexPointer(3, GL_FLOAT, 0, VERTEX);
glTexCoordPointer(2, GL_FLOAT, 0, COORDS);
glDrawArrays(GL_TRIANGLE_FAN, 0, 4);
glDisableClientState(GL_TEXTURE_COORD_ARRAY);
glDisable(GL_TEXTURE_EXTERNAL_OES);
utime_update("setup", &t0);
// capture buffers
GLint format = TEXGZ_BGRA;
GLint type = GL_UNSIGNED_BYTE;
glGetIntegerv(GL_IMPLEMENTATION_COLOR_READ_FORMAT_OES, &format);
glGetIntegerv(GL_IMPLEMENTATION_COLOR_READ_TYPE_OES, &type);
if((format == TEXGZ_BGRA) && (type == GL_UNSIGNED_BYTE))
{
LOGD("readpixels format=0x%X, type=0x%X", format, type);
// TODO - check for texgz errors
// process buffers
texgz_tex_t* bc = self->buffer_color;
texgz_tex_t* bg = self->buffer_gray;
texgz_tex_t* bsx = self->buffer_sx;
texgz_tex_t* bsy = self->buffer_sy;
glReadPixels(self->sphero_x - RADIUS_BALL, (SCREEN_H - self->sphero_y - 1) - RADIUS_BALL,
bc->width, bc->height, bc->format, bc->type, (void*) bc->pixels);
#ifdef DEBUG_BUFFERS
texgz_tex_export(bc, "/sdcard/laser-shark/color.texgz");
#endif
utime_update("readpixels", &t0);
texgz_tex_computegray(bc, bg);
utime_update("computegray", &t0);
texgz_tex_computeedges3x3(bg, bsx, bsy);
utime_update("computeedges", &t0);
// compute peak
{
int x;
int y;
int peak_x = 0;
int peak_y = 0;
float peak = 0.0f;
float* gpixels = (float*) bg->pixels;
float* xpixels = (float*) bsx->pixels;
float* ypixels = (float*) bsy->pixels;
for(x = 0; x < bg->width; ++x)
{
for(y = 0; y < bg->height; ++y)
{
int idx = bg->width*y + x;
// compute magnitude squared
float magsq = xpixels[idx]*xpixels[idx] + ypixels[idx]*ypixels[idx];
gpixels[idx] = magsq;
if(magsq > peak)
{
peak_x = x;
peak_y = y;
peak = magsq;
}
}
}
LOGD("peak=%f, peak_x=%i, peak_y=%i", peak, peak_x, peak_y);
utime_update("computepeak", &t0);
#ifdef DEBUG_BUFFERS
texgz_tex_export(bg, "/sdcard/laser-shark/peak.texgz");
#endif
// move sphero center to match peak
self->sphero_x += (float) peak_x - (float) bg->width / 2.0f;
self->sphero_y -= (float) peak_y - (float) bg->height / 2.0f;
}
}
else
{
LOGE("unsupported format=0x%X, type=0x%X", format, type);
}
// compute phone X, Y center
compute_position(self, SCREEN_CX, SCREEN_CY, &self->phone_X, &self->phone_Y);
// compute sphero X, Y
compute_position(self, self->sphero_x, self->sphero_y, &self->sphero_X, &self->sphero_Y);
utime_update("computeposition", &t0);
// compute goal
float dx = self->phone_X - self->sphero_X;
float dy = self->phone_Y - self->sphero_Y;
float a = fix_angle(atan2f(dx, dy) * 180.0f / M_PI);
self->sphero_goal = a - self->sphero_heading_offset;
// compute speed
float dotp = cosf((a - self->sphero_heading) * M_PI / 180.0f);
if(dotp > 0.0f)
{
// linearly interpolate speed based on the turning angle
self->sphero_speed = SPEED_MAX*dotp + SPEED_MIN*(1.0f - dotp);
}
else
{
// go slow to turn around
self->sphero_speed = SPEED_MIN;
}
// draw camera cross-hair
{
float x = SCREEN_CX;
float y = SCREEN_CY;
float r = RADIUS_CROSS;
limit_position(r, &x, &y);
lzs_renderer_crosshair(y - r, x - r, y + r, x + r, 1.0f, 0.0f, 0.0f);
}
// draw sphero search box
{
float r = RADIUS_BALL;
limit_position(r, &self->sphero_x, &self->sphero_y);
float x = self->sphero_x;
float y = self->sphero_y;
lzs_renderer_drawbox(y - r, x - r, y + r, x + r, 0.0f, 1.0f, 0.0f, 0);
}
lzs_renderer_step(self);
// draw string
a3d_texstring_printf(self->string_sphero, "sphero: head=%i, x=%0.1f, y=%0.1f, spd=%0.2f, goal=%i", (int) fix_angle(self->sphero_heading + self->sphero_heading_offset), self->sphero_X, self->sphero_Y, self->sphero_speed, (int) fix_angle(self->sphero_goal));
a3d_texstring_printf(self->string_phone, "phone: heading=%i, slope=%i, x=%0.1f, y=%0.1f", (int) fix_angle(self->phone_heading), (int) fix_angle(self->phone_slope), self->phone_X, self->phone_Y);
a3d_texstring_draw(self->string_sphero, 400.0f, 16.0f, 800, 480);
a3d_texstring_draw(self->string_phone, 400.0f, 16.0f + self->string_sphero->size, 800, 480);
a3d_texstring_draw(self->string_fps, (float) SCREEN_W - 16.0f, (float) SCREEN_H - 16.0f, SCREEN_W, SCREEN_H);
utime_update("draw", &t0);
//texgz_tex_t* screen = texgz_tex_new(SCREEN_W, SCREEN_H, SCREEN_W, SCREEN_H, TEXGZ_UNSIGNED_BYTE, TEXGZ_BGRA, NULL);
//glReadPixels(0, 0, screen->width, screen->height, screen->format, screen->type, (void*) screen->pixels);
//texgz_tex_export(screen, "/sdcard/laser-shark/screen.texgz");
//texgz_tex_delete(&screen);
A3D_GL_GETERROR();
}
double advance_orbit_set_up(BEMRI * b, double chi, double r_current, double dt1)
{
double t_current, t_new, psi_h_target, theta_h_target, psi_b_advanced, Mb, t_elapsed, new_true;
//find current hole eccentric anomaly
psi_h = acos( (eh + cos(theta_h)) / (1+eh*cos(theta_h)) );
//find current time in the orbit (since last periapse)
t_current = Ph/(2*PI) * (psi_h - eh*sin(psi_h));
//now find target (new) values
psi_h_target = 2*PI - acos( (1 - chi/ah) / eh); //hole eccentric anomaly value to skip to
theta_h_target = fix_angle( 2*atan2(sqrt(1+eh)*sin(psi_h_target/2.0) , sqrt(1-eh)*cos(psi_h_target/2)) ); //hole true anomaly value to skip to
t_new = Ph/(2*PI) * (psi_h_target - eh*sin(psi_h_target)); //new time value from Kepler's equation
//then the elapsed time is given by:
t_elapsed = t_new - t_current;
//printf("\nt_elapsed = %.5e",t_elapsed);
//_+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//force t_elapsed to equal an integer multiple of dt1
t_elapsed = floor(t_elapsed/dt1) * dt1; //now t_elapsed is good
t_new = t_elapsed + t_current; //now we're recalculating the hole orbit anomalies
//printf("\nMh = %.15e",2*PI/Ph*t_new);
psi_h_target = solve_kepler(2*PI/Ph*t_new,eh); //here is the new target eccentric anomaly
theta_h_target = fix_angle( 2*atan2(sqrt(1+eh)*sin(psi_h_target/2.0) , sqrt(1-eh)*cos(psi_h_target/2.0)) ); //new hole true anomaly value to skip to
//printf("\npsi_h_target = %.3e\nsolving keplers eqn for t_new = %.3f",psi_h_target, Ph/(2*PI)*kepler(psi_h_target,eh));
//printf("\nt_elapsed / dt1 = %.20f\n",t_elapsed / dt1);
//_+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//now the mean anomaly of the BEMRI orbit will have advanced by:
Mb = 2*PI/Pb*t_elapsed - 2*PI*floor(t_elapsed/Pb); //note this is not the current mean anomaly of the BEMRI, but how much it has advanced
//here is how much the eccentric anomaly of the BEMRI has advanced
psi_b_advanced = solve_kepler(Mb,eb);
//so the new value of the true anomaly of the BEMRI is given by: (current + advanced)
new_true = theta_b + fix_angle((2*atan2(sqrt(1+eb)*sin(psi_b_advanced/2.0),sqrt(1-eb)*cos(psi_b_advanced/2.0))) );
//_+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//printf("\ncurrent hole anomaly is %.3f, target anomaly is %.3f\n",theta_h, theta_h_target);
//printf("chi = %.3e\nah = %.3e\neh=%.3e\n",chi,ah,eh);
//_+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//before advancing the BH orbit, freeze the BEMRI CM motion so the new CM motion can be added on easily
for(int i=0; i < 3; i++)
{
V1[i] -= V4[i]; //remove CM motion for mass 1
V2[i] -= V4[i]; //remove CM motion for mass 2
R1[i] -= R4[i];
R2[i] -= R4[i];
}
//now that we know the new true anomaly of the hole AND the BEMRI, we can advance the two orbits to the new positions
advance_orbit(&(b->binary_b),lb,new_true);
//now advance the BH orbit
advance_orbit(&(b->binary_h),lh,theta_h_target);
//and now the BEMRI CM values need to be updated:
//BEMRI_CM_update(b);
return t_elapsed;
}
int calc_angles(binary * b)
{
double x,y,z,vx,vy,vz,r,v,k,h[3],R[3],V[3],e_vec[3],vxh1[3],vxh2[3],R_temp[3],theta,edotr,ip;
double i,z_hat[3] = {0,0,1}, ea, lan_calc, aop_calc, n[3];
double beta;
int ret=0;
//First translate to mass 1 rest frame
x = b->x2[0] - b->x1[0];
y = b->x2[1] - b->x1[1];
z = b->x2[2] - b->x1[2];
vx = b->v2[0] - b->v1[0];
vy = b->v2[1] - b->v1[1];
vz = b->v2[2] - b->v1[2];
r = sqrt(x*x + y*y + z*z);
v = sqrt(vx*vx + vy*vy + vz*vz);
k = G*b->mass1 + G*b->mass2; //here k is the standard gravitational parameter
//put together vectors of r and v
R[0] = x;
R[1] = y;
R[2] = z;
V[0] = vx;
V[1] = vy;
V[2] = vz;
//calculate specific angular momentum vector h
cross_prod(R,V,h);
//calculate the eccentricity vector
cross_prod(V,h,vxh1);
vect_mult_scalar(vxh1, 1/k, vxh2, 3);
vect_mult_scalar(R,-1/r,R_temp,3);
vect_add(vxh2,R_temp,e_vec,3);
//find true anomaly from r
edotr = dot_prod(e_vec,R,3);
theta = acos(edotr / ( mag(e_vec,3) * mag(R,3) ) );
if(dot_prod(R,V,3) < 0)
{
theta = 2*PI - theta;
ret = 1;
}
b->true_anomaly = theta;
i = acos(h[2]/mag(h,3));
//calculate n, unit normal vector
vect_mult_scalar(h,1/mag(h,3),n,3);
lan_calc = atan2(n[0],-n[1]);
lan_calc = fix_angle(lan_calc); //keep angles in range 0 to 2*pi
if(i==0) //zero inclination makes lan meaningless. Keep it at zero
lan_calc = 0;
beta = atan2(-cos(i) * sin(lan_calc) * x + cos(i) * cos(lan_calc) * y + sin(i) * z , cos(lan_calc) * x + sin(lan_calc) * y);
beta = fix_angle(beta); //keep angles in range 0 to 2*pi
aop_calc = PI + beta - theta;
aop_calc = fix_angle(aop_calc); //keep angles in range 0 to 2*pi
ea = acos((mag(e_vec,3) + cos(theta)) / (1.0 + mag(e_vec,3)*cos(theta)));
b->i_a = i;
b->la_n = lan_calc;
b->ao_p = aop_calc;
return ret;
}
