00001 #ifndef __MATH_VECBASE__
00002 #define __MATH_VECBASE__
00003
00004 #include <cstdio>
00005 #include <cstring>
00006 #include <cmath>
00007
00008 #include <math/print.hpp>
00009 #include <math/discrete/discrete.hpp>
00010
00011 namespace math {
00012 template <typename T, int N> class vecbase {
00013 public:
00014 typedef T Type;
00015 public:
00019 vecbase() {}
00020 vecbase(vecbase<T,N> const & rhs) {
00021 for(int i = 0; i < N; ++i) {
00022 v[i] = rhs.v[i];
00023 }
00024 }
00025 vecbase(T const * const rhs) {
00026 memcpy(v, rhs, N * sizeof(T));
00027 }
00029 void loadZero() {
00030 for(int i = 0; i < N; ++i) {
00031 v[i] = 0;
00032 }
00033 }
00034 T magnitude() const {
00035 T a;
00036 for(int i = 0; i < N; ++i) {
00037 a += v[i]*v[i];
00038 }
00039 return sqrt(a);
00040 }
00041 void Normalize() {
00042
00043 T length = magnitude();
00044
00045 if(length==1.0f) return;
00046
00047 if(length==0.0f) {
00048 printf("normalize zero vector\n");
00049 throw;
00050 }
00051
00052 T scalefactor = 1.0f/length;
00053
00054 (*this) *= scalefactor;
00055 }
00056 bool IsFinite() const {
00057 for(int i = 0; i < N; ++i) {
00058 if(isinf(v[i])) return false;
00059 }
00060 return true;
00061 }
00062 bool IsNan() const {
00063 if(isnan(v[0])) return true;
00064 if(isnan(v[1])) return true;
00065 if(isnan(v[2])) return true;
00066 return false;
00067 }
00068 void write(FILE* file) const {
00069 fwrite(v, sizeof(T), N, file);
00070 }
00071 void read(FILE* file) {
00072 fread(v, sizeof(T), N, file);
00073 }
00074 T& operator[](int i) { return v[i]; }
00075
00079 bool operator==(const vecbase<T,N> & rhs) const {
00080 for(int i = 0; i < N; ++i) {
00081 if(v[i]!=rhs.v[i]) return false;
00082 }
00083 return true;
00084 }
00085 bool operator!=(const vecbase<T,N> & rhs) const {
00086 return !((*this)==rhs);
00087 }
00094 T dot(vecbase<T,N> const & rhs) const {
00095 T res = 0;
00096 for(int i = 0; i < N; ++i) {
00097 res += v[i] * rhs.v[i];
00098 }
00099 return res;
00100 }
00106 vecbase<T,N>& operator+=(const vecbase<T,N> & rhs) {
00107 for(int i = 0; i < N; ++i) {
00108 v[i] += rhs.v[i];
00109 }
00110 return *this;
00111 }
00112 vecbase<T,N>& operator-=(const vecbase<T,N> & rhs) {
00113 for(int i = 0; i < N; ++i) {
00114 v[i] -= rhs.v[i];
00115 }
00116 return *this;
00117 }
00118 vecbase<T,N> operator*=(const T rhs) {
00119 for(int i = 0; i < N; ++i) {
00120 v[i] *= rhs;
00121 }
00122 return *this;
00123
00124 }
00125 vecbase<T,N> operator/=(const T rhs) {
00126 for(int i = 0; i < N; ++i) {
00127 v[i] /= rhs;
00128 }
00129 return *this;
00130 }
00131 void Add(const vecbase<T,N> & v2, vecbase<T,N> & result) {
00132 for(int i = 0; i < N; ++i) {
00133 result.v[i]=v[i]+v2.v[i];
00134 }
00135 }
00136 void Subtract(const vecbase<T,N> & v2, vecbase<T,N> & result) {
00137 for(int i = 0; i < N; ++i) {
00138 result.v[i]=v[i]-v2.v[i];
00139 }
00140 }
00146 vecbase<T,N>& minus() {
00147 for(int i = 0; i < N; ++i) {
00148 v[i] = -v[i];
00149 }
00150 return *this;
00151 }
00152 vecbase<T,N>& abs() {
00153 for(int i = 0; i < N; ++i) {
00154 v[i] = fabs(v[i]);
00155 }
00156 return *this;
00157 }
00160 void print() const {
00161 for(int i = 0; i < N; i++) {
00162 ::math::print(v[i]);
00163 }
00164 }
00165 protected:
00166 bool operator<(vecbase<T,N> const & rhs) {
00167 for(int i = 0; i < N; i++) {
00168 if(v[i] >= rhs.v[i]) return false;
00169 }
00170 return true;
00171 }
00172 public:
00173 T v[N];
00174 };
00175
00176 template <typename T, int R, int C> class matbase: public vecbase<T,R*C> {
00177 public:
00178 matbase(math::matbase<T,R,C> const & rhs): math::matbase<T,R,C>(rhs) {
00179 memcpy(v, rhs.v, R*C*sizeof(T));
00180 }
00181 matbase(const T * rhs) {
00182 memcpy(v, rhs, R*C*sizeof(T));
00183 }
00184
00185 T& v(int i, int j) {
00186 return v[i*C + j];
00187 }
00188
00189
00190
00191 math::vecbase<T,C> getRow(int r) const {
00192 math::vecbase<T,C> ret;
00193 for(int c = 0; c < C; c++) {
00194 ret[c] = v(r,c);
00195 }
00196 return ret;
00197 }
00198 math::vecbase<T,R> getColumn(int c) const {
00199 math::vecbase<T,R> ret;
00200 for(int r = 0; r < R; r++) {
00201 ret[r] = v(r,c);
00202 }
00203 return ret;
00204 }
00205
00206
00207
00208 math::matbase<T,R,C> operator+(math::matbase<T,R,C> const & rhs) const {
00209 math::matbase<T,R,C> m;
00210 for(int i = 0; i < 9; ++i) m.v[i] = v[i] + rhs.v[i];
00211 return m;
00212 }
00213 math::matbase<T,R,C> operator-(math::matbase<T,R,C> const & rhs) const {
00214 math::matbase<T,R,C> m;
00215 for(int i = 0; i < 9; ++i) m.v[i] = v[i] - rhs.v[i];
00216 return m;
00217 }
00218
00219 math::matbase<T,R,R> operator*(math::matbase<T,C,R> const & rhs) const {
00220 math::matbase<T,R,C> m;
00221
00222 for(int r = 0; r < R; r++) {
00223 for(int c = 0; c < C; c++) {
00224 for(int c1 = 0; c1 < C; c1++) {
00225 m.v(r,c) = v(r,c1) * rhs.v(c1,r);
00226 }
00227
00228 }
00229 }
00230
00231 }
00232 bool operator==(math::matbase<T,R,C> const & rhs) const {
00233 for(int i=0; i<16; i++)
00234 {
00235 if(v[i]!=rhs.v[i])
00236 return false;
00237 }
00238 return true;
00239 }
00240 bool operator!=(const math::matbase<T,R,C> & rhs) const {
00241 return !((*this)==rhs);
00242 }
00243 void operator+=(const math::matbase<T,R,C> & rhs) {
00244 (*this)=(*this)+rhs;
00245 }
00246 void operator-=(const math::matbase<T,R,C> & rhs)
00247 {
00248 for(int i = 0; i < 9; ++i) v[i] *= rhs.v[i];
00249 }
00250 void operator*=(const math::matbase<T,R,C> & rhs) {
00251 (*this) = (*this) * rhs;
00252 }
00253 void operator*=(const T rhs) {
00254 for(int i = 0; i < 9; ++i) v[i] *= rhs;
00255 }
00256 void operator/=(const T rhs)
00257 {
00258 (*this)=(*this)/rhs;
00259 }
00260 math::matbase<T,R,C> operator-() const {
00261 math::matbase<T,R,C> result(*this);
00262
00263 for(int i=0; i<16; i++)
00264 result.v[i]=-result.v[i];
00265
00266 return result;
00267 }
00268 math::vecbase<T,R> operator*(const vecbase<T,C> rhs) const {
00269 math::vecbase<T,R> ret;
00270
00271 for(int r = 0; r < R; r++) {
00272 ret[r] = 0;
00273 for(int c = 0; c < C; c++) {
00274 ret[r] += v(r,c) * rhs[c];
00275 }
00276 }
00277 return ret;
00278 }
00279 void print() {
00280 for(int r = 0; r < R; ++r)
00281 {
00282 for(int c = 0; c < C; ++c)
00283 {
00284 printf("%f ",v(r,c));
00285 }
00286 printf("\n");
00287 }
00288 }
00289
00290 };
00291 template <typename T, int N> class matsqu: public matbase<T,N,N> {
00292 public:
00293 matsqu() {
00294 loadIdentity();
00295 }
00296 matsqu(math::matsqu<T,N> const & rhs): math::matbase<T,N,N>(rhs) {
00297
00298 }
00299
00300 void loadIdentity() {
00301 vecbase<T,N*N>::loadZero();
00302 for(int i = 0; i < N; i++) {
00303 matbase<T,N,N>::v(i,i) = 1.0;
00304 }
00305 }
00306
00308 T det() {
00309 int** perm;
00310 int* sig;
00311 math::discrete::permutations(perm, sig, N);
00312 int p = math::discrete::p(N,N);
00313
00314 T res = 0;
00315 for(int i = 0; i < p; ++i) {
00316 T prod = 1;
00317 for (int j = 0; j < N; ++j) {
00318 prod *= matbase<T,N,N>::v(j,perm[i][j]);
00319 }
00320 res += sig[i] * prod;
00321 }
00322 }
00324 T det(int* a, int* b, int n) {
00325 int** perm;
00326 int* sig;
00327 math::discrete::permutations(perm, sig, N);
00328 int p = math::discrete::p(N,N);
00329
00330 T res = 0;
00331 for(int i = 0; i < p; ++i) {
00332 T prod = 1;
00333 for (int j = 0; j < N; ++j) {
00334 prod *= matbase<T,N,N>::v(a[j],b[perm[i][j]]);
00335 }
00336 res += sig[i] * prod;
00337 }
00338
00339 }
00340 private:
00341
00342 };
00343
00344 }
00345
00346
00347 #endif
00348
