00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #ifndef TOON_INCLUDE_HELPERS_H
00033 #define TOON_INCLUDE_HELPERS_H
00034
00035 #include <TooN/TooN.h>
00036 #include <cmath>
00037
00038 #ifndef M_PI
00039 #define M_PI 3.14159265358979323846
00040 #define M_SQRT1_2 0.70710678118654752440
00041 #endif
00042
00043 namespace TooN {
00044
00045
00046 template<int Size, class Precision, class Base> void Fill(Vector<Size, Precision, Base>& v, const Precision& p)
00047 {
00048 for(int i=0; i < v.size(); i++)
00049 v[i]= p;
00050 }
00051
00052 template<int Rows, int Cols, class Precision, class Base> void Fill(Matrix<Rows, Cols, Precision, Base>& m, const Precision& p)
00053 {
00054 for(int i=0; i < m.num_rows(); i++)
00055 for(int j=0; j < m.num_cols(); j++)
00056 m[i][j] = p;
00057 }
00058
00059 template<int Size, class Precision, class Base> inline Precision norm(const Vector<Size, Precision, Base>& v)
00060 {
00061 using std::sqrt;
00062 return sqrt(v*v);
00063 }
00064
00065 template<int Size, class Precision, class Base> inline Precision norm_sq(const Vector<Size, Precision, Base>& v)
00066 {
00067 return v*v;
00068 }
00069
00070 template<int Size, class Precision, class Base> inline Vector<Size, Precision> unit(const Vector<Size, Precision, Base> & v)
00071 {
00072 using std::sqrt;
00073 return v * (1/sqrt(v*v));
00074 }
00075
00076
00077
00078
00082 template<int Size, class Precision, class Base> inline void normalize(Vector<Size, Precision, Base> v)
00083 {
00084 using std::sqrt;
00085 v /= sqrt(v*v);
00086 }
00087
00088
00090 template<int Size, class Precision> inline void normalize(Vector<Size, Precision> & v)
00091 {
00092 normalize(v.as_slice());
00093 }
00094
00095 template<int Size, typename Precision, typename Base> inline Vector<Size-1, Precision> project( const Vector<Size, Precision, Base> & v){
00096 return v.template slice<0,Size-1>() / v[Size-1];
00097 }
00098
00099 template<typename Precision, typename Base> inline Vector<-1, Precision> project( const Vector<-1, Precision, Base> & v){
00100 return v.slice(0,v.size()-1) / v[v.size()-1];
00101 }
00102
00103 template<int Size, typename Precision, typename Base> inline Vector<Size+1, Precision> unproject( const Vector<Size, Precision, Base> & v){
00104 Vector<Size+1, Precision> result;
00105 result.template slice<0,Size>() = v;
00106 result[Size] = 1;
00107 return result;
00108 }
00109
00110 template<typename Precision, typename Base> inline Vector<-1, Precision> unproject( const Vector<-1, Precision, Base> & v){
00111 Vector<-1, Precision> result(v.size()+1);
00112 result.slice(0,v.size()) = v;
00113 result[v.size()] = 1;
00114 return result;
00115 }
00116
00118 template <int R, int C, typename P, typename B>
00119 P inline norm_fro( const Matrix<R,C,P,B> & m ){
00120 using std::sqrt;
00121 P n = 0;
00122 for(int r = 0; r < m.num_rows(); ++r)
00123 for(int c = 0; c < m.num_cols(); ++c)
00124 n += m[r][c] * m[r][c];
00125
00126 return sqrt(n);
00127 }
00128
00131 template <int R, int C, typename P, typename B>
00132 P inline norm_inf( const Matrix<R,C,P,B> & m ){
00133 using std::abs;
00134 using std::max;
00135 P n = 0;
00136 for(int r = 0; r < m.num_rows(); ++r){
00137 P s = 0;
00138 for(int c = 0; c < m.num_cols(); ++c)
00139 s += abs(m(r,c));
00140 n = max(n,s);
00141 }
00142 return n;
00143 }
00144
00147 template <int R, int C, typename P, typename B>
00148 P inline norm_1( const Matrix<R,C,P,B> & m ){
00149 using std::abs;
00150 using std::max;
00151 P n = 0;
00152 for(int c = 0; c < m.num_cols(); ++c){
00153 P s = 0;
00154 for(int r = 0; r < m.num_rows(); ++r)
00155 s += abs(m(r,c));
00156 n = max(n,s);
00157 }
00158 return n;
00159 }
00160
00161 namespace Internal {
00162 template <int R, int C, typename P, typename B>
00163 inline Matrix<R, C, P> exp_taylor( const Matrix<R,C,P,B> & m ){
00164 SizeMismatch<R, C>::test(m.num_rows(), m.num_cols());
00165 Matrix<R,C,P> result = TooN::Zeros(m.num_rows(), m.num_cols());
00166 Matrix<R,C,P> f = TooN::Identity(m.num_rows());
00167 P k = 1;
00168 while(norm_inf((result+f)-result) > 0){
00169 result += f;
00170 f = (m * f) / k;
00171 k += 1;
00172 }
00173 return result;
00174 }
00175 };
00176
00182 template <int R, int C, typename P, typename B>
00183 inline Matrix<R, C, P> exp( const Matrix<R,C,P,B> & m ){
00184 using std::max;
00185 SizeMismatch<R, C>::test(m.num_rows(), m.num_cols());
00186 const P l = log2(norm_inf(m));
00187 const int s = max(0,(int)ceil(l));
00188 Matrix<R,C,P> result = Internal::exp_taylor(m/(1<<s));
00189 for(int i = 0; i < s; ++i)
00190 result = result * result;
00191 return result;
00192 }
00193
00195 template<int S, class P, class B> bool isfinite(const Vector<S, P, B>& v)
00196 {
00197 using std::isfinite;
00198 for(int i=0; i < v.size(); i++)
00199 if(!isfinite(v[i]))
00200 return 0;
00201 return 1;
00202 }
00203
00205 template<int S, class P, class B> bool isnan(const Vector<S, P, B>& v)
00206 {
00207 using std::isnan;
00208 for(int i=0; i < v.size(); i++)
00209 if(isnan(v[i]))
00210 return 1;
00211 return 0;
00212 }
00213
00215 template<int Rows, int Cols, typename Precision, typename Base>
00216 void Symmetrize(Matrix<Rows,Cols,Precision,Base>& m){
00217 SizeMismatch<Rows,Cols>::test(m.num_rows(), m.num_cols());
00218 for(int r=0; r<m.num_rows()-1; r++){
00219 for(int c=r+1; c<m.num_cols(); c++){
00220 const Precision temp=(m(r,c)+m(c,r))/2;
00221 m(r,c)=temp;
00222 m(c,r)=temp;
00223 }
00224 }
00225 }
00226
00227
00229 template<int Rows, int Cols, typename Precision, typename Base>
00230 Precision trace(const Matrix<Rows, Cols, Precision, Base> & m ){
00231 SizeMismatch<Rows,Cols>::test(m.num_rows(), m.num_cols());
00232 Precision tr = 0;
00233 for(int i = 0; i < m.num_rows(); ++i)
00234 tr += m(i,i);
00235 return tr;
00236 }
00237
00238 }
00239 #endif