MPU6050のサンプルプログラム2

Dependencies:   ConfigFile SDFileSystem mbed

Fork of LAURUS_program by LAURUS

Vector/Vector.cpp

Committer:
ojan
Date:
2015-06-24
Revision:
23:79cdc1432160
Parent:
13:df1e8a650185

File content as of revision 23:79cdc1432160:

#include "myConstants.h"
#include "Vector.h"


Vector::Vector(int dim) : dim(dim), components(0){
    components = new float[dim];
    if (!components) error("Memory Allocation Error");
    for(int i=0; i<dim; i++) components[i] = 0.0f;
}


Vector::~Vector() {
    delete[] components;
}

Vector::Vector(const Vector& v) : dim(v.dim), components(0) {
    components = new float[dim];
    if (!components) error("Memory Allocation Error");
    memcpy(components, v.GetpComponents(), sizeof(float)*dim);
}

Vector& Vector::operator=(const Vector& v) {
    if (this == &v) return *this;
    dim = v.dim;
    delete[] components;
    components = new float[dim];
    if (!components) error("Memory Allocation Error");
    memcpy(components, v.GetpComponents(), sizeof(float)*dim);

    return *this;
}

Vector Vector::operator+() {
    return *this;
}

Vector Vector::operator-() {
    Vector retVec(*this);
    retVec *= -1;
    return retVec;
}

Vector& Vector::operator*=(float c) {
    for (int i = 0; i < dim; i++) {
        components[i] *= c;
    }

    return *this;
}

Vector& Vector::operator/=(float c) {
    if (fabs(c) < NEARLY_ZERO) error("Division by Zero");
    for (int i = 0; i < dim; i++) {
        components[i] /= c;
    }

    return *this;
}

Vector& Vector::operator+=(const Vector& v) {
    if (dim != v.dim) error("failed to add: Irregular Dimention");
    for (int i = 0; i < dim; i++) {
        components[i] += v.components[i];
    }

    this->CleanUp();

    return *this;
}

Vector& Vector::operator-=(const Vector& v) {
    if (dim != v.dim) error("failed to subtract: Irregular Dimention");
    for (int i = 0; i < dim; i++) {
        components[i] -= v.components[i];
    }

    this->CleanUp();

    return *this;
}

void Vector::SetComp(int dimNo, float val) {
    if (dimNo > dim) error("Index Out of Bounds Error");
    components[dimNo-1] = val;
}

void Vector::SetComps(float* pComps) {
    memcpy(components, pComps, sizeof(float) * dim);
}

float Vector::GetNorm() const {
    float norm = 0.0f;
    for (int i = 0; i < dim; i++) {
        norm += components[i] * components[i];
    }
    return sqrt(norm);
}

Vector Vector::Normalize() const {
    float norm = GetNorm();
    Vector temp(*this);
    for (int i = 0; i < dim; i++) {
        temp.components[i] /= norm;
    }
    temp.CleanUp();
    return temp;
}

Vector Vector::GetParaCompTo(Vector v) {
    Vector norm_v = v.Normalize();
    return (*this * norm_v) * norm_v;
}

Vector Vector::GetPerpCompTo(Vector v) {
    return (*this - this->GetParaCompTo(v));
}

void Vector::CleanUp() {
    float maxComp = 0.0f;
    for (int i = 0; i < dim; i++) {
        if (fabs(components[i]) > maxComp) maxComp = fabs(components[i]);
    }
    if (maxComp > NEARLY_ZERO) {
        for (int i = 0; i < dim; i++) {
            if (fabs(components[i]) / maxComp < ZERO_TOLERANCE) components[i] = 0.0f;
        }
    }
}

Vector operator+(const Vector& lhv, const Vector& rhv) {
    Vector retVec(lhv);
    retVec += rhv;
    return retVec;
}

Vector operator-(const Vector& lhv, const Vector& rhv) {
    Vector retVec(lhv);
    retVec -= rhv;
    return retVec;
}

Vector Cross(const Vector& lhv, const Vector& rhv) {
    if (lhv.GetDim() != 3) error("failed to cross: variable 'dim' must be 3");
    if (lhv.GetDim() != rhv.GetDim()) error("failed to cross: Irregular Dimention");

    Vector retVec(lhv.GetDim());

    for (int i = 0; i < lhv.GetDim(); i++) {
        retVec.SetComp(i + 1, lhv.GetComp((i + 1) % 3 + 1) * rhv.GetComp((i + 2) % 3 + 1)
            - lhv.GetComp((i + 2) % 3 + 1) * rhv.GetComp((i + 1) % 3 + 1));
    }

    return retVec;
}

Vector operator*(const float c, const Vector& rhv) {
    Vector retVec(rhv);
    retVec *= c;
    return retVec;
}

Vector operator*(const Vector& lhv, const float c) {
    Vector retVec(lhv);
    retVec *= c;
    return retVec;
}

float operator*(const Vector& lhv, const Vector& rhv) {
    if (lhv.GetDim() != rhv.GetDim()) error("Irregular Dimention");
    float retVal = 0.0f;

    for (int i = 1; i <= lhv.GetDim(); i++) {
        retVal += lhv.GetComp(i) * rhv.GetComp(i);
    }

    return retVal;
}