Atsumi Toda
/
UAV_Logger1
UAVの姿勢推定に使用するプログラム。
Diff: Vector/Vector.cpp
- Revision:
- 4:21a356ae0747
- Parent:
- 3:3fa7882a5fd0
--- a/Vector/Vector.cpp Wed Jul 24 12:00:01 2019 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,178 +0,0 @@ -#include "myConstants.h" -#include "Vector.h" - - -Vector::Vector(int dim) : dim(dim), components(0){ - components = new float[dim]; - if (!components) error("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) error("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) error("Memory Allocation Error"); - memcpy(components, v.GetpComponents(), sizeof(float)*dim); - - return *this; -} - -Vector Vector::operator+() { - return *this; -} - -Vector Vector::operator-() { - Vector retVec(*this); - retVec *= -1; - return retVec; -} - -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) error("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) error("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) error("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) error("Index Out of Bounds Error"); - components[dimNo-1] = val; -} - -void Vector::SetComps(float* pComps) { - memcpy(components, pComps, sizeof(float) * dim); -} - -float Vector::GetNorm() const { - float norm = 0.0f; - for (int i = 0; i < dim; i++) { - norm += components[i] * components[i]; - } - return sqrt(norm); -} - -//以下ではclaen up関数は無効化してある。 -Vector Vector::Normalize() const { - float norm = GetNorm(); - Vector temp(*this); - for (int i = 0; i < dim; i++) { - temp.components[i] /= norm; - } - //temp.CleanUp(); - return temp; -} - -Vector Vector::GetParaCompTo(Vector v) { - Vector norm_v = v.Normalize(); - return (*this * norm_v) * norm_v; -} - -Vector Vector::GetPerpCompTo(Vector v) { - return (*this - this->GetParaCompTo(v)); -} - -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) error("failed to cross: variable 'dim' must be 3"); - if (lhv.GetDim() != rhv.GetDim()) error("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; -} - -float operator*(const Vector& lhv, const Vector& rhv) { - if (lhv.GetDim() != rhv.GetDim()) error("Irregular Dimention"); - float retVal = 0.0f; - - for (int i = 1; i <= lhv.GetDim(); i++) { - retVal += lhv.GetComp(i) * rhv.GetComp(i); - } - - return retVal; -}