1 /* -*- c++ -*-
2 FILE: Math4d.cpp
3 RCS REVISION: $Revision: 1.10 $
4
5 COPYRIGHT: (c) 1999 -- 2003 Melinda Green, Don Hatch, and Jay Berkenbilt - Superliminal Software
6
7 LICENSE: Free to use and modify for non-commercial purposes as long as the
8 following conditions are adhered to:
9 1) Obvious credit for the source of this code and the designs it embodies
10 are clearly made, and
11 2) Ports and derived versions of 4D Magic Cube programs are not distributed
12 without the express written permission of the authors.
13
14 DESCRIPTION:
15 Math4d implementation
16 */
17
18 #include "Math4d.h"
19
20 void
get3dRotMatrixAboutAxis(real axis[3],real angle,real mat[3][3])21 Math4d::get3dRotMatrixAboutAxis(real axis[3], real angle, real mat[3][3])
22 {
23 int i;
24 real norm, temp[3][3], mat2[2][2];
25
26 IDENTMAT3(mat);
27 SET3(mat[2], axis);
28 if (ABS(axis[0]) < ABS(axis[1]))
29 {
30 /* it's closer to the y axis, so cross with x axis first */
31 VXV3(mat[1], mat[2], mat[0]);
32 VXV3(mat[0], mat[1], mat[2]);
33 }
34 else
35 {
36 VXV3(mat[0], mat[1], mat[2]);
37 VXV3(mat[1], mat[2], mat[0]);
38 }
39 for (i = 0; i < 3; ++i)
40 {
41 norm = sqrt(NORMSQRD3(mat[i]));
42 VDS3(mat[i], mat[i], norm);
43 }
44 /*
45 * mat is now an orthogonal matrix that takes the z-axis to
46 * the rotation axis.
47 */
48 TRANSPOSE3(temp, mat);
49 /*
50 * temp is the matrix that takes the rotation axis to the z axis.
51 * (inverse of an orthogonal matrix is its transpose).
52 */
53
54 mat2[0][0] = cos(angle);
55 mat2[0][1] = sin(angle);
56 XV2(mat2[1], mat2[0]);
57
58 M2XM3r(mat, mat2, mat);
59 MXM3r(mat, temp, mat);
60
61 /* result is (mat taking rot axis to z axis) * (2d rot mat) * (mat taking
62 z axis back to rot axis */
63 }
64
65 void
get4dRotMatrix(real center[4],real axispoint[4],real angle,real mat[4][4])66 Math4d::get4dRotMatrix(real center[4], real axispoint[4], real angle,
67 real mat[4][4])
68 {
69 int ax, sgn, M[4][4], MINV[4][4];
70 real _3d_center[4], _3d_axispoint[4], mat3[3][3], temp[4][4];
71
72 assert(!!center[0] + !!center[1] + !!center[2] + !!center[3] == 1);
73 for (ax = 0; ax < 4; ++ax)
74 if (center[ax])
75 break;
76 assert(INRANGE(0 <=, ax, <4));
77 sgn = SGN(center[ax]);
78 Math4d::getCanonicalMatThatTakesAxisToMinusW(ax, sgn, M);
79 TRANSPOSE4i(MINV, M);
80
81 VXM4(_3d_center, center, M);
82 assert(ISZEROVEC3(_3d_center) && _3d_center[3] == -3); /* it's a grip */
83 VXM4(_3d_axispoint, axispoint, M);
84 get3dRotMatrixAboutAxis(_3d_axispoint, angle, mat3);
85 M4XM3(temp, M, mat3);
86 MXM4(mat, temp, MINV);
87 }
88
89 void
get4dTwistMat(int coords[4],real total_angle,int imat[4][4])90 Math4d::get4dTwistMat(int coords[4], real total_angle, int imat[4][4])
91 {
92 int i;
93 real center[4], real_stickercoords[4], rmat[4][4];
94 /*
95 * Calculate rotation center; it's the center of the face containing
96 * the grip
97 */
98 for (i = 0; i < 4; ++i)
99 {
100 if (ABS(coords[i]) == 3)
101 center[i] = coords[i];
102 else
103 center[i] = 0;
104 }
105
106 // total_angle = polymgr_get_twist_total_angle(grip, dir);
107 SET4(real_stickercoords, coords);
108
109 get4dRotMatrix(center, real_stickercoords, total_angle, rmat);
110 ROUNDMAT4(imat, rmat);
111 }
112
113 // following routines originally from end of polymgr.c -- DG
114
115 void
getCanonicalMatThatTakesAxisToMinusW(int ax,int sgn,int mat[4][4])116 Math4d::getCanonicalMatThatTakesAxisToMinusW(int ax, int sgn, int mat[4][4])
117 {
118 IDENTMAT4(mat);
119 if (ax != W)
120 {
121 mat[W][W] = 0;
122 mat[ax][ax] = 0;
123 mat[W][ax] = sgn;
124 mat[ax][W] = -sgn;
125 }
126 else if (sgn > 0)
127 {
128 /* must rotate the W axis into the -W axis */
129 mat[W][W] = -1;
130 mat[Z][Z] = -1;
131 }
132 }
133
134 #define VEQVXS3(v,w,s) ((v)[0] == (w)[0] * (s) && \
135 (v)[1] == (w)[1] * (s) && \
136 (v)[2] == (w)[2] * (s))
137
138 /*
139 * Return 1 on success, 0 on failure.
140 * FIX THIS-- find a better way to do this. I would
141 * be really surprised if there are no typos below.
142 */
143 int
get4dMatThatRotatesThese3ToThose3(int these0[4],int these1[4],int these2[4],int those0[4],int those1[4],int those2[4],int mat[4][4])144 Math4d::get4dMatThatRotatesThese3ToThose3(
145 int these0[4], int these1[4], int these2[4],
146 int those0[4], int those1[4], int those2[4], int mat[4][4]) /* OUTPUT */
147 {
148 int i, j, k, l, ii, temp[4];
149 int these[4][4], those[4][4]; /* actually [3][4] */
150 int transpthese[4][4], transpthose[4][4]; /* actually [4][3] */
151
152 SET4(these[0], these0);
153 SET4(these[1], these1);
154 SET4(these[2], these2);
155 TRANSPOSE4(transpthese, these);
156 SET4(those[0], those0);
157 SET4(those[1], those1);
158 SET4(those[2], those2);
159 TRANSPOSE4(transpthose, those);
160
161 ZEROMAT4(mat);
162 for (i = 0; i < 4; ++i)
163 {
164 for (mat[0][i] = 1; mat[0][i] >= -1; mat[0][i] -= 2)
165 {
166 if (!VEQVXS3(transpthese[0], transpthose[i], mat[0][i]))
167 continue;
168 for (j = 0; j < 4; ++j)
169 {
170 if (j == i)
171 continue;
172 for (mat[1][j] = 1; mat[1][j] >= -1; mat[1][j] -= 2)
173 {
174 if (!VEQVXS3(transpthese[1], transpthose[j], mat[1][j]))
175 continue;
176 for (k = 0; k < 4; ++k)
177 {
178 if (k == i || k == j)
179 continue;
180 for (mat[2][k] = 1; mat[2][k] >= -1; mat[2][k] -= 2)
181 {
182 if (!VEQVXS3(transpthese[2],
183 transpthose[k], mat[2][k]))
184 continue;
185 l = 6 - i - j - k;
186 for (mat[3][l] = 1; mat[3][l] >= -1;
187 mat[3][l] -= 2)
188 {
189 if (!VEQVXS3(transpthese[3],
190 transpthose[l], mat[3][l]))
191 continue;
192 /*
193 * Found a good matrix!!
194 * Assert that it works.
195 */
196 for (ii = 0; ii < 3; ++ii)
197 {
198 VXM4(temp, these[ii], mat);
199 assert(EQVEC4(temp, those[ii]));
200 }
201 return 1;
202 }
203 mat[3][l] = 0;
204 }
205 mat[2][k] = 0;
206 }
207 }
208 mat[1][j] = 0;
209 }
210 }
211 mat[0][i] = 0;
212 }
213 return 0;
214 }
215
216 // Local Variables:
217 // c-basic-offset: 4
218 // c-comment-only-line-offset: 0
219 // c-file-offsets: ((defun-block-intro . +) (block-open . 0) (substatement-open . 0) (statement-cont . +) (statement-case-open . +4) (arglist-intro . +) (arglist-close . +) (inline-open . 0))
220 // indent-tabs-mode: nil
221 // End:
222