00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #include "matrix.h"
00025
00026
00027
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
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
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