//Basic first order semi-Lagrangian advection of velocities
void FluidSim::advect(float dt) {
temp_u.assign(0);
temp_v.assign(0);
temp_w.assign(0);
//semi-Lagrangian advection on u-component of velocity
for(int k = 0; k < nk; ++k) for(int j = 0; j < nj; ++j) for(int i = 0; i < ni+1; ++i) {
Vec3f pos(i*dx, (j+0.5f)*dx, (k+0.5f)*dx);
pos = trace_rk2(pos, -dt);
temp_u(i,j,k) = get_velocity(pos)[0];
}
//semi-Lagrangian advection on v-component of velocity
for(int k = 0; k < nk; ++k) for(int j = 0; j < nj+1; ++j) for(int i = 0; i < ni; ++i) {
Vec3f pos((i+0.5f)*dx, j*dx, (k+0.5f)*dx);
pos = trace_rk2(pos, -dt);
temp_v(i,j,k) = get_velocity(pos)[1];
}
//semi-Lagrangian advection on w-component of velocity
for(int k = 0; k < nk+1; ++k) for(int j = 0; j < nj; ++j) for(int i = 0; i < ni; ++i) {
Vec3f pos((i+0.5f)*dx, (j+0.5f)*dx, k*dx);
pos = trace_rk2(pos, -dt);
temp_w(i,j,k) = get_velocity(pos)[2];
}
//move update velocities into u/v vectors
u = temp_u;
v = temp_v;
w = temp_w;
}
//Perform 2nd order Runge Kutta to move the particles in the fluid
void FluidSim::advect_particles(float dt) {
for(unsigned int p = 0; p < particles.size(); ++p) {
Vec2f before = particles[p];
Vec2f start_velocity = get_velocity(before);
Vec2f midpoint = before + 0.5f*dt*start_velocity;
Vec2f mid_velocity = get_velocity(midpoint);
particles[p] += dt*mid_velocity;
Vec2f after = particles[p];
if(dist(before,after) > 3*dx) {
std::cout << "Before: " << before << " " << "After: " << after << std::endl;
std::cout << "Mid point: " << midpoint << std::endl;
std::cout << "Start velocity: " << start_velocity << " Time step: " << dt << std::endl;
std::cout << "Mid velocity: " << mid_velocity << std::endl;
}
//Particles can still occasionally leave the domain due to truncation errors,
//interpolation error, or large timesteps, so we project them back in for good measure.
//Try commenting this section out to see the degree of accumulated error.
float phi_value = interpolate_value(particles[p]/dx, nodal_solid_phi);
if(phi_value < 0) {
Vec2f normal;
interpolate_gradient(normal, particles[p]/dx, nodal_solid_phi);
normalize(normal);
particles[p] -= phi_value*normal;
}
}
}
//Apply RK2 to advect a point in the domain.
Vec3f FluidSim::trace_rk2(const Vec3f& position, float dt) {
Vec3f input = position;
Vec3f velocity = get_velocity(input);
velocity = get_velocity(input + 0.5f*dt*velocity);
input += dt*velocity;
return input;
}
void PlayerCar::left()
{
glm::vec3 prev_direction = direction_;
direction_ += (0.001f * 10 * glm::normalize(glm::cross(direction_, up_direction_)));
direction_ = glm::normalize(direction_);
rotation_.Rotate(up_direction_, -1 * acosf(glm::dot(prev_direction, direction_)) * 360.0f / (2.0f*3.1415f));
set_velocity(glm::normalize(get_velocity() + direction_ * 0.001f) * glm::length(get_velocity()));
}
/* See
http://www.movable-type.co.uk/scripts/latlong.html
for formulas */
double GPSFilter::get_bearing() {
double lat, lon, delta_lat, delta_lon, x, y;
get_lat_long(&lat, &lon);
get_velocity(&delta_lat, &delta_lon);
/* Convert to radians */
double to_radians = PI / 180.0;
lat *= to_radians;
lon *= to_radians;
delta_lat *= to_radians;
delta_lon *= to_radians;
/* Do math */
double lat1 = lat - delta_lat;
y = sin(delta_lon) * cos(lat);
x = cos(lat1) * sin(lat) - sin(lat1) * cos(lat) * cos(delta_lon);
double bearing = atan2(y, x);
/* Convert to degrees */
bearing = bearing / to_radians;
while (bearing >= 360.0) {
bearing -= 360.0;
}
while (bearing < 0.0) {
bearing += 360.0;
}
return bearing;
}
int main() {
int inst = 0;
ViviController *viviController = new ViviController(inst);
NoteBeginning begin;
begin.physical.string_number = 0;
begin.physical.dynamic = 0;
begin.physical.finger_position = 0.19;
begin.physical.bow_force = 1.5;
begin.physical.bow_bridge_distance = get_distance(inst, 0);
begin.physical.bow_velocity = get_velocity(inst, 0);
begin.midi_target = 59;
NoteEnding end;
double K = 1.05;
viviController->filesNew("test-pitch");
viviController->comment("pitch test, 0 0");
viviController->load_ears_training(begin.physical.string_number,
"final/violin/0.mpl");
viviController->set_stable_K(0, 0, 0, K);
viviController->note(begin, PLAY, end);
delete viviController;
}
//For extrapolated points, replace the normal component
//of velocity with the object velocity (in this case zero).
void FluidSim::constrain_velocity() {
temp_u = u;
temp_v = v;
//(At lower grid resolutions, the normal estimate from the signed
//distance function is poor, so it doesn't work quite as well.
//An exact normal would do better.)
//constrain u
for(int j = 0; j < u.nj; ++j) for(int i = 0; i < u.ni; ++i) {
if(u_weights(i,j) == 0) {
//apply constraint
Vec2f pos(i*dx, (j+0.5f)*dx);
Vec2f vel = get_velocity(pos);
Vec2f normal(0,0);
interpolate_gradient(normal, pos/dx, nodal_solid_phi);
normalize(normal);
float perp_component = dot(vel, normal);
vel -= perp_component*normal;
temp_u(i,j) = vel[0];
}
}
//constrain v
for(int j = 0; j < v.nj; ++j) for(int i = 0; i < v.ni; ++i) {
if(v_weights(i,j) == 0) {
//apply constraint
Vec2f pos((i+0.5f)*dx, j*dx);
Vec2f vel = get_velocity(pos);
Vec2f normal(0,0);
interpolate_gradient(normal, pos/dx, nodal_solid_phi);
normalize(normal);
float perp_component = dot(vel, normal);
vel -= perp_component*normal;
temp_v(i,j) = vel[1];
}
}
//update
u = temp_u;
v = temp_v;
}
//Basic first order semi-Lagrangian advection of velocities
void FluidSim::advect(float dt) {
//semi-Lagrangian advection on u-component of velocity
for(int j = 0; j < nj; ++j) for(int i = 0; i < ni+1; ++i) {
Vec2f pos(i*dx, (j+0.5f)*dx);
pos = trace_rk2(pos, -dt);
temp_u(i,j) = get_velocity(pos)[0];
}
//semi-Lagrangian advection on v-component of velocity
for(int j = 0; j < nj+1; ++j) for(int i = 0; i < ni; ++i) {
Vec2f pos((i+0.5f)*dx, j*dx);
pos = trace_rk2(pos, -dt);
temp_v(i,j) = get_velocity(pos)[1];
}
//move update velocities into u/v vectors
u = temp_u;
v = temp_v;
}
void trajectory_parabola::set_distance(int dist)
{
_dist = std::abs((float)dist);
_v = get_velocity();
_initialized = true;
_max_point = get_y_point(_dist/2);
_reducefactor = _max_point/64;
if (_reducefactor <= 0) {
_reducefactor = 0.4;
}
//std::cout << ">>>> DEBUG #2 _reducefactor: " << _reducefactor << ", _max_point: " << _max_point << " speed: " << _v << " <<<<<" << std::endl;
}
void meteorite_t::on_collide(physic_element_if *other)
{
if (!is_descedant)
{
auto parent = get_parent();
if (parent)
{
auto descedant_1 = parent->add_child(new meteorite_t(_position, _size / 2, get_world()));
auto descedant_2 = parent->add_child(new meteorite_t(_position, _size / 2, get_world()));
descedant_1->is_descedant = true;
descedant_2->is_descedant = true;
vec2 vel_1 = vec2(mat3::rotation(30 * M_PI / 180) * (vec3(get_velocity(), 0)));
vec2 vel_2 = vec2(mat3::rotation(-30 * M_PI / 180) * (vec3(get_velocity(), 0)));
descedant_1->set_velocity(vel_1);
descedant_2->set_velocity(vel_2);
}
}
this->destroy();
}
void test_physics(){
printf("testing physics\n");
world_handle hello_world = new_world();
//tests movement, set/get velocity, set/get location(position)
po_vector center;
center.x = 1.0;
center.y = 1.0;
po_circle circle2 = create_circ(center, 1.0);
po_geometry geo_circle2 = create_geom_circ(circle2, 1.0);
po_handle circle02 = add_object (hello_world, &geo_circle2, 20.0, 1.0, 0.0);
po_vector location = get_position(circle02);
center.x = 0.0;
center.y = 0.0;
po_circle circle1 = create_circ(center, 5.0);
po_geometry geo_circle1 = create_geom_circ(circle1, 1.0);
po_handle circle01 = add_object (hello_world, &geo_circle1, 50.0, 0.0, 0.0);
set_velocity(circle02,-1.0,0.0);
po_vector velocity = get_velocity(circle02);
update(hello_world,3);
po_vector pos02 = get_position(circle02);
po_vector pos01 = get_position(circle01);
assert(pos02.x == 17.0);
assert(pos02.y == 1.0);
assert(pos01.x == 50.0);
assert(pos01.y == 0.0);
set_location(circle01, 0.0, 0.0);
set_velocity(circle01, -7.76,1.7);
set_location(circle02, 20.0, 0.0);
set_velocity(circle02, -9.0,2.0);
update(hello_world,1);
po_vector pos002 = get_position(circle02);
po_vector pos001 = get_position(circle01);
assert( pos002.x == 11.0);
assert( pos002.y == 2.0);
assert(fabs(pos001.x - -7.76) <= EPSILON);
assert(fabs(pos001.y - 1.7)<= EPSILON);
}
void test_variable_timestep() {
KalmanFilter f = alloc_filter_velocity2d(1.0);
/* Move at a constant speed but taking slower and slower readings */
int east_distance = 0;
for (int i = 0; i < 20; ++i) {
east_distance += i;
update_velocity2d(f, 0.0, east_distance * 0.0001, i);
}
double dlat, dlon;
get_velocity(f, &dlat, &dlon);
assert(abs(dlat) < 0.000001);
assert(abs(dlon - 0.0001) < 0.000001);
free_filter(f);
}
void test_bearing_north() {
KalmanFilter f = alloc_filter_velocity2d(1.0);
for (int i = 0; i < 100; ++i) {
update_velocity2d(f, i * 0.0001, 0.0, 1.0);
}
double bearing = get_bearing(f);
assert(abs(bearing - 0.0) < 0.01);
/* Velocity should be 0.0001 x units per timestep */
double dlat, dlon;
get_velocity(f, &dlat, &dlon);
assert(abs(dlat - 0.0001) < 0.00001);
assert(abs(dlon) < 0.00001);
free_filter(f);
}
void
nav_fix (void)
{
int n;
double tr_time[N_channels + 1], ipart, clock_error;
static double t_cor[N_channels + 1];
unsigned int i;
struct measurement *meas = measurements.measurement;
int tr_prn[N_channels + 1];
ecef rp_ecef;
eceft dm_gps_sat[N_channels + 1], dp_gps_sat[N_channels + 1];
int year, month, day, hour, minute, flag;
double second;
n = 1;
i = 0;
/* For every measurement we receive (they're null terminated) */
while ((meas->transmit_time_offset != 0)
&& (meas->transmit_time_offset != 0)
&& i < N_channels) {
/* Extract the PRN */
int prn = meas->doppler_prn & 0x1f;
/* Make sure we have a good healthy ephemeris for this PRN */
if ((gps_eph[prn].valid == 1) && (gps_eph[prn].health == 0)) {
meas_dop[n] =
(double) (meas->doppler_prn) / 1048576.0 / TIC_dt * (double) (nav_tic);
/**
* (1024 phases / half chip) * (2046 half chips / ms)
* * (1000 ms / sec) = 2096104000 phases / second
**/
tr_time[n] = (double) measurements.bit_of_week / 50.0
+ (double) meas->transmit_time_offset / 2095104000.0;
tr_prn[n] = prn;
n++;
}
/* Next measurement */
meas = &measurements.measurement[++i];
}
n_track = n - 1;
TIC_dt = measurements.i_TIC_dt * 175.0e-9; /*each clock count is 175 ns */
/* use basic TIC interval for ICP */
/* if (ICP_CTL==1)TIC_dt=TIC_dt/float(nav_tic); */
if (out_debug)
fprintf (debug, "n_track= %d\n", n_track);
for (i = 1; i <= (unsigned) n_track; i++) {
track_sat[i] = satpos_ephemeris (tr_time[i], gps_eph + tr_prn[i]);
/* process Carrier Tracking Loop or Integrated Carrier Phase */
if (ICP_CTL == 0) { /* satellite velocity */
dm_gps_sat[i] = satpos_ephemeris (tr_time[i] - TIC_dt / 2.0,
gps_eph + tr_prn[i]); /* for CTL */
dp_gps_sat[i] = satpos_ephemeris (tr_time[i] + TIC_dt / 2.0,
gps_eph + tr_prn[i]);
d_sat[i].x =
(dp_gps_sat[i].x - dm_gps_sat[i].x) / TIC_dt -
track_sat[i].y * omegae;
d_sat[i].y =
(dp_gps_sat[i].y - dm_gps_sat[i].y) / TIC_dt +
track_sat[i].x * omegae;
d_sat[i].z = (dp_gps_sat[i].z - dm_gps_sat[i].z) / TIC_dt;
}
else {
dm_gps_sat[i] = satpos_ephemeris (tr_time[i] - TIC_dt / (float) (nav_tic), gps_eph + tr_prn[i]); /* for ICP */
dp_gps_sat[i] = track_sat[i];
d_sat[i].x =
(dp_gps_sat[i].x - dm_gps_sat[i].x) / TIC_dt * (float) (nav_tic) -
track_sat[i].y * omegae;
d_sat[i].y =
(dp_gps_sat[i].y - dm_gps_sat[i].y) / TIC_dt * (float) (nav_tic) +
track_sat[i].x * omegae;
d_sat[i].z =
(dp_gps_sat[i].z - dm_gps_sat[i].z) / TIC_dt * (float) (nav_tic);
}
t_cor[i] = track_sat[i].tb -
tropo_iono (tr_prn[i], track_sat[i].az, track_sat[i].el, tr_time[i]);
dt[i] = m_time[1] - (tr_time[i] - t_cor[i]);
}
if (n_track >= 4) {
rpvt = pos_vel_time (n_track);
cbias = rpvt.dt;
clock_error = rpvt.df;
m_time[1] = m_time[1] - cbias;
rp_ecef.x = rpvt.x;
rp_ecef.y = rpvt.y;
rp_ecef.z = rpvt.z;
rp_llh = ecef_to_llh (rp_ecef);
/* a position reasonableness check */
if (rp_llh.hae > -200000.0 && rp_llh.hae < 1800000) {
/**
* Translate velocity into North, East, Up coordinates
**/
get_velocity ();
/* a velocity reasonableness check */
if (sqrt (SQ (receiver.vel.north) +
SQ (receiver.vel.east) +
SQ (receiver.vel.up)) < 514.0) {
if (fabs (clock_error) < 500.0)
clock_offset = clock_error;
/* if we have a good fix we're navigating */
status = navigating;
if (align_t == 1) {
long TIC_counter;
delta_m_time = modf (m_time[1], &ipart);
if (nav_up < 1.0) {
delta_m_time = modf (delta_m_time / nav_up, &ipart);
if (delta_m_time > 0.5)
m_error = (delta_m_time - 1.0) * nav_up;
else
m_error = delta_m_time * nav_up;
}
else {
if (delta_m_time > 0.5)
m_error = (delta_m_time - 1.0) / nav_up;
else
m_error = delta_m_time / nav_up;
}
TIC_counter =
(TIC_ref - m_error * TIC_ref / 10) * (1.0 -
clock_offset * 1.0e-6);
set_TIC (TIC_counter);
}
rec_pos_llh.lon = rp_llh.lon;
rec_pos_llh.lat = rp_llh.lat;
rec_pos_llh.hae = rp_llh.hae;
current_loc.lon = rp_llh.lon;
current_loc.lat = rp_llh.lat;
current_loc.hae = rp_llh.hae;
rec_pos_xyz.x = rp_ecef.x;
rec_pos_xyz.y = rp_ecef.y;
rec_pos_xyz.z = rp_ecef.z;
/**
* Calculate DOPS
**/
dops (n_track);
if (out_pos == 1)
fprintf (output,
"%20.10f, %f, %f, %f,",
m_time[1], rec_pos_llh.lat * r_to_d,
rec_pos_llh.lon * r_to_d, rec_pos_llh.hae);
if (out_vel == 1)
fprintf (output, " %f, %f, %f,",
receiver.vel.north, receiver.vel.east, receiver.vel.up);
if (out_time == 1)
fprintf (output, " %f,", clock_offset);
if (out_pos || out_vel || out_time)
fprintf (output, " %f, %f, %f\n", hdop, vdop, tdop);
/**
* Since we have a valid position/velocity narrow the
* doppler search window to +-5 doppler bins
**/
search_max_f = 5;
m_time[0] = m_time[1];
}
}
}
else {
/* less than 4 sats */
m_time[1] = m_time[1] + TIC_dt * (1.0 + clock_offset / 1.e6);
rp_ecef.x = 0.0;
rp_ecef.y = 0.0;
rp_ecef.z = 0.0;
rpvt.xv = 0.0;
rpvt.yv = 0.0;
rpvt.zv = 0.0;
}
if (out_kalman == 1) { /* Kalman filter output */
fprintf (kalm,
"time %20.10f, rpx %15.10f, rpy %15.10f, rpz %15.10f, ",
m_time[1], rp_ecef.x, rp_ecef.y, rp_ecef.z);
fprintf (kalm, "rvx %15.10f, rvy %15.10f, rvz %15.10f, Nsats %d\n",
rpvt.xv, rpvt.yv, rpvt.zv, n_track);
}
if (out_rinex == 1) { /* RINEX OBSERVATION EPOCH/SAT */
/* Calculate the calendar GPS time corresponding to the current GPS time
adding the leapseconds as these are automatically taken out */
gps2utc(m_time[1]+dtls, 1024+gps_eph[tr_prn[1]].week, &year, &month, &day, &hour, &minute, &second);
/* Check if this is the first observation, if so, write the
RINEX header */
if(write_rinex_obs_head)
{
write_rinex_obs_head = 0;
rinex_head_obs(year+2000, month, day, hour, minute, second);
}
/* Follows the RINEX-2 Format */
flag = 0;
fprintf(rinex_obs," %02d %02d %02d %02d %02d%11.7f %1d%3d", year, month, day, hour, minute, second, flag, n_track);
for (i = 1; i <= (unsigned) n_track; i++)
{
/* Print each satellite in view */
fprintf(rinex_obs,"G%2d", tr_prn[i]);
}
fprintf(rinex_obs,"\n");
}
for (i = 1; i <= (unsigned) n_track; i++) {
satellite[tr_prn[i]].Pr = (m_time[1] - tr_time[i]) * c; /*(m_time[1] - (tr_time[i] - t_cor[i])) * c;*/
satellite[tr_prn[i]].dPr = meas_dop[i] * lambda;
if (out_kalman == 1) { /* Kalman filter output */
fprintf (kalm,
" PRN %2d, px %20.10f, py %20.10f, pz %20.10f, ",
tr_prn[i], track_sat[i].x, track_sat[i].y, track_sat[i].z);
fprintf (kalm, " vx %16.10f, vy %16.10f, vz %16.10f, ",
d_sat[i].x, d_sat[i].y, d_sat[i].z);
fprintf (kalm, " Pr %20.10f, dPr %16.10f\n",
satellite[tr_prn[i]].Pr + t_cor[i] * c, satellite[tr_prn[i]].dPr);
}
if(out_rinex == 1) { /* RINEX data output */
/* Currently we only are only saving the pseudorange and Doppler. The LLI and signal
strength indicators need to be implemented as well and are currently left blank */
fprintf(rinex_obs, "%14.3f %14.3f\n", satellite[tr_prn[i]].Pr, meas_dop[i]);
}
}
}
/*!
\brief Runge-Kutta45 (inner steps)
\param region pointer to current 3D region
\param velocity_field pointer to array of 3 3D raster maps (velocity field)
\param point pointer to array of geographic coordinates of starting point
\param[out] next_point pointer to array of geographic coordinates of integrated point
\param delta_t integration time step
\param error pointer to array of error values
\return 0 success
\return -1 out of region or null values
*/
static int rk45_next(RASTER3D_Region * region, struct Gradient_info *gradient_info,
const double *point, double *next_point,
const double delta_t, double *velocity, double *error)
{
double tmp1[6][3]; /* 3 is 3 dimensions, 6 is the number of k's */
double tmp_point[3];
double vel_x, vel_y, vel_z;
double sum_tmp;
int i, j, k;
double vel_sq;
if (get_velocity(region, gradient_info, point[0], point[1], point[2],
&vel_x, &vel_y, &vel_z) < 0)
return -1;
tmp1[0][0] = vel_x;
tmp1[0][1] = vel_y;
tmp1[0][2] = vel_z;
/* compute k's */
for (i = 1; i < 6; i++) {
for (j = 0; j < 3; j++) { /* for each coordinate */
sum_tmp = 0;
for (k = 0; k < i; k++) { /* k1; k1, k2; ... */
sum_tmp += B[i - 1][k] * tmp1[k][j];
}
tmp_point[j] = point[j] + delta_t * sum_tmp;
}
if (get_velocity
(region, gradient_info, tmp_point[0], tmp_point[1], tmp_point[2],
&vel_x, &vel_y, &vel_z) < 0)
return -1;
tmp1[i][0] = vel_x;
tmp1[i][1] = vel_y;
tmp1[i][2] = vel_z;
}
vel_sq = 0;
/* compute next point */
for (j = 0; j < 3; j++) {
sum_tmp = 0;
for (i = 0; i < 6; i++) {
sum_tmp += C[i] * tmp1[i][j];
}
next_point[j] = point[j] + delta_t * sum_tmp;
vel_sq += sum_tmp * sum_tmp;
}
*velocity = sqrt(vel_sq);
if (!Rast3d_is_valid_location
(region, next_point[1], next_point[0], next_point[2]))
return -1;
/* compute error vector */
for (j = 0; j < 3; j++) {
sum_tmp = 0;
for (i = 0; i < 6; i++) {
sum_tmp += DC[i] * tmp1[i][j];
}
error[j] = delta_t * sum_tmp;
}
return 0;
}
bool is_moving() const { return get_velocity() != Vector(0, 0); }
double GPSFilter::get_mph() {
double lat, lon, delta_lat, delta_lon;
get_lat_long(&lat, &lon);
get_velocity(&delta_lat, &delta_lon);
return calculate_mph(lat, lon, delta_lat, delta_lon);
}
void AngularVelocity::show(std::ostream &out) const {
out << "Angular velocity " << get_velocity() << std::endl;
}
//Apply RK2 to advect a point in the domain.
Vec2f FluidSim::trace_rk2(const Vec2f& position, float dt) {
Vec2f velocity = get_velocity(position);
velocity = get_velocity(position + 0.5f*dt*velocity);
return position + dt*velocity;
}
