Diff: Vector.cpp

Revision:

2:4a6b46653abf

Child:

3:40559ebef0f1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Vector.cpp	Mon Apr 20 14:54:55 2015 +0000
@@ -0,0 +1,191 @@
+#include "mbed.h"
+#include "myConstants.h"
+#include "Vector.h"
+
+
+Vector::Vector(int dim) : dim(dim), components(0){
+    components = new float[dim];
+    if (!components) AbortWithMsg("Memory Allocation Error");
+    for(int i=0; i<dim; i++) {
+        components[i] = 0.0f;
+    }
+}
+
+
+Vector::~Vector() {
+    delete[] components;
+}
+
+Vector::Vector(const Vector& v) : dim(v.dim), components(0) {
+    components = new float[dim];
+    if (!components) AbortWithMsg("Memory Allocation Error");
+    memcpy(components, v.GetpComponents(), sizeof(float)*dim);
+}
+
+Vector& Vector::operator=(const Vector& v) {
+    if (this == &v) return *this;
+    dim = v.dim;
+    delete[] components;
+    components = new float[dim];
+    if (!components) AbortWithMsg("Memory Allocation Error");
+    memcpy(components, v.GetpComponents(), sizeof(float)*dim);
+
+    return *this;
+}
+
+Vector& Vector::operator*=(float c) {
+    for (int i = 0; i < dim; i++) {
+        components[i] *= c;
+    }
+
+    return *this;
+}
+
+Vector& Vector::operator/=(float c) {
+    if (fabs(c) < NEARLY_ZERO) AbortWithMsg("Division by Zero");
+    for (int i = 0; i < dim; i++) {
+        components[i] /= c;
+    }
+
+    return *this;
+}
+
+Vector& Vector::operator+=(const Vector& v) {
+    if (dim != v.dim) AbortWithMsg("failed to add: Irregular Dimention");
+    for (int i = 0; i < dim; i++) {
+        components[i] += v.components[i];
+    }
+
+    this->CleanUp();
+
+    return *this;
+}
+
+Vector& Vector::operator-=(const Vector& v) {
+    if (dim != v.dim) AbortWithMsg("failed to subtract: Irregular Dimention");
+    for (int i = 0; i < dim; i++) {
+        components[i] -= v.components[i];
+    }
+
+    this->CleanUp();
+
+    return *this;
+}
+
+void Vector::SetComp(int dimNo, float val) {
+    if (dimNo > dim) AbortWithMsg("Index Out of Bounds Error");
+    components[dimNo-1] = val;
+}
+
+float Vector::GetNorm() const {
+    float norm = 0.0f;
+    for (int i = 0; i < dim; i++) {
+        norm += components[i] * components[i];
+    }
+    return sqrt(norm);
+}
+
+Vector Vector::GetUnit() const {
+    float norm = GetNorm();
+    Vector temp(*this);
+    for (int i = 0; i < dim; i++) {
+        temp.components[i] /= norm;
+    }
+    temp.CleanUp();
+    return temp;
+}
+
+void Vector::CleanUp() {
+    float maxComp = 0.0f;
+    for (int i = 0; i < dim; i++) {
+        if (fabs(components[i]) > maxComp) maxComp = fabs(components[i]);
+    }
+    if (maxComp > NEARLY_ZERO) {
+        for (int i = 0; i < dim; i++) {
+            if (fabs(components[i]) / maxComp < ZERO_TOLERANCE) components[i] = 0.0f;
+        }
+    }
+}
+
+Vector operator+(const Vector& lhv, const Vector& rhv) {
+    Vector retVec(lhv);
+    retVec += rhv;
+    return retVec;
+}
+
+Vector operator-(const Vector& lhv, const Vector& rhv) {
+    Vector retVec(lhv);
+    retVec -= rhv;
+    return retVec;
+}
+
+Vector Cross(const Vector& lhv, const Vector& rhv) {
+    if (lhv.GetDim() != 3) AbortWithMsg("failed to cross: variable 'dim' must be 3");
+    if (lhv.GetDim() != rhv.GetDim()) AbortWithMsg("failed to cross: Irregular Dimention");
+
+    Vector retVec(lhv.GetDim());
+
+    for (int i = 0; i < lhv.GetDim(); i++) {
+        retVec.SetComp(i + 1, lhv.GetComp((i + 1) % 3 + 1) * rhv.GetComp((i + 2) % 3 + 1)
+            - lhv.GetComp((i + 2) % 3 + 1) * rhv.GetComp((i + 1) % 3 + 1));
+    }
+
+    return retVec;
+}
+
+Vector operator*(const float c, const Vector& rhv) {
+    Vector retVec(rhv);
+    retVec *= c;
+    return retVec;
+}
+
+Vector operator*(const Vector& lhv, const float c) {
+    Vector retVec(lhv);
+    retVec *= c;
+    return retVec;
+}
+
+Vector operator*(const Matrix& lhm, const Vector& rhv) {
+    if (lhm.GetCol() != rhv.GetDim()) AbortWithMsg("Irregular Dimention");
+    Vector retVec(lhm.GetRow());
+
+    for (int i = 1; i <= lhm.GetRow(); i++) {
+        float temp = 0.0f;
+        for (int j = 1; j <= rhv.GetDim(); j++) {
+            temp += lhm.GetComp(i, j)*rhv.GetComp(j);
+        }
+        retVec.SetComp(i, temp);
+    }
+
+    retVec.CleanUp();
+
+    return retVec;
+}
+
+Vector operator*(const Vector& lhv, const Matrix& rhm) {
+    if (lhv.GetDim() != rhm.GetRow()) AbortWithMsg("Irregular Dimention");
+    Vector retVec(rhm.GetCol());
+
+    for (int i = 1; i <= rhm.GetCol(); i++) {
+        float temp = 0.0f;
+        for (int j = 1; j <= lhv.GetDim(); j++) {
+            temp += lhv.GetComp(j) * rhm.GetComp(j, i);
+        }
+        retVec.SetComp(i, temp);
+    }
+
+    retVec.CleanUp();
+
+    return retVec;
+}
+
+float operator*(const Vector& lhv, const Vector& rhv) {
+    if (lhv.GetDim() != rhv.GetDim()) AbortWithMsg("Irregular Dimention");
+    float retVal = 0.0f;
+
+    for (int i = 1; i <= lhv.GetDim(); i++) {
+        retVal += lhv.GetComp(i) * rhv.GetComp(i);
+    }
+
+    return retVal;
+}