ICRS Eurobot 2013

Dependencies:   mbed mbed-rtos Servo QEI

Committer:
madcowswe
Date:
Tue Apr 09 15:33:36 2013 +0000
Revision:
20:70d651156779
Parent:
15:9c5aaeda36dc
Predict loop running, update loop not done.

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: General.h,v 1.13 2007-06-23 15:58:59 opetzold Exp $
madcowswe 15:9c5aaeda36dc 22 */
madcowswe 15:9c5aaeda36dc 23
madcowswe 15:9c5aaeda36dc 24 #ifndef TVMET_UTIL_GENERAL_H
madcowswe 15:9c5aaeda36dc 25 #define TVMET_UTIL_GENERAL_H
madcowswe 15:9c5aaeda36dc 26
madcowswe 15:9c5aaeda36dc 27
madcowswe 15:9c5aaeda36dc 28 /** forward */
madcowswe 15:9c5aaeda36dc 29 namespace tvmet {
madcowswe 15:9c5aaeda36dc 30 template<class T, std::size_t Rows, std::size_t Cols> class Matrix;
madcowswe 15:9c5aaeda36dc 31 template<class T, std::size_t Sz> class Vector;
madcowswe 15:9c5aaeda36dc 32 }
madcowswe 15:9c5aaeda36dc 33
madcowswe 15:9c5aaeda36dc 34 namespace tvmet {
madcowswe 15:9c5aaeda36dc 35
madcowswe 15:9c5aaeda36dc 36 namespace util {
madcowswe 15:9c5aaeda36dc 37
madcowswe 15:9c5aaeda36dc 38 /*
madcowswe 15:9c5aaeda36dc 39 * \defgroup _util_function
madcowswe 15:9c5aaeda36dc 40 * \brief Usefull utility functions
madcowswe 15:9c5aaeda36dc 41 */
madcowswe 15:9c5aaeda36dc 42
madcowswe 15:9c5aaeda36dc 43 /**
madcowswe 15:9c5aaeda36dc 44 * \fn Gemm(const Matrix<T, Rows, Cols>& m1, const Matrix<T, Rows, Cols>& m2, Matrix<T, Rows, Cols>& m3)
madcowswe 15:9c5aaeda36dc 45 * \brief General matrix matrix multiplication using loops.
madcowswe 15:9c5aaeda36dc 46 * \ingroup _util_function
madcowswe 15:9c5aaeda36dc 47 */
madcowswe 15:9c5aaeda36dc 48 template<class T, std::size_t Rows, std::size_t Cols>
madcowswe 15:9c5aaeda36dc 49 inline
madcowswe 15:9c5aaeda36dc 50 void
madcowswe 15:9c5aaeda36dc 51 Gemm(const Matrix<T, Rows, Cols>& m1, const Matrix<T, Rows, Cols>& m2,
madcowswe 15:9c5aaeda36dc 52 Matrix<T, Rows, Cols>& m3)
madcowswe 15:9c5aaeda36dc 53 {
madcowswe 15:9c5aaeda36dc 54 for (std::size_t i = 0; i < Rows; ++i) {
madcowswe 15:9c5aaeda36dc 55 for (std::size_t j = 0; j < Cols; ++j) {
madcowswe 15:9c5aaeda36dc 56 T sum(0);
madcowswe 15:9c5aaeda36dc 57 for (std::size_t k = 0; k < Cols; ++k) {
madcowswe 15:9c5aaeda36dc 58 sum += m1(i,k) * m2(k,j);
madcowswe 15:9c5aaeda36dc 59 }
madcowswe 15:9c5aaeda36dc 60 m3(i,j) = sum;
madcowswe 15:9c5aaeda36dc 61 }
madcowswe 15:9c5aaeda36dc 62 }
madcowswe 15:9c5aaeda36dc 63 }
madcowswe 15:9c5aaeda36dc 64
madcowswe 15:9c5aaeda36dc 65
madcowswe 15:9c5aaeda36dc 66 /**
madcowswe 15:9c5aaeda36dc 67 * \fn Gemv(const Matrix<T, Rows, Cols>& m, const Vector<T, Cols>& v, Vector<T, Cols>& v2)
madcowswe 15:9c5aaeda36dc 68 * \brief General matrix vector multiplication using loops.
madcowswe 15:9c5aaeda36dc 69 * \ingroup _util_function
madcowswe 15:9c5aaeda36dc 70 */
madcowswe 15:9c5aaeda36dc 71 template<class T, std::size_t Rows, std::size_t Cols>
madcowswe 15:9c5aaeda36dc 72 inline
madcowswe 15:9c5aaeda36dc 73 void
madcowswe 15:9c5aaeda36dc 74 Gemv(const Matrix<T, Rows, Cols>& m, const Vector<T, Cols>& v,
madcowswe 15:9c5aaeda36dc 75 Vector<T, Cols>& v2)
madcowswe 15:9c5aaeda36dc 76 {
madcowswe 15:9c5aaeda36dc 77 for (std::size_t i = 0; i < Rows; ++i){
madcowswe 15:9c5aaeda36dc 78 v2(i) = T(0); // clean up before use
madcowswe 15:9c5aaeda36dc 79 for (std::size_t j = 0; j < Cols; ++j) {
madcowswe 15:9c5aaeda36dc 80 v2(i) += m(i,j) * v(j);
madcowswe 15:9c5aaeda36dc 81 }
madcowswe 15:9c5aaeda36dc 82 }
madcowswe 15:9c5aaeda36dc 83 }
madcowswe 15:9c5aaeda36dc 84
madcowswe 15:9c5aaeda36dc 85
madcowswe 15:9c5aaeda36dc 86 /**
madcowswe 15:9c5aaeda36dc 87 * \fn Gevvmul(const Vector<T, Sz>& v1, const Vector<T, Sz>& v2, Vector<T, Sz>& v3)
madcowswe 15:9c5aaeda36dc 88 * \brief General vector vector elementwise multiplication using loop.
madcowswe 15:9c5aaeda36dc 89 * \ingroup _util_function
madcowswe 15:9c5aaeda36dc 90 */
madcowswe 15:9c5aaeda36dc 91 template<class T, std::size_t Sz>
madcowswe 15:9c5aaeda36dc 92 inline
madcowswe 15:9c5aaeda36dc 93 void
madcowswe 15:9c5aaeda36dc 94 Gevvmul(const Vector<T, Sz>& v1, const Vector<T, Sz>& v2,
madcowswe 15:9c5aaeda36dc 95 Vector<T, Sz>& v3)
madcowswe 15:9c5aaeda36dc 96 {
madcowswe 15:9c5aaeda36dc 97 for(std::size_t i = 0; i < Sz; ++i)
madcowswe 15:9c5aaeda36dc 98 v3(i) = v1(i) * v2(i);
madcowswe 15:9c5aaeda36dc 99 }
madcowswe 15:9c5aaeda36dc 100
madcowswe 15:9c5aaeda36dc 101
madcowswe 15:9c5aaeda36dc 102 /**
madcowswe 15:9c5aaeda36dc 103 * \fn Gevvadd(const Vector<T, Sz>& v1, const Vector<T, Sz>& v2, Vector<T, Sz>& v3)
madcowswe 15:9c5aaeda36dc 104 * \brief General vector vector elementwise multiplication using loop.
madcowswe 15:9c5aaeda36dc 105 * \ingroup _util_function
madcowswe 15:9c5aaeda36dc 106 */
madcowswe 15:9c5aaeda36dc 107 template<class T, std::size_t Sz>
madcowswe 15:9c5aaeda36dc 108 inline
madcowswe 15:9c5aaeda36dc 109 void
madcowswe 15:9c5aaeda36dc 110 Gevvadd(const Vector<T, Sz>& v1, const Vector<T, Sz>& v2,
madcowswe 15:9c5aaeda36dc 111 Vector<T, Sz>& v3)
madcowswe 15:9c5aaeda36dc 112 {
madcowswe 15:9c5aaeda36dc 113 for(std::size_t i = 0; i < Sz; ++i)
madcowswe 15:9c5aaeda36dc 114 v3(i) = v1(i) + v2(i);
madcowswe 15:9c5aaeda36dc 115 }
madcowswe 15:9c5aaeda36dc 116
madcowswe 15:9c5aaeda36dc 117 } // namespace util
madcowswe 15:9c5aaeda36dc 118
madcowswe 15:9c5aaeda36dc 119 } // namespace tvmet
madcowswe 15:9c5aaeda36dc 120
madcowswe 15:9c5aaeda36dc 121 #endif // TVMET_UTIL_GENERAL_H
madcowswe 15:9c5aaeda36dc 122
madcowswe 15:9c5aaeda36dc 123 // Local Variables:
madcowswe 15:9c5aaeda36dc 124 // mode:C++
madcowswe 15:9c5aaeda36dc 125 // tab-width:8
madcowswe 15:9c5aaeda36dc 126 // End: