vector3

Easy vector, quaternion library designed for rotations and sensor fusion filters in C++14. It is also mobile device agnostic as mobile devices,such as iphones use y axis front and pitch is around X axis.

quaternions

quaternion from euler all the angles should be in radians

auto roll = rad(40.0);
auto pitch = rad(40.0);
auto yaw = rad(40.0);
auto q = Quaternion(roll,pitch,yaw);

change euler order in constructor of specific quaternion,ex: first roll then pitch then yaw

auto roll = rad(40.0);
auto pitch = rad(40.0);
auto yaw = rad(40.0);
auto q = Quaternion(roll,pitch,yaw,RotationOrder::rpy);

to euler as roll,pitch,yaw

auto roll =  Quaternion(roll,pitch,yaw).roll();
auto pitch =  Quaternion(roll,pitch,yaw).pitch();
auto yaw =  Quaternion(roll,pitch,yaw).yaw();

to euler as Vector3

Vector3 euler =  Quaternion(roll,pitch,yaw).euler();

concatanating rotations is as simple as multiplying quaternions (q1,q2,q3,q4) in respective order q1*q2*q3*q4
rotating vector3 is as simple as multiplying with quaternion v1*q1
reverse rotation is negating -q1
rotation between two quaternions -q1*q2
slerp is as simple as multiplying quaternion with scalar ex half angle rotation of q1 is 0.5*q1
mid rotation between two quaternions is q1*(0.5*(-q1*q2))

vectors

constructor and constant static variables Vector3 v(0.5,0.7,0.3);
from constant static variables Vector3 v = Vector3::X;
c++11 version auto v = Vector3::X;

to Vector2

Vector2 xy = Vector3::X.xy(); 
auto xz = Vector3::X.xz(); 
auto yz = Vector3::X.yz();

negating auto negx = -Vector3::X;
arithmetic operations auto v = (0.3*(Vector3::X+Vector3::Y)).normalized();
multiplying with scalar all xyz auto v2 = v1*0.5

multiplying with vector

    Vector3 coef(0.5,0.3,0.1);
    auto v = coef*Vector3::X;

angle between two vectors

    auto angleInRad = Vector3::X.angle(Vector3::Y);
    auto angleInDegree = deg(angleInRad);

using in filtering

   auto beta = 0.1;
   auto result = Vector3::X*beta + Vector3::Y*(1-beta);

rotation quaternion between two vectors Quaternion q = Vector3::X.rotation(Vector3::Y);
rotation quaternion around specific axis (not that there are many axis that can yield to same roation result) Quaternion q = Vector3::X.rotation(Vector3::Y,-Vector3::Z);
to array vector<scalar> v = Vector3::X;
from array or initializer_list Vector3 v = {0.2,0.30,0.5};

from vector iterator

   vector<scalar> vec = {0.2,0.30,0.5,0.45};
   Vector3 v = vec.begin()+1;

vector3 arithmetic helper functions

degree to radian and vice versa conversion

   inline scalar rad(scalar deg);
   inline scalar deg(scalar rad);

max,min,pow,abs

    inline Vector3 pow(const Vector3& v,scalar p);
    inline Vector3 abs(const Vector3& v);
    inline Vector3 max(const Vector3& v1,const Vector3& v2);
    inline Vector3 min(const Vector3& v1,const Vector3& v2);

mobile device vs others

mobile devices definition of roll,pitch and reference frame XYZ is different than other engineering areas, define MOBILE if your application is for mobile or mobile data ``` #define MOBILE ````

it is also automatically defined in header as

#ifdef TARGET_OS_IOS  or TARGET_IPHONE_SIMULATOR  or __ANDROID__             
    #define MOBILE
#endif

sensors

sensor type

enum SensorType{
    none = 0,accelerometer,gyroscope,magnetometer,coreMotion,position,velocity,userAcceleration,pixels
};

sensor data

class SensorData{
    public:
    double time = 0; // in secs
    SensorType type = SensorType::none;
    Vector3 v = Vector3::Zero;
 };

file operations

File read/write operations are simplified

reading vector3 list from file

    vector<Vector3> rtn;
    ifstream(path)>>rtn;

reading SensorData list from file

    vector<SensorData> rtn;
    ifstream(path)>>rtn;

writing vector3 list to file

    vector<Vector3> rtn = {Vector3::X,Vector3::Y,Vector3::Z};
    ofstream(path)<<rtn<<endl;

writing SensorData list to file

    vector<SensorData> rtn;
    ofstream(path)<<rtn<<endl;

n dimensional ternary search

use n dimensional search when local minima is equal to global minima, otherwise it probably will find one of local minimas
```
vector<scalar> ternary(vector<scalar> start, vector<scalar> end, function<scalar(vector<scalar>)> eval);
```

exmaple code for finding calibration parameters of magnetometer

void findMagBiasCollectively(const vector<SensorData>& list){
   if(list.size()==0) return;
   auto mlist = SensorData::filter(list, SensorType::magnetometer);
   Vector3 mx = mlist[0].v,mn=mlist[0].v;
   for(auto i:mlist){
       mx = vector3::max(i.v,mx);
       mn = vector3::min(i.v,mn);
   }

   auto biasList =
   vector3::ternary( mn.array()   , mx , [&](vector<scalar> p){
       Vector3 v(p);
       auto r = 0.0;
       for(auto i : mlist){
           r += ((i.v - v)).length();
       }
       r /= mlist.size();
       auto error = 0.0;
       for(auto i : mlist){
           auto t = ((i.v - v)).length() - r;
           error += t*t;
       }
       return error;
   });
   magBias = biasList;
}

Example codes:

    using namespace vector3;
    
    auto v = Vector3::X;
    
    auto roll = rad(40.0);
    auto pitch = rad(40.0);
    auto yaw = rad(40.0);
    
    auto q = Quaternion(roll,pitch,yaw);
    
    q = Quaternion(roll,pitch,yaw,RotationOrder::rpy);
    
    q = q*Quaternion(roll,0,0);
    
    Vector3 to = v*q;
    
    Quaternion q1 = v.rotation(to);
    
    std::cout << "roll: " << deg(q.roll())<<", pitch: " << deg(q.pitch()) 
              << ", yaw: " << deg(q.yaw()) << endl;
    
    std::cout << "roll: " << deg(to.roll())<<", pitch: " << deg(to.pitch()) 
              << ", yaw: " << deg(to.yaw()) << endl;

by Maksim Piriyev @ https://dron.ee

Dronee/vector3