output only raw data (acceleration, anguler rate, geomagnetism, air pressure)
Dependencies: mbed SDFileSystem ConfigFile
Vector/Vector.cpp@9:6d4578dcc1ed, 2015-06-15 (annotated)
- Committer:
- ojan
- Date:
- Mon Jun 15 10:40:18 2015 +0000
- Revision:
- 9:6d4578dcc1ed
- Parent:
- 3:5358a691a100
- Child:
- 13:df1e8a650185
add code to calculate Euler angle
Who changed what in which revision?
User | Revision | Line number | New 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 | 0:bc6f14fc60c7 | 88 | float Vector::GetNorm() const { |
ojan | 0:bc6f14fc60c7 | 89 | float norm = 0.0f; |
ojan | 0:bc6f14fc60c7 | 90 | for (int i = 0; i < dim; i++) { |
ojan | 0:bc6f14fc60c7 | 91 | norm += components[i] * components[i]; |
ojan | 0:bc6f14fc60c7 | 92 | } |
ojan | 0:bc6f14fc60c7 | 93 | return sqrt(norm); |
ojan | 0:bc6f14fc60c7 | 94 | } |
ojan | 0:bc6f14fc60c7 | 95 | |
ojan | 0:bc6f14fc60c7 | 96 | Vector Vector::Normalize() const { |
ojan | 0:bc6f14fc60c7 | 97 | float norm = GetNorm(); |
ojan | 0:bc6f14fc60c7 | 98 | Vector temp(*this); |
ojan | 0:bc6f14fc60c7 | 99 | for (int i = 0; i < dim; i++) { |
ojan | 0:bc6f14fc60c7 | 100 | temp.components[i] /= norm; |
ojan | 0:bc6f14fc60c7 | 101 | } |
ojan | 0:bc6f14fc60c7 | 102 | temp.CleanUp(); |
ojan | 0:bc6f14fc60c7 | 103 | return temp; |
ojan | 0:bc6f14fc60c7 | 104 | } |
ojan | 0:bc6f14fc60c7 | 105 | |
ojan | 9:6d4578dcc1ed | 106 | Vector Vector::GetParaCompTo(Vector v) { |
ojan | 9:6d4578dcc1ed | 107 | Vector norm_v = v.Normalize(); |
ojan | 9:6d4578dcc1ed | 108 | return (*this * norm_v) * norm_v; |
ojan | 9:6d4578dcc1ed | 109 | } |
ojan | 9:6d4578dcc1ed | 110 | |
ojan | 9:6d4578dcc1ed | 111 | Vector Vector::GetPerpCompTo(Vector v) { |
ojan | 9:6d4578dcc1ed | 112 | return (*this - this->GetParaCompTo(v)); |
ojan | 9:6d4578dcc1ed | 113 | } |
ojan | 9:6d4578dcc1ed | 114 | |
ojan | 0:bc6f14fc60c7 | 115 | void Vector::CleanUp() { |
ojan | 0:bc6f14fc60c7 | 116 | float maxComp = 0.0f; |
ojan | 0:bc6f14fc60c7 | 117 | for (int i = 0; i < dim; i++) { |
ojan | 0:bc6f14fc60c7 | 118 | if (fabs(components[i]) > maxComp) maxComp = fabs(components[i]); |
ojan | 0:bc6f14fc60c7 | 119 | } |
ojan | 0:bc6f14fc60c7 | 120 | if (maxComp > NEARLY_ZERO) { |
ojan | 0:bc6f14fc60c7 | 121 | for (int i = 0; i < dim; i++) { |
ojan | 0:bc6f14fc60c7 | 122 | if (fabs(components[i]) / maxComp < ZERO_TOLERANCE) components[i] = 0.0f; |
ojan | 0:bc6f14fc60c7 | 123 | } |
ojan | 0:bc6f14fc60c7 | 124 | } |
ojan | 0:bc6f14fc60c7 | 125 | } |
ojan | 0:bc6f14fc60c7 | 126 | |
ojan | 0:bc6f14fc60c7 | 127 | Vector operator+(const Vector& lhv, const Vector& rhv) { |
ojan | 0:bc6f14fc60c7 | 128 | Vector retVec(lhv); |
ojan | 0:bc6f14fc60c7 | 129 | retVec += rhv; |
ojan | 0:bc6f14fc60c7 | 130 | return retVec; |
ojan | 0:bc6f14fc60c7 | 131 | } |
ojan | 0:bc6f14fc60c7 | 132 | |
ojan | 0:bc6f14fc60c7 | 133 | Vector operator-(const Vector& lhv, const Vector& rhv) { |
ojan | 0:bc6f14fc60c7 | 134 | Vector retVec(lhv); |
ojan | 0:bc6f14fc60c7 | 135 | retVec -= rhv; |
ojan | 0:bc6f14fc60c7 | 136 | return retVec; |
ojan | 0:bc6f14fc60c7 | 137 | } |
ojan | 0:bc6f14fc60c7 | 138 | |
ojan | 0:bc6f14fc60c7 | 139 | Vector Cross(const Vector& lhv, const Vector& rhv) { |
ojan | 0:bc6f14fc60c7 | 140 | if (lhv.GetDim() != 3) AbortWithMsg("failed to cross: variable 'dim' must be 3"); |
ojan | 0:bc6f14fc60c7 | 141 | if (lhv.GetDim() != rhv.GetDim()) AbortWithMsg("failed to cross: Irregular Dimention"); |
ojan | 0:bc6f14fc60c7 | 142 | |
ojan | 0:bc6f14fc60c7 | 143 | Vector retVec(lhv.GetDim()); |
ojan | 0:bc6f14fc60c7 | 144 | |
ojan | 0:bc6f14fc60c7 | 145 | for (int i = 0; i < lhv.GetDim(); i++) { |
ojan | 0:bc6f14fc60c7 | 146 | retVec.SetComp(i + 1, lhv.GetComp((i + 1) % 3 + 1) * rhv.GetComp((i + 2) % 3 + 1) |
ojan | 0:bc6f14fc60c7 | 147 | - lhv.GetComp((i + 2) % 3 + 1) * rhv.GetComp((i + 1) % 3 + 1)); |
ojan | 0:bc6f14fc60c7 | 148 | } |
ojan | 0:bc6f14fc60c7 | 149 | |
ojan | 0:bc6f14fc60c7 | 150 | return retVec; |
ojan | 0:bc6f14fc60c7 | 151 | } |
ojan | 0:bc6f14fc60c7 | 152 | |
ojan | 0:bc6f14fc60c7 | 153 | Vector operator*(const float c, const Vector& rhv) { |
ojan | 0:bc6f14fc60c7 | 154 | Vector retVec(rhv); |
ojan | 0:bc6f14fc60c7 | 155 | retVec *= c; |
ojan | 0:bc6f14fc60c7 | 156 | return retVec; |
ojan | 0:bc6f14fc60c7 | 157 | } |
ojan | 0:bc6f14fc60c7 | 158 | |
ojan | 0:bc6f14fc60c7 | 159 | Vector operator*(const Vector& lhv, const float c) { |
ojan | 0:bc6f14fc60c7 | 160 | Vector retVec(lhv); |
ojan | 0:bc6f14fc60c7 | 161 | retVec *= c; |
ojan | 0:bc6f14fc60c7 | 162 | return retVec; |
ojan | 0:bc6f14fc60c7 | 163 | } |
ojan | 0:bc6f14fc60c7 | 164 | |
ojan | 0:bc6f14fc60c7 | 165 | float operator*(const Vector& lhv, const Vector& rhv) { |
ojan | 0:bc6f14fc60c7 | 166 | if (lhv.GetDim() != rhv.GetDim()) AbortWithMsg("Irregular Dimention"); |
ojan | 0:bc6f14fc60c7 | 167 | float retVal = 0.0f; |
ojan | 0:bc6f14fc60c7 | 168 | |
ojan | 0:bc6f14fc60c7 | 169 | for (int i = 1; i <= lhv.GetDim(); i++) { |
ojan | 0:bc6f14fc60c7 | 170 | retVal += lhv.GetComp(i) * rhv.GetComp(i); |
ojan | 0:bc6f14fc60c7 | 171 | } |
ojan | 0:bc6f14fc60c7 | 172 | |
ojan | 0:bc6f14fc60c7 | 173 | return retVal; |
ojan | 0:bc6f14fc60c7 | 174 | } |