hiroya taura / Mbed 2 deprecated LAURUS_program

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

Dependencies:   ConfigFile SDFileSystem mbed

Fork of LAURUS_program by LAURUS

Vector/Vector.cpp@13:df1e8a650185, 2015-06-19 (annotated)

Committer:
ojan
Date:
Fri Jun 19 01:08:35 2015 +0000
Revision:
13:df1e8a650185
Parent:
9:6d4578dcc1ed
Child:
23:79cdc1432160
add scripts to avoid Spiral.; modify Kalman filter model.

Who changed what in which revision?

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