//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;
}
