384 lines
8.3 KiB
C++
384 lines
8.3 KiB
C++
// Copyright 2003 David Hilvert <dhilvert@auricle.dyndns.org>,
|
|
// <dhilvert@ugcs.caltech.edu>
|
|
|
|
/* This file is part of the Anti-Lamenessing Engine.
|
|
|
|
The Anti-Lamenessing Engine is free software; you can redistribute it
|
|
and/or modify it under the terms of the GNU General Public License as
|
|
published by the Free Software Foundation; either version 3 of the License,
|
|
or (at your option) any later version.
|
|
|
|
The Anti-Lamenessing Engine is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with the Anti-Lamenessing Engine; if not, write to the Free Software
|
|
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
|
*/
|
|
|
|
/*
|
|
* d3/et.h: Represent three-dimensional Euclidean transformations.
|
|
*/
|
|
|
|
#ifndef __et_h__
|
|
#define __et_h__
|
|
|
|
#include "point.h"
|
|
|
|
#ifndef M_PI
|
|
#define M_PI 3.14159265358979323846
|
|
#endif
|
|
|
|
/*
|
|
* Structure to describe a three-dimensional euclidean transformation.
|
|
*
|
|
* We consider points to be transformed as homogeneous coordinate vectors
|
|
* multiplied on the right of the transformation matrix. The transformations
|
|
* are assumed to be small, so xyz Euler angles are used to specify rotation.
|
|
* The order of transformations is (starting with a point representation in the
|
|
* original coordinate system):
|
|
*
|
|
* i) translation of the point
|
|
* ii) x-rotation about the origin
|
|
* iii) y-rotation about the origin
|
|
* iv) z-rotation about the origin
|
|
*
|
|
* For more information on Euler angles, see:
|
|
*
|
|
* http://mathworld.wolfram.com/EulerAngles.html
|
|
*
|
|
* For more information on rotation matrices, see:
|
|
*
|
|
* http://mathworld.wolfram.com/RotationMatrix.html
|
|
*
|
|
* XXX: inconsistently with the 2D class, this class uses radians to represent
|
|
* rotation values.
|
|
*
|
|
*/
|
|
|
|
struct et {
|
|
private:
|
|
ale_pos translation[3];
|
|
ale_pos rotation[3];
|
|
mutable ale_pos matrix[4][4];
|
|
mutable ale_pos matrix_inverse[4][4];
|
|
mutable int resultant_memo;
|
|
mutable int resultant_inverse_memo;
|
|
|
|
|
|
/*
|
|
* Create an identity matrix
|
|
*/
|
|
void mident(ale_pos m1[4][4]) const {
|
|
for (int i = 0; i < 4; i++)
|
|
for (int j = 0; j < 4; j++)
|
|
m1[i][j] = (i == j) ? 1 : 0;
|
|
}
|
|
|
|
/*
|
|
* Create a rotation matrix
|
|
*/
|
|
void mrot(ale_pos m1[4][4], ale_pos angle, int index) const {
|
|
mident(m1);
|
|
m1[(index+1)%3][(index+1)%3] = cos(angle);
|
|
m1[(index+2)%3][(index+2)%3] = cos(angle);
|
|
m1[(index+1)%3][(index+2)%3] = sin(angle);
|
|
m1[(index+2)%3][(index+1)%3] = -sin(angle);
|
|
}
|
|
|
|
/*
|
|
* Multiply matrices M1 and M2; return result M3.
|
|
*/
|
|
void mmult(ale_pos m3[4][4], ale_pos m1[4][4], ale_pos m2[4][4]) const {
|
|
int k;
|
|
|
|
for (int i = 0; i < 4; i++)
|
|
for (int j = 0; j < 4; j++)
|
|
for (m3[i][j] = 0, k = 0; k < 4; k++)
|
|
m3[i][j] += m1[i][k] * m2[k][j];
|
|
}
|
|
|
|
/*
|
|
* Calculate resultant matrix values.
|
|
*/
|
|
void resultant() const {
|
|
|
|
/*
|
|
* If we already know the answers, don't bother calculating
|
|
* them again.
|
|
*/
|
|
|
|
if (resultant_memo)
|
|
return;
|
|
|
|
/*
|
|
* Multiply matrices.
|
|
*/
|
|
|
|
ale_pos m1[4][4], m2[4][4], m3[4][4];
|
|
|
|
/*
|
|
* Translation
|
|
*/
|
|
|
|
mident(m1);
|
|
|
|
for (int i = 0; i < 3; i++)
|
|
m1[i][3] = translation[i];
|
|
|
|
/*
|
|
* Rotation about x
|
|
*/
|
|
|
|
mrot(m2, rotation[0], 0);
|
|
mmult(m3, m2, m1);
|
|
|
|
/*
|
|
* Rotation about y
|
|
*/
|
|
|
|
mrot(m1, rotation[1], 1);
|
|
mmult(m2, m1, m3);
|
|
|
|
/*
|
|
* Rotation about z
|
|
*/
|
|
|
|
mrot(m3, rotation[2], 2);
|
|
mmult(matrix, m3, m2);
|
|
|
|
resultant_memo = 1;
|
|
}
|
|
|
|
/*
|
|
* Calculate the inverse transform matrix values.
|
|
*/
|
|
void resultant_inverse () const {
|
|
|
|
/*
|
|
* If we already know the answers, don't bother calculating
|
|
* them again.
|
|
*/
|
|
|
|
if (resultant_inverse_memo)
|
|
return;
|
|
|
|
/*
|
|
* The inverse can be explicitly calculated from the rotation
|
|
* and translation parameters. We invert the individual
|
|
* matrices and reverse the order of multiplication.
|
|
*/
|
|
|
|
ale_pos m1[4][4], m2[4][4], m3[4][4];
|
|
|
|
/*
|
|
* Translation
|
|
*/
|
|
|
|
mident(m1);
|
|
|
|
for (int i = 0; i < 3; i++)
|
|
m1[i][3] = -translation[i];
|
|
|
|
/*
|
|
* Rotation about x
|
|
*/
|
|
|
|
mrot(m2, -rotation[0], 0);
|
|
mmult(m3, m1, m2);
|
|
|
|
/*
|
|
* Rotation about y
|
|
*/
|
|
|
|
mrot(m1, -rotation[1], 1);
|
|
mmult(m2, m3, m1);
|
|
|
|
/*
|
|
* Rotation about z
|
|
*/
|
|
|
|
mrot(m3, -rotation[2], 2);
|
|
mmult(matrix_inverse, m2, m3);
|
|
|
|
resultant_inverse_memo = 1;
|
|
}
|
|
|
|
public:
|
|
/*
|
|
* Transform point p.
|
|
*/
|
|
struct point transform(struct point p) const {
|
|
struct point result;
|
|
|
|
resultant();
|
|
|
|
for (int i = 0; i < 3; i++) {
|
|
for (int k = 0; k < 3; k++)
|
|
result[i] += matrix[i][k] * p[k];
|
|
result[i] += matrix[i][3];
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
/*
|
|
* operator() is the transformation operator.
|
|
*/
|
|
struct point operator()(struct point p) const {
|
|
return transform(p);
|
|
}
|
|
|
|
/*
|
|
* Map point p using the inverse of the transform.
|
|
*/
|
|
struct point inverse_transform(struct point p) const {
|
|
struct point result;
|
|
|
|
resultant_inverse();
|
|
|
|
for (int i = 0; i < 3; i++) {
|
|
for (int k = 0; k < 3; k++)
|
|
result[i] += matrix_inverse[i][k] * p[k];
|
|
result[i] += matrix_inverse[i][3];
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
/*
|
|
* Default constructor
|
|
*
|
|
* Returns the identity transformation.
|
|
*
|
|
* Note: identity() depends on this.
|
|
*/
|
|
et() {
|
|
resultant_memo = 0;
|
|
resultant_inverse_memo = 0;
|
|
|
|
translation[0] = 0;
|
|
translation[1] = 0;
|
|
translation[2] = 0;
|
|
|
|
rotation[0] = 0;
|
|
rotation[1] = 0;
|
|
rotation[2] = 0;
|
|
}
|
|
|
|
et(ale_pos x, ale_pos y, ale_pos z, ale_pos P, ale_pos Y, ale_pos R) {
|
|
resultant_memo = 0;
|
|
resultant_inverse_memo = 0;
|
|
|
|
translation[0] = x;
|
|
translation[1] = y;
|
|
translation[2] = z;
|
|
|
|
rotation[0] = P;
|
|
rotation[1] = Y;
|
|
rotation[2] = R;
|
|
}
|
|
|
|
/*
|
|
* Return identity transformation.
|
|
*/
|
|
static struct et identity() {
|
|
struct et r;
|
|
|
|
return r;
|
|
}
|
|
|
|
/*
|
|
* Set Euclidean parameters (x, y, z, P, Y, R).
|
|
*/
|
|
void set(double values[6]) {
|
|
for (int i = 0; i < 3; i++)
|
|
translation[i] = values[i];
|
|
for (int i = 0; i < 3; i++)
|
|
rotation[i] = values[i + 3];
|
|
}
|
|
|
|
/*
|
|
* Adjust translation in the indicated manner.
|
|
*/
|
|
void modify_translation(int i1, ale_pos diff) {
|
|
assert (i1 >= 0);
|
|
assert (i1 < 3);
|
|
|
|
resultant_memo = 0;
|
|
resultant_inverse_memo = 0;
|
|
|
|
translation[i1] += diff;
|
|
}
|
|
|
|
void set_translation(int i1, ale_pos new_value) {
|
|
assert (i1 >= 0);
|
|
assert (i1 < 3);
|
|
|
|
resultant_memo = 0;
|
|
resultant_inverse_memo = 0;
|
|
|
|
translation[i1] = new_value;
|
|
}
|
|
|
|
/*
|
|
* Adjust rotation in the indicated manner.
|
|
*/
|
|
void modify_rotation(int i1, ale_pos diff) {
|
|
assert (i1 >= 0);
|
|
assert (i1 < 3);
|
|
|
|
resultant_memo = 0;
|
|
resultant_inverse_memo = 0;
|
|
|
|
rotation[i1] += diff;
|
|
}
|
|
|
|
void set_rotation(int i1, ale_pos new_value) {
|
|
assert (i1 >= 0);
|
|
assert (i1 < 3);
|
|
|
|
resultant_memo = 0;
|
|
resultant_inverse_memo = 0;
|
|
|
|
rotation[i1] = new_value;
|
|
}
|
|
|
|
ale_pos get_rotation(int i1) {
|
|
assert (i1 >= 0);
|
|
assert (i1 < 3);
|
|
|
|
return rotation[i1];
|
|
}
|
|
|
|
ale_pos get_translation(int i1) {
|
|
assert (i1 >= 0);
|
|
assert (i1 < 3);
|
|
|
|
return translation[i1];
|
|
}
|
|
|
|
|
|
void debug_output() {
|
|
fprintf(stderr, "[et.do t=[%f %f %f] r=[%f %f %f]\n"
|
|
" m=[[%f %f %f %f] [%f %f %f %f] [%f %f %f %f] [%f %f %f %f]]\n"
|
|
" mi=[[%f %f %f %f] [%f %f %f %f] [%f %f %f %f] [%f %f %f %f]]\n"
|
|
" rm=%d rim=%d]\n",
|
|
(double) translation[0], (double) translation[1], (double) translation[2],
|
|
(double) rotation[0], (double) rotation[1], (double) rotation[2],
|
|
(double) matrix[0][0], (double) matrix[0][1], (double) matrix[0][2], (double) matrix[0][3],
|
|
(double) matrix[1][0], (double) matrix[1][1], (double) matrix[1][2], (double) matrix[1][3],
|
|
(double) matrix[2][0], (double) matrix[2][1], (double) matrix[2][2], (double) matrix[2][3],
|
|
(double) matrix[3][0], (double) matrix[3][1], (double) matrix[3][2], (double) matrix[3][3],
|
|
(double) matrix_inverse[0][0], (double) matrix_inverse[0][1], (double) matrix_inverse[0][2], (double) matrix_inverse[0][3],
|
|
(double) matrix_inverse[1][0], (double) matrix_inverse[1][1], (double) matrix_inverse[1][2], (double) matrix_inverse[1][3],
|
|
(double) matrix_inverse[2][0], (double) matrix_inverse[2][1], (double) matrix_inverse[2][2], (double) matrix_inverse[2][3],
|
|
(double) matrix_inverse[3][0], (double) matrix_inverse[3][1], (double) matrix_inverse[3][2], (double) matrix_inverse[3][3],
|
|
resultant_memo, resultant_inverse_memo);
|
|
}
|
|
};
|
|
|
|
#endif
|