src/Time/mymath.cxx

   1 /* -*- Mode: C++ -*- *****************************************************
   2  * mymath.cc
   3  * Written by Durk Talsma, around 1995/1996.
   4  *
   5  * This program is free software; you can redistribute it and/or
   6  * modify it under the terms of the GNU General Public License as
   7  * published by the Free Software Foundation; either version 2 of the
   8  * License, or (at your option) any later version.
   9  *
  10  * This program is distributed in the hope that it will be useful, but
  11  * WITHOUT ANY WARRANTY; without even the implied warranty of
  12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  13  * General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this program; if not, write to the Free Software
  17  * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  18  *
  19  **************************************************************************/
  20
  21 /********************************************************************
  22  * This file defines a simple Vector and Matrix library. These were
  23  * originally written for my (yet) unpublished planetarium / solar
  24  * system simulator program. The are included here, because I have
  25  * experience the code to calculate angles between vectors. I'm sure
  26  * similar functions exist already somewhere else so I don't mind
  27  * whether this gets eventually replaced by something more suitable
  28  * The functions are based on a description in Tyler. A. (1994). C++
  29  * Real-time 3D graphics. Sigma press, Wilmslow, England.
  30  *
  31  * The original versions were written under windows, hence the occasional
  32  *::MessageBox() statements between conditional compile statements
  33  *
  34  ********************************************************************/
  35
  36
  37
  38 #include "mymath.h"
  39
  40 const double PiOver180 = 1.74532925199433E-002;
  41 const double Pix4dif3  = 4.1887902;
  42
  43 Vector::Vector(int size)
  44 {
  45         build(size);
  46     for (int i = 0; i < nrData; i++)
  47         data[i] = 0.00;
  48 }
  49
  50 double Vector::VecLen()
  51 {
  52
  53    double length = 0;
  54    for (int i= 0; i < nrData; i++)
  55                 length += pow(data[i],2);
  56    return sqrt(length);
  57 }
  58
  59
  60 double VecDot(Vector first, Vector second)
  61 {
  62         /*double
  63         result = ((first.xVal*second.xVal) +
  64                                   (first.yVal*second.yVal) +
  65                           (first.zVal*second.zVal));
  66     return result; */
  67
  68     double result = 0;
  69     for (int i = 0; i < first.nrData; i++)
  70         result += first.data[i] * second.data[i];
  71
  72     return result;
  73 }
  74
  75 Vector VecCross(Vector first, Vector second)
  76 {
  77
  78         /*return Vec3 ( (first.yVal*second.zVal - first.zVal*second.yVal),
  79                                   (first.zVal*second.xVal - first.zVal*second.xVal),
  80                       (first.xVal*second.yVal - first.yVal*second.xVal) );
  81                 */
  82         #ifdef DEBUG
  83         if ( (first.nrData != 4) || (first.nrData != second.nrData) )
  84         {
  85                 ::MessageBox(0, "Attempting to calculate Cross product with 2\n"
  86                          "unequally sized vectors in\n"
  87                          "Vector VecCross(Vector, Vector", "Error",
  88                          MB_OK);
  89             exit(1);
  90         }
  91         #endif
  92
  93         double x = first.data[1] * second.data[2] - first.data[2]*second.data[1];
  94         double y = first.data[2] * second.data[0] - first.data[0]*second.data[2];
  95         double z = first.data[0] * second.data[1] - first.data[1]*second.data[0];
  96         Vector result(x,y,z);
  97         return result;
  98 }
  99
 100
 101
 102
 103 Vector operator- (Vector first, Vector second)
 104 {
 105         /*return Vec3( first.xVal - second.xVal,
 106                          first.yVal - second.yVal,
 107                  first.zVal - second.zVal );
 108     */
 109             #ifdef DEBUG
 110         if ( first.nrData != second.nrData )
 111         {
 112                 ::MessageBox(0, "Attempting to subtract 2 \n"
 113                          "unequally sized vectors in\n"
 114                          "Vector operator-(Vector, Vector", "Error",
 115                          MB_OK);
 116             //exit(1);
 117             return Vector(0);
 118         }
 119         #endif
 120         double *temp = new double[first.nrData];
 121         for (int i = 0; i < first.nrData; i++)
 122                 temp[i] = first.data[i] - second.data[i];
 123         Vector result(first.nrData, temp);
 124         delete [] temp;
 125         return result;
 126 }
 127
 128
 129 Vector operator+ (Vector first, Vector second)
 130 {
 131         /*return Vec3( first.xVal + second.xVal,
 132                          first.yVal + second.yVal,
 133                  first.zVal + second.zVal );
 134     */
 135     #ifdef DEBUG
 136     if ( first.nrData != second.nrData )
 137         {
 138         ::MessageBox(0, "Attempting to add 2\n"
 139                          "unequally sized vectors in\n"
 140                          "Vector operator+(Vector, Vector", "Error",
 141                          MB_OK);
 142         exit(1);
 143     }
 144     #endif
 145     double *temp = new double[first.nrData];
 146     for (int i = 0; i < first.nrData; i++)
 147         temp[i] = first.data[i] + second.data[i];
 148     Vector result(first.nrData, temp);
 149     delete [] temp;
 150     return result;
 151 }
 152
 153 Vector Vector::operator +=(Vector other)
 154 {
 155         #ifdef DEBUG
 156     if ( first.nrData != second.nrData )
 157     {
 158         ::MessageBox(0, "Attempting to add 2\n"
 159                          "unequally sized vectors in\n"
 160                          "Vector operator+(Vector, Vector", "Error",
 161                          MB_OK);
 162         exit(1);
 163     }
 164     #endif
 165     for (int i = 0; i < nrData; i++)
 166         data[i] += other.data[i];
 167     return *this;
 168 }
 169
 170 Vector Vector::operator -=(Vector other)
 171 {
 172         #ifdef DEBUG
 173     if ( first.nrData != second.nrData )
 174     {
 175         ::MessageBox(0, "Attempting to add 2\n"
 176                          "unequally sized vectors in\n"
 177                          "Vector operator+(Vector, Vector", "Error",
 178                          MB_OK);
 179         exit(1);
 180     }
 181     #endif
 182     for (int i = 0; i < nrData; i++)
 183         data[i] -= other.data[i];
 184     return *this;
 185 }
 186
 187
 188
 189
 190
 191 Vector operator* (Matrix mx, Vector vc)
 192 {
 193         int sizes[3];
 194     sizes[0] = vc.nrData;
 195     sizes[1] = mx.rows;
 196     sizes[2] = mx.columns;
 197
 198     #ifdef DEBUG
 199     if ( (sizes[0] != sizes[1]) || (sizes[0] != sizes[2]) )
 200     {
 201         char buffer[50];
 202         sprintf(buffer, "Sizes don't match in function\n"
 203                                         "Vector operator*(Matrix, Vector)\n"
 204                         "sizes are: %d, %d, %d", sizes[0], sizes[1],sizes[2]);
 205         MessageBox(0, buffer, "Error", MB_OK | MB_ICONEXCLAMATION);
 206         exit(1);
 207     }
 208     #endif
 209     double* result = new double[sizes[0]];
 210     int col, row;
 211
 212     for (col = 0; col < sizes[0]; col++)
 213     {
 214         result[col] = 0;
 215         for (row = 0; row < sizes[0]; row++)
 216                 result[col] += vc[row] * mx[row][col];
 217     }
 218     Vector res(4, result);
 219
 220    /*
 221     #ifdef DEBUG
 222     char buffer[200];
 223     sprintf(buffer, "return value of vector * matrix multiplication is\n"
 224                                 "(%f, %f, %f, %f) ", result[0], result[1], result[2], result[3]);
 225     ::MessageBox(0, buffer, "Information", MB_OK);
 226     #endif
 227    */
 228         delete [] result;
 229     return res;
 230 }
 231
 232 Vector operator*(Vector v1, Vector v2)
 233 {
 234         int size1 = v1.nrData;
 235
 236     #ifdef DEBUG
 237
 238     int size2 = v2.nrData;
 239     if(size1 != size2)
 240     {
 241         ::MessageBox(0, "Vector sizes don't match in Vector operator*(Vector, Vector)",
 242                                 "Error", MB_OK);
 243         exit(1);
 244     }
 245     #endif
 246     double *tempRes = new double[size1];
 247     for (int i= 0; i < size1; i++)
 248         tempRes[i] = v1[i] * v2[i];
 249     Vector result(size1, tempRes);
 250     delete tempRes;
 251     return result;
 252 }
 253
 254
 255 Vector operator*(Vector vec, double d)
 256 {
 257         double* tempRes = new double[vec.nrData];
 258     for (int i = 0; i < vec.nrData; i++)
 259         tempRes[i] = vec[i] * d;
 260     Vector result(vec.nrData, tempRes);
 261     delete tempRes;
 262     return result;
 263 }
 264
 265 Vector operator/(Vector vec, double d)
 266 {
 267         double* tempRes = new double[vec.nrData];
 268     for (int i = 0; i < vec.nrData; i++)
 269         tempRes[i] = vec[i] / d;
 270     Vector result(vec.nrData, tempRes);
 271     delete tempRes;
 272     return result;
 273 }
 274
 275 ostream& operator << (ostream& os, Vector& vec)
 276 {
 277   os << /*setw(4) << */vec.nrData << '\t';
 278     for (int i = 0; i < vec.nrData; i++)
 279       os /*<< setw(25) */<< vec[i] << '\t';
 280     return os;
 281 }
 282
 283 istream& operator >> (istream& is, Vector& vec)
 284 {
 285         is >> vec.nrData;
 286     if (vec.data)
 287         delete [] vec.data;
 288     vec.data = new double[vec.nrData];
 289     for (int i = 0; i < vec.nrData; i++)
 290         is >> vec.data[i];
 291     return is;
 292 }
 293
 294 ostream& Vector::binSave(ostream& os)
 295 {
 296         os.write((char*) &nrData, sizeof(int));
 297     os.write((char*) data, nrData* sizeof(double));
 298         return os;
 299 }
 300
 301
 302
 303
 304
 305
 306 /******************************************************************************
 307                 Matrix manipulation routines
 308 ******************************************************************************/
 309
 310 Matrix::Matrix(int r, int c, double*dta)
 311 {
 312         build(r,c);
 313     for (int i = 0; i < rows; i++)
 314         for (int j = 0; j < columns; j++)
 315                 data[i][j] = (*dta++);
 316 }
 317
 318 Matrix::Matrix(int r, int c, double** dta)
 319 {
 320         build(r,c);
 321     for (int i = 0; i < rows; i++)
 322         for (int j = 0; j < columns; j++)
 323                         data[i][j] = dta[i][j];
 324 }
 325
 326 Matrix::Matrix(int r, int c, Vector* dta)
 327 {
 328         build(r,c);
 329     for (int i = 0; i < rows; i++)
 330         data[i] = dta[i];
 331 }
 332
 333 Matrix::Matrix(Matrix& other)
 334 {
 335         build (other.rows, other.columns);
 336     for (int i = 0; i< rows; i++)
 337         (*this)[i] = other[i];
 338 }
 339
 340 void Matrix::build(int row, int col)
 341 {
 342     rows = row;
 343     columns = col;
 344
 345         data = new Vector [rows];
 346     for (int i = 0; i < rows; i++)
 347         data[i].build(col);
 348 }
 349
 350 Matrix& Matrix::operator =(Matrix& other)
 351 {
 352         rows = other.rows;
 353     columns = other.columns;
 354     for (int i = 0; i < rows; i++)
 355         (*this)[i] = other[i];
 356     return *this;
 357 }
 358
 359
 360 Vector Matrix::operator ()(int col)
 361 {
 362         Vector Col(rows);
 363     for (int i = 0; i < rows; i++)
 364         Col[i] = data[i][col];
 365     return Col;
 366 }
 367
 368 void Matrix::norm(int scale)
 369 {
 370         for (int i = 0; i < rows; i++)
 371         for (int j = 0; j < columns; j++)
 372                 data[i][j] /= scale;
 373 }