misc_math.c

Go to the documentation of this file.

/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
 * for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, see <http://www.gnu.org/licenses/>
 */

#include <stdbool.h>
#include <math.h>
#include "misc_math.h"          /* API declarations */
#include "physical_constants.h"

float bound_min_max(float val, float min, float max)
{
        if (val < min)
                return min;
        if (val > max)
                return max;
        return val;
}

float bound_sym(float val, float range)
{
        return (bound_min_max(val, -range, range));
}

float circular_modulus_deg(float err)
{
        float val = fmodf(err + 180.0f, 360.0f);

        // fmodf converts negative values into the negative remainder
        // so we must add 360 to make sure this ends up correct and
        // behaves like positive output modulus
        if (val < 0)
                val += 180;
        else
                val -= 180;

        return val;

}


float circular_modulus_rad(float err)
{
        float val = fmodf(err + PI, 2*PI);

        // fmodf converts negative values into the negative remainder
        // so we must add 360 to make sure this ends up correct and
        // behaves like positive output modulus
        if (val < 0)
                val += PI;
        else
                val -= PI;

        return val;

}

float expo3(float x, int32_t g)
{
        return (x * ((100 - g) / 100.0f) + powf(x, 3) * (g / 100.0f));
}

float expoM(float x, int32_t g, float exponent) {
        float out = x * (100 - g) * .01f;

        if (x > 0) {
                out += powapprox(x, exponent) * g * 0.01f;
        } else {
                out -= powapprox(-x, exponent) * g * 0.01f;
        }

        if (out < -1) {
                return -1;
        }

        if (out > 1) {
                return 1;
        }

        return out;
}

float interpolate_value(const float fraction, const float beginVal,
        const float endVal) {
        return beginVal + (endVal - beginVal) * bound_min_max(fraction, 0, 1);
}

float vectorn_magnitude(const float *v, int n)
{
        float sum=0;

        for (int i=0; i<n; i++) {
                sum += powf(v[i], 2);
        }

        return sqrtf(sum);
}

float vector3_distances(const float *actual,
        const float *desired, float *out, bool normalize)
{
        out[0] = desired[0] - actual[0];
        out[1] = desired[1] - actual[1];
        out[2] = desired[2] - actual[2];

        if (normalize) {
                float mag=vectorn_magnitude(out, 3);

                if (mag < 0.00001f) {
                        /* Just pick a direction. */
                        out[0] = 1.0; out[1] = 0.0; out[2] = 0.0;
                } else {
                        out[0] /= mag;  out[1] /= mag;  out[2] /= mag;
                }

                return mag;
        }

        return 0.0;
}

void vector2_clip(float *vels, float limit)
{
        float mag = vectorn_magnitude(vels, 2);    // only horiz component

        if (mag > limit && mag != 0) {
                float scale = limit / mag;
                vels[0] *= scale;
                vels[1] *= scale;
        }
}

void vector2_rotate(const float *original, float *out, float angle) {
        angle *= DEG2RAD;

        out[0] = original[0] * cosf(angle) - original[1] * sinf(angle);
        out[1] = original[0] * sinf(angle) + original[1] * cosf(angle);
}

float cubic_deadband(float in, float w, float b, float m, float r)
{
        // First get the nice linear bits -- outside the deadband-- out of
        // the way.
        if (in <= -w) {
                return in+w-r;
        } else if (in >= w) {
                return in-w+r;
        }

        return powf(m*in, 3)+b*in;
}

void cubic_deadband_setup(float w, float b, float *m, float *r)
{
        /* So basically.. we want the function to be tangent to the
         ** linear sections-- have a slope of 1-- at -w and w.  In the
         ** middle we want a slope of b.   So the cube here does all the
         ** work b isn't doing. */
        *m = cbrtf((1-b)/(3*powf(w,2)));

        *r = powf(*m*w, 3)+b*w;
}

float linear_interpolate(float const input, float const * curve, uint8_t num_points, const float input_min, const float input_max)
{
        // shift our input [min,max] into the typical range [0,1]
        float scale = fmaxf( (input - input_min) / (input_max - input_min), 0.0f) * (float) (num_points - 1);
        // select a starting bin via truncation
        int idx1 = scale;
        // save the offset from the starting bin for linear interpolation
        scale -= (float)idx1;
        // select an ending bin (linear interpolation occurs between starting and ending bins)
        int idx2 = idx1 + 1;
        // if the ending bin bin is above the last bin
        if (idx2 >= num_points) {
                //clamp to highest entry in table
                idx2 = num_points -1;
                // if the starting bin is above the last bin
                if (idx1 >= num_points) {
                        // we no longer do interpolation; instead, we just select the max point on the curve
                        return curve[num_points-1];
                }
        }
        return curve[idx1] * (1.0f - scale) + curve[idx2] * scale;
}

static uint32_t random_state[4] = { 0xdeadbeef, 0xfeedfeed, 0xcafebabe, 1234 };

void randomize_addseed(uint32_t seed)
{
        randomize_int(0);
        random_state[0] ^= seed;
}

static uint32_t randomize_int32()
{
        /* TODO: Not re-entrant.  Need to think how best to handle this
         * once there's more users of this API.
         */

        /* Xorshift 128 bit variant RNG */
        uint32_t t = random_state[3] ^ (random_state[3] << 11);
        t ^= t >> 8;
        t ^= random_state[0] ^ (random_state[0] >> 19);

        random_state[3] = random_state[2];
        random_state[2] = random_state[1];
        random_state[1] = random_state[0];
        random_state[0] = t;

        return t;
}

uint32_t randomize_int(uint32_t interval)
{
        /* TODO: This could improve a lot from constant static
         * value evaluation in the optimizer, so it may belong in
         * the header as inline...
         */
        uint32_t thresh = UINT32_MAX;
        uint32_t rand_val;

        if (interval) {
                uint32_t remainder = UINT32_MAX % interval;

                if (remainder != (interval - 1)) {
                        thresh -= remainder + 1;
                }
        }

        /* All of this junk is here so we can avoid biases by retrying
         * in the case of really large drawn numbers */
        do {
                rand_val = randomize_int32();
        } while (rand_val > thresh);

        if (interval == 0) {
                return rand_val;
        }

        return rand_val % interval;
}

void apply_channel_deadband(float *value, float deadband)
{
        if (deadband < 0.0001f) return; /* ignore tiny deadband value */
        if (deadband >= 0.85f) return;  /* reject nonsensical db values */

        if (fabsf(*value) < deadband) {
                *value = 0.0f;
        } else {
                if (*value > 0.0f) {
                        *value -= deadband;
                } else {
                        *value += deadband;
                }

                *value /= (1 - deadband);
        }
}