LAURUS / Mbed 2 deprecated LAURUS_program

output only raw data (acceleration, anguler rate, geomagnetism, air pressure)

Dependencies:   mbed SDFileSystem ConfigFile

Vector/Vector.cpp@3:5358a691a100, 2015-05-30 (annotated)

Committer:
ojan
Date:
Sat May 30 18:08:34 2015 +0000
Revision:
3:5358a691a100
Parent:
0:bc6f14fc60c7
Child:
9:6d4578dcc1ed
estimate geomagnetism vector with Kalman Filter

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 0:bc6f14fc60c7 34 Vector& Vector::operator*=(float c) {
ojan 0:bc6f14fc60c7 35 for (int i = 0; i < dim; i++) {
ojan 0:bc6f14fc60c7 36 components[i] *= c;
ojan 0:bc6f14fc60c7 37 }
ojan 0:bc6f14fc60c7 38
ojan 0:bc6f14fc60c7 39 return *this;
ojan 0:bc6f14fc60c7 40 }
ojan 0:bc6f14fc60c7 41
ojan 0:bc6f14fc60c7 42 Vector& Vector::operator/=(float c) {
ojan 0:bc6f14fc60c7 43 if (fabs(c) < NEARLY_ZERO) AbortWithMsg("Division by Zero");
ojan 0:bc6f14fc60c7 44 for (int i = 0; i < dim; i++) {
ojan 0:bc6f14fc60c7 45 components[i] /= c;
ojan 0:bc6f14fc60c7 46 }
ojan 0:bc6f14fc60c7 47
ojan 0:bc6f14fc60c7 48 return *this;
ojan 0:bc6f14fc60c7 49 }
ojan 0:bc6f14fc60c7 50
ojan 0:bc6f14fc60c7 51 Vector& Vector::operator+=(const Vector& v) {
ojan 0:bc6f14fc60c7 52 if (dim != v.dim) AbortWithMsg("failed to add: Irregular Dimention");
ojan 0:bc6f14fc60c7 53 for (int i = 0; i < dim; i++) {
ojan 0:bc6f14fc60c7 54 components[i] += v.components[i];
ojan 0:bc6f14fc60c7 55 }
ojan 0:bc6f14fc60c7 56
ojan 0:bc6f14fc60c7 57 this->CleanUp();
ojan 0:bc6f14fc60c7 58
ojan 0:bc6f14fc60c7 59 return *this;
ojan 0:bc6f14fc60c7 60 }
ojan 0:bc6f14fc60c7 61
ojan 0:bc6f14fc60c7 62 Vector& Vector::operator-=(const Vector& v) {
ojan 0:bc6f14fc60c7 63 if (dim != v.dim) AbortWithMsg("failed to subtract: Irregular Dimention");
ojan 0:bc6f14fc60c7 64 for (int i = 0; i < dim; i++) {
ojan 0:bc6f14fc60c7 65 components[i] -= v.components[i];
ojan 0:bc6f14fc60c7 66 }
ojan 0:bc6f14fc60c7 67
ojan 0:bc6f14fc60c7 68 this->CleanUp();
ojan 0:bc6f14fc60c7 69
ojan 0:bc6f14fc60c7 70 return *this;
ojan 0:bc6f14fc60c7 71 }
ojan 0:bc6f14fc60c7 72
ojan 0:bc6f14fc60c7 73 void Vector::SetComp(int dimNo, float val) {
ojan 0:bc6f14fc60c7 74 if (dimNo > dim) AbortWithMsg("Index Out of Bounds Error");
ojan 0:bc6f14fc60c7 75 components[dimNo-1] = val;
ojan 0:bc6f14fc60c7 76 }
ojan 0:bc6f14fc60c7 77
ojan 0:bc6f14fc60c7 78 float Vector::GetNorm() const {
ojan 0:bc6f14fc60c7 79 float norm = 0.0f;
ojan 0:bc6f14fc60c7 80 for (int i = 0; i < dim; i++) {
ojan 0:bc6f14fc60c7 81 norm += components[i] * components[i];
ojan 0:bc6f14fc60c7 82 }
ojan 0:bc6f14fc60c7 83 return sqrt(norm);
ojan 0:bc6f14fc60c7 84 }
ojan 0:bc6f14fc60c7 85
ojan 0:bc6f14fc60c7 86 Vector Vector::Normalize() const {
ojan 0:bc6f14fc60c7 87 float norm = GetNorm();
ojan 0:bc6f14fc60c7 88 Vector temp(*this);
ojan 0:bc6f14fc60c7 89 for (int i = 0; i < dim; i++) {
ojan 0:bc6f14fc60c7 90 temp.components[i] /= norm;
ojan 0:bc6f14fc60c7 91 }
ojan 0:bc6f14fc60c7 92 temp.CleanUp();
ojan 0:bc6f14fc60c7 93 return temp;
ojan 0:bc6f14fc60c7 94 }
ojan 0:bc6f14fc60c7 95
ojan 0:bc6f14fc60c7 96 void Vector::CleanUp() {
ojan 0:bc6f14fc60c7 97 float maxComp = 0.0f;
ojan 0:bc6f14fc60c7 98 for (int i = 0; i < dim; i++) {
ojan 0:bc6f14fc60c7 99 if (fabs(components[i]) > maxComp) maxComp = fabs(components[i]);
ojan 0:bc6f14fc60c7 100 }
ojan 0:bc6f14fc60c7 101 if (maxComp > NEARLY_ZERO) {
ojan 0:bc6f14fc60c7 102 for (int i = 0; i < dim; i++) {
ojan 0:bc6f14fc60c7 103 if (fabs(components[i]) / maxComp < ZERO_TOLERANCE) components[i] = 0.0f;
ojan 0:bc6f14fc60c7 104 }
ojan 0:bc6f14fc60c7 105 }
ojan 0:bc6f14fc60c7 106 }
ojan 0:bc6f14fc60c7 107
ojan 0:bc6f14fc60c7 108 Vector operator+(const Vector& lhv, const Vector& rhv) {
ojan 0:bc6f14fc60c7 109 Vector retVec(lhv);
ojan 0:bc6f14fc60c7 110 retVec += rhv;
ojan 0:bc6f14fc60c7 111 return retVec;
ojan 0:bc6f14fc60c7 112 }
ojan 0:bc6f14fc60c7 113
ojan 0:bc6f14fc60c7 114 Vector operator-(const Vector& lhv, const Vector& rhv) {
ojan 0:bc6f14fc60c7 115 Vector retVec(lhv);
ojan 0:bc6f14fc60c7 116 retVec -= rhv;
ojan 0:bc6f14fc60c7 117 return retVec;
ojan 0:bc6f14fc60c7 118 }
ojan 0:bc6f14fc60c7 119
ojan 0:bc6f14fc60c7 120 Vector Cross(const Vector& lhv, const Vector& rhv) {
ojan 0:bc6f14fc60c7 121 if (lhv.GetDim() != 3) AbortWithMsg("failed to cross: variable 'dim' must be 3");
ojan 0:bc6f14fc60c7 122 if (lhv.GetDim() != rhv.GetDim()) AbortWithMsg("failed to cross: Irregular Dimention");
ojan 0:bc6f14fc60c7 123
ojan 0:bc6f14fc60c7 124 Vector retVec(lhv.GetDim());
ojan 0:bc6f14fc60c7 125
ojan 0:bc6f14fc60c7 126 for (int i = 0; i < lhv.GetDim(); i++) {
ojan 0:bc6f14fc60c7 127 retVec.SetComp(i + 1, lhv.GetComp((i + 1) % 3 + 1) * rhv.GetComp((i + 2) % 3 + 1)
ojan 0:bc6f14fc60c7 128 - lhv.GetComp((i + 2) % 3 + 1) * rhv.GetComp((i + 1) % 3 + 1));
ojan 0:bc6f14fc60c7 129 }
ojan 0:bc6f14fc60c7 130
ojan 0:bc6f14fc60c7 131 return retVec;
ojan 0:bc6f14fc60c7 132 }
ojan 0:bc6f14fc60c7 133
ojan 0:bc6f14fc60c7 134 Vector operator*(const float c, const Vector& rhv) {
ojan 0:bc6f14fc60c7 135 Vector retVec(rhv);
ojan 0:bc6f14fc60c7 136 retVec *= c;
ojan 0:bc6f14fc60c7 137 return retVec;
ojan 0:bc6f14fc60c7 138 }
ojan 0:bc6f14fc60c7 139
ojan 0:bc6f14fc60c7 140 Vector operator*(const Vector& lhv, const float c) {
ojan 0:bc6f14fc60c7 141 Vector retVec(lhv);
ojan 0:bc6f14fc60c7 142 retVec *= c;
ojan 0:bc6f14fc60c7 143 return retVec;
ojan 0:bc6f14fc60c7 144 }
ojan 0:bc6f14fc60c7 145
ojan 0:bc6f14fc60c7 146 float operator*(const Vector& lhv, const Vector& rhv) {
ojan 0:bc6f14fc60c7 147 if (lhv.GetDim() != rhv.GetDim()) AbortWithMsg("Irregular Dimention");
ojan 0:bc6f14fc60c7 148 float retVal = 0.0f;
ojan 0:bc6f14fc60c7 149
ojan 0:bc6f14fc60c7 150 for (int i = 1; i <= lhv.GetDim(); i++) {
ojan 0:bc6f14fc60c7 151 retVal += lhv.GetComp(i) * rhv.GetComp(i);
ojan 0:bc6f14fc60c7 152 }
ojan 0:bc6f14fc60c7 153
ojan 0:bc6f14fc60c7 154 return retVal;
ojan 0:bc6f14fc60c7 155 }