Permalink
Browse files

Add to/from euler_rad functions to e3d_q and e3d_mat

  • Loading branch information...
1 parent e65818b commit 54f64e32c018d5143ac525a78cc7248d909d8a5b @dgud dgud committed Feb 8, 2012
Showing with 97 additions and 9 deletions.
  1. +26 −1 e3d/e3d_mat.erl
  2. +71 −8 e3d/e3d_q.erl
View
@@ -16,7 +16,8 @@
-export([identity/0,is_identity/1,determinant/1,print/1,
compress/1,expand/1,
translate/1,translate/3,scale/1,scale/3,
- rotate/2,rotate_to_z/1,rotate_s_to_t/2,
+ rotate/2,rotate_from_euler_rad/1, rotate_from_euler_rad/3,
+ rotate_to_z/1,rotate_s_to_t/2,
project_to_plane/1,
transpose/1,invert/1,
add/2,mul/2,mul_point/2,mul_vector/2,eigenv3/1]).
@@ -118,6 +119,30 @@ rotate(A0, {X,Y,Z}) when is_float(X), is_float(Y), is_float(Z) ->
U3+NegS*U3+C3, U6+NegS*U6+C6, U9+S*(1.0-U9),
0.0,0.0,0.0}.
+%% rotate_from_euler_rad is a shortcut for
+%% Rad2deg = 180/math:pi(),
+%% Mx = e3d_mat:rotate(Rx * Rad2deg, {1.0, 0.0, 0.0}),
+%% My = e3d_mat:rotate(Ry * Rad2deg, {0.0, 1.0, 0.0}),
+%% Mz = e3d_mat:rotate(Rz * Rad2deg, {0.0, 0.0, 1.0}),
+%% Rot = e3d_mat:mul(Mz, e3d_mat:mul(My,Mx)),
+-spec rotate_from_euler_rad(Vector::e3d_vector()) -> e3d_compact_matrix().
+rotate_from_euler_rad({X,Y,Z}) ->
+ rotate_from_euler_rad(X,Y,Z).
+
+-spec rotate_from_euler_rad(X::number(), Y::number(), Z::number()) ->
+ e3d_compact_matrix().
+rotate_from_euler_rad(Rx,Ry,Rz) ->
+ Cz = math:cos(Rz), Sz = math:sin(Rz),
+ Cy = math:cos(Ry), Sy = math:sin(Ry),
+ Cx = math:cos(Rx), Sx = math:sin(Rx),
+ Sxsy=Sx*Sy, Cxsy=Cx*Sy,
+ TZ = 0.0,
+ {Cy*Cz, Cy*Sz, -Sy,
+ Sxsy*Cz-Cx*Sz, Sxsy*Sz+Cx*Cz, Sx*Cy,
+ Cxsy*Cz+Sx*Sz, Cxsy*Sz-Sx*Cz, Cx*Cy,
+ TZ, TZ, TZ}.
+
+
%% Project to plane perpendicular to vector Vec.
-spec project_to_plane(Vector::e3d_vector()) -> e3d_compact_matrix().
View
@@ -22,8 +22,9 @@
add/2, scale/2, slerp/3,
magnitude/1, conjugate/1,
to_rotation_matrix/1, from_rotation_matrix/1,
- to_angle_axis/1, from_angle_axis/2,
- from_vec/1,
+ to_angle_axis/1, from_angle_axis/2, from_angle_axis_rad/2,
+ from_euler_rad/1, from_euler_rad/3, to_euler_rad/1,
+ from_compact/1, to_compact/1,
rotate_s_to_t/2,
vec_rotate/2]).
@@ -35,8 +36,12 @@ identity() ->
magnitude({{Qx,Qy,Qz},Qw})
when is_float(Qx),is_float(Qy),is_float(Qz),is_float(Qw) ->
- math:sqrt(Qx*Qx+Qy*Qy+Qz*Qz+Qw*Qw).
-
+ MagSqr = Qx*Qx+Qy*Qy+Qz*Qz+Qw*Qw,
+ if MagSqr > 0.99999, MagSqr < 1.00001 ->
+ 1.0;
+ true ->
+ math:sqrt(MagSqr)
+ end.
conjugate({{Qx,Qy,Qz},Qw})
when is_float(Qx), is_float(Qy), is_float(Qz), is_float(Qw) ->
{{-Qx,-Qy,-Qz},Qw}.
@@ -47,9 +52,11 @@ inverse(Q) ->
norm(Q = {{Qx,Qy,Qz},Qw})
when is_float(Qx),is_float(Qy),is_float(Qz),is_float(Qw) ->
M = magnitude(Q),
- case catch {{Qx/M,Qy/M,Qz/M},Qw/M} of
- {'EXIT', _} -> {{0.0,0.0,0.0},0.0};
- R -> R
+ if M =:= 1.0 -> Q;
+ M < 0.00001 -> {{0.0,0.0,0.0},0.0};
+ true ->
+ IM = 1/M,
+ {{Qx*IM,Qy*IM,Qz*IM},Qw*IM}
end.
add({{X1,Y1,Z1},W1}, {{X2,Y2,Z2},W2})
@@ -139,6 +146,49 @@ from_rotation_matrix({M0, M4, M8,
from_rotation_matrix(M) when size(M) =:= 16 ->
from_rotation_matrix(e3d_mat:compress(M)).
+%% From/to euler
+%% From http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm
+%% Note: We rotate X first then Y and last Z as in:
+%% RX = e3d_q:from_angle_axis_rad(X, ?X),
+%% RY = e3d_q:from_angle_axis_rad(Y, ?Y),
+%% RZ = e3d_q:from_angle_axis_rad(Z, ?Z),
+%% e3d_q:mul(RZ, e3d_q:mul(RY, RX)).
+%%
+from_euler_rad({X, Y, Z}) ->
+ from_euler_rad(X, Y, Z).
+
+from_euler_rad(RX, RY, RZ) ->
+ RX2 = RX*0.5, RY2 = RY*0.5, RZ2 = RZ*0.5,
+ CX = math:cos(RX2),
+ SX = math:sin(RX2),
+ CY = math:cos(RY2),
+ SY = math:sin(RY2),
+ CZ = math:cos(RZ2),
+ SZ = math:sin(RZ2),
+ CXCY = CX*CY, CXSY = CX*SY,
+ SXSY = SX*SY, SXCY = SX*CY,
+ W = CXCY*CZ + SXSY*SZ,
+ X = SXCY*CZ - CXSY*SZ,
+ Y = CXSY*CZ + SXCY*SZ,
+ Z = CXCY*SZ - SXSY*CZ,
+ {{X,Y,Z},W}.
+
+to_euler_rad({{P1,P2,P3}, P0}) ->
+ P1P1 = P1*P1, P2P2 = P2*P2, P3P3 = P3*P3, P0P0 = P0*P0,
+ Unit = P1P1+P2P2+P3P3+P0P0,
+ Test = P0*P2 - P1*P3,
+ Limit = 0.4999*Unit,
+ if Test > Limit -> %% singularity at north pole
+ {math:atan2(P1,P0), math:pi()/2, 0.0};
+ Test < -Limit -> %% singularity at south pole
+ {math:atan2(P1,P0), -math:pi()/2, 0.0};
+ true ->
+ RX = math:atan2(2.0*(P0*P1+P2*P3), 1.0-2.0*(P1P1+P2P2)),
+ RY = math:asin(2.0*Test),
+ RZ = math:atan2(2.0*(P0*P3+P1*P2), 1.0-2.0*(P2P2+P3P3)),
+ {RX,RY,RZ}
+ end.
+
%% The Axis must be a unit-length vector.
from_angle_axis(Angle, Axis) ->
HalfAngle = Angle*(math:pi()/180.0/2.0),
@@ -147,6 +197,13 @@ from_angle_axis(Angle, Axis) ->
{X,Y,Z} = Axis,
{{X*Sin,Y*Sin,Z*Sin},Cos}.
+from_angle_axis_rad(Angle, Axis) ->
+ HalfAngle = Angle*0.5,
+ Sin = math:sin(HalfAngle),
+ Cos = math:cos(HalfAngle),
+ {X,Y,Z} = Axis,
+ {{X*Sin,Y*Sin,Z*Sin},Cos}.
+
to_angle_axis(Q) ->
{{Qx,Qy,Qz},Qw} = norm(Q),
Cos = Qw,
@@ -159,13 +216,19 @@ to_angle_axis(Q) ->
{Angle,{Qx/Sin,Qy/Sin,Qz/Sin}}.
%% The Axis must be a unit-length vector.
-from_vec(R={Rx,Ry,Rz}) ->
+from_compact(R={Rx,Ry,Rz}) ->
T = 1.0 - (Rx * Rx) - (Ry * Ry) - (Rz * Rz),
case T < 0.0 of
true -> {R, 0.0};
false -> {R, -math:sqrt(T)}
end.
+to_compact(Q) ->
+ case norm(Q) of
+ {Vec, W} when W < 0.0 -> Vec;
+ {{Rx,Ry,Rz}, _} -> {-Rx,-Ry,-Rz}
+ end.
+
%% vec_rotate(Vec, Q)
%% Rotate a vector or point using quaternion Q.
vec_rotate({X2,Y2,Z2}, {{X1,Y1,Z1},W1})

0 comments on commit 54f64e3

Please sign in to comment.