turbomath.cpp

Go to the documentation of this file.

/*
 *
 * BSD 3-Clause License
 *
 * Copyright (c) 2017, James Jackson - BYU MAGICC Lab, Provo UT
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * * Redistributions of source code must retain the above copyright notice, this
 *   list of conditions and the following disclaimer.
 *
 * * Redistributions in binary form must reproduce the above copyright notice,
 *   this list of conditions and the following disclaimer in the documentation
 *   and/or other materials provided with the distribution.
 *
 * * Neither the name of the copyright holder nor the names of its
 *   contributors may be used to endorse or promote products derived from
 *   this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include <turbomath/turbomath.h>
#include <string.h>

namespace turbomath
{

Vector::Vector(const Vector &v) :
  x(*arr), y(*(arr+1)), z(*(arr+2))
{
  arr[0] = v.arr[0];
  arr[1] = v.arr[1];
  arr[2] = v.arr[2];
}

Vector::Vector() :
  x(*arr), y(*(arr+1)), z(*(arr+2))
{}

Vector::Vector(float x_, float y_, float z_) :
  arr{x_, y_, z_},
  x(*arr), y(*(arr+1)), z(*(arr+2))
{}


float Vector::norm() const
{
  return 1.0f/inv_sqrt(x*x + y*y + z*z);
}


float Vector::sqrd_norm() const
{
  return x*x + y*y + z*z;
}


Vector &Vector::normalize()
{
  float recip_norm = inv_sqrt(x*x + y*y + z*z);
  x *= recip_norm;
  y *= recip_norm;
  z *= recip_norm;
  return *this;
}


Vector Vector::normalized() const
{
  float recip_norm = inv_sqrt(x*x + y*y + z*z);
  Vector out(x*recip_norm, y*recip_norm, z*recip_norm);
  return out;
}

Vector &Vector::operator =(const Vector &v)
{
  arr[0] = v.arr[0];
  arr[1] = v.arr[1];
  arr[2] = v.arr[2];
  return *this;
}


Vector Vector::operator+(const Vector &v) const
{
  return Vector(x + v.x, y + v.y, z + v.z);
}


Vector Vector::operator-(const Vector &v) const
{
  return Vector(x - v.x, y - v.y, z - v.z);
}


Vector &Vector::operator +=(const Vector &v)
{
  x += v.x;
  y += v.y;
  z += v.z;
  return *this;
}


Vector &Vector::operator -=(const Vector &v)
{
  x -= v.x;
  y -= v.y;
  z -= v.z;
  return *this;
}


Vector Vector::operator *(float s) const
{
  return Vector(x*s, y*s, z*s);
}


Vector Vector::operator /(float s) const
{
  return Vector(x/s, y/s, z/s);
}


Vector &Vector::operator *=(float s)
{
  x *= s;
  y *= s;
  z *= s;
  return *this;
}


Vector &Vector::operator /=(float s)
{
  x /= s;
  y /= s;
  z /= s;
  return *this;
}


float Vector::dot(const Vector &v) const
{
  return x*v.x + y*v.y + z*v.z;
}


Vector Vector::cross(const Vector &v) const
{
  return Vector(y * v.z - z * v.y,
                z * v.x - x * v.z,
                x * v.y - y * v.x);
}

Quaternion::Quaternion() : w(1.0f), x(0.0f), y(0.0f), z(0.0f)
{}

Quaternion::Quaternion(float w_, float x_, float y_, float z_) : w(w_), x(x_), y(y_), z(z_)
{}

Quaternion::Quaternion(const Vector &u, const Vector &v)
{
  from_two_unit_vectors(u, v);
}

Quaternion::Quaternion(float roll, float pitch, float yaw)
{
  from_RPY(roll, pitch, yaw);
}

Quaternion &Quaternion::normalize()
{
  float recip_norm = inv_sqrt(w*w + x*x + y*y + z*z);
  w *= recip_norm;
  x *= recip_norm;
  y *= recip_norm;
  z *= recip_norm;

  // Make sure the quaternion is canonical (w is always positive)
  if (w < 0.0f)
  {
    w *= -1.0f;
    x *= -1.0f;
    y *= -1.0f;
    z *= -1.0f;
  }

  return *this;
}

Quaternion Quaternion::operator *(const Quaternion &q) const
{
  return Quaternion(w*q.w - x*q.x - y*q.y - z*q.z,
                    w*q.x + x*q.w - y*q.z + z*q.y,
                    w*q.y + x*q.z + y*q.w - z*q.x,
                    w*q.z - x*q.y + y*q.x + z*q.w);
}

Quaternion &Quaternion::operator *=(const Quaternion &q)
{
  w = w*q.w - x*q.x - y*q.y - z*q.z;
  x = w*q.x + x*q.w - y*q.z + z*q.y;
  y = w*q.y + x*q.z + y*q.w - z*q.x;
  z = w*q.z - x*q.y + y*q.x + z*q.w;
  return *this;
}

Vector Quaternion::rotate(const Vector &v) const
{
  return Vector((1.0f - 2.0f*y*y - 2.0f*z*z) * v.x + (2.0f*(x*y + w*z))*v.y + 2.0f*(x*z - w*y)*v.z,
                (2.0f*(x*y - w*z)) * v.x + (1.0f - 2.0f*x*x - 2.0f*z*z) * v.y + 2.0f*(y*z + w*x)*v.z,
                (2.0f*(x*z + w*y)) * v.x + 2.0f*(y*z - w*x)*v.y + (1.0f - 2.0f*x*x - 2.0f*y*y)*v.z);
}

Vector Quaternion::operator *(const Vector &v) const
{
  return rotate(v);
}

Quaternion Quaternion::inverse() const
{
  return Quaternion(w, -x, -y, -z);
}

Quaternion &Quaternion::invert()
{
  x *= -1.0f;
  y *= -1.0f;
  z *= -1.0f;
  return *this;
}

Quaternion &Quaternion::from_two_unit_vectors(const Vector &u, const Vector &v)
{
  // Adapted From the Ogre3d source code
  // https://bitbucket.org/sinbad/ogre/src/9db75e3ba05c/OgreMain/include/OgreVector3.h?fileviewer=file-view-default#cl-651
  float d = u.dot(v);
  if (d >= 1.0f)
  {
    w = 1.0f;
    x = 0.0f;
    y = 0.0f;
    z = 0.0f;
    return *this;
  }
  else
  {
    float invs = inv_sqrt(2.0f*(1.0f+d));
    Vector xyz = u.cross(v)*invs;
    w = 0.5f/invs;
    x = xyz.x;
    y = xyz.y;
    z = xyz.z;
  }
  normalize();
  return *this;
}

Quaternion &Quaternion::from_RPY(float roll, float pitch, float yaw)
{
  // p 259 of "Small unmanned aircraft: Theory and Practice" by Randy Beard and Tim McLain
  float cp = turbomath::cos(roll/2.0);
  float sp = turbomath::sin(roll/2.0);
  float ct = turbomath::cos(pitch/2.0);
  float st = turbomath::sin(pitch/2.0);
  float cs = turbomath::cos(yaw/2.0);
  float ss = turbomath::sin(yaw/2.0);

  w = cs*ct*cp + ss*st*sp;
  x = cs*ct*sp - ss*st*cp;
  y = cs*st*cp + ss*ct*sp;
  z = ss*ct*cp - cs*st*sp;

  normalize();
  return *this;
}

Vector Quaternion::boxminus(const Quaternion &q) const
{
  Quaternion dq = q.inverse() * (*this);
  if (dq.w < 0.0)
  {
    dq.w *= -1.0;
    dq.x *= -1.0;
    dq.y *= -1.0;
    dq.z *= -1.0;
  }
  return log(dq);
}

void Quaternion::get_RPY(float *roll, float *pitch, float *yaw) const
{
  *roll = turbomath::atan2(2.0f * (w*x + y*z), 1.0f - 2.0f * (x*x + y*y));
  *pitch = turbomath::asin(2.0f*(w*y - z*x));
  *yaw = turbomath::atan2(2.0f * (w*z + x*y), 1.0f - 2.0f * (y*y + z*z));
}



#ifndef M_PI
#define M_PI 3.14159265359
#endif

static const float atan_max_x = 1.000000;
static const float atan_min_x = 0.000000;
static const float atan_scale_factor = 41720.240162;
static const int16_t atan_num_entries = 125;
static const int16_t atan_lookup_table[125] =
{
  0,    334,    667,    1001,   1335,   1668,   2001,   2334,   2666,   2999,
  3331, 3662,   3993,   4323,   4653,   4983,   5311,   5639,   5967,   6293,
  6619, 6944,   7268,   7592,   7914,   8235,   8556,   8875,   9194,   9511,
  9827, 10142,  10456,  10768,  11080,  11390,  11699,  12006,  12313,  12617,
  12921,        13223,  13524,  13823,  14120,  14417,  14711,  15005,  15296,  15586,
  15875,        16162,  16447,  16731,  17013,  17293,  17572,  17849,  18125,  18399,
  18671,        18941,  19210,  19477,  19742,  20006,  20268,  20528,  20786,  21043,
  21298,        21551,  21802,  22052,  22300,  22546,  22791,  23034,  23275,  23514,
  23752,        23988,  24222,  24454,  24685,  24914,  25142,  25367,  25591,  25814,
  26034,        26253,  26471,  26686,  26900,  27113,  27324,  27533,  27740,  27946,
  28150,        28353,  28554,  28754,  28952,  29148,  29343,  29537,  29728,  29919,
  30108,        30295,  30481,  30665,  30848,  31030,  31210,  31388,  31566,  31741,
  31916,        32089,  32260,  32431,  32599,
};

static const float asin_max_x = 1.000000;
static const float asin_min_x = 0.000000;
static const float asin_scale_factor = 20860.120081;
static const int16_t asin_num_entries = 200;
static const int16_t asin_lookup_table[200] =
{
  0,    104,    209,    313,    417,    522,    626,    730,    835,    939,
  1043, 1148,   1252,   1357,   1461,   1566,   1671,   1775,   1880,   1985,
  2090, 2194,   2299,   2404,   2509,   2614,   2720,   2825,   2930,   3035,
  3141, 3246,   3352,   3458,   3564,   3669,   3775,   3881,   3988,   4094,
  4200, 4307,   4413,   4520,   4627,   4734,   4841,   4948,   5056,   5163,
  5271, 5379,   5487,   5595,   5703,   5811,   5920,   6029,   6138,   6247,
  6356, 6465,   6575,   6685,   6795,   6905,   7015,   7126,   7237,   7348,
  7459, 7570,   7682,   7794,   7906,   8019,   8131,   8244,   8357,   8471,
  8584, 8698,   8812,   8927,   9042,   9157,   9272,   9388,   9504,   9620,
  9737, 9854,   9971,   10089,  10207,  10325,  10444,  10563,  10682,  10802,
  10922,        11043,  11164,  11285,  11407,  11530,  11652,  11776,  11899,  12024,
  12148,        12273,  12399,  12525,  12652,  12779,  12907,  13035,  13164,  13293,
  13424,        13554,  13686,  13817,  13950,  14083,  14217,  14352,  14487,  14623,
  14760,        14898,  15036,  15176,  15316,  15457,  15598,  15741,  15885,  16029,
  16175,        16321,  16469,  16618,  16767,  16918,  17070,  17224,  17378,  17534,
  17691,        17849,  18009,  18170,  18333,  18497,  18663,  18830,  19000,  19171,
  19343,        19518,  19695,  19874,  20055,  20239,  20424,  20613,  20803,  20997,
  21194,        21393,  21596,  21802,  22012,  22225,  22443,  22664,  22891,  23122,
  23359,        23601,  23849,  24104,  24366,  24637,  24916,  25204,  25504,  25816,
  26143,        26485,  26847,  27232,  27644,  28093,  28588,  29149,  29814,  30680,
};


static const float max_pressure = 106598.405011;
static const float min_pressure = 69681.635473;
static const float pressure_scale_factor = 10.754785;
static const int16_t pressure_num_entries = 200;
static const int16_t pressure_lookup_table[200] =
{
  32767,        32544,  32321,  32098,  31876,  31655,  31434,  31213,  30993,  30773,
  30554,        30335,  30117,  29899,  29682,  29465,  29248,  29032,  28816,  28601,
  28386,        28172,  27958,  27745,  27532,  27319,  27107,  26895,  26684,  26473,
  26263,        26053,  25843,  25634,  25425,  25217,  25009,  24801,  24594,  24387,
  24181,        23975,  23769,  23564,  23359,  23155,  22951,  22748,  22544,  22341,
  22139,        21937,  21735,  21534,  21333,  21133,  20932,  20733,  20533,  20334,
  20135,        19937,  19739,  19542,  19344,  19148,  18951,  18755,  18559,  18364,
  18169,        17974,  17780,  17586,  17392,  17199,  17006,  16813,  16621,  16429,
  16237,        16046,  15855,  15664,  15474,  15284,  15095,  14905,  14716,  14528,
  14339,        14151,  13964,  13777,  13590,  13403,  13217,  13031,  12845,  12659,
  12474,        12290,  12105,  11921,  11737,  11554,  11370,  11188,  11005,  10823,
  10641,        10459,  10278,  10096,  9916,   9735,   9555,   9375,   9195,   9016,
  8837, 8658,   8480,   8302,   8124,   7946,   7769,   7592,   7415,   7239,
  7063, 6887,   6711,   6536,   6361,   6186,   6012,   5837,   5664,   5490,
  5316, 5143,   4970,   4798,   4626,   4454,   4282,   4110,   3939,   3768,
  3597, 3427,   3257,   3087,   2917,   2748,   2578,   2409,   2241,   2072,
  1904, 1736,   1569,   1401,   1234,   1067,   901,    734,    568,    402,
  237,  71,     -94,    -259,   -424,   -588,   -752,   -916,   -1080,  -1243,
  -1407,        -1570,  -1732,  -1895,  -2057,  -2219,  -2381,  -2543,  -2704,  -2865,
  -3026,        -3187,  -3347,  -3507,  -3667,  -3827,  -3987,  -4146,  -4305,  -4464,
};

static const float sin_max_x = 3.141593;
static const float sin_min_x = 0.000000;
static const float sin_scale_factor = 32767.000000;
static const int16_t sin_num_entries = 125;
static const int16_t sin_lookup_table[125] =
{
  0,    823,    1646,   2468,   3289,   4107,   4922,   5735,   6544,   7349,
  8149, 8944,   9733,   10516,  11293,  12062,  12824,  13578,  14323,  15059,
  15786,        16502,  17208,  17904,  18588,  19260,  19920,  20568,  21202,  21823,
  22431,        23024,  23602,  24166,  24715,  25247,  25764,  26265,  26749,  27216,
  27666,        28099,  28513,  28910,  29289,  29648,  29990,  30312,  30615,  30899,
  31163,        31408,  31633,  31837,  32022,  32187,  32331,  32454,  32558,  32640,
  32702,        32744,  32764,  32764,  32744,  32702,  32640,  32558,  32454,  32331,
  32187,        32022,  31837,  31633,  31408,  31163,  30899,  30615,  30312,  29990,
  29648,        29289,  28910,  28513,  28099,  27666,  27216,  26749,  26265,  25764,
  25247,        24715,  24166,  23602,  23024,  22431,  21823,  21202,  20568,  19920,
  19260,        18588,  17904,  17208,  16502,  15786,  15059,  14323,  13578,  12824,
  12062,        11293,  10516,  9733,   8944,   8149,   7349,   6544,   5735,   4922,
  4107, 3289,   2468,   1646,   823
};

float fsign(float y)
{
  return (0.0f < y) - (y < 0.0f);
}

float cos(float x)
{
  return sin(M_PI/2.0 - x);
}

float sin(float x)
{
  // wrap down to +/x PI
  while (x > M_PI)
    x -= 2.0*M_PI;

  while (x <= -M_PI)
    x += 2.0*M_PI;

  // sin is symmetric
  if (x < 0)
    return -1.0*sin(-x);

  // wrap onto (0, PI)
  if (x > M_PI)
    return -1.0*sin(x - M_PI);

  // Now, all we have left is the range 0 to PI, use the lookup table
  float t = (x - sin_min_x)/(sin_max_x - sin_min_x) * static_cast<float>(sin_num_entries);
  int16_t index = static_cast<int16_t>(t);
  float delta_x = t - index;

  if (index >= sin_num_entries)
    return sin_lookup_table[sin_num_entries - 1]/sin_scale_factor;
  else if (index < sin_num_entries - 1)
    return sin_lookup_table[index]/sin_scale_factor + delta_x * (sin_lookup_table[index + 1] -
           sin_lookup_table[index])/sin_scale_factor;
  else
    return sin_lookup_table[index]/sin_scale_factor + delta_x * (sin_lookup_table[index] - sin_lookup_table[index -
           1])/sin_scale_factor;
}


float atan(float x)
{
  // atan is symmetric
  if (x < 0)
  {
    return -1.0*atan(-1.0*x);
  }
  // This uses a sweet identity to wrap the domain of atan onto (0,1)
  if (x > 1.0)
  {
    return M_PI/2.0 - atan(1.0/x);
  }

  float t = (x - atan_min_x)/(atan_max_x - atan_min_x) * static_cast<float>(atan_num_entries);
  int16_t index = static_cast<int16_t>(t);
  float delta_x = t - index;

  if (index >= atan_num_entries)
    return atan_lookup_table[atan_num_entries-1]/atan_scale_factor;
  else if (index < atan_num_entries - 1)
    return atan_lookup_table[index]/atan_scale_factor + delta_x * (atan_lookup_table[index + 1] -
           atan_lookup_table[index])/atan_scale_factor;
  else
    return atan_lookup_table[index]/atan_scale_factor + delta_x * (atan_lookup_table[index] - atan_lookup_table[index -
           1])/atan_scale_factor;
}


float atan2(float y, float x)
{
  // algorithm from wikipedia: https://en.wikipedia.org/wiki/Atan2
  if (x == 0.0)
  {
    if (y < 0.0)
    {
      return - M_PI/2.0;
    }
    else if (y > 0.0)
    {
      return M_PI/2.0;
    }
    else
    {
      return 0.0;
    }
  }

  float arctan = atan(y/x);

  if (x < 0.0)
  {
    if (y < 0)
    {
      return arctan - M_PI;
    }
    else
    {
      return arctan + M_PI;
    }
  }

  else
  {
    return arctan;
  }
}


float asin(float x)
{
  if (x < 0.0)
  {
    return -1.0*asin(-1.0*x);
  }

  float t = (x - asin_min_x)/(asin_max_x - asin_min_x) * static_cast<float>(asin_num_entries);
  int16_t index = static_cast<int16_t>(t);
  float delta_x = t - index;

  if (index >= asin_num_entries)
    return asin_lookup_table[asin_num_entries - 1]/asin_scale_factor;
  else if (index < asin_num_entries - 1)
    return asin_lookup_table[index]/asin_scale_factor + delta_x * (asin_lookup_table[index + 1] -
           asin_lookup_table[index])/asin_scale_factor;
  else
    return asin_lookup_table[index]/asin_scale_factor + delta_x * (asin_lookup_table[index] - asin_lookup_table[index -
           1])/asin_scale_factor;
}

float alt(float press)
{

  if (press < max_pressure && press > min_pressure)
  {
    float t = (press - min_pressure)/(max_pressure - min_pressure) * static_cast<float>(pressure_num_entries);
    int16_t index = static_cast<int16_t>(t);
    float delta_x = t - index;

    if (index >= pressure_num_entries)
      return asin_lookup_table[pressure_num_entries - 1]/pressure_scale_factor;
    else if (index < pressure_num_entries - 1)
      return pressure_lookup_table[index]/pressure_scale_factor + delta_x * (pressure_lookup_table[index + 1] -
             pressure_lookup_table[index])/pressure_scale_factor;
    else
      return pressure_lookup_table[index]/pressure_scale_factor + delta_x * (pressure_lookup_table[index] -
             pressure_lookup_table[index - 1])/pressure_scale_factor;
  }
  else
    return 0.0;
}

float fabs(float x)
{
  if (x < 0)
    return -x;
  else
    return x;

}

float inv_sqrt(float x)
{
  volatile float x2;
  volatile float_converter_t y, i;
  const float threehalfs = 1.5F;

  x2 = x * 0.5F;
  y.fvalue  = x;
  i.ivalue  = y.ivalue;                             // evil floating point bit level hacking
  i.ivalue  = 0x5f3759df - (i.ivalue >> 1);
  y.fvalue = i.fvalue;
  y.fvalue  = y.fvalue * (threehalfs - (x2 * y.fvalue * y.fvalue));       // 1st iteration
  y.fvalue  = y.fvalue * (threehalfs - (x2 * y.fvalue * y.fvalue));       // 2nd iteration, this can be removed

  return fabs(y.fvalue);
}

} // namespace turbomath