File content as of revision 1:79adfaec299a:

/*
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();
    
    // do each integral seperatly in the future
    
    accel.x = ax;
    accel.y = ay;
    accel.z = az;
    
    vel.x += (accel.x/dataRate);
    vel.y += (accel.y/dataRate);
    vel.z += (accel.z/dataRate);
    
    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;  
}