src/Time/mymath.h

   1 /* -*- Mode: C++ -*- *****************************************************
   2  * mymath.h
   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 #ifndef _MY_MATH_H_
  38 #define _MY_MATH_H__
  39
  40 #include <simgear/compiler.h>
  41
  42 #include <math.h>
  43 #include STL_FSTREAM
  44 #include STL_IOMANIP
  45
  46 FG_USING_NAMESPACE(std);
  47
  48 #include <simgear/constants.h>
  49
  50 extern const  double PiOver180;
  51 extern const double Pix4dif3;
  52
  53
  54
  55 class Matrix;
  56
  57 class Vector
  58 {
  59     private:
  60         int nrData;
  61         double* data;
  62
  63         public:
  64         Vector();
  65         Vector(int size);
  66         Vector(int size, double* dta);
  67         Vector(double x, double y, double z);
  68         Vector(Vector& other);
  69         Vector(istream& is);
  70         ~Vector();
  71
  72         void SetVal(double x, double y, double z);
  73
  74         void build(int);
  75         Vector* GetVector();
  76         double GetX();
  77         double GetY();
  78         double GetZ();
  79         void AddX(double x);
  80         void AddY(double y);
  81         void AddZ(double z);
  82         void SubtractX(double x);
  83         void SubtractY(double y);
  84         void SubtractZ(double z);
  85         double VecLen();
  86         Vector& operator = (Vector&);
  87         double& operator[] (int);
  88         int getDim();
  89
  90         ostream& binSave(ostream& os);
  91
  92         Vector operator +=(Vector other);
  93         Vector operator -=(Vector other);
  94
  95         friend double VecDot(Vector first, Vector second);
  96         friend Vector VecCross(Vector first, Vector second);
  97         friend Vector operator-(Vector first, Vector second);
  98         friend Vector operator+(Vector first, Vector second);
  99         friend Vector operator*(Matrix mx, Vector vc);
 100         friend Vector operator*(Vector v1, Vector v2);
 101         friend Vector operator*(Vector vec, double d);
 102         friend Vector operator/(Vector vec, double d);
 103
 104         friend ostream& operator << (ostream& os, Vector& vec);
 105         friend istream& operator >> (istream& is, Vector& vec);
 106 };
 107
 108 /*-----------------------------------------------------------------------------
 109                                 nonmember friend functions
 110 ------------------------------------------------------------------------------*/
 111
 112 double VecDot(Vector first, Vector second);
 113 Vector VecCross(Vector first, Vector second);
 114 Vector operator-(Vector first, Vector second);
 115 Vector operator+(Vector first, Vector second);
 116 Vector operator*(Matrix mx, Vector vc);
 117 Vector operator*(Vector v1, Vector v2);
 118 Vector operator*(Vector vec, double d);
 119 Vector operator/(Vector vec, double d);
 120
 121 ostream& operator << (ostream& os, Vector& vec);
 122 istream& operator >> (istream& is, Vector& vec);
 123
 124
 125 /*------------------------------------------------------------------------------
 126                         inline member functions
 127 ------------------------------------------------------------------------------*/
 128
 129 inline Vector::Vector()
 130 {
 131         nrData = 0;
 132     data = 0;
 133 }
 134
 135 inline void Vector::build(int size)
 136 {
 137         nrData = size;
 138     data = new double[nrData];
 139     #ifdef DEBUG
 140     if (!data)
 141     {
 142                 ::MessageBox(0, "Error Allocating Memory for a new Vector", "Error",
 143                                 MB_OK);
 144         exit(1);
 145     }
 146     #endif
 147     //for (int i = 0; i < nrData; i++)
 148     //  data[i] = 0.00;
 149 }
 150
 151
 152 inline Vector::Vector(int size, double* dta)
 153 {
 154         build(size);
 155     memcpy(data, dta, nrData*sizeof(double));
 156 }
 157
 158
 159 inline Vector::Vector(Vector& other)
 160 {
 161         build(other.nrData);
 162     memcpy(data,other.data,nrData*sizeof(double));
 163 }
 164
 165 inline Vector::Vector(double x, double y, double z)
 166 {
 167     build(4);      // one extra for matrix multiplication...
 168         data[0] = x;
 169         data[1] = y;
 170         data[2] = z;
 171     data[3] = 0.00;
 172
 173 }
 174
 175 inline Vector::Vector(istream& is)
 176 {
 177         is.read((char*) &nrData, sizeof(int));
 178     data = new double[nrData];
 179     is.read((char*) data, nrData * sizeof(double));
 180 }
 181
 182 inline Vector::~Vector()
 183 {
 184         delete [] data;
 185 }
 186
 187
 188 inline void Vector::SetVal(double x, double y, double z)
 189 {
 190         #ifdef DEBUG
 191     if (nrData != 4)
 192     {
 193         ::MessageBox(0, "Attempt to assign data to a vector\n"
 194                         "With size unequal to 4 in function\n"
 195                         " Vector::Setval(double, double, double", "Error" , MB_OK);
 196         exit(1);
 197     }
 198     #endif
 199     data[0] = x,
 200     data[1] = y;
 201     data[2] = z;
 202     data[3] = 0.00;
 203 }
 204
 205 inline Vector* Vector::GetVector()
 206 {
 207         return this;
 208 }
 209
 210 inline double Vector::GetX()
 211 {
 212         #ifdef DEBUG
 213     if (nrData < 1)
 214     {
 215         ::MessageBox(0, "Attempt to retrieve x-value of a vector\n"
 216                         "With size smaller than 1 in function\n"
 217                         " Vector::GetX();", "Error", MB_OK);
 218         exit(1);
 219     }
 220     #endif
 221     return data[0];
 222 }
 223
 224 inline double Vector::GetY()
 225 {
 226         #ifdef DEBUG
 227     if (nrData < 2)
 228     {
 229         ::MessageBox(0, "Attempt to retrieve the y value of a vector\n"
 230                         "With size smaller than 2 in function\n"
 231                         " Vector::GetY();", "Error", MB_OK);
 232         exit(1);
 233     }
 234     #endif
 235     return data[1];
 236 }
 237
 238 inline double Vector::GetZ()
 239 {
 240         #ifdef DEBUG
 241     if (nrData < 3)
 242     {
 243                 ::MessageBox(0, "Attempt to retrieve the z value of a vector\n"
 244                         "With size smaller than 2 in function\n"
 245                         " Vector::GetZ();", "Error", MB_OK);
 246         exit(1);
 247     }
 248     #endif
 249     return data[2];
 250 }
 251
 252 inline void Vector::AddX(double x)
 253 {
 254         #ifdef DEBUG
 255     if (nrData < 1)
 256     {
 257         ::MessageBox(0, "Attempt to chance x-value to a vector\n"
 258                         "With size smaller than 1 in function\n"
 259                         " Vector::AddX(double);", "Error", MB_OK);
 260         exit(1);
 261     }
 262     #endif
 263     data[0] += x;
 264 }
 265
 266 inline void Vector::AddY(double y)
 267 {
 268         #ifdef DEBUG
 269     if (nrData < 2)
 270     {
 271         ::MessageBox(0, "Attempt to chance y-value to a vector\n"
 272                         "With size smaller than 2 in function\n"
 273                         " Vector::AddY(double);", "Error", MB_OK);
 274         exit(1);
 275     }
 276     #endif
 277     data[1] += y;
 278 }
 279
 280 inline void Vector::AddZ(double z)
 281 {
 282         #ifdef DEBUG
 283     if (nrData < 3)
 284     {
 285         ::MessageBox(0, "Attempt to chance z-value to a vector\n"
 286                         "With size smaller than 3 in function\n"
 287                         " Vector::AddZ(double);", "Error", MB_OK);
 288         exit(1);
 289     }
 290     #endif
 291     data[2] += z;
 292 }
 293
 294 inline void Vector::SubtractX(double x)
 295 {
 296         #ifdef DEBUG
 297     if (nrData < 1)
 298     {
 299         ::MessageBox(0, "Attempt to chance x-value to a vector\n"
 300                         "With size smaller than 1 in function\n"
 301                         " Vector::SubtractX(double);", "Error", MB_OK);
 302         exit(1);
 303     }
 304     #endif
 305     data[0] -= x;
 306 }
 307
 308 inline void Vector::SubtractY(double y)
 309 {
 310         #ifdef DEBUG
 311     if (nrData < 2)
 312     {
 313         ::MessageBox(0, "Attempt to chance y-value to a vector\n"
 314                         "With size smaller than 2 in function\n"
 315                         " Vector::SubractY(double);", "Error", MB_OK);
 316         exit(1);
 317     }
 318     #endif
 319     data[1] -= y;
 320 }
 321
 322 inline void Vector::SubtractZ(double z)
 323 {
 324         #ifdef DEBUG
 325     if (nrData < 3)
 326     {
 327         ::MessageBox(0, "Attempt to chance z-value to a vector\n"
 328                         "With size smaller than 3 in function\n"
 329                         " Vector::SubtractZ(double);", "Error", MB_OK);
 330         exit(1);
 331     }
 332     #endif
 333     data[2] -= z;
 334 }
 335
 336
 337 inline Vector& Vector::operator= (Vector& other)
 338 {
 339     if (data)
 340         delete[] data;
 341     build(other.nrData);
 342     memcpy(data, other.data, nrData*sizeof(double));
 343     return *this;
 344 }
 345
 346 inline double& Vector::operator [](int index)
 347 {
 348         return *(data+index);
 349 }
 350
 351
 352 inline int Vector::getDim()
 353 {
 354         return nrData;
 355 }
 356
 357 /*-----------------------------------------------------------------------------
 358                                                         Some generic conversion routines
 359 ------------------------------------------------------------------------------*/
 360
 361 float CosD(float angle);
 362 float SinD(float angle);
 363 float Radians(float angle);
 364 int Round(float value);
 365
 366 /* ----------------------------------------------------------------------------
 367                                 And their inlined implementation
 368 ------------------------------------------------------------------------------*/
 369
 370 inline float CosD(float angle)
 371 {
 372         return cos(Radians(angle));
 373 }
 374
 375 inline float SinD(float angle)
 376 {
 377         return(Radians(angle));
 378 }
 379
 380
 381 inline float Radians(float angle)
 382 {
 383         return (angle*PiOver180);
 384 }
 385
 386 inline int Round(float value)
 387 {
 388         return ( (int) (value+0.5));
 389 }
 390
 391
 392
 393 /******************************************************************************
 394
 395                                                         Matrix class
 396
 397 ******************************************************************************/
 398
 399 class Matrix
 400 {
 401     protected:
 402         int rows;
 403         int columns;
 404         Vector* data;
 405
 406     public:
 407
 408         Matrix();
 409         Matrix(int r, int c);
 410                 Matrix(int r, int c, double* dta);
 411         Matrix(int r, int c, double** dta);
 412         Matrix(int r, int c, Vector*dta);
 413         Matrix(Matrix&);
 414         ~Matrix();
 415
 416         void build(int r, int c);
 417         Matrix& operator=(Matrix&);
 418         Vector& operator[](int);
 419         Vector operator ()(int);
 420
 421                 int getrows();
 422         int getcols();
 423         void norm(int scal);
 424
 425         friend Vector operator*(Matrix mc, Vector vc);
 426 };
 427
 428 /*------------------------------------------------------------------------------
 429                                         inline Matrix routines
 430 ------------------------------------------------------------------------------*/
 431
 432 inline Matrix::Matrix()
 433 {
 434         rows = 0;
 435     columns = 0;
 436     data = 0;
 437 }
 438
 439 inline Matrix::Matrix(int r, int c)
 440 {
 441         build(r, c);
 442 }
 443
 444 inline Matrix::~Matrix()
 445 {
 446         delete [] data;
 447 }
 448
 449
 450 inline Vector& Matrix::operator[] (int row)
 451 {
 452         return data[row];
 453 }
 454
 455
 456 #endif // _MYMATH_H_
 457
 458