Jaesung Oh
/
HydraulicControlBoard_SW
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed Eigen FastPWM
Revision 25:f83396e3d25c, committed 2019-10-14
- Comitter:
- jsoh91
- Date:
- Mon Oct 14 10:08:13 2019 +0000
- Parent:
- 24:ef6e1092e9e6
- Commit message:
- withMPC
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Array.cpp Mon Oct 14 10:08:13 2019 +0000
@@ -0,0 +1,33 @@
+// $Id: Array.cc 201 2008-05-18 19:47:38Z digasper $
+// This file is part of QuadProg++:
+// Copyright (C) 2006--2009 Luca Di Gaspero.
+//
+// This software may be modified and distributed under the terms
+// of the MIT license. See the LICENSE file for details.
+
+#include "Array.h"
+
+/**
+ Index utilities
+ */
+
+namespace quadprogpp {
+
+std::set<unsigned int> seq(unsigned int s, unsigned int e)
+{
+ std::set<unsigned int> tmp;
+ for (unsigned int i = s; i <= e; i++)
+ tmp.insert(i);
+
+ return tmp;
+}
+
+std::set<unsigned int> singleton(unsigned int i)
+{
+ std::set<unsigned int> tmp;
+ tmp.insert(i);
+
+ return tmp;
+}
+
+} // namespace quadprogpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Array.h Mon Oct 14 10:08:13 2019 +0000
@@ -0,0 +1,2543 @@
+// $Id: Array.hh 249 2008-11-20 09:58:23Z schaerf $
+// This file is part of EasyLocalpp: a C++ Object-Oriented framework
+// aimed at easing the development of Local Search algorithms.
+// Copyright (C) 2001--2008 Andrea Schaerf, Luca Di Gaspero.
+//
+// This software may be modified and distributed under the terms
+// of the MIT license. See the LICENSE file for details.
+
+#if !defined(_ARRAY_HH)
+#define _ARRAY_HH
+
+#include <set>
+#include <stdexcept>
+#include <iostream>
+#include <iomanip>
+#include <cmath>
+#include <cstdlib>
+
+namespace quadprogpp {
+
+enum MType { DIAG };
+
+template <typename T>
+class Vector
+{
+public:
+ Vector();
+ Vector(const unsigned int n);
+ Vector(const T& a, const unsigned int n); //initialize to constant value
+ Vector(const T* a, const unsigned int n); // Initialize to array
+ Vector(const Vector &rhs); // copy constructor
+ ~Vector(); // destructor
+
+ inline void set(const T* a, const unsigned int n);
+ Vector<T> extract(const std::set<unsigned int>& indexes) const;
+ inline T& operator[](const unsigned int& i); //i-th element
+ inline const T& operator[](const unsigned int& i) const;
+
+ inline unsigned int size() const;
+ inline void resize(const unsigned int n);
+ inline void resize(const T& a, const unsigned int n);
+
+ Vector<T>& operator=(const Vector<T>& rhs); //assignment
+ Vector<T>& operator=(const T& a); //assign a to every element
+ inline Vector<T>& operator+=(const Vector<T>& rhs);
+ inline Vector<T>& operator-=(const Vector<T>& rhs);
+ inline Vector<T>& operator*=(const Vector<T>& rhs);
+ inline Vector<T>& operator/=(const Vector<T>& rhs);
+ inline Vector<T>& operator^=(const Vector<T>& rhs);
+ inline Vector<T>& operator+=(const T& a);
+ inline Vector<T>& operator-=(const T& a);
+ inline Vector<T>& operator*=(const T& a);
+ inline Vector<T>& operator/=(const T& a);
+ inline Vector<T>& operator^=(const T& a);
+private:
+ unsigned int n; // size of array. upper index is n-1
+ T* v; // storage for data
+};
+
+template <typename T>
+Vector<T>::Vector()
+ : n(0), v(0)
+{}
+
+template <typename T>
+Vector<T>::Vector(const unsigned int n)
+ : v(new T[n])
+{
+ this->n = n;
+}
+
+template <typename T>
+Vector<T>::Vector(const T& a, const unsigned int n)
+ : v(new T[n])
+{
+ this->n = n;
+ for (unsigned int i = 0; i < n; i++)
+ v[i] = a;
+}
+
+template <typename T>
+Vector<T>::Vector(const T* a, const unsigned int n)
+ : v(new T[n])
+{
+ this->n = n;
+ for (unsigned int i = 0; i < n; i++)
+ v[i] = *a++;
+}
+
+template <typename T>
+Vector<T>::Vector(const Vector<T>& rhs)
+ : v(new T[rhs.n])
+{
+ this->n = rhs.n;
+ for (unsigned int i = 0; i < n; i++)
+ v[i] = rhs[i];
+}
+
+template <typename T>
+Vector<T>::~Vector()
+{
+ if (v != 0)
+ delete[] (v);
+}
+
+template <typename T>
+void Vector<T>::resize(const unsigned int n)
+{
+ if (n == this->n)
+ return;
+ if (v != 0)
+ delete[] (v);
+ v = new T[n];
+ this->n = n;
+}
+
+template <typename T>
+void Vector<T>::resize(const T& a, const unsigned int n)
+{
+ resize(n);
+ for (unsigned int i = 0; i < n; i++)
+ v[i] = a;
+}
+
+
+template <typename T>
+inline Vector<T>& Vector<T>::operator=(const Vector<T>& rhs)
+// postcondition: normal assignment via copying has been performed;
+// if vector and rhs were different sizes, vector
+// has been resized to match the size of rhs
+{
+ if (this != &rhs)
+ {
+ resize(rhs.n);
+ for (unsigned int i = 0; i < n; i++)
+ v[i] = rhs[i];
+ }
+ return *this;
+}
+
+template <typename T>
+inline Vector<T> & Vector<T>::operator=(const T& a) //assign a to every element
+{
+ for (unsigned int i = 0; i < n; i++)
+ v[i] = a;
+ return *this;
+}
+
+template <typename T>
+inline T & Vector<T>::operator[](const unsigned int& i) //subscripting
+{
+ return v[i];
+}
+
+template <typename T>
+inline const T& Vector<T>::operator[](const unsigned int& i) const //subscripting
+{
+ return v[i];
+}
+
+template <typename T>
+inline unsigned int Vector<T>::size() const
+{
+ return n;
+}
+
+template <typename T>
+inline void Vector<T>::set(const T* a, unsigned int n)
+{
+ resize(n);
+ for (unsigned int i = 0; i < n; i++)
+ v[i] = a[i];
+}
+
+template <typename T>
+inline Vector<T> Vector<T>::extract(const std::set<unsigned int>& indexes) const
+{
+ Vector<T> tmp(indexes.size());
+ unsigned int i = 0;
+
+ for (std::set<unsigned int>::const_iterator el = indexes.begin(); el != indexes.end(); el++)
+ {
+ if (*el >= n)
+ throw std::logic_error("Error extracting subvector: the indexes are out of vector bounds");
+ tmp[i++] = v[*el];
+ }
+
+ return tmp;
+}
+
+template <typename T>
+inline Vector<T>& Vector<T>::operator+=(const Vector<T>& rhs)
+{
+ if (this->size() != rhs.size())
+ throw std::logic_error("Operator+=: vectors have different sizes");
+ for (unsigned int i = 0; i < n; i++)
+ v[i] += rhs[i];
+
+ return *this;
+}
+
+
+template <typename T>
+inline Vector<T>& Vector<T>::operator+=(const T& a)
+{
+ for (unsigned int i = 0; i < n; i++)
+ v[i] += a;
+
+ return *this;
+}
+
+template <typename T>
+inline Vector<T> operator+(const Vector<T>& rhs)
+{
+ return rhs;
+}
+
+template <typename T>
+inline Vector<T> operator+(const Vector<T>& lhs, const Vector<T>& rhs)
+{
+ if (lhs.size() != rhs.size())
+ throw std::logic_error("Operator+: vectors have different sizes");
+ Vector<T> tmp(lhs.size());
+ for (unsigned int i = 0; i < lhs.size(); i++)
+ tmp[i] = lhs[i] + rhs[i];
+
+ return tmp;
+}
+
+template <typename T>
+inline Vector<T> operator+(const Vector<T>& lhs, const T& a)
+{
+ Vector<T> tmp(lhs.size());
+ for (unsigned int i = 0; i < lhs.size(); i++)
+ tmp[i] = lhs[i] + a;
+
+ return tmp;
+}
+
+template <typename T>
+inline Vector<T> operator+(const T& a, const Vector<T>& rhs)
+{
+ Vector<T> tmp(rhs.size());
+ for (unsigned int i = 0; i < rhs.size(); i++)
+ tmp[i] = a + rhs[i];
+
+ return tmp;
+}
+
+template <typename T>
+inline Vector<T>& Vector<T>::operator-=(const Vector<T>& rhs)
+{
+ if (this->size() != rhs.size())
+ throw std::logic_error("Operator-=: vectors have different sizes");
+ for (unsigned int i = 0; i < n; i++)
+ v[i] -= rhs[i];
+
+ return *this;
+}
+
+
+template <typename T>
+inline Vector<T>& Vector<T>::operator-=(const T& a)
+{
+ for (unsigned int i = 0; i < n; i++)
+ v[i] -= a;
+
+ return *this;
+}
+
+template <typename T>
+inline Vector<T> operator-(const Vector<T>& rhs)
+{
+ return (T)(-1) * rhs;
+}
+
+template <typename T>
+inline Vector<T> operator-(const Vector<T>& lhs, const Vector<T>& rhs)
+{
+ if (lhs.size() != rhs.size())
+ throw std::logic_error("Operator-: vectors have different sizes");
+ Vector<T> tmp(lhs.size());
+ for (unsigned int i = 0; i < lhs.size(); i++)
+ tmp[i] = lhs[i] - rhs[i];
+
+ return tmp;
+}
+
+template <typename T>
+inline Vector<T> operator-(const Vector<T>& lhs, const T& a)
+{
+ Vector<T> tmp(lhs.size());
+ for (unsigned int i = 0; i < lhs.size(); i++)
+ tmp[i] = lhs[i] - a;
+
+ return tmp;
+}
+
+template <typename T>
+inline Vector<T> operator-(const T& a, const Vector<T>& rhs)
+{
+ Vector<T> tmp(rhs.size());
+ for (unsigned int i = 0; i < rhs.size(); i++)
+ tmp[i] = a - rhs[i];
+
+ return tmp;
+}
+
+template <typename T>
+inline Vector<T>& Vector<T>::operator*=(const Vector<T>& rhs)
+{
+ if (this->size() != rhs.size())
+ throw std::logic_error("Operator*=: vectors have different sizes");
+ for (unsigned int i = 0; i < n; i++)
+ v[i] *= rhs[i];
+
+ return *this;
+}
+
+
+template <typename T>
+inline Vector<T>& Vector<T>::operator*=(const T& a)
+{
+ for (unsigned int i = 0; i < n; i++)
+ v[i] *= a;
+
+ return *this;
+}
+
+template <typename T>
+inline Vector<T> operator*(const Vector<T>& lhs, const Vector<T>& rhs)
+{
+ if (lhs.size() != rhs.size())
+ throw std::logic_error("Operator*: vectors have different sizes");
+ Vector<T> tmp(lhs.size());
+ for (unsigned int i = 0; i < lhs.size(); i++)
+ tmp[i] = lhs[i] * rhs[i];
+
+ return tmp;
+}
+
+template <typename T>
+inline Vector<T> operator*(const Vector<T>& lhs, const T& a)
+{
+ Vector<T> tmp(lhs.size());
+ for (unsigned int i = 0; i < lhs.size(); i++)
+ tmp[i] = lhs[i] * a;
+
+ return tmp;
+}
+
+template <typename T>
+inline Vector<T> operator*(const T& a, const Vector<T>& rhs)
+{
+ Vector<T> tmp(rhs.size());
+ for (unsigned int i = 0; i < rhs.size(); i++)
+ tmp[i] = a * rhs[i];
+
+ return tmp;
+}
+
+template <typename T>
+inline Vector<T>& Vector<T>::operator/=(const Vector<T>& rhs)
+{
+ if (this->size() != rhs.size())
+ throw std::logic_error("Operator/=: vectors have different sizes");
+ for (unsigned int i = 0; i < n; i++)
+ v[i] /= rhs[i];
+
+ return *this;
+}
+
+
+template <typename T>
+inline Vector<T>& Vector<T>::operator/=(const T& a)
+{
+ for (unsigned int i = 0; i < n; i++)
+ v[i] /= a;
+
+ return *this;
+}
+
+template <typename T>
+inline Vector<T> operator/(const Vector<T>& lhs, const Vector<T>& rhs)
+{
+ if (lhs.size() != rhs.size())
+ throw std::logic_error("Operator/: vectors have different sizes");
+ Vector<T> tmp(lhs.size());
+ for (unsigned int i = 0; i < lhs.size(); i++)
+ tmp[i] = lhs[i] / rhs[i];
+
+ return tmp;
+}
+
+template <typename T>
+inline Vector<T> operator/(const Vector<T>& lhs, const T& a)
+{
+ Vector<T> tmp(lhs.size());
+ for (unsigned int i = 0; i < lhs.size(); i++)
+ tmp[i] = lhs[i] / a;
+
+ return tmp;
+}
+
+template <typename T>
+inline Vector<T> operator/(const T& a, const Vector<T>& rhs)
+{
+ Vector<T> tmp(rhs.size());
+ for (unsigned int i = 0; i < rhs.size(); i++)
+ tmp[i] = a / rhs[i];
+
+ return tmp;
+}
+
+template <typename T>
+inline Vector<T> operator^(const Vector<T>& lhs, const Vector<T>& rhs)
+{
+ if (lhs.size() != rhs.size())
+ throw std::logic_error("Operator^: vectors have different sizes");
+ Vector<T> tmp(lhs.size());
+ for (unsigned int i = 0; i < lhs.size(); i++)
+ tmp[i] = pow(lhs[i], rhs[i]);
+
+ return tmp;
+}
+
+template <typename T>
+inline Vector<T> operator^(const Vector<T>& lhs, const T& a)
+{
+ Vector<T> tmp(lhs.size());
+ for (unsigned int i = 0; i < lhs.size(); i++)
+ tmp[i] = pow(lhs[i], a);
+
+ return tmp;
+}
+
+template <typename T>
+inline Vector<T> operator^(const T& a, const Vector<T>& rhs)
+{
+ Vector<T> tmp(rhs.size());
+ for (unsigned int i = 0; i < rhs.size(); i++)
+ tmp[i] = pow(a, rhs[i]);
+
+ return tmp;
+}
+
+template <typename T>
+inline Vector<T>& Vector<T>::operator^=(const Vector<T>& rhs)
+{
+ if (this->size() != rhs.size())
+ throw std::logic_error("Operator^=: vectors have different sizes");
+ for (unsigned int i = 0; i < n; i++)
+ v[i] = pow(v[i], rhs[i]);
+
+ return *this;
+}
+
+template <typename T>
+inline Vector<T>& Vector<T>::operator^=(const T& a)
+{
+ for (unsigned int i = 0; i < n; i++)
+ v[i] = pow(v[i], a);
+
+ return *this;
+}
+
+template <typename T>
+inline bool operator==(const Vector<T>& v, const Vector<T>& w)
+{
+ if (v.size() != w.size())
+ throw std::logic_error("Vectors of different size are not confrontable");
+ for (unsigned i = 0; i < v.size(); i++)
+ if (v[i] != w[i])
+ return false;
+ return true;
+}
+
+template <typename T>
+inline bool operator!=(const Vector<T>& v, const Vector<T>& w)
+{
+ if (v.size() != w.size())
+ throw std::logic_error("Vectors of different size are not confrontable");
+ for (unsigned i = 0; i < v.size(); i++)
+ if (v[i] != w[i])
+ return true;
+ return false;
+}
+
+template <typename T>
+inline bool operator<(const Vector<T>& v, const Vector<T>& w)
+{
+ if (v.size() != w.size())
+ throw std::logic_error("Vectors of different size are not confrontable");
+ for (unsigned i = 0; i < v.size(); i++)
+ if (v[i] >= w[i])
+ return false;
+ return true;
+}
+
+template <typename T>
+inline bool operator<=(const Vector<T>& v, const Vector<T>& w)
+{
+ if (v.size() != w.size())
+ throw std::logic_error("Vectors of different size are not confrontable");
+ for (unsigned i = 0; i < v.size(); i++)
+ if (v[i] > w[i])
+ return false;
+ return true;
+}
+
+template <typename T>
+inline bool operator>(const Vector<T>& v, const Vector<T>& w)
+{
+ if (v.size() != w.size())
+ throw std::logic_error("Vectors of different size are not confrontable");
+ for (unsigned i = 0; i < v.size(); i++)
+ if (v[i] <= w[i])
+ return false;
+ return true;
+}
+
+template <typename T>
+inline bool operator>=(const Vector<T>& v, const Vector<T>& w)
+{
+ if (v.size() != w.size())
+ throw std::logic_error("Vectors of different size are not confrontable");
+ for (unsigned i = 0; i < v.size(); i++)
+ if (v[i] < w[i])
+ return false;
+ return true;
+}
+
+/**
+ Input/Output
+*/
+template <typename T>
+inline std::ostream& operator<<(std::ostream& os, const Vector<T>& v)
+{
+ os << std::endl << v.size() << std::endl;
+ for (unsigned int i = 0; i < v.size() - 1; i++)
+ os << std::setw(20) << std::setprecision(16) << v[i] << ", ";
+ os << std::setw(20) << std::setprecision(16) << v[v.size() - 1] << std::endl;
+
+ return os;
+}
+
+template <typename T>
+std::istream& operator>>(std::istream& is, Vector<T>& v)
+{
+ int elements;
+ char comma;
+ is >> elements;
+ v.resize(elements);
+ for (unsigned int i = 0; i < elements; i++)
+ is >> v[i] >> comma;
+
+ return is;
+}
+
+/**
+ Index utilities
+*/
+
+std::set<unsigned int> seq(unsigned int s, unsigned int e);
+
+std::set<unsigned int> singleton(unsigned int i);
+
+template <typename T>
+class CanonicalBaseVector : public Vector<T>
+{
+public:
+ CanonicalBaseVector(unsigned int i, unsigned int n);
+ inline void reset(unsigned int i);
+private:
+ unsigned int e;
+};
+
+template <typename T>
+CanonicalBaseVector<T>::CanonicalBaseVector(unsigned int i, unsigned int n)
+ : Vector<T>((T)0, n), e(i)
+{ (*this)[e] = (T)1; }
+
+template <typename T>
+inline void CanonicalBaseVector<T>::reset(unsigned int i)
+{
+ (*this)[e] = (T)0;
+ e = i;
+ (*this)[e] = (T)1;
+}
+
+#include <stdexcept>
+
+template <typename T>
+inline T sum(const Vector<T>& v)
+{
+ T tmp = (T)0;
+ for (unsigned int i = 0; i < v.size(); i++)
+ tmp += v[i];
+
+ return tmp;
+}
+
+template <typename T>
+inline T prod(const Vector<T>& v)
+{
+ T tmp = (T)1;
+ for (unsigned int i = 0; i < v.size(); i++)
+ tmp *= v[i];
+
+ return tmp;
+}
+
+template <typename T>
+inline T mean(const Vector<T>& v)
+{
+ T sum = (T)0;
+ for (unsigned int i = 0; i < v.size(); i++)
+ sum += v[i];
+ return sum / v.size();
+}
+
+template <typename T>
+inline T median(const Vector<T>& v)
+{
+ Vector<T> tmp = sort(v);
+ if (v.size() % 2 == 1) // it is an odd-sized vector
+ return tmp[v.size() / 2];
+ else
+ return 0.5 * (tmp[v.size() / 2 - 1] + tmp[v.size() / 2]);
+}
+
+template <typename T>
+inline T stdev(const Vector<T>& v, bool sample_correction = false)
+{
+ return sqrt(var(v, sample_correction));
+}
+
+template <typename T>
+inline T var(const Vector<T>& v, bool sample_correction = false)
+{
+ T sum = (T)0, ssum = (T)0;
+ unsigned int n = v.size();
+ for (unsigned int i = 0; i < n; i++)
+ {
+ sum += v[i];
+ ssum += (v[i] * v[i]);
+ }
+ if (!sample_correction)
+ return (ssum / n) - (sum / n) * (sum / n);
+ else
+ return n * ((ssum / n) - (sum / n) * (sum / n)) / (n - 1);
+}
+
+template <typename T>
+inline T max(const Vector<T>& v)
+{
+ T value = v[0];
+ for (unsigned int i = 1; i < v.size(); i++)
+ value = std::max(v[i], value);
+
+ return value;
+}
+
+template <typename T>
+inline T min(const Vector<T>& v)
+{
+ T value = v[0];
+ for (unsigned int i = 1; i < v.size(); i++)
+ value = std::min(v[i], value);
+
+ return value;
+}
+
+template <typename T>
+inline unsigned int index_max(const Vector<T>& v)
+{
+ unsigned int max = 0;
+ for (unsigned int i = 1; i < v.size(); i++)
+ if (v[i] > v[max])
+ max = i;
+
+ return max;
+}
+
+template <typename T>
+inline unsigned int index_min(const Vector<T>& v)
+{
+ unsigned int min = 0;
+ for (unsigned int i = 1; i < v.size(); i++)
+ if (v[i] < v[min])
+ min = i;
+
+ return min;
+}
+
+
+template <typename T>
+inline T dot_prod(const Vector<T>& a, const Vector<T>& b)
+{
+ T sum = (T)0;
+ if (a.size() != b.size())
+ throw std::logic_error("Dotprod error: the vectors are not the same size");
+ for (unsigned int i = 0; i < a.size(); i++)
+ sum += a[i] * b[i];
+
+ return sum;
+}
+
+/**
+ Single element mathematical functions
+*/
+
+template <typename T>
+inline Vector<T> exp(const Vector<T>& v)
+{
+ Vector<T> tmp(v.size());
+ for (unsigned int i = 0; i < v.size(); i++)
+ tmp[i] = exp(v[i]);
+
+ return tmp;
+}
+
+template <typename T>
+inline Vector<T> log(const Vector<T>& v)
+{
+ Vector<T> tmp(v.size());
+ for (unsigned int i = 0; i < v.size(); i++)
+ tmp[i] = log(v[i]);
+
+ return tmp;
+}
+
+template <typename T>
+inline Vector<T> vec_sqrt(const Vector<T>& v)
+{
+ Vector<T> tmp(v.size());
+ for (unsigned int i = 0; i < v.size(); i++)
+ tmp[i] = sqrt(v[i]);
+
+ return tmp;
+}
+
+template <typename T>
+inline Vector<T> pow(const Vector<T>& v, double a)
+{
+ Vector<T> tmp(v.size());
+ for (unsigned int i = 0; i < v.size(); i++)
+ tmp[i] = pow(v[i], a);
+
+ return tmp;
+}
+
+template <typename T>
+inline Vector<T> abs(const Vector<T>& v)
+{
+ Vector<T> tmp(v.size());
+ for (unsigned int i = 0; i < v.size(); i++)
+ tmp[i] = (T)fabs(v[i]);
+
+ return tmp;
+}
+
+template <typename T>
+inline Vector<T> sign(const Vector<T>& v)
+{
+ Vector<T> tmp(v.size());
+ for (unsigned int i = 0; i < v.size(); i++)
+ tmp[i] = v[i] > 0 ? +1 : v[i] == 0 ? 0 : -1;
+
+ return tmp;
+}
+
+template <typename T>
+inline unsigned int partition(Vector<T>& v, unsigned int begin, unsigned int end)
+{
+ unsigned int i = begin + 1, j = begin + 1;
+ T pivot = v[begin];
+ while (j <= end)
+ {
+ if (v[j] < pivot) {
+ std::swap(v[i], v[j]);
+ i++;
+ }
+ j++;
+ }
+ v[begin] = v[i - 1];
+ v[i - 1] = pivot;
+ return i - 2;
+}
+
+
+template <typename T>
+inline void quicksort(Vector<T>& v, unsigned int begin, unsigned int end)
+{
+ if (end > begin)
+ {
+ unsigned int index = partition(v, begin, end);
+ quicksort(v, begin, index);
+ quicksort(v, index + 2, end);
+ }
+}
+
+template <typename T>
+inline Vector<T> sort(const Vector<T>& v)
+{
+ Vector<T> tmp(v);
+
+ quicksort<T>(tmp, 0, tmp.size() - 1);
+
+ return tmp;
+}
+
+template <typename T>
+inline Vector<double> rank(const Vector<T>& v)
+{
+ Vector<T> tmp(v);
+ Vector<double> tmp_rank(0.0, v.size());
+
+ for (unsigned int i = 0; i < tmp.size(); i++)
+ {
+ unsigned int smaller = 0, equal = 0;
+ for (unsigned int j = 0; j < tmp.size(); j++)
+ if (i == j)
+ continue;
+ else
+ if (tmp[j] < tmp[i])
+ smaller++;
+ else if (tmp[j] == tmp[i])
+ equal++;
+ tmp_rank[i] = smaller + 1;
+ if (equal > 0)
+ {
+ for (unsigned int j = 1; j <= equal; j++)
+ tmp_rank[i] += smaller + 1 + j;
+ tmp_rank[i] /= (double)(equal + 1);
+ }
+ }
+
+ return tmp_rank;
+}
+
+//enum MType { DIAG };
+
+template <typename T>
+class Matrix
+{
+public:
+ Matrix(); // Default constructor
+ Matrix(const unsigned int n, const unsigned int m); // Construct a n x m matrix
+ Matrix(const T& a, const unsigned int n, const unsigned int m); // Initialize the content to constant a
+ Matrix(MType t, const T& a, const T& o, const unsigned int n, const unsigned int m);
+ Matrix(MType t, const Vector<T>& v, const T& o, const unsigned int n, const unsigned int m);
+ Matrix(const T* a, const unsigned int n, const unsigned int m); // Initialize to array
+ Matrix(const Matrix<T>& rhs); // Copy constructor
+ ~Matrix(); // destructor
+
+ inline T* operator[](const unsigned int& i) { return v[i]; } // Subscripting: row i
+ inline const T* operator[](const unsigned int& i) const { return v[i]; }; // const subsctipting
+
+ inline void resize(const unsigned int n, const unsigned int m);
+ inline void resize(const T& a, const unsigned int n, const unsigned int m);
+
+
+ inline Vector<T> extractRow(const unsigned int i) const;
+ inline Vector<T> extractColumn(const unsigned int j) const;
+ inline Vector<T> extractDiag() const;
+ inline Matrix<T> extractRows(const std::set<unsigned int>& indexes) const;
+ inline Matrix<T> extractColumns(const std::set<unsigned int>& indexes) const;
+ inline Matrix<T> extract(const std::set<unsigned int>& r_indexes, const std::set<unsigned int>& c_indexes) const;
+
+ inline void set(const T* a, unsigned int n, unsigned int m);
+ inline void set(const std::set<unsigned int>& r_indexes, const std::set<unsigned int>& c_indexes, const Matrix<T>& m);
+ inline void setRow(const unsigned int index, const Vector<T>& v);
+ inline void setRow(const unsigned int index, const Matrix<T>& v);
+ inline void setRows(const std::set<unsigned int>& indexes, const Matrix<T>& m);
+ inline void setColumn(const unsigned int index, const Vector<T>& v);
+ inline void setColumn(const unsigned int index, const Matrix<T>& v);
+ inline void setColumns(const std::set<unsigned int>& indexes, const Matrix<T>& m);
+
+
+ inline unsigned int nrows() const { return n; } // number of rows
+ inline unsigned int ncols() const { return m; } // number of columns
+
+ inline Matrix<T>& operator=(const Matrix<T>& rhs); // Assignment operator
+ inline Matrix<T>& operator=(const T& a); // Assign to every element value a
+ inline Matrix<T>& operator+=(const Matrix<T>& rhs);
+ inline Matrix<T>& operator-=(const Matrix<T>& rhs);
+ inline Matrix<T>& operator*=(const Matrix<T>& rhs);
+ inline Matrix<T>& operator/=(const Matrix<T>& rhs);
+ inline Matrix<T>& operator^=(const Matrix<T>& rhs);
+ inline Matrix<T>& operator+=(const T& a);
+ inline Matrix<T>& operator-=(const T& a);
+ inline Matrix<T>& operator*=(const T& a);
+ inline Matrix<T>& operator/=(const T& a);
+ inline Matrix<T>& operator^=(const T& a);
+ inline operator Vector<T>();
+private:
+ unsigned int n; // number of rows
+ unsigned int m; // number of columns
+ T **v; // storage for data
+};
+
+template <typename T>
+Matrix<T>::Matrix()
+ : n(0), m(0), v(0)
+{}
+
+template <typename T>
+Matrix<T>::Matrix(unsigned int n, unsigned int m)
+ : v(new T*[n])
+{
+ register unsigned int i;
+ this->n = n; this->m = m;
+ v[0] = new T[m * n];
+ for (i = 1; i < n; i++)
+ v[i] = v[i - 1] + m;
+}
+
+template <typename T>
+Matrix<T>::Matrix(const T& a, unsigned int n, unsigned int m)
+ : v(new T*[n])
+{
+ register unsigned int i, j;
+ this->n = n; this->m = m;
+ v[0] = new T[m * n];
+ for (i = 1; i < n; i++)
+ v[i] = v[i - 1] + m;
+ for (i = 0; i < n; i++)
+ for (j = 0; j < m; j++)
+ v[i][j] = a;
+}
+
+template <class T>
+Matrix<T>::Matrix(const T* a, unsigned int n, unsigned int m)
+ : v(new T*[n])
+{
+ register unsigned int i, j;
+ this->n = n; this->m = m;
+ v[0] = new T[m * n];
+ for (i = 1; i < n; i++)
+ v[i] = v[i - 1] + m;
+ for (i = 0; i < n; i++)
+ for (j = 0; j < m; j++)
+ v[i][j] = *a++;
+}
+
+template <class T>
+Matrix<T>::Matrix(MType t, const T& a, const T& o, unsigned int n, unsigned int m)
+ : v(new T*[n])
+{
+ register unsigned int i, j;
+ this->n = n; this->m = m;
+ v[0] = new T[m * n];
+ for (i = 1; i < n; i++)
+ v[i] = v[i - 1] + m;
+ switch (t)
+ {
+ case DIAG:
+ for (i = 0; i < n; i++)
+ for (j = 0; j < m; j++)
+ if (i != j)
+ v[i][j] = o;
+ else
+ v[i][j] = a;
+ break;
+ default:
+ throw std::logic_error("Matrix type not supported");
+ }
+}
+
+template <class T>
+Matrix<T>::Matrix(MType t, const Vector<T>& a, const T& o, unsigned int n, unsigned int m)
+ : v(new T*[n])
+{
+ register unsigned int i, j;
+ this->n = n; this->m = m;
+ v[0] = new T[m * n];
+ for (i = 1; i < n; i++)
+ v[i] = v[i - 1] + m;
+ switch (t)
+ {
+ case DIAG:
+ for (i = 0; i < n; i++)
+ for (j = 0; j < m; j++)
+ if (i != j)
+ v[i][j] = o;
+ else
+ v[i][j] = a[i];
+ break;
+ default:
+ throw std::logic_error("Matrix type not supported");
+ }
+}
+
+template <typename T>
+Matrix<T>::Matrix(const Matrix<T>& rhs)
+ : v(new T*[rhs.n])
+{
+ register unsigned int i, j;
+ n = rhs.n; m = rhs.m;
+ v[0] = new T[m * n];
+ for (i = 1; i < n; i++)
+ v[i] = v[i - 1] + m;
+ for (i = 0; i < n; i++)
+ for (j = 0; j < m; j++)
+ v[i][j] = rhs[i][j];
+}
+
+template <typename T>
+Matrix<T>::~Matrix()
+{
+ if (v != 0) {
+ delete[] (v[0]);
+ delete[] (v);
+ }
+}
+
+template <typename T>
+inline Matrix<T>& Matrix<T>::operator=(const Matrix<T> &rhs)
+// postcondition: normal assignment via copying has been performed;
+// if matrix and rhs were different sizes, matrix
+// has been resized to match the size of rhs
+{
+ register unsigned int i, j;
+ if (this != &rhs)
+ {
+ resize(rhs.n, rhs.m);
+ for (i = 0; i < n; i++)
+ for (j = 0; j < m; j++)
+ v[i][j] = rhs[i][j];
+ }
+ return *this;
+}
+
+template <typename T>
+inline Matrix<T>& Matrix<T>::operator=(const T& a) // assign a to every element
+{
+ register unsigned int i, j;
+ for (i = 0; i < n; i++)
+ for (j = 0; j < m; j++)
+ v[i][j] = a;
+ return *this;
+}
+
+
+template <typename T>
+inline void Matrix<T>::resize(const unsigned int n, const unsigned int m)
+{
+ register unsigned int i;
+ if (n == this->n && m == this->m)
+ return;
+ if (v != 0)
+ {
+ delete[] (v[0]);
+ delete[] (v);
+ }
+ this->n = n; this->m = m;
+ v = new T*[n];
+ v[0] = new T[m * n];
+ for (i = 1; i < n; i++)
+ v[i] = v[i - 1] + m;
+}
+
+template <typename T>
+inline void Matrix<T>::resize(const T& a, const unsigned int n, const unsigned int m)
+{
+ register unsigned int i, j;
+ resize(n, m);
+ for (i = 0; i < n; i++)
+ for (j = 0; j < m; j++)
+ v[i][j] = a;
+}
+
+
+
+template <typename T>
+inline Vector<T> Matrix<T>::extractRow(const unsigned int i) const
+{
+ if (i >= n)
+ throw std::logic_error("Error in extractRow: trying to extract a row out of matrix bounds");
+ Vector<T> tmp(v[i], m);
+
+ return tmp;
+}
+
+template <typename T>
+inline Vector<T> Matrix<T>::extractColumn(const unsigned int j) const
+{
+ register unsigned int i;
+ if (j >= m)
+ throw std::logic_error("Error in extractRow: trying to extract a row out of matrix bounds");
+ Vector<T> tmp(n);
+
+ for (i = 0; i < n; i++)
+ tmp[i] = v[i][j];
+
+ return tmp;
+}
+
+template <typename T>
+inline Vector<T> Matrix<T>::extractDiag() const
+{
+ register unsigned int d = std::min(n, m), i;
+
+ Vector<T> tmp(d);
+
+ for (i = 0; i < d; i++)
+ tmp[i] = v[i][i];
+
+ return tmp;
+
+}
+
+template <typename T>
+inline Matrix<T> Matrix<T>::extractRows(const std::set<unsigned int>& indexes) const
+{
+ Matrix<T> tmp(indexes.size(), m);
+ register unsigned int i = 0, j;
+
+ for (std::set<unsigned int>::const_iterator el = indexes.begin(); el != indexes.end(); el++)
+ {
+ for (j = 0; j < m; j++)
+ {
+ if (*el >= n)
+ throw std::logic_error("Error extracting rows: the indexes are out of matrix bounds");
+ tmp[i][j] = v[*el][j];
+ }
+ i++;
+ }
+
+ return tmp;
+}
+
+template <typename T>
+inline Matrix<T> Matrix<T>::extractColumns(const std::set<unsigned int>& indexes) const
+{
+ Matrix<T> tmp(n, indexes.size());
+ register unsigned int i, j = 0;
+
+ for (std::set<unsigned int>::const_iterator el = indexes.begin(); el != indexes.end(); el++)
+ {
+ for (i = 0; i < n; i++)
+ {
+ if (*el >= m)
+ throw std::logic_error("Error extracting columns: the indexes are out of matrix bounds");
+ tmp[i][j] = v[i][*el];
+ }
+ j++;
+ }
+
+ return tmp;
+}
+
+template <typename T>
+inline Matrix<T> Matrix<T>::extract(const std::set<unsigned int>& r_indexes, const std::set<unsigned int>& c_indexes) const
+{
+ Matrix<T> tmp(r_indexes.size(), c_indexes.size());
+ register unsigned int i = 0, j;
+
+ for (std::set<unsigned int>::const_iterator r_el = r_indexes.begin(); r_el != r_indexes.end(); r_el++)
+ {
+ if (*r_el >= n)
+ throw std::logic_error("Error extracting submatrix: the indexes are out of matrix bounds");
+ j = 0;
+ for (std::set<unsigned int>::const_iterator c_el = c_indexes.begin(); c_el != c_indexes.end(); c_el++)
+ {
+ if (*c_el >= m)
+ throw std::logic_error("Error extracting rows: the indexes are out of matrix bounds");
+ tmp[i][j] = v[*r_el][*c_el];
+ j++;
+ }
+ i++;
+ }
+
+ return tmp;
+}
+
+template <typename T>
+inline void Matrix<T>::setRow(unsigned int i, const Vector<T>& a)
+{
+ if (i >= n)
+ throw std::logic_error("Error in setRow: trying to set a row out of matrix bounds");
+ if (this->m != a.size())
+ throw std::logic_error("Error setting matrix row: ranges are not compatible");
+ for (unsigned int j = 0; j < ncols(); j++)
+ v[i][j] = a[j];
+}
+
+template <typename T>
+inline void Matrix<T>::setRow(unsigned int i, const Matrix<T>& a)
+{
+ if (i >= n)
+ throw std::logic_error("Error in setRow: trying to set a row out of matrix bounds");
+ if (this->m != a.ncols())
+ throw std::logic_error("Error setting matrix column: ranges are not compatible");
+ if (a.nrows() != 1)
+ throw std::logic_error("Error setting matrix column with a non-row matrix");
+ for (unsigned int j = 0; j < ncols(); j++)
+ v[i][j] = a[0][j];
+}
+
+template <typename T>
+inline void Matrix<T>::setRows(const std::set<unsigned int>& indexes, const Matrix<T>& m)
+{
+ unsigned int i = 0;
+
+ if (indexes.size() != m.nrows() || this->m != m.ncols())
+ throw std::logic_error("Error setting matrix rows: ranges are not compatible");
+ for (std::set<unsigned int>::const_iterator el = indexes.begin(); el != indexes.end(); el++)
+ {
+ for (unsigned int j = 0; j < ncols(); j++)
+ {
+ if (*el >= n)
+ throw std::logic_error("Error in setRows: trying to set a row out of matrix bounds");
+ v[*el][j] = m[i][j];
+ }
+ i++;
+ }
+}
+
+template <typename T>
+inline void Matrix<T>::setColumn(unsigned int j, const Vector<T>& a)
+{
+ if (j >= m)
+ throw std::logic_error("Error in setColumn: trying to set a column out of matrix bounds");
+ if (this->n != a.size())
+ throw std::logic_error("Error setting matrix column: ranges are not compatible");
+ for (unsigned int i = 0; i < nrows(); i++)
+ v[i][j] = a[i];
+}
+
+template <typename T>
+inline void Matrix<T>::setColumn(unsigned int j, const Matrix<T>& a)
+{
+ if (j >= m)
+ throw std::logic_error("Error in setColumn: trying to set a column out of matrix bounds");
+ if (this->n != a.nrows())
+ throw std::logic_error("Error setting matrix column: ranges are not compatible");
+ if (a.ncols() != 1)
+ throw std::logic_error("Error setting matrix column with a non-column matrix");
+ for (unsigned int i = 0; i < nrows(); i++)
+ v[i][j] = a[i][0];
+}
+
+
+template <typename T>
+inline void Matrix<T>::setColumns(const std::set<unsigned int>& indexes, const Matrix<T>& a)
+{
+ unsigned int j = 0;
+
+ if (indexes.size() != a.ncols() || this->n != a.nrows())
+ throw std::logic_error("Error setting matrix columns: ranges are not compatible");
+ for (std::set<unsigned int>::const_iterator el = indexes.begin(); el != indexes.end(); el++)
+ {
+ for (unsigned int i = 0; i < nrows(); i++)
+ {
+ if (*el >= m)
+ throw std::logic_error("Error in setColumns: trying to set a column out of matrix bounds");
+ v[i][*el] = a[i][j];
+ }
+ j++;
+ }
+}
+
+template <typename T>
+inline void Matrix<T>::set(const std::set<unsigned int>& r_indexes, const std::set<unsigned int>& c_indexes, const Matrix<T>& a)
+{
+ unsigned int i = 0, j;
+ if (c_indexes.size() != a.ncols() || r_indexes.size() != a.nrows())
+ throw std::logic_error("Error setting matrix elements: ranges are not compatible");
+
+ for (std::set<unsigned int>::const_iterator r_el = r_indexes.begin(); r_el != r_indexes.end(); r_el++)
+ {
+ if (*r_el >= n)
+ throw std::logic_error("Error in set: trying to set a row out of matrix bounds");
+ j = 0;
+ for (std::set<unsigned int>::const_iterator c_el = c_indexes.begin(); c_el != c_indexes.end(); c_el++)
+ {
+ if (*c_el >= m)
+ throw std::logic_error("Error in set: trying to set a column out of matrix bounds");
+ v[*r_el][*c_el] = a[i][j];
+ j++;
+ }
+ i++;
+ }
+}
+
+template <typename T>
+inline void Matrix<T>::set(const T* a, unsigned int n, unsigned int m)
+{
+ if (this->n != n || this->m != m)
+ resize(n, m);
+ unsigned int k = 0;
+ for (unsigned int i = 0; i < n; i++)
+ for (unsigned int j = 0; j < m; j++)
+ v[i][j] = a[k++];
+}
+
+
+template <typename T>
+Matrix<T> operator+(const Matrix<T>& rhs)
+{
+ return rhs;
+}
+
+template <typename T>
+Matrix<T> operator+(const Matrix<T>& lhs, const Matrix<T>& rhs)
+{
+ if (lhs.ncols() != rhs.ncols() || lhs.nrows() != rhs.nrows())
+ throw std::logic_error("Operator+: matrices have different sizes");
+ Matrix<T> tmp(lhs.nrows(), lhs.ncols());
+ for (unsigned int i = 0; i < lhs.nrows(); i++)
+ for (unsigned int j = 0; j < lhs.ncols(); j++)
+ tmp[i][j] = lhs[i][j] + rhs[i][j];
+
+ return tmp;
+}
+
+template <typename T>
+Matrix<T> operator+(const Matrix<T>& lhs, const T& a)
+{
+ Matrix<T> tmp(lhs.nrows(), lhs.ncols());
+ for (unsigned int i = 0; i < lhs.nrows(); i++)
+ for (unsigned int j = 0; j < lhs.ncols(); j++)
+ tmp[i][j] = lhs[i][j] + a;
+
+ return tmp;
+}
+
+template <typename T>
+Matrix<T> operator+(const T& a, const Matrix<T>& rhs)
+{
+ Matrix<T> tmp(rhs.nrows(), rhs.ncols());
+ for (unsigned int i = 0; i < rhs.nrows(); i++)
+ for (unsigned int j = 0; j < rhs.ncols(); j++)
+ tmp[i][j] = a + rhs[i][j];
+
+ return tmp;
+}
+
+template <typename T>
+inline Matrix<T>& Matrix<T>::operator+=(const Matrix<T>& rhs)
+{
+ if (m != rhs.ncols() || n != rhs.nrows())
+ throw std::logic_error("Operator+=: matrices have different sizes");
+ for (unsigned int i = 0; i < n; i++)
+ for (unsigned int j = 0; j < m; j++)
+ v[i][j] += rhs[i][j];
+
+ return *this;
+}
+
+template <typename T>
+inline Matrix<T>& Matrix<T>::operator+=(const T& a)
+{
+ for (unsigned int i = 0; i < n; i++)
+ for (unsigned int j = 0; j < m; j++)
+ v[i][j] += a;
+
+ return *this;
+}
+
+template <typename T>
+Matrix<T> operator-(const Matrix<T>& rhs)
+{
+ return (T)(-1) * rhs;
+}
+
+template <typename T>
+Matrix<T> operator-(const Matrix<T>& lhs, const Matrix<T>& rhs)
+{
+ if (lhs.ncols() != rhs.ncols() || lhs.nrows() != rhs.nrows())
+ throw std::logic_error("Operator-: matrices have different sizes");
+ Matrix<T> tmp(lhs.nrows(), lhs.ncols());
+ for (unsigned int i = 0; i < lhs.nrows(); i++)
+ for (unsigned int j = 0; j < lhs.ncols(); j++)
+ tmp[i][j] = lhs[i][j] - rhs[i][j];
+
+ return tmp;
+}
+
+template <typename T>
+Matrix<T> operator-(const Matrix<T>& lhs, const T& a)
+{
+ Matrix<T> tmp(lhs.nrows(), lhs.ncols());
+ for (unsigned int i = 0; i < lhs.nrows(); i++)
+ for (unsigned int j = 0; j < lhs.ncols(); j++)
+ tmp[i][j] = lhs[i][j] - a;
+
+ return tmp;
+}
+
+template <typename T>
+Matrix<T> operator-(const T& a, const Matrix<T>& rhs)
+{
+ Matrix<T> tmp(rhs.nrows(), rhs.ncols());
+ for (unsigned int i = 0; i < rhs.nrows(); i++)
+ for (unsigned int j = 0; j < rhs.ncols(); j++)
+ tmp[i][j] = a - rhs[i][j];
+
+ return tmp;
+}
+
+template <typename T>
+inline Matrix<T>& Matrix<T>::operator-=(const Matrix<T>& rhs)
+{
+ if (m != rhs.ncols() || n != rhs.nrows())
+ throw std::logic_error("Operator-=: matrices have different sizes");
+ for (unsigned int i = 0; i < n; i++)
+ for (unsigned int j = 0; j < m; j++)
+ v[i][j] -= rhs[i][j];
+
+ return *this;
+}
+
+template <typename T>
+inline Matrix<T>& Matrix<T>::operator-=(const T& a)
+{
+ for (unsigned int i = 0; i < n; i++)
+ for (unsigned int j = 0; j < m; j++)
+ v[i][j] -= a;
+
+ return *this;
+}
+
+template <typename T>
+Matrix<T> operator*(const Matrix<T>& lhs, const Matrix<T>& rhs)
+{
+ if (lhs.ncols() != rhs.ncols() || lhs.nrows() != rhs.nrows())
+ throw std::logic_error("Operator*: matrices have different sizes");
+ Matrix<T> tmp(lhs.nrows(), lhs.ncols());
+ for (unsigned int i = 0; i < lhs.nrows(); i++)
+ for (unsigned int j = 0; j < lhs.ncols(); j++)
+ tmp[i][j] = lhs[i][j] * rhs[i][j];
+
+ return tmp;
+}
+
+template <typename T>
+Matrix<T> operator*(const Matrix<T>& lhs, const T& a)
+{
+ Matrix<T> tmp(lhs.nrows(), lhs.ncols());
+ for (unsigned int i = 0; i < lhs.nrows(); i++)
+ for (unsigned int j = 0; j < lhs.ncols(); j++)
+ tmp[i][j] = lhs[i][j] * a;
+
+ return tmp;
+}
+
+template <typename T>
+Matrix<T> operator*(const T& a, const Matrix<T>& rhs)
+{
+ Matrix<T> tmp(rhs.nrows(), rhs.ncols());
+ for (unsigned int i = 0; i < rhs.nrows(); i++)
+ for (unsigned int j = 0; j < rhs.ncols(); j++)
+ tmp[i][j] = a * rhs[i][j];
+
+ return tmp;
+}
+
+template <typename T>
+inline Matrix<T>& Matrix<T>::operator*=(const Matrix<T>& rhs)
+{
+ if (m != rhs.ncols() || n != rhs.nrows())
+ throw std::logic_error("Operator*=: matrices have different sizes");
+ for (unsigned int i = 0; i < n; i++)
+ for (unsigned int j = 0; j < m; j++)
+ v[i][j] *= rhs[i][j];
+
+ return *this;
+}
+
+template <typename T>
+inline Matrix<T>& Matrix<T>::operator*=(const T& a)
+{
+ for (unsigned int i = 0; i < n; i++)
+ for (unsigned int j = 0; j < m; j++)
+ v[i][j] *= a;
+
+ return *this;
+}
+
+template <typename T>
+Matrix<T> operator/(const Matrix<T>& lhs, const Matrix<T>& rhs)
+{
+ if (lhs.ncols() != rhs.ncols() || lhs.nrows() != rhs.nrows())
+ throw std::logic_error("Operator+: matrices have different sizes");
+ Matrix<T> tmp(lhs.nrows(), lhs.ncols());
+ for (unsigned int i = 0; i < lhs.nrows(); i++)
+ for (unsigned int j = 0; j < lhs.ncols(); j++)
+ tmp[i][j] = lhs[i][j] / rhs[i][j];
+
+ return tmp;
+}
+
+template <typename T>
+Matrix<T> operator/(const Matrix<T>& lhs, const T& a)
+{
+ Matrix<T> tmp(lhs.nrows(), lhs.ncols());
+ for (unsigned int i = 0; i < lhs.nrows(); i++)
+ for (unsigned int j = 0; j < lhs.ncols(); j++)
+ tmp[i][j] = lhs[i][j] / a;
+
+ return tmp;
+}
+
+template <typename T>
+Matrix<T> operator/(const T& a, const Matrix<T>& rhs)
+{
+ Matrix<T> tmp(rhs.nrows(), rhs.ncols());
+ for (unsigned int i = 0; i < rhs.nrows(); i++)
+ for (unsigned int j = 0; j < rhs.ncols(); j++)
+ tmp[i][j] = a / rhs[i][j];
+
+ return tmp;
+}
+
+template <typename T>
+inline Matrix<T>& Matrix<T>::operator/=(const Matrix<T>& rhs)
+{
+ if (m != rhs.ncols() || n != rhs.nrows())
+ throw std::logic_error("Operator+=: matrices have different sizes");
+ for (unsigned int i = 0; i < n; i++)
+ for (unsigned int j = 0; j < m; j++)
+ v[i][j] /= rhs[i][j];
+
+ return *this;
+}
+
+template <typename T>
+inline Matrix<T>& Matrix<T>::operator/=(const T& a)
+{
+ for (unsigned int i = 0; i < n; i++)
+ for (unsigned int j = 0; j < m; j++)
+ v[i][j] /= a;
+
+ return *this;
+}
+
+template <typename T>
+Matrix<T> operator^(const Matrix<T>& lhs, const T& a)
+{
+ Matrix<T> tmp(lhs.nrows(), lhs.ncols());
+ for (unsigned int i = 0; i < lhs.nrows(); i++)
+ for (unsigned int j = 0; j < lhs.ncols(); j++)
+ tmp[i][j] = pow(lhs[i][j], a);
+
+ return tmp;
+}
+
+template <typename T>
+inline Matrix<T>& Matrix<T>::operator^=(const Matrix<T>& rhs)
+{
+ if (m != rhs.ncols() || n != rhs.nrows())
+ throw std::logic_error("Operator^=: matrices have different sizes");
+ for (unsigned int i = 0; i < n; i++)
+ for (unsigned int j = 0; j < m; j++)
+ v[i][j] = pow(v[i][j], rhs[i][j]);
+
+ return *this;
+}
+
+
+template <typename T>
+inline Matrix<T>& Matrix<T>::operator^=(const T& a)
+{
+ for (unsigned int i = 0; i < n; i++)
+ for (unsigned int j = 0; j < m; j++)
+ v[i][j] = pow(v[i][j], a);
+
+ return *this;
+}
+
+template <typename T>
+inline Matrix<T>::operator Vector<T>()
+{
+ if (n > 1 && m > 1)
+ throw std::logic_error("Error matrix cast to vector: trying to cast a multi-dimensional matrix");
+ if (n == 1)
+ return extractRow(0);
+ else
+ return extractColumn(0);
+}
+
+template <typename T>
+inline bool operator==(const Matrix<T>& a, const Matrix<T>& b)
+{
+ if (a.nrows() != b.nrows() || a.ncols() != b.ncols())
+ throw std::logic_error("Matrices of different size are not confrontable");
+ for (unsigned i = 0; i < a.nrows(); i++)
+ for (unsigned j = 0; j < a.ncols(); j++)
+ if (a[i][j] != b[i][j])
+ return false;
+ return true;
+}
+
+template <typename T>
+inline bool operator!=(const Matrix<T>& a, const Matrix<T>& b)
+{
+ if (a.nrows() != b.nrows() || a.ncols() != b.ncols())
+ throw std::logic_error("Matrices of different size are not confrontable");
+ for (unsigned i = 0; i < a.nrows(); i++)
+ for (unsigned j = 0; j < a.ncols(); j++)
+ if (a[i][j] != b[i][j])
+ return true;
+ return false;
+}
+
+
+
+/**
+ Input/Output
+*/
+template <typename T>
+std::ostream& operator<<(std::ostream& os, const Matrix<T>& m)
+{
+ os << std::endl << m.nrows() << " " << m.ncols() << std::endl;
+ for (unsigned int i = 0; i < m.nrows(); i++)
+ {
+ for (unsigned int j = 0; j < m.ncols() - 1; j++)
+ os << std::setw(20) << std::setprecision(16) << m[i][j] << ", ";
+ os << std::setw(20) << std::setprecision(16) << m[i][m.ncols() - 1] << std::endl;
+ }
+
+ return os;
+}
+
+template <typename T>
+std::istream& operator>>(std::istream& is, Matrix<T>& m)
+{
+ int rows, cols;
+ char comma;
+ is >> rows >> cols;
+ m.resize(rows, cols);
+ for (unsigned int i = 0; i < rows; i++)
+ for (unsigned int j = 0; j < cols; j++)
+ is >> m[i][j] >> comma;
+
+ return is;
+}
+
+template <typename T>
+T sign(const T& v)
+{
+ if (v >= (T)0.0)
+ return (T)1.0;
+ else
+ return (T)-1.0;
+}
+
+template <typename T>
+T dist(const T& a, const T& b)
+{
+ T abs_a = (T)fabs(a), abs_b = (T)fabs(b);
+ if (abs_a > abs_b)
+ return abs_a * sqrt((T)1.0 + (abs_b / abs_a) * (abs_b / abs_a));
+ else
+ return (abs_b == (T)0.0 ? (T)0.0 : abs_b * sqrt((T)1.0 + (abs_a / abs_b) * (abs_a / abs_b)));
+}
+
+template <typename T>
+void svd(const Matrix<T>& A, Matrix<T>& U, Vector<T>& W, Matrix<T>& V)
+{
+ int m = A.nrows(), n = A.ncols(), i, j, k, l, jj, nm;
+ const unsigned int max_its = 30;
+ bool flag;
+ Vector<T> rv1(n);
+ U = A;
+ W.resize(n);
+ V.resize(n, n);
+ T anorm, c, f, g, h, s, scale, x, y, z;
+ g = scale = anorm = (T)0.0;
+
+ // Householder reduction to bidiagonal form
+ for (i = 0; i < n; i++)
+ {
+ l = i + 1;
+ rv1[i] = scale * g;
+ g = s = scale = (T)0.0;
+ if (i < m)
+ {
+ for (k = i; k < m; k++)
+ scale += fabs(U[k][i]);
+ if (scale != (T)0.0)
+ {
+ for (k = i; k < m; k++)
+ {
+ U[k][i] /= scale;
+ s += U[k][i] * U[k][i];
+ }
+ f = U[i][i];
+ g = -sign(f) * sqrt(s);
+ h = f * g - s;
+ U[i][i] = f - g;
+ for (j = l; j < n; j++)
+ {
+ s = (T)0.0;
+ for (k = i; k < m; k++)
+ s += U[k][i] * U[k][j];
+ f = s / h;
+ for (k = i; k < m; k++)
+ U[k][j] += f * U[k][i];
+ }
+ for (k = i; k < m; k++)
+ U[k][i] *= scale;
+ }
+ }
+ W[i] = scale * g;
+ g = s = scale = (T)0.0;
+ if (i < m && i != n - 1)
+ {
+ for (k = l; k < n; k++)
+ scale += fabs(U[i][k]);
+ if (scale != (T)0.0)
+ {
+ for (k = l; k < n; k++)
+ {
+ U[i][k] /= scale;
+ s += U[i][k] * U[i][k];
+ }
+ f = U[i][l];
+ g = -sign(f) * sqrt(s);
+ h = f * g - s;
+ U[i][l] = f - g;
+ for (k = l; k <n; k++)
+ rv1[k] = U[i][k] / h;
+ for (j = l; j < m; j++)
+ {
+ s = (T)0.0;
+ for (k = l; k < n; k++)
+ s += U[j][k] * U[i][k];
+ for (k = l; k < n; k++)
+ U[j][k] += s * rv1[k];
+ }
+ for (k = l; k < n; k++)
+ U[i][k] *= scale;
+ }
+ }
+ anorm = std::max(anorm, fabs(W[i]) + fabs(rv1[i]));
+ }
+ // Accumulation of right-hand transformations
+ for (i = n - 1; i >= 0; i--)
+ {
+ if (i < n - 1)
+ {
+ if (g != (T)0.0)
+ {
+ for (j = l; j < n; j++)
+ V[j][i] = (U[i][j] / U[i][l]) / g;
+ for (j = l; j < n; j++)
+ {
+ s = (T)0.0;
+ for (k = l; k < n; k++)
+ s += U[i][k] * V[k][j];
+ for (k = l; k < n; k++)
+ V[k][j] += s * V[k][i];
+ }
+ }
+ for (j = l; j < n; j++)
+ V[i][j] = V[j][i] = (T)0.0;
+ }
+ V[i][i] = (T)1.0;
+ g = rv1[i];
+ l = i;
+ }
+ // Accumulation of left-hand transformations
+ for (i = std::min(m, n) - 1; i >= 0; i--)
+ {
+ l = i + 1;
+ g = W[i];
+ for (j = l; j < n; j++)
+ U[i][j] = (T)0.0;
+ if (g != (T)0.0)
+ {
+ g = (T)1.0 / g;
+ for (j = l; j < n; j++)
+ {
+ s = (T)0.0;
+ for (k = l; k < m; k++)
+ s += U[k][i] * U[k][j];
+ f = (s / U[i][i]) * g;
+ for (k = i; k < m; k++)
+ U[k][j] += f * U[k][i];
+ }
+ for (j = i; j < m; j++)
+ U[j][i] *= g;
+ }
+ else
+ for (j = i; j < m; j++)
+ U[j][i] = (T)0.0;
+ U[i][i]++;
+ }
+ // Diagonalization of the bidiagonal form: loop over singular values, and over allowed iterations.
+ for (k = n - 1; k >= 0; k--)
+ {
+ for (unsigned int its = 0; its < max_its; its++)
+ {
+ flag = true;
+ for (l = k; l >= 0; l--) // FIXME: in NR it was l >= 1 but there subscripts start from one
+ { // Test for splitting
+ nm = l - 1; // Note that rV[0] is always zero
+ if ((T)(fabs(rv1[l]) + anorm) == anorm)
+ {
+ flag = false;
+ break;
+ }
+ if ((T)(fabs(W[nm]) + anorm) == anorm)
+ break;
+ }
+ if (flag)
+ {
+ // Cancellation of rv1[l], if l > 0 FIXME: it was l > 1 in NR
+ c = (T)0.0;
+ s = (T)1.0;
+ for (i = l; i <= k; i++)
+ {
+ f = s * rv1[i];
+ rv1[i] *= c;
+ if ((T)(fabs(f) + anorm) == anorm)
+ break;
+ g = W[i];
+ h = dist(f, g);
+ W[i] = h;
+ h = (T)1.0 / h;
+ c = g * h;
+ s = -f * h;
+ for (j = 0; j < m; j++)
+ {
+ y = U[j][nm];
+ z = U[j][i];
+ U[j][nm] = y * c + z * s;
+ U[j][i] = z * c - y * s;
+ }
+ }
+ }
+ z = W[k];
+ if (l == k)
+ { // Convergence
+ if (z < (T)0.0)
+ { // Singular value is made nonnegative
+ W[k] = -z;
+ for (j = 0; j < n; j++)
+ V[j][k] = -V[j][k];
+ }
+ break;
+ }
+ if (its == max_its)
+ throw std::logic_error("Error svd: no convergence in the maximum number of iterations");
+ x = W[l];
+ nm = k - 1;
+ y = W[nm];
+ g = rv1[nm];
+ h = rv1[k];
+ f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0 * h * y);
+ g = dist(f, (T)1.0);
+ f = ((x - z) * (x + z) + h * ((y / (f + sign(f)*fabs(g))) - h)) / x;
+ c = s = (T)1.0; // Next QR transformation
+ for (j = l; j <= nm; j++)
+ {
+ i = j + 1;
+ g = rv1[i];
+ y = W[i];
+ h = s * g;
+ g *= c;
+ z = dist(f, h);
+ rv1[j] = z;
+ c = f / z;
+ s = h / z;
+ f = x * c + g * s;
+ g = g * c - x * s;
+ h = y * s;
+ y *= c;
+ for (jj = 0; jj < n; jj++)
+ {
+ x = V[jj][j];
+ z = V[jj][i];
+ V[jj][j] = x * c + z * s;
+ V[jj][i] = z * c - x * s;
+ }
+ z = dist(f, h);
+ W[j] = z;
+ if (z != 0) // Rotation can be arbitrary if z = 0
+ {
+ z = (T)1.0 / z;
+ c = f * z;
+ s = h * z;
+ }
+ f = c * g + s * y;
+ x = c * y - s * g;
+ for (jj = 0; jj < m; jj++)
+ {
+ y = U[jj][j];
+ z = U[jj][i];
+ U[jj][j] = y * c + z * s;
+ U[jj][i] = z * c - y * s;
+ }
+ }
+ rv1[l] = (T)0.0;
+ rv1[k] = f;
+ W[k] = x;
+ }
+ }
+}
+
+template <typename T>
+Matrix<T> pinv(const Matrix<T>& A)
+{
+ Matrix<T> U, V, x, tmp(A.ncols(), A.nrows());
+ Vector<T> W;
+ CanonicalBaseVector<T> e(0, A.nrows());
+ svd(A, U, W, V);
+ for (unsigned int i = 0; i < A.nrows(); i++)
+ {
+ e.reset(i);
+ tmp.setColumn(i, dot_prod(dot_prod(dot_prod(V, Matrix<double>(DIAG, 1.0 / W, 0.0, W.size(), W.size())), t(U)), e));
+ }
+
+ return tmp;
+}
+
+template <typename T>
+int lu(const Matrix<T>& A, Matrix<T>& LU, Vector<unsigned int>& index)
+{
+ if (A.ncols() != A.nrows())
+ throw std::logic_error("Error in LU decomposition: matrix must be squared");
+ int i, p, j, k, n = A.ncols(), ex;
+ T val, tmp;
+ Vector<T> d(n);
+ LU = A;
+ index.resize(n);
+
+ ex = 1;
+ for (i = 0; i < n; i++)
+ {
+ index[i] = i;
+ val = (T)0.0;
+ for (j = 0; j < n; j++)
+ val = std::max(val, (T)fabs(LU[i][j]));
+ if (val == (T)0.0)
+ std::logic_error("Error in LU decomposition: matrix was singular");
+ d[i] = val;
+ }
+
+ for (k = 0; k < n - 1; k++)
+ {
+ p = k;
+ val = fabs(LU[k][k]) / d[k];
+ for (i = k + 1; i < n; i++)
+ {
+ tmp = fabs(LU[i][k]) / d[i];
+ if (tmp > val)
+ {
+ val = tmp;
+ p = i;
+ }
+ }
+ if (val == (T)0.0)
+ std::logic_error("Error in LU decomposition: matrix was singular");
+ if (p > k)
+ {
+ ex = -ex;
+ std::swap(index[k], index[p]);
+ std::swap(d[k], d[p]);
+ for (j = 0; j < n; j++)
+ std::swap(LU[k][j], LU[p][j]);
+ }
+
+ for (i = k + 1; i < n; i++)
+ {
+ LU[i][k] /= LU[k][k];
+ for (j = k + 1; j < n; j++)
+ LU[i][j] -= LU[i][k] * LU[k][j];
+ }
+ }
+ if (LU[n - 1][n - 1] == (T)0.0)
+ std::logic_error("Error in LU decomposition: matrix was singular");
+
+ return ex;
+}
+
+template <typename T>
+Vector<T> lu_solve(const Matrix<T>& LU, const Vector<T>& b, Vector<unsigned int>& index)
+{
+ if (LU.ncols() != LU.nrows())
+ throw std::logic_error("Error in LU solve: LU matrix should be squared");
+ unsigned int n = LU.ncols();
+ if (b.size() != n)
+ throw std::logic_error("Error in LU solve: b vector must be of the same dimensions of LU matrix");
+ Vector<T> x((T)0.0, n);
+ int i, j, p;
+ T sum;
+
+ p = index[0];
+ x[0] = b[p];
+
+ for (i = 1; i < n; i++)
+ {
+ sum = (T)0.0;
+ for (j = 0; j < i; j++)
+ sum += LU[i][j] * x[j];
+ p = index[i];
+ x[i] = b[p] - sum;
+ }
+ x[n - 1] /= LU[n - 1][n - 1];
+ for (i = n - 2; i >= 0; i--)
+ {
+ sum = (T)0.0;
+ for (j = i + 1; j < n; j++)
+ sum += LU[i][j] * x[j];
+ x[i] = (x[i] - sum) / LU[i][i];
+ }
+ return x;
+}
+
+template <typename T>
+void lu_solve(const Matrix<T>& LU, Vector<T>& x, const Vector<T>& b, Vector<unsigned int>& index)
+{
+ x = lu_solve(LU, b, index);
+}
+
+template <typename T>
+Matrix<T> lu_inverse(const Matrix<T>& A)
+{
+ if (A.ncols() != A.nrows())
+ throw std::logic_error("Error in LU invert: matrix must be squared");
+ unsigned int n = A.ncols();
+ Matrix<T> A1(n, n), LU;
+ Vector<unsigned int> index;
+
+ lu(A, LU, index);
+ CanonicalBaseVector<T> e(0, n);
+ for (unsigned i = 0; i < n; i++)
+ {
+ e.reset(i);
+ A1.setColumn(i, lu_solve(LU, e, index));
+ }
+
+ return A1;
+}
+
+template <typename T>
+T lu_det(const Matrix<T>& A)
+{
+ if (A.ncols() != A.nrows())
+ throw std::logic_error("Error in LU determinant: matrix must be squared");
+ unsigned int d;
+ Matrix<T> LU;
+ Vector<unsigned int> index;
+
+ d = lu(A, LU, index);
+
+ return d * prod(LU.extractDiag());
+}
+
+template <typename T>
+void cholesky(const Matrix<T> A, Matrix<T>& LL)
+{
+ if (A.ncols() != A.nrows())
+ throw std::logic_error("Error in Cholesky decomposition: matrix must be squared");
+ register int i, j, k, n = A.ncols();
+ register double sum;
+ LL = A;
+
+ for (i = 0; i < n; i++)
+ {
+ for (j = i; j < n; j++)
+ {
+ sum = LL[i][j];
+ for (k = i - 1; k >= 0; k--)
+ sum -= LL[i][k] * LL[j][k];
+ if (i == j)
+ {
+ if (sum <= 0.0)
+ throw std::logic_error("Error in Cholesky decomposition: matrix is not postive definite");
+ LL[i][i] = sqrt(sum);
+ }
+ else
+ LL[j][i] = sum / LL[i][i];
+ }
+ for (k = i + 1; k < n; k++)
+ LL[i][k] = LL[k][i];
+ }
+}
+
+template <typename T>
+Matrix<T> cholesky(const Matrix<T> A)
+{
+ Matrix<T> LL;
+ cholesky(A, LL);
+
+ return LL;
+}
+
+template <typename T>
+Vector<T> cholesky_solve(const Matrix<T>& LL, const Vector<T>& b)
+{
+ if (LL.ncols() != LL.nrows())
+ throw std::logic_error("Error in Cholesky solve: matrix must be squared");
+ unsigned int n = LL.ncols();
+ if (b.size() != n)
+ throw std::logic_error("Error in Cholesky decomposition: b vector must be of the same dimensions of LU matrix");
+ Vector<T> x, y;
+
+ /* Solve L * y = b */
+ forward_elimination(LL, y, b);
+ /* Solve L^T * x = y */
+ backward_elimination(LL, x, y);
+
+ return x;
+}
+
+template <typename T>
+void cholesky_solve(const Matrix<T>& LL, Vector<T>& x, const Vector<T>& b)
+{
+ x = cholesky_solve(LL, b);
+}
+
+template <typename T>
+void forward_elimination(const Matrix<T>& L, Vector<T>& y, const Vector<T> b)
+{
+ if (L.ncols() != L.nrows())
+ throw std::logic_error("Error in Forward elimination: matrix must be squared (lower triangular)");
+ if (b.size() != L.nrows())
+ throw std::logic_error("Error in Forward elimination: b vector must be of the same dimensions of L matrix");
+ register int i, j, n = b.size();
+ y.resize(n);
+
+ y[0] = b[0] / L[0][0];
+ for (i = 1; i < n; i++)
+ {
+ y[i] = b[i];
+ for (j = 0; j < i; j++)
+ y[i] -= L[i][j] * y[j];
+ y[i] = y[i] / L[i][i];
+ }
+}
+
+template <typename T>
+Vector<T> forward_elimination(const Matrix<T>& L, const Vector<T> b)
+{
+ Vector<T> y;
+ forward_elimination(L, y, b);
+
+ return y;
+}
+
+template <typename T>
+void backward_elimination(const Matrix<T>& U, Vector<T>& x, const Vector<T>& y)
+{
+ if (U.ncols() != U.nrows())
+ throw std::logic_error("Error in Backward elimination: matrix must be squared (upper triangular)");
+ if (y.size() != U.nrows())
+ throw std::logic_error("Error in Backward elimination: b vector must be of the same dimensions of U matrix");
+ register int i, j, n = y.size();
+ x.resize(n);
+
+ x[n - 1] = y[n - 1] / U[n - 1][n - 1];
+ for (i = n - 2; i >= 0; i--)
+ {
+ x[i] = y[i];
+ for (j = i + 1; j < n; j++)
+ x[i] -= U[i][j] * x[j];
+ x[i] = x[i] / U[i][i];
+ }
+}
+
+template <typename T>
+Vector<T> backward_elimination(const Matrix<T>& U, const Vector<T> y)
+{
+ Vector<T> x;
+ forward_elimination(U, x, y);
+
+ return x;
+}
+
+/* Setting default linear systems machinery */
+
+#define det lu_det
+//#define inverse lu_inverse
+#define solve lu_solve
+
+/* Random */
+
+template <typename T>
+void random(Matrix<T>& m)
+{
+ for (unsigned int i = 0; i < m.nrows(); i++)
+ for (unsigned int j = 0; j < m.ncols(); j++)
+ m[i][j] = (T)(rand() / double(RAND_MAX));
+}
+
+/**
+ Aggregate functions
+*/
+
+template <typename T>
+Vector<T> sum(const Matrix<T>& m)
+{
+ Vector<T> tmp((T)0, m.ncols());
+ for (unsigned int j = 0; j < m.ncols(); j++)
+ for (unsigned int i = 0; i < m.nrows(); i++)
+ tmp[j] += m[i][j];
+ return tmp;
+}
+
+template <typename T>
+Vector<T> r_sum(const Matrix<T>& m)
+{
+ Vector<T> tmp((T)0, m.nrows());
+ for (unsigned int i = 0; i < m.nrows(); i++)
+ for (unsigned int j = 0; j < m.ncols(); j++)
+ tmp[i] += m[i][j];
+ return tmp;
+}
+
+template <typename T>
+T all_sum(const Matrix<T>& m)
+{
+ T tmp = (T)0;
+ for (unsigned int i = 0; i < m.nrows(); i++)
+ for (unsigned int j = 0; j < m.ncols(); j++)
+ tmp += m[i][j];
+ return tmp;
+}
+
+template <typename T>
+Vector<T> prod(const Matrix<T>& m)
+{
+ Vector<T> tmp((T)1, m.ncols());
+ for (unsigned int j = 0; j < m.ncols(); j++)
+ for (unsigned int i = 0; i < m.nrows(); i++)
+ tmp[j] *= m[i][j];
+ return tmp;
+}
+
+template <typename T>
+Vector<T> r_prod(const Matrix<T>& m)
+{
+ Vector<T> tmp((T)1, m.nrows());
+ for (unsigned int i = 0; i < m.nrows(); i++)
+ for (unsigned int j = 0; j < m.ncols(); j++)
+ tmp[i] *= m[i][j];
+ return tmp;
+}
+
+template <typename T>
+T all_prod(const Matrix<T>& m)
+{
+ T tmp = (T)1;
+ for (unsigned int i = 0; i < m.nrows(); i++)
+ for (unsigned int j = 0; j < m.ncols(); j++)
+ tmp *= m[i][j];
+ return tmp;
+}
+
+template <typename T>
+Vector<T> mean(const Matrix<T>& m)
+{
+ Vector<T> res((T)0, m.ncols());
+ for (unsigned int j = 0; j < m.ncols(); j++)
+ {
+ for (unsigned int i = 0; i < m.nrows(); i++)
+ res[j] += m[i][j];
+ res[j] /= m.nrows();
+ }
+
+ return res;
+}
+
+template <typename T>
+Vector<T> r_mean(const Matrix<T>& m)
+{
+ Vector<T> res((T)0, m.rows());
+ for (unsigned int i = 0; i < m.nrows(); i++)
+ {
+ for (unsigned int j = 0; j < m.ncols(); j++)
+ res[i] += m[i][j];
+ res[i] /= m.nrows();
+ }
+
+ return res;
+}
+
+template <typename T>
+T all_mean(const Matrix<T>& m)
+{
+ T tmp = (T)0;
+ for (unsigned int i = 0; i < m.nrows(); i++)
+ for (unsigned int j = 0; j < m.ncols(); j++)
+ tmp += m[i][j];
+ return tmp / (m.nrows() * m.ncols());
+}
+
+template <typename T>
+Vector<T> var(const Matrix<T>& m, bool sample_correction = false)
+{
+ Vector<T> res((T)0, m.ncols());
+ unsigned int n = m.nrows();
+ double sum, ssum;
+ for (unsigned int j = 0; j < m.ncols(); j++)
+ {
+ sum = (T)0.0; ssum = (T)0.0;
+ for (unsigned int i = 0; i < m.nrows(); i++)
+ {
+ sum += m[i][j];
+ ssum += (m[i][j] * m[i][j]);
+ }
+ if (!sample_correction)
+ res[j] = (ssum / n) - (sum / n) * (sum / n);
+ else
+ res[j] = n * ((ssum / n) - (sum / n) * (sum / n)) / (n - 1);
+ }
+
+ return res;
+}
+
+template <typename T>
+Vector<T> stdev(const Matrix<T>& m, bool sample_correction = false)
+{
+ return vec_sqrt(var(m, sample_correction));
+}
+
+template <typename T>
+Vector<T> r_var(const Matrix<T>& m, bool sample_correction = false)
+{
+ Vector<T> res((T)0, m.nrows());
+ double sum, ssum;
+ unsigned int n = m.ncols();
+ for (unsigned int i = 0; i < m.nrows(); i++)
+ {
+ sum = 0.0; ssum = 0.0;
+ for (unsigned int j = 0; j < m.ncols(); j++)
+ {
+ sum += m[i][j];
+ ssum += (m[i][j] * m[i][j]);
+ }
+ if (!sample_correction)
+ res[i] = (ssum / n) - (sum / n) * (sum / n);
+ else
+ res[i] = n * ((ssum / n) - (sum / n) * (sum / n)) / (n - 1);
+ }
+
+ return res;
+}
+
+template <typename T>
+Vector<T> r_stdev(const Matrix<T>& m, bool sample_correction = false)
+{
+ return vec_sqrt(r_var(m, sample_correction));
+}
+
+template <typename T>
+Vector<T> max(const Matrix<T>& m)
+{
+ Vector<T> res(m.ncols());
+ double value;
+ for (unsigned int j = 0; j < m.ncols(); j++)
+ {
+ value = m[0][j];
+ for (unsigned int i = 1; i < m.nrows(); i++)
+ value = std::max(m[i][j], value);
+ res[j] = value;
+ }
+
+ return res;
+}
+
+template <typename T>
+Vector<T> r_max(const Matrix<T>& m)
+{
+ Vector<T> res(m.nrows());
+ double value;
+ for (unsigned int i = 0; i < m.nrows(); i++)
+ {
+ value = m[i][0];
+ for (unsigned int j = 1; j < m.ncols(); j++)
+ value = std::max(m[i][j], value);
+ res[i] = value;
+ }
+
+ return res;
+}
+
+template <typename T>
+Vector<T> min(const Matrix<T>& m)
+{
+ Vector<T> res(m.ncols());
+ double value;
+ for (unsigned int j = 0; j < m.ncols(); j++)
+ {
+ value = m[0][j];
+ for (unsigned int i = 1; i < m.nrows(); i++)
+ value = std::min(m[i][j], value);
+ res[j] = value;
+ }
+
+ return res;
+}
+
+template <typename T>
+Vector<T> r_min(const Matrix<T>& m)
+{
+ Vector<T> res(m.nrows());
+ double value;
+ for (unsigned int i = 0; i < m.nrows(); i++)
+ {
+ value = m[i][0];
+ for (unsigned int j = 1; j < m.ncols(); j++)
+ value = std::min(m[i][j], value);
+ res[i] = value;
+ }
+
+ return res;
+}
+
+
+
+/**
+ Single element mathematical functions
+*/
+
+template <typename T>
+Matrix<T> exp(const Matrix<T>&m)
+{
+ Matrix<T> tmp(m.nrows(), m.ncols());
+
+ for (unsigned int i = 0; i < m.nrows(); i++)
+ for (unsigned int j = 0; j < m.ncols(); j++)
+ tmp[i][j] = exp(m[i][j]);
+
+ return tmp;
+}
+
+template <typename T>
+Matrix<T> mat_sqrt(const Matrix<T>&m)
+{
+ Matrix<T> tmp(m.nrows(), m.ncols());
+
+ for (unsigned int i = 0; i < m.nrows(); i++)
+ for (unsigned int j = 0; j < m.ncols(); j++)
+ tmp[i][j] = sqrt(m[i][j]);
+
+ return tmp;
+}
+
+/**
+ Matrix operators
+*/
+
+template <typename T>
+Matrix<T> kron(const Vector<T>& b, const Vector<T>& a)
+{
+ Matrix<T> tmp(b.size(), a.size());
+ for (unsigned int i = 0; i < b.size(); i++)
+ for (unsigned int j = 0; j < a.size(); j++)
+ tmp[i][j] = a[j] * b[i];
+
+ return tmp;
+}
+
+template <typename T>
+Matrix<T> t(const Matrix<T>& a)
+{
+ Matrix<T> tmp(a.ncols(), a.nrows());
+ for (unsigned int i = 0; i < a.nrows(); i++)
+ for (unsigned int j = 0; j < a.ncols(); j++)
+ tmp[j][i] = a[i][j];
+
+ return tmp;
+}
+
+template <typename T>
+Matrix<T> dot_prod(const Matrix<T>& a, const Matrix<T>& b)
+{
+ if (a.ncols() != b.nrows())
+ throw std::logic_error("Error matrix dot product: dimensions of the matrices are not compatible");
+ Matrix<T> tmp(a.nrows(), b.ncols());
+ for (unsigned int i = 0; i < tmp.nrows(); i++)
+ for (unsigned int j = 0; j < tmp.ncols(); j++)
+ {
+ tmp[i][j] = (T)0;
+ for (unsigned int k = 0; k < a.ncols(); k++)
+ tmp[i][j] += a[i][k] * b[k][j];
+ }
+
+ return tmp;
+}
+
+template <typename T>
+Matrix<T> dot_prod(const Matrix<T>& a, const Vector<T>& b)
+{
+ if (a.ncols() != b.size())
+ throw std::logic_error("Error matrix dot product: dimensions of the matrix and the vector are not compatible");
+ Matrix<T> tmp(a.nrows(), 1);
+ for (unsigned int i = 0; i < tmp.nrows(); i++)
+ {
+ tmp[i][0] = (T)0;
+ for (unsigned int k = 0; k < a.ncols(); k++)
+ tmp[i][0] += a[i][k] * b[k];
+ }
+
+ return tmp;
+}
+
+template <typename T>
+Matrix<T> dot_prod(const Vector<T>& a, const Matrix<T>& b)
+{
+ if (a.size() != b.ncols())
+ throw std::logic_error("Error matrix dot product: dimensions of the vector and matrix are not compatible");
+ Matrix<T> tmp(1, b.ncols());
+ for (unsigned int j = 0; j < tmp.ncols(); j++)
+ {
+ tmp[0][j] = (T)0;
+ for (unsigned int k = 0; k < a.size(); k++)
+ tmp[0][j] += a[k] * b[k][j];
+ }
+
+ return tmp;
+}
+
+template <typename T>
+inline Matrix<double> rank(const Matrix<T> m)
+{
+ Matrix<double> tmp(m.nrows(), m.ncols());
+ for (unsigned int j = 0; j < m.ncols(); j++)
+ tmp.setColumn(j, rank<T>(m.extractColumn(j)));
+
+ return tmp;
+}
+
+template <typename T>
+inline Matrix<double> r_rank(const Matrix<T> m)
+{
+ Matrix<double> tmp(m.nrows(), m.ncols());
+ for (unsigned int i = 0; i < m.nrows(); i++)
+ tmp.setRow(i, rank<T>(m.extractRow(i)));
+
+ return tmp;
+}
+
+} // namespace quadprogpp
+
+#endif // define _ARRAY_HH_
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Eigen.lib Mon Oct 14 10:08:13 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/jsoh91/code/Eigen/#3b8049da21b8
--- a/function_utilities/function_utilities.cpp Thu Sep 26 06:08:32 2019 +0000
+++ b/function_utilities/function_utilities.cpp Mon Oct 14 10:08:13 2019 +0000
@@ -41,7 +41,7 @@
int16_t DIR_VALVE_ENC = 0;
double SUPPLY_VOLTAGE = 12000.0;
-double VALVE_VOLTAGE_LIMIT = 12000.0; //mv
+double VALVE_VOLTAGE_LIMIT = 5000.0; //mv
double P_GAIN_VALVE_POSITION = 0.0;
double I_GAIN_VALVE_POSITION= 0.0;
@@ -333,7 +333,7 @@
void make_delay(void) {
int i = 0;
- for (i = 0; i < 100000; i++) {
+ for (i = 0; i < 10000; i++) {
;
}
@@ -431,7 +431,7 @@
SUPPLY_VOLTAGE = (double) (flashReadInt(Rom_Sector, RID_VOLATGE_SUPPLY)) *0.1;
SUPPLY_VOLTAGE = 12000.0;
VALVE_VOLTAGE_LIMIT = (double) (flashReadInt(Rom_Sector, RID_VOLTAGE_VALVE)) * 0.1;
- VALVE_VOLTAGE_LIMIT = 12000.0;
+ VALVE_VOLTAGE_LIMIT = 5000.0;
P_GAIN_VALVE_POSITION = flashReadInt(Rom_Sector, RID_P_GAIN_VALVE_POSITION);
I_GAIN_VALVE_POSITION = flashReadInt(Rom_Sector, RID_I_GAIN_VALVE_POSITION);
D_GAIN_VALVE_POSITION = flashReadInt(Rom_Sector, RID_D_GAIN_VALVE_POSITION);
--- a/main.cpp Thu Sep 26 06:08:32 2019 +0000
+++ b/main.cpp Mon Oct 14 10:08:13 2019 +0000
@@ -1,3 +1,163 @@
+
+//JS MODI
+#include "quadprog.h"
+#include <cstdlib>
+#include "Eigen/Dense.h"
+
+using namespace std;
+using namespace Eigen;
+
+class JS_QP
+{
+private:
+ int NUMCOLS;//length of X
+ int NUMEQ;
+ int NUMINEQ;
+
+ MatrixXd A_ineq,B_ineq;
+ MatrixXd A_eq,B_eq;
+
+public:
+ MatrixXd H,F;
+ MatrixXd X;
+
+public:
+ void setNums(int _xlength, int numEqConstraints, int numIneqConstraints)
+ {
+ NUMCOLS = _xlength;//should be 18+12+3*contact
+ NUMEQ = numEqConstraints;//NUMCOLS(dynamics) + NUMCOLS(contactconstraints + swingleg)
+ //contact to be weight?
+ NUMINEQ = numIneqConstraints;//friction cone approximation -ufz<fx<ufz, -ufz<fy<ufz ->4
+
+ //maybe matrix assignment here
+ X = MatrixXd::Zero(NUMCOLS,1);
+ A_eq = MatrixXd::Zero(NUMEQ, NUMCOLS);
+ B_eq = MatrixXd::Zero(NUMEQ, 1);
+
+ A_ineq = MatrixXd::Zero(NUMINEQ, NUMCOLS);
+ B_ineq = MatrixXd::Zero(NUMINEQ, 1);
+
+
+ H = MatrixXd::Zero(NUMCOLS,NUMCOLS);
+ F = MatrixXd::Zero(NUMCOLS,1);
+ }
+
+ void make_EQ(MatrixXd A, MatrixXd b)
+ {
+ //Aeq*X = Beq
+ A_eq = A;
+ B_eq = b;
+ }
+
+ void make_IEQ(MatrixXd A, MatrixXd b)
+ {
+ // Aineq*X < Bineq
+ A_ineq = A;
+ B_ineq = b;
+ }
+
+ void make_HF(MatrixXd A, MatrixXd b, int mode=-1)
+ {
+ //min (0.5* x H x + F x)
+ if(mode == -1) {
+ H = A.transpose() * A + MatrixXd::Identity(NUMCOLS,NUMCOLS)*1e-3;
+ F = -A.transpose() * b;
+
+ } else {
+ H = A;
+ F = b;
+
+ }
+
+ }
+
+ MatrixXd solve_QP()
+ {
+ quadprogpp::Vector<double> outX;
+ quadprogpp::Matrix<double> G;
+ quadprogpp::Vector<double> g0;
+ quadprogpp::Matrix<double> CE;
+ quadprogpp::Vector<double> ce0;
+ quadprogpp::Matrix<double> CI;
+ quadprogpp::Vector<double> ci0;
+ //min 0.5 * x G x + g0 x
+ //CE^T x + ce0 = 0
+ //CI^T x + ci0 >= 0
+
+ G.resize(NUMCOLS,NUMCOLS);
+ g0.resize(NUMCOLS);
+ for(int i=0; i<NUMCOLS; i++) {
+ for(int j=0; j<NUMCOLS; j++) {
+ G[i][j] = H(i,j);
+ }
+ g0[i] = F(i,0);
+ }
+ CE.resize(NUMCOLS,NUMEQ);
+ ce0.resize(NUMEQ);
+ for(int i=0; i<NUMCOLS; i++) {
+ for(int j=0; j<NUMEQ; j++) {
+ CE[i][j] = -A_eq(j,i);
+ }
+ }
+ for(int j=0; j<NUMEQ; j++) {
+ ce0[j] = B_eq(j,0);
+ }
+ CI.resize(NUMCOLS,NUMINEQ);
+ ci0.resize(NUMINEQ);
+ for(int i=0; i<NUMCOLS; i++) {
+ for(int j=0; j<NUMINEQ; j++) {
+ CI[i][j] = -A_ineq(j,i);
+ }
+ }
+ for(int j=0; j<NUMINEQ; j++) {
+ ci0[j] = B_ineq(j,0);
+ }
+ outX.resize(NUMCOLS);
+
+ solve_quadprog(G, g0, CE, ce0, CI, ci0, outX);
+ for(int i=0; i<NUMCOLS; i++) {
+ X(i,0) = outX[i];
+ }
+
+ return X;
+
+ }
+
+public:
+
+};
+
+JS_QP js_qp;
+
+
+MatrixXd Aa = MatrixXd::Zero(20,2);
+MatrixXd Ba = MatrixXd::Zero(20,10);
+MatrixXd Ga = MatrixXd::Zero(20,20);
+
+MatrixXd ref_a = MatrixXd::Zero(20,1);
+MatrixXd w0_a = MatrixXd::Zero(20,1);
+
+MatrixXd At;
+MatrixXd Bt;
+
+MatrixXd X_state = MatrixXd::Zero(2,1);
+
+MatrixXd Q = MatrixXd::Identity(20,20);
+MatrixXd R = MatrixXd::Identity(10,10);
+
+MatrixXd H;
+MatrixXd F;
+
+MatrixXd out;
+
+MatrixXd Aieq = MatrixXd::Zero(20,10);
+MatrixXd bieq = MatrixXd::Zero(20,1);
+
+float iq_err_sum = 0.0;
+MatrixXd i_ref = MatrixXd::Zero(10,1);
+float iq = 0.0;
+
+//original
#include "mbed.h"
#include "FastPWM.h"
#include "INIT_HW.h"
@@ -9,6 +169,8 @@
#include "stm32f4xx_flash.h"
#include "FlashWriter.h"
+Timer t;
+
// dac & check ///////////////////////////////////////////
DigitalOut check(PC_2);
DigitalOut check_2(PC_3);
@@ -190,15 +352,15 @@
can.attach(&CAN_RX_HANDLER);
CAN_ID_INIT();
make_delay();
-
+
//Timer priority
NVIC_SetPriority(TIM3_IRQn, 2);
NVIC_SetPriority(TIM2_IRQn, 3);
NVIC_SetPriority(TIM4_IRQn, 4);
-
+
//can.reset();
can.filter(msg.id, 0xFFFFF000, CANStandard);
-
+
// spi _ enc
spi_enc_set_init();
make_delay();
@@ -215,14 +377,102 @@
ID_index_array[i] = (i+1) * 0.5;
}
+
+
+//JS MODI
+
+
+ Aa <<
+ 1.000000,-1.000000
+ ,0.000000,-0.666667
+ ,1.000000,-0.333333
+ ,0.000000,0.444444
+ ,1.000000,-0.777778
+ ,0.000000,-0.296296
+ ,1.000000,-0.481481
+ ,0.000000,0.197531
+ ,1.000000,-0.679012
+ ,0.000000,-0.131687
+ ,1.000000,-0.547325
+ ,0.000000,0.087791
+ ,1.000000,-0.635117
+ ,0.000000,-0.058528
+ ,1.000000,-0.576589
+ ,0.000000,0.039018
+ ,1.000000,-0.615607
+ ,0.000000,-0.026012
+ ,1.000000,-0.589595
+ ,0.000000,0.017342;
+
+
+ Ba <<
+ 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,0.066667,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,-0.066667,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,-0.044444,0.066667,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,-0.022222,-0.066667,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,0.029630,-0.044444,0.066667,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,-0.051852,-0.022222,-0.066667,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,-0.019753,0.029630,-0.044444,0.066667,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,-0.032099,-0.051852,-0.022222,-0.066667,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,0.013169,-0.019753,0.029630,-0.044444,0.066667,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,-0.045267,-0.032099,-0.051852,-0.022222,-0.066667,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,-0.008779,0.013169,-0.019753,0.029630,-0.044444,0.066667,0.000000,0.000000,0.000000,0.000000
+ ,-0.036488,-0.045267,-0.032099,-0.051852,-0.022222,-0.066667,0.000000,0.000000,0.000000,0.000000
+ ,0.005853,-0.008779,0.013169,-0.019753,0.029630,-0.044444,0.066667,0.000000,0.000000,0.000000
+ ,-0.042341,-0.036488,-0.045267,-0.032099,-0.051852,-0.022222,-0.066667,0.000000,0.000000,0.000000
+ ,-0.003902,0.005853,-0.008779,0.013169,-0.019753,0.029630,-0.044444,0.066667,0.000000,0.000000
+ ,-0.038439,-0.042341,-0.036488,-0.045267,-0.032099,-0.051852,-0.022222,-0.066667,0.000000,0.000000
+ ,0.002601,-0.003902,0.005853,-0.008779,0.013169,-0.019753,0.029630,-0.044444,0.066667,0.000000
+ ,-0.041040,-0.038439,-0.042341,-0.036488,-0.045267,-0.032099,-0.051852,-0.022222,-0.066667,0.000000
+ ,-0.001734,0.002601,-0.003902,0.005853,-0.008779,0.013169,-0.019753,0.029630,-0.044444,0.066667;
+
+
+
+ Ga <<
+ 1.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,1.000000,0.000000,1.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,0.000000,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,0.000000,0.000000,0.000000,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000
+ ,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,0.000000,0.000000
+ ,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,-0.000000,0.000000,0.000000
+ ,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000
+ ,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,-0.000000;
+
+
+
+ Aieq.block(0,0,10,10) = MatrixXd::Identity(10,10);
+ Aieq.block(10,0,10,10) = -MatrixXd::Identity(10,10);
+
+ for(int i=0; i<10; i++) {
+ bieq(i,0) = 5.0;
+ bieq(i+10,0) = 5.0;
+ }
+
+ for(int i=0; i<10; i++) {
+ i_ref(i,0) = 10e-3;
+ }
+
/************************************
*** Program is operating!
*************************************/
while(1) {
if(timer_while==1000) {
//i2c
- read_field(i2c_slave_addr1);
- if(DIR_VALVE_ENC < 0) value = 1023 - value;
+ //read_field(i2c_slave_addr1);
+ //if(DIR_VALVE_ENC < 0) value = 1023 - value;
// if(LED==1) {
// LED=0;
@@ -278,8 +528,8 @@
} else if(REF_VALVE_POS < VALVE_MIN_POS) {
REF_VALVE_POS = VALVE_MIN_POS;
}
-
-
+
+
// if(REF_VALVE_POS >= VALVE_POS_VS_PWM[0])
// {
// if(REF_VALVE_POS <= VALVE_POS_VS_PWM[1]) {
@@ -412,7 +662,7 @@
double alpha_update_cur = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0)); // f_cutoff : 500Hz
double cur_new = ((double)ADC3->DR-2048.0)*20.0/4096.0; // unit : mA
cur.sen=cur.sen*(1.0-alpha_update_cur)+cur_new*(alpha_update_cur);
- cur.sen = raw_cur;
+// cur.sen = raw_cur;
/*******************************************************
*** Timer Counting & etc.
@@ -434,6 +684,13 @@
if (TIM3->SR & TIM_SR_UIF ) {
ENC_UPDATE();
+
+
+
+
+
+
+
// CONTROL LOOP ------------------------------------------------------------
switch (CONTROL_MODE) {
@@ -493,8 +750,8 @@
}
case MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION: {
-
-
+
+
double VALVE_POS_RAW_POS_FB = 0.0; // Valve Position by Position Feedback
//double VALVE_POS_RAW_POS_FF = 0.0; // Valve Position by Position Feedforward
double VALVE_POS_RAW_FORCE_FB = 0.0; // Valve Position by Force Feedback
@@ -532,8 +789,8 @@
valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_JOINT_POS_FF(vel.ref) + VALVE_POS_RAW_FORCE_FB;
//valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_CENTER;
VALVE_POS_CONTROL(valve_pos.ref);
-
-
+
+
break;
}
@@ -1622,6 +1879,37 @@
V_out = V_out;
}
+
+ iq = cur.sen*0.001;
+ iq_err_sum += i_ref(0,0) - iq;
+ X_state(0,0) = iq_err_sum;
+ X_state(1,0) = iq;
+
+ for (int i=0; i<10; i++) {
+ w0_a(i*2,0) = i_ref(i,0);
+ w0_a(i*2+1,0) = 0.0;
+
+ ref_a(i*2,0) = 0.0;
+ ref_a(i*2+1,0) = i_ref(i,0);
+ }
+
+ Bt = ref_a - Aa*X_state - Ga*w0_a;
+ At = Ba;
+
+
+ H = At.transpose() * Q * At + R;
+ F = -At.transpose()*Bt;
+
+ js_qp.setNums(10,0,20);
+
+ js_qp.make_HF(H,F,1);
+ js_qp.make_IEQ(Aieq,bieq);
+
+
+
+ out = js_qp.solve_QP();
+ V_out = out(0,0) * 1000.0;
+
/*******************************************************
*** PWM
********************************************************/
@@ -1647,6 +1935,9 @@
TIM4->CCR2 = (PWM_ARR)*(1.0-dtc_v);
TIM4->CCR1 = (PWM_ARR)*(1.0-dtc_w);
+
+
+ CAN_TX_POSITION((int32_t) V_out, (int32_t) (cur.sen * 10.0));
}
TIM3->SR = 0x0; // reset the status register
@@ -1664,7 +1955,8 @@
//CAN ----------------------------------------------------------------------
if (flag_data_request[0] == HIGH) {
//position+velocity
- CAN_TX_POSITION((int32_t) pos.sen, (int32_t) vel.sen);
+ //CAN_TX_POSITION((int32_t) pos.sen, (int32_t) vel.sen);
+ CAN_TX_POSITION((int32_t) V_out, (int32_t) cur.sen);
//CAN_TX_POSITION((int32_t) valve_pos.ref, (int32_t) 0);
//CAN_TX_POSITION((int32_t) VALVE_PWM_RAW_FF, (int32_t) VALVE_POS_VS_PWM[10]);
//pc.printf("can good");
@@ -1719,7 +2011,6 @@
}
-
void CurrentControl()
{
cur.err = cur.ref - cur.sen;
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/quadprog.cpp Mon Oct 14 10:08:13 2019 +0000
@@ -0,0 +1,738 @@
+/*
+File $Id: QuadProg++.cc 232 2007-06-21 12:29:00Z digasper $
+
+ Author: Luca Di Gaspero
+ DIEGM - University of Udine, Italy
+ luca.digaspero@uniud.it
+ http://www.diegm.uniud.it/digaspero/
+
+ This software may be modified and distributed under the terms
+ of the MIT license. See the LICENSE file for details.
+
+ */
+
+#include <iostream>
+#include <algorithm>
+#include <cmath>
+#include <limits>
+#include <sstream>
+#include <stdexcept>
+#include "quadprog.h"
+#include "math.h"
+
+//#define TRACE_SOLVER
+
+namespace quadprogpp
+{
+
+// Utility functions for updating some data needed by the solution method
+void compute_d(Vector<double>& d, const Matrix<double>& J, const Vector<double>& np);
+void update_z(Vector<double>& z, const Matrix<double>& J, const Vector<double>& d, int iq);
+void update_r(const Matrix<double>& R, Vector<double>& r, const Vector<double>& d, int iq);
+bool add_constraint(Matrix<double>& R, Matrix<double>& J, Vector<double>& d, int& iq, double& rnorm);
+void delete_constraint(Matrix<double>& R, Matrix<double>& J, Vector<int>& A, Vector<double>& u, int n, int p, int& iq, int l);
+
+// Utility functions for computing the Cholesky decomposition and solving
+// linear systems
+void cholesky_decomposition(Matrix<double>& A);
+void cholesky_solve(const Matrix<double>& L, Vector<double>& x, const Vector<double>& b);
+void forward_elimination(const Matrix<double>& L, Vector<double>& y, const Vector<double>& b);
+void backward_elimination(const Matrix<double>& U, Vector<double>& x, const Vector<double>& y);
+
+// Utility functions for computing the scalar product and the euclidean
+// distance between two numbers
+double scalar_product(const Vector<double>& x, const Vector<double>& y);
+double distance(double a, double b);
+
+// Utility functions for printing vectors and matrices
+void print_matrix(char* name, const Matrix<double>& A, int n = -1, int m = -1);
+
+template<typename T>
+void print_vector(char* name, const Vector<T>& v, int n = -1);
+
+// The Solving function, implementing the Goldfarb-Idnani method
+
+double solve_quadprog(Matrix<double>& G, Vector<double>& g0,
+ const Matrix<double>& CE, const Vector<double>& ce0,
+ const Matrix<double>& CI, const Vector<double>& ci0,
+ Vector<double>& x)
+{
+ std::ostringstream msg;
+ int n = G.ncols(), p = CE.ncols(), m = CI.ncols();
+ if (G.nrows() != n) {
+ msg << "The matrix G is not a squared matrix (" << G.nrows() << " x " << G.ncols() << ")";
+// throw std::logic_error(msg.str());
+ }
+ if (CE.nrows() != n) {
+ msg << "The matrix CE is incompatible (incorrect number of rows " << CE.nrows() << " , expecting " << n << ")";
+// throw std::logic_error(msg.str());
+ }
+ if (ce0.size() != p) {
+ msg << "The vector ce0 is incompatible (incorrect dimension " << ce0.size() << ", expecting " << p << ")";
+// throw std::logic_error(msg.str());
+ }
+ if (CI.nrows() != n) {
+ msg << "The matrix CI is incompatible (incorrect number of rows " << CI.nrows() << " , expecting " << n << ")";
+// throw std::logic_error(msg.str());
+ }
+ if (ci0.size() != m) {
+ msg << "The vector ci0 is incompatible (incorrect dimension " << ci0.size() << ", expecting " << m << ")";
+// throw std::logic_error(msg.str());
+ }
+ x.resize(n);
+ register int i, j, k, l; /* indices */
+ int ip; // this is the index of the constraint to be added to the active set
+ Matrix<double> R(n, n), J(n, n);
+ Vector<double> s(m + p), z(n), r(m + p), d(n), np(n), u(m + p), x_old(n), u_old(m + p);
+ double f_value, psi, c1, c2, sum, ss, R_norm;
+ double inf;
+ if (std::numeric_limits<double>::has_infinity)
+ inf = std::numeric_limits<double>::infinity();
+ else
+ inf = 1.0E300;
+ double t, t1, t2; /* t is the step lenght, which is the minimum of the partial step length t1
+ * and the full step length t2 */
+ Vector<int> A(m + p), A_old(m + p), iai(m + p);
+ int q, iq, iter = 0;
+ Vector<bool> iaexcl(m + p);
+
+ /* p is the number of equality constraints */
+ /* m is the number of inequality constraints */
+ q = 0; /* size of the active set A (containing the indices of the active constraints) */
+#ifdef TRACE_SOLVER
+ std::cout << std::endl << "Starting solve_quadprog" << std::endl;
+ print_matrix("G", G);
+ print_vector("g0", g0);
+ print_matrix("CE", CE);
+ print_vector("ce0", ce0);
+ print_matrix("CI", CI);
+ print_vector("ci0", ci0);
+#endif
+
+ /*
+ * Preprocessing phase
+ */
+
+ /* compute the trace of the original matrix G */
+ c1 = 0.0;
+ for (i = 0; i < n; i++) {
+ c1 += G[i][i];
+ }
+ /* decompose the matrix G in the form L^T L */
+ cholesky_decomposition(G);
+#ifdef TRACE_SOLVER
+ print_matrix("G", G);
+#endif
+ /* initialize the matrix R */
+ for (i = 0; i < n; i++) {
+ d[i] = 0.0;
+ for (j = 0; j < n; j++)
+ R[i][j] = 0.0;
+ }
+ R_norm = 1.0; /* this variable will hold the norm of the matrix R */
+
+ /* compute the inverse of the factorized matrix G^-1, this is the initial value for H */
+ c2 = 0.0;
+ for (i = 0; i < n; i++) {
+ d[i] = 1.0;
+ forward_elimination(G, z, d);
+ for (j = 0; j < n; j++)
+ J[i][j] = z[j];
+ c2 += z[i];
+ d[i] = 0.0;
+ }
+#ifdef TRACE_SOLVER
+ print_matrix("J", J);
+#endif
+
+ /* c1 * c2 is an estimate for cond(G) */
+
+ /*
+ * Find the unconstrained minimizer of the quadratic form 0.5 * x G x + g0 x
+ * this is a feasible point in the dual space
+ * x = G^-1 * g0
+ */
+ cholesky_solve(G, x, g0);
+ for (i = 0; i < n; i++)
+ x[i] = -x[i];
+ /* and compute the current solution value */
+ f_value = 0.5 * scalar_product(g0, x);
+#ifdef TRACE_SOLVER
+ std::cout << "Unconstrained solution: " << f_value << std::endl;
+ print_vector("x", x);
+#endif
+
+ /* Add equality constraints to the working set A */
+ iq = 0;
+ for (i = 0; i < p; i++) {
+ for (j = 0; j < n; j++)
+ np[j] = CE[j][i];
+ compute_d(d, J, np);
+ update_z(z, J, d, iq);
+ update_r(R, r, d, iq);
+#ifdef TRACE_SOLVER
+ print_matrix("R", R, n, iq);
+ print_vector("z", z);
+ print_vector("r", r, iq);
+ print_vector("d", d);
+#endif
+
+ /* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint
+ becomes feasible */
+ t2 = 0.0;
+ if (fabs(scalar_product(z, z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
+ t2 = (-scalar_product(np, x) - ce0[i]) / scalar_product(z, np);
+
+ /* set x = x + t2 * z */
+ for (k = 0; k < n; k++)
+ x[k] += t2 * z[k];
+
+ /* set u = u+ */
+ u[iq] = t2;
+ for (k = 0; k < iq; k++)
+ u[k] -= t2 * r[k];
+
+ /* compute the new solution value */
+ f_value += 0.5 * (t2 * t2) * scalar_product(z, np);
+ A[i] = -i - 1;
+
+ if (!add_constraint(R, J, d, iq, R_norm)) {
+ // Equality constraints are linearly dependent
+// throw std::runtime_error("Constraints are linearly dependent");
+ return f_value;
+ }
+ }
+
+ /* set iai = K \ A */
+ for (i = 0; i < m; i++)
+ iai[i] = i;
+
+l1:
+ iter++;
+#ifdef TRACE_SOLVER
+ print_vector("x", x);
+#endif
+ /* step 1: choose a violated constraint */
+ for (i = p; i < iq; i++) {
+ ip = A[i];
+ iai[ip] = -1;
+ }
+
+ /* compute s[x] = ci^T * x + ci0 for all elements of K \ A */
+ ss = 0.0;
+ psi = 0.0; /* this value will contain the sum of all infeasibilities */
+ ip = 0; /* ip will be the index of the chosen violated constraint */
+ for (i = 0; i < m; i++) {
+ iaexcl[i] = true;
+ sum = 0.0;
+ for (j = 0; j < n; j++)
+ sum += CI[j][i] * x[j];
+ sum += ci0[i];
+ s[i] = sum;
+ psi += std::min(0.0, sum);
+ }
+#ifdef TRACE_SOLVER
+ print_vector("s", s, m);
+#endif
+
+
+ if (fabs(psi) <= m * std::numeric_limits<double>::epsilon() * c1 * c2* 100.0) {
+ /* numerically there are not infeasibilities anymore */
+ q = iq;
+
+ return f_value;
+ }
+
+ /* save old values for u and A */
+ for (i = 0; i < iq; i++) {
+ u_old[i] = u[i];
+ A_old[i] = A[i];
+ }
+ /* and for x */
+ for (i = 0; i < n; i++)
+ x_old[i] = x[i];
+
+l2: /* Step 2: check for feasibility and determine a new S-pair */
+ for (i = 0; i < m; i++) {
+ if (s[i] < ss && iai[i] != -1 && iaexcl[i]) {
+ ss = s[i];
+ ip = i;
+ }
+ }
+ if (ss >= 0.0) {
+ q = iq;
+
+ return f_value;
+ }
+
+ /* set np = n[ip] */
+ for (i = 0; i < n; i++)
+ np[i] = CI[i][ip];
+ /* set u = [u 0]^T */
+ u[iq] = 0.0;
+ /* add ip to the active set A */
+ A[iq] = ip;
+
+#ifdef TRACE_SOLVER
+ std::cout << "Trying with constraint " << ip << std::endl;
+ print_vector("np", np);
+#endif
+
+l2a:/* Step 2a: determine step direction */
+ /* compute z = H np: the step direction in the primal space (through J, see the paper) */
+ compute_d(d, J, np);
+ update_z(z, J, d, iq);
+ /* compute N* np (if q > 0): the negative of the step direction in the dual space */
+ update_r(R, r, d, iq);
+#ifdef TRACE_SOLVER
+ std::cout << "Step direction z" << std::endl;
+ print_vector("z", z);
+ print_vector("r", r, iq + 1);
+ print_vector("u", u, iq + 1);
+ print_vector("d", d);
+ print_vector("A", A, iq + 1);
+#endif
+
+ /* Step 2b: compute step length */
+ l = 0;
+ /* Compute t1: partial step length (maximum step in dual space without violating dual feasibility */
+ t1 = inf; /* +inf */
+ /* find the index l s.t. it reaches the minimum of u+[x] / r */
+ for (k = p; k < iq; k++) {
+ if (r[k] > 0.0) {
+ if (u[k] / r[k] < t1) {
+ t1 = u[k] / r[k];
+ l = A[k];
+ }
+ }
+ }
+ /* Compute t2: full step length (minimum step in primal space such that the constraint ip becomes feasible */
+ if (fabs(scalar_product(z, z)) > std::numeric_limits<double>::epsilon()) { // i.e. z != 0
+ t2 = -s[ip] / scalar_product(z, np);
+ if (t2 < 0) // patch suggested by Takano Akio for handling numerical inconsistencies
+ t2 = inf;
+ } else
+ t2 = inf; /* +inf */
+
+ /* the step is chosen as the minimum of t1 and t2 */
+ t = std::min(t1, t2);
+#ifdef TRACE_SOLVER
+ std::cout << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2 << ") ";
+#endif
+
+ /* Step 2c: determine new S-pair and take step: */
+
+ /* case (i): no step in primal or dual space */
+ if (t >= inf) {
+ /* QPP is infeasible */
+ // FIXME: unbounded to raise
+ q = iq;
+ return inf;
+ }
+ /* case (ii): step in dual space */
+ if (t2 >= inf) {
+ /* set u = u + t * [-r 1] and drop constraint l from the active set A */
+ for (k = 0; k < iq; k++)
+ u[k] -= t * r[k];
+ u[iq] += t;
+ iai[l] = l;
+ delete_constraint(R, J, A, u, n, p, iq, l);
+#ifdef TRACE_SOLVER
+ std::cout << " in dual space: "
+ << f_value << std::endl;
+ print_vector("x", x);
+ print_vector("z", z);
+ print_vector("A", A, iq + 1);
+#endif
+ goto l2a;
+ }
+
+ /* case (iii): step in primal and dual space */
+
+ /* set x = x + t * z */
+ for (k = 0; k < n; k++)
+ x[k] += t * z[k];
+ /* update the solution value */
+ f_value += t * scalar_product(z, np) * (0.5 * t + u[iq]);
+ /* u = u + t * [-r 1] */
+ for (k = 0; k < iq; k++)
+ u[k] -= t * r[k];
+ u[iq] += t;
+#ifdef TRACE_SOLVER
+ std::cout << " in both spaces: "
+ << f_value << std::endl;
+ print_vector("x", x);
+ print_vector("u", u, iq + 1);
+ print_vector("r", r, iq + 1);
+ print_vector("A", A, iq + 1);
+#endif
+
+ if (fabs(t - t2) < std::numeric_limits<double>::epsilon()) {
+#ifdef TRACE_SOLVER
+ std::cout << "Full step has taken " << t << std::endl;
+ print_vector("x", x);
+#endif
+ /* full step has taken */
+ /* add constraint ip to the active set*/
+ if (!add_constraint(R, J, d, iq, R_norm)) {
+ iaexcl[ip] = false;
+ delete_constraint(R, J, A, u, n, p, iq, ip);
+#ifdef TRACE_SOLVER
+ print_matrix("R", R);
+ print_vector("A", A, iq);
+ print_vector("iai", iai);
+#endif
+ for (i = 0; i < m; i++)
+ iai[i] = i;
+ for (i = p; i < iq; i++) {
+ A[i] = A_old[i];
+ u[i] = u_old[i];
+ iai[A[i]] = -1;
+ }
+ for (i = 0; i < n; i++)
+ x[i] = x_old[i];
+ goto l2; /* go to step 2 */
+ } else
+ iai[ip] = -1;
+#ifdef TRACE_SOLVER
+ print_matrix("R", R);
+ print_vector("A", A, iq);
+ print_vector("iai", iai);
+#endif
+ goto l1;
+ }
+
+ /* a patial step has taken */
+#ifdef TRACE_SOLVER
+ std::cout << "Partial step has taken " << t << std::endl;
+ print_vector("x", x);
+#endif
+ /* drop constraint l */
+ iai[l] = l;
+ delete_constraint(R, J, A, u, n, p, iq, l);
+#ifdef TRACE_SOLVER
+ print_matrix("R", R);
+ print_vector("A", A, iq);
+#endif
+
+ /* update s[ip] = CI * x + ci0 */
+ sum = 0.0;
+ for (k = 0; k < n; k++)
+ sum += CI[k][ip] * x[k];
+ s[ip] = sum + ci0[ip];
+
+#ifdef TRACE_SOLVER
+ print_vector("s", s, m);
+#endif
+ goto l2a;
+}
+
+inline void compute_d(Vector<double>& d, const Matrix<double>& J, const Vector<double>& np)
+{
+ register int i, j, n = d.size();
+ register double sum;
+
+ /* compute d = H^T * np */
+ for (i = 0; i < n; i++) {
+ sum = 0.0;
+ for (j = 0; j < n; j++)
+ sum += J[j][i] * np[j];
+ d[i] = sum;
+ }
+}
+
+inline void update_z(Vector<double>& z, const Matrix<double>& J, const Vector<double>& d, int iq)
+{
+ register int i, j, n = z.size();
+
+ /* setting of z = H * d */
+ for (i = 0; i < n; i++) {
+ z[i] = 0.0;
+ for (j = iq; j < n; j++)
+ z[i] += J[i][j] * d[j];
+ }
+}
+
+inline void update_r(const Matrix<double>& R, Vector<double>& r, const Vector<double>& d, int iq)
+{
+ register int i, j, n = d.size();
+ register double sum;
+
+ /* setting of r = R^-1 d */
+ for (i = iq - 1; i >= 0; i--) {
+ sum = 0.0;
+ for (j = i + 1; j < iq; j++)
+ sum += R[i][j] * r[j];
+ r[i] = (d[i] - sum) / R[i][i];
+ }
+}
+
+bool add_constraint(Matrix<double>& R, Matrix<double>& J, Vector<double>& d, int& iq, double& R_norm)
+{
+ int n = d.size();
+#ifdef TRACE_SOLVER
+ std::cout << "Add constraint " << iq << '/';
+#endif
+ register int i, j, k;
+ double cc, ss, h, t1, t2, xny;
+
+ /* we have to find the Givens rotation which will reduce the element
+ d[j] to zero.
+ if it is already zero we don't have to do anything, except of
+ decreasing j */
+ for (j = n - 1; j >= iq + 1; j--) {
+ /* The Givens rotation is done with the matrix (cc cs, cs -cc).
+ If cc is one, then element (j) of d is zero compared with element
+ (j - 1). Hence we don't have to do anything.
+ If cc is zero, then we just have to switch column (j) and column (j - 1)
+ of J. Since we only switch columns in J, we have to be careful how we
+ update d depending on the sign of gs.
+ Otherwise we have to apply the Givens rotation to these columns.
+ The i - 1 element of d has to be updated to h. */
+ cc = d[j - 1];
+ ss = d[j];
+ h = distance(cc, ss);
+ if (fabs(h) < std::numeric_limits<double>::epsilon()) // h == 0
+ continue;
+ d[j] = 0.0;
+ ss = ss / h;
+ cc = cc / h;
+ if (cc < 0.0) {
+ cc = -cc;
+ ss = -ss;
+ d[j - 1] = -h;
+ } else
+ d[j - 1] = h;
+ xny = ss / (1.0 + cc);
+ for (k = 0; k < n; k++) {
+ t1 = J[k][j - 1];
+ t2 = J[k][j];
+ J[k][j - 1] = t1 * cc + t2 * ss;
+ J[k][j] = xny * (t1 + J[k][j - 1]) - t2;
+ }
+ }
+ /* update the number of constraints added*/
+ iq++;
+ /* To update R we have to put the iq components of the d vector
+ into column iq - 1 of R
+ */
+ for (i = 0; i < iq; i++)
+ R[i][iq - 1] = d[i];
+#ifdef TRACE_SOLVER
+ std::cout << iq << std::endl;
+ print_matrix("R", R, iq, iq);
+ print_matrix("J", J);
+ print_vector("d", d, iq);
+#endif
+
+ if (fabs(d[iq - 1]) <= std::numeric_limits<double>::epsilon() * R_norm) {
+ // problem degenerate
+ return false;
+ }
+ R_norm = std::max<double>(R_norm, fabs(d[iq - 1]));
+ return true;
+}
+
+void delete_constraint(Matrix<double>& R, Matrix<double>& J, Vector<int>& A, Vector<double>& u, int n, int p, int& iq, int l)
+{
+#ifdef TRACE_SOLVER
+ std::cout << "Delete constraint " << l << ' ' << iq;
+#endif
+ register int i, j, k, qq = -1; // just to prevent warnings from smart compilers
+ double cc, ss, h, xny, t1, t2;
+
+ /* Find the index qq for active constraint l to be removed */
+ for (i = p; i < iq; i++)
+ if (A[i] == l) {
+ qq = i;
+ break;
+ }
+
+ /* remove the constraint from the active set and the duals */
+ for (i = qq; i < iq - 1; i++) {
+ A[i] = A[i + 1];
+ u[i] = u[i + 1];
+ for (j = 0; j < n; j++)
+ R[j][i] = R[j][i + 1];
+ }
+
+ A[iq - 1] = A[iq];
+ u[iq - 1] = u[iq];
+ A[iq] = 0;
+ u[iq] = 0.0;
+ for (j = 0; j < iq; j++)
+ R[j][iq - 1] = 0.0;
+ /* constraint has been fully removed */
+ iq--;
+#ifdef TRACE_SOLVER
+ std::cout << '/' << iq << std::endl;
+#endif
+
+ if (iq == 0)
+ return;
+
+ for (j = qq; j < iq; j++) {
+ cc = R[j][j];
+ ss = R[j + 1][j];
+ h = distance(cc, ss);
+ if (fabs(h) < std::numeric_limits<double>::epsilon()) // h == 0
+ continue;
+ cc = cc / h;
+ ss = ss / h;
+ R[j + 1][j] = 0.0;
+ if (cc < 0.0) {
+ R[j][j] = -h;
+ cc = -cc;
+ ss = -ss;
+ } else
+ R[j][j] = h;
+
+ xny = ss / (1.0 + cc);
+ for (k = j + 1; k < iq; k++) {
+ t1 = R[j][k];
+ t2 = R[j + 1][k];
+ R[j][k] = t1 * cc + t2 * ss;
+ R[j + 1][k] = xny * (t1 + R[j][k]) - t2;
+ }
+ for (k = 0; k < n; k++) {
+ t1 = J[k][j];
+ t2 = J[k][j + 1];
+ J[k][j] = t1 * cc + t2 * ss;
+ J[k][j + 1] = xny * (J[k][j] + t1) - t2;
+ }
+ }
+}
+
+inline double distance(double a, double b)
+{
+ register double a1, b1, t;
+ a1 = fabs(a);
+ b1 = fabs(b);
+ if (a1 > b1) {
+ t = (b1 / a1);
+ return a1 * sqrt(1.0 + t * t);
+ } else if (b1 > a1) {
+ t = (a1 / b1);
+ return b1 * sqrt(1.0 + t * t);
+ }
+ return a1 * sqrt(2.0);
+}
+
+
+inline double scalar_product(const Vector<double>& x, const Vector<double>& y)
+{
+ register int i, n = x.size();
+ register double sum;
+
+ sum = 0.0;
+ for (i = 0; i < n; i++)
+ sum += x[i] * y[i];
+ return sum;
+}
+
+void cholesky_decomposition(Matrix<double>& A)
+{
+ register int i, j, k, n = A.nrows();
+ register double sum;
+
+ for (i = 0; i < n; i++) {
+ for (j = i; j < n; j++) {
+ sum = A[i][j];
+ for (k = i - 1; k >= 0; k--)
+ sum -= A[i][k]*A[j][k];
+ if (i == j) {
+ if (sum <= 0.0) {
+ std::ostringstream os;
+ // raise error
+ print_matrix("A", A);
+ os << "Error in cholesky decomposition, sum: " << sum;
+// throw std::logic_error(os.str());
+// exit(-1);
+ }
+ A[i][i] = sqrt(sum);
+ } else
+ A[j][i] = sum / A[i][i];
+ }
+ for (k = i + 1; k < n; k++)
+ A[i][k] = A[k][i];
+ }
+}
+
+void cholesky_solve(const Matrix<double>& L, Vector<double>& x, const Vector<double>& b)
+{
+ int n = L.nrows();
+ Vector<double> y(n);
+
+ /* Solve L * y = b */
+ forward_elimination(L, y, b);
+ /* Solve L^T * x = y */
+ backward_elimination(L, x, y);
+}
+
+inline void forward_elimination(const Matrix<double>& L, Vector<double>& y, const Vector<double>& b)
+{
+ register int i, j, n = L.nrows();
+
+ y[0] = b[0] / L[0][0];
+ for (i = 1; i < n; i++) {
+ y[i] = b[i];
+ for (j = 0; j < i; j++)
+ y[i] -= L[i][j] * y[j];
+ y[i] = y[i] / L[i][i];
+ }
+}
+
+inline void backward_elimination(const Matrix<double>& U, Vector<double>& x, const Vector<double>& y)
+{
+ register int i, j, n = U.nrows();
+
+ x[n - 1] = y[n - 1] / U[n - 1][n - 1];
+ for (i = n - 2; i >= 0; i--) {
+ x[i] = y[i];
+ for (j = i + 1; j < n; j++)
+ x[i] -= U[i][j] * x[j];
+ x[i] = x[i] / U[i][i];
+ }
+}
+
+void print_matrix(char* name, const Matrix<double>& A, int n, int m)
+{
+ std::ostringstream s;
+ std::string t;
+ if (n == -1)
+ n = A.nrows();
+ if (m == -1)
+ m = A.ncols();
+
+ s << name << ": " << std::endl;
+ for (int i = 0; i < n; i++) {
+ s << " ";
+ for (int j = 0; j < m; j++)
+ s << A[i][j] << ", ";
+ s << std::endl;
+ }
+ t = s.str();
+ t = t.substr(0, t.size() - 3); // To remove the trailing space, comma and newline
+
+ std::cout << t << std::endl;
+}
+
+template<typename T>
+void print_vector(char* name, const Vector<T>& v, int n)
+{
+ std::ostringstream s;
+ std::string t;
+ if (n == -1)
+ n = v.size();
+
+ s << name << ": " << std::endl << " ";
+ for (int i = 0; i < n; i++) {
+ s << v[i] << ", ";
+ }
+ t = s.str();
+ t = t.substr(0, t.size() - 2); // To remove the trailing space and comma
+
+ std::cout << t << std::endl;
+}
+
+} // namespace quadprogpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/quadprog.h Mon Oct 14 10:08:13 2019 +0000
@@ -0,0 +1,76 @@
+/*
+ File $Id: QuadProg++.hh 232 2007-06-21 12:29:00Z digasper $
+
+ The quadprog_solve() function implements the algorithm of Goldfarb and Idnani
+ for the solution of a (convex) Quadratic Programming problem
+ by means of an active-set dual method.
+
+The problem is in the form:
+
+min 0.5 * x G x + g0 x
+s.t.
+ CE^T x + ce0 = 0
+ CI^T x + ci0 >= 0
+
+ The matrix and vectors dimensions are as follows:
+ G: n * n
+ g0: n
+
+ CE: n * p
+ ce0: p
+
+ CI: n * m
+ ci0: m
+
+ x: n
+
+ The function will return the cost of the solution written in the x vector or
+ std::numeric_limits::infinity() if the problem is infeasible. In the latter case
+ the value of the x vector is not correct.
+
+ References: D. Goldfarb, A. Idnani. A numerically stable dual method for solving
+ strictly convex quadratic programs. Mathematical Programming 27 (1983) pp. 1-33.
+
+ Notes:
+ 1. pay attention in setting up the vectors ce0 and ci0.
+ If the constraints of your problem are specified in the form
+ A^T x = b and C^T x >= d, then you should set ce0 = -b and ci0 = -d.
+ 2. The matrices have column dimension equal to MATRIX_DIM,
+ a constant set to 20 in this file (by means of a #define macro).
+ If the matrices are bigger than 20 x 20 the limit could be
+ increased by means of a -DMATRIX_DIM=n on the compiler command line.
+ 3. The matrix G is modified within the function since it is used to compute
+ the G = L^T L cholesky factorization for further computations inside the function.
+ If you need the original matrix G you should make a copy of it and pass the copy
+ to the function.
+
+ Author: Luca Di Gaspero
+ DIEGM - University of Udine, Italy
+ luca.digaspero@uniud.it
+ http://www.diegm.uniud.it/digaspero/
+
+ The author will be grateful if the researchers using this software will
+ acknowledge the contribution of this function in their research papers.
+
+ Copyright (c) 2007-2016 Luca Di Gaspero
+
+ This software may be modified and distributed under the terms
+ of the MIT license. See the LICENSE file for details.
+*/
+
+
+#ifndef _QUADPROGPP
+#define _QUADPROGPP
+
+#include "Array.h"
+
+namespace quadprogpp {
+
+double solve_quadprog(Matrix<double>& G, Vector<double>& g0,
+ const Matrix<double>& CE, const Vector<double>& ce0,
+ const Matrix<double>& CI, const Vector<double>& ci0,
+ Vector<double>& x);
+
+} // namespace quadprogpp
+
+#endif // #define _QUADPROGPP