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: Matrix.h,v 1.19 2007-06-23 15:58:59 opetzold Exp $
madcowswe 15:9c5aaeda36dc 22 */
madcowswe 15:9c5aaeda36dc 23
madcowswe 15:9c5aaeda36dc 24 #ifndef TVMET_META_MATRIX_H
madcowswe 15:9c5aaeda36dc 25 #define TVMET_META_MATRIX_H
madcowswe 15:9c5aaeda36dc 26
madcowswe 15:9c5aaeda36dc 27 #include <tvmet/NumericTraits.h>
madcowswe 15:9c5aaeda36dc 28 #include <tvmet/xpr/Null.h>
madcowswe 15:9c5aaeda36dc 29
madcowswe 15:9c5aaeda36dc 30 namespace tvmet {
madcowswe 15:9c5aaeda36dc 31
madcowswe 15:9c5aaeda36dc 32 namespace meta {
madcowswe 15:9c5aaeda36dc 33
madcowswe 15:9c5aaeda36dc 34
madcowswe 15:9c5aaeda36dc 35 /**
madcowswe 15:9c5aaeda36dc 36 * \class Matrix Matrix.h "tvmet/meta/Matrix.h"
madcowswe 15:9c5aaeda36dc 37 * \brief Meta %Matrix class using expression and meta templates.
madcowswe 15:9c5aaeda36dc 38 */
madcowswe 15:9c5aaeda36dc 39 template<std::size_t Rows, std::size_t Cols,
madcowswe 15:9c5aaeda36dc 40 std::size_t M=0, std::size_t N=0>
madcowswe 15:9c5aaeda36dc 41 class Matrix
madcowswe 15:9c5aaeda36dc 42 {
madcowswe 15:9c5aaeda36dc 43 Matrix();
madcowswe 15:9c5aaeda36dc 44 Matrix(const Matrix&);
madcowswe 15:9c5aaeda36dc 45 Matrix& operator=(const Matrix&);
madcowswe 15:9c5aaeda36dc 46
madcowswe 15:9c5aaeda36dc 47 private:
madcowswe 15:9c5aaeda36dc 48 enum {
madcowswe 15:9c5aaeda36dc 49 doRows = (M < Rows - 1) ? 1 : 0, /**< recursive counter Rows. */
madcowswe 15:9c5aaeda36dc 50 doCols = (N < Cols - 1) ? 1 : 0 /**< recursive counter Cols. */
madcowswe 15:9c5aaeda36dc 51 };
madcowswe 15:9c5aaeda36dc 52
madcowswe 15:9c5aaeda36dc 53 public:
madcowswe 15:9c5aaeda36dc 54 /** assign an expression on columns on given row using the functional assign_fn. */
madcowswe 15:9c5aaeda36dc 55 template<class Dest, class Src, class Assign>
madcowswe 15:9c5aaeda36dc 56 static inline
madcowswe 15:9c5aaeda36dc 57 void assign2(Dest& lhs, const Src& rhs, const Assign& assign_fn) {
madcowswe 15:9c5aaeda36dc 58 assign_fn.apply_on(lhs(M, N), rhs(M, N));
madcowswe 15:9c5aaeda36dc 59 Matrix<Rows * doCols, Cols * doCols,
madcowswe 15:9c5aaeda36dc 60 M * doCols, (N+1) * doCols>::assign2(lhs, rhs, assign_fn);
madcowswe 15:9c5aaeda36dc 61 }
madcowswe 15:9c5aaeda36dc 62
madcowswe 15:9c5aaeda36dc 63 /** assign an expression on row-wise using the functional assign_fn. */
madcowswe 15:9c5aaeda36dc 64 template<class Dest, class Src, class Assign>
madcowswe 15:9c5aaeda36dc 65 static inline
madcowswe 15:9c5aaeda36dc 66 void assign(Dest& lhs, const Src& rhs, const Assign& assign_fn) {
madcowswe 15:9c5aaeda36dc 67 Matrix<Rows, Cols,
madcowswe 15:9c5aaeda36dc 68 M, 0>::assign2(lhs, rhs, assign_fn);
madcowswe 15:9c5aaeda36dc 69 Matrix<Rows * doRows, Cols * doRows,
madcowswe 15:9c5aaeda36dc 70 (M+1) * doRows, 0>::assign(lhs, rhs, assign_fn);
madcowswe 15:9c5aaeda36dc 71 }
madcowswe 15:9c5aaeda36dc 72
madcowswe 15:9c5aaeda36dc 73 /** evaluate a given matrix expression, column wise. */
madcowswe 15:9c5aaeda36dc 74 template<class E>
madcowswe 15:9c5aaeda36dc 75 static inline
madcowswe 15:9c5aaeda36dc 76 bool all_elements2(const E& e) {
madcowswe 15:9c5aaeda36dc 77 if(!e(M, N)) return false;
madcowswe 15:9c5aaeda36dc 78 return Matrix<Rows * doCols, Cols * doCols,
madcowswe 15:9c5aaeda36dc 79 M * doCols, (N+1) * doCols>::all_elements2(e);
madcowswe 15:9c5aaeda36dc 80 }
madcowswe 15:9c5aaeda36dc 81
madcowswe 15:9c5aaeda36dc 82 /** evaluate a given matrix expression, row wise. */
madcowswe 15:9c5aaeda36dc 83 template<class E>
madcowswe 15:9c5aaeda36dc 84 static inline
madcowswe 15:9c5aaeda36dc 85 bool all_elements(const E& e) {
madcowswe 15:9c5aaeda36dc 86 if(!Matrix<Rows, Cols, M, 0>::all_elements2(e) ) return false;
madcowswe 15:9c5aaeda36dc 87 return Matrix<Rows * doRows, Cols * doRows,
madcowswe 15:9c5aaeda36dc 88 (M+1) * doRows, 0>::all_elements(e);
madcowswe 15:9c5aaeda36dc 89 }
madcowswe 15:9c5aaeda36dc 90
madcowswe 15:9c5aaeda36dc 91 /** evaluate a given matrix expression, column wise. */
madcowswe 15:9c5aaeda36dc 92 template<class E>
madcowswe 15:9c5aaeda36dc 93 static inline
madcowswe 15:9c5aaeda36dc 94 bool any_elements2(const E& e) {
madcowswe 15:9c5aaeda36dc 95 if(e(M, N)) return true;
madcowswe 15:9c5aaeda36dc 96 return Matrix<Rows * doCols, Cols * doCols,
madcowswe 15:9c5aaeda36dc 97 M * doCols, (N+1) * doCols>::any_elements2(e);
madcowswe 15:9c5aaeda36dc 98 }
madcowswe 15:9c5aaeda36dc 99
madcowswe 15:9c5aaeda36dc 100 /** evaluate a given matrix expression, row wise. */
madcowswe 15:9c5aaeda36dc 101 template<class E>
madcowswe 15:9c5aaeda36dc 102 static inline
madcowswe 15:9c5aaeda36dc 103 bool any_elements(const E& e) {
madcowswe 15:9c5aaeda36dc 104 if(Matrix<Rows, Cols, M, 0>::any_elements2(e) ) return true;
madcowswe 15:9c5aaeda36dc 105 return Matrix<Rows * doRows, Cols * doRows,
madcowswe 15:9c5aaeda36dc 106 (M+1) * doRows, 0>::any_elements(e);
madcowswe 15:9c5aaeda36dc 107 }
madcowswe 15:9c5aaeda36dc 108
madcowswe 15:9c5aaeda36dc 109 /** trace a given matrix expression. */
madcowswe 15:9c5aaeda36dc 110 template<class E>
madcowswe 15:9c5aaeda36dc 111 static inline
madcowswe 15:9c5aaeda36dc 112 typename E::value_type
madcowswe 15:9c5aaeda36dc 113 trace(const E& e) {
madcowswe 15:9c5aaeda36dc 114 return e(M, N)
madcowswe 15:9c5aaeda36dc 115 + Matrix<Rows * doCols, Cols * doCols,
madcowswe 15:9c5aaeda36dc 116 (M+1) * doCols, (N+1) * doCols>::trace(e);
madcowswe 15:9c5aaeda36dc 117 }
madcowswe 15:9c5aaeda36dc 118
madcowswe 15:9c5aaeda36dc 119 };
madcowswe 15:9c5aaeda36dc 120
madcowswe 15:9c5aaeda36dc 121
madcowswe 15:9c5aaeda36dc 122 /**
madcowswe 15:9c5aaeda36dc 123 * \class Matrix<0, 0, 0, 0> Matrix.h "tvmet/meta/Matrix.h"
madcowswe 15:9c5aaeda36dc 124 * \brief Meta %Matrix specialized for recursion.
madcowswe 15:9c5aaeda36dc 125 */
madcowswe 15:9c5aaeda36dc 126 template<>
madcowswe 15:9c5aaeda36dc 127 class Matrix<0, 0, 0, 0>
madcowswe 15:9c5aaeda36dc 128 {
madcowswe 15:9c5aaeda36dc 129 Matrix();
madcowswe 15:9c5aaeda36dc 130 Matrix(const Matrix&);
madcowswe 15:9c5aaeda36dc 131 Matrix& operator=(const Matrix&);
madcowswe 15:9c5aaeda36dc 132
madcowswe 15:9c5aaeda36dc 133 public:
madcowswe 15:9c5aaeda36dc 134 template<class Dest, class Src, class Assign>
madcowswe 15:9c5aaeda36dc 135 static inline void assign2(Dest&, const Src&, const Assign&) { }
madcowswe 15:9c5aaeda36dc 136
madcowswe 15:9c5aaeda36dc 137 template<class Dest, class Src, class Assign>
madcowswe 15:9c5aaeda36dc 138 static inline void assign(Dest&, const Src&, const Assign&) { }
madcowswe 15:9c5aaeda36dc 139
madcowswe 15:9c5aaeda36dc 140 template<class E>
madcowswe 15:9c5aaeda36dc 141 static inline bool all_elements2(const E&) { return true; }
madcowswe 15:9c5aaeda36dc 142
madcowswe 15:9c5aaeda36dc 143 template<class E>
madcowswe 15:9c5aaeda36dc 144 static inline bool all_elements(const E&) { return true; }
madcowswe 15:9c5aaeda36dc 145
madcowswe 15:9c5aaeda36dc 146 template<class E>
madcowswe 15:9c5aaeda36dc 147 static inline bool any_elements2(const E&) { return false; }
madcowswe 15:9c5aaeda36dc 148
madcowswe 15:9c5aaeda36dc 149 template<class E>
madcowswe 15:9c5aaeda36dc 150 static inline bool any_elements(const E&) { return false; }
madcowswe 15:9c5aaeda36dc 151
madcowswe 15:9c5aaeda36dc 152 template<class E>
madcowswe 15:9c5aaeda36dc 153 static inline XprNull trace(const E&) { return XprNull(); }
madcowswe 15:9c5aaeda36dc 154 };
madcowswe 15:9c5aaeda36dc 155
madcowswe 15:9c5aaeda36dc 156
madcowswe 15:9c5aaeda36dc 157 } // namespace meta
madcowswe 15:9c5aaeda36dc 158
madcowswe 15:9c5aaeda36dc 159 } // namespace tvmet
madcowswe 15:9c5aaeda36dc 160
madcowswe 15:9c5aaeda36dc 161 #endif /* TVMET_META_MATRIX_H */
madcowswe 15:9c5aaeda36dc 162
madcowswe 15:9c5aaeda36dc 163 // Local Variables:
madcowswe 15:9c5aaeda36dc 164 // mode:C++
madcowswe 15:9c5aaeda36dc 165 // tab-width:8
madcowswe 15:9c5aaeda36dc 166 // End: