Cooper Liu / Mbed 2 deprecated Eurobot2013_Co-Processor

working version with calibration done

Fork of Eurobot2013 by Oskar Weigl

tvmet/util/General.h@11:5ba926692210, 2013-04-09 (annotated)

Committer:
xiaxia686
Date:
Tue Apr 09 15:32:47 2013 +0000
Revision:
11:5ba926692210
Parent:
1:6799c07fe510 
woking version (calibrated)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sv 1:6799c07fe510 1 /*
sv 1:6799c07fe510 2 * Tiny Vector Matrix Library
sv 1:6799c07fe510 3 * Dense Vector Matrix Libary of Tiny size using Expression Templates
sv 1:6799c07fe510 4 *
sv 1:6799c07fe510 5 * Copyright (C) 2001 - 2007 Olaf Petzold <opetzold@users.sourceforge.net>
sv 1:6799c07fe510 6 *
sv 1:6799c07fe510 7 * This library is free software; you can redistribute it and/or
sv 1:6799c07fe510 8 * modify it under the terms of the GNU Lesser General Public
sv 1:6799c07fe510 9 * License as published by the Free Software Foundation; either
sv 1:6799c07fe510 10 * version 2.1 of the License, or (at your option) any later version.
sv 1:6799c07fe510 11 *
sv 1:6799c07fe510 12 * This library is distributed in the hope that it will be useful,
sv 1:6799c07fe510 13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
sv 1:6799c07fe510 14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
sv 1:6799c07fe510 15 * Lesser General Public License for more details.
sv 1:6799c07fe510 16 *
sv 1:6799c07fe510 17 * You should have received a copy of the GNU Lesser General Public
sv 1:6799c07fe510 18 * License along with this library; if not, write to the Free Software
sv 1:6799c07fe510 19 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
sv 1:6799c07fe510 20 *
sv 1:6799c07fe510 21 * $Id: General.h,v 1.13 2007-06-23 15:58:59 opetzold Exp $
sv 1:6799c07fe510 22 */
sv 1:6799c07fe510 23
sv 1:6799c07fe510 24 #ifndef TVMET_UTIL_GENERAL_H
sv 1:6799c07fe510 25 #define TVMET_UTIL_GENERAL_H
sv 1:6799c07fe510 26
sv 1:6799c07fe510 27
sv 1:6799c07fe510 28 /** forward */
sv 1:6799c07fe510 29 namespace tvmet {
sv 1:6799c07fe510 30 template<class T, std::size_t Rows, std::size_t Cols> class Matrix;
sv 1:6799c07fe510 31 template<class T, std::size_t Sz> class Vector;
sv 1:6799c07fe510 32 }
sv 1:6799c07fe510 33
sv 1:6799c07fe510 34 namespace tvmet {
sv 1:6799c07fe510 35
sv 1:6799c07fe510 36 namespace util {
sv 1:6799c07fe510 37
sv 1:6799c07fe510 38 /*
sv 1:6799c07fe510 39 * \defgroup _util_function
sv 1:6799c07fe510 40 * \brief Usefull utility functions
sv 1:6799c07fe510 41 */
sv 1:6799c07fe510 42
sv 1:6799c07fe510 43 /**
sv 1:6799c07fe510 44 * \fn Gemm(const Matrix<T, Rows, Cols>& m1, const Matrix<T, Rows, Cols>& m2, Matrix<T, Rows, Cols>& m3)
sv 1:6799c07fe510 45 * \brief General matrix matrix multiplication using loops.
sv 1:6799c07fe510 46 * \ingroup _util_function
sv 1:6799c07fe510 47 */
sv 1:6799c07fe510 48 template<class T, std::size_t Rows, std::size_t Cols>
sv 1:6799c07fe510 49 inline
sv 1:6799c07fe510 50 void
sv 1:6799c07fe510 51 Gemm(const Matrix<T, Rows, Cols>& m1, const Matrix<T, Rows, Cols>& m2,
sv 1:6799c07fe510 52 Matrix<T, Rows, Cols>& m3)
sv 1:6799c07fe510 53 {
sv 1:6799c07fe510 54 for (std::size_t i = 0; i < Rows; ++i) {
sv 1:6799c07fe510 55 for (std::size_t j = 0; j < Cols; ++j) {
sv 1:6799c07fe510 56 T sum(0);
sv 1:6799c07fe510 57 for (std::size_t k = 0; k < Cols; ++k) {
sv 1:6799c07fe510 58 sum += m1(i,k) * m2(k,j);
sv 1:6799c07fe510 59 }
sv 1:6799c07fe510 60 m3(i,j) = sum;
sv 1:6799c07fe510 61 }
sv 1:6799c07fe510 62 }
sv 1:6799c07fe510 63 }
sv 1:6799c07fe510 64
sv 1:6799c07fe510 65
sv 1:6799c07fe510 66 /**
sv 1:6799c07fe510 67 * \fn Gemv(const Matrix<T, Rows, Cols>& m, const Vector<T, Cols>& v, Vector<T, Cols>& v2)
sv 1:6799c07fe510 68 * \brief General matrix vector multiplication using loops.
sv 1:6799c07fe510 69 * \ingroup _util_function
sv 1:6799c07fe510 70 */
sv 1:6799c07fe510 71 template<class T, std::size_t Rows, std::size_t Cols>
sv 1:6799c07fe510 72 inline
sv 1:6799c07fe510 73 void
sv 1:6799c07fe510 74 Gemv(const Matrix<T, Rows, Cols>& m, const Vector<T, Cols>& v,
sv 1:6799c07fe510 75 Vector<T, Cols>& v2)
sv 1:6799c07fe510 76 {
sv 1:6799c07fe510 77 for (std::size_t i = 0; i < Rows; ++i){
sv 1:6799c07fe510 78 v2(i) = T(0); // clean up before use
sv 1:6799c07fe510 79 for (std::size_t j = 0; j < Cols; ++j) {
sv 1:6799c07fe510 80 v2(i) += m(i,j) * v(j);
sv 1:6799c07fe510 81 }
sv 1:6799c07fe510 82 }
sv 1:6799c07fe510 83 }
sv 1:6799c07fe510 84
sv 1:6799c07fe510 85
sv 1:6799c07fe510 86 /**
sv 1:6799c07fe510 87 * \fn Gevvmul(const Vector<T, Sz>& v1, const Vector<T, Sz>& v2, Vector<T, Sz>& v3)
sv 1:6799c07fe510 88 * \brief General vector vector elementwise multiplication using loop.
sv 1:6799c07fe510 89 * \ingroup _util_function
sv 1:6799c07fe510 90 */
sv 1:6799c07fe510 91 template<class T, std::size_t Sz>
sv 1:6799c07fe510 92 inline
sv 1:6799c07fe510 93 void
sv 1:6799c07fe510 94 Gevvmul(const Vector<T, Sz>& v1, const Vector<T, Sz>& v2,
sv 1:6799c07fe510 95 Vector<T, Sz>& v3)
sv 1:6799c07fe510 96 {
sv 1:6799c07fe510 97 for(std::size_t i = 0; i < Sz; ++i)
sv 1:6799c07fe510 98 v3(i) = v1(i) * v2(i);
sv 1:6799c07fe510 99 }
sv 1:6799c07fe510 100
sv 1:6799c07fe510 101
sv 1:6799c07fe510 102 /**
sv 1:6799c07fe510 103 * \fn Gevvadd(const Vector<T, Sz>& v1, const Vector<T, Sz>& v2, Vector<T, Sz>& v3)
sv 1:6799c07fe510 104 * \brief General vector vector elementwise multiplication using loop.
sv 1:6799c07fe510 105 * \ingroup _util_function
sv 1:6799c07fe510 106 */
sv 1:6799c07fe510 107 template<class T, std::size_t Sz>
sv 1:6799c07fe510 108 inline
sv 1:6799c07fe510 109 void
sv 1:6799c07fe510 110 Gevvadd(const Vector<T, Sz>& v1, const Vector<T, Sz>& v2,
sv 1:6799c07fe510 111 Vector<T, Sz>& v3)
sv 1:6799c07fe510 112 {
sv 1:6799c07fe510 113 for(std::size_t i = 0; i < Sz; ++i)
sv 1:6799c07fe510 114 v3(i) = v1(i) + v2(i);
sv 1:6799c07fe510 115 }
sv 1:6799c07fe510 116
sv 1:6799c07fe510 117 } // namespace util
sv 1:6799c07fe510 118
sv 1:6799c07fe510 119 } // namespace tvmet
sv 1:6799c07fe510 120
sv 1:6799c07fe510 121 #endif // TVMET_UTIL_GENERAL_H
sv 1:6799c07fe510 122
sv 1:6799c07fe510 123 // Local Variables:
sv 1:6799c07fe510 124 // mode:C++
sv 1:6799c07fe510 125 // tab-width:8
sv 1:6799c07fe510 126 // End: