1%% 2%% e3d_mat.erl -- 3%% 4%% Operations on matrices. 5%% 6%% Copyright (c) 2001-2011 Bjorn Gustavsson and Dan Gudmundsson 7%% 8%% See the file "license.terms" for information on usage and redistribution 9%% of this file, and for a DISCLAIMER OF ALL WARRANTIES. 10%% 11%% $Id$ 12%% 13 14-module(e3d_mat). 15 16-export([identity/0,is_identity/1,determinant/1,print/1, 17 compress/1,expand/1, 18 translate/1,translate/3,scale/1,scale/3, 19 rotate/2,rotate_from_euler_rad/1, rotate_from_euler_rad/3, 20 rotate_to_z/1,rotate_s_to_t/2, 21 project_to_plane/1, 22 transpose/1,invert/1, 23 add/2,mul/1,mul/2,mul_point/2,mul_vector/2,eigenv3/1]). 24-export_type([compact_matrix/0,matrix/0]). 25 26-compile(inline). 27-include("e3d.hrl"). 28 29%% Compact 4x4 matrix representation. 30-type compact_matrix() :: 31 {float(),float(),float(), 32 float(),float(),float(), 33 float(),float(),float(), 34 float(),float(),float()}. 35 36%% General 4x4 matrix represention. 37-type matrix() :: 'identity' | compact_matrix() | 38 {float(),float(),float(),float(), 39 float(),float(),float(),float(), 40 float(),float(),float(),float(), 41 float(),float(),float(),float()}. 42 43-type e3d_vector() :: e3d_vec:vector(). 44 45-spec identity() -> compact_matrix(). 46 47-define(EPSILON, 1.0e-06). 48 49identity() -> 50 Zero = 0.0, 51 One = 1.0, 52 {One,Zero,Zero, 53 Zero,One,Zero, 54 Zero,Zero,One, 55 Zero,Zero,Zero}. 56 57-spec is_identity(matrix()) -> boolean(). 58 59is_identity(identity) -> true; 60is_identity({1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0}) -> true; 61is_identity({1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0, 62 0.0,0.0,0.0,1.0}) -> true; 63is_identity({_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_}) -> false; 64is_identity({_,_,_,_,_,_,_,_,_,_,_,_}) -> false. 65 66-spec compress(matrix()) -> compact_matrix(). 67 68compress(identity=I) -> I; 69compress({A,B,C,Z1,D,E,F,Z2,G,H,I,Z3,Tx,Ty,Tz,One}) -> 70 case eps(Z1) andalso eps(Z2) andalso eps(Z3) andalso eps(One-1.0) of 71 false -> exit(not_compressable); 72 true -> ok 73 end, 74 {A,B,C,D,E,F,G,H,I,Tx,Ty,Tz}; 75compress(Mat) 76 when tuple_size(Mat) =:= 12 -> 77 Mat. 78 79-spec expand(matrix()) -> matrix(). 80 81expand(identity) -> 82 {1.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,1.0}; 83expand({_A,_B,_C,_,_D,_E,_F,_,_G,_H,_I,_,_Tx,_Ty,_Tz,_}=Mat) -> Mat; 84expand({A,B,C,D,E,F,G,H,I,Tx,Ty,Tz}) -> 85 {A,B,C,0.0,D,E,F,0.0,G,H,I,0.0,Tx,Ty,Tz,1.0}. 86 87-spec translate(e3d_vector()) -> compact_matrix(). 88 89translate({X,Y,Z}) -> translate(X, Y, Z). 90 91-spec translate(X::float(), Y::float(), Z::float()) -> compact_matrix(). 92 93translate(Tx, Ty, Tz) -> 94 Zero = 0.0, 95 One = 1.0, 96 {One,Zero,Zero, 97 Zero,One,Zero, 98 Zero,Zero,One, 99 Tx,Ty,Tz}. 100 101-spec scale({float(),float(),float()} | float()) -> compact_matrix(). 102 103scale({X,Y,Z}) -> scale(X, Y, Z); 104scale(Sc) when is_float(Sc) -> scale(Sc, Sc, Sc). 105 106scale(Sx, Sy, Sz) -> 107 Zero = 0.0, 108 {Sx,Zero,Zero, 109 Zero,Sy,Zero, 110 Zero,Zero,Sz, 111 Zero,Zero,Zero}. 112 113-spec rotate(Angle::number(), Vector::e3d_vector()) -> compact_matrix(). 114 115rotate(A0, {X,Y,Z}) when is_float(X), is_float(Y), is_float(Z) -> 116 A = A0*(math:pi()/180), 117 CosA = math:cos(A), 118 SinA = math:sin(A), 119 XSinA = X*SinA, 120 YSinA = Y*SinA, 121 ZSinA = Z*SinA, 122 {C2,C3, C4,C6, C7,C8} = 123 {-ZSinA,YSinA, 124 ZSinA,-XSinA, 125 -YSinA,XSinA}, 126 {U1,U2,U3, U5,U6, U9} = 127 {X*X,X*Y,X*Z, 128 Y*Y,Y*Z, 129 Z*Z}, 130 U4 = U2, 131 U7 = U3, 132 U8 = U6, 133 S = CosA, 134 NegS = -S, 135 {U1+S*(1.0-U1), U4+NegS*U4+C4, U7+NegS*U7+C7, 136 U2+NegS*U2+C2, U5+S*(1.0-U5), U8+NegS*U8+C8, 137 U3+NegS*U3+C3, U6+NegS*U6+C6, U9+S*(1.0-U9), 138 0.0,0.0,0.0}. 139 140%% rotate_from_euler_rad is a shortcut for 141%% Rad2deg = 180/math:pi(), 142%% Mx = e3d_mat:rotate(Rx * Rad2deg, {1.0, 0.0, 0.0}), 143%% My = e3d_mat:rotate(Ry * Rad2deg, {0.0, 1.0, 0.0}), 144%% Mz = e3d_mat:rotate(Rz * Rad2deg, {0.0, 0.0, 1.0}), 145%% Rot = e3d_mat:mul(Mz, e3d_mat:mul(My,Mx)), 146-spec rotate_from_euler_rad(Vector::e3d_vector()) -> compact_matrix(). 147rotate_from_euler_rad({X,Y,Z}) -> 148 rotate_from_euler_rad(X,Y,Z). 149 150-spec rotate_from_euler_rad(X::number(), Y::number(), Z::number()) -> 151 compact_matrix(). 152rotate_from_euler_rad(Rx,Ry,Rz) -> 153 Cz = math:cos(Rz), Sz = math:sin(Rz), 154 Cy = math:cos(Ry), Sy = math:sin(Ry), 155 Cx = math:cos(Rx), Sx = math:sin(Rx), 156 Sxsy=Sx*Sy, Cxsy=Cx*Sy, 157 TZ = 0.0, 158 {Cy*Cz, Cy*Sz, -Sy, 159 Sxsy*Cz-Cx*Sz, Sxsy*Sz+Cx*Cz, Sx*Cy, 160 Cxsy*Cz+Sx*Sz, Cxsy*Sz-Sx*Cz, Cx*Cy, 161 TZ, TZ, TZ}. 162 163 164%% Project to plane perpendicular to vector Vec. 165 166-spec project_to_plane(Vector::e3d_vector()) -> compact_matrix(). 167 168project_to_plane(Vec) -> 169 %% T 170 %% P = QQ 171 %% (Strang: Linear Algebra and its Applications, 3rd edition, p 170.) 172 {Ux,Vx,_, 173 Uy,Vy,_, 174 Uz,Vz,_, 175 _,_,_} = rotate_to_z(Vec), 176 if 177 is_float(Ux), is_float(Uy), is_float(Uz), 178 is_float(Vx), is_float(Vy), is_float(Vz) -> 179 {Ux*Ux+Vx*Vx,Uy*Ux+Vy*Vx,Uz*Ux+Vz*Vx, 180 Ux*Uy+Vx*Vy,Uy*Uy+Vy*Vy,Uz*Uy+Vz*Vy, 181 Ux*Uz+Vx*Vz,Uy*Uz+Vy*Vz,Uz*Uz+Vz*Vz, 182 0.0,0.0,0.0} 183 end. 184 185-spec rotate_to_z(Vector::e3d_vector()) -> compact_matrix(). 186 187rotate_to_z(Vec) -> 188 {Vx,Vy,Vz} = V = 189 case e3d_vec:norm(Vec) of 190 {Wx,Wy,Wz}=W when abs(Wx) =< abs(Wy), abs(Wx) < abs(Wz) -> 191 e3d_vec:norm(0.0, Wz, -Wy); 192 {Wx,Wy,Wz}=W when abs(Wy) < abs(Wz) -> 193 e3d_vec:norm(Wz, 0.0, -Wx); 194 {Wx,Wy,Wz}=W -> 195 e3d_vec:norm(Wy, -Wx, 0.0) 196 end, 197 {Ux,Uy,Uz} = e3d_vec:cross(V, W), 198 {Ux,Vx,Wx, 199 Uy,Vy,Wy, 200 Uz,Vz,Wz, 201 0.0,0.0,0.0}. 202 203-spec rotate_s_to_t(S::e3d_vector(), T::e3d_vector()) -> compact_matrix(). 204 205rotate_s_to_t(S, T) -> 206 %% Tomas Moller/Eric Haines: Real-Time Rendering (ISBN 1-56881-101-2). 207 %% 3.3. Quaternions; Rotating one vector to another 208 case e3d_vec:dot(S, T) of 209 E when abs(E) > 0.999999 -> 210 almost_parallel(S, T); 211 E -> 212 V = e3d_vec:cross(S, T), 213 rotate_s_to_t_1(V, E) 214 end. 215 216-spec transpose(matrix()) -> matrix(). 217 218transpose(identity=I) -> I; 219transpose({M1,M2,M3,M4,M5,M6,M7,M8,M9,0.0=Z,0.0,0.0}) -> 220 {M1,M4,M7, 221 M2,M5,M8, 222 M3,M6,M9, 223 Z,Z,Z}; 224transpose({A,B,C,WX,D,E,F,WY,G,H,I,WZ,Tx,Ty,Tz,WW}) -> 225 { A, D, G,Tx, 226 B, E, H,Ty, 227 C, F, I,Tz, 228 WX,WY,WZ,WW}. 229 230 231-spec add(M::matrix(), N::matrix()) -> matrix(). 232add({B_a,B_b,B_c,B_d,B_e,B_f,B_g,B_h,B_i,B_tx,B_ty,B_tz}, 233 {A_a,A_b,A_c,A_d,A_e,A_f,A_g,A_h,A_i,A_tx,A_ty,A_tz}) 234 when is_float(A_a), is_float(A_b), is_float(A_c), is_float(A_d), is_float(A_e), 235 is_float(A_f), is_float(A_g), is_float(A_h), is_float(A_i), is_float(A_tx), 236 is_float(A_ty), is_float(A_tz), 237 is_float(B_a), is_float(B_b), is_float(B_c), is_float(B_d), is_float(B_e), 238 is_float(B_f), is_float(B_g), is_float(B_h), is_float(B_i), is_float(B_tx), 239 is_float(B_ty), is_float(B_tz) -> 240 {A_a+B_a, A_b+B_b, A_c+B_c, 241 A_d+B_d, A_e+B_e, A_f+B_f, 242 A_g+B_g, A_h+B_h, A_i+B_i, 243 A_tx+B_tx, A_ty+B_ty, A_tz+B_tz}; 244add({B_a,B_b,B_c,B_w0,B_d,B_e,B_f,B_w1,B_g,B_h,B_i,B_w2,B_tx,B_ty,B_tz,B_w3}, 245 {A_a,A_b,A_c,A_w0,A_d,A_e,A_f,A_w1,A_g,A_h,A_i,A_w2,A_tx,A_ty,A_tz,A_w3}) 246 when is_float(A_a), is_float(A_b), is_float(A_c), is_float(A_d), is_float(A_e), 247 is_float(A_f), is_float(A_g), is_float(A_h), is_float(A_i), is_float(A_tx), 248 is_float(A_ty), is_float(A_tz), 249 is_float(A_w0),is_float(A_w1),is_float(A_w2),is_float(A_w3), 250 is_float(B_a), is_float(B_b), is_float(B_c), is_float(B_d), is_float(B_e), 251 is_float(B_f), is_float(B_g), is_float(B_h), is_float(B_i), is_float(B_tx), 252 is_float(B_ty), is_float(B_tz), 253 is_float(B_w0),is_float(B_w1),is_float(B_w2),is_float(B_w3) -> 254 {A_a+B_a, A_b+B_b, A_c+B_c, A_w0+B_w0, 255 A_d+B_d, A_e+B_e, A_f+B_f, A_w1+B_w1, 256 A_g+B_g, A_h+B_h, A_i+B_i, A_w2+B_w2, 257 A_tx+B_tx, A_ty+B_ty, A_tz+B_tz, A_w3+B_w3}; 258 259add(M1,M2) when tuple_size(M1) =:= 12; tuple_size(M2) =:= 12 -> 260 add(e3d_mat:expand(M1), e3d_mat:expand(M2)). 261 262 263-spec mul(M::matrix(), N::matrix()) -> 264 matrix(); 265 (M::matrix(), number()) -> 266 matrix(); 267 (M::matrix(), {float(),float(),float(),float()}) -> 268 {float(),float(),float(),float()}. 269mul(M, identity) -> M; 270mul(identity, M) when not is_number(M) -> M; 271mul(identity,M2) -> mul(expand(identity), M2); 272mul({1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,B_tx,B_ty,B_tz}, 273 {A_a,A_b,A_c,A_d,A_e,A_f,A_g,A_h,A_i,A_tx,A_ty,A_tz}) 274 when is_float(A_tx), is_float(A_ty), is_float(A_tz), 275 is_float(B_tx), is_float(B_ty), is_float(B_tz) -> 276 {A_a, A_b, A_c, 277 A_d, A_e, A_f, 278 A_g, A_h, A_i, 279 A_tx + B_tx, 280 A_ty + B_ty, 281 A_tz + B_tz}; 282mul({B_a,B_b,B_c,B_d,B_e,B_f,B_g,B_h,B_i,B_tx,B_ty,B_tz}, 283 {A_a,A_b,A_c,A_d,A_e,A_f,A_g,A_h,A_i,A_tx,A_ty,A_tz}) 284 when is_float(A_a), is_float(A_b), is_float(A_c), is_float(A_d), is_float(A_e), 285 is_float(A_f), is_float(A_g), is_float(A_h), is_float(A_i), is_float(A_tx), 286 is_float(A_ty), is_float(A_tz), 287 is_float(B_a), is_float(B_b), is_float(B_c), is_float(B_d), is_float(B_e), 288 is_float(B_f), is_float(B_g), is_float(B_h), is_float(B_i), is_float(B_tx), 289 is_float(B_ty), is_float(B_tz) -> 290 {A_a*B_a + A_b*B_d + A_c*B_g, 291 A_a*B_b + A_b*B_e + A_c*B_h, 292 A_a*B_c + A_b*B_f + A_c*B_i, 293 A_d*B_a + A_e*B_d + A_f*B_g, 294 A_d*B_b + A_e*B_e + A_f*B_h, 295 A_d*B_c + A_e*B_f + A_f*B_i, 296 A_g*B_a + A_h*B_d + A_i*B_g, 297 A_g*B_b + A_h*B_e + A_i*B_h, 298 A_g*B_c + A_h*B_f + A_i*B_i, 299 A_tx*B_a + A_ty*B_d + A_tz*B_g + B_tx, 300 A_tx*B_b + A_ty*B_e + A_tz*B_h + B_ty, 301 A_tx*B_c + A_ty*B_f + A_tz*B_i + B_tz}; 302mul({B_a,B_b,B_c,B_d,B_e,B_f,B_g,B_h,B_i,B_j,B_k,B_l,B_tx,B_ty,B_tz,B_w}, 303 {A_a,A_b,A_c,A_d,A_e,A_f,A_g,A_h,A_i,A_j,A_k,A_l,A_tx,A_ty,A_tz,A_w}) 304 when is_float(A_a), is_float(A_b), is_float(A_c), is_float(A_d), 305 is_float(A_e), is_float(A_f), is_float(A_g), is_float(A_h), 306 is_float(A_i), is_float(A_j), is_float(A_k), is_float(A_l), 307 is_float(A_tx),is_float(A_ty), is_float(A_tz), is_float(A_w) -> 308 {A_a*B_a + A_b*B_e + A_c*B_i + A_d*B_tx, 309 A_a*B_b + A_b*B_f + A_c*B_j + A_d*B_ty, 310 A_a*B_c + A_b*B_g + A_c*B_k + A_d*B_tz, 311 A_a*B_d + A_b*B_h + A_c*B_l + A_d*B_w, 312 313 A_e*B_a + A_f*B_e + A_g*B_i + A_h*B_tx, 314 A_e*B_b + A_f*B_f + A_g*B_j + A_h*B_ty, 315 A_e*B_c + A_f*B_g + A_g*B_k + A_h*B_tz, 316 A_e*B_d + A_f*B_h + A_g*B_l + A_h*B_w, 317 318 A_i*B_a + A_j*B_e + A_k*B_i + A_l*B_tx, 319 A_i*B_b + A_j*B_f + A_k*B_j + A_l*B_ty, 320 A_i*B_c + A_j*B_g + A_k*B_k + A_l*B_tz, 321 A_i*B_d + A_j*B_h + A_k*B_l + A_l*B_w, 322 323 A_tx*B_a + A_ty*B_e + A_tz*B_i + A_w*B_tx, 324 A_tx*B_b + A_ty*B_f + A_tz*B_j + A_w*B_ty, 325 A_tx*B_c + A_ty*B_g + A_tz*B_k + A_w*B_tz, 326 A_tx*B_d + A_ty*B_h + A_tz*B_l + A_w*B_w}; 327mul({A,B,C,Q0,D,E,F,Q1,G,H,I,Q2,Tx,Ty,Tz,Q3}, {X,Y,Z,W}) 328 when is_float(A), is_float(B), is_float(C), is_float(D), is_float(E), 329 is_float(F), is_float(G), is_float(H), is_float(I), 330 is_float(Tx), is_float(Ty), is_float(Tz), 331 is_float(Q0), is_float(Q1), is_float(Q2), is_float(Q3), 332 is_float(X), is_float(Y), is_float(Z) -> 333 {X*A + Y*D + Z*G + W*Tx, 334 X*B + Y*E + Z*H + W*Ty, 335 X*C + Y*F + Z*I + W*Tz, 336 X*Q0 + Y*Q1 + Z*Q2 + W*Q3}; 337mul({A,B,C,Q0,D,E,F,Q1,G,H,I,Q2,Tx,Ty,Tz,Q3}, W) 338 when is_float(A), is_float(B), is_float(C), is_float(D), is_float(E), 339 is_float(F), is_float(G), is_float(H), is_float(I), 340 is_float(Tx), is_float(Ty), is_float(Tz), 341 is_float(Q0), is_float(Q1), is_float(Q2), is_float(Q3), 342 is_number(W) -> 343 {A*W, B*W, C*W, Q0*W, 344 D*W, E*W, F*W, Q1*W, 345 G*W, H*W, I*W, Q2*W, 346 Tx*W,Ty*W,Tz*W, Q3*W}; 347mul({A,B,C,D,E,F,G,H,I,Tx,Ty,Tz}, W) 348 when is_float(A), is_float(B), is_float(C), is_float(D), is_float(E), 349 is_float(F), is_float(G), is_float(H), is_float(I), 350 is_float(Tx), is_float(Ty), is_float(Tz), is_number(W) -> 351 {A*W, B*W, C*W, 352 D*W, E*W, F*W, 353 G*W, H*W, I*W, 354 Tx*W,Ty*W,Tz*W}; 355mul(M1,M2) 356 when tuple_size(M1) =:= 12; tuple_size(M2) =:= 12 -> 357 mul(expand(M1), expand(M2)). 358%%-------------------------------------------------------------- 359%% mul([Rx,Ry,Rz]) = mul([mul(Ry,Rx),Rz]) 360%%-------------------------------------------------------------- 361-spec mul([matrix]) -> matrix(). 362mul([A, B | T ]) -> mul([mul(B,A) | T]); 363mul([A]) -> A. 364 365 366-spec mul_point(Matrix::matrix(), Point::e3d_vector()) -> e3d_vector(). 367 368mul_point(identity, P) -> P; 369mul_point({A,B,C,D,E,F,G,H,I,Tx,Ty,Tz}, {X,Y,Z}) 370 when is_float(A), is_float(B), is_float(C), is_float(D), is_float(E), 371 is_float(F), is_float(G), is_float(H), is_float(I), 372 is_float(Tx), is_float(Ty), is_float(Tz), is_float(X), is_float(Y), is_float(Z) -> 373 share(X*A + Y*D + Z*G + Tx, 374 X*B + Y*E + Z*H + Ty, 375 X*C + Y*F + Z*I + Tz); 376mul_point({A,B,C,0.0,D,E,F,0.0,G,H,I,0.0,Tx,Ty,Tz,1.0}, {X,Y,Z}) 377 when is_float(A), is_float(B), is_float(C), is_float(D), is_float(E), 378 is_float(F), is_float(G), is_float(H), is_float(I), 379 is_float(Tx), is_float(Ty), is_float(Tz), is_float(X), is_float(Y), is_float(Z) -> 380 share(X*A + Y*D + Z*G + Tx, 381 X*B + Y*E + Z*H + Ty, 382 X*C + Y*F + Z*I + Tz); 383mul_point({A,B,C,WX,D,E,F,WY,G,H,I,WZ,Tx,Ty,Tz,WW}, {X,Y,Z}) 384 when is_float(A), is_float(B), is_float(C), is_float(D), is_float(E), 385 is_float(F), is_float(G), is_float(H), is_float(I), 386 is_float(Tx), is_float(Ty), is_float(Tz), 387 is_float(WX), is_float(WY), is_float(WZ), is_float(WW), 388 is_float(X), is_float(Y), is_float(Z) -> 389 W = WX*X + WY*Y + WZ * Z + WW, 390 case W =:= 1.0 of 391 true -> 392 share(X*A + Y*D + Z*G + Tx, 393 X*B + Y*E + Z*H + Ty, 394 X*C + Y*F + Z*I + Tz); 395 false -> 396 share((X*A + Y*D + Z*G + Tx)/W, 397 (X*B + Y*E + Z*H + Ty)/W, 398 (X*C + Y*F + Z*I + Tz)/W) 399 end. 400 401-spec mul_vector(Matrix::matrix(), Vector::e3d_vector()) -> e3d_vector(). 402 403mul_vector(identity, Vec) -> Vec; 404mul_vector({A,B,C,D,E,F,G,H,I,Tx,Ty,Tz}, {X,Y,Z}) 405 when is_float(A), is_float(B), is_float(C), is_float(D), is_float(E), 406 is_float(F), is_float(G), is_float(H), is_float(I), 407 is_float(Tx), is_float(Ty), is_float(Tz), is_float(X), is_float(Y), is_float(Z) -> 408 share(X*A + Y*D + Z*G, 409 X*B + Y*E + Z*H, 410 X*C + Y*F + Z*I); 411mul_vector({A,B,C,_,D,E,F,_,G,H,I,_,Tx,Ty,Tz,_}, {X,Y,Z}) 412 when is_float(A), is_float(B), is_float(C), is_float(D), is_float(E), 413 is_float(F), is_float(G), is_float(H), is_float(I), 414 is_float(Tx), is_float(Ty), is_float(Tz), is_float(X), is_float(Y), is_float(Z) -> 415 share(X*A + Y*D + Z*G, 416 X*B + Y*E + Z*H, 417 X*C + Y*F + Z*I). 418 419%%-------------------------------------------------------------------- 420%% @doc Calculates the determinant 421%% @end 422%%-------------------------------------------------------------------- 423-spec determinant(matrix()) -> float(). 424determinant(identity) -> 1.0; 425determinant(Mat) 426 when tuple_size(Mat) =:= 12 -> 427 determinant(e3d_mat:expand(Mat)); 428determinant({M0,M1,M2,M3,M4,M5,M6,M7,M8,M9,M10,M11,M12,M13,M14,M15}) -> 429 A0 = M0*M5 - M1*M4, A1 = M0*M6 - M2*M4, 430 A2 = M0*M7 - M3*M4, A3 = M1*M6 - M2*M5, 431 A4 = M1*M7 - M3*M5, A5 = M2*M7 - M3*M6, 432 433 B0 = M8*M13 - M9*M12, B1 = M8*M14 - M10*M12, 434 B2 = M8*M15 - M11*M12, B3 = M9*M14 - M10*M13, 435 B4 = M9*M15 - M11*M13, B5 = M10*M15 - M11*M14, 436 437 A0*B5 - A1*B4 + A2*B3 + A3*B2 - A4*B1 + A5*B0. 438 439%%-------------------------------------------------------------------- 440%% @doc Calculates the inverse matrix 441%% @end 442%%-------------------------------------------------------------------- 443-spec invert(matrix()) -> matrix(). 444invert(identity) -> identity; 445invert(Mat) 446 when tuple_size(Mat) =:= 12 -> 447 invert(e3d_mat:expand(Mat)); 448invert({M0, M1, M2, M3, 449 M4, M5, M6, M7, 450 M8, M9, M10,M11, 451 M12,M13,M14,M15}) -> 452 A0 = M0*M5 - M1*M4, A1 = M0*M6 - M2*M4, 453 A2 = M0*M7 - M3*M4, A3 = M1*M6 - M2*M5, 454 A4 = M1*M7 - M3*M5, A5 = M2*M7 - M3*M6, 455 456 B0 = M8*M13 - M9*M12, B1 = M8*M14 - M10*M12, 457 B2 = M8*M15 - M11*M12, B3 = M9*M14 - M10*M13, 458 B4 = M9*M15 - M11*M13, B5 = M10*M15 - M11*M14, 459 460 Det = A0*B5 - A1*B4 + A2*B3 + A3*B2 - A4*B1 + A5*B0, 461 case abs(Det) > ?EPSILON of 462 true -> 463 InvDet = 1.0/Det, 464 {(+ M5*B5 - M6*B4 + M7*B3) * InvDet, 465 (- M1*B5 + M2*B4 - M3*B3) * InvDet, 466 (+ M13*A5 - M14*A4 + M15*A3) * InvDet, 467 (- M9*A5 + M10*A4 - M11*A3) * InvDet, 468 (- M4*B5 + M6*B2 - M7*B1) * InvDet, 469 (+ M0*B5 - M2*B2 + M3*B1) * InvDet, 470 (- M12*A5 + M14*A2 - M15*A1) * InvDet, 471 (+ M8*A5 - M10*A2 + M11*A1) * InvDet, 472 (+ M4*B4 - M5*B2 + M7*B0) * InvDet, 473 (- M0*B4 + M1*B2 - M3*B0) * InvDet, 474 (+ M12*A4 - M13*A2 + M15*A0) * InvDet, 475 (- M8*A4 + M9*A2 - M11*A0) * InvDet, 476 (- M4*B3 + M5*B1 - M6*B0) * InvDet, 477 (+ M0*B3 - M1*B1 + M2*B0) * InvDet, 478 (- M12*A3 + M13*A1 - M14*A0) * InvDet, 479 (+ M8*A3 - M9*A1 + M10*A0) * InvDet 480 }; 481 false -> 482 exit(singular_matrix) 483 end. 484 485%%-------------------------------------------------------------------- 486%% @doc Prints a matrix 487%% @end 488%%-------------------------------------------------------------------- 489-spec print(e3d_transform:transform()) -> ok. 490 491print(#e3d_transf{mat=Mat}) -> 492 print_1(Mat); 493print(identity) -> 494 print_1(e3d_mat:identity()); 495print(Mat) when tuple_size(Mat) =:= 12; 496 tuple_size(Mat) =:= 16 -> 497 print_1(Mat). 498 499print_1({A,B,C,D,E,F,G,H,I,Tx,Ty,Tz}) -> 500 print_1({A,B,C,0.0,D,E,F,0.0,G,H,I,0.0,Tx,Ty,Tz,1.0}); 501print_1(_Mat = {A,B,C,D,E,F,G,H,I,J,K,L,TX,TY,TZ,W}) -> 502 io:format(" ~10.3g ~10.3g ~10.3g ~10.3g~n ~10.3g ~10.3g ~10.3g ~10.3g~n" 503 " ~10.3g ~10.3g ~10.3g ~10.3g~n ~10.3g ~10.3g ~10.3g ~10.3g~n~n", 504 [A,E,I,TX, 505 B,F,J,TY, 506 C,G,K,TZ, 507 D,H,L,W]). 508 509%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 510%% Calculates Eigenvalues and vectors 511%% This is converted from Dave Eberly's MAGIC library 512%% Returns ordered by least EigenValue first 513%% {Evals={V1,V2,V3},Evects={X1,Y1,Z1,X2,Y2,Z2,X3,Y3,Z3}} 514 515eigenv3(Mat0={A,B,C,D,E,F,G,H,I}) 516 when is_float(A), is_float(B), is_float(C), 517 is_float(D), is_float(E), is_float(F), 518 is_float(G), is_float(H), is_float(I) -> 519 {Mat1,Diag,SubD} = eig_triDiag3(Mat0), 520 {{Va1,Va2,Va3},Vecs} = eig_ql(0,0,Diag,SubD,Mat1), 521 {X1,X2,X3,Y1,Y2,Y3,Z1,Z2,Z3} = Vecs, 522 if (Va1 =< Va2), (Va2 =< Va3) -> 523 {{Va1,Va2,Va3},{X1,Y1,Z1,X2,Y2,Z2,X3,Y3,Z3}}; 524 (Va1 =< Va3), (Va3 =< Va2) -> 525 {{Va1,Va3,Va2},{X1,Y1,Z1,X3,Y3,Z3,X2,Y2,Z2}}; 526 (Va2 =< Va1), (Va1 =< Va3) -> 527 {{Va2,Va1,Va3},{X2,Y2,Z2,X1,Y1,Z1,X3,Y3,Z3}}; 528 (Va2 =< Va3), (Va3 =< Va1) -> 529 {{Va2,Va3,Va1},{X2,Y2,Z2,X3,Y3,Z3,X1,Y1,Z1}}; 530 (Va3 =< Va1), (Va1 =< Va2) -> 531 {{Va3,Va1,Va2},{X3,Y3,Z3,X1,Y1,Z1,X2,Y2,Z2}}; 532 (Va3 =< Va2), (Va2 =< Va1) -> 533 {{Va3,Va2,Va1},{X3,Y3,Z3,X2,Y2,Z2,X1,Y1,Z1}} 534 end. 535 536%%% 537%%% Local functions. 538%%% 539 540share(X, X, X) -> {X,X,X}; 541share(X, X, Z) -> {X,X,Z}; 542share(X, Y, Y) -> {X,Y,Y}; 543share(X, Y, X) -> {X,Y,X}; 544share(X, Y, Z) -> {X,Y,Z}. 545 546 547almost_parallel(S, T) -> 548 %% Parallel case as in Moller/Hughes: 549 %% http://www.acm.org/jgt/papers/MollerHughes99 550 Axis = closest_axis(S), 551 U = e3d_vec:sub(Axis, S), 552 V = e3d_vec:sub(Axis, T), 553 554 C1 = 2.0 / e3d_vec:dot(U, U), 555 C2 = 2.0 / e3d_vec:dot(V, V), 556 C3 = C1 * C2 * e3d_vec:dot(U, V), 557 C = {C1,C2,C3,U,V}, 558 559 {1.0+ael(C, 1, 1),ael(C, 2, 1),ael(C, 3, 1), 560 ael(C, 1, 2),1.0+ael(C, 2, 2),ael(C, 3, 2), 561 ael(C, 1, 3),ael(C, 2, 3),1.0+ael(C, 3, 3), 562 0.0,0.0,0.0}. 563 564ael({C1,C2,C3,U,V}, I, J) -> 565 -C1 * element(I, U) * element(J, U) - 566 C2 * element(I, V) * element(J, V) + 567 C3 * element(I, V) * element(J, U). 568 569closest_axis({X0,Y0,Z0}) -> 570 X = abs(X0), 571 Y = abs(Y0), 572 Z = abs(Z0), 573 if 574 X < Y -> 575 if 576 X < Z -> {1.0,0.0,0.0}; 577 true -> {0.0,0.0,1.0} 578 end; 579 true -> 580 if 581 Y < Z -> {0.0,1.0,0.0}; 582 true -> {0.0,0.0,1.0} 583 end 584 end. 585 586rotate_s_to_t_1({Vx,Vy,Vz}, E) when is_float(Vx), is_float(Vy), is_float(Vz) -> 587 H = (1.0 - E)/(Vx*Vx+Vy*Vy+Vz*Vz), 588 HVx = H*Vx, 589 HVz = H*Vz, 590 HVxy = HVx*Vy, 591 HVxz = HVx*Vz, 592 HVyz = HVz*Vy, 593 {E+HVx*Vx,HVxy+Vz,HVxz-Vy, 594 HVxy-Vz,E+H*Vy*Vy,HVyz+Vx, 595 HVxz+Vy,HVyz-Vx,E+HVz*Vz, 596 0.0,0.0,0.0}. 597 598eig_triDiag3({A0,B0,C0,_,D0,E0,_,_,F0}) 599 when is_float(A0), is_float(B0), is_float(C0), 600 is_float(D0), is_float(E0), is_float(F0) -> 601 Di0 = A0, 602 if abs(C0) >= ?EPSILON -> 603 Ell = math:sqrt(B0*B0+C0*C0), 604 B = B0/Ell, 605 C = C0/Ell, 606 Q = 2*B*E0+C*(F0-D0), 607 Di1 = D0+C*Q, 608 Di2 = F0-C*Q, 609 Su0 = Ell, 610 Su1 = E0-B*Q, 611 Mat = {1.0, 0.0, 0.0, 612 0.0, B, C, 613 0.0, C, -B}, 614 {Mat,{Di0,Di1,Di2},{Su0,Su1,0.0}}; 615 true -> 616 Mat = {1.0, 0.0, 0.0, 617 0.0, 1.0, 0.0, 618 0.0, 0.0, 1.0}, 619 {Mat,{Di0,D0,F0},{B0,E0,0.0}} 620 end. 621 622eps(E) -> 623 abs(E) < ?EPSILON. 624 625-define(S(I),element(I+1,Subd0)). 626-define(D(I),element(I+1,Diag0)). 627-define(Set(I,Val,Tup),setelement(I+1,Tup,Val)). 628 629-define(M_SIZE,3). 630-define(MAX_ITER,32). 631 632eig_ql(I0,I1,Diag0,Subd0,Mat0) when I0 < ?M_SIZE, I1 < ?MAX_ITER -> 633 case eig_cont(I0,Diag0,Subd0) of 634 I0 -> 635 eig_ql(I0+1,0,Diag0,Subd0,Mat0); 636 I2 -> 637 FG0 = (?D(I0+1)-?D(I0))/(2.0*?S(I0)), 638 FR = math:sqrt(FG0*FG0+1.0), 639 FG1 = if FG0 < 0.0 -> 640 ?D(I2)-?D(I0)+?S(I0)/(FG0-FR); 641 true -> 642 ?D(I2)-?D(I0)+?S(I0)/(FG0+FR) 643 end, 644 {FP,FG,Diag1,Subd1,Mat} = 645 eig_ql2(I2-1,I0,1.0,1.0,0.0,FG1,Diag0,Subd0,Mat0), 646 Diag = ?Set(I0,element(I0+1,Diag1)-FP,Diag1), 647 Subd2 = ?Set(I0,FG,Subd1), 648 Subd = ?Set(I2,0.0,Subd2), 649 eig_ql(I0,I1+1,Diag,Subd,Mat) 650 end; 651eig_ql(I0,I1,Diag,Subd,Mat) when I1 >= ?MAX_ITER -> 652 io:format("Hmm I1>MAX_ITER,original algo break fails here~n",[]), 653 eig_ql(I0+1,0,Diag,Subd,Mat); 654eig_ql(_,_,D,_,M) -> 655 {D,M}. 656 657eig_cont(I2,Diag0,Subd0) when I2 =< ?M_SIZE-2 -> 658 Ftmp = abs(?D(I2))+abs(?D(I2+1)), 659 if (abs(?S(I2))+Ftmp) == Ftmp -> 660 I2; 661 true -> 662 eig_cont(I2+1,Diag0,Subd0) 663 end; 664eig_cont(I2,_,_) -> I2. 665 666eig_ql2(I3,I0,Sin0,Cos0,FP0,FG0,Diag0,Subd0,Mat0) when I3 >= I0 -> 667 FF = Sin0*?S(I3), 668 FB = Cos0*?S(I3), 669 {Si3p1,Sin1,Cos1} = 670 if abs(FF) >= abs(FG0) -> 671 eig_up(FG0,FF,pos); 672 true -> 673 eig_up(FF,FG0,neg) 674 end, 675 FG1 = ?D(I3+1)-FP0, 676 FR = (?D(I3)-FG1)*Sin1+2.0*FB*Cos1, 677 FP1 = Sin1*FR, 678 Di3p1 = FG1+FP1, 679 Mat = eig_vec(0,I3,Sin1,Cos1,Mat0), 680 eig_ql2(I3-1,I0,Sin1,Cos1,FP1,Cos1*FR-FB, 681 ?Set(I3+1,Di3p1,Diag0),?Set(I3+1,Si3p1,Subd0),Mat); 682eig_ql2(_,_,_,_,FP,FG,Diag,Subd,Mat) -> 683 {FP,FG,Diag,Subd,Mat}. 684 685eig_up(FG,FF,Type) -> 686 Cos = FG/FF, 687 FR = math:sqrt(Cos*Cos+1.0), 688 FSin = 1.0/FR, 689 case Type of 690 pos -> 691 {FF*FR,FSin,Cos*FSin}; 692 neg -> 693 {FF*FR,Cos*FSin,FSin} 694 end. 695 696eig_vec(I4,I3,Sin,Cos,Mat0) when I4 < ?M_SIZE -> 697 Idx43p1 = I4*?M_SIZE+I3+1+1, 698 Idx43 = I4*?M_SIZE+I3+1, 699 Mat43 = element(Idx43,Mat0), 700 FF = element(Idx43p1,Mat0), 701 Mat1 = setelement(Idx43p1, Mat0, Sin*Mat43+Cos*FF), 702 Mat2 = setelement(Idx43, Mat1, Cos*Mat43-Sin*FF), 703 eig_vec(I4+1,I3,Sin,Cos,Mat2); 704eig_vec(_,_,_,_,Mat) -> Mat. 705 706%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 707