hiroya taura / Mbed 2 deprecated LAURUS_program

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

Dependencies:   ConfigFile SDFileSystem mbed

Fork of LAURUS_program by LAURUS

Vector/Vector.cpp@0:bc6f14fc60c7, 2015-05-15 (annotated)

Committer:
ojan
Date:
Fri May 15 17:24:32 2015 +0000
Revision:
0:bc6f14fc60c7
Child:
3:5358a691a100
Laurus integrated program (ver.0.0)

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