1%% 2%% e3d_vec.erl -- 3%% 4%% Arithmetic on vectors and points (represented as three-tuples). 5%% 6%% Copyright (c) 2001-2011 Bjorn Gustavsson 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_vec). 15 16-export([zero/0,is_zero/1,add/1,add/2,add_prod/3,sub/1,sub/2,lerp/3, 17 norm_sub/2,mul/2,divide/2,neg/1,dot/2,cross/2, 18 len/1,len_sqr/1,dist/2,dist_sqr/2, 19 norm/1,norm/3,normal/3,normal/1,average/1,average/2,average/4, 20 bounding_box/1,area/3,degrees/2, 21 plane/1,plane/2,plane/3, 22 plane_side/2,plane_dist/2, 23 line_dist/3, line_dist_sqr/3, line_line_intersect/4, 24 project_2d/2, largest_dir/1 25 ]). 26 27-export_type([vector/0, point/0]). 28-export([format/1]). 29 30-include("e3d.hrl"). 31 32-compile(inline). 33-compile({inline_size,24}). 34 35%% 3D vector or location. 36-type vector() :: {float(),float(),float()}. 37-type point() :: {float(),float(),float()}. 38 39-type plane() :: {vector(), float()}. 40 41-spec zero() -> vector(). 42 43zero() -> 44 {0.0,0.0,0.0}. 45 46-spec is_zero(vector()) -> boolean(). 47 48is_zero({0.0,0.0,0.0}) -> true; 49is_zero(_) -> false. 50 51-spec add(vector(), vector()) -> vector(). 52 53add({V10,V11,V12}, {V20,V21,V22}) when is_float(V10), is_float(V11), is_float(V12) -> 54 {V10+V20,V11+V21,V12+V22}. 55 56add_prod({V10,V11,V12}, {V20,V21,V22}, S) when is_float(S) -> 57 {S*V20+V10,S*V21+V11,S*V22+V12}. 58 59-spec add([vector()]) -> vector(). 60 61add([{V10,V11,V12}|T]) -> 62 add(T, V10, V11, V12). 63 64-spec sub(vector(), vector()) -> vector(). 65 66sub({V10,V11,V12}, {V20,V21,V22}) -> 67 {V10-V20,V11-V21,V12-V22}. 68 69-spec norm_sub(vector(), vector()) -> vector(). 70 71norm_sub({V10,V11,V12}, {V20,V21,V22}) 72 when is_float(V10), is_float(V11), is_float(V12) -> 73 Nx = V10-V20, 74 Ny = V11-V21, 75 Nz = V12-V22, 76 SqrLen = Nx*Nx + Ny*Ny + Nz*Nz, 77 norm(SqrLen, Nx, Ny, Nz). 78 79-spec sub([vector()]) -> vector(). 80 81sub([{V10,V11,V12}|T]) -> 82 sub(V10, V11, V12, T). 83 84-spec mul(vector(), S::float()) -> vector(). 85 86mul({V10,V11,V12}, S) when is_float(S) -> 87 {V10*S,V11*S,V12*S}. 88 89-spec divide(vector(), S::float()) -> vector(). 90 91divide({V10,V11,V12}, S) -> 92 InvS = 1/S, 93 {V10*InvS,V11*InvS,V12*InvS}. 94 95-spec neg(vector()) -> vector(). 96 97neg({X,Y,Z}) -> {-X,-Y,-Z}. 98 99-spec dot(vector(), vector()) -> float(). 100 101dot({V10,V11,V12}, {V20,V21,V22}) when is_float(V10), is_float(V11), is_float(V12) -> 102 V10*V20 + V11*V21 + V12*V22. 103 104-spec cross(vector(), vector()) -> vector(). 105 106cross({V10,V11,V12}, {V20,V21,V22}) 107 when is_float(V10), is_float(V11), is_float(V12), 108 is_float(V20), is_float(V21), is_float(V22) -> 109 {V11*V22-V12*V21,V12*V20-V10*V22,V10*V21-V11*V20}. 110 111-spec len(vector()) -> float(). 112 113len({X,Y,Z}) when is_float(X), is_float(Y), is_float(Z) -> 114 math:sqrt(X*X+Y*Y+Z*Z). 115 116-spec len_sqr(vector()) -> float(). 117 118len_sqr({X,Y,Z}) when is_float(X), is_float(Y), is_float(Z) -> 119 X*X+Y*Y+Z*Z. 120 121-spec lerp(vector(), vector(), float()) -> vector(). 122 123lerp({V10,V11,V12}, {V20,V21,V22}, T) 124 when is_float(V10), is_float(V11), is_float(V12), 125 is_float(V20), is_float(V21), is_float(V22) -> 126 {V10+(V20-V10)*T, V11+(V21-V11)*T, V12+(V22-V12)*T}. 127 128-spec dist(vector(), vector()) -> float(). 129 130dist({V10,V11,V12}, {V20,V21,V22}) when is_float(V10), is_float(V11), is_float(V12), 131 is_float(V20), is_float(V21), is_float(V22) -> 132 X = V10-V20, 133 Y = V11-V21, 134 Z = V12-V22, 135 math:sqrt(X*X+Y*Y+Z*Z). 136 137-spec dist_sqr(vector(), vector()) -> float(). 138 139dist_sqr({V10,V11,V12}, {V20,V21,V22}) 140 when is_float(V10), is_float(V11), is_float(V12) -> 141 X = V10-V20, 142 Y = V11-V21, 143 Z = V12-V22, 144 X*X+Y*Y+Z*Z. 145 146-spec norm(vector()) -> vector(). 147 148norm({V1,V2,V3}) -> 149 norm(V1, V2, V3). 150 151-spec norm(X::float(), Y::float(), Z::float()) -> vector(). 152 153norm(V1, V2, V3) when is_float(V1), is_float(V2), is_float(V3) -> 154 norm(V1*V1+V2*V2+V3*V3, V1, V2, V3). 155 156-spec normal(vector(), vector(), vector()) -> vector(). 157 158normal({V10,V11,V12}, {V20,V21,V22}, {V30,V31,V32}) 159 when is_float(V10), is_float(V11), is_float(V12), 160 is_float(V20), is_float(V21), is_float(V22), 161 is_float(V30), is_float(V31), is_float(V32) -> 162 D10 = V10-V20, 163 D11 = V11-V21, 164 D12 = V12-V22, 165 D20 = V20-V30, 166 D21 = V21-V31, 167 D22 = V22-V32, 168 N0 = D11*D22-D12*D21, 169 N1 = D12*D20-D10*D22, 170 N2 = D10*D21-D11*D20, 171 D = math:sqrt(N0*N0+N1*N1+N2*N2), 172 try {N0/D,N1/D,N2/D} 173 catch 174 error:badarith -> {0.0,0.0,0.0} 175 end. 176 177%% normal([{X,Y,Z}]) -> 178%% Calculate the averaged normal for the polygon using Newell's method. 179 180-spec normal([vector()]) -> vector(). 181 182normal([{Ax,Ay,Az},{Bx,By,Bz},{Cx,Cy,Cz}]) 183 when is_float(Ax), is_float(Ay), is_float(Az), 184 is_float(Bx), is_float(By), is_float(Bz) -> 185 Sx = (Ay-By)*(Az+Bz) + (By-Cy)*(Bz+Cz) + (Cy-Ay)*(Cz+Az), 186 Sy = (Az-Bz)*(Ax+Bx) + (Bz-Cz)*(Bx+Cx) + (Cz-Az)*(Cx+Ax), 187 Sz = (Ax-Bx)*(Ay+By) + (Bx-Cx)*(By+Cy) + (Cx-Ax)*(Cy+Ay), 188 SqrLen = Sx*Sx + Sy*Sy + Sz*Sz, 189 norm(SqrLen, Sx, Sy, Sz); 190normal([{Ax,Ay,Az},{Bx,By,Bz},{Cx,Cy,Cz},{Dx,Dy,Dz}]) 191 when is_float(Ax), is_float(Ay), is_float(Az), 192 is_float(Bx), is_float(By), is_float(Bz) -> 193 %% The same result as the Newell normal (after normalization) 194 %% can be calculated by taking the cross product of the vectors 195 %% formed by the diagonals of the quad. (From Christer Ericson: 196 %% "Real-Time Collision Detection", Chapter 12.) 197 V10 = Dx-Bx, V11 = Dy-By, V12 = Dz-Bz, 198 V20 = Ax-Cx, V21 = Ay-Cy, V22 = Az-Cz, 199 Nx = V11*V22-V12*V21, 200 Ny = V12*V20-V10*V22, 201 Nz = V10*V21-V11*V20, 202 SqrLen = Nx*Nx + Ny*Ny + Nz*Nz, 203 norm(SqrLen, Nx, Ny, Nz); 204normal([{Ax,Ay,Az},{Bx,By,Bz}|[{Cx,Cy,Cz}|_]=T]=First) 205 when is_float(Ax), is_float(Ay), is_float(Az), 206 is_float(Bx), is_float(By), is_float(Bz) -> 207 Sx = (Ay-By)*(Az+Bz) + (By-Cy)*(Bz+Cz), 208 Sy = (Az-Bz)*(Ax+Bx) + (Bz-Cz)*(Bx+Cx), 209 Sz = (Ax-Bx)*(Ay+By) + (Bx-Cx)*(By+Cy), 210 normal_1(T, First, Sx, Sy, Sz). 211 212normal_1([{Ax,Ay,Az}], [{Bx,By,Bz}|_], Sx, Sy, Sz) 213 when is_float(Ax), is_float(Ay), is_float(Az), 214 is_float(Sx), is_float(Sy), is_float(Sz) -> 215 Nx = Sx + (Ay-By)*(Az+Bz), 216 Ny = Sy + (Az-Bz)*(Ax+Bx), 217 Nz = Sz + (Ax-Bx)*(Ay+By), 218 SqrLen = Nx*Nx + Ny*Ny + Nz*Nz, 219 norm(SqrLen, Nx, Ny, Nz); 220normal_1([{Ax,Ay,Az}|[{Bx,By,Bz}|_]=T], First, Sx0, Sy0, Sz0) 221 when is_float(Ax), is_float(Ay), is_float(Az), 222 is_float(Sx0), is_float(Sy0), is_float(Sz0) -> 223 Sx = Sx0 + (Ay-By)*(Az+Bz), 224 Sy = Sy0 + (Az-Bz)*(Ax+Bx), 225 Sz = Sz0 + (Ax-Bx)*(Ay+By), 226 normal_1(T, First, Sx, Sy, Sz). 227 228%% average([{X,Y,Z}]) -> {Ax,Ay,Az} 229%% Average the given list of points. 230 231-spec average([vector()]) -> vector(). 232 233average([{V10,V11,V12},B]) -> 234 {V20,V21,V22} = B, 235 V0 = if 236 V10 =:= V20 -> V10; 237 is_float(V10) -> 0.5*(V10+V20) 238 end, 239 V1 = if 240 V11 =:= V21 -> V11; 241 is_float(V11) -> 0.5*(V11+V21) 242 end, 243 if 244 V12 =:= V22 -> {V0,V1,V12}; 245 is_float(V12) -> {V0,V1,0.5*(V12+V22)} 246 end; 247average([{V10,V11,V12}|T]=All) -> 248 average(T, V10, V11, V12, length(All)). 249 250-spec average(vector(), vector()) -> vector(). 251 252average({V10,V11,V12}, {V20,V21,V22}) -> 253 V0 = if 254 V10 =:= V20 -> V10; 255 is_float(V10) -> 0.5*(V10+V20) 256 end, 257 V1 = if 258 V11 =:= V21 -> V11; 259 is_float(V11) -> 0.5*(V11+V21) 260 end, 261 if 262 V12 =:= V22 -> {V0,V1,V12}; 263 is_float(V12) -> {V0,V1,0.5*(V12+V22)} 264 end. 265 266-spec average(vector(), vector(), vector(), vector()) -> vector(). 267 268average({V10,V11,V12}, {V20,V21,V22}, {V30,V31,V32}, {V40,V41,V42}) 269 when is_float(V10), is_float(V11), is_float(V12) -> 270 L = 0.25, 271 {L*(V10+V20+V30+V40),L*(V11+V21+V31+V41),L*(V12+V22+V32+V42)}. 272 273 274-spec area(vector(), vector(), vector()) -> float(). 275 276area({V10,V11,V12}, {V20,V21,V22}, {V30,V31,V32}) 277 when is_float(V10), is_float(V11), is_float(V12), 278 is_float(V20), is_float(V21), is_float(V22), 279 is_float(V30), is_float(V31), is_float(V32) -> 280 D10 = V10-V20, 281 D11 = V11-V21, 282 D12 = V12-V22, 283 D20 = V20-V30, 284 D21 = V21-V31, 285 D22 = V22-V32, 286 N0 = D11*D22-D12*D21, 287 N1 = D12*D20-D10*D22, 288 N2 = D10*D21-D11*D20, 289 math:sqrt(N0*N0+N1*N1+N2*N2)*0.5. 290 291%% Calculate plane coefficients from point {CX,CY,CZ} and normal {A,B,C} 292%% Use reference of : http://mathworld.wolfram.com/Plane.html 293-spec plane(Center::vector(), Normal::vector()) -> plane(). 294plane({CX,CY,CZ}, {A,B,C}) 295 when is_float(CX), is_float(CY), is_float(CZ), 296 is_float(A), is_float(A), is_float(A) -> 297 D = -A*CX-B*CY-C*CZ, 298 {{A,B,C},D}. 299 300-spec plane(point(),point(),point()) -> plane(). 301plane(P1, P2, P3) -> 302 plane(average([P1,P2,P3]), normal(P1,P2,P3)). 303 304-spec plane([point()]) -> plane(). 305plane(Polygon) -> 306 plane(average(Polygon), normal(Polygon)). 307 308%% Helper function used to sort points according to which side of a plane they are on. 309-spec plane_side(point(), plane()) -> -1|1. 310plane_side({X,Y,Z},{{A,B,C},D}) -> 311 Temp=A*X+B*Y+C*Z+D, 312 if Temp < 0.0000 -> -1; true -> 1 end. 313 314%% Using Coeff to calculate signed distance from point to plane. 315-spec plane_dist(point(), plane()) -> float(). 316plane_dist({X,Y,Z},{{A,B,C},D}) -> 317 (A*X+B*Y+C*Z+D)/math:sqrt(A*A+B*B+C*C). 318 319%% Dist from point P to line (A,B) 320-spec line_dist(point(), point(), point()) -> float(). 321line_dist(P,A,B) -> 322 math:sqrt(line_dist_sqr(P,A,B)). 323 324-spec line_dist_sqr(point(), point(), point()) -> float(). 325line_dist_sqr(P,A,B) -> 326 AB = sub(B, A), 327 AP = sub(P, A), 328 case dot(AP, AB) =< 0.0 of 329 true -> len_sqr(AP); 330 false -> 331 BP = sub(B,P), 332 case dot(BP, AB) =< 0.0 of 333 true -> len_sqr(BP); 334 false -> len_sqr(cross(AB,AP)) / len_sqr(AB) 335 end 336 end. 337 338%% Calculate the line segment PaPb that is the shortest route between 339%% two lines P1P2 and P3P4. Calculate also the values of mua and mub where 340%% Pa = P1 + mua (P2 - P1) 341%% Pb = P3 + mub (P4 - P3) 342%% Return FALSE if no solution exists. 343%% (from http://paulbourke.net/geometry/pointlineplane/) 344-define(EPS, 0.000001). 345-spec line_line_intersect(point(),point(),point(),point()) -> 346 false | {point(), point(), float(), float()}. 347line_line_intersect(P1,P2,P3,P4) -> 348 P13 = sub(P1,P3), 349 {X1,Y1,Z1} = P43 = sub(P4,P3), 350 {X0,Y0,Z0} = P21 = sub(P2,P1), 351 352 if X1 < ?EPS, Y1 < ?EPS, Z1 < ?EPS -> false; %% Point intersect? 353 X0 < ?EPS, Y0 < ?EPS, Z0 < ?EPS -> false; 354 true -> 355 D1343 = dot(P13, P43), 356 D4321 = dot(P43, P21), 357 D1321 = dot(P13, P21), 358 D4343 = dot(P43, P43), 359 D2121 = dot(P21, P21), 360 Denom = D2121 * D4343 - D4321 * D4321, 361 if Denom < ?EPS -> false; 362 true -> 363 Numer = D1343 * D4321 - D1321 * D4343, 364 Mua = Numer / Denom, 365 Mub = (D1343 + D4321 * Mua) / D4343, 366 {add_prod(P1,P21,Mua), 367 add_prod(P3,P43,Mub), 368 Mua, Mub} 369 end 370 end. 371 372-spec project_2d(point(), x|y|z) -> point(). 373project_2d({_X,Y,Z}, x) -> %% Project onto plane YZ 374 {Y,Z, 0.0}; 375project_2d({X,_Y,Z}, y) -> %% Project onto plane XZ 376 {X,Z, 0.0}; 377project_2d({X,Y,_Z}, z) -> %% Project onto plane XY 378 {X,Y, 0.0}. 379 380-spec largest_dir(point()) -> x|y|z. 381largest_dir({X,Y,Z}) -> 382 AX=abs(X), AY=abs(Y), AZ=abs(Z), 383 if AY < AZ, AX < AZ -> z; 384 AX < AY -> y; 385 true -> x 386 end. 387 388%% Should be removed and calls should be changed to e3d_bv instead. 389-spec bounding_box([vector()]) -> [vector()]. 390bounding_box(List) when is_list(List) -> 391 tuple_to_list(e3d_bv:box(List)). 392 393-spec degrees(vector(), vector()) -> float(). 394 395degrees(V0, V1) -> 396 Dot = e3d_vec:dot(V0,V1), 397 LenMul = e3d_vec:len(V0) * e3d_vec:len(V1), 398 %%% protect against divide-by-zero 399 RawCos = if (abs(LenMul) > 1.0E-30) -> Dot / LenMul; 400 true -> 1.0 401 end, 402 %%% protect against invalid cosine values 403 Cos = if 404 (RawCos > +1.0) -> +1.0; 405 (RawCos < -1.0) -> -1.0; 406 true -> RawCos 407 end, 408 math:acos(Cos) * (180.0 / math:pi()). 409 410 411-spec format(point()) -> io_lib:chars(). 412format({A,B,C}) -> 413 io_lib:format("{~.3f,~.3f,~.3f}",[A,B,C]). 414 415%%% 416%%% Internal functions. 417%%% 418 419add([{V10,V11,V12},{V20,V21,V22},{V30,V31,V32}|T], A0, A1, A2) 420 when is_float(V10), is_float(V11), is_float(V12), 421 is_float(V20), is_float(V21), is_float(V22), 422 is_float(V30), is_float(V31), is_float(V32), 423 is_float(A0), is_float(A1), is_float(A2) -> 424 add(T, A0+V10+V20+V30, A1+V11+V21+V31, A2+V12+V22+V32); 425add([{V10,V11,V12},{V20,V21,V22}|T], A0, A1, A2) 426 when is_float(V10), is_float(V11), is_float(V12), 427 is_float(V20), is_float(V21), is_float(V22), 428 is_float(A0), is_float(A1), is_float(A2) -> 429 add(T, A0+V10+V20, A1+V11+V21, A2+V12+V22); 430add([{V10,V11,V12}|T], A0, A1, A2) 431 when is_float(V10), is_float(V11), is_float(V12), 432 is_float(A0), is_float(A1), is_float(A2) -> 433 add(T, A0+V10, A1+V11, A2+V12); 434add([], A0, A1, A2) -> {A0,A1,A2}. 435 436sub(A0, A1, A2, [{V10,V11,V12}|T]) -> 437 sub(A0-V10, A1-V11, A2-V12, T); 438sub(A0, A1, A2, []) -> {A0,A1,A2}. 439 440norm(SqrLen, _, _, _) when SqrLen < 1.0E-16 -> 441 {0.0,0.0,0.0}; 442norm(SqrLen, V1, V2, V3) -> 443 D = math:sqrt(SqrLen), 444 try {V1/D,V2/D,V3/D} 445 catch 446 error:badarith -> {0.0,0.0,0.0} 447 end. 448 449average([{V10,V11,V12},{V20,V21,V22},{V30,V31,V32}|T], A0, A1, A2, L) 450 when is_float(V10), is_float(V11), is_float(V12), 451 is_float(V20), is_float(V21), is_float(V22), 452 is_float(V30), is_float(V31), is_float(V32), 453 is_float(A0), is_float(A1), is_float(A2) -> 454 average(T, A0+V10+V20+V30, A1+V11+V21+V31, A2+V12+V22+V32, L); 455average([{V10,V11,V12},{V20,V21,V22}|T], A0, A1, A2, L) 456 when is_float(V10), is_float(V11), is_float(V12), 457 is_float(V20), is_float(V21), is_float(V22), 458 is_float(A0), is_float(A1), is_float(A2) -> 459 average(T, A0+V10+V20, A1+V11+V21, A2+V12+V22, L); 460average([{V10,V11,V12}|T], A0, A1, A2, L) 461 when is_float(V10), is_float(V11), is_float(V12), 462 is_float(A0), is_float(A1), is_float(A2) -> 463 average(T, A0+V10, A1+V11, A2+V12, L); 464average([], A0, A1, A2, L0) -> 465 L = 1.0/float(L0), 466 {A0*L,A1*L,A2*L}. 467