File content as of revision 3:7e7159e2f589:

/*
kinematics library
by clemo


go to header file for instructions


*/


#include "kinematics.h"
#include <math.h>


//////////////////
//Kinetics Class//
//////////////////
Kinematics::Kinematics(int dataRate_){
    
    this->dataRate = dataRate_;
    this->inv_dataRate = 1/dataRate_;
    timestamp = 0;
    
    
    
    accel.reset();
    vel.reset();
    pos.reset();
}

double Kinematics::uptadeDistTraveled(){
    //distTraveled += vel.getabs();
    return distTraveled;
}

int Kinematics::updateTime(){
    timestamp++;
    return timestamp;
}

void Kinematics::updateAll(double ax, double ay, double az){
    updateTime();
    accel.set(ax,ay,az);
    updateVel();
    updatePos();
    
}

void Kinematics::updateAll(LinAccel accel_){
    updateTime();
    accel = accel_;
    updateVel();
    updatePos();
    
}

void Kinematics::updateVel(){
    vel.x += (accel.x/dataRate);
    vel.y += (accel.y/dataRate);
    vel.z += (accel.z/dataRate);
}

void Kinematics::updatePos(){
    pos.x += vel.x/dataRate;
    pos.y += vel.y/dataRate;
    pos.z += vel.z/dataRate;  
}

void Kinematics::resetAll(){
    accel.reset();
    vel.reset();
    pos.reset();    
    timestamp = 0;
}

int Kinematics::getTime(){
    return timestamp;
}

double Kinematics::getTimeSec(){
    return timestamp/dataRate;
}

//////////////////////
//Acceleration class//
//////////////////////

LinAccel::LinAccel(){
    reset();
}

double LinAccel::getabs(){
    return sqrt(x*x+z*z+y*y);
    
}

void LinAccel::reset(){
    set(0.0, 0.0, 0.0);
}
    
void LinAccel::set(double ax, double ay, double az){
    x = ax; y = ay; z = az;  
}

//////////////////
//Velocity class//
//////////////////

Velocity::Velocity(){
    reset();
}

double Velocity::getabs(){
    return sqrt(x*x+z*z+y*y);
}

void Velocity::reset(){
    set(0.0, 0.0, 0.0);
}
    
void Velocity::set(double vx, double vy, double vz){
    x = vx; y = vy; z = vz;  
}

//////////////////
//Position class//
//////////////////
Position::Position(){
    reset();
}

double Position::getabs(){
    return sqrt(x*x+z*z+y*y);
}

double Position::getDistTo(double px, double py, double pz){
    double dx = px-x;
    double dy = py-y;
    double dz = pz-z;
    return sqrt(dx*dx+dy*dy+dz*dz);
}

void Position::reset(){
    set(0.0, 0.0, 0.0);
}
    
void Position::set(double px, double py, double pz){
    x = px; y = py; z = pz;  
}

/////////////////////
//Orientation class//
/////////////////////

Orientation::Orientation(){
    resetQuat();
    resetEuler();
}

void Orientation::setQuat(double qw_, double qx_, double qy_, double qz_){
    qw = qw_;
    qx = qx_;
    qy = qy_;
    qz = qz_;
}   
 
void Orientation::resetQuat(){
    setQuat(0,0,0,0);
}

void Orientation::setEuler(double r_, double p_, double y_){
    r = r_;
    p = p_;
    y = y_;
}

void Orientation::resetEuler(){
    setEuler(0,0,0);
}

void Orientation::setGRV(double grvX_, double grvY_, double grvZ_){
    grvX = grvX_;
    grvY = grvY_;
    grvZ = grvZ_;
}

void Orientation::resetGRV(){
    setGRV(0,0,0);
}
/////////////////
//RawData class//
/////////////////

RawData::RawData(){
    ax = 0;
    ay = 0;
    az = 0;
    resetMag();
}
void RawData::setMag(double mx_, double my_, double mz_){
    mx = mx_;
    my = my_;
    mz = mz_;
}

void RawData::resetMag(){
    setMag(0,0,0);
}

void RawData::setA(double ax_, double ay_, double az_){
    ax = ax_;
    ay = ay_;
    az = az_;
}

void RawData::resetA(){
    setA(0,0,0);
}

void RawData::setG(double gx_, double gy_, double gz_){
    gx = gx_;
    gy = gy_;
    gz = gz_;
}

void RawData::resetG(){
    setG(0,0,0);
}