strafelib.hpp

Go to the documentation of this file.

#include <cmath>

constexpr const double JUMP_SPEED = 268.3281572999748;

template<int N>
inline double dot_product(const double *__restrict a, const double *__restrict b)
{
    double res = 0;
    for (int i = 0; i < N; ++i) {
        res += a[i] * b[i];
    }
    return res;
}

double fric_speed(double speed, double E, double tau_k)
{
    if (speed >= E) {
        return speed * (1 - tau_k);
    }

    const double tau_E_k = tau_k * E;
    if (speed >= tau_E_k && speed >= 0.1) {
        return speed - tau_E_k;
    }

    return 0;
}

void fric_vel(double *__restrict vel, double speed, double E, double tau_k)
{
    if (speed >= E) {
        const double tmp = 1 - tau_k;
        vel[0] *= tmp;
        vel[1] *= tmp;
        return;
    }

    const double tau_E_k = tau_k * E;
    if (speed >= tau_E_k && speed >= 0.1) {
        const double tmp = tau_E_k / speed;
        vel[0] -= vel[0] * tmp;
        vel[1] -= vel[1] * tmp;
        return;
    }

    vel[0] = 0;
    vel[1] = 0;
}    

double fric_speedsq(double speedsq, double E, double tau_k)
{
    if (speedsq >= E * E) {
        const double tmp = 1 - tau_k;
        return speedsq * tmp * tmp;
    }

    const double tau_E_k = tau_k * E;
    const double tau_E_k_sq = tau_E_k * tau_E_k;
    if (speedsq >= tau_E_k_sq && speedsq >= 0.01) {
        return speedsq - 2 * std::sqrt(speedsq) * tau_E_k + tau_E_k_sq;
    }

    return 0;
}

double fme_speed(double speed, double costheta, double L, double ke_tau_M_A)
{
    const double gamma2 = L - speed * costheta;
    if (gamma2 <= 0) {
        return speed;
    }

    double mu = ke_tau_M_A;
    if (gamma2 < mu) {
        mu = gamma2;
    }

    return std::sqrt(speed * (speed + 2 * mu * costheta) + mu * mu);
}

void fme_vel_theta(double *__restrict vel, double speed, double costheta, double sintheta, double L, double ke_tau_M_A)
{
    const double gamma2 = L - speed * costheta;
    if (gamma2 <= 0) {
        return;
    }

    const double mu = std::min(ke_tau_M_A, gamma2);
    const double tmp = mu / speed;
    const double ax = vel[0] * costheta + vel[1] * sintheta;
    const double ay = vel[1] * costheta - vel[0] * sintheta;
    vel[0] += tmp * ax;
    vel[1] += tmp * ay;
}

void fme_maxaccel_cossin_theta(double speed, double L, double ke_tau_M_A, double *__restrict costheta, double *__restrict sintheta)
{
    if (ke_tau_M_A >= 0) {
        if (L <= ke_tau_M_A) {
            if (L >= 0) {
                *costheta = 0;
                *sintheta = 1;
                return;
            }
            *costheta = 1;
            *sintheta = 0;
            return;
        }

        const double tmp = L - ke_tau_M_A;
        if (tmp <= speed) {
            const double ct = tmp / speed;
            *costheta = ct;
            *sintheta = std::sqrt(1 - ct * ct);
            return;
        }
        *costheta = 1;
        *sintheta = 0;
        return;
    }

    if (-L < speed) {
        *costheta = -1;
        *sintheta = 0;
        return;
    }
    *costheta = 1;
    *sintheta = 0;
    return;
}

double fme_maxaccel_speed(double speed, double L, double ke_tau_M_A)
{
    if (ke_tau_M_A >= 0) {
        if (L <= ke_tau_M_A) {
            if (L >= 0) {
                return std::sqrt(speed * speed + L * L);
            }
            return speed;
        }

        const double tmp = L - ke_tau_M_A;
        if (tmp <= speed) {
            return std::sqrt(speed * speed + ke_tau_M_A * (L + tmp));
        }
        return speed + ke_tau_M_A;
    }

    if (-L < speed) {
        return speed - ke_tau_M_A;
    }
    return speed;
}

double fme_maxaccel_speed_C(double speedsq, double L, double ke_tau_M_A)
{
    if (ke_tau_M_A >= 0) {
        if (L <= ke_tau_M_A) {
            if (L >= 0) {
                return L * L;
            }
            return 0;
        }

        const double tmp = L - ke_tau_M_A;
        if (tmp * tmp <= speedsq) {
            return ke_tau_M_A * (L + tmp);
        }
        return (2 * std::sqrt(speedsq) + ke_tau_M_A) * ke_tau_M_A;
    }

    if (L >= 0 || L * L < speedsq) {
        return (ke_tau_M_A - 2 * std::sqrt(speedsq)) * ke_tau_M_A;
    }
    return 0;
}

double fme_minaccel_speed(double speed, double L, double ke_tau_M_A)
{
    if (ke_tau_M_A >= 0) {
        if (L >= 0) {
            if (L >= ke_tau_M_A) {
                return std::fabs(speed - ke_tau_M_A);
            }

            const double tmp = L - ke_tau_M_A;
            if (L <= speed || -tmp <= speed) {
                return std::fabs(speed - ke_tau_M_A);
            }
            return L;
        }

        if (-L < speed) {
            if (ke_tau_M_A - L <= speed) {
                return std::fabs(speed - ke_tau_M_A);
            }
            return L; 
        }
        return speed;
    }

    return speed;
}

template<int N>
void collision_vel(double *__restrict v, const double *__restrict n, double b)
{
    const double tmp = b * dot_product<N>(v, n);
    for (int i = 0; i < N; ++i) {
        v[i] -= tmp * n[i];
    }
}

template<int N>
void snark_hunt_vel(double *__restrict v, const double *__restrict dir)
{
    const double speed = std::sqrt(dot_product<N>(v, v));
    double tmp = 1.2;
    if (speed > 95. / 3) {
        tmp = 50 / (speed + 10);
    }
    for (int i = 0; i < N; ++i) {
        v[i] = tmp * v[i] + 300 * dir[i];
    }
}

inline double tau_g_to_p(double tau_g)
{
    return 0.001 * std::floor(1000 * tau_g);
}

void water_vel(double *__restrict v, double speed, const double *__restrict a, double geomfric, double M, double ke_tau_M_A)
{
    const double m = 0.8 * M;
    const double gamma2 = m - geomfric * speed;
    if (gamma2 <= 0 || m < 0.1) {
        return;
    }

    const double mu = std::min(0.8 * ke_tau_M_A, gamma2);
    for (int i = 0; i < 3; ++i) {
        v[i] = geomfric * v[i] + mu * a[i];
    }
}