//Interpolate velocity from the MAC grid. Vec2f FluidSim::get_velocity(const Vec2f& position) { //Interpolate the velocity from the u and v grids float u_value = interpolate_value(position / dx - Vec2f(0, 0.5f), u); float v_value = interpolate_value(position / dx - Vec2f(0.5f, 0), v); return Vec2f(u_value, v_value); }
//8 particles per cell void FluidSim::set_liquid(float (*phi)(const Vec3f&)) { //surface.reset_phi(phi, dx, Vec3f(0.5f*dx,0.5f*dx,0.5f*dx), ni, nj, nk); //initialize particles int seed = 0; for(int k = 0; k < nk; ++k) for(int j = 0; j < nj; ++j) for(int i = 0; i < ni; ++i) { Vec3f pos(i*dx,j*dx,k*dx); float a = randhashf(seed++); float b = randhashf(seed++); float c = randhashf(seed++); pos += dx * Vec3f(a,b,c); if(phi(pos) <= -particle_radius) { float solid_phi = interpolate_value(pos/dx, nodal_solid_phi); if(solid_phi >= 0){ particles.push_back(pos); particlesVel.push_back(Vec3f(0,0,0)); particlesCell.push_back(Vec3f(floor(pos[0]/dx+0.5),floor(pos[1]/dx+0.5),floor(pos[2]/dx+0.5))); particlesMass.push_back(0.1); //Kg particlesDensity.push_back(0); particlesVolume.push_back(0); particlesDeformation.push_back(Eigen::Matrix3f::Identity()); //3x3 init with identity plasticDeformationGradient.push_back(Eigen::Matrix3f::Identity()); elasticDeformationGradient.push_back(Eigen::Matrix3f::Identity()); } } } ParticleToGrid(); ParticleVolumeDensity(); }
void compute_volume_fractions(const Array2f& levelset, Array2f& fractions, Vec2f fraction_origin, int subdivision) { //Assumes levelset and fractions have the same dx float sub_dx = 1.0f / subdivision; int sample_max = subdivision*subdivision; for(int j = 0; j < fractions.nj; ++j) { for(int i = 0; i < fractions.ni; ++i) { float start_x = fraction_origin[0] + (float)i; float start_y = fraction_origin[1] + (float)j; int incount = 0; for(int sub_j = 0; sub_j < subdivision; ++sub_j) { for(int sub_i = 0; sub_i < subdivision; ++sub_i) { float x_pos = start_x + (sub_i+0.5f)*sub_dx; float y_pos = start_y + (sub_j+0.5f)*sub_dx; float phi_val = interpolate_value(Vec2f(x_pos,y_pos), levelset); if(phi_val < 0) ++incount; } } fractions(i,j) = (float)incount / (float)sample_max; } } }
//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; } } }
void FluidSim::set_liquid(float (*phi)(const Vec3f&)) { //surface.reset_phi(phi, dx, Vec3f(0.5f*dx,0.5f*dx,0.5f*dx), ni, nj, nk); //initialize particles int seed = 0; for(int k = 0; k < nk; ++k) for(int j = 0; j < nj; ++j) for(int i = 0; i < ni; ++i) { Vec3f pos(i*dx,j*dx,k*dx); float a = randhashf(seed++); float b = randhashf(seed++); float c = randhashf(seed++); pos += dx * Vec3f(a,b,c); if(phi(pos) <= -particle_radius) { float solid_phi = interpolate_value(pos/dx, nodal_solid_phi); if(solid_phi >= 0) particles.push_back(pos); } } }
void FluidSim::advect_particles(float dt) { for(unsigned int p = 0; p < particles.size(); ++p) { particles[p] = trace_rk2(particles[p], dt); //check boundaries and project exterior particles back in float phi_val = interpolate_value(particles[p]/dx, nodal_solid_phi); if(phi_val < 0) { Vec3f grad; interpolate_gradient(grad, particles[p]/dx, nodal_solid_phi); if(mag(grad) > 0) normalize(grad); particles[p] -= phi_val * grad; } } }
float MarchingTriangles::eval( float i, float j ) { // int p, q; // double f, g; // // get_barycentric(i+0.5f, p, f, 1, phi.ni); // get_barycentric(j+0.5f, q, g, 1, phi.nj); // // double wx0, wx1, wx2, wy0, wy1, wy2; // quadratic_bspline_weights(f, wx0, wx1, wx2); // quadratic_bspline_weights(g, wy0, wy1, wy2); // // return wx0*( wy0*phi(p-1,q-1) + wy1*phi(p-1,q) + wy2*phi(p-1,q+1) ) // +wx1*( wy0*phi(p,q-1) + wy1*phi(p,q) + wy2*phi(p,q+1) ) // +wx2*( wy0*phi(p+1,q-1) + wy1*phi(p+1,q) + wy2*phi(p+1,q+1) ); return interpolate_value( Vec2f(i,j), phi ); }
//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) { particles[p] = trace_rk2(particles[p], dt); //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; } } }
float interpolate_phi(const Vec2f& point, const Array2f& grid, const Vec2f& origin, const float dx) { float inv_dx = 1/dx; Vec2f temp = (point-origin)*inv_dx; return interpolate_value(temp, grid); }
static void generate_special_channels(const stp_vars_t *v) { stpi_channel_group_t *cg = get_channel_group(v); int i, j; const unsigned short *input_cache = NULL; const unsigned short *output_cache = NULL; const unsigned short *input; unsigned short *output; int offset; int outbytes; if (!cg) return; input = cg->input_data; output = cg->multi_tmp; offset = (cg->black_channel >= 0 ? 0 : -1); outbytes = cg->aux_output_channels * sizeof(unsigned short); for (i = 0; i < cg->width; input += cg->input_channels, output += cg->aux_output_channels, i++) { if (input_cache && short_eq(input_cache, input, cg->input_channels)) { memcpy(output, output_cache, outbytes); } else { int c = input[STP_ECOLOR_C + offset]; int m = input[STP_ECOLOR_M + offset]; int y = input[STP_ECOLOR_Y + offset]; int min = FMIN(c, FMIN(m, y)); int max = FMAX(c, FMAX(m, y)); if (max > min) /* Otherwise it's gray, and we don't care */ { double hue; /* * We're only interested in converting color components * to special inks. We want to compute the hue and * luminosity to determine what we want to convert. * Since we're eliminating all grayscale component, the * computations become simpler. */ c -= min; m -= min; y -= min; max -= min; if (offset == 0) output[STP_ECOLOR_K] = input[STP_ECOLOR_K]; hue = compute_hue(c, m, y, max); for (j = 1; j < cg->aux_output_channels - offset; j++) { stpi_channel_t *ch = &(cg->c[j]); if (ch->hue_map) output[j + offset] = max * interpolate_value(ch->hue_map, hue * ch->h_count / 6.0); else output[j + offset] = 0; } output[STP_ECOLOR_C + offset] += min; output[STP_ECOLOR_M + offset] += min; output[STP_ECOLOR_Y + offset] += min; } else { for (j = 0; j < 4 + offset; j++) output[j] = input[j]; for (j = 4 + offset; j < cg->aux_output_channels; j++) output[j] = 0; } } input_cache = input; output_cache = output; } }
/** * Compute desired velocity to follow the desired path from the current location. * @param[in] dT the time since last evaluation * @param[in] pathDesired the desired path to follow * @param[out] progress the current progress information along that path * @returns 0 if successful, <0 if an error occurred * * The calculated velocity to attempt is stored in @ref VelocityDesired */ int32_t vtol_follower_control_path(const float dT, const PathDesiredData *pathDesired, struct path_status *progress) { PositionActualData positionActual; PositionActualGet(&positionActual); VelocityActualData velocityActual; VelocityActualGet(&velocityActual); PathStatusData pathStatus; PathStatusGet(&pathStatus); const float cur_pos_ned[3] = { positionActual.North + velocityActual.North * guidanceSettings.PositionFeedforward, positionActual.East + velocityActual.East * guidanceSettings.PositionFeedforward, positionActual.Down }; path_progress(pathDesired, cur_pos_ned, progress); // Check if we have already completed this leg bool current_leg_completed = (pathStatus.Status == PATHSTATUS_STATUS_COMPLETED) && (pathStatus.Waypoint == pathDesired->Waypoint); pathStatus.fractional_progress = progress->fractional_progress; pathStatus.error = progress->error; pathStatus.Waypoint = pathDesired->Waypoint; // Figure out how low (high) we should be and the error const float altitudeSetpoint = interpolate_value(progress->fractional_progress, pathDesired->Start[2], pathDesired->End[2]); const float downError = altitudeSetpoint - positionActual.Down; // If leg is completed signal this if (current_leg_completed || pathStatus.fractional_progress > 1.0f) { const bool criterion_altitude = (downError > -guidanceSettings.WaypointAltitudeTol) || (!guidanceSettings.ThrottleControl); // Once we complete leg and hit altitude criterion signal this // waypoint is done. Or if we're not controlling throttle, // ignore height for completion. // Waypoint heights are thus treated as crossing restrictions- // cross this point at or above... if (criterion_altitude || current_leg_completed) { pathStatus.Status = PATHSTATUS_STATUS_COMPLETED; PathStatusSet(&pathStatus); } else { pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS; PathStatusSet(&pathStatus); } // Wait here for new path segment return vtol_follower_control_impl(dT, pathDesired->End, 0, false); } // Interpolate desired velocity and altitude along the path float groundspeed = interpolate_value(progress->fractional_progress, pathDesired->StartingVelocity, pathDesired->EndingVelocity); float error_speed = cubic_deadband(progress->error, guidanceSettings.PathDeadbandWidth, guidanceSettings.PathDeadbandCenterGain, vtol_path_m, vtol_path_r) * guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP]; /* Sum the desired path movement vector with the correction vector */ float commands_ned[3]; commands_ned[0] = progress->path_direction[0] * groundspeed + progress->correction_direction[0] * error_speed; commands_ned[1] = progress->path_direction[1] * groundspeed + progress->correction_direction[1] * error_speed; /* Limit the total velocity based on the configured value. */ vector2_clip(commands_ned, guidanceSettings.HorizontalVelMax); commands_ned[2] = pid_apply_antiwindup(&vtol_pids[DOWN_POSITION], downError, -guidanceSettings.VerticalVelMax, guidanceSettings.VerticalVelMax, dT); VelocityDesiredData velocityDesired; VelocityDesiredGet(&velocityDesired); velocityDesired.North = commands_ned[0]; velocityDesired.East = commands_ned[1]; velocityDesired.Down = commands_ned[2]; VelocityDesiredSet(&velocityDesired); pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS; PathStatusSet(&pathStatus); return 0; }