UAVの姿勢推定に使用するプログラム。

Dependencies:   MPU6050_alter

Revision:
4:21a356ae0747
Parent:
3:3fa7882a5fd0
--- a/Vector/Vector.cpp	Wed Jul 24 12:00:01 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,178 +0,0 @@
-#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);
-}
-
-//以下ではclaen up関数は無効化してある。
-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;
-}