![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
This is some awesome robot code
Dependencies: mbed-rtos mbed QEI
Fork of ICRSEurobot13 by
tvmet/MatrixImpl.h@15:9c5aaeda36dc, 2013-04-06 (annotated)
- Committer:
- madcowswe
- Date:
- Sat Apr 06 20:57:54 2013 +0000
- Revision:
- 15:9c5aaeda36dc
Encoders fairly tuned, still has random noise in it
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
madcowswe | 15:9c5aaeda36dc | 1 | /* |
madcowswe | 15:9c5aaeda36dc | 2 | * Tiny Vector Matrix Library |
madcowswe | 15:9c5aaeda36dc | 3 | * Dense Vector Matrix Libary of Tiny size using Expression Templates |
madcowswe | 15:9c5aaeda36dc | 4 | * |
madcowswe | 15:9c5aaeda36dc | 5 | * Copyright (C) 2001 - 2007 Olaf Petzold <opetzold@users.sourceforge.net> |
madcowswe | 15:9c5aaeda36dc | 6 | * |
madcowswe | 15:9c5aaeda36dc | 7 | * This library is free software; you can redistribute it and/or |
madcowswe | 15:9c5aaeda36dc | 8 | * modify it under the terms of the GNU lesser General Public |
madcowswe | 15:9c5aaeda36dc | 9 | * License as published by the Free Software Foundation; either |
madcowswe | 15:9c5aaeda36dc | 10 | * version 2.1 of the License, or (at your option) any later version. |
madcowswe | 15:9c5aaeda36dc | 11 | * |
madcowswe | 15:9c5aaeda36dc | 12 | * This library is distributed in the hope that it will be useful, |
madcowswe | 15:9c5aaeda36dc | 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
madcowswe | 15:9c5aaeda36dc | 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
madcowswe | 15:9c5aaeda36dc | 15 | * lesser General Public License for more details. |
madcowswe | 15:9c5aaeda36dc | 16 | * |
madcowswe | 15:9c5aaeda36dc | 17 | * You should have received a copy of the GNU lesser General Public |
madcowswe | 15:9c5aaeda36dc | 18 | * License along with this library; if not, write to the Free Software |
madcowswe | 15:9c5aaeda36dc | 19 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
madcowswe | 15:9c5aaeda36dc | 20 | * |
madcowswe | 15:9c5aaeda36dc | 21 | * $Id: MatrixImpl.h,v 1.31 2007-06-23 15:58:58 opetzold Exp $ |
madcowswe | 15:9c5aaeda36dc | 22 | */ |
madcowswe | 15:9c5aaeda36dc | 23 | |
madcowswe | 15:9c5aaeda36dc | 24 | #ifndef TVMET_MATRIX_IMPL_H |
madcowswe | 15:9c5aaeda36dc | 25 | #define TVMET_MATRIX_IMPL_H |
madcowswe | 15:9c5aaeda36dc | 26 | |
madcowswe | 15:9c5aaeda36dc | 27 | #include <iomanip> // setw |
madcowswe | 15:9c5aaeda36dc | 28 | |
madcowswe | 15:9c5aaeda36dc | 29 | #include <tvmet/Functional.h> |
madcowswe | 15:9c5aaeda36dc | 30 | #include <tvmet/Io.h> |
madcowswe | 15:9c5aaeda36dc | 31 | |
madcowswe | 15:9c5aaeda36dc | 32 | |
madcowswe | 15:9c5aaeda36dc | 33 | namespace tvmet { |
madcowswe | 15:9c5aaeda36dc | 34 | |
madcowswe | 15:9c5aaeda36dc | 35 | |
madcowswe | 15:9c5aaeda36dc | 36 | /* |
madcowswe | 15:9c5aaeda36dc | 37 | * member operators for i/o |
madcowswe | 15:9c5aaeda36dc | 38 | */ |
madcowswe | 15:9c5aaeda36dc | 39 | template<class T, std::size_t NRows, std::size_t NCols> |
madcowswe | 15:9c5aaeda36dc | 40 | std::ostream& Matrix<T, NRows, NCols>::print_xpr(std::ostream& os, std::size_t l) const |
madcowswe | 15:9c5aaeda36dc | 41 | { |
madcowswe | 15:9c5aaeda36dc | 42 | os << IndentLevel(l++) << "Matrix[" << ops << "]<" |
madcowswe | 15:9c5aaeda36dc | 43 | << typeid(T).name() << ", " << Rows << ", " << Cols << ">," |
madcowswe | 15:9c5aaeda36dc | 44 | << IndentLevel(--l) |
madcowswe | 15:9c5aaeda36dc | 45 | << std::endl; |
madcowswe | 15:9c5aaeda36dc | 46 | |
madcowswe | 15:9c5aaeda36dc | 47 | return os; |
madcowswe | 15:9c5aaeda36dc | 48 | } |
madcowswe | 15:9c5aaeda36dc | 49 | |
madcowswe | 15:9c5aaeda36dc | 50 | |
madcowswe | 15:9c5aaeda36dc | 51 | template<class T, std::size_t NRows, std::size_t NCols> |
madcowswe | 15:9c5aaeda36dc | 52 | std::ostream& Matrix<T, NRows, NCols>::print_on(std::ostream& os) const |
madcowswe | 15:9c5aaeda36dc | 53 | { |
madcowswe | 15:9c5aaeda36dc | 54 | enum { |
madcowswe | 15:9c5aaeda36dc | 55 | complex_type = NumericTraits<value_type>::is_complex |
madcowswe | 15:9c5aaeda36dc | 56 | }; |
madcowswe | 15:9c5aaeda36dc | 57 | |
madcowswe | 15:9c5aaeda36dc | 58 | std::streamsize w = IoPrintHelper<Matrix>::width(dispatch<complex_type>(), *this); |
madcowswe | 15:9c5aaeda36dc | 59 | |
madcowswe | 15:9c5aaeda36dc | 60 | os << std::setw(0) << "[\n"; |
madcowswe | 15:9c5aaeda36dc | 61 | for(std::size_t i = 0; i < Rows; ++i) { |
madcowswe | 15:9c5aaeda36dc | 62 | os << " ["; |
madcowswe | 15:9c5aaeda36dc | 63 | for(std::size_t j = 0; j < (Cols - 1); ++j) { |
madcowswe | 15:9c5aaeda36dc | 64 | os << std::setw(w) << this->operator()(i, j) << ", "; |
madcowswe | 15:9c5aaeda36dc | 65 | } |
madcowswe | 15:9c5aaeda36dc | 66 | os << std::setw(w) << this->operator()(i, Cols - 1) |
madcowswe | 15:9c5aaeda36dc | 67 | << (i != (Rows-1) ? "],\n" : "]\n"); |
madcowswe | 15:9c5aaeda36dc | 68 | } |
madcowswe | 15:9c5aaeda36dc | 69 | os << "]"; |
madcowswe | 15:9c5aaeda36dc | 70 | |
madcowswe | 15:9c5aaeda36dc | 71 | return os; |
madcowswe | 15:9c5aaeda36dc | 72 | } |
madcowswe | 15:9c5aaeda36dc | 73 | |
madcowswe | 15:9c5aaeda36dc | 74 | |
madcowswe | 15:9c5aaeda36dc | 75 | /* |
madcowswe | 15:9c5aaeda36dc | 76 | * member operators with scalars, per se element wise |
madcowswe | 15:9c5aaeda36dc | 77 | */ |
madcowswe | 15:9c5aaeda36dc | 78 | #define TVMET_IMPLEMENT_MACRO(NAME, OP) \ |
madcowswe | 15:9c5aaeda36dc | 79 | template<class T, std::size_t NRows, std::size_t NCols> \ |
madcowswe | 15:9c5aaeda36dc | 80 | inline \ |
madcowswe | 15:9c5aaeda36dc | 81 | Matrix<T, NRows, NCols>& \ |
madcowswe | 15:9c5aaeda36dc | 82 | Matrix<T, NRows, NCols>::operator OP (value_type rhs) { \ |
madcowswe | 15:9c5aaeda36dc | 83 | typedef XprLiteral<value_type> expr_type; \ |
madcowswe | 15:9c5aaeda36dc | 84 | this->M_##NAME(XprMatrix<expr_type, Rows, Cols>(expr_type(rhs))); \ |
madcowswe | 15:9c5aaeda36dc | 85 | return *this; \ |
madcowswe | 15:9c5aaeda36dc | 86 | } |
madcowswe | 15:9c5aaeda36dc | 87 | |
madcowswe | 15:9c5aaeda36dc | 88 | TVMET_IMPLEMENT_MACRO(add_eq, +=) |
madcowswe | 15:9c5aaeda36dc | 89 | TVMET_IMPLEMENT_MACRO(sub_eq, -=) |
madcowswe | 15:9c5aaeda36dc | 90 | TVMET_IMPLEMENT_MACRO(mul_eq, *=) |
madcowswe | 15:9c5aaeda36dc | 91 | TVMET_IMPLEMENT_MACRO(div_eq, /=) |
madcowswe | 15:9c5aaeda36dc | 92 | #undef TVMET_IMPLEMENT_MACRO |
madcowswe | 15:9c5aaeda36dc | 93 | |
madcowswe | 15:9c5aaeda36dc | 94 | |
madcowswe | 15:9c5aaeda36dc | 95 | #define TVMET_IMPLEMENT_MACRO(NAME, OP) \ |
madcowswe | 15:9c5aaeda36dc | 96 | template<class T, std::size_t NRows, std::size_t NCols> \ |
madcowswe | 15:9c5aaeda36dc | 97 | inline \ |
madcowswe | 15:9c5aaeda36dc | 98 | Matrix<T, NRows, NCols>& \ |
madcowswe | 15:9c5aaeda36dc | 99 | Matrix<T, NRows, NCols>::operator OP (std::size_t rhs) { \ |
madcowswe | 15:9c5aaeda36dc | 100 | typedef XprLiteral<value_type> expr_type; \ |
madcowswe | 15:9c5aaeda36dc | 101 | this->M_##NAME(XprMatrix<expr_type, Rows, Cols>(expr_type(rhs))); \ |
madcowswe | 15:9c5aaeda36dc | 102 | return *this; \ |
madcowswe | 15:9c5aaeda36dc | 103 | } |
madcowswe | 15:9c5aaeda36dc | 104 | |
madcowswe | 15:9c5aaeda36dc | 105 | TVMET_IMPLEMENT_MACRO(mod_eq, %=) |
madcowswe | 15:9c5aaeda36dc | 106 | TVMET_IMPLEMENT_MACRO(xor_eq,^=) |
madcowswe | 15:9c5aaeda36dc | 107 | TVMET_IMPLEMENT_MACRO(and_eq, &=) |
madcowswe | 15:9c5aaeda36dc | 108 | TVMET_IMPLEMENT_MACRO(or_eq, |=) |
madcowswe | 15:9c5aaeda36dc | 109 | TVMET_IMPLEMENT_MACRO(shl_eq, <<=) |
madcowswe | 15:9c5aaeda36dc | 110 | TVMET_IMPLEMENT_MACRO(shr_eq, >>=) |
madcowswe | 15:9c5aaeda36dc | 111 | #undef TVMET_IMPLEMENT_MACRO |
madcowswe | 15:9c5aaeda36dc | 112 | |
madcowswe | 15:9c5aaeda36dc | 113 | |
madcowswe | 15:9c5aaeda36dc | 114 | /* |
madcowswe | 15:9c5aaeda36dc | 115 | * member functions (operators) with matrizes, for use with +=,-= ... <<= |
madcowswe | 15:9c5aaeda36dc | 116 | */ |
madcowswe | 15:9c5aaeda36dc | 117 | #define TVMET_IMPLEMENT_MACRO(NAME) \ |
madcowswe | 15:9c5aaeda36dc | 118 | template<class T1, std::size_t NRows, std::size_t NCols> \ |
madcowswe | 15:9c5aaeda36dc | 119 | template <class T2> \ |
madcowswe | 15:9c5aaeda36dc | 120 | inline \ |
madcowswe | 15:9c5aaeda36dc | 121 | Matrix<T1, NRows, NCols>& \ |
madcowswe | 15:9c5aaeda36dc | 122 | Matrix<T1, NRows, NCols>::M_##NAME (const Matrix<T2, Rows, Cols>& rhs) { \ |
madcowswe | 15:9c5aaeda36dc | 123 | this->M_##NAME( XprMatrix<typename Matrix<T2, Rows, Cols>::ConstReference, Rows, Cols>(rhs.const_ref()) ); \ |
madcowswe | 15:9c5aaeda36dc | 124 | return *this; \ |
madcowswe | 15:9c5aaeda36dc | 125 | } |
madcowswe | 15:9c5aaeda36dc | 126 | |
madcowswe | 15:9c5aaeda36dc | 127 | TVMET_IMPLEMENT_MACRO(add_eq) |
madcowswe | 15:9c5aaeda36dc | 128 | TVMET_IMPLEMENT_MACRO(sub_eq) |
madcowswe | 15:9c5aaeda36dc | 129 | TVMET_IMPLEMENT_MACRO(mul_eq) |
madcowswe | 15:9c5aaeda36dc | 130 | TVMET_IMPLEMENT_MACRO(div_eq) |
madcowswe | 15:9c5aaeda36dc | 131 | TVMET_IMPLEMENT_MACRO(mod_eq) |
madcowswe | 15:9c5aaeda36dc | 132 | TVMET_IMPLEMENT_MACRO(xor_eq) |
madcowswe | 15:9c5aaeda36dc | 133 | TVMET_IMPLEMENT_MACRO(and_eq) |
madcowswe | 15:9c5aaeda36dc | 134 | TVMET_IMPLEMENT_MACRO(or_eq) |
madcowswe | 15:9c5aaeda36dc | 135 | TVMET_IMPLEMENT_MACRO(shl_eq) |
madcowswe | 15:9c5aaeda36dc | 136 | TVMET_IMPLEMENT_MACRO(shr_eq) |
madcowswe | 15:9c5aaeda36dc | 137 | #undef TVMET_IMPLEMENT_MACRO |
madcowswe | 15:9c5aaeda36dc | 138 | |
madcowswe | 15:9c5aaeda36dc | 139 | |
madcowswe | 15:9c5aaeda36dc | 140 | /* |
madcowswe | 15:9c5aaeda36dc | 141 | * member functions (operators) with expressions, for use width +=,-= ... <<= |
madcowswe | 15:9c5aaeda36dc | 142 | */ |
madcowswe | 15:9c5aaeda36dc | 143 | #define TVMET_IMPLEMENT_MACRO(NAME) \ |
madcowswe | 15:9c5aaeda36dc | 144 | template<class T, std::size_t NRows, std::size_t NCols> \ |
madcowswe | 15:9c5aaeda36dc | 145 | template<class E> \ |
madcowswe | 15:9c5aaeda36dc | 146 | inline \ |
madcowswe | 15:9c5aaeda36dc | 147 | Matrix<T, NRows, NCols>& \ |
madcowswe | 15:9c5aaeda36dc | 148 | Matrix<T, NRows, NCols>::M_##NAME (const XprMatrix<E, Rows, Cols>& rhs) { \ |
madcowswe | 15:9c5aaeda36dc | 149 | rhs.assign_to(*this, Fcnl_##NAME<value_type, typename E::value_type>()); \ |
madcowswe | 15:9c5aaeda36dc | 150 | return *this; \ |
madcowswe | 15:9c5aaeda36dc | 151 | } |
madcowswe | 15:9c5aaeda36dc | 152 | |
madcowswe | 15:9c5aaeda36dc | 153 | TVMET_IMPLEMENT_MACRO(add_eq) |
madcowswe | 15:9c5aaeda36dc | 154 | TVMET_IMPLEMENT_MACRO(sub_eq) |
madcowswe | 15:9c5aaeda36dc | 155 | TVMET_IMPLEMENT_MACRO(mul_eq) |
madcowswe | 15:9c5aaeda36dc | 156 | TVMET_IMPLEMENT_MACRO(div_eq) |
madcowswe | 15:9c5aaeda36dc | 157 | TVMET_IMPLEMENT_MACRO(mod_eq) |
madcowswe | 15:9c5aaeda36dc | 158 | TVMET_IMPLEMENT_MACRO(xor_eq) |
madcowswe | 15:9c5aaeda36dc | 159 | TVMET_IMPLEMENT_MACRO(and_eq) |
madcowswe | 15:9c5aaeda36dc | 160 | TVMET_IMPLEMENT_MACRO(or_eq) |
madcowswe | 15:9c5aaeda36dc | 161 | TVMET_IMPLEMENT_MACRO(shl_eq) |
madcowswe | 15:9c5aaeda36dc | 162 | TVMET_IMPLEMENT_MACRO(shr_eq) |
madcowswe | 15:9c5aaeda36dc | 163 | #undef TVMET_IMPLEMENT_MACRO |
madcowswe | 15:9c5aaeda36dc | 164 | |
madcowswe | 15:9c5aaeda36dc | 165 | |
madcowswe | 15:9c5aaeda36dc | 166 | /* |
madcowswe | 15:9c5aaeda36dc | 167 | * aliased member functions (operators) with matrizes, |
madcowswe | 15:9c5aaeda36dc | 168 | * for use with +=,-= ... <<= |
madcowswe | 15:9c5aaeda36dc | 169 | */ |
madcowswe | 15:9c5aaeda36dc | 170 | #define TVMET_IMPLEMENT_MACRO(NAME) \ |
madcowswe | 15:9c5aaeda36dc | 171 | template<class T1, std::size_t NRows, std::size_t NCols> \ |
madcowswe | 15:9c5aaeda36dc | 172 | template <class T2> \ |
madcowswe | 15:9c5aaeda36dc | 173 | inline \ |
madcowswe | 15:9c5aaeda36dc | 174 | Matrix<T1, NRows, NCols>& \ |
madcowswe | 15:9c5aaeda36dc | 175 | Matrix<T1, NRows, NCols>::alias_##NAME (const Matrix<T2, Rows, Cols>& rhs) { \ |
madcowswe | 15:9c5aaeda36dc | 176 | this->alias_##NAME( XprMatrix<typename Matrix<T2, Rows, Cols>::ConstReference, Rows, Cols>(rhs.const_ref()) ); \ |
madcowswe | 15:9c5aaeda36dc | 177 | return *this; \ |
madcowswe | 15:9c5aaeda36dc | 178 | } |
madcowswe | 15:9c5aaeda36dc | 179 | |
madcowswe | 15:9c5aaeda36dc | 180 | TVMET_IMPLEMENT_MACRO(assign) |
madcowswe | 15:9c5aaeda36dc | 181 | TVMET_IMPLEMENT_MACRO(add_eq) |
madcowswe | 15:9c5aaeda36dc | 182 | TVMET_IMPLEMENT_MACRO(sub_eq) |
madcowswe | 15:9c5aaeda36dc | 183 | TVMET_IMPLEMENT_MACRO(mul_eq) |
madcowswe | 15:9c5aaeda36dc | 184 | TVMET_IMPLEMENT_MACRO(div_eq) |
madcowswe | 15:9c5aaeda36dc | 185 | #undef TVMET_IMPLEMENT_MACRO |
madcowswe | 15:9c5aaeda36dc | 186 | |
madcowswe | 15:9c5aaeda36dc | 187 | |
madcowswe | 15:9c5aaeda36dc | 188 | /* |
madcowswe | 15:9c5aaeda36dc | 189 | * aliased member functions (operators) with expressions, |
madcowswe | 15:9c5aaeda36dc | 190 | * for use width +=,-= ... <<= and aliased(), |
madcowswe | 15:9c5aaeda36dc | 191 | */ |
madcowswe | 15:9c5aaeda36dc | 192 | #define TVMET_IMPLEMENT_MACRO(NAME) \ |
madcowswe | 15:9c5aaeda36dc | 193 | template<class T, std::size_t NRows, std::size_t NCols> \ |
madcowswe | 15:9c5aaeda36dc | 194 | template<class E> \ |
madcowswe | 15:9c5aaeda36dc | 195 | inline \ |
madcowswe | 15:9c5aaeda36dc | 196 | Matrix<T, NRows, NCols>& \ |
madcowswe | 15:9c5aaeda36dc | 197 | Matrix<T, NRows, NCols>::alias_##NAME (const XprMatrix<E, Rows, Cols>& rhs) { \ |
madcowswe | 15:9c5aaeda36dc | 198 | typedef Matrix<T, NRows, NCols> temp_type; \ |
madcowswe | 15:9c5aaeda36dc | 199 | temp_type(rhs).assign_to(*this, Fcnl_##NAME<value_type, typename E::value_type>()); \ |
madcowswe | 15:9c5aaeda36dc | 200 | return *this; \ |
madcowswe | 15:9c5aaeda36dc | 201 | } |
madcowswe | 15:9c5aaeda36dc | 202 | |
madcowswe | 15:9c5aaeda36dc | 203 | TVMET_IMPLEMENT_MACRO(assign) |
madcowswe | 15:9c5aaeda36dc | 204 | TVMET_IMPLEMENT_MACRO(add_eq) |
madcowswe | 15:9c5aaeda36dc | 205 | TVMET_IMPLEMENT_MACRO(sub_eq) |
madcowswe | 15:9c5aaeda36dc | 206 | TVMET_IMPLEMENT_MACRO(mul_eq) |
madcowswe | 15:9c5aaeda36dc | 207 | TVMET_IMPLEMENT_MACRO(div_eq) |
madcowswe | 15:9c5aaeda36dc | 208 | #undef TVMET_IMPLEMENT_MACRO |
madcowswe | 15:9c5aaeda36dc | 209 | |
madcowswe | 15:9c5aaeda36dc | 210 | |
madcowswe | 15:9c5aaeda36dc | 211 | } // namespace tvmet |
madcowswe | 15:9c5aaeda36dc | 212 | |
madcowswe | 15:9c5aaeda36dc | 213 | #endif // TVMET_MATRIX_IMPL_H |
madcowswe | 15:9c5aaeda36dc | 214 | |
madcowswe | 15:9c5aaeda36dc | 215 | // Local Variables: |
madcowswe | 15:9c5aaeda36dc | 216 | // mode:C++ |
madcowswe | 15:9c5aaeda36dc | 217 | // tab-width:8 |
madcowswe | 15:9c5aaeda36dc | 218 | // End: |