Beispiel #1
0
int main() {
    const uint32_t samples_per_pixel = 16;
    const uint32_t width = 1280;
    const uint32_t height = 720;

    const vec3 cam_pos(0, 0, 25);
    const vec3 look_at(0, 0, -1);
    const vec3 up(0, 1, 0);
    camera cam(cam_pos, look_at, up, 90, (float)width / height);
    image img(width, height);

    sphere s(vec3(0, 0, -0.5), 1);
    vec3 light_dir = normalize(vec3(-1, -1, -1));
    vec3 ambient_color = vec3(1.0, 0, 0);

    vec3 *pixels = img.pixels;
    for (uint32_t y = 0; y < height; ++y) {
        for (uint32_t x = 0; x < width; ++x) {
            
            vec3 color(0, 0, 0);
            for (uint32_t sample = 0; sample < samples_per_pixel; ++sample) {
                const ray r = make_ray(
                    cam, 
                    (x + rand_float(generator)) / width, 
                    (height - (y + rand_float(generator))) / height
                );

                vec3 hit;
                vec3 normal;
                if (intersect(s, r, &hit, &normal)) {
                    vec3 dir_light_color = fmaxf(0, dot(normal, -light_dir)) * vec3(1.0, 1.0, 1.0);
                    color += saturate(ambient_color + dir_light_color);
                }
            }

            color /= samples_per_pixel;
            *pixels++ = color;
        }
    }

    save_as_png(img, "test.png");

    return 0;
}
Beispiel #2
0
/**
 * Determines the worst resulting aspect ratio of a set of rectangles laid out in a larger one.
 *
 * @param sum       The sum of all areas in the list.
 * @param min_dim   The value of the outer rectangle's minimum dimension.
 * @param areas     The areas to be laid out.
 * @param num_rects The number of rectangles in the list.
 */
float worst(float sum, float min_dim, float *areas, size_t num_rects) {
    float sum_sq = sum * sum;
    float min_dim_sq = min_dim * min_dim;

    float min_area = areas[0];
    float max_area = areas[0];
    size_t i;
    
    /* Determine the rectangles with the largest and smallest areas */
    for(i = 1; i < num_rects; ++i) {
        if(areas[i] < min_area) {
            min_area = areas[i];
        } else if(areas[i] > max_area) {
            max_area = areas[i];
        }
    }

    return fmaxf(min_dim_sq * max_area / sum_sq, sum_sq / (min_dim_sq * min_area));
}
Beispiel #3
0
/* shoot on circle */
uint8_t dc_circle(float interval, float start) {
  dc_autoshoot = DC_AUTOSHOOT_CIRCLE;
  dc_gps_count = 0;
  dc_circle_interval = fmodf(fmaxf(interval, 1.), 360.);

  if(start == DC_IGNORE) {
    start = DegOfRad(estimator_psi);
  }

  dc_circle_start_angle = fmodf(start, 360.);
  if (start < 0.)
    start += 360.;
  //dc_circle_last_block = floorf(dc_circle_start_angle/dc_circle_interval);
  dc_circle_last_block = 0;
  dc_circle_max_blocks = floorf(360./dc_circle_interval);
  dc_probing = TRUE;
  dc_info();
  return 0;
}
Beispiel #4
0
void Camera::setCameraOrbitLocation(float yawDegrees, float pitchDegrees, float radius)
{
    m_cameraOrbitYawDegrees= wrap_degrees(yawDegrees);
    m_cameraOrbitPitchDegrees= clampf(pitchDegrees, 0.f, 60.f);
    m_cameraOrbitRadius= fmaxf(radius, k_camera_min_zoom);

    const float yawRadians= degrees_to_radians(m_cameraOrbitYawDegrees);
    const float pitchRadians= degrees_to_radians(m_cameraOrbitPitchDegrees);
    const float xzRadiusAtPitch= m_cameraOrbitRadius*cosf(pitchRadians);

    m_cameraPosition= 
        glm::vec3(
            xzRadiusAtPitch*sinf(yawRadians),
            m_cameraOrbitRadius*sinf(pitchRadians),
            xzRadiusAtPitch*cosf(yawRadians))
        + glm::vec3(0.f, k_camera_y_offset, 0.f);

    publishCameraViewMatrix();
}
Beispiel #5
0
void graphModel::setRange( float _min, float _max )
{
	if( _min != m_minValue || _max != m_maxValue )
	{
		m_minValue = _min;
		m_maxValue = _max;

		if( !m_samples.isEmpty() )
		{
			// Trim existing values
			for( int i=0; i < length(); i++ )
			{
				m_samples[i] = fmaxf( _min, fminf( m_samples[i], _max ) );
			}
		}

		emit rangeChanged();
	}
}
/*
 * Attitude rates controller.
 * Input: '_rates_sp' vector, '_thrust_sp'
 * Output: '_att_control' vector
 */
void
MulticopterAttitudeControl::control_attitude_rates(float dt)
{
	/* reset integral if disarmed */
	if (!_armed.armed || !_vehicle_status.is_rotary_wing) {
		_rates_int.zero();
	}

	/* current body angular rates */
	math::Vector<3> rates;
	rates(0) = _ctrl_state.roll_rate;
	rates(1) = _ctrl_state.pitch_rate;
	rates(2) = _ctrl_state.yaw_rate;

	/* throttle pid attenuation factor */
	float tpa =  fmaxf(0.0f, fminf(1.0f, 1.0f - _params.tpa_slope * (fabsf(_v_rates_sp.thrust) - _params.tpa_breakpoint)));

	/* angular rates error */
	math::Vector<3> rates_err = _rates_sp - rates;

	_att_control = _params.rate_p.emult(rates_err * tpa) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int +
		       _params.rate_ff.emult(_rates_sp);

	_rates_sp_prev = _rates_sp;
	_rates_prev = rates;

	/* update integral only if not saturated on low limit and if motor commands are not saturated */
	if (_thrust_sp > MIN_TAKEOFF_THRUST && !_motor_limits.lower_limit && !_motor_limits.upper_limit) {
		for (int i = AXIS_INDEX_ROLL; i < AXIS_COUNT; i++) {
			if (fabsf(_att_control(i)) < _thrust_sp) {
				float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;

				if (PX4_ISFINITE(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
				    _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT &&
				    /* if the axis is the yaw axis, do not update the integral if the limit is hit */
				    !((i == AXIS_INDEX_YAW) && _motor_limits.yaw)) {
					_rates_int(i) = rate_i;
				}
			}
		}
	}
}
Beispiel #7
0
float ArmClass::Validate_Turret_Command(float cmd, bool ispidcmd)
{
#if 01
	int tur = GetTurretEncoder();
	if (cmd >= 0.0f)   // if cmd is moving turret toward lower angle...
	{
		if (tur < ARM_TURRET_MIN_ENCODER)  // and it is past the lowest angle allowed
		{
			//float error = ARM_TURRET_MIN_ENCODER - tur;
			// BAD!
			return 0.0f;
		}
	}
	else if (cmd <= 0.0f)  // if cmd is moving turret toward higher angles...
	{
		if (tur > ARM_TURRET_MAX_ENCODER)  // and it is past the highest angle allowed
		{
			// BAD!
			return 0.0f;
		}
	}
#endif

	if(ispidcmd)
	{
		//randon .002 commands...
		if(cmd > 0.003f)
		{
			cmd = fmaxf(cmd, MIN_TURRET_CMD);
		}
		else if (cmd < -.003f)
		{
			cmd = fminf(cmd, -MIN_TURRET_CMD);
		}
		else if (cmd < .003f && cmd > -.003f)
		{
			cmd = 0.0f;
		}

	}
	return cmd;
}
glm::vec3 BlinnMicrofacetBxDF::SampleAndEvaluateScatteredEnergy(const glm::vec3 &wo, glm::vec3 &wi_ret, float rand1, float rand2, float &pdf_ret) const{

    float cos_theta( powf( rand1, 1.f / ( exponent + 1.f ) ) );
    float sin_theta( sqrtf( fmaxf( 0.f, 1.f - cos_theta * cos_theta ) ) );

    float phi( rand2 * TWO_PI );
    float cos_phi( cosf( phi ) );
    float sin_phi( sinf( phi ) );

    glm::vec3 wh( sin_theta * cos_phi, sin_theta * sin_phi, cos_theta );

    float woDwh( glm::dot( wo, wh ) );

    if( woDwh < 0.f ) wh = -wh;

    wi_ret = -wo + 2.f * woDwh * wh;
    pdf_ret = PDF( wo, wi_ret );

    return EvaluateScatteredEnergy( wo, wi_ret );
}
Beispiel #9
0
/* shoot on circle */
uint8_t dc_circle(float interval, float start)
{
  dc_autoshoot = DC_AUTOSHOOT_CIRCLE;
  dc_gps_count = 0;
  dc_circle_interval = fmodf(fmaxf(interval, 1.), 360.);

  if (start == DC_IGNORE) {
    start = DegOfRad(stateGetNedToBodyEulers_f()->psi);
  }

  dc_circle_start_angle = fmodf(start, 360.);
  if (start < 0.) {
    start += 360.;
  }
  //dc_circle_last_block = floorf(dc_circle_start_angle/dc_circle_interval);
  dc_circle_last_block = 0;
  dc_circle_max_blocks = floorf(360. / dc_circle_interval);
  dc_info();
  return 0;
}
Beispiel #10
0
int BlockLocalPositionEstimator::mocapMeasure(Vector<float, n_y_mocap> &y)
{
	uint8_t x_variance = _sub_mocap_odom.get().COVARIANCE_MATRIX_X_VARIANCE;
	uint8_t y_variance = _sub_mocap_odom.get().COVARIANCE_MATRIX_Y_VARIANCE;
	uint8_t z_variance = _sub_mocap_odom.get().COVARIANCE_MATRIX_Z_VARIANCE;

	if (PX4_ISFINITE(_sub_mocap_odom.get().pose_covariance[x_variance])) {
		// check if the mocap data is valid based on the covariances
		_mocap_eph = sqrtf(fmaxf(_sub_mocap_odom.get().pose_covariance[x_variance],
					 _sub_mocap_odom.get().pose_covariance[y_variance]));
		_mocap_epv = sqrtf(_sub_mocap_odom.get().pose_covariance[z_variance]);
		_mocap_xy_valid = _mocap_eph <= EP_MAX_STD_DEV;
		_mocap_z_valid = _mocap_epv <= EP_MAX_STD_DEV;

	} else {
		// if we don't have covariances, assume every reading
		_mocap_xy_valid = true;
		_mocap_z_valid = true;
	}

	if (!_mocap_xy_valid || !_mocap_z_valid) {
		_time_last_mocap = _sub_mocap_odom.get().timestamp;
		return -1;

	} else {
		_time_last_mocap = _sub_mocap_odom.get().timestamp;

		if (PX4_ISFINITE(_sub_mocap_odom.get().x)) {
			y.setZero();
			y(Y_mocap_x) = _sub_mocap_odom.get().x;
			y(Y_mocap_y) = _sub_mocap_odom.get().y;
			y(Y_mocap_z) = _sub_mocap_odom.get().z;
			_mocapStats.update(y);

			return OK;

		} else {
			return -1;
		}
	}
}
Beispiel #11
0
static void print_bounds(zarray_t * vertices)
{
    // Approximate centroid: Just average all vertices, regardless of
    // the frequency they are referenced by an index
    float minMax[3][2] = {{FLT_MAX,-FLT_MAX},{FLT_MAX,-FLT_MAX},{FLT_MAX,-FLT_MAX}};
    for (int i = 0; i < zarray_size(vertices); i++) {
        float pt[3];
        zarray_get(vertices, i, &pt);

        // iterate over xyz coords
        for (int j = 0; j < 3; j++) {
            minMax[j][0] = fminf(pt[j], minMax[j][0]);
            minMax[j][1] = fmaxf(pt[j], minMax[j][1]);
        }
    }

    printf(" Object bounds: xyz [%.2f, %.2f][%.2f, %.2f][%.2f, %.2f]\n",
           minMax[0][0],minMax[0][1],
           minMax[1][0],minMax[1][1],
           minMax[2][0],minMax[2][1]);
}
// on "init" you need to initialize your instance
bool HelloWorld::init()
{
    //////////////////////////////
    // 1. super init first
    if ( !Layer::init() )
    {
        return false;
    }
    {
        auto background = ({
            Sprite *sprite = Sprite::create("home_babies.png");
            Size winSize = Director::getInstance()->getWinSize();
            sprite->setPosition(Vec2(winSize.width/2, winSize.height/2));
            float scaleX = winSize.width / sprite->getContentSize().width;
            float scaleY = winSize.height / sprite->getContentSize().height;
            float scale = fmaxf(scaleY, scaleX);
            sprite->setScale(scale);
            sprite;
        });
        this->addChild(background, 0);
    }
Beispiel #13
0
void Renderer::calcSceneParams(unsigned int w, unsigned int h,
        float* offsets) {
    // number of cells along the larger screen dimension
    const float NCELLS_MAJOR = MAX_INSTANCES_PER_SIDE;
    // cell size in scene space
    const float CELL_SIZE = 2.0f / NCELLS_MAJOR;

    // Calculations are done in "landscape", i.e. assuming dim[0] >= dim[1].
    // Only at the end are values put in the opposite order if h > w.
    const float dim[2] = {fmaxf(w,h), fminf(w,h)};
    const float aspect[2] = {dim[0] / dim[1], dim[1] / dim[0]};
    const float scene2clip[2] = {1.0f, aspect[0]};
    const long ncells[2] = {
            (long)NCELLS_MAJOR,
            (long)floorf(NCELLS_MAJOR * aspect[1])
    };

    float centers[2][MAX_INSTANCES_PER_SIDE];
    for (int d = 0; d < 2; d++) {
        float offset = -ncells[d] / NCELLS_MAJOR; // -1.0 for d=0
        for (int i = 0; i < ncells[d]; i++) {
            centers[d][i] = scene2clip[d] * (CELL_SIZE*(i + 0.5f) + offset);
        }
    }

    int major = w >= h ? 0 : 1;
    int minor = w >= h ? 1 : 0;
    // outer product of centers[0] and centers[1]
    for (int i = 0; i < ncells[0]; i++) {
        for (int j = 0; j < ncells[1]; j++) {
            int idx = i*ncells[1] + j;
            offsets[2*idx + major] = centers[0][i];
            offsets[2*idx + minor] = centers[1][j];
        }
    }

    mNumInstances = ncells[0] * ncells[1];
    mScale[major] = 0.5f * CELL_SIZE * scene2clip[0];
    mScale[minor] = 0.5f * CELL_SIZE * scene2clip[1];
}
// calculate filtered offset between GPS height measurement and EKF height estimate
// offset should be subtracted from GPS measurement to match filter estimate
// offset is used to switch reversion to GPS from alternate height data source
void NavEKF3_core::calcFiltGpsHgtOffset()
{
    // Estimate the WGS-84 height of the EKF's origin using a Bayes filter

    // calculate the variance of our a-priori estimate of the ekf origin height
    float deltaTime = constrain_float(0.001f * (imuDataDelayed.time_ms - lastOriginHgtTime_ms), 0.0f, 1.0f);
    if (activeHgtSource == HGT_SOURCE_BARO) {
        // Use the baro drift rate
        const float baroDriftRate = 0.05f;
        ekfOriginHgtVar += sq(baroDriftRate * deltaTime);
    } else if (activeHgtSource == HGT_SOURCE_RNG) {
        // use the worse case expected terrain gradient and vehicle horizontal speed
        const float maxTerrGrad = 0.25f;
        ekfOriginHgtVar += sq(maxTerrGrad * norm(stateStruct.velocity.x , stateStruct.velocity.y) * deltaTime);
    } else if (activeHgtSource == HGT_SOURCE_GPS) {
        // by definition we are using GPS height as the EKF datum in this mode
        // so cannot run this filter
        return;
    }
    lastOriginHgtTime_ms = imuDataDelayed.time_ms;

    // calculate the observation variance assuming EKF error relative to datum is independant of GPS observation error
    // when not using GPS as height source
    float originHgtObsVar = sq(gpsHgtAccuracy) + P[8][8];

    // calculate the correction gain
    float gain = ekfOriginHgtVar / (ekfOriginHgtVar + originHgtObsVar);

    // calculate the innovation
    float innovation = - stateStruct.position.z - gpsDataDelayed.hgt;

    // check the innovation variance ratio
    float ratio = sq(innovation) / (ekfOriginHgtVar + originHgtObsVar);

    // correct the EKF origin and variance estimate if the innovation variance ratio is < 5-sigma
    if (ratio < 5.0f) {
        EKF_origin.alt -= (int)(100.0f * gain * innovation);
        ekfOriginHgtVar -= fmaxf(gain * ekfOriginHgtVar , 0.0f);
    }
}
Beispiel #15
0
static gboolean
expose (GtkWidget *widget, GdkEventExpose *event, dt_iop_module_t *self)
{
    // capture gui color picked event.
    if(darktable.gui->reset) return FALSE;
    if(self->picked_color_max[0] < self->picked_color_min[0]) return FALSE;
    if(!self->request_color_pick) return FALSE;
    static float old[3] = {0, 0, 0};
    const float *grayrgb = self->picked_color;
    if(grayrgb[0] == old[0] && grayrgb[1] == old[1] && grayrgb[2] == old[2]) return FALSE;
    for(int k=0; k<3; k++) old[k] = grayrgb[k];
    dt_iop_temperature_params_t *p = (dt_iop_temperature_params_t *)self->params;
    for(int k=0; k<3; k++) p->coeffs[k] = (grayrgb[k] > 0.001f) ? 1.0f/grayrgb[k] : 1.0f;
    // normalize green:
    p->coeffs[0] /= p->coeffs[1];
    p->coeffs[2] /= p->coeffs[1];
    p->coeffs[1] = 1.0;
    for(int k=0; k<3; k++) p->coeffs[k] = fmaxf(0.0f, fminf(8.0f, p->coeffs[k]));
    gui_update_from_coeffs(self);
    dt_dev_add_history_item(darktable.develop, self, TRUE);
    return FALSE;
}
Beispiel #16
0
static inline void process_filmic(struct dt_iop_module_t *self, dt_dev_pixelpipe_iop_t *piece,
                                  void *ivoid, void *ovoid, const dt_iop_roi_t *roi_in, const dt_iop_roi_t *roi_out,
                                  dt_iop_global_tonemap_data_t *data)
{
  float *in  = (float *)ivoid;
  float *out = (float *)ovoid;
  const int ch = piece->colors;

#ifdef _OPENMP
  #pragma omp parallel for default(none) shared(roi_out, in, out, data) schedule(static)
#endif
  for(size_t k=0; k<(size_t)roi_out->width*roi_out->height; k++)
  {
    float *inp = in + ch*k;
    float *outp = out + ch*k;
    float l = inp[0]/100.0;
    float x = fmaxf(0.0f, l-0.004f);
    outp[0] = 100.0 * ((x*(6.2*x+.5))/(x*(6.2*x+1.7)+0.06));
    outp[1] = inp[1];
    outp[2] = inp[2];
  }
}
Beispiel #17
0
css_dim_t measure(void *context, float width, float height) {
  const char *text = (const char *)context;
  css_dim_t dim;
  if (strcmp(text, SMALL_TEXT) == 0) {
    if (width != width) {
      width = 1000000;
    }
    dim.dimensions[CSS_WIDTH] = fminf(SMALL_WIDTH, width);
    dim.dimensions[CSS_HEIGHT] = SMALL_HEIGHT;
    return dim;
  }
  if (strcmp(text, LONG_TEXT) == 0) {
    if (width != width) {
      width = 1000000;
    }
    dim.dimensions[CSS_WIDTH] = width >= BIG_WIDTH ? BIG_WIDTH : fmaxf(BIG_MIN_WIDTH, width);
    dim.dimensions[CSS_HEIGHT] = width >= BIG_WIDTH ? SMALL_HEIGHT : BIG_HEIGHT;
    return dim;
  }

  if (strcmp(text, MEASURE_WITH_RATIO_2) == 0) {
    if (width > 0) {
      dim.dimensions[CSS_WIDTH] = width;
      dim.dimensions[CSS_HEIGHT] = width * 2;
    } else if (height > 0) {
      dim.dimensions[CSS_WIDTH] = height * 2;
      dim.dimensions[CSS_HEIGHT] = height;
    } else {
      dim.dimensions[CSS_WIDTH] = 99999;
      dim.dimensions[CSS_HEIGHT] = 99999;
    }
    return dim;
  }

  // Should not go here
  dim.dimensions[CSS_WIDTH] = CSS_UNDEFINED;
  dim.dimensions[CSS_HEIGHT] = CSS_UNDEFINED;
  return dim;
}
css_dim_t measure(void *context, float width) {
  const char *text = context;
  css_dim_t dim;
  if (width != width) {
    width = 1000000;
  }
  if (strcmp(text, SMALL_TEXT) == 0) {
    dim.dimensions[CSS_WIDTH] = fminf(SMALL_WIDTH, width);
    dim.dimensions[CSS_HEIGHT] = SMALL_HEIGHT;
    return dim;
  }
  if (strcmp(text, LONG_TEXT) == 0) {
    dim.dimensions[CSS_WIDTH] = width >= BIG_WIDTH ? BIG_WIDTH : fmaxf(BIG_MIN_WIDTH, width);
    dim.dimensions[CSS_HEIGHT] = width >= BIG_WIDTH ? SMALL_HEIGHT : BIG_HEIGHT;
    return dim;
  }

  // Should not go here
  dim.dimensions[CSS_WIDTH] = CSS_UNDEFINED;
  dim.dimensions[CSS_HEIGHT] = CSS_UNDEFINED;
  return dim;
}
Beispiel #19
0
bool sphere_intersect(sphere_t s, vector_t ray_start, vector_t ray_direction, vector_t* hit)
{
    float a = vector_magnitude_2(ray_direction);
    vector_t recentered = vector_sub(ray_start, s.center);
    float b = 2 * vector_dot(ray_direction, recentered);
    float c = vector_magnitude_2(recentered) - (s.radius * s.radius);
    float discrim = (b * b) - (4.0 * a * c);
    if (discrim < 0.0)
        return false;
    float sqrt_discrim = sqrtf(discrim);
    float t1 = (-b + sqrt_discrim) / (2.0 * a);
    float t2 = (-b - sqrt_discrim) / (2.0 * a);
    float t;
    if (t1 > 0.0 && t2 > 0.0) {
        t = fminf(t1, t2);
    } else {
        t = fmaxf(t1, t2);
    }
    if (t <= 0.0)
        return false;
    *hit = vector_add(ray_start, vector_scale(ray_direction, t));
    return true;
}
Beispiel #20
0
void solution_check(central2d_t* sim)
{
    int nx = sim->nx, ny = sim->ny;
    float* u = sim->u;
    float h_sum = 0, hu_sum = 0, hv_sum = 0;
    float hmin = u[central2d_offset(sim,0,0,0)];
    float hmax = hmin;
    for (int j = 0; j < ny; ++j)
        for (int i = 0; i < nx; ++i) {
            float h = u[central2d_offset(sim,0,i,j)];
            h_sum += h;
            hu_sum += u[central2d_offset(sim,1,i,j)];
            hv_sum += u[central2d_offset(sim,2,i,j)];
            hmax = fmaxf(h, hmax);
            hmin = fminf(h, hmin);
        }
    float cell_area = sim->dx * sim->dy;
    h_sum *= cell_area;
    hu_sum *= cell_area;
    hv_sum *= cell_area;
    printf("-\n  Volume: %g\n  Momentum: (%g, %g)\n  Range: [%g, %g]\n",
           h_sum, hu_sum, hv_sum, hmin, hmax);
    assert(hmin > 0);
}
static void testSize()
{
    start_test("Lock free FIFO - size");
    
    const int es = sizeof(int);
    const int c = 100;
    
    const int nCases = 5;
    int nPush[nCases] = {10, 20, 30, 5, 90};
    int nPop[nCases] = {5, 1, 60, 10, 90};
    
    for (int i = 0; i < nCases; i++)
    {
        drLockFreeFIFO f;
        drLockFreeFIFO_init(&f, c, es);
        
        for (int j = 0; j < nPush[i]; j++)
        {
            int success = drLockFreeFIFO_push(&f, &j);
        }
        
        for (int j = 0; j < nPop[i]; j++)
        {
            int val = 0;
            int success = drLockFreeFIFO_pop(&f, &val);
        }
        
        const int expectedSize = fmaxf(0.0f, nPush[i] - nPop[i]);
        const int size = drLockFreeFIFO_getNumElements(&f);
        fail_unless(expectedSize == size, "FIFO size should be the same after pushing and popping the same number of items");
        
        drLockFreeFIFO_deinit(&f);
    }
    
    
}
Beispiel #22
0
// untested - algorithm sound but could be typos
CollisionData sat_hull_ray(const ConvexHull &A, const ray &r)
{
    CollisionData cd = { false, INFINITY }; // setup return value

    std::vector<vec2> axes;
    
    axes.reserve(A.verts.size());
    for (int i = 0; i < A.verts.size(); ++i)
        axes.push_back(perp(normal(A.verts[i] - A.verts[(i + 1) % A.verts.size()])));


    float tmin = 0,  //"Entering" scalar for the ray
          tmax = 1;  //"Leaving"  scalar for the ray            

    vec2 cnormal;
    float tpmin;

    for (int i = 0; i < axes.size(); ++i)
    {    
        float N =  dot(axes[i], r.position - A.verts[i]);
        float D = -dot(axes[i], r.direction);

        float t = N / D;

        if (D < 0 && t > tmin)
        {
                tmin = fmaxf(tmin, t);
                cnormal = axes[i];
                cd = { tmin < tmax, (tmax-tmin) * r.length, axes[i], r.position + r.direction * r.length *tmin };
        }
        else    tmax = fminf(tmax, t);

        if (tmin > tmax) return cd;
    }
    return cd;
}
Beispiel #23
0
 /// clamp each vector values
 inline __host__ __device__ Vec3 clamp(const Vec3& min_v, const Vec3& max_v) const {
     return Vec3( fminf( fmaxf(x, min_v.x), max_v.x),
                  fminf( fmaxf(y, min_v.y), max_v.y),
                  fminf( fmaxf(z, min_v.z), max_v.z));
 }
Beispiel #24
0
 /// clamp each vector values
 inline __host__ __device__ Vec3 clamp(float min_v, float max_v) const {
     return Vec3( fminf( fmaxf(x, min_v), max_v),
                  fminf( fmaxf(y, min_v), max_v),
                  fminf( fmaxf(z, min_v), max_v));
 }
Beispiel #25
0
 /// value of the max coordinate
 inline __host__ __device__ float get_max() const {
     return fmaxf(fmaxf(x,y),z);
 }
/////////////////////////////////////////////////////////////////////////////////
//	calculate the intersection of a sphere the given ray
//	the ray has an origin and a direction, ray = origin + t*direction
//	find the t parameter, return true if it is between 0.0 and 1.0, false 
//	otherwise, write the results in following variables:
//	depth	- t \in [0.0 1.0]
//	posX	- x position of intersection point, nothing if no intersection
//	posY	- y position of intersection point, nothing if no intersection
//	posZ	- z position of intersection point, nothing if no intersection
//	normalX	- x component of normal at intersection point, nothing if no intersection
//	normalX	- y component of normal at intersection point, nothing if no intersection
//	normalX	- z component of normal at intersection point, nothing if no intersection
//
//	attention: a sphere has usually two intersection points make sure to return 
//	the one that is closest to the ray's origin and still in the viewing frustum
//
/////////////////////////////////////////////////////////////////////////////////
bool 
Sphere::intersect(Ray ray, double *depth,	
				  double *posX, double *posY, double *posZ,
				  double *normalX, double *normalY, double *normalZ)

{
	//////////*********** START OF CODE TO CHANGE *******////////////

	// from slides:
	// (cx + t * vx)^2 + (cy + t * vy)^2 + (cz + t * vy)^2 = r^2

	// text:
	// (e+td−c)·(e+td−c)−R2 = 0
	// (d·d)t^2 +2d·(e−c)t+(e−c)·(e−c)−R^2 = 0

	// d: the direction vector of the ray
	// e: point at which the ray starts
	// c: center point of the sphere

	Vec3 dvec(	ray.direction[0],
				ray.direction[1],
				ray.direction[2]);

	Vec3 evec(	ray.origin[0],
				ray.origin[1],
				ray.origin[2]);

	Vec3 cvec(	this->center[0],
				this->center[1],
				this->center[2]);

	// use the quadratic equation, since we have the form At^2 + Bt + C = 0.

	double a = dvec.dot(dvec);
	double b = dvec.scale(2).dot(evec.subtract(cvec));

	Vec3 eMinusCvec = evec.subtract(cvec);
	double c = eMinusCvec.dot(eMinusCvec) - (this->radius * this->radius);

	// discriminant: b^2 - 4ac
	double discriminant = (b * b) - (4 * a * c);

	// From text: If the discriminant is negative, its square root 
	// is imaginary and the line and sphere do not intersect.
	if (discriminant < 0) {
		
		//printf("No intersection with sphere - 1\n");
		return false;

	} else {
		// there is at least one intersection point
		double t1 = (-b + sqrt(discriminant)) / (2 * a);
		double t2 = (-b - sqrt(discriminant)) / (2 * a);

		double tmin = fminf(t1, t2);
		double tmax = fmaxf(t1, t2);

		double t = 0; // t is set to either tmin or tmax (or the function returns false)

		if (tmin >= 0) { //} && tmin <= 1) {

			t = tmin;

		} else if (tmax >= 0) { //} && tmax <= 1) {

			t = tmax;

		} else {

			// return false if neither interestion point is within [0, 1]
			//printf("No intersection with sphere. t values (%f, %f)\n", t1, t2);
			return false;

		}

		*depth = t;
		
		// position: (e + td)
		Vec3 posvec = dvec.scale(t).add(evec);
		*posX = posvec[0];
		*posY = posvec[1];
		*posZ = posvec[2];

		// normal: 2(p - c)
		Vec3 normalvec = posvec.subtract(cvec).scale(2);
		normalvec.normalize();
		*normalX = normalvec[0];
		*normalY = normalvec[1];
		*normalZ = normalvec[2];
	}

	//////////*********** END OF CODE TO CHANGE *******////////////
	//printf("Sphere intersection found (%f, %f, %f) \n", *posX, *posY, *posZ);
	return true;
}
Beispiel #27
0
float ArmClass::Validate_Lift_Command(float cmd, bool ispidcmd)
{
#if 01
	// Validating the lift command.
	// We broke the limits down into two cases.
	// 1)  When the turret is facing forward (0deg) the arm can go down to 0deg or up to (180-39.2 = 140.8deg)
	// 2)  When the turret is facing sideways (90deg or 270deg) the arm can go down to 14.6deg or up to (180-14.6 = 165.4deg)

	int lift_enc = GetLifterEncoder();
	float lift_angle = Lift_Encoder_To_Degrees(lift_enc);
	int turret_enc = GetTurretEncoder();
	float turret_angle = Turret_Encoder_To_Degrees(turret_enc);

	// Special case: Turret is centered.
	if (TurretRoughlyCentered())
	{
		if (cmd >= 0.0f)	// if we are pushing the arm down (might have to switch this to > 0.0
		{
			if (lift_angle < ARM_LIFT_MIN_WHEN_CENTERED)
			{
				// BAD!  pushing the arm into the frame of the robot
				float error = ARM_LIFT_MIN_WHEN_CENTERED - lift_angle;
				return Compute_Lift_Error_Correction_Command(error);
			}
		}
		if (cmd <= 0.0f) // lifting, limit at 140 deg
		{
			if (lift_angle > ARM_LIFT_MAX_WHEN_CENTERED)
			{
				float error = ARM_LIFT_MAX_WHEN_CENTERED - lift_angle;
				return Compute_Lift_Error_Correction_Command(error);
			}
		}
	}
	else if (fabs(turret_angle) < 45.0f)   // Mostly Forward case
	{
		if (cmd >= 0.0f)	// if we are pushing the arm down (might have to switch this to > 0.0
		{
			if (lift_angle < ARM_LIFT_MIN_WHEN_FWD)
			{
				// BAD!  pushing the arm into the frame of the robot
				float error = ARM_LIFT_MIN_WHEN_FWD - lift_angle;
				return Compute_Lift_Error_Correction_Command(error);
			}
		}
		if (cmd <= 0.0f) // lifting, limit at 140 deg
		{
			if (lift_angle > ARM_LIFT_MAX_WHEN_FWD)
			{
				float error = ARM_LIFT_MAX_WHEN_FWD - lift_angle;
				return Compute_Lift_Error_Correction_Command(error);
			}
		}
	}
	else  // Sideways case
	{
		if (cmd >= 0.0f)	// if we are pushing the arm down (might have to switch this to > 0.0
		{
			if (lift_angle < ARM_LIFT_MIN_WHEN_SIDEWAYS)
			{
				// BAD!  violating the envelop to one side of the robot
				float error = ARM_LIFT_MIN_WHEN_SIDEWAYS - lift_angle;
				return Compute_Lift_Error_Correction_Command(error);
			}
		}
		if (cmd <= 0.0f) // lifting, limit at 140 deg
		{
			if (lift_angle > ARM_LIFT_MAX_WHEN_SIDEWAYS)
			{
				// BAD! violating the envelope to the other side of the robot
				float error = ARM_LIFT_MAX_WHEN_SIDEWAYS - lift_angle;
				return Compute_Lift_Error_Correction_Command(error);
			}
		}
	}
#endif

	if(ispidcmd)
	{
		//randon .002 commands...
		if(cmd > 0.003f)
		{
			cmd = fmaxf(cmd, MIN_ARM_LIFT_CMD);
		}
		else if (cmd < -.003f)
		{
			cmd = fminf(cmd, -MIN_ARM_LIFT_CMD);
		}
		else if (cmd < .003f && cmd > -.003f)
		{
			cmd = 0.0f;
		}
	}

	return cmd;
}
Beispiel #28
0
int main(int argc, char ** argv) {
    
    /* BEGIN DECLARATIONS */
    
    WINFO wi;        /* struct for command line input */
    
    /* workspace */
    
    float * v;       /* velocity field */
    float * p1;      /* pressure field, current time step */
    float * p0;      /* pressure field, last time step */
    
    float * tr;      /* storage for traces */
    float * tmp;     /* used to swap p1 and p0 */
    
    int ix, it;      /* counters */
    int isrc;        /* source counter */
    int imf;         /* movie frame counter */
    int isx;         /* source location, in units of dx */
    int nxz;         /* number of spatial grid points */
    /* int nz;          local number of gridpoints */
    int ntr;         /* number of traces */
    int nsam;        /* number of trace samples */
    int nsrc;        /* number of shots */
    float rz,rx,s;   /* precomputed coefficients */
    float vmax,vmin; /* max, min velocity values */
    /* float two;        two */
    
    /* END DECLARATIONS */
    
    sf_init(argc,argv);
    
    /* read inputs from command line */
    getinputs(true,&wi);
    
    /* compute number of shots */
    nsrc = (wi.isxend-wi.isxbeg)/(wi.iskip); nsrc++;
    
    /* compute number of spatial grid points */
    nxz=wi.nx * wi.nz;
    
    /* compute number of traces, samples in each record */
    ntr=wi.igxend-wi.igxbeg+1;
    nsam=ntr*wi.nt;
    
    /* allocate, initialize p0, p1, v, traces */
    p0=sf_floatalloc(nxz);
    p1=sf_floatalloc(nxz);
    v =sf_floatalloc(nxz);
    tr=sf_floatalloc(nsam);
    
    /* read velocity */
    sf_floatread(v,nxz,wi.vfile);
    
    /* CFL, sanity checks */
    vmax=fgetmax(v,nxz);
    vmin=fgetmin(v,nxz);
    if (vmax*wi.dt>CFL*fmaxf(wi.dx,wi.dz)) {
	sf_warning("CFL criterion violated");
	sf_warning("vmax=%e dx=%e dz=%e dt=%e\n",vmax,wi.dx,wi.dz,wi.dt);
	sf_error("max permitted dt=%e\n",CFL*fmaxf(wi.dx,wi.dz)/vmax);
    }
    if (vmin<=0.0) 
	sf_error("min velocity nonpositive");
    
    /* only square of velocity array needed from here on */
    fsquare(v,nxz);
    
    /* precalculate some coefficients */
    rz=wi.dt*wi.dt/(wi.dz*wi.dz);
    rx=wi.dt*wi.dt/(wi.dx*wi.dx);
    s =2.0*(rz+rx);
/*    two=2.0;
      nz=wi.nz; */
    
    /* shot loop */
    isrc=0;
    isx=wi.isxbeg;
    while (isx <= wi.isxend) {
	
	/* initialize pressure fields, traces */
	fzeros(p0,nxz);
	fzeros(p1,nxz);
	fzeros(tr,nsam);
	
	/* initialize movie frame counter */
	imf=0;
	
	/* time loop */
	for (it=0;it<wi.nt;it++) {
	    
	    /* construct next time step, overwrite on p0 */
	    
	    step_forward(p0,p1,v,wi.nz,wi.nx,rz,rx,s);
	    
	    /* tack on source */
	    p0[wi.isz+isx*wi.nz]+=fgetrick(it*wi.dt,wi.freq);
	    
	    /* swap pointers */
	    tmp=p0;
	    p0=p1;
	    p1=tmp;
	    
	    /* store trace samples if necessary */
	    if (NULL != wi.tfile) 
		for (ix=0;ix<ntr;ix++) 
		    tr[ix*wi.nt+it]=p1[(wi.igxbeg+ix)*wi.nz+wi.igz];
	    
	    /* write movie snap to file if necessary */
	    if (NULL != wi.mfile && wi.nm && !(it%wi.nm)) {
		sf_floatwrite(p1,nxz,wi.mfile);
		imf++;
	    }
	    
	    /* next t */
	}

	/* write traces to file if necessary */
	if (NULL != wi.tfile) 
	    sf_floatwrite(tr,nsam,wi.tfile);
	
	isx += wi.iskip;
	isrc++;
    } 


    exit(0);
}
Beispiel #29
0
void BattleGesture::UpdateTrackingMarker()
{
	Unit* unit = _trackingMarker->GetUnit();

	glm::vec2 screenTouchPosition = _trackingTouch->GetPosition();
	glm::vec2 screenMarkerPosition = screenTouchPosition + glm::vec2(0, 1) * (_offsetToMarker * GetFlipSign());
	glm::vec2 touchPosition = _battleView->GetTerrainPosition3(screenTouchPosition).xy();
	glm::vec2 markerPosition = _battleView->GetTerrainPosition3(screenMarkerPosition).xy();

	Unit* enemyUnit = FindEnemyUnit(touchPosition, markerPosition);
	glm::vec2 unitCenter = unit->state.center;

	bool isModifierMode = _tappedModiferArea || _modifierTouch != nullptr || _trackingTouch->GetCurrentButtons().right;
	_trackingMarker->SetRenderOrientation(isModifierMode);

	if (!isModifierMode)
	{
		std::vector<glm::vec2>& path = _trackingMarker->_path;

		glm::vec2 currentDestination = path.size() != 0 ? *(path.end() - 1) : unit->state.center;

		bounds2f contentBounds = _battleView->GetContentBounds();
		glm::vec2 contentCenter = contentBounds.center();
		float contentRadius = contentBounds.width() / 2;

		glm::vec2 differenceToCenter = contentCenter - markerPosition;
		float distanceToCenter = glm::length(differenceToCenter);
		if (distanceToCenter > contentRadius)
		{
			markerPosition += differenceToCenter * (distanceToCenter - contentRadius) / distanceToCenter;
		}

		float movementLimit = -1;
		float delta = 1.0f / fmaxf(1, glm::length(currentDestination - markerPosition));
		for (float k = delta; k < 1; k += delta)
		{
			GroundMap* groundMap = _battleView->GetSimulator()->GetGroundMap();
			if (groundMap != nullptr && groundMap->IsImpassable(glm::mix(currentDestination, markerPosition, k)))
			{
				movementLimit = k;
				break;
			}
		}

		if (movementLimit >= 0)
		{
			glm::vec2 diff = markerPosition - currentDestination;
			markerPosition = currentDestination + diff * movementLimit;
			markerPosition -= glm::normalize(diff) * 10.0f;

			enemyUnit = nullptr;
		}

		if (enemyUnit && !_trackingMarker->GetMeleeTarget())
			SoundPlayer::singleton->Play(SoundBufferCommandMod);

		_trackingMarker->SetMeleeTarget(enemyUnit);
		_trackingMarker->SetDestination(&markerPosition);

		if (enemyUnit != nullptr)
			MovementRules::UpdateMovementPath(_trackingMarker->_path, unitCenter, enemyUnit->state.center);
		else
			MovementRules::UpdateMovementPath(_trackingMarker->_path, unitCenter, markerPosition);

		if (enemyUnit != nullptr)
		{
			glm::vec2 destination = enemyUnit->state.center;
			glm::vec2 orientation = destination + glm::normalize(destination - unitCenter) * 18.0f;
			_trackingMarker->SetOrientation(&orientation);
		}
		else if (MovementRules::Length(_trackingMarker->_path) > KEEP_ORIENTATION_TRESHHOLD)
		{
			glm::vec2 dir = glm::normalize(markerPosition - unitCenter);
			if (path.size() >= 2)
				dir = glm::normalize(*(path.end() - 1) - *(path.end() - 2));
			glm::vec2 orientation = markerPosition + dir * 18.0f;
			_trackingMarker->SetOrientation(&orientation);
		}
		else
		{
			glm::vec2 orientation = markerPosition + 18.0f * vector2_from_angle(unit->state.bearing);
			_trackingMarker->SetOrientation(&orientation);
		}
	}
	else
	{
		MovementRules::UpdateMovementPathStart(_trackingMarker->_path, unitCenter);

		bool holdFire = false;
		if (_trackingMarker->GetUnit()->state.unitMode == UnitMode_Standing && _trackingMarker->GetUnit()->stats.maximumRange > 0)
		{
			bounds2f unitCurrentBounds = GetUnitCurrentBounds(_trackingMarker->GetUnit());
			holdFire = glm::distance(screenMarkerPosition, unitCurrentBounds.center()) <= unitCurrentBounds.x().radius();
		}

		if (holdFire)
		{
			_trackingMarker->SetMissileTarget(_trackingMarker->GetUnit());
			_trackingMarker->SetOrientation(nullptr);
		}
		else
		{
			//if (!_tappedUnitCenter)
			//	enemyUnit = nullptr;
			if (!_allowTargetEnemyUnit)
				enemyUnit = nullptr;

			if (enemyUnit != nullptr && _trackingMarker->GetMissileTarget() == nullptr)
				SoundPlayer::singleton->Play(SoundBufferCommandMod);

			_trackingMarker->SetMissileTarget(enemyUnit);
			_trackingMarker->SetOrientation(&markerPosition);
		}
	}
}
Beispiel #30
0
void
Mission::set_mission_items()
{
	/* make sure param is up to date */
	updateParams();

	/* reset the altitude foh logic, if altitude foh is enabled (param) a new foh element starts now */
	altitude_sp_foh_reset();

	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();

	/* set previous position setpoint to current */
	set_previous_pos_setpoint();

	/* Copy previous mission item altitude (can be extended to a copy of the full mission item if needed) */
	if (pos_sp_triplet->previous.valid) {
		_mission_item_previous_alt = get_absolute_altitude_for_item(_mission_item);
	}

	/* the home dist check provides user feedback, so we initialize it to this */
	bool user_feedback_done = false;

	/* try setting onboard mission item */
	if (_param_onboard_enabled.get() && read_mission_item(true, true, &_mission_item)) {
		/* if mission type changed, notify */
		if (_mission_type != MISSION_TYPE_ONBOARD) {
			mavlink_log_critical(_navigator->get_mavlink_fd(), "onboard mission now running");
			user_feedback_done = true;
		}
		_mission_type = MISSION_TYPE_ONBOARD;

	/* try setting offboard mission item */
	} else if (read_mission_item(false, true, &_mission_item)) {
		/* if mission type changed, notify */
		if (_mission_type != MISSION_TYPE_OFFBOARD) {
			mavlink_log_info(_navigator->get_mavlink_fd(), "offboard mission now running");
			user_feedback_done = true;
		}
		_mission_type = MISSION_TYPE_OFFBOARD;
	} else {
		/* no mission available or mission finished, switch to loiter */
		if (_mission_type != MISSION_TYPE_NONE) {
			/* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */
			mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering");
			user_feedback_done = true;

			/* use last setpoint for loiter */
			_navigator->set_can_loiter_at_sp(true);

		}

		_mission_type = MISSION_TYPE_NONE;

		/* set loiter mission item and ensure that there is a minimum clearance from home */
		set_loiter_item(&_mission_item, _param_takeoff_alt.get());

		/* update position setpoint triplet  */
		pos_sp_triplet->previous.valid = false;
		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
		pos_sp_triplet->next.valid = false;

		/* reuse setpoint for LOITER only if it's not IDLE */
		_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);

		set_mission_finished();

		if (!user_feedback_done) {
			/* only tell users that we got no mission if there has not been any
			 * better, more specific feedback yet
			 * https://en.wikipedia.org/wiki/Loiter_(aeronautics)
			 */

			if (_navigator->get_vstatus()->condition_landed) {
				/* landed, refusing to take off without a mission */

				mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, refusing takeoff");
			} else {
				mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, loitering");
			}

			user_feedback_done = true;

		}

		_navigator->set_position_setpoint_triplet_updated();
		return;
	}

	if (pos_sp_triplet->current.valid) {
		_on_arrival_yaw = _mission_item.yaw;
	}

	/* do takeoff on first waypoint for rotary wing vehicles */
	if (_navigator->get_vstatus()->is_rotary_wing) {
		/* force takeoff if landed (additional protection) */
		if (!_takeoff && _navigator->get_vstatus()->condition_landed) {
			_need_takeoff = true;
		}

		/* new current mission item set, check if we need takeoff */
		if (_need_takeoff && (
				_mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
				_mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
				_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
				_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
				_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
				_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH)) {
			_takeoff = true;
			_need_takeoff = false;
		}
	}

	if (_takeoff) {
		/* do takeoff before going to setpoint */
		/* set mission item as next position setpoint */
		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->next);
		/* next SP is not takeoff anymore */
		pos_sp_triplet->next.type = position_setpoint_s::SETPOINT_TYPE_POSITION;

		/* calculate takeoff altitude */
		float takeoff_alt = get_absolute_altitude_for_item(_mission_item);

		/* takeoff to at least NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */
		if (_navigator->get_vstatus()->condition_landed) {
			takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _param_takeoff_alt.get());

		} else {
			takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get());
		}

		/* check if we already above takeoff altitude */
		if (_navigator->get_global_position()->alt < takeoff_alt) {
			mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));

			_mission_item.nav_cmd = NAV_CMD_TAKEOFF;
			_mission_item.lat = _navigator->get_global_position()->lat;
			_mission_item.lon = _navigator->get_global_position()->lon;
			_mission_item.yaw = NAN;
			_mission_item.altitude = takeoff_alt;
			_mission_item.altitude_is_relative = false;
			_mission_item.autocontinue = true;
			_mission_item.time_inside = 0;

			mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);

			_navigator->set_position_setpoint_triplet_updated();
			return;

		} else {
			/* skip takeoff */
			_takeoff = false;
		}
	}

	if (_takeoff_finished) {
		/* we just finished takeoff */
		/* in case we still have to move to the takeoff waypoint we need a waypoint mission item */
		_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
		_takeoff_finished = false;
	}

	/* set current position setpoint from mission item */
	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);

	/* require takeoff after landing or idle */
	if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
		_need_takeoff = true;
	}

	_navigator->set_can_loiter_at_sp(false);
	reset_mission_item_reached();

	if (_mission_type == MISSION_TYPE_OFFBOARD) {
		set_current_offboard_mission_item();
	}
	// TODO: report onboard mission item somehow

	if (_mission_item.autocontinue && _mission_item.time_inside <= 0.001f) {
		/* try to read next mission item */
		struct mission_item_s mission_item_next;

		if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) {
			/* got next mission item, update setpoint triplet */
			mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next);
		} else {
			/* next mission item is not available */
			pos_sp_triplet->next.valid = false;
		}

	} else {
		/* vehicle will be paused on current waypoint, don't set next item */
		pos_sp_triplet->next.valid = false;
	}

	/* Save the distance between the current sp and the previous one */
	if (pos_sp_triplet->current.valid && pos_sp_triplet->previous.valid) {
		_distance_current_previous = get_distance_to_next_waypoint(pos_sp_triplet->current.lat,
				pos_sp_triplet->current.lon,
				pos_sp_triplet->previous.lat,
				pos_sp_triplet->previous.lon);
	}

	_navigator->set_position_setpoint_triplet_updated();
}