И опять про трапециевидный рамп

// inner part of `calc`, could be called recoursively for hard case
static void unlockedcalc(movemodel_t *m, moveparam_t *x, double t){
    // signs
    double sign_a01 = 0., sign_a23 = 0., sign_vset = 0.; // accelerations on stages ACCEL and DECEL, speed on maxspeed stage
    // times
    double dt01 = 0., dt12 = 0., dt23 = 0.;
    // absolute speed at stage 23 (or in that point); absolute max acceleration
    double abs_vset = x->speed, abs_a = m->Max.accel;
    // absolute target movement
    double abs_Dx = fabs(x->coord - m->curparams.coord);
    if(m->state == ST_STOP && abs_Dx < coord_tolerance){
        DBG("Movement too small -> stay at place");
        return;
    }
    // signs of Dx and current speed
    double sign_Dx = (x->coord > m->curparams.coord) ? 1. : -1.;
    double v0 = m->curparams.speed;
    double sign_v0 = v0 < 0. ? -1 : 1., abs_v0 = fabs(v0);
    if(v0 == 0.) sign_v0 = 0.;
    // preliminary calculations (vset and dependent values could be changed)
    dt01 = fabs(abs_v0 - abs_vset) / abs_a;
    double abs_dx23 = abs_vset * abs_vset / 2. / abs_a;
    dt23 = abs_vset / abs_a;
    double abs_dx_stop = abs_v0 * abs_v0 / 2. / abs_a;
    if(sign_Dx * sign_v0 >= 0. && abs_dx_stop < abs_Dx){ // we shouldn't change speed direction
        if(fabs(abs_dx_stop - abs_Dx) <= coord_tolerance){ // simplest case: just stop
            //DBG("Simplest case: stop");
            dt01 = dt12 = 0.;
            sign_a23 = -sign_v0;
            dt23 = abs_v0 / abs_a;
        }else if(abs_vset < abs_v0){ // move with smaller speed than now: very simple case
            //DBG("Move with smaller speed");
            sign_a01 = sign_a23 = -sign_v0;
            sign_vset = sign_v0;
            double abs_dx01 = abs_v0 * dt01 - abs_a * dt01 * dt01 / 2.;
            double abs_dx12 = abs_Dx - abs_dx01 - abs_dx23;
            dt12 = abs_dx12 / abs_vset;
        }else{// move with larget speed
            //DBG("Move with larger speed");
            double abs_dx01 = abs_v0 * dt01 + abs_a * dt01 * dt01 / 2.;
            if(abs_Dx < abs_dx01 + abs_dx23){ // recalculate target speed and other
                abs_vset = sqrt(abs_a * abs_Dx + abs_v0 * abs_v0 / 2.);
                dt01 = fabs(abs_v0 - abs_vset) / abs_a;
                abs_dx01 = abs_v0 * dt01 + abs_a * dt01 * dt01 / 2.;
                dt23 = abs_vset / abs_a;
                abs_dx23 = abs_vset * abs_vset / 2. / abs_a;
                DBG("Can't reach target speed %g, take %g instead", x->speed, abs_vset);
            }
            sign_a01 = sign_Dx; // sign_v0 could be ZERO!!!
            sign_a23 = -sign_Dx;
            sign_vset = sign_Dx;
            double abs_dx12 = abs_Dx - abs_dx01 - abs_dx23;
            dt12 = abs_dx12 / abs_vset;
        }
    }else{
        // if we are here, we have the worst case: change speed direction
        DBG("Hardest case: change speed direction");
        // now we should calculate coordinate at which model stops and biuld new trapezium from that point
        double x0 = m->curparams.coord, v0 = m->curparams.speed;
        double xstop = x0 + sign_v0 * abs_dx_stop, tstop = t + abs_v0 / abs_a;
        m->state = ST_STOP;
        m->curparams.accel = 0.; m->curparams.coord = xstop; m->curparams.speed = 0.;
        unlockedcalc(m, x, tstop); // calculate new ramp
        // and change started conditions
        m->curparams.coord = x0; m->curparams.speed = v0;
        m->Times[STAGE_ACCEL] = t;
        m->Params[STAGE_ACCEL].coord = x0;
        m->Params[STAGE_ACCEL].speed = v0;
        DBG("NOW t[0]=%g, X[0]=%g, V[0]=%g", t, x0, v0);
        return;
    }
    m->state = ST_MOVE;
    m->movingstage = STAGE_ACCEL;
    // some knot parameters
    double a01 = sign_a01 * abs_a, a23 = sign_a23 * abs_a;
    double v1, v2, x0, x1, x2;
    v2 = v1 = sign_vset * abs_vset;
    x0 = m->curparams.coord;
    x1 = x0 + v0 * dt01 + a01 * dt01 * dt01 / 2.;
    x2 = x1 + v1 * dt12;
    // fill knot parameters
    moveparam_t *p = &m->Params[STAGE_ACCEL]; // 0-1 - change started speed
    p->accel = a01;
    p->speed = m->curparams.speed;
    p->coord = x0;
    m->Times[STAGE_ACCEL] = t;
    p = &m->Params[STAGE_MAXSPEED]; // 1-2 - constant speed
    p->accel = 0.;
    p->speed = v1;
    p->coord = x1;
    m->Times[STAGE_MAXSPEED] = m->Times[STAGE_ACCEL] + dt01;
    p = &m->Params[STAGE_DECEL]; // 2-3 - decrease speed
    p->accel = a23;
    p->speed = v2;
    p->coord = x2;
    m->Times[STAGE_DECEL] = m->Times[STAGE_MAXSPEED] + dt12;
    p = &m->Params[STAGE_STOPPED]; // 3 - stop at target
    p->accel = p->speed = 0.;
    p->coord = x->coord;
    m->Times[STAGE_STOPPED] = m->Times[STAGE_DECEL] + dt23;
}
 
/**
 * @brief calc - moving calculation
 * @param x - using max speed (>0!!!) and coordinate
 * @param t - current time value
 * @return FALSE if can't move with given parameters
 */
static int calc(movemodel_t *m, moveparam_t *x, double t) {
    //DBG("target coord/speed: %g/%g; current: %g/%g", x->coord, x->speed, m->curparams.coord, m->curparams.speed);
    if (!x || !m) return FALSE;
    pthread_mutex_lock(&m->mutex);
    int ret = FALSE;
    // Validate input parameters
    if(x->coord < m->Min.coord || x->coord > m->Max.coord){
        DBG("Wrong coordinate [%g, %g]", m->Min.coord, m->Max.coord);
        goto ret;
    }
    if(x->speed < m->Min.speed || x->speed > m->Max.speed){
        DBG("Wrong speed [%g, %g]", m->Min.speed, m->Max.speed);
        goto ret;
    }
    ret = TRUE; // now there's no chanses to make error
    unlockedcalc(m, x, t);
    // Debug output
    /*for(int i = 0; i < STAGE_AMOUNT; i++){
        DBG("Stage %d: t=%.6f, coord=%.6f, speed=%.6f, accel=%.6f",
            i, m->Times[i], m->Params[i].coord, m->Params[i].speed, m->Params[i].accel);
    }*/
ret:
    pthread_mutex_unlock(&m->mutex);
    return ret;
}