d3/et.h

   1 // Copyright 2003 David Hilvert <dhilvert@auricle.dyndns.org>,
   2 //                              <dhilvert@ugcs.caltech.edu>
   3
   4 /*  This file is part of the Anti-Lamenessing Engine.
   5
   6     The Anti-Lamenessing Engine is free software; you can redistribute it
   7     and/or modify it under the terms of the GNU General Public License as
   8     published by the Free Software Foundation; either version 3 of the License,
   9     or (at your option) any later version.
  10
  11     The Anti-Lamenessing Engine is distributed in the hope that it will be useful,
  12     but WITHOUT ANY WARRANTY; without even the implied warranty of
  13     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14     GNU General Public License for more details.
  15
  16     You should have received a copy of the GNU General Public License
  17     along with the Anti-Lamenessing Engine; if not, write to the Free Software
  18     Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  19 */
  20
  21 /*
  22  * d3/et.h: Represent three-dimensional Euclidean transformations.
  23  */
  24
  25 #ifndef __et_h__
  26 #define __et_h__
  27
  28 #include "point.h"
  29
  30 #ifndef M_PI
  31 #define M_PI 3.14159265358979323846
  32 #endif
  33
  34 /*
  35  * Structure to describe a three-dimensional euclidean transformation.
  36  *
  37  * We consider points to be transformed as homogeneous coordinate vectors
  38  * multiplied on the right of the transformation matrix.  The transformations
  39  * are assumed to be small, so xyz Euler angles are used to specify rotation.
  40  * The order of transformations is (starting with a point representation in the
  41  * original coordinate system):
  42  *
  43  * i)   translation of the point
  44  * ii)  x-rotation about the origin
  45  * iii) y-rotation about the origin
  46  * iv)  z-rotation about the origin
  47  *
  48  * For more information on Euler angles, see:
  49  *
  50  * http://mathworld.wolfram.com/EulerAngles.html
  51  *
  52  * For more information on rotation matrices, see:
  53  *
  54  * http://mathworld.wolfram.com/RotationMatrix.html
  55  *
  56  * XXX: inconsistently with the 2D class, this class uses radians to represent
  57  * rotation values.
  58  *
  59  */
  60
  61 struct et {
  62 private:
  63         ale_pos translation[3];
  64         ale_pos rotation[3];
  65         mutable ale_pos matrix[4][4];
  66         mutable ale_pos matrix_inverse[4][4];
  67         mutable int resultant_memo;
  68         mutable int resultant_inverse_memo;
  69
  70
  71         /*
  72          * Create an identity matrix
  73          */
  74         void mident(ale_pos m1[4][4]) const {
  75                 for (int i = 0; i < 4; i++)
  76                 for (int j = 0; j < 4; j++)
  77                         m1[i][j] = (i == j) ? 1 : 0;
  78         }
  79
  80         /*
  81          * Create a rotation matrix
  82          */
  83         void mrot(ale_pos m1[4][4], ale_pos angle, int index) const {
  84                 mident(m1);
  85                 m1[(index+1)%3][(index+1)%3] = cos(angle);
  86                 m1[(index+2)%3][(index+2)%3] = cos(angle);
  87                 m1[(index+1)%3][(index+2)%3] = sin(angle);
  88                 m1[(index+2)%3][(index+1)%3] = -sin(angle);
  89         }
  90
  91         /*
  92          * Multiply matrices M1 and M2; return result M3.
  93          */
  94         void mmult(ale_pos m3[4][4], ale_pos m1[4][4], ale_pos m2[4][4]) const {
  95                 int k;
  96
  97                 for (int i = 0; i < 4; i++)
  98                 for (int j = 0; j < 4; j++)
  99                 for (m3[i][j] = 0, k = 0; k < 4; k++)
 100                         m3[i][j] += m1[i][k] * m2[k][j];
 101         }
 102
 103         /*
 104          * Calculate resultant matrix values.
 105          */
 106         void resultant() const {
 107
 108                 /*
 109                  * If we already know the answers, don't bother calculating
 110                  * them again.
 111                  */
 112
 113                 if (resultant_memo)
 114                         return;
 115
 116                 /*
 117                  * Multiply matrices.
 118                  */
 119
 120                 ale_pos m1[4][4], m2[4][4], m3[4][4];
 121
 122                 /*
 123                  * Translation
 124                  */
 125
 126                 mident(m1);
 127
 128                 for (int i = 0; i < 3; i++)
 129                         m1[i][3] = translation[i];
 130
 131                 /*
 132                  * Rotation about x
 133                  */
 134
 135                 mrot(m2, rotation[0], 0);
 136                 mmult(m3, m2, m1);
 137
 138                 /*
 139                  * Rotation about y
 140                  */
 141
 142                 mrot(m1, rotation[1], 1);
 143                 mmult(m2, m1, m3);
 144
 145                 /*
 146                  * Rotation about z
 147                  */
 148
 149                 mrot(m3, rotation[2], 2);
 150                 mmult(matrix, m3, m2);
 151
 152                 resultant_memo = 1;
 153         }
 154
 155         /*
 156          * Calculate the inverse transform matrix values.
 157          */
 158         void resultant_inverse () const {
 159
 160                 /*
 161                  * If we already know the answers, don't bother calculating
 162                  * them again.
 163                  */
 164
 165                 if (resultant_inverse_memo)
 166                         return;
 167
 168                 /*
 169                  * The inverse can be explicitly calculated from the rotation
 170                  * and translation parameters.  We invert the individual
 171                  * matrices and reverse the order of multiplication.
 172                  */
 173
 174                 ale_pos m1[4][4], m2[4][4], m3[4][4];
 175
 176                 /*
 177                  * Translation
 178                  */
 179
 180                 mident(m1);
 181
 182                 for (int i = 0; i < 3; i++)
 183                         m1[i][3] = -translation[i];
 184
 185                 /*
 186                  * Rotation about x
 187                  */
 188
 189                 mrot(m2, -rotation[0], 0);
 190                 mmult(m3, m1, m2);
 191
 192                 /*
 193                  * Rotation about y
 194                  */
 195
 196                 mrot(m1, -rotation[1], 1);
 197                 mmult(m2, m3, m1);
 198
 199                 /*
 200                  * Rotation about z
 201                  */
 202
 203                 mrot(m3, -rotation[2], 2);
 204                 mmult(matrix_inverse, m2, m3);
 205
 206                 resultant_inverse_memo = 1;
 207         }
 208
 209 public:
 210         /*
 211          * Transform point p.
 212          */
 213         struct point transform(struct point p) const {
 214                 struct point result;
 215
 216                 resultant();
 217
 218                 for (int i = 0; i < 3; i++) {
 219                         for (int k = 0; k < 3; k++)
 220                                 result[i] += matrix[i][k] * p[k];
 221                         result[i] += matrix[i][3];
 222                 }
 223
 224                 return result;
 225         }
 226
 227         /*
 228          * operator() is the transformation operator.
 229          */
 230         struct point operator()(struct point p) const {
 231                 return transform(p);
 232         }
 233
 234         /*
 235          * Map point p using the inverse of the transform.
 236          */
 237         struct point inverse_transform(struct point p) const {
 238                 struct point result;
 239
 240                 resultant_inverse();
 241
 242                 for (int i = 0; i < 3; i++) {
 243                         for (int k = 0; k < 3; k++)
 244                                 result[i] += matrix_inverse[i][k] * p[k];
 245                         result[i] += matrix_inverse[i][3];
 246                 }
 247
 248                 return result;
 249         }
 250
 251         /*
 252          * Default constructor
 253          *
 254          * Returns the identity transformation.
 255          *
 256          * Note: identity() depends on this.
 257          */
 258         et() {
 259                 resultant_memo = 0;
 260                 resultant_inverse_memo = 0;
 261
 262                 translation[0] = 0;
 263                 translation[1] = 0;
 264                 translation[2] = 0;
 265
 266                 rotation[0] = 0;
 267                 rotation[1] = 0;
 268                 rotation[2] = 0;
 269         }
 270
 271         et(ale_pos x, ale_pos y, ale_pos z, ale_pos P, ale_pos Y, ale_pos R) {
 272                 resultant_memo = 0;
 273                 resultant_inverse_memo = 0;
 274
 275                 translation[0] = x;
 276                 translation[1] = y;
 277                 translation[2] = z;
 278
 279                 rotation[0] = P;
 280                 rotation[1] = Y;
 281                 rotation[2] = R;
 282         }
 283
 284         /*
 285          * Return identity transformation.
 286          */
 287         static struct et identity() {
 288                 struct et r;
 289
 290                 return r;
 291         }
 292
 293         /*
 294          * Set Euclidean parameters (x, y, z, P, Y, R).
 295          */
 296         void set(double values[6]) {
 297                 for (int i = 0; i < 3; i++)
 298                         translation[i] = values[i];
 299                 for (int i = 0; i < 3; i++)
 300                         rotation[i] = values[i + 3];
 301         }
 302
 303         /*
 304          * Adjust translation in the indicated manner.
 305          */
 306         void modify_translation(int i1, ale_pos diff) {
 307                 assert (i1 >= 0);
 308                 assert (i1 < 3);
 309
 310                 resultant_memo = 0;
 311                 resultant_inverse_memo = 0;
 312
 313                 translation[i1] += diff;
 314         }
 315
 316         void set_translation(int i1, ale_pos new_value) {
 317                 assert (i1 >= 0);
 318                 assert (i1 < 3);
 319
 320                 resultant_memo = 0;
 321                 resultant_inverse_memo = 0;
 322
 323                 translation[i1] = new_value;
 324         }
 325
 326         /*
 327          * Adjust rotation in the indicated manner.
 328          */
 329         void modify_rotation(int i1, ale_pos diff) {
 330                 assert (i1 >= 0);
 331                 assert (i1 < 3);
 332
 333                 resultant_memo = 0;
 334                 resultant_inverse_memo = 0;
 335
 336                 rotation[i1] += diff;
 337         }
 338
 339         void set_rotation(int i1, ale_pos new_value) {
 340                 assert (i1 >= 0);
 341                 assert (i1 < 3);
 342
 343                 resultant_memo = 0;
 344                 resultant_inverse_memo = 0;
 345
 346                 rotation[i1] = new_value;
 347         }
 348
 349         ale_pos get_rotation(int i1) {
 350                 assert (i1 >= 0);
 351                 assert (i1 < 3);
 352
 353                 return rotation[i1];
 354         }
 355
 356         ale_pos get_translation(int i1) {
 357                 assert (i1 >= 0);
 358                 assert (i1 < 3);
 359
 360                 return translation[i1];
 361         }
 362
 363
 364         void debug_output() {
 365                 fprintf(stderr, "[et.do t=[%f %f %f] r=[%f %f %f]\n"
 366                                 "       m=[[%f %f %f %f] [%f %f %f %f] [%f %f %f %f] [%f %f %f %f]]\n"
 367                                 "       mi=[[%f %f %f %f] [%f %f %f %f] [%f %f %f %f] [%f %f %f %f]]\n"
 368                                 "       rm=%d rim=%d]\n",
 369                         (double) translation[0], (double) translation[1], (double) translation[2],
 370                         (double) rotation[0], (double) rotation[1], (double) rotation[2],
 371                         (double) matrix[0][0], (double) matrix[0][1], (double) matrix[0][2], (double) matrix[0][3],
 372                         (double) matrix[1][0], (double) matrix[1][1], (double) matrix[1][2], (double) matrix[1][3],
 373                         (double) matrix[2][0], (double) matrix[2][1], (double) matrix[2][2], (double) matrix[2][3],
 374                         (double) matrix[3][0], (double) matrix[3][1], (double) matrix[3][2], (double) matrix[3][3],
 375                         (double) matrix_inverse[0][0], (double) matrix_inverse[0][1], (double) matrix_inverse[0][2], (double) matrix_inverse[0][3],
 376                         (double) matrix_inverse[1][0], (double) matrix_inverse[1][1], (double) matrix_inverse[1][2], (double) matrix_inverse[1][3],
 377                         (double) matrix_inverse[2][0], (double) matrix_inverse[2][1], (double) matrix_inverse[2][2], (double) matrix_inverse[2][3],
 378                         (double) matrix_inverse[3][0], (double) matrix_inverse[3][1], (double) matrix_inverse[3][2], (double) matrix_inverse[3][3],
 379                         resultant_memo, resultant_inverse_memo);
 380         }
 381 };
 382
 383 #endif