From fab110d21f147e5064ff140aadac649017fa466e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Aug 2013 18:13:01 +0200 Subject: Moved math library to library dir, improved sensor-level HIL, cleaned up geo / conversion libs --- src/lib/mathlib/math/generic/Vector.hpp | 245 ++++++++++++++++++++++++++++++++ 1 file changed, 245 insertions(+) create mode 100644 src/lib/mathlib/math/generic/Vector.hpp (limited to 'src/lib/mathlib/math/generic/Vector.hpp') diff --git a/src/lib/mathlib/math/generic/Vector.hpp b/src/lib/mathlib/math/generic/Vector.hpp new file mode 100644 index 000000000..8cfdc676d --- /dev/null +++ b/src/lib/mathlib/math/generic/Vector.hpp @@ -0,0 +1,245 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Vector.h + * + * math vector + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "../Vector.hpp" + +namespace math +{ + +class __EXPORT Vector +{ +public: + // constructor + Vector(size_t rows) : + _rows(rows), + _data((float *)calloc(rows, sizeof(float))) { + } + Vector(size_t rows, const float *data) : + _rows(rows), + _data((float *)malloc(getSize())) { + memcpy(getData(), data, getSize()); + } + // deconstructor + virtual ~Vector() { + delete [] getData(); + } + // copy constructor (deep) + Vector(const Vector &right) : + _rows(right.getRows()), + _data((float *)malloc(getSize())) { + memcpy(getData(), right.getData(), + right.getSize()); + } + // assignment + inline Vector &operator=(const Vector &right) { +#ifdef VECTOR_ASSERT + ASSERT(getRows() == right.getRows()); +#endif + + if (this != &right) { + memcpy(getData(), right.getData(), + right.getSize()); + } + + return *this; + } + // element accessors + inline float &operator()(size_t i) { +#ifdef VECTOR_ASSERT + ASSERT(i < getRows()); +#endif + return getData()[i]; + } + inline const float &operator()(size_t i) const { +#ifdef VECTOR_ASSERT + ASSERT(i < getRows()); +#endif + return getData()[i]; + } + // output + inline void print() const { + for (size_t i = 0; i < getRows(); i++) { + float sig; + int exp; + float num = (*this)(i); + float2SigExp(num, sig, exp); + printf("%6.3fe%03.3d,", (double)sig, exp); + } + + printf("\n"); + } + // boolean ops + inline bool operator==(const Vector &right) const { + for (size_t i = 0; i < getRows(); i++) { + if (fabsf(((*this)(i) - right(i))) > 1e-30f) + return false; + } + + return true; + } + // scalar ops + inline Vector operator+(const float &right) const { + Vector result(getRows()); + + for (size_t i = 0; i < getRows(); i++) { + result(i) = (*this)(i) + right; + } + + return result; + } + inline Vector operator-(const float &right) const { + Vector result(getRows()); + + for (size_t i = 0; i < getRows(); i++) { + result(i) = (*this)(i) - right; + } + + return result; + } + inline Vector operator*(const float &right) const { + Vector result(getRows()); + + for (size_t i = 0; i < getRows(); i++) { + result(i) = (*this)(i) * right; + } + + return result; + } + inline Vector operator/(const float &right) const { + Vector result(getRows()); + + for (size_t i = 0; i < getRows(); i++) { + result(i) = (*this)(i) / right; + } + + return result; + } + // vector ops + inline Vector operator+(const Vector &right) const { +#ifdef VECTOR_ASSERT + ASSERT(getRows() == right.getRows()); +#endif + Vector result(getRows()); + + for (size_t i = 0; i < getRows(); i++) { + result(i) = (*this)(i) + right(i); + } + + return result; + } + inline Vector operator-(const Vector &right) const { +#ifdef VECTOR_ASSERT + ASSERT(getRows() == right.getRows()); +#endif + Vector result(getRows()); + + for (size_t i = 0; i < getRows(); i++) { + result(i) = (*this)(i) - right(i); + } + + return result; + } + inline Vector operator-(void) const { + Vector result(getRows()); + + for (size_t i = 0; i < getRows(); i++) { + result(i) = -((*this)(i)); + } + + return result; + } + // other functions + inline float dot(const Vector &right) const { + float result = 0; + + for (size_t i = 0; i < getRows(); i++) { + result += (*this)(i) * (*this)(i); + } + + return result; + } + inline float norm() const { + return sqrtf(dot(*this)); + } + inline float length() const { + return norm(); + } + inline Vector unit() const { + return (*this) / norm(); + } + inline Vector normalized() const { + return unit(); + } + inline void normalize() { + (*this) = (*this) / norm(); + } + inline static Vector zero(size_t rows) { + Vector result(rows); + // calloc returns zeroed memory + return result; + } + inline void setAll(const float &val) { + for (size_t i = 0; i < getRows(); i++) { + (*this)(i) = val; + } + } + inline void set(const float *data) { + memcpy(getData(), data, getSize()); + } + inline size_t getRows() const { return _rows; } +protected: + inline size_t getSize() const { return sizeof(float) * getRows(); } + inline float *getData() { return _data; } + inline const float *getData() const { return _data; } + inline void setData(float *data) { _data = data; } +private: + size_t _rows; + float *_data; +}; + +} // math -- cgit v1.2.3