ICRS Eurobot 2013

Dependencies:   mbed mbed-rtos Servo QEI

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?

UserRevisionLine numberNew 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: