00001
00007 #ifndef LINALG_H_
00008 #define LINALG_H_
00009
00010 #if defined(WIN32)
00011 #define hypot _hypot
00012 #endif
00013
00014 #include <cmath>
00015
00020 typedef struct {
00021 double _[3];
00022 } vec_t;
00023
00025 inline vec_t make_vec(double x, double y, double z)
00026 {
00027 vec_t m = { {x, y, z} };
00028 return m;
00029 }
00030
00032 inline vec_t make_vec(double th, double phi)
00033 {
00034 double cosphi = cos(phi);
00035 double x = cosphi * cos(th), y = cosphi * sin(th), z = sin(phi);
00036 return make_vec(x, y, z);
00037 }
00038
00040 inline vec_t make_vec_s(double *sph)
00041 {
00042 double th = sph[0], phi = sph[1];
00043 return make_vec(th, phi);
00044 }
00045
00047 inline vec_t make_vec_c(double *m)
00048 {
00049 return make_vec(m[0], m[1], m[2]);
00050 }
00051
00053 inline void vec2sph(vec_t m, double *sph)
00054 {
00055 double x = m._[0], y = m._[1], z = m._[2];
00056 sph[0] = atan2(y, x);
00057 sph[1] = atan2(z, hypot(x, y));
00058 }
00059
00061 inline void vec2mem(vec_t v, double *m)
00062 {
00063 for (int i = 0; i < 3; i++) {
00064 m[i] = v._[i];
00065 }
00066 }
00067
00068
00069
00071 inline vec_t operator-(vec_t a)
00072 {
00073 return make_vec(-a._[0], -a._[1], -a._[2]);
00074 }
00075
00077 inline vec_t operator+(vec_t a, vec_t b)
00078 {
00079 return make_vec(a._[0] + b._[0], a._[1] + b._[1], a._[2] + b._[2]);
00080 }
00081
00083 inline vec_t operator+(vec_t a, double b)
00084 {
00085 return make_vec(a._[0] + b, a._[1] + b, a._[2] + b);
00086 }
00087
00089 inline vec_t operator+(double a, vec_t b)
00090 {
00091 return b + a;
00092 }
00093
00095 inline void operator+=(vec_t &a, double b)
00096 {
00097 a._[0] += b;
00098 a._[1] += b;
00099 a._[2] += b;
00100 }
00101
00103 inline vec_t operator-(vec_t a, vec_t b)
00104 {
00105 return make_vec(a._[0] - b._[0], a._[1] - b._[1], a._[2] - b._[2]);
00106 }
00107
00109 inline vec_t operator-(vec_t a, double b)
00110 {
00111 return make_vec(a._[0] - b, a._[1] - b, a._[2] - b);
00112 }
00113
00115 inline vec_t operator-(double a, vec_t b)
00116 {
00117 return make_vec(a - b._[0], a - b._[1], a - b._[2]);
00118 }
00119
00121 inline void operator-=(vec_t &a, double b)
00122 {
00123 a._[0] -= b;
00124 a._[1] -= b;
00125 a._[2] -= b;
00126 }
00127
00129 inline vec_t operator*(vec_t a, double b)
00130 {
00131 return make_vec(a._[0] * b, a._[1] * b, a._[2] * b);
00132 }
00133
00135 inline vec_t operator*(double a, vec_t b)
00136 {
00137 return b * a;
00138 }
00139
00141 inline void operator*=(vec_t &a, double b)
00142 {
00143 a._[0] *= b;
00144 a._[1] *= b;
00145 a._[2] *= b;
00146 }
00147
00149 inline vec_t operator/(vec_t a, double b)
00150 {
00151 return make_vec(a._[0] / b, a._[1] / b, a._[2] / b);
00152 }
00153
00155 inline vec_t operator/(double a, vec_t b)
00156 {
00157 return make_vec(a / b._[0], a / b._[1], a / b._[2]);
00158 }
00159
00161 inline void operator/=(vec_t &a, double b)
00162 {
00163 a._[0] /= b;
00164 a._[1] /= b;
00165 a._[2] /= b;
00166 }
00167
00169 inline double norm(vec_t a)
00170 {
00171 return sqrt(a._[0] * a._[0] + a._[1] * a._[1] + a._[2] * a._[2]);
00172 }
00173
00175 inline double norm2(vec_t a)
00176 {
00177 return a._[0] * a._[0] + a._[1] * a._[1] + a._[2] * a._[2];
00178 }
00179
00181 inline double dot(vec_t a, vec_t b)
00182 {
00183 return a._[0] * b._[0] + a._[1] * b._[1] + a._[2] * b._[2];
00184 }
00185
00190 typedef struct {
00191 double _[9];
00192 } mat_t;
00193
00195 inline mat_t make_mat(double a, double b, double c,
00196 double d, double e, double f,
00197 double g, double h, double i)
00198 {
00199 mat_t m = { {a, b, c, d, e, f, g, h, i} };
00200 return m;
00201 }
00202
00204 inline mat_t diag(double a, double b, double c)
00205 {
00206 mat_t m = { {a, 0, 0, 0, b, 0, 0, 0, c} };
00207 return m;
00208 }
00209
00211 inline mat_t t(mat_t m)
00212 {
00213 return make_mat(m._[0], m._[3], m._[6],
00214 m._[1], m._[4], m._[7],
00215 m._[2], m._[5], m._[8]);
00216 }
00217
00219 inline mat_t operator*(mat_t a, double b)
00220 {
00221 return make_mat(a._[0] * b, a._[1] * b, a._[2] * b,
00222 a._[3] * b, a._[4] * b, a._[5] * b,
00223 a._[6] * b, a._[7] * b, a._[8] * b);
00224 }
00225
00227 inline mat_t operator*(double a, mat_t b)
00228 {
00229 return b * a;
00230 }
00231
00233 inline vec_t operator*(mat_t a, vec_t b)
00234 {
00235 return make_vec(a._[0] * b._[0] + a._[1] * b._[1] + a._[2] * b._[2],
00236 a._[3] * b._[0] + a._[4] * b._[1] + a._[5] * b._[2],
00237 a._[6] * b._[0] + a._[7] * b._[1] + a._[8] * b._[2]);
00238 }
00239
00241 inline mat_t operator*(mat_t a, mat_t b)
00242 {
00243 return make_mat(a._[0] * b._[0] + a._[1] * b._[3] + a._[2] * b._[6],
00244 a._[0] * b._[1] + a._[1] * b._[4] + a._[2] * b._[7],
00245 a._[0] * b._[2] + a._[1] * b._[5] + a._[2] * b._[8],
00246
00247 a._[3] * b._[0] + a._[4] * b._[3] + a._[5] * b._[6],
00248 a._[3] * b._[1] + a._[4] * b._[4] + a._[5] * b._[7],
00249 a._[3] * b._[2] + a._[4] * b._[5] + a._[5] * b._[8],
00250
00251 a._[6] * b._[0] + a._[7] * b._[3] + a._[8] * b._[6],
00252 a._[6] * b._[1] + a._[7] * b._[4] + a._[8] * b._[7],
00253 a._[6] * b._[2] + a._[7] * b._[5] + a._[8] * b._[8]);
00254 }
00255
00257 inline double det(mat_t M)
00258 {
00259 return M._[0] * (M._[4] * M._[8] - M._[5] * M._[7]) -
00260 M._[1] * (M._[3] * M._[8] - M._[5] * M._[6]) +
00261 M._[2] * (M._[3] * M._[7] - M._[4] * M._[6]);
00262 }
00263
00265 inline mat_t ct(mat_t M)
00266 {
00267 return make_mat( M._[0], -M._[3], M._[6],
00268 -M._[1], M._[4], -M._[7],
00269 M._[2], -M._[5], M._[8]);
00270 }
00271
00273 inline mat_t inv(mat_t M)
00274 {
00275 return (1 / det(M)) * ct(M);
00276 }
00277
00279 inline mat_t diffusion(vec_t m, double l1, double l2)
00280 {
00281 double x = m._[0], y = m._[1], z = m._[2];
00282 mat_t R = make_mat(x, y, z,
00283 y, y * y / (1 + x) - 1, y * z / (1 + x),
00284 z, y * z / (1 + x), z * z / (1 + x) - 1);
00285 return R * diag(l1, l2, l2) * t(R) * 1e-6;
00286 }
00287
00289 inline mat_t rotation(double theta, double phi, double psi)
00290 {
00291 double c_th = cos(theta);
00292 double s_th = sin(theta);
00293 double c_ph = cos(phi);
00294 double s_ph = sin(phi);
00295 double c_ps = cos(psi);
00296 double s_ps = sin(psi);
00297
00298 double q11 = c_th * c_ph * c_ps - s_ph * s_ps;
00299 double q21 = c_th * c_ps * s_ph + c_ph * s_ps;
00300 double q31 = -c_ps * s_th;
00301 double q12 = -c_ps * s_ph - c_th * c_ph * s_ps;
00302 double q22 = c_ph * c_ps - c_th * s_ph * s_ps;
00303 double q32 = s_th * s_ps;
00304 double q13 = c_ph * s_th;
00305 double q23 = s_th * s_ph;
00306 double q33 = c_th;
00307
00308 mat_t Q = make_mat(q11, q12, q13,
00309 q21, q22, q23,
00310 q31, q32, q33);
00311 return Q;
00312 }
00313
00315 inline vec_t rotation_main_dir(double theta, double phi, double psi)
00316 {
00317 double c_th = cos(theta);
00318 double s_th = sin(theta);
00319 double c_ph = cos(phi);
00320 double s_ph = sin(phi);
00321 double c_ps = cos(psi);
00322 double s_ps = sin(psi);
00323
00324 double q11 = c_th * c_ph * c_ps - s_ph * s_ps;
00325 double q21 = c_th * c_ps * s_ph + c_ph * s_ps;
00326 double q31 = -c_ps * s_th;
00327
00328 return make_vec(q11, q21, q31);
00329 }
00330
00332 inline mat_t diffusion_euler(double theta, double phi, double psi,
00333 double l1, double l2, double l3)
00334 {
00335 mat_t Q = rotation(theta, phi, psi);
00336 return Q * diag(l1, l2, l3) * t(Q) * 1e-6;
00337 }
00338
00339 #endif // LINALG_H_