hiroya taura / Mbed 2 deprecated LAURUS_program

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

Dependencies:   ConfigFile SDFileSystem mbed

Fork of LAURUS_program by LAURUS

Vector/Vector.cpp@36:94dc027e05cd, 2015-12-07 (annotated)

Committer:
taurin
Date:
Mon Dec 07 02:11:56 2015 +0000
Revision:
36:94dc027e05cd
Parent:
23:79cdc1432160 
MPU?HMC??????ver

Who changed what in which revision?

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