END_TEST START_TEST(test_vector_three) { u32 i, t; seed_rng(); for (t = 0; t < LINALG_NUM; t++) { double A[3], B[3], C[3], tmp[3]; double D, E, F, norm; for (i = 0; i < 3; i++) { A[i] = mrand / 1e20; B[i] = mrand / 1e20; C[i] = mrand / 1e20; } /* Check triple product identity */ vector_cross(A, B, tmp); D = vector_dot(3, C, tmp); vector_cross(B, C, tmp); E = vector_dot(3, A, tmp); vector_cross(C, A, tmp); F = vector_dot(3, B, tmp); norm = (vector_norm(3, A) + vector_norm(3, B) + vector_norm(3, C))/3; fail_unless(fabs(E - D) < LINALG_TOL * norm, "Triple product failure between %lf and %lf", D, E); fail_unless(fabs(E - F) < LINALG_TOL * norm, "Triple product failure between %lf and %lf", E, F); fail_unless(fabs(F - D) < LINALG_TOL * norm, "Triple product failure between %lf and %lf", F, D); } }
/** 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; }
vector3d ahrs_drift_correction(dataexchange_t *data) { vector3d corr_vector, acc_g; corr_vector.x = 0.0; corr_vector.y = 0.0; corr_vector.z = 0.0; //do correction only then acceleration is close to 1G (unreliable if greater) acc_g = adxl345_raw_to_g(data, SCALE_2G_10B); double acc_magnitude = vector_magnitude(acc_g); if (fabs(acc_magnitude - 1.0) <= 0.15) { float corr_strength = 0.15; //vectors for rotation matrix vector3d acc_v, mag_v; acc_v.x = (*data).acc_x; acc_v.y = (*data).acc_y; acc_v.z = (*data).acc_z; mag_v.x = (*data).mag_x; mag_v.y = (*data).mag_y; mag_v.z = (*data).mag_z; hmc5883l_applyCalibration(&mag_v, calib_ptr); vector3d down = vector_inv(acc_v); vector3d east = vector_cross(down, mag_v); vector3d north = vector_cross(east, down); //normalize vectors vector_norm(&down); vector_norm(&east); vector_norm(&north); //matrix from rotation quaternion matrix3x3d rot_matrix = quaternion_to_matrix((*data).qr); //correction vector calculation vector3d sum1 = vector_sum(vector_cross(north, matrix_row_to_vector(&rot_matrix, 1)), vector_cross(east, matrix_row_to_vector(&rot_matrix, 2))); vector3d sum2 = vector_sum(sum1, vector_cross(down, matrix_row_to_vector(&rot_matrix, 3))); corr_vector = vector_scale(sum2, corr_strength); } return corr_vector; }
vec3 vector_normalized(vec3 v) { float norm = vector_norm(v); v.x /= norm; v.y /= norm; v.z /= norm; return v; }
/* * Calculate light color */ color lightColor(const intersection *inter, const ray *srcRay) { vec3 v = vector_normalized(vector_float_mul(-1.0f, srcRay->dir)); color cD = BLACK, refCoef = reflectCoef(inter->mat.reflect_coef, inter->normal, srcRay->dir); // For each light calculate and sum color for (int k = 0; k < num_lights; ++k) { vec3 l = vector_minus(lights[k].position, inter->position); float normL = vector_norm(l); l = vector_normalized(l); // Look for object between light source and current intersection ray objectRay; // Ray from object to light ray_init(&objectRay, inter->position, l, EPSILON, normL, 0); float interFound = 0.0f; for (int i = 0; i < object_count; ++i) { interFound += interDist(&objectRay, &(scene[i])); } if (!interFound) { vec3 h = vector_normalized(vector_add(l, v)); float sc_nl = clamp(vector_dot(inter->normal, l), 0.0f, 1.0f); float sc_hn = clamp(vector_dot(h, inter->normal), 0.0f, 1.0f); cD = vector_add(cD, vector_vec_mul(lights[k].col, vector_float_mul(sc_nl, (vector_add(vector_float_mul(1 / M_PI, inter->mat.kd), (vector_float_mul(pow(sc_hn, inter->mat.shininess), vector_float_mul(((inter->mat.shininess + 8) / (8 * M_PI)), refCoef)))))))); } } return cD; }
void raytrace(t_env *env) { int x; int y; t_ray ray; t_col col; y = 0; while (y <= WIN_Y) { x = 0; while (x <= WIN_X) { ray.start = OBJ.cam_s; ray.start.x += x; ray.start.y += y; ray.dir = OBJ.cam_dir; vector_norm(&ray.dir); set_col(&OBJ.col, 0, 0, 0); env->br = 0; col = shoot_ray(ray, 10, env); save_to_img(env, col, x, y); x++; } y++; } }
struct vector* vector_normalize(struct vector* v) { struct vector* vnorm = vector_new(v->length); double norm = vector_norm(v); assert(norm != 0); vector_normalize_into(vnorm, v); return vnorm; }
void vector_normalize_into(struct vector* reciever, struct vector* v) { double norm = vector_norm(v); assert(norm != 0); for(int i = 0; i < v->length; i++) { VECTOR_IDX_INTO(reciever, i) = VECTOR_IDX_INTO(v, i) / norm; } }
void sgp_get_acceleration(vector v_g)// only j2 perturbations taken { vector v_r_ecef, v_g_ecef; float R, R2, R3, R4; //eci2ecef(v_r, v_r_ecef);//see change R = vector_norm(r_ecef_ash); // R2 = pow(R, 2); R2 = (1.5 * J2 * R_E2) / R2; R3 = pow(R, 3); R4 = pow(R, 4); R4 = (7.5 * J2 * pow(r_ecef_ash[2],2) * R_E2) / R4; // v_g_ecef[0] = (-1 * GM * r_ecef_ash[0] * (1 + R2 - R4)) / R3;// v_g_ecef[1] = (-1 * GM * r_ecef_ash[1] * (1 + R2 - R4)) / R3;// v_g_ecef[2] = (-1 * GM * r_ecef_ash[2] * (1 + 3 * R2 - R4)) / R3;// ecef2eci(v_g_ecef, v_g); /* uint8_t sent[3]; for (int i=0;i<2;i=i+1) { sent[i] = (uint8_t)((v_g_ecef[i])); transmit_UART0(sent[i]); }*/ }
int calculate_distance_map (trajectory_frame *frame, void *p) { unsigned int i, j; unsigned int *nbodies = (unsigned int *) p; distance_map *map = distance_map_alloc (*nbodies); /* assign distance map to frame data */ frame->data = map; /* calculate the distance map */ for (i=0; i<*nbodies; i++) { for (j=i+1; j<*nbodies; j++) { /* calculate the distance vector */ t_vector diff; vector_diff (diff, frame->body_i_state [i].r, frame->body_i_state [j].r); /* set the distance map */ set_distance_ij (i, j, vector_norm (diff), map); } } /* print distance map for each frame */ print_distance_map (map); return LOAD_TRAJ_FRAME_SUCCESS; }
static void fine_fun(double const* x, double* s) { double coarse = 0.5; double fine = 0.2; double radius = vector_norm(x, 3); double d = fabs(radius - 0.5); s[0] = coarse * d + fine * (1 - d); }
static void dye_fun(double const coords[3], double* v) { double x[3]; double const l[3] = {.25, .5, 0}; double const r[3] = {.75, .5, 0}; double dir = 1; subtract_vectors(coords, l, x, 3); if (vector_norm(x, 3) > .25) { dir = -1; subtract_vectors(coords, r, x, 3); } if (vector_norm(x, 3) > .25) { v[0] = 0; return; } v[0] = 4 * dir * (.25 - vector_norm(x, 3)); }
int get_heading (data_t* data) { // Initialize heading value - it's an integer because it is // going to eventually be binned into North, South, East, or West vector north, east, mag_vector, accel_vector, from; int heading; // Initialize north and east vectors vector_init(&north, 0, 0, 0); vector_init(&east, 0, 0, 0); // Put magnetometer and accelerometer data into vector format vector_init(&mag_vector, data->Mx, data->My, data-> Mz); vector_init(&accel_vector, data->Ax, data->Ay, data-> Az); // Initialize "from" vector based on carrier board design vector_init(&from, 1, 0, 0); // Subtract calibration offset from magnetometer readings mag_vector.x -= ((float) MAG_MAX_X + MAG_MIN_X) / 2.0; mag_vector.y -= ((float) MAG_MAX_Y + MAG_MIN_Y) / 2.0; mag_vector.z -= ((float) MAG_MAX_Z + MAG_MIN_Z) / 2.0; // Compute East and North vector_cross(&mag_vector, &accel_vector, &east); vector_norm(&east); vector_cross(&accel_vector, &east, &north); vector_norm(&north); // Compute the heading and make sure it's positive heading = (int)(atan2(vector_dot(&east, &from), vector_dot(&north, &from))* 180 / PI); if (heading < 0) { heading += 360; } // Memory cleanup vector_destroy(&north); vector_destroy(&east); vector_destroy(&mag_vector); vector_destroy(&accel_vector); vector_destroy(&from); return heading; }
END_TEST START_TEST(test_vector_normalize) { u32 i, t; seed_rng(); for (t = 0; t < LINALG_NUM; t++) { u32 n = sizerand(MSIZE_MAX); double A[n]; for (i = 0; i < n; i++) A[i] = mrand; vector_normalize(n, A); double vnorm = vector_norm(n, A); fail_unless(fabs(vnorm - 1) < LINALG_TOL, "Norm differs from 1: %lf", vector_norm(n, A)); } }
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)); }
float distance1( const Point * A , const Point * B , const Point * C ) { Vector v1 , v2 , v3 , v4; vector_from_points( &v1 , A , B ); if ( 0.0f == vector_norm( &v1 , &v1 ) ) return -1.0f; vector_from_points( &v2 , A , C ); vector_mul( &v3 , &v1 , vector_dot( &v1 , &v2 ) ); vector_sub( &v4 , &v3 , &v2 ); return vector_length( &v4 ); }
quaternion ahrs_orientation_from_accel_mag(dataexchange_t *data) { //vectors for rotation matrix vector3d acc_v, mag_v; matrix3x3d rmat; quaternion q; acc_v.x = (*data).acc_x; acc_v.y = (*data).acc_y; acc_v.z = (*data).acc_z; mag_v.x = (*data).mag_x; mag_v.y = (*data).mag_y; mag_v.z = (*data).mag_z; hmc5883l_applyCalibration(&mag_v, calib_ptr); vector3d down = vector_inv(acc_v); vector3d east = vector_cross(down, mag_v); vector3d north = vector_cross(east, down); //normalize vectors vector_norm(&down); vector_norm(&east); vector_norm(&north); //vectors to matrix matrix_vector_to_row(&rmat, north, 1); matrix_vector_to_row(&rmat, east, 2); matrix_vector_to_row(&rmat, down, 3); //matrix to quaternion q = quaternion_from_matrix(&rmat); //normalize quaternion_norm(&q); (*data).qr = q; return q; }
void gameinfo_collision_ball_slime(GameInfo *gi, TEAM t) { Ball *b = gi->ball; Slime *s; if (t == BLUE ) { s = gi->slime_blue; } else { s = gi->slime_red; } Vector v_s = {b->vx, b->vy}; Vector v_a = {s->x - b->x, WIN_HEIGHT-b->y}; Vector v_b = {s->x - (b->x + b->vx), WIN_HEIGHT - (b->y + b->vy)}; double c = vector_inner_product (v_a, v_s) * vector_inner_product (v_b, v_s) ; double d = fabs(vector_outer_product (v_s, v_a))/ vector_norm(v_s); double r = SLIME_RADIUS + BALL_RADIUS; if ( d <= r && ( c<=0 || r > vector_norm (v_a) || r > vector_norm (v_b))) { double dx_0 = b->x - s->x; double dy_0 = b->y - WIN_HEIGHT; double dist_0 = sqrt(pow(dx_0, 2) + pow(dy_0, 2)); double angle = acos (dx_0/dist_0); double cos2x = cos(2*angle); double sin2x = sin(2*angle); double vx = - b->vx * cos2x + b->vy * sin2x; double vy = b->vx * sin2x + b->vy * cos2x; b->vx = vx; b->vy = vy; #if 0 double pe = GRAVITY * (WIN_HEIGHT - b->y ); double ke = 0.5 * (pow(b->vx, 2) + pow(b->vy, 2)); pe = GRAVITY * (WIN_HEIGHT - b->y ); ke = 0.5 * (pow(b->vx, 2) + pow(b->vy, 2)); g_print("POST x=%+.2f vx=%+.2f y=%+.2f vy=%+.2f pe=%+.4f ke=%+.4f pe+ke=%+.4f\n", b->x, b->vx, b->y, b->vy, pe, ke, pe+ke); #endif gi->ball_owner = t; gi->ball_count++; } }
/** Approximate chi square test * Scales least squares residuals by estimated variance, * compares against threshold. * * \param threshold The test threshold. * Value 5.5 recommended for float/fixed baseline residuals. * \param num_dds Length of residuals. * \param residuals Residual vector calculated by lesq_solution_float. * \param residual If not null, used to output double value of scaled residual. * * \return true if norm is below threshold. */ static bool chi_test(double threshold, u8 num_dds, const double *residuals, double *residual) { assert(num_dds < MAX_CHANNELS); double sigma = DEFAULT_PHASE_VAR_KF; double norm = vector_norm(num_dds, residuals) / sqrt(sigma); if (residual) { *residual = norm; } return norm < threshold; }
t_vec vector_cross_product(t_vec u, t_vec v) { t_vec result; double norm; result.x = v.z * u.y - v.y * u.z; result.y = v.x * u.z - v.z * u.x; result.z = v.y * u.x - v.x * u.y; norm = vector_norm(result); if (norm != 0) result = vector_scalar_mult(result, 1 / norm); return (result); }
void vector_normalize(d3_t *v) { double len; if (v != NULL) { len = vector_norm(v); v->x /= len; v->y /= len; v->z /= len; } }
void rWeaponRaybeam::animate(float spf) { triggereded = triggered; if (trigger) fire(); trigger = false; timeReloading -= spf; if (timeReloading < 0) { timeReloading = 0.0f; if (remainingAmmo == 0 && remainingClips != 0) { remainingAmmo = clipSize; if (remainingClips > 0) remainingClips--; } } timeFiring -= spf; if (timeFiring < 0) { timeFiring = 0.0f; return; } float* source = weaponPosef; // Vector that points to the Mountpoint relative to the eye. float* mpnt = &source[12]; //vector_print(mpnt); // Mountpoint-Forward Vector relative to the eye. float* source_8 = &source[8]; float mfwd[3]; vector_cpy(mfwd, source_8); //vector_print(mfwd); vector_norm(mfwd, mfwd); // Interpolate single points towards the "tip" of the ray. float offset = 80 * (1.0f - (timeFiring / 0.7)); float worldpos[] = {0, 0, 0}; vector_scale(worldpos, mfwd, -offset); vector_add(worldpos, worldpos, mpnt); //vector_print(worldpos); // Collide and Damage with single point on ray. { float radius = 0.5; int roles = 0; float damage = 25; int damaged = damageByParticle(worldpos, radius, roles, damage); // Stop ray on collision. if (damaged > 0) timeFiring = 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); }
libquat_err unit_vec(const vec_3d *a, vec_3d *n) { float norm; vector_norm(a, &norm); if (fabsf(norm) < LIBQUAT_EPSILON) return LIBQUAT_DIV_BY_ZERO; if (norm != 1.F) // perform division only if needed { n->x = a->x / norm; n->y = a->y / norm; n->z = a->z / norm; } return LIBQUAT_OK; }
ADPMode* translational_mode(ChemicalUnit* unit,int direction,sym_mat3<double> metrical_matrix) { vector<Atom*> atoms = unit->get_atoms(); vector<Atom*>::iterator atom; vec3<double> r(0,0,0); r[direction]=1; r=r/vector_norm(r,metrical_matrix); ADPMode* mode = new ADPMode(); for(atom=atoms.begin(); atom!=atoms.end(); atom++) mode->add_atom(*atom,r); return mode; }
/** Determine the azimuth and elevation of a point in WGS84 Earth Centered, * Earth Fixed (ECEF) Cartesian coordinates from a reference point given in * WGS84 ECEF coordinates. * * First the vector between the points is converted into the local North, East, * Down frame of the reference point. Then we can directly calculate the * azimuth and elevation. * * \param ecef Cartesian coordinates of the point, passed as [X, Y, Z], * all in meters. * \param ref_ecef Cartesian coordinates of the reference point from which the * azimuth and elevation is to be determined, passed as * [X, Y, Z], all in meters. * \param azimuth Pointer to where to store the calculated azimuth output. * \param elevation Pointer to where to store the calculated elevation output. */ void wgsecef2azel(const double ecef[3], const double ref_ecef[3], double* azimuth, double* elevation) { double ned[3]; /* Calculate the vector from the reference point in the local North, East, * Down frame of the reference point. */ wgsecef2ned_d(ecef, ref_ecef, ned); *azimuth = atan2(ned[1], ned[0]); /* atan2 returns angle in range [-pi, pi], usually azimuth is defined in the * range [0, 2pi]. */ if (*azimuth < 0) *azimuth += 2*M_PI; *elevation = asin(-ned[2]/vector_norm(3, ned)); }
ADPMode* rot_mode(ChemicalUnit* unit,vec3<double> axis , vec3<double> point_on_axis ,sym_mat3<double> metrical_matrix) { vector<Atom*> atoms = unit->get_atoms(); vector<Atom*>::iterator atom; axis = axis/vector_norm(axis, metrical_matrix.inverse()); ADPMode* mode = new ADPMode(); /* mat3<double> rotation(1/sqrt(3),-2/sqrt(3),0,2/sqrt(3),-1/sqrt(3),0,0,0,0); */ for(atom=atoms.begin(); atom!=atoms.end(); atom++) mode->add_atom(*atom,metrical_matrix*(axis.cross((*atom)->r - point_on_axis))/sqrt(metrical_matrix.determinant())); return mode; }
static void warp_fun(double const* coords, double* v) { double x[3]; double const mid[3] = {.5, .5, 0}; subtract_vectors(coords, mid, x, 3); double polar_a = atan2(x[1], x[0]); double polar_r = vector_norm(x, 3); double rot_a = 0; if (polar_r < .5) rot_a = the_rotation * (2 * (.5 - polar_r)); double dest_a = polar_a + rot_a; double dst[3]; dst[0] = cos(dest_a) * polar_r; dst[1] = sin(dest_a) * polar_r; dst[2] = 0; subtract_vectors(dst, x, v, 3); }
/** Checks pvt_iter residuals. * * \param n_used length of omp * \param omp residual vector calculated by pvt_solve * \param rx_state reference to pvt solver state allocated in calc_PVT * \param residual If not null, used to output double value of residual * * \return residual < PVT_RESIDUAL_THRESHOLD */ static bool residual_test(u8 n_used, double omp[n_used], const double rx_state[], double *residual) { /* Very liberal threshold. Typical range 20 - 120 */ #define PVT_RESIDUAL_THRESHOLD 3000 /* Need to add clock offset to observed-minus-predicted calculated by last * iteration of pvt_solve before computing residual. */ for (int i = 0; i < n_used; i++) { omp[i] -= rx_state[3]; } double norm = vector_norm(n_used, omp); if (residual) { *residual = norm; } return norm < PVT_RESIDUAL_THRESHOLD; }
/** 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; }