Atsumi Toda / Mbed OS UAV_Logger1

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   MPU6050_alter

Vector/Vector.cpp@2:e6496a794bde, 2019-05-24 (annotated)

Committer:
Joeatsumi
Date:
Fri May 24 05:57:12 2019 +0000
Revision:
2:e6496a794bde
mpu6050 and GHC5883L for atitude estimation for UAV.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Joeatsumi 2:e6496a794bde 1 #include "myConstants.h"
Joeatsumi 2:e6496a794bde 2 #include "Vector.h"
Joeatsumi 2:e6496a794bde 3
Joeatsumi 2:e6496a794bde 4
Joeatsumi 2:e6496a794bde 5 Vector::Vector(int dim) : dim(dim), components(0){
Joeatsumi 2:e6496a794bde 6 components = new float[dim];
Joeatsumi 2:e6496a794bde 7 if (!components) error("Memory Allocation Error");
Joeatsumi 2:e6496a794bde 8 for(int i=0; i<dim; i++) components[i] = 0.0f;
Joeatsumi 2:e6496a794bde 9 }
Joeatsumi 2:e6496a794bde 10
Joeatsumi 2:e6496a794bde 11
Joeatsumi 2:e6496a794bde 12 Vector::~Vector() {
Joeatsumi 2:e6496a794bde 13 delete[] components;
Joeatsumi 2:e6496a794bde 14 }
Joeatsumi 2:e6496a794bde 15
Joeatsumi 2:e6496a794bde 16 Vector::Vector(const Vector& v) : dim(v.dim), components(0) {
Joeatsumi 2:e6496a794bde 17 components = new float[dim];
Joeatsumi 2:e6496a794bde 18 if (!components) error("Memory Allocation Error");
Joeatsumi 2:e6496a794bde 19 memcpy(components, v.GetpComponents(), sizeof(float)*dim);
Joeatsumi 2:e6496a794bde 20 }
Joeatsumi 2:e6496a794bde 21
Joeatsumi 2:e6496a794bde 22 Vector& Vector::operator=(const Vector& v) {
Joeatsumi 2:e6496a794bde 23 if (this == &v) return *this;
Joeatsumi 2:e6496a794bde 24 dim = v.dim;
Joeatsumi 2:e6496a794bde 25 delete[] components;
Joeatsumi 2:e6496a794bde 26 components = new float[dim];
Joeatsumi 2:e6496a794bde 27 if (!components) error("Memory Allocation Error");
Joeatsumi 2:e6496a794bde 28 memcpy(components, v.GetpComponents(), sizeof(float)*dim);
Joeatsumi 2:e6496a794bde 29
Joeatsumi 2:e6496a794bde 30 return *this;
Joeatsumi 2:e6496a794bde 31 }
Joeatsumi 2:e6496a794bde 32
Joeatsumi 2:e6496a794bde 33 Vector Vector::operator+() {
Joeatsumi 2:e6496a794bde 34 return *this;
Joeatsumi 2:e6496a794bde 35 }
Joeatsumi 2:e6496a794bde 36
Joeatsumi 2:e6496a794bde 37 Vector Vector::operator-() {
Joeatsumi 2:e6496a794bde 38 Vector retVec(*this);
Joeatsumi 2:e6496a794bde 39 retVec *= -1;
Joeatsumi 2:e6496a794bde 40 return retVec;
Joeatsumi 2:e6496a794bde 41 }
Joeatsumi 2:e6496a794bde 42
Joeatsumi 2:e6496a794bde 43 Vector& Vector::operator*=(float c) {
Joeatsumi 2:e6496a794bde 44 for (int i = 0; i < dim; i++) {
Joeatsumi 2:e6496a794bde 45 components[i] *= c;
Joeatsumi 2:e6496a794bde 46 }
Joeatsumi 2:e6496a794bde 47
Joeatsumi 2:e6496a794bde 48 return *this;
Joeatsumi 2:e6496a794bde 49 }
Joeatsumi 2:e6496a794bde 50
Joeatsumi 2:e6496a794bde 51 Vector& Vector::operator/=(float c) {
Joeatsumi 2:e6496a794bde 52 if (fabs(c) < NEARLY_ZERO) error("Division by Zero");
Joeatsumi 2:e6496a794bde 53 for (int i = 0; i < dim; i++) {
Joeatsumi 2:e6496a794bde 54 components[i] /= c;
Joeatsumi 2:e6496a794bde 55 }
Joeatsumi 2:e6496a794bde 56
Joeatsumi 2:e6496a794bde 57 return *this;
Joeatsumi 2:e6496a794bde 58 }
Joeatsumi 2:e6496a794bde 59
Joeatsumi 2:e6496a794bde 60 Vector& Vector::operator+=(const Vector& v) {
Joeatsumi 2:e6496a794bde 61 if (dim != v.dim) error("failed to add: Irregular Dimention");
Joeatsumi 2:e6496a794bde 62 for (int i = 0; i < dim; i++) {
Joeatsumi 2:e6496a794bde 63 components[i] += v.components[i];
Joeatsumi 2:e6496a794bde 64 }
Joeatsumi 2:e6496a794bde 65
Joeatsumi 2:e6496a794bde 66 this->CleanUp();
Joeatsumi 2:e6496a794bde 67
Joeatsumi 2:e6496a794bde 68 return *this;
Joeatsumi 2:e6496a794bde 69 }
Joeatsumi 2:e6496a794bde 70
Joeatsumi 2:e6496a794bde 71 Vector& Vector::operator-=(const Vector& v) {
Joeatsumi 2:e6496a794bde 72 if (dim != v.dim) error("failed to subtract: Irregular Dimention");
Joeatsumi 2:e6496a794bde 73 for (int i = 0; i < dim; i++) {
Joeatsumi 2:e6496a794bde 74 components[i] -= v.components[i];
Joeatsumi 2:e6496a794bde 75 }
Joeatsumi 2:e6496a794bde 76
Joeatsumi 2:e6496a794bde 77 this->CleanUp();
Joeatsumi 2:e6496a794bde 78
Joeatsumi 2:e6496a794bde 79 return *this;
Joeatsumi 2:e6496a794bde 80 }
Joeatsumi 2:e6496a794bde 81
Joeatsumi 2:e6496a794bde 82 void Vector::SetComp(int dimNo, float val) {
Joeatsumi 2:e6496a794bde 83 if (dimNo > dim) error("Index Out of Bounds Error");
Joeatsumi 2:e6496a794bde 84 components[dimNo-1] = val;
Joeatsumi 2:e6496a794bde 85 }
Joeatsumi 2:e6496a794bde 86
Joeatsumi 2:e6496a794bde 87 void Vector::SetComps(float* pComps) {
Joeatsumi 2:e6496a794bde 88 memcpy(components, pComps, sizeof(float) * dim);
Joeatsumi 2:e6496a794bde 89 }
Joeatsumi 2:e6496a794bde 90
Joeatsumi 2:e6496a794bde 91 float Vector::GetNorm() const {
Joeatsumi 2:e6496a794bde 92 float norm = 0.0f;
Joeatsumi 2:e6496a794bde 93 for (int i = 0; i < dim; i++) {
Joeatsumi 2:e6496a794bde 94 norm += components[i] * components[i];
Joeatsumi 2:e6496a794bde 95 }
Joeatsumi 2:e6496a794bde 96 return sqrt(norm);
Joeatsumi 2:e6496a794bde 97 }
Joeatsumi 2:e6496a794bde 98
Joeatsumi 2:e6496a794bde 99 //以下ではclaen up関数は無効化してある。
Joeatsumi 2:e6496a794bde 100 Vector Vector::Normalize() const {
Joeatsumi 2:e6496a794bde 101 float norm = GetNorm();
Joeatsumi 2:e6496a794bde 102 Vector temp(*this);
Joeatsumi 2:e6496a794bde 103 for (int i = 0; i < dim; i++) {
Joeatsumi 2:e6496a794bde 104 temp.components[i] /= norm;
Joeatsumi 2:e6496a794bde 105 }
Joeatsumi 2:e6496a794bde 106 //temp.CleanUp();
Joeatsumi 2:e6496a794bde 107 return temp;
Joeatsumi 2:e6496a794bde 108 }
Joeatsumi 2:e6496a794bde 109
Joeatsumi 2:e6496a794bde 110 Vector Vector::GetParaCompTo(Vector v) {
Joeatsumi 2:e6496a794bde 111 Vector norm_v = v.Normalize();
Joeatsumi 2:e6496a794bde 112 return (*this * norm_v) * norm_v;
Joeatsumi 2:e6496a794bde 113 }
Joeatsumi 2:e6496a794bde 114
Joeatsumi 2:e6496a794bde 115 Vector Vector::GetPerpCompTo(Vector v) {
Joeatsumi 2:e6496a794bde 116 return (*this - this->GetParaCompTo(v));
Joeatsumi 2:e6496a794bde 117 }
Joeatsumi 2:e6496a794bde 118
Joeatsumi 2:e6496a794bde 119 void Vector::CleanUp() {
Joeatsumi 2:e6496a794bde 120 float maxComp = 0.0f;
Joeatsumi 2:e6496a794bde 121 for (int i = 0; i < dim; i++) {
Joeatsumi 2:e6496a794bde 122 if (fabs(components[i]) > maxComp) maxComp = fabs(components[i]);
Joeatsumi 2:e6496a794bde 123 }
Joeatsumi 2:e6496a794bde 124 if (maxComp > NEARLY_ZERO) {
Joeatsumi 2:e6496a794bde 125 for (int i = 0; i < dim; i++) {
Joeatsumi 2:e6496a794bde 126 if (fabs(components[i]) / maxComp < ZERO_TOLERANCE) components[i] = 0.0f;
Joeatsumi 2:e6496a794bde 127 }
Joeatsumi 2:e6496a794bde 128 }
Joeatsumi 2:e6496a794bde 129 }
Joeatsumi 2:e6496a794bde 130
Joeatsumi 2:e6496a794bde 131 Vector operator+(const Vector& lhv, const Vector& rhv) {
Joeatsumi 2:e6496a794bde 132 Vector retVec(lhv);
Joeatsumi 2:e6496a794bde 133 retVec += rhv;
Joeatsumi 2:e6496a794bde 134 return retVec;
Joeatsumi 2:e6496a794bde 135 }
Joeatsumi 2:e6496a794bde 136
Joeatsumi 2:e6496a794bde 137 Vector operator-(const Vector& lhv, const Vector& rhv) {
Joeatsumi 2:e6496a794bde 138 Vector retVec(lhv);
Joeatsumi 2:e6496a794bde 139 retVec -= rhv;
Joeatsumi 2:e6496a794bde 140 return retVec;
Joeatsumi 2:e6496a794bde 141 }
Joeatsumi 2:e6496a794bde 142
Joeatsumi 2:e6496a794bde 143 Vector Cross(const Vector& lhv, const Vector& rhv) {
Joeatsumi 2:e6496a794bde 144 if (lhv.GetDim() != 3) error("failed to cross: variable 'dim' must be 3");
Joeatsumi 2:e6496a794bde 145 if (lhv.GetDim() != rhv.GetDim()) error("failed to cross: Irregular Dimention");
Joeatsumi 2:e6496a794bde 146
Joeatsumi 2:e6496a794bde 147 Vector retVec(lhv.GetDim());
Joeatsumi 2:e6496a794bde 148
Joeatsumi 2:e6496a794bde 149 for (int i = 0; i < lhv.GetDim(); i++) {
Joeatsumi 2:e6496a794bde 150 retVec.SetComp(i + 1, lhv.GetComp((i + 1) % 3 + 1) * rhv.GetComp((i + 2) % 3 + 1)
Joeatsumi 2:e6496a794bde 151 - lhv.GetComp((i + 2) % 3 + 1) * rhv.GetComp((i + 1) % 3 + 1));
Joeatsumi 2:e6496a794bde 152 }
Joeatsumi 2:e6496a794bde 153
Joeatsumi 2:e6496a794bde 154 return retVec;
Joeatsumi 2:e6496a794bde 155 }
Joeatsumi 2:e6496a794bde 156
Joeatsumi 2:e6496a794bde 157 Vector operator*(const float c, const Vector& rhv) {
Joeatsumi 2:e6496a794bde 158 Vector retVec(rhv);
Joeatsumi 2:e6496a794bde 159 retVec *= c;
Joeatsumi 2:e6496a794bde 160 return retVec;
Joeatsumi 2:e6496a794bde 161 }
Joeatsumi 2:e6496a794bde 162
Joeatsumi 2:e6496a794bde 163 Vector operator*(const Vector& lhv, const float c) {
Joeatsumi 2:e6496a794bde 164 Vector retVec(lhv);
Joeatsumi 2:e6496a794bde 165 retVec *= c;
Joeatsumi 2:e6496a794bde 166 return retVec;
Joeatsumi 2:e6496a794bde 167 }
Joeatsumi 2:e6496a794bde 168
Joeatsumi 2:e6496a794bde 169 float operator*(const Vector& lhv, const Vector& rhv) {
Joeatsumi 2:e6496a794bde 170 if (lhv.GetDim() != rhv.GetDim()) error("Irregular Dimention");
Joeatsumi 2:e6496a794bde 171 float retVal = 0.0f;
Joeatsumi 2:e6496a794bde 172
Joeatsumi 2:e6496a794bde 173 for (int i = 1; i <= lhv.GetDim(); i++) {
Joeatsumi 2:e6496a794bde 174 retVal += lhv.GetComp(i) * rhv.GetComp(i);
Joeatsumi 2:e6496a794bde 175 }
Joeatsumi 2:e6496a794bde 176
Joeatsumi 2:e6496a794bde 177 return retVal;
Joeatsumi 2:e6496a794bde 178 }