/* Presumes that the first alm entry is the reference sat. */
s8 assign_de_mtx(u8 num_sats, const sdiff_t *sats_with_ref_first,
const double ref_ecef[3], double *DE)
{
assert(sats_with_ref_first != NULL);
assert(ref_ecef != NULL);
assert(DE != NULL);
if (num_sats <= 1) {
log_debug("assign_de_mtx: not enough sats");
return -1;
}
assert(num_sats > 1);
/* Vector to reference satellite. */
double e_0[3];
vector_subtract(3, sats_with_ref_first[0].sat_pos, ref_ecef, e_0);
vector_normalize(3, e_0);
for (u8 i=1; i<num_sats; i++) {
/* Vector to satellite i */
double e_i[3];
vector_subtract(3, sats_with_ref_first[i].sat_pos, ref_ecef, e_i);
vector_normalize(3, e_i);
/* DE row = e_i - e_0 */
vector_subtract(3, e_i, e_0, &DE[3*(i-1)]);
}
return 0;
}
static void
plane_normal(plane p, vector *n)
{
vector v1, v2;
vector_subtract(p.p1, p.p2, &v1);
vector_subtract(p.p1, p.p3, &v2);
vector_cross(v2, v1, n);
}
/** Simulates real observations for the current position and the satellite almanac and week
* given in simulator_data.
*
* NOTES:
*
* - This simulates the pseudorange as the true distance to the satellite + noise.
* - This simulates the carrier phase as the true distance in wavelengths + bais + noise.
* - The bias is an integer multiple of 10 for easy debugging.
* - The satellite SNR/CN0 is proportional to the elevation of the satellite.
*
* USES:
* - Pipe observations into internals for testing
* - For integration testing with other devices that has to carry the radio signal.
*
* \param elapsed Number of seconds elapsed since last simulation step.
*/
void simulation_step_tracking_and_observations(double elapsed)
{
(void)elapsed;
u8 week = -1;
double t = sim_state.noisy_solution.time.tow;
/* First we calculate all the current sat positions, velocities */
for (u8 i=0; i<simulation_num_almanacs; i++) {
calc_sat_state_almanac(&simulation_almanacs[i], t, week,
simulation_sats_pos[i], simulation_sats_vel[i]);
}
/* Calculate the first sim_settings.num_sats amount of visible sats */
u8 num_sats_selected = 0;
double az, el;
for (u8 i=0; i<simulation_num_almanacs; i++) {
calc_sat_az_el_almanac(&simulation_almanacs[i], t, week,
sim_state.pos, &az, &el);
if (el > 0 &&
num_sats_selected < sim_settings.num_sats &&
num_sats_selected < MAX_CHANNELS) {
/* Generate a code measurement which is just the pseudorange: */
double points_to_sat[3];
double base_points_to_sat[3];
vector_subtract(3, simulation_sats_pos[i], sim_state.pos, points_to_sat);
vector_subtract(3, simulation_sats_pos[i], sim_settings.base_ecef, base_points_to_sat);
double distance_to_sat = vector_norm(3, points_to_sat);
double base_distance_to_sat = vector_norm(3, base_points_to_sat);
/* Fill out the observation details into the NAV_MEAS structure for this satellite, */
/* We simulate the pseudorange as a noisy range measurement, and */
/* the carrier phase as a noisy range in wavelengths + an integer offset. */
populate_nav_meas(&sim_state.nav_meas[num_sats_selected],
distance_to_sat, el, i);
populate_nav_meas(&sim_state.base_nav_meas[num_sats_selected],
base_distance_to_sat, el, i);
/* As for tracking, we just set each sat consecutively in each channel. */
/* This will cause weird jumps when a satellite rises or sets. */
sim_state.tracking_channel[num_sats_selected].state = TRACKING_RUNNING;
sim_state.tracking_channel[num_sats_selected].prn = simulation_almanacs[i].prn + SIM_PRN_OFFSET;
sim_state.tracking_channel[num_sats_selected].cn0 = sim_state.nav_meas[num_sats_selected].snr;
num_sats_selected++;
}
}
sim_state.noisy_solution.n_used = num_sats_selected;
}
void object_render_flatshading(surface_t* s, object_t* obj, vector_t* pbuffer, char* stencil, vector_t* light) {
int c, diff;
//int l, top, bottom;
vector_t *i, *j, *k;
vector_t q1, q2, N;
mesh_t* mesh = obj->mesh;
vector_t* m = mesh->points;
triangle_t* t = mesh->triangles;
int li;
object_apply_transformations(obj, pbuffer, 128, 96); // FIXME: must use surface center
m = mesh->points;
diff = (char*)pbuffer - (char*)m;
for (c = 0; c < mesh->tcount; c++) {
i = (vector_t*)((char*)t->vertexes[0] + diff);
j = (vector_t*)((char*)t->vertexes[1] + diff);
k = (vector_t*)((char*)t->vertexes[2] + diff);
vector_subtract(j, i, &q1);
vector_subtract(k, i, &q2);
vector_cross_product(&q1, &q2, &N);
if (N.z > 0) {
vector_normalize(&N, &N);
//li = vector_dot_product(&N, light)*5;
//li = f2i(li);
//li = (li < 0) ? 0 : ((li > 4) ? 4 : li);
// FIXME: can optimize memset's by covering only "top" to "bottom" lines
// in this case we should require clean buffers to start with.
// and we should clean them after rendering.
//memset(low, MODE2_HEIGHT << 1, 64); // FIXME: must use surface height
//memset(high, MODE2_HEIGHT << 1, 0); // FIXME: must use surface height
stencil_init(stencil);
// calculate polygon
stencil_add_side(i->x, i->y, j->x, j->y, stencil);
stencil_add_side(j->x, j->y, k->x, k->y, stencil);
stencil_add_side(k->x, k->y, i->x, i->y, stencil);
/*
top = (i->y < j->y) ? ((i->y < k->y) ? i->y : k->y) : ((j->y < k->y) ? j->y : k->y);
bottom = (i->y > j->y) ? ((i->y > k->y) ? i->y : k->y) : ((j->y > k->y) ? j->y : k->y);
for (l = top; l <= bottom; l++) {
surface_hline(s, low[l], l, high[l], DITHER(li, l));
}
*/
surface_stencil_render(s, stencil, vector_dot_product(&N, light)/6);
//surface_stencil_render(s, stencil, li);
}
t++;
}
}
u8 propagate(u8 n, double ref_ecef[3],
navigation_measurement_t *m_in_base, gps_time_t *t_base,
navigation_measurement_t *m_in_rover, gps_time_t *t_rover,
navigation_measurement_t *m_out_base)
{
double dt = gpsdifftime(*t_rover, *t_base);
(void)dt;
double dr_[n];
for (u8 i=0; i<n; i++) {
m_out_base[i].prn = m_in_base[i].prn;
m_out_base[i].snr = m_in_base[i].snr;
m_out_base[i].lock_time = m_in_base[i].lock_time;
/* Calculate delta range. */
double dr[3];
vector_subtract(3, m_in_rover[i].sat_pos, m_in_base[i].sat_pos, dr);
/* Subtract linear term (initial satellite velocity * dt),
* we are going to add back in the linear term derived from the Doppler
* instead. */
/*vector_add_sc(3, dr, m_in_base[i].sat_vel, -dt, dr);*/
/* Make unit vector to satellite, e. */
double e[3];
vector_subtract(3, m_in_rover[i].sat_pos, ref_ecef, e);
vector_normalize(3, e);
/* Project onto the line of sight vector. */
dr_[i] = vector_dot(3, dr, e);
/*printf("# ddr_ = %f\n", dr_[i]);*/
/* Add back in linear term now using Doppler. */
/*dr_[i] -= m_in_base[i].raw_doppler * dt * GPS_L1_LAMBDA;*/
/*printf("# dr_dopp = %f\n", -m_in_base[i].raw_doppler * dt * GPS_L1_LAMBDA);*/
/*printf("# raw dopp = %f\n", m_in_rover[i].raw_doppler);*/
/*printf("# my dopp = %f\n", -vector_dot(3, e, m_in_rover[i].sat_vel) / GPS_L1_LAMBDA);*/
/*printf("# ddopp = %f\n", m_in_rover[i].raw_doppler - vector_dot(3, e, m_in_rover[i].sat_vel) / GPS_L1_LAMBDA);*/
/*printf("[%f, %f],", m_in_base[i].raw_doppler, vector_dot(3, e, m_in_rover[i].sat_vel) / GPS_L1_LAMBDA);*/
/*printf("# dr_ = %f\n", dr_[i]);*/
m_out_base[i].raw_pseudorange = m_in_base[i].raw_pseudorange + dr_[i];
m_out_base[i].pseudorange = m_in_base[i].pseudorange;
m_out_base[i].carrier_phase = m_in_base[i].carrier_phase - dr_[i] / GPS_L1_LAMBDA;
m_out_base[i].raw_doppler = m_in_base[i].raw_doppler;
m_out_base[i].doppler = m_in_base[i].doppler;
/*m_in_base[i].carrier_phase -= dr_[i] / GPS_L1_LAMBDA;*/
}
return 0;
}
/** Using available almanac and ephemeris information, determine
* whether a satellite is in view and the range of doppler frequencies
* in which we expect to find it.
*
* \param prn 0-indexed PRN
* \param t Time at which to evaluate ephemeris and almanac (typically system's
* estimate of current time)
* \param dopp_hint_low, dopp_hint_high Pointers to store doppler search range
* from ephemeris or almanac, if available and elevation > mask
* \return Score (higher is better)
*/
static u16 manage_warm_start(u8 prn, gps_time_t t,
float *dopp_hint_low, float *dopp_hint_high)
{
/* Do we have any idea where/when we are? If not, no score. */
/* TODO: Stricter requirement on time and position uncertainty?
We ought to keep track of a quantitative uncertainty estimate. */
if (time_quality < TIME_GUESS &&
position_quality < POSITION_GUESS)
return SCORE_COLDSTART;
float el = 0;
double el_d, _, dopp_hint = 0, dopp_uncertainty = DOPP_UNCERT_ALMANAC;
/* Do we have a suitable ephemeris for this sat? If so, use
that in preference to the almanac. */
if (ephemeris_good(&es[prn], t)) {
double sat_pos[3], sat_vel[3], el_d;
calc_sat_state(&es[prn], t, sat_pos, sat_vel, &_, &_);
wgsecef2azel(sat_pos, position_solution.pos_ecef, &_, &el_d);
el = (float)(el_d) * R2D;
if (el < elevation_mask)
return SCORE_BELOWMASK;
vector_subtract(3, sat_pos, position_solution.pos_ecef, sat_pos);
vector_normalize(3, sat_pos);
/* sat_pos now holds unit vector from us to satellite */
vector_subtract(3, sat_vel, position_solution.vel_ecef, sat_vel);
/* sat_vel now holds velocity of sat relative to us */
dopp_hint = -GPS_L1_HZ * (vector_dot(3, sat_pos, sat_vel) / GPS_C
+ position_solution.clock_bias);
/* TODO: Check sign of receiver frequency offset correction */
if (time_quality >= TIME_FINE)
dopp_uncertainty = DOPP_UNCERT_EPHEM;
} else if (almanac[prn].valid) {
calc_sat_az_el_almanac(&almanac[prn], t.tow, t.wn-1024,
position_solution.pos_ecef, &_, &el_d);
el = (float)(el_d) * R2D;
if (el < elevation_mask)
return SCORE_BELOWMASK;
dopp_hint = -calc_sat_doppler_almanac(&almanac[prn], t.tow, t.wn,
position_solution.pos_ecef);
} else {
return SCORE_COLDSTART; /* Couldn't determine satellite state. */
}
/* Return the doppler hints and a score proportional to elevation */
*dopp_hint_low = dopp_hint - dopp_uncertainty;
*dopp_hint_high = dopp_hint + dopp_uncertainty;
return SCORE_COLDSTART + SCORE_WARMSTART * el / 90.f;
}
void simulation_render_lagrange_points(simulation_t* sim,celestial_body_t* secondary)
{
vector_t lagrange_points[5];
vector_t rel_position=vector_subtract(secondary->base.position,secondary->orbit.primary->base.position);
vector_t direction=vector_normalize(rel_position);
float a=secondary->base.mass/(secondary->base.mass+secondary->orbit.primary->base.mass);
float offset=powf(a*0.3333333,0.333333);
lagrange_points[0]=vector_multiply(secondary->orbit.semi_major_axis*(1+offset),direction);
lagrange_points[1]=vector_multiply(secondary->orbit.semi_major_axis*(1-offset),direction);
lagrange_points[2]=vector_multiply(-secondary->orbit.semi_major_axis*(1+(5.0/12.0)*a),direction);
lagrange_points[3].x=rel_position.x*cos(M_PI/3)+rel_position.y*sin(M_PI/3);
lagrange_points[3].y=rel_position.x*-sin(M_PI/3)+rel_position.y*cos(M_PI/3);
lagrange_points[4].x=rel_position.x*cos(-M_PI/3)+rel_position.y*sin(-M_PI/3);
lagrange_points[4].y=rel_position.x*-sin(-M_PI/3)+rel_position.y*cos(-M_PI/3);
int i;
for(i=0;i<5;i++)
{
char str[128];
sprintf(str,"%s-%s L%d point",secondary->orbit.primary->name,secondary->name,i+1);
vector_t screen_pos=vector_transform(vector_add(secondary->orbit.primary->base.position,lagrange_points[i]),sim->camera);
draw_cross(screen_pos,get_color(0,0,255));
draw_text(screen_pos.x+10,screen_pos.y,str);
}
}
static void system_monitor_thread(void *arg)
{
(void)arg;
chRegSetThreadName("system monitor");
systime_t time = chVTGetSystemTime();
bool ant_status = 0;
while (TRUE) {
if (ant_status != frontend_ant_status()) {
ant_status = frontend_ant_status();
if (ant_status && frontend_ant_setting() == AUTO) {
log_info("Now using external antenna.");
}
else if (frontend_ant_setting() == AUTO) {
log_info("Now using patch antenna.");
}
}
u32 status_flags = ant_status << 31 | SBP_MAJOR_VERSION << 16 | SBP_MINOR_VERSION << 8;
sbp_send_msg(SBP_MSG_HEARTBEAT, sizeof(status_flags), (u8 *)&status_flags);
/* If we are in base station mode then broadcast our known location. */
if (broadcast_surveyed_position && position_quality == POSITION_FIX) {
double tmp[3];
double base_ecef[3];
double base_distance;
llhdeg2rad(base_llh, tmp);
wgsllh2ecef(tmp, base_ecef);
vector_subtract(3, base_ecef, position_solution.pos_ecef, tmp);
base_distance = vector_norm(3, tmp);
if (base_distance > BASE_STATION_DISTANCE_THRESHOLD) {
log_warn("Invalid surveyed position coordinates\n");
} else {
sbp_send_msg(SBP_MSG_BASE_POS_ECEF, sizeof(msg_base_pos_ecef_t), (u8 *)&base_ecef);
}
}
msg_iar_state_t iar_state;
if (simulation_enabled_for(SIMULATION_MODE_RTK)) {
iar_state.num_hyps = 1;
} else {
iar_state.num_hyps = dgnss_iar_num_hyps();
}
sbp_send_msg(SBP_MSG_IAR_STATE, sizeof(msg_iar_state_t), (u8 *)&iar_state);
DO_EVERY(2,
send_thread_states();
);
sleep_until(&time, MS2ST(heartbeat_period_milliseconds));
}
ErrorCode rotate_coords(FloatValue *coords, FloatValue rotangle, IndexValue bond, struct ResidueData *residues, IndexValue res)
{
IndexValue atom1, atom2;
FloatValue x1, y1, z1;
FloatValue x2, y2, z2;
FloatValue rx, ry, rz;
FloatValue matxx, matxy, matxz;
FloatValue matyx, matyy, matyz;
FloatValue matzx, matzy, matzz;
IndexValue lastatom;
FloatValue dx, dy, dz;
FloatValue x, y, z;
IndexValue atom;
atom1 = residues[res].bonds[bond].atom1;
x1 = coords[xcoord_index(atom1)];
y1 = coords[ycoord_index(atom1)];
z1 = coords[zcoord_index(atom1)];
atom2 = residues[res].bonds[bond].atom2;
x2 = coords[xcoord_index(atom2)];
y2 = coords[ycoord_index(atom2)];
z2 = coords[zcoord_index(atom2)];
// This is the index of the last atom in the array affected by this rotation,.
lastatom = residues[res].bonds[bond].lastatom;
// Calculating normalized rotation axis.
vector_subtract(&rx, &ry, &rz, x2, y2, z2, x1, y1, z1);
vector_normalize(&rx, &ry, &rz);
// Calculating rotation matrix for rotation angle rotangle around rotation axis (rx, ry, rz).
create_rotation_matrix(&matxx, &matxy, &matxz, &matyx, &matyy, &matyz, &matzx, &matzy, &matzz, rotangle, rx, ry, rz);
// for (atom = atom2 + 1; atom < lastatom; atom++)
for (atom = atom2 + 1; atom <= lastatom; atom++)
{
dx = coords[xcoord_index(atom)] - x2;
dy = coords[ycoord_index(atom)] - y2;
dz = coords[zcoord_index(atom)] - z2;
x = matxx * dx + matxy * dy + matxz * dz;
y = matyx * dx + matyy * dy + matyz * dz;
z = matzx * dx + matzy * dy + matzz * dz;
coords[xcoord_index(atom)] = x + x2;
coords[ycoord_index(atom)] = y + y2;
coords[zcoord_index(atom)] = z + z2;
}
return NO_ERROR;
}
void spacecraft_calculate_orbit(simulation_t* sim,spacecraft_t* spacecraft)
{
//Determine smallest s.o.i that the spacecraft lies within
celestial_body_t* primary=sim->primary;
double smallest_soi=vector_magnitude(vector_subtract(sim->primary->base.position,spacecraft->base.position));
int i;
for(i=0;i<sim->num_celestial_bodies;i++)
{
celestial_body_t* body=&sim->celestial_bodies[i];
if(body->orbit.primary!=NULL&&body->sphere_of_influence<smallest_soi&&body->sphere_of_influence>vector_magnitude(vector_subtract(body->base.position,spacecraft->base.position)))
{
primary=body;
smallest_soi=body->sphere_of_influence;
}
}
//Calculate spacecraft orbit
spacecraft->orbit=orbit_calculate(primary,vector_subtract(spacecraft->base.position,primary->base.position),vector_subtract(spacecraft->base.velocity,primary->base.velocity));
}
Ray Raytracer::make_reflection_ray(const Vector3d &normal,
const Ray &ray,
const Point3d &intersection)
{
Ray reflection(intersection,
normalize(
vector_subtract(ray.direction(),
scalar_multiply(normal,
2 * dot_product(ray.direction(), normal)))));
return reflection;
}
spacecraft_dock(spacecraft_t* a,int a_dock_index,spacecraft_t* b,int b_dock_index)
{
dock_t* a_dock=a->docks+a_dock_index;
dock_t* b_dock=b->docks+b_dock_index;
a_dock->docked_spacecraft=b;
a_dock->docked_dock=b_dock_index;
b_dock->docked_spacecraft=a;
b_dock->docked_dock=a_dock_index;
b->base.position=vector_add(a->base.position,vector_subtract(rotate_vector(a_dock->position,a_dock->orientation),rotate_vector(b_dock->position,b_dock->orientation)));
b->parent=a->parent==NULL?a:a->parent;
}
vector_t calculate_gravitation(object_t* a,object_t* b)
{
//Get distance and direction
vector_t displacement=vector_subtract(a->position,b->position);
double distance=vector_magnitude(displacement);
vector_t direction=vector_multiply(1/distance,displacement);
//Calculate gravity
double gravity=GRAVITATIONAL_CONSTANT*(a->mass*b->mass)/(distance*distance);
return vector_multiply(gravity,direction);
}
int main(int argc, char * argv[])
{
int n, n_threads = 0;
matrix_t * A,
* A_orig;
vector_t * b,
* b_orig,
* x,
* check_res_mat;
double t_start, t_end;
get_input_params(argc, argv, &n);
initialize_global_state();
/* initialize data */
A_orig = matrix_create_rand(n, n),
A = matrix_clone(A_orig),
b_orig = vector_create_rand(n),
b = vector_clone(b_orig),
x = vector_create(n),
check_res_mat = vector_create(n);
t_start = get_time_in_sec();
perform_gaussian_elimination_on_matrix(A, b);
perform_back_substitution(A, x, b);
t_end = get_time_in_sec();
/* calculate Ax - b and find it's l2norm to test for correctness */
matrix_mult_vector(A_orig, x, check_res_mat);
vector_subtract(check_res_mat, check_res_mat, b_orig); /* c = c - b */
printf("num-procs = %d\n", omp_get_num_procs());
#pragma omp parallel num_threads(num_threads)
#pragma omp single
n_threads = omp_get_num_threads();
printf("num-threads = %d\n", n_threads);
printf("Performed Gaussian Elimination in %.12lfs\n", t_end - t_start);
printf("Ax-b l2norm = %.6le\n", vector_l2norm(check_res_mat));
puts("done");
return 0;
}
double predict_range(double rx_pos[3],
gps_time_t tot,
ephemeris_t *ephemeris)
{
double sat_pos[3];
double sat_vel[3];
double temp[3];
double clock_err, clock_rate_err;
calc_sat_pos(sat_pos, sat_vel, &clock_err, &clock_rate_err, ephemeris, tot);
vector_subtract(3, sat_pos, rx_pos, temp); // temp = sat_pos - rx_pos
return vector_norm(3, temp);
}
void player_handle_movement(Player *p, Wall *walls, float dt)
{
// Calculate angle based on where facing
p->angle = atan2((p->face.y - p->exact_pos.y),(p->face.x - p->exact_pos.x));
// apply friction
p->vel.x *= p->friction;
p->vel.y *= p->friction;
// apply acceleration
p->vel.x += p->x_input * p->acc;
p->vel.y -= p->y_input * p->acc;
// update position
fvector new_pos;
new_pos.x = p->exact_pos.x + p->vel.x*dt;
new_pos.y = p->exact_pos.y + p->vel.y*dt;
// check for collisions with walls
int i;
for (i=0; i < WALL_MAX; i++)
{
if (walls[i].ex)
{
Intersection sect;
fvector evec1, evec2, wvec, norm, snorm;
evec1 = f_vector(walls[i].end1.x, walls[i].end1.y);
evec2 = f_vector(walls[i].end2.x, walls[i].end2.y);
wvec = vector_subtract(evec1, evec2);
norm = get_normal(wvec);
if (dot_product_f_vectors(norm, p->vel) > 0) norm = scale_f_vector(norm, -1);
snorm = scale_f_vector(norm, 20);
sect = intersect_line_segments(p->exact_pos, new_pos, translate_f_vector(evec1, snorm), translate_f_vector(evec2, snorm));
if (sect.intersects)
{
new_pos = translate_f_vector(sect.point, norm);
}
}
}
p->exact_pos=new_pos;
// handle collision with screen edges
player_check_bounds(p);
}
void calculate_forces(simulation_t* sim)
{
int i,j;
for(i=0;i<sim->num_celestial_bodies;i++)
{
//Calculate forces between celestial_bodies
for(j=i+1;j<sim->num_celestial_bodies;j++)
{
vector_t force=calculate_gravitation((object_t*)(&sim->celestial_bodies[j]),(object_t*)(&sim->celestial_bodies[i]));
sim->celestial_bodies[i].base.force=vector_add(sim->celestial_bodies[i].base.force,force);
sim->celestial_bodies[j].base.force=vector_subtract(sim->celestial_bodies[j].base.force,force);
}
}
}
/************************************************
|| ray_from_eye_throug()
|| Purpose: calculate a ray for the current pixel
************************************************/
Ray ray_from_eye_through(Vector curPixel)
{
Ray tempRay;
//Camera
tempRay.u.x = 0;
tempRay.u.y = 0;
tempRay.u.z = 1;
tempRay.u.w = 1;
tempRay.v=unit_vector(vector_subtract(curPixel , tempRay.u));
//printf("\nUnit Vector\nX: %f, Y: %f, Z: %f\n", tempRay.v.x, tempRay.v.y, tempRay.v.z);
//printf("\nX: %f, Y: %f, Z: %f\n", tempRay.v.x, tempRay.v.y, tempRay.v.z);
return tempRay;
}
/**
* Performs a simulation step for the given duration, by moving
* our simulated position in a circle at a given radius and speed
* around the simulation's center point.
*/
void simulation_step_position_in_circle(double elapsed)
{
/* Update the angle, making a small angle approximation. */
sim_state.angle += (sim_settings.speed * elapsed) / sim_settings.radius;
if (sim_state.angle > 2*M_PI) {
sim_state.angle = 0;
}
double pos_ned[3] = {
sim_settings.radius * sin(sim_state.angle),
sim_settings.radius * cos(sim_state.angle),
0
};
/* Fill out position simulation's gnss_solution pos_ECEF, pos_LLH structures */
wgsned2ecef_d(pos_ned,
sim_settings.base_ecef,
sim_state.pos);
/* Calculate an accurate baseline for simulating RTK */
vector_subtract(3,
sim_state.pos,
sim_settings.base_ecef,
sim_state.baseline);
/* Add gaussian noise to PVT position */
double* pos_ecef = sim_state.noisy_solution.pos_ecef;
double pos_variance = sim_settings.pos_sigma * sim_settings.pos_sigma;
pos_ecef[0] = sim_state.pos[0] + rand_gaussian(pos_variance);
pos_ecef[1] = sim_state.pos[1] + rand_gaussian(pos_variance);
pos_ecef[2] = sim_state.pos[2] + rand_gaussian(pos_variance);
wgsecef2llh(sim_state.noisy_solution.pos_ecef, sim_state.noisy_solution.pos_llh);
/* Calculate Velocity vector tangent to the sphere */
double noisy_speed = sim_settings.speed +
rand_gaussian(sim_settings.speed_sigma *
sim_settings.speed_sigma);
sim_state.noisy_solution.vel_ned[0] = noisy_speed * cos(sim_state.angle);
sim_state.noisy_solution.vel_ned[1] = noisy_speed * -1.0 * sin(sim_state.angle);
sim_state.noisy_solution.vel_ned[2] = 0.0;
wgsned2ecef(sim_state.noisy_solution.vel_ned,
sim_state.noisy_solution.pos_ecef,
sim_state.noisy_solution.vel_ecef);
}
void spacecraft_collision(simulation_t* sim,spacecraft_t* spacecraft)
{
vector_t displacement=vector_subtract(spacecraft->base.position,spacecraft->orbit.primary->base.position);
double distance=vector_magnitude(displacement);
double penetration=spacecraft->orbit.primary->radius-distance;
if(penetration>0)
{
displacement=vector_multiply((distance+penetration)/distance,displacement);
spacecraft->base.position=vector_add(spacecraft->orbit.primary->base.position,displacement);
spacecraft->base.velocity=spacecraft->orbit.primary->base.velocity;
spacecraft->base.velocity.x+=spacecraft->orbit.primary->base.delta_rot*displacement.y;
spacecraft->base.velocity.y+=-spacecraft->orbit.primary->base.delta_rot*displacement.x;
spacecraft->base.rotation=atan2(-displacement.y,displacement.x)+M_PI/2;
spacecraft->base.delta_rot=0;
}
}
/** Calculate the Doppler shift of a satellite as observed at a reference
* position given the satellite almanac.
*
* \param alm Pointer to an almanac structure for the satellite of interest.
* \param t GPS time of week at which to calculate the Doppler shift.
* \param week GPS week number modulo 1024 or pass -1 to assume within one
* half-week of the almanac time of applicability.
* \param ref ECEF coordinates of the reference point from which the azimuth
* and elevation is to be determined, passed as [X, Y, Z], all in
* meters.
* \return The Doppler shift in Hz.
*/
double calc_sat_doppler_almanac(almanac_t* alm, double t, s16 week,
double ref[3])
{
double sat_pos[3];
double sat_vel[3];
double vec_ref_sat[3];
calc_sat_state_almanac(alm, t, week, sat_pos, sat_vel);
/* Find the vector from the reference position to the satellite. */
vector_subtract(3, sat_pos, ref, vec_ref_sat);
/* Find the satellite velocity projected on the line of sight vector from the
* reference position to the satellite. */
double radial_velocity = vector_dot(3, vec_ref_sat, sat_vel) / \
vector_norm(3, vec_ref_sat);
/* Return the Doppler shift. */
return GPS_L1_HZ * radial_velocity / NAV_C;
}
END_TEST
START_TEST(test_vector_subtract) {
u32 i, t;
seed_rng();
for (t = 0; t < LINALG_NUM; t++) {
u32 n = sizerand(MSIZE_MAX);
double A[n], B[n], C[n];
for (i = 0; i < n; i++) {
A[i] = mrand;
B[i] = A[i];
}
vector_subtract(n, A, B, C);
for (i = 0; i < n; i++)
fail_unless(fabs(C[i]) < LINALG_TOL,
"Vector element differs from 0: %lf",
C[i]);
}
}
/************************************************
|| shadow()
|| Purpose: find if an object is in shadow or not
|| by comparing the object # of the closest
|| object
************************************************/
int shadow(ObjectAttributes obj, int i)
{
ObjectAttributes closestObj;
Ray ray;
//calculate ray to shoot from light to sphere
ray.u = lightSources[i].position;
ray.u.w = 1;
ray.v=unit_vector(vector_subtract(obj.worldPoint , ray.u));
//find the closest intersection
closestObj = closest_intersection(ray);
//See if it's in shadow
if(closestObj.objNumber == obj.objNumber)
return 1; //true
else
return 0; //false
}
void render_celestial_body_info(celestial_body_t* celestial_body,int x,int y)
{
char str[64];
double value;
char si_prefix;
draw_text(x,y,celestial_body->name);
y+=10;
sprintf(str,"Mass : %.2ekg",celestial_body->base.mass);
draw_text(x,y,str);
y+=10;
get_si_prefix(celestial_body->radius,&value,&si_prefix);
sprintf(str,"Radius : %.2f%cm",value,si_prefix);
draw_text(x,y,str);
y+=10;
if(celestial_body->orbit.primary==NULL)return;
vector_t rel_position=vector_subtract(celestial_body->base.position,celestial_body->orbit.primary->base.position);
render_orbit_info(&celestial_body->orbit,atan2(rel_position.y,rel_position.x),x,y);
}
/************************************************
|| reflection()
|| Purpose: this is supposed to calculate reflections
|| but it does not. this is never called
|| in the current version
************************************************/
Ray reflection(ObjectAttributes obj, Ray ray)
{
Vector n,L,R;
Matrix temp;
int iNum;
Ray newRay;
iNum = obj.objNumber;
obj.objPoint.w = 0;
temp = (objects[iNum].transform.transformation);
//Intersection point in world space
newRay.u = obj.worldPoint;
//Direction v-2n(n.v)
n = matrixTimesVector(temp, obj.objPoint);
newRay.v = vector_subtract(ray.v ,vector_X_n(vector_X_n(n, dot_product(n,ray.v)),2));
return newRay;
}
/** Returns the vector \e to a point given in WGS84 Earth Centered, Earth Fixed
* (ECEF) Cartesian coordinates \e from a reference point, also given in WGS84
* ECEF coordinates, in the local North, East, Down (NED) frame of the
* reference point.
*
* \see wgsecef2ned.
*
* \param ecef Cartesian coordinates of the point, passed as [X, Y, Z],
* all in meters.
* \param ref_ecef Cartesian coordinates of the reference point, passed as
* [X, Y, Z], all in meters.
* \param ned The North, East, Down vector is written into this array as
* [N, E, D], all in meters.
*/
void wgsecef2ned_d(const double ecef[3], const double ref_ecef[3],
double ned[3]) {
double tempv[3];
vector_subtract(3, ecef, ref_ecef, tempv);
wgsecef2ned(tempv, ref_ecef, ned);
}
void model_recalc_normals_mesh(model_type *model,int mesh_idx,bool only_tangent)
{
int n,k,t,neg_v;
float u10,u20,v10,v20,f_denom;
bool vertex_in_poly;
d3vct p10,p20,vlft,vrgt,v_num;
d3vct *normals,*nptr,*tangents,*tptr;
d3pnt *pt,*pt_1,*pt_2;
model_mesh_type *mesh;
model_vertex_type *vertex;
model_poly_type *poly;
mesh=&model->meshes[mesh_idx];
if ((mesh->nvertex==0) || (mesh->npoly==0)) return;
// memory for tangent space
normals=(d3vct*)malloc(mesh->npoly*sizeof(d3vct));
if (normals==NULL) return;
tangents=(d3vct*)malloc(mesh->npoly*sizeof(d3vct));
if (tangents==NULL) {
free(normals);
return;
}
// find tangent and binormal for triangles
poly=mesh->polys;
nptr=normals;
tptr=tangents;
for (n=0; n!=mesh->npoly; n++) {
// treat all polys as triangles
neg_v=poly->ptsz-1;
// get the side vectors (p1-p0) and (p2-p0)
pt=&mesh->vertexes[poly->v[0]].pnt;
pt_1=&mesh->vertexes[poly->v[1]].pnt;
pt_2=&mesh->vertexes[poly->v[neg_v]].pnt;
vector_create(&p10,pt_1->x,pt_1->y,pt_1->z,pt->x,pt->y,pt->z);
vector_create(&p20,pt_2->x,pt_2->y,pt_2->z,pt->x,pt->y,pt->z);
// calculate the normal by the cross
vector_cross_product(nptr,&p10,&p20);
nptr++;
// get the UV scalars (u1-u0), (u2-u0), (v1-v0), (v2-v0)
u10=poly->gx[1]-poly->gx[0];
u20=poly->gx[neg_v]-poly->gx[0];
v10=poly->gy[1]-poly->gy[0];
v20=poly->gy[neg_v]-poly->gy[0];
// calculate the tangent
// (v20xp10)-(v10xp20) / (u10*v20)-(v10*u20)
vector_scalar_multiply(&vlft,&p10,v20);
vector_scalar_multiply(&vrgt,&p20,v10);
vector_subtract(&v_num,&vlft,&vrgt);
f_denom=(u10*v20)-(v10*u20);
if (f_denom!=0.0f) f_denom=1.0f/f_denom;
vector_scalar_multiply(tptr,&v_num,f_denom);
vector_normalize(tptr);
tptr++;
poly++;
}
// average tangent space for each
// poly vertex to find one for shared
// vertexes
vertex=mesh->vertexes;
for (n=0; n!=mesh->nvertex; n++) {
vertex->tangent_space.tangent.x=vertex->tangent_space.tangent.y=vertex->tangent_space.tangent.z=0.0f;
vertex->tangent_space.normal.x=vertex->tangent_space.normal.y=vertex->tangent_space.normal.z=0.0f;
poly=mesh->polys;
nptr=normals;
tptr=tangents;
for (k=0; k!=mesh->npoly; k++) {
vertex_in_poly=FALSE;
for (t=0; t!=poly->ptsz; t++) {
if (poly->v[t]==n) {
vertex_in_poly=TRUE;
break;
}
}
if (vertex_in_poly) {
vertex->tangent_space.tangent.x+=tptr->x;
vertex->tangent_space.tangent.y+=tptr->y;
vertex->tangent_space.tangent.z+=tptr->z;
vertex->tangent_space.normal.x+=nptr->x;
vertex->tangent_space.normal.y+=nptr->y;
vertex->tangent_space.normal.z+=nptr->z;
}
poly++;
nptr++;
tptr++;
}
vector_normalize(&vertex->tangent_space.tangent);
if (!only_tangent) vector_normalize(&vertex->tangent_space.normal);
vertex++;
}
// free the tangent spaces
free(normals);
free(tangents);
// fix any normals that are 0,0,0
// this usually happens when there are bad
// polys
for (k=0; k!=mesh->nvertex; k++) {
if ((vertex->tangent_space.normal.x==0.0f) && (vertex->tangent_space.normal.y==0.0f) && (vertex->tangent_space.normal.z==0.0f)) {
pt=&mesh->vertexes[k].pnt;
vertex->tangent_space.normal.x=(float)(pt->x-vertex->pnt.x);
vertex->tangent_space.normal.y=(float)(pt->y-vertex->pnt.y);
vertex->tangent_space.normal.z=(float)(pt->z-vertex->pnt.z);
vector_normalize(&vertex->tangent_space.normal);
}
}
// determine in-out to map flips
// skip if not calculating normals
if (only_tangent) return;
// determine in/out and invert
vertex=mesh->vertexes;
for (n=0; n!=mesh->nvertex; n++) {
if (!model_recalc_normals_determine_vector_in_out(model,mesh_idx,n)) {
vertex->tangent_space.normal.x=-vertex->tangent_space.normal.x;
vertex->tangent_space.normal.y=-vertex->tangent_space.normal.y;
vertex->tangent_space.normal.z=-vertex->tangent_space.normal.z;
}
vertex++;
}
}
void test_vector()
{
Vector *v = vector_new(1,2,3);
CU_ASSERT(v->x == 1);
CU_ASSERT(v->y == 2);
CU_ASSERT(v->z == 3);
Vector *vc = vector_copy(v);
CU_ASSERT(vc->x == 1);
CU_ASSERT(vc->y == 2);
CU_ASSERT(vc->z == 3);
Vector *u = vector_new(4,5,6);
vector_add(v,u);
CU_ASSERT(v->x == 5);
CU_ASSERT(v->y == 7);
CU_ASSERT(v->z == 9);
vector_subtract(v,u);
CU_ASSERT(v->x == 1);
CU_ASSERT(v->y == 2);
CU_ASSERT(v->z == 3);
vector_multiply(v,2);
CU_ASSERT(v->x == 2);
CU_ASSERT(v->y == 4);
CU_ASSERT(v->z == 6);
Vector *w = vector_cross(v,u);
CU_ASSERT(w->x == -6);
CU_ASSERT(w->y == 12);
CU_ASSERT(w->z == -6);
vector_free(v);
vector_free(u);
vector_free(w);
v = vector_new(3,4,0);
u = vector_new(0,3,4);
CU_ASSERT(v->x==u->y);
CU_ASSERT(v->y==u->z);
w = vector_cross(v,u);
CU_ASSERT(w->x == 16);
CU_ASSERT(w->y == -12);
CU_ASSERT(w->z == 9);
CU_ASSERT(within_tol(vector_magnitude(v),5));
CU_ASSERT(within_tol(vector_magnitude(u),5));
vector_free(v);
vector_free(u);
vector_free(w);
v = vector_new(1,0,0);
u = vector_new(0,0,1);
double a = vector_angle(v,u)/D2R;
CU_ASSERT(within_tol(a,90));
CU_ASSERT(within_tol(vector_angle(u,v),90.*D2R));
w = vector_cross(v,u);
CU_ASSERT(within_tol(vector_magnitude(w),1));
CU_ASSERT(w->x==0);
CU_ASSERT(w->y==-1);
CU_ASSERT(w->z==0);
}
/* This function is the key to GPS solution, so it's commented
* liberally. It does a single step of a multi-dimensional
* Newton-Raphson solution for the variables X, Y, Z (in ECEF) plus
* the clock offset for each receiver used to make pseudorange
* measurements. The steps involved are roughly the following:
*
* 1. Account for the Earth's rotation during transmission
*
* 2. Estimate the ECEF position for each satellite measured using
* the downloaded ephemeris
*
* 3. Compute the Jacobian of pseudorange versus estimated state.
* There's no explicit differentiation; it's done symbolically
* first and just coded as a "line of sight" vector.
*
* 4. Get the inverse of the Jacobian times its transpose. This
* matrix is normalized to one, but it tells us the direction we
* must move the state estimate during this step.
*
* 5. Multiply this inverse matrix (H) by the transpose of the
* Jacobian (to yield X). This maps the direction of our state
* error into a direction of pseudorange error.
*
* 6. Multiply this matrix (X) by the error between the estimated
* (ephemeris) position and the measured pseudoranges. This
* yields a vector of corrections to our state estimate. We apply
* these to our current estimate and recurse to the next step.
*
* 7. If our corrections are very small, we've arrived at a good
* enough solution. Solve for the receiver's velocity (with
* vel_solve) and do some bookkeeping to pass the solution back
* out.
*/
static double pvt_solve(double rx_state[],
const u8 n_used,
const navigation_measurement_t const nav_meas[n_used],
double H[4][4])
{
double p_pred[n_used];
/* Vector of prediction errors */
double omp[n_used];
/* G is a geometry matrix tells us how our pseudoranges relate to
* our state estimates -- it's the Jacobian of d(p_i)/d(x_j) where
* x_j are x, y, z, Δt. */
double G[n_used][4];
double Gtrans[4][n_used];
double GtG[4][4];
/* H is the square of the Jacobian matrix; it tells us the shape of
our error (or, if you prefer, the direction in which we need to
move to get a better solution) in terms of the receiver state. */
/* X is just H * Gtrans -- it maps our pseudoranges onto our
* Jacobian update */
double X[4][n_used];
double tempv[3];
double los[3];
double xk_new[3];
double tempd;
double correction[4];
for (u8 j=0; j<4; j++) {
correction[j] = 0.0;
}
for (u8 j = 0; j < n_used; j++) {
/* The satellite positions need to be corrected for Earth's rotation during
* the signal time of flight. */
/* TODO: Explain more about how this corrects for the Sagnac effect. */
/* Magnitude of range vector converted into an approximate time in secs. */
vector_subtract(3, rx_state, nav_meas[j].sat_pos, tempv);
double tau = vector_norm(3, tempv) / GPS_C;
/* Rotation of Earth during time of flight in radians. */
double wEtau = GPS_OMEGAE_DOT * tau;
/* Apply linearlised rotation about Z-axis which will adjust for the
* satellite's position at time t-tau. Note the rotation is through
* -wEtau because it is the ECEF frame that is rotating with the Earth and
* hence in the ECEF frame free falling bodies appear to rotate in the
* opposite direction.
*
* Making a small angle approximation here leads to less than 1mm error in
* the satellite position. */
xk_new[0] = nav_meas[j].sat_pos[0] + wEtau * nav_meas[j].sat_pos[1];
xk_new[1] = nav_meas[j].sat_pos[1] - wEtau * nav_meas[j].sat_pos[0];
xk_new[2] = nav_meas[j].sat_pos[2];
/* Line of sight vector. */
vector_subtract(3, xk_new, rx_state, los);
/* Predicted range from satellite position and estimated Rx position. */
p_pred[j] = vector_norm(3, los);
/* omp means "observed minus predicted" range -- this is E, the
* prediction error vector (or innovation vector in Kalman/LS
* filtering terms).
*/
omp[j] = nav_meas[j].pseudorange - p_pred[j];
/* Construct a geometry matrix. Each row (satellite) is
* independently normalized into a unit vector. */
for (u8 i=0; i<3; i++) {
G[j][i] = -los[i] / p_pred[j];
}
/* Set time covariance to 1. */
G[j][3] = 1;
} /* End of channel loop. */
/* Solve for position corrections using batch least-squares. When
* all-at-once least-squares estimation for a nonlinear problem is
* mixed with numerical iteration (not time-series recursion, but
* iteration on a single set of measurements), it's basically
* Newton's method. There's a reasonably clear explanation of this
* in Wikipedia's article on GPS.
*/
/* Gt := G^{T} */
matrix_transpose(n_used, 4, (double *) G, (double *) Gtrans);
/* GtG := G^{T} G */
matrix_multiply(4, n_used, 4, (double *) Gtrans, (double *) G, (double *) GtG);
/* H \elem \mathbb{R}^{4 \times 4} := GtG^{-1} */
matrix_inverse(4, (const double *) GtG, (double *) H);
/* X := H * G^{T} */
matrix_multiply(4, 4, n_used, (double *) H, (double *) Gtrans, (double *) X);
/* correction := X * E (= X * omp) */
matrix_multiply(4, n_used, 1, (double *) X, (double *) omp, (double *) correction);
/* Increment ecef estimate by the new corrections */
for (u8 i=0; i<3; i++) {
rx_state[i] += correction[i];
}
/* Set the Δt estimates according to this solution */
for (u8 i=3; i<4; i++) {
rx_state[i] = correction[i];
}
/* Look at the magnintude of the correction to see if
* the solution has converged yet.
*/
tempd = vector_norm(3, correction);
if(tempd > 0.001) {
/* The solution has not converged, return a negative value to
* indicate that we should continue iterating.
*/
return -tempd;
}
/* The solution has converged! */
/* Perform the velocity solution. */
vel_solve(&rx_state[4], n_used, nav_meas, (const double (*)[4]) G, (const double (*)[n_used]) X);
return tempd;
}
myCamera::myCamera(float posx, float posy, float posz, float atx, float aty, float atz, float upx, float upy, float upz, int t)
{
move_speed = 0;
wave = 0;
isSprinting = false;
sprintTime = 100;
type = t;
fovy = 65.0;
aspect = 1.0;
zNear = .1;
zFar = 200.0;
top = 5;
bottom = -5;
left = -5;
right = 5;
pos[0] = posx;
pos[1] = posy;
pos[2] = posz;
pos[3] = 1.0;
look[0] = atx;
look[1] = aty;
look[2] = atz;
look[3] = 1.0;
up[0] = upx;
up[1] = upy;
up[2] = upz;
GLfloat* r = normalize(look);
w[0] = -r[0];
w[1] = -r[1];
w[2] = -r[2];
delete[] r;
GLfloat up_w = dot_product(up, w);
// mult_w = (up dot w) * w
GLfloat *mult_w = scalar_mult(up_w, w);
// up_sub_mult_w = up - (up dot w)*w;
GLfloat* up_sub_mult_w = vector_subtract(up, mult_w);
r = normalize(up_sub_mult_w);
v[0] = r[0];
v[1] = r[1];
v[2] = r[2];
delete [] r;
r = cross_product(v, w);
u[0] = r[0];
u[1] = r[1];
u[2] = r[2];
generateT();
generateR();
generateS();
generateM();
delete [] mult_w;
delete [] up_sub_mult_w;
delete [] r;
}
