hiroya taura / Mbed 2 deprecated LAURUS_program

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

Dependencies:   ConfigFile SDFileSystem mbed

Fork of LAURUS_program by LAURUS

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?

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 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 }