matrix.c

00001 /*
00002  * matrix
00003  *
00004  * Copyright 2008 Dean Ferreyra <dferreyra@igc.org>, All rights reserved
00005  *
00006  * This file is part of Footprint-Update.
00007  * 
00008  * Footprint-Update is free software: you can redistribute it and/or modify
00009  * it under the terms of the GNU General Public License as published by
00010  * the Free Software Foundation, either version 3 of the License, or
00011  * (at your option) any later version.
00012  * 
00013  * Footprint-Update is distributed in the hope that it will be useful,
00014  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00015  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00016  * GNU General Public License for more details.
00017  * 
00018  * You should have received a copy of the GNU General Public License
00019  * along with Footprint-Update.  If not, see <http://www.gnu.org/licenses/>.
00020  *
00021  * $Id: matrix.c,v 1.3 2008-05-22 05:29:29 dean Exp $
00022  */
00023 
00024 #include "matrix.h"
00025 
00026 /*
00027  * 3x1 Vectors
00028  */
00029 
00030 void
00031 make_vec(Vector3x1 vec, double x, double y)
00032 {
00033   vec[0][0] = x;
00034   vec[1][0] = y;
00035   vec[2][0] = 1;
00036 }
00037 
00038 CheapPointType
00039 vec_to_point(Vector3x1 vec)
00040 {
00041   return make_point(round(vec[0][0]), round(vec[1][0]));
00042 }
00043 
00044 void
00045 point_to_vec(CheapPointType pt, Vector3x1 vec)
00046 {
00047   make_vec(vec, pt.X, pt.Y);
00048 }
00049 
00050 /*
00051  * 3x3 Matrices
00052  */
00053 
00054 void
00055 copy_matrix(Matrix3x3 src, Matrix3x3 dst)
00056 {
00057   memcpy(dst, src, sizeof(Matrix3x3));
00058 }
00059 
00060 void
00061 make_identity_matrix(Matrix3x3 mat)
00062 {
00063   memset(mat, 0, sizeof(Matrix3x3));
00064   int i;
00065   for (i = 0; i < 3; i++) {
00066     mat[i][i] = 1;
00067   }
00068 }
00069 
00070 void
00071 make_translation_matrix(Matrix3x3 mat, double dx, double dy)
00072 {
00073   make_identity_matrix(mat);
00074   mat[0][2] = dx;
00075   mat[1][2] = dy;
00076 }
00077 
00078 void
00079 make_rotation_matrix(Matrix3x3 mat, double theta)
00080 {
00081   double c = cos(theta);
00082   double s = sin(theta);
00083   make_identity_matrix(mat);
00084   mat[0][0] = c; mat[0][1] = -s;
00085   mat[1][0] = s; mat[1][1] = c;
00086 }
00087 
00088 void
00089 make_reflection_matrix_x_axis(Matrix3x3 mat)
00090 {
00091   make_identity_matrix(mat);
00092   mat[1][1] = -1;
00093 }
00094 
00095 void
00096 make_reflection_matrix_y_axis(Matrix3x3 mat)
00097 {
00098   make_identity_matrix(mat);
00099   mat[0][0] = -1;
00100 }
00101 
00102 void
00103 multiply_matrix_vector(Matrix3x3 mat, Vector3x1 vec,
00104                        Vector3x1 result)
00105 {
00106   int i;
00107   for (i = 0; i < 3; i++) {
00108     double s = 0;
00109     int j;
00110     for (j = 0; j < 3; j++) {
00111       s += mat[i][j] * vec[j][0];
00112     }
00113     result[i][0] = s;
00114   }
00115 }
00116 
00117 void
00118 multiply_matrix_matrix(Matrix3x3 mat1, Matrix3x3 mat2,
00119                        Matrix3x3 result)
00120 {
00121   int k;
00122   for (k = 0; k < 3; k++) {
00123     int i;
00124     for (i = 0; i < 3; i++) {
00125       double s = 0;
00126       int j;
00127       for (j = 0; j < 3; j++) {
00128         s += mat1[i][j] * mat2[j][k];
00129       }
00130       result[i][k] = s;
00131     }
00132   }
00133 }
00134 
00135 void
00136 multiply_matrix_matrix_inplace(Matrix3x3 mat1, Matrix3x3 mat2)
00137 {
00138   Matrix3x3 result;
00139   multiply_matrix_matrix(mat1, mat2, result);
00140   memcpy(mat2, result, sizeof(Matrix3x3));
00141 }
00142 
00143 CheapPointType
00144 transform_point(Matrix3x3 mat, CheapPointType pt)
00145 {
00146   Vector3x1 vec;
00147   Vector3x1 vec_result;
00148 
00149   point_to_vec(pt, vec);
00150   multiply_matrix_vector(mat, vec, vec_result);
00151   return vec_to_point(vec_result);
00152 }
00153 
00154 /*
00155  * Logging
00156  */
00157 
00158 void
00159 log_vector(Vector3x1 vec)
00160 {
00161   base_log("Vector\n");
00162   int i;
00163   for (i = 0; i < 3; i++) {
00164     base_log("%7.4f\n", vec[i][0]);
00165   }
00166 }
00167 
00168 void
00169 debug_log_vector(Vector3x1 vec)
00170 {
00171 #if DEBUG
00172   log_vector(vec);
00173 #endif
00174 }
00175 
00176 void
00177 log_matrix(Matrix3x3 mat)
00178 {
00179   base_log("Matrix\n");
00180   int i;
00181   for (i = 0; i < 3; i++) {
00182     base_log("%7.4f %7.4f %7.4f\n",
00183              mat[i][0], mat[i][1], mat[i][2]);
00184   }
00185 }
00186 
00187 void
00188 debug_log_matrix(Matrix3x3 mat)
00189 {
00190 #if DEBUG
00191   debug_log_matrix(mat);
00192 #endif
00193 }
00194 

Generated on Tue Aug 17 15:28:04 2010 for pcb-plugins by  doxygen 1.4.6-NO