28 #include <jama/jama_svd.h>
45 p_x = p_y = p_xp = p_yp = 0.0;
59 Affine::Affine(
const Affine::AMatrix &a) {
62 p_invmat = invert(p_matrix);
63 p_x = p_y = p_xp = p_yp = 0.0;
74 Affine::AMatrix Affine::getIdentity() {
75 AMatrix ident(3, 3, 0.0);
76 for(
int i = 0 ; i < ident.dim2() ; i++) {
86 void Affine::Identity() {
87 p_matrix = getIdentity();
88 p_invmat = getIdentity();
105 void Affine::Solve(
const double x[],
const double y[],
106 const double xp[],
const double yp[],
int n) {
114 for(
int i = 0; i < n; i++) {
115 vector<double> coord(2);
133 p_matrix[2][0] = 0.0;
134 p_matrix[2][1] = 0.0;
135 p_matrix[2][2] = 1.0;
138 p_invmat = invert(p_matrix);
147 void Affine::Translate(
double tx,
double ty) {
148 AMatrix trans = getIdentity();
152 p_matrix = TNT::matmult(trans, p_matrix);
156 p_invmat = TNT::matmult(p_invmat, trans);
164 void Affine::Rotate(
double angle) {
165 AMatrix rot = getIdentity();
167 double angleRadians = angle *
Isis::PI / 180.0;
168 rot[0][0] = cos(angleRadians);
169 rot[0][1] = -sin(angleRadians);
170 rot[1][0] = sin(angleRadians);
171 rot[1][1] = cos(angleRadians);
172 p_matrix = TNT::matmult(rot, p_matrix);
174 angleRadians = -angleRadians;
175 rot[0][0] = cos(angleRadians);
176 rot[0][1] = -sin(angleRadians);
177 rot[1][0] = sin(angleRadians);
178 rot[1][1] = cos(angleRadians);
179 p_invmat = TNT::matmult(p_invmat, rot);
187 void Affine::Scale(
double scaleFactor) {
188 AMatrix scale = getIdentity();
189 scale[0][0] = scaleFactor;
190 scale[1][1] = scaleFactor;
191 p_matrix = TNT::matmult(scale, p_matrix);
194 p_invmat = invert(p_matrix);
204 void Affine::Compute(
double x,
double y) {
207 p_xp = p_matrix[0][0] * x + p_matrix[0][1] * y + p_matrix[0][2];
208 p_yp = p_matrix[1][0] * x + p_matrix[1][1] * y + p_matrix[1][2];
218 void Affine::ComputeInverse(
double xp,
double yp) {
221 p_x = p_invmat[0][0] * xp + p_invmat[0][1] * yp + p_invmat[0][2];
222 p_y = p_invmat[1][0] * xp + p_invmat[1][1] * yp + p_invmat[1][2];
231 vector<double> Affine::Coefficients(
int var) {
233 vector <double> coef;
234 coef.push_back(p_matrix[index][0]);
235 coef.push_back(p_matrix[index][1]);
236 coef.push_back(p_matrix[index][2]);
246 vector<double> Affine::InverseCoefficients(
int var) {
248 vector <double> coef;
249 coef.push_back(p_invmat[index][0]);
250 coef.push_back(p_invmat[index][1]);
251 coef.push_back(p_invmat[index][2]);
260 void Affine::checkDims(
const AMatrix &am)
const {
261 if((am.dim1() != 3) && (am.dim2() != 3)) {
263 mess <<
"Affine matrices must be 3x3 - this one is " << am.dim1()
280 Affine::AMatrix Affine::invert(
const AMatrix &a)
const {
285 JAMA::SVD<double> svd(a);
293 for(
int i = 0; i < invS.dim1(); i++) {
294 if(invS[i][i] == 0.0) {
295 string msg =
"Affine transform not invertible";
298 invS[i][i] = 1.0 / invS[i][i];
304 AMatrix transU(U.dim2(), U.dim1());
305 for(
int r = 0; r < U.dim1(); r++) {
306 for(
int c = 0; c < U.dim2(); c++) {
307 transU[c][r] = U[r][c];
312 AMatrix VinvS = TNT::matmult(V, invS);
313 return (TNT::matmult(VinvS, transU));