Related
I have a rotation and I want to decompose it into a series of rotations around 3 orthogonal arbitrary axes. It's a bit like a generalisation of Euler decomposition where the rotations are not around the X, Y and Z axes
I've tried to find a closed form solution but not been successful so I have produced a numerical solution based on minimising the difference between the rotation I want and the product of 3 quaternions representing the 3 axes roations with the 3 angles being the unknowns. 'SimplexMinimize' is just an abstraction of the code to find the 3 angles that minimises the error.
double GSUtil::ThreeAxisDecomposition(const Quaternion &target, const Vector &ax1, const Vector &ax2, const Vector &ax3, double *ang1, double *ang2, double *ang3)
{
DataContainer data = {target, ax1, ax2, ax3};
VaraiablesContainer variables = {ang1, ang2, ang3};
error = SimplexMinimize(ThreeAxisDecompositionError, data, variables);
}
double GSUtil::ThreeAxisDecompositionError(const Quaternion &target, const Vector &ax1, const Vector &ax2, const Vector &ax3, double ang1, double ang2, double ang3)
{
Quaternion product = MakeQFromAxisAngle(ax3, ang3) * MakeQFromAxisAngle(ax2, ang2) * MakeQFromAxisAngle(ax1, ang1);
// now we need a distance metric between product and target. I could just calculate the angle between them:
// theta = acos(2?q1,q2?^2-1) where ?q1,q2? is the inner product (n1n2 + x1x2+ y1y2 + z1z2)
// but there are other quantities that will do a similar job in less time
// 1-(q1,q2)^2 should be faster to calculate and is 0 when they are identical and 1 when they are 180 degrees apart
double innerProduct = target.n * product.n + target.v.x * product.v.x + target.v.x * product.v.x + target.v.x * product.v.x;
double error = 1 - innerProduct * innerProduct;
return error;
}
It works (I think) but obviously it is quite slow. My feeling is there ought to be a closed form solution. At the very least there ought to be a gradient to the function so I can use a faster optimiser.
There is indeed a closed form solution. Since the axes form an orthonormal basis A (each axe is a column of the matrix), you can decompose a rotation R on the three axes by transforming R into the basis A and then do Euler Angle decomposition on the three main axes:
R = A*R'*A^t = A*X*Y*Z*A^t = (A*X*A^t)*(A*Y*A^t)*(A*Z*A^t)
This translates into the following algorithm:
Compute R' = A^t*R*A
Decompose R' into Euler Angles around main axes to obtain matrices X, Y, Z
Compute the three rotations around the given axes:
X' = A*X*A^t
Y' = A*Y*A^t
Z' = A*Y*A^t
As a reference, here's the Mathematica code I used to test my answer
(*Generate random axes and a rotation matrix for testing purposes*)
a = RotationMatrix[RandomReal[{0, \[Pi]}],
Normalize[RandomReal[{-1, 1}, 3]]];
t1 = RandomReal[{0, \[Pi]}];
t2 = RandomReal[{0, \[Pi]}];
t3 = RandomReal[{0, \[Pi]}];
r = RotationMatrix[t1, a[[All, 1]]].
RotationMatrix[t2, a[[All, 2]]].
RotationMatrix[t2, a[[All, 3]]];
(*Decompose rotation matrix 'r' into the axes of 'a'*)
rp = Transpose[a].r.a;
{a1, a2, a3} = EulerAngles[rp, {1, 2, 3}];
xp = a.RotationMatrix[a1, {1, 0, 0}].Transpose[a];
yp = a.RotationMatrix[a2, {0, 1, 0}].Transpose[a];
zp = a.RotationMatrix[a3, {0, 0, 1}].Transpose[a];
(*Test that the generated matrix is equal to 'r' (should give 0)*)
xp.yp.zp - r // MatrixForm
(*Test that the individual rotations preserve the axes (should give 0)*)
xp.a[[All, 1]] - a[[All, 1]]
yp.a[[All, 2]] - a[[All, 2]]
zp.a[[All, 3]] - a[[All, 3]]
I was doing the same thing in python and found #Gilles-PhilippePaillé 's answer really helpful although I had to tweak a couple of things, mostly getting the euler angles out in reverse. Thought I would add my python version here for reference anyway in case it helps anyone.
import numpy as np
from scipy.spatial.transform import Rotation
def normalise(v: np.ndarray) -> np.ndarray:
"""Normalise an array along its final dimension."""
return v / norm(v, axis=-1, keepdims=True)
# Generate random basis
A = Rotation.from_rotvec(normalise(np.random.random(3)) * np.random.rand() * np.pi).as_matrix()
# Generate random rotation matrix
t0 = np.random.rand() * np.pi
t1 = np.random.rand() * np.pi
t2 = np.random.rand() * np.pi
R = Rotation.from_rotvec(A[:, 0] * t0) * Rotation.from_rotvec(A[:, 1] * t1) * Rotation.from_rotvec(A[:, 2] * t2)
R = R.as_matrix()
# Decompose rotation matrix R into the axes of A
rp = Rotation.from_matrix(A.T # R # A)
a3, a2, a1 = rp.as_euler('zyx')
xp = A # Rotation.from_rotvec(a1 * np.array([1, 0, 0])).as_matrix() # A.T
yp = A # Rotation.from_rotvec(a2 * np.array([0, 1, 0])).as_matrix() # A.T
zp = A # Rotation.from_rotvec(a3 * np.array([0, 0, 1])).as_matrix() # A.T
# Test that the generated matrix is equal to 'r' (should give 0)
assert np.allclose(xp # yp # zp, R)
# Test that the individual rotations preserve the axes (should give 0)
assert np.allclose(xp # A[:, 0], A[:, 0])
assert np.allclose(yp # A[:, 1], A[:, 1])
assert np.allclose(zp # A[:, 2], A[:, 2])
If I have the coordinates of the points on the outline of an arbitrary 2D shape, how can I find the coordinates of points composing the outline of a stair step curve, which best represents the original outline, but only use a set of known coordinates (xi, i=1,...,n and yi, i=1,...,m). For example the original triangle is represented by the thick solid blue line. it's different from the matlab stairs function, if my understanding is correct.
matlab code will be nice, but in other language is also ok, algorithm is most important.Thanks.
I'll start by defining a set of sample data based on your plot. Assuming that the pixel centers are aligned at integer values (the convention MATLAB follows) and that the lower left corner is at (0.5, 0.5), here's the data we get:
vx = [1.5; 9.7; 3.7; 1.5]; % X values of triangle vertices
vy = [8.3; 6.0; 1.7; 8.3]; % Y values of triangle vertices
x = 1:10; % X pixel center coordinates
y = 1:9; % Y pixel center coordinates
Note that the vertex coordinates are ordered starting at the top left corner of the triangle and proceeding clockwise, repeating the first vertex at the end to close the polygon.
Getting the mask (the easy part):
There is an easy way to compute the dark gray mask if you have the Image Processing Toolbox: use poly2mask:
mask = poly2mask(vx, vy, numel(y), numel(x));
The algorithm this function uses is discussed here. However, if you'd like to use a pure MATLAB approach that requires no special toolboxes, you can use inpolygon instead:
[cx, cy] = meshgrid(x, y); % Generate a grid of x and y values
mask = inpolygon(cx, cy, vx, vy);
In this case, a pixel is included in the mask as long as its center point lies within the polygon. In this particular example these two approaches yield the same resulting mask, but they won't always due to the differences in their criteria for deciding if a pixel is included or not.
Getting the outline coordinates:
It's a little more involved to get the coordinates of the mask outline, ordered appropriately around the perimeter. To accomplish this, we can represent the mask as a series of vertices and triangular facets (using the triangulation function), then compute the free boundary (i.e. edges that are only present on one triangular facet):
% Create raw triangulation data:
[cx, cy] = meshgrid(x, y);
xTri = bsxfun(#plus, [0; 1; 1; 0], cx(mask).');
yTri = bsxfun(#plus, [0; 0; 1; 1], cy(mask).');
V = [xTri(:) yTri(:)];
F = reshape(bsxfun(#plus, [1; 2; 3; 1; 3; 4], 0:4:(4*nnz(mask)-4)), 3, []).';
% Trim triangulation data:
[V, ~, Vindex] = unique(V, 'rows');
V = V-0.5;
F = Vindex(F);
% Create triangulation and find free edge coordinates:
TR = triangulation(F, V);
freeEdges = freeBoundary(TR).';
xOutline = V(freeEdges(1, [1:end 1]), 1); % Ordered edge x coordinates
yOutline = V(freeEdges(1, [1:end 1]), 2); % Ordered edge y coordinates
And we can plot this like so:
imagesc(x, y, mask);
axis equal
set(gca, 'XLim', [min(x)-0.5 max(x)+0.5], ...
'YLim', [min(y)-0.5 max(y)+0.5], ...
'XTick', x, 'YTick', y, 'YDir', 'normal');
colormap([0.9 0.9 0.9; 0.6 0.6 0.6]);
hold on;
plot(xOutline, yOutline, 'b', 'LineWidth', 2);
plot(xOutline(1), yOutline(1), 'go', 'LineWidth', 2);
plot(vx, vy, 'r', 'LineWidth', 2);
The outline coordinates in xOutline and yOutline are ordered starting from the green circle going counter-clockwise around the mask region.
Seems you need any line rasterization algorithm (that gives coordinate of integer grid points approximating line segment).
Consider Bresenham algortihm or DDA one.
If I have the coordinates of the points on the outline of an arbitrary 2D shape, how can I find the coordinates of points composing the outline of a stair step curve, which best represents the original outline, but only use a set of known coordinates (xi, i=1,...,n and yi, i=1,...,m). For example the original triangle is represented by the thick solid blue line. it's different from the matlab stairs function, if my understanding is correct.
matlab code will be nice, but in other language is also ok, algorithm is most important.Thanks.
I'll start by defining a set of sample data based on your plot. Assuming that the pixel centers are aligned at integer values (the convention MATLAB follows) and that the lower left corner is at (0.5, 0.5), here's the data we get:
vx = [1.5; 9.7; 3.7; 1.5]; % X values of triangle vertices
vy = [8.3; 6.0; 1.7; 8.3]; % Y values of triangle vertices
x = 1:10; % X pixel center coordinates
y = 1:9; % Y pixel center coordinates
Note that the vertex coordinates are ordered starting at the top left corner of the triangle and proceeding clockwise, repeating the first vertex at the end to close the polygon.
Getting the mask (the easy part):
There is an easy way to compute the dark gray mask if you have the Image Processing Toolbox: use poly2mask:
mask = poly2mask(vx, vy, numel(y), numel(x));
The algorithm this function uses is discussed here. However, if you'd like to use a pure MATLAB approach that requires no special toolboxes, you can use inpolygon instead:
[cx, cy] = meshgrid(x, y); % Generate a grid of x and y values
mask = inpolygon(cx, cy, vx, vy);
In this case, a pixel is included in the mask as long as its center point lies within the polygon. In this particular example these two approaches yield the same resulting mask, but they won't always due to the differences in their criteria for deciding if a pixel is included or not.
Getting the outline coordinates:
It's a little more involved to get the coordinates of the mask outline, ordered appropriately around the perimeter. To accomplish this, we can represent the mask as a series of vertices and triangular facets (using the triangulation function), then compute the free boundary (i.e. edges that are only present on one triangular facet):
% Create raw triangulation data:
[cx, cy] = meshgrid(x, y);
xTri = bsxfun(#plus, [0; 1; 1; 0], cx(mask).');
yTri = bsxfun(#plus, [0; 0; 1; 1], cy(mask).');
V = [xTri(:) yTri(:)];
F = reshape(bsxfun(#plus, [1; 2; 3; 1; 3; 4], 0:4:(4*nnz(mask)-4)), 3, []).';
% Trim triangulation data:
[V, ~, Vindex] = unique(V, 'rows');
V = V-0.5;
F = Vindex(F);
% Create triangulation and find free edge coordinates:
TR = triangulation(F, V);
freeEdges = freeBoundary(TR).';
xOutline = V(freeEdges(1, [1:end 1]), 1); % Ordered edge x coordinates
yOutline = V(freeEdges(1, [1:end 1]), 2); % Ordered edge y coordinates
And we can plot this like so:
imagesc(x, y, mask);
axis equal
set(gca, 'XLim', [min(x)-0.5 max(x)+0.5], ...
'YLim', [min(y)-0.5 max(y)+0.5], ...
'XTick', x, 'YTick', y, 'YDir', 'normal');
colormap([0.9 0.9 0.9; 0.6 0.6 0.6]);
hold on;
plot(xOutline, yOutline, 'b', 'LineWidth', 2);
plot(xOutline(1), yOutline(1), 'go', 'LineWidth', 2);
plot(vx, vy, 'r', 'LineWidth', 2);
The outline coordinates in xOutline and yOutline are ordered starting from the green circle going counter-clockwise around the mask region.
Seems you need any line rasterization algorithm (that gives coordinate of integer grid points approximating line segment).
Consider Bresenham algortihm or DDA one.
What's the algorithm for computing a least squares plane in (x, y, z) space, given a set of 3D data points? In other words, if I had a bunch of points like (1, 2, 3), (4, 5, 6), (7, 8, 9), etc., how would one go about calculating the best fit plane f(x, y) = ax + by + c? What's the algorithm for getting a, b, and c out of a set of 3D points?
If you have n data points (x[i], y[i], z[i]), compute the 3x3 symmetric matrix A whose entries are:
sum_i x[i]*x[i], sum_i x[i]*y[i], sum_i x[i]
sum_i x[i]*y[i], sum_i y[i]*y[i], sum_i y[i]
sum_i x[i], sum_i y[i], n
Also compute the 3 element vector b:
{sum_i x[i]*z[i], sum_i y[i]*z[i], sum_i z[i]}
Then solve Ax = b for the given A and b. The three components of the solution vector are the coefficients to the least-square fit plane {a,b,c}.
Note that this is the "ordinary least squares" fit, which is appropriate only when z is expected to be a linear function of x and y. If you are looking more generally for a "best fit plane" in 3-space, you may want to learn about "geometric" least squares.
Note also that this will fail if your points are in a line, as your example points are.
The equation for a plane is: ax + by + c = z. So set up matrices like this with all your data:
x_0 y_0 1
A = x_1 y_1 1
...
x_n y_n 1
And
a
x = b
c
And
z_0
B = z_1
...
z_n
In other words: Ax = B. Now solve for x which are your coefficients. But since (I assume) you have more than 3 points, the system is over-determined so you need to use the left pseudo inverse. So the answer is:
a
b = (A^T A)^-1 A^T B
c
And here is some simple Python code with an example:
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import numpy as np
N_POINTS = 10
TARGET_X_SLOPE = 2
TARGET_y_SLOPE = 3
TARGET_OFFSET = 5
EXTENTS = 5
NOISE = 5
# create random data
xs = [np.random.uniform(2*EXTENTS)-EXTENTS for i in range(N_POINTS)]
ys = [np.random.uniform(2*EXTENTS)-EXTENTS for i in range(N_POINTS)]
zs = []
for i in range(N_POINTS):
zs.append(xs[i]*TARGET_X_SLOPE + \
ys[i]*TARGET_y_SLOPE + \
TARGET_OFFSET + np.random.normal(scale=NOISE))
# plot raw data
plt.figure()
ax = plt.subplot(111, projection='3d')
ax.scatter(xs, ys, zs, color='b')
# do fit
tmp_A = []
tmp_b = []
for i in range(len(xs)):
tmp_A.append([xs[i], ys[i], 1])
tmp_b.append(zs[i])
b = np.matrix(tmp_b).T
A = np.matrix(tmp_A)
fit = (A.T * A).I * A.T * b
errors = b - A * fit
residual = np.linalg.norm(errors)
print("solution:")
print("%f x + %f y + %f = z" % (fit[0], fit[1], fit[2]))
print("errors:")
print(errors)
print("residual:")
print(residual)
# plot plane
xlim = ax.get_xlim()
ylim = ax.get_ylim()
X,Y = np.meshgrid(np.arange(xlim[0], xlim[1]),
np.arange(ylim[0], ylim[1]))
Z = np.zeros(X.shape)
for r in range(X.shape[0]):
for c in range(X.shape[1]):
Z[r,c] = fit[0] * X[r,c] + fit[1] * Y[r,c] + fit[2]
ax.plot_wireframe(X,Y,Z, color='k')
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
plt.show()
unless someone tells me how to type equations here, let me just write down the final computations you have to do:
first, given points r_i \n \R, i=1..N, calculate the center of mass of all points:
r_G = \frac{\sum_{i=1}^N r_i}{N}
then, calculate the normal vector n, that together with the base vector r_G defines the plane by calculating the 3x3 matrix A as
A = \sum_{i=1}^N (r_i - r_G)(r_i - r_G)^T
with this matrix, the normal vector n is now given by the eigenvector of A corresponding to the minimal eigenvalue of A.
To find out about the eigenvector/eigenvalue pairs, use any linear algebra library of your choice.
This solution is based on the Rayleight-Ritz Theorem for the Hermitian matrix A.
See 'Least Squares Fitting of Data' by David Eberly for how I came up with this one to minimize the geometric fit (orthogonal distance from points to the plane).
bool Geom_utils::Fit_plane_direct(const arma::mat& pts_in, Plane& plane_out)
{
bool success(false);
int K(pts_in.n_cols);
if(pts_in.n_rows == 3 && K > 2) // check for bad sizing and indeterminate case
{
plane_out._p_3 = (1.0/static_cast<double>(K))*arma::sum(pts_in,1);
arma::mat A(pts_in);
A.each_col() -= plane_out._p_3; //[x1-p, x2-p, ..., xk-p]
arma::mat33 M(A*A.t());
arma::vec3 D;
arma::mat33 V;
if(arma::eig_sym(D,V,M))
{
// diagonalization succeeded
plane_out._n_3 = V.col(0); // in ascending order by default
if(plane_out._n_3(2) < 0)
{
plane_out._n_3 = -plane_out._n_3; // upward pointing
}
success = true;
}
}
return success;
}
Timed at 37 micro seconds fitting a plane to 1000 points (Windows 7, i7, 32bit program)
This reduces to the Total Least Squares problem, that can be solved using SVD decomposition.
C++ code using OpenCV:
float fitPlaneToSetOfPoints(const std::vector<cv::Point3f> &pts, cv::Point3f &p0, cv::Vec3f &nml) {
const int SCALAR_TYPE = CV_32F;
typedef float ScalarType;
// Calculate centroid
p0 = cv::Point3f(0,0,0);
for (int i = 0; i < pts.size(); ++i)
p0 = p0 + conv<cv::Vec3f>(pts[i]);
p0 *= 1.0/pts.size();
// Compose data matrix subtracting the centroid from each point
cv::Mat Q(pts.size(), 3, SCALAR_TYPE);
for (int i = 0; i < pts.size(); ++i) {
Q.at<ScalarType>(i,0) = pts[i].x - p0.x;
Q.at<ScalarType>(i,1) = pts[i].y - p0.y;
Q.at<ScalarType>(i,2) = pts[i].z - p0.z;
}
// Compute SVD decomposition and the Total Least Squares solution, which is the eigenvector corresponding to the least eigenvalue
cv::SVD svd(Q, cv::SVD::MODIFY_A|cv::SVD::FULL_UV);
nml = svd.vt.row(2);
// Calculate the actual RMS error
float err = 0;
for (int i = 0; i < pts.size(); ++i)
err += powf(nml.dot(pts[i] - p0), 2);
err = sqrtf(err / pts.size());
return err;
}
As with any least-squares approach, you proceed like this:
Before you start coding
Write down an equation for a plane in some parameterization, say 0 = ax + by + z + d in thee parameters (a, b, d).
Find an expression D(\vec{v};a, b, d) for the distance from an arbitrary point \vec{v}.
Write down the sum S = \sigma_i=0,n D^2(\vec{x}_i), and simplify until it is expressed in terms of simple sums of the components of v like \sigma v_x, \sigma v_y^2, \sigma v_x*v_z ...
Write down the per parameter minimization expressions dS/dx_0 = 0, dS/dy_0 = 0 ... which gives you a set of three equations in three parameters and the sums from the previous step.
Solve this set of equations for the parameters.
(or for simple cases, just look up the form). Using a symbolic algebra package (like Mathematica) could make you life much easier.
The coding
Write code to form the needed sums and find the parameters from the last set above.
Alternatives
Note that if you actually had only three points, you'd be better just finding the plane that goes through them.
Also, if the analytic solution in unfeasible (not the case for a plane, but possible in general) you can do steps 1 and 2, and use a Monte Carlo minimizer on the sum in step 3.
CGAL::linear_least_squares_fitting_3
Function linear_least_squares_fitting_3 computes the best fitting 3D
line or plane (in the least squares sense) of a set of 3D objects such
as points, segments, triangles, spheres, balls, cuboids or tetrahedra.
http://www.cgal.org/Manual/latest/doc_html/cgal_manual/Principal_component_analysis_ref/Function_linear_least_squares_fitting_3.html
It sounds like all you want to do is linear regression with 2 regressors. The wikipedia page on the subject should tell you all you need to know and then some.
All you'll have to do is to solve the system of equations.
If those are your points:
(1, 2, 3), (4, 5, 6), (7, 8, 9)
That gives you the equations:
3=a*1 + b*2 + c
6=a*4 + b*5 + c
9=a*7 + b*8 + c
So your question actually should be: How do I solve a system of equations?
Therefore I recommend reading this SO question.
If I've misunderstood your question let us know.
EDIT:
Ignore my answer as you probably meant something else.
We first present a linear least-squares plane fitting method that minimizes the residuals between the estimated normal vector and provided points.
Recall that the equation for a plane passing through origin is Ax + By + Cz = 0, where (x, y, z) can be any point on the plane and (A, B, C) is the normal vector perpendicular to this plane.
The equation for a general plane (that may or may not pass through origin) is Ax + By + Cz + D = 0, where the additional coefficient D represents how far the plane is away from the origin, along the direction of the normal vector of the plane. [Note that in this equation (A, B, C) forms a unit normal vector.]
Now, we can apply a trick here and fit the plane using only provided point coordinates. Divide both sides by D and rearrange this term to the right-hand side. This leads to A/D x + B/D y + C/D z = -1. [Note that in this equation (A/D, B/D, C/D) forms a normal vector with length 1/D.]
We can set up a system of linear equations accordingly, and then solve it by an Eigen solver in C++ as follows.
// Example for 5 points
Eigen::Matrix<double, 5, 3> matA; // row: 5 points; column: xyz coordinates
Eigen::Matrix<double, 5, 1> matB = -1 * Eigen::Matrix<double, 5, 1>::Ones();
// Find the plane normal
Eigen::Vector3d normal = matA.colPivHouseholderQr().solve(matB);
// Check if the fitting is healthy
double D = 1 / normal.norm();
normal.normalize(); // normal is a unit vector from now on
bool planeValid = true;
for (int i = 0; i < 5; ++i) { // compare Ax + By + Cz + D with 0.2 (ideally Ax + By + Cz + D = 0)
if ( fabs( normal(0)*matA(i, 0) + normal(1)*matA(i, 1) + normal(2)*matA(i, 2) + D) > 0.2) {
planeValid = false; // 0.2 is an experimental threshold; can be tuned
break;
}
}
We then discuss its equivalence to the typical SVD-based method and their comparison.
The aforementioned linear least-squares (LLS) method fits the general plane equation Ax + By + Cz + D = 0, whereas the SVD-based method replaces D with D = - (Ax0 + By0 + Cz0) and fits the plane equation A(x-x0) + B(y-y0) + C(z-z0) = 0, where (x0, y0, z0) is the mean of all points that serves as the origin of the new local coordinate frame.
Comparison between two methods:
The LLS fitting method is much faster than the SVD-based method, and is suitable for use when points are known to be roughly in a plane shape.
The SVD-based method is more numerically stable when the plane is far away from origin, because the LLS method would require more digits after decimal to be stored and processed in such cases.
The LLS method can detect outliers by checking the dot product residual between each point and the estimated normal vector, whereas the SVD-based method can detect outliers by checking if the smallest eigenvalue of the covariance matrix is significantly smaller than the two larger eigenvalues (i.e. checking the shape of the covariance matrix).
We finally provide a test case in C++ and MATLAB.
// Test case in C++ (using LLS fitting method)
matA(0,0) = 5.4637; matA(0,1) = 10.3354; matA(0,2) = 2.7203;
matA(1,0) = 5.8038; matA(1,1) = 10.2393; matA(1,2) = 2.7354;
matA(2,0) = 5.8565; matA(2,1) = 10.2520; matA(2,2) = 2.3138;
matA(3,0) = 6.0405; matA(3,1) = 10.1836; matA(3,2) = 2.3218;
matA(4,0) = 5.5537; matA(4,1) = 10.3349; matA(4,2) = 1.8796;
// With this sample data, LLS fitting method can produce the following result
// fitted normal vector = (-0.0231143, -0.0838307, -0.00266429)
// unit normal vector = (-0.265682, -0.963574, -0.0306241)
// D = 11.4943
% Test case in MATLAB (using SVD-based method)
points = [5.4637 10.3354 2.7203;
5.8038 10.2393 2.7354;
5.8565 10.2520 2.3138;
6.0405 10.1836 2.3218;
5.5537 10.3349 1.8796]
covariance = cov(points)
[V, D] = eig(covariance)
normal = V(:, 1) % pick the eigenvector that corresponds to the smallest eigenvalue
% normal = (0.2655, 0.9636, 0.0306)
What are the best algorithms (and explanations) for representing and rotating the pieces of a tetris game? I always find the piece rotation and representation schemes confusing.
Most tetris games seem to use a naive "remake the array of blocks" at each rotation:
http://www.codeplex.com/Project/ProjectDirectory.aspx?ProjectSearchText=tetris
However, some use pre-built encoded numbers and bit shifting to represent each piece:
http://www.codeplex.com/wintris
Is there a method to do this using mathematics (not sure that would work on a cell based board)?
When I was trying to figure out how rotations would work for my tetris game, this was the first question that I found on stack overflow. Even though this question is old, I think my input will help others trying to figure this out algorithmically. First, I disagree that hard coding each piece and rotation will be easier. Gamecat's answer is correct, but I wanted to elaborate on it. Here are the steps I used to solve the rotation problem in Java.
For each shape, determine where its origin will be. I used the points on the diagram from this page to assign my origin points. Keep in mind that, depending on your implementation, you may have to modify the origin every time the piece is moved by the user.
Rotation assumes the origin is located at point (0,0), so you will have to translate each block before it can be rotated. For example, suppose your origin is currently at point (4, 5). This means that before the shape can be rotated, each block must be translated -4 in the x-coordinate and -5 in the y-coordinate to be relative to (0,0).
In Java, a typical coordinate plane starts with point (0,0) in the upper left most corner and then increases to the right and down. To compensate for this in my implementation, I multiplied each point by -1 before rotation.
Here are the formulae I used to figure out the new x and y coordinate after a counter-clockwise rotation. For more information on this, I would check out the Wikipedia page on Rotation Matrix. x' and y' are the new coordinates:
x' = x * cos(PI/2) - y * sin(PI/2) and y' = x * sin(PI/2) + y * cos(PI/2)
.
For the last step, I just went through steps 2 and 3 in reverse order. So I multiplied my results by -1 again and then translated the blocks back to their original coordinates.
Here is the code that worked for me (in Java) to get an idea of how to do it in your language:
public synchronized void rotateLeft(){
Point[] rotatedCoordinates = new Point[MAX_COORDINATES];
for(int i = 0; i < MAX_COORDINATES; i++){
// Translates current coordinate to be relative to (0,0)
Point translationCoordinate = new Point(coordinates[i].x - origin.x, coordinates[i].y - origin.y);
// Java coordinates start at 0 and increase as a point moves down, so
// multiply by -1 to reverse
translationCoordinate.y *= -1;
// Clone coordinates, so I can use translation coordinates
// in upcoming calculation
rotatedCoordinates[i] = (Point)translationCoordinate.clone();
// May need to round results after rotation
rotatedCoordinates[i].x = (int)Math.round(translationCoordinate.x * Math.cos(Math.PI/2) - translationCoordinate.y * Math.sin(Math.PI/2));
rotatedCoordinates[i].y = (int)Math.round(translationCoordinate.x * Math.sin(Math.PI/2) + translationCoordinate.y * Math.cos(Math.PI/2));
// Multiply y-coordinate by -1 again
rotatedCoordinates[i].y *= -1;
// Translate to get new coordinates relative to
// original origin
rotatedCoordinates[i].x += origin.x;
rotatedCoordinates[i].y += origin.y;
// Erase the old coordinates by making them black
matrix.fillCell(coordinates[i].x, coordinates[i].y, Color.black);
}
// Set new coordinates to be drawn on screen
setCoordinates(rotatedCoordinates.clone());
}
This method is all that is needed to rotate your shape to the left, which turns out to be much smaller (depending on your language) than defining each rotation for every shape.
There is a limited amount of shapes, so I would use a fixed table and no calculation. That saves time.
But there are rotation algorithms.
Chose a centerpoint and rotate pi/2.
If a block of a piece starts at (1,2) it moves clockwise to (2,-1) and (-1,-2) and (-1, 2).
Apply this for each block and the piece is rotated.
Each x is the previous y and each y - the previous x. Which gives the following matrix:
[ 0 1 ]
[ -1 0 ]
For counterclockwise rotation, use:
[ 0 -1 ]
[ 1 0 ]
This is how I did it recently in a jQuery/CSS based tetris game.
Work out the centre of the block (to be used as a pivot point), i.e. the centre of the block shape.
Call that (px, py).
Each brick that makes up the block shape will rotate around that point.
For each brick, you can apply the following calculation...
Where each brick's width and height is q, the brick's current location (of the upper left corner) is (x1, y1) and the new brick location is (x2, y2):
x2 = (y1 + px - py)
y2 = (px + py - x1 - q)
To rotate the opposite direction:
x2 = (px + py - y1 - q)
y2 = (x1 + py - px)
This calculation is based on a 2D affine matrix transformation.
If you are interested in how I got to this let me know.
Personally I've always just represented the rotations by hand - with very few shapes, it's easy to code that way. Basically I had (as pseudo-code)
class Shape
{
Color color;
ShapeRotation[] rotations;
}
class ShapeRotation
{
Point[4] points;
}
class Point
{
int x, y;
}
At least conceptually - a multi-dimensional array of points directly in shape would do the trick too :)
You can rotate a matrix only by applying mathematical operations to it. If you have a matrix, say:
Mat A = [1,1,1]
[0,0,1]
[0,0,0]
To rotate it, multiply it by its transpose and then by this matrix ([I]dentity [H]orizontaly [M]irrored):
IHM(A) = [0,0,1]
[0,1,0]
[1,0,0]
Then you'll have:
Mat Rotation = Trn(A)*IHM(A) = [1,0,0]*[0,0,1] = [0,0,1]
[1,0,0] [0,1,0] = [0,0,1]
[1,1,0] [1,0,0] = [0,1,1]
Note: Center of rotation will be the center of the matrix, in this case at (2,2).
Representation
Represent each piece in the minimum matrix where 1's represent spaces occupied by the tetriminoe and 0's represent empty space. Example:
originalMatrix =
[0, 0, 1]
[1, 1, 1]
Rotation Formula
clockwise90DegreesRotatedMatrix = reverseTheOrderOfColumns(Transpose(originalMatrix))
anticlockwise90DegreesRotatedMatrix = reverseTheOrderOfRows(Transpose(originalMatrix))
Illustration
originalMatrix =
x y z
a[0, 0, 1]
b[1, 1, 1]
transposed = transpose(originalMatrix)
a b
x[0, 1]
y[0, 1]
z[1, 1]
counterClockwise90DegreesRotated = reverseTheOrderOfRows(transposed)
a b
z[1, 1]
y[0, 1]
x[0, 1]
clockwise90DegreesRotated = reverseTheOrderOfColumns(transposed)
b a
x[1, 0]
y[1, 0]
z[1, 1]
Since there are only 4 possible orientations for each shape, why not use an array of states for the shape and rotating CW or CCW simply increments or decrements the index of the shape state (with wraparound for the index)? I would think that might be quicker than performing rotation calculations and whatnot.
I derived a rotation algorithm from matrix rotations here. To sum it up: If you have a list of coordinates for all cells that make up the block, e.g. [(0, 1), (1, 1), (2, 1), (3, 1)] or [(1, 0), (0, 1), (1, 1), (2, 1)]:
0123 012
0.... 0.#.
1#### or 1###
2.... 2...
3....
you can calculate the new coordinates using
x_new = y_old
y_new = 1 - (x_old - (me - 2))
for clockwise rotation and
x_new = 1 - (y_old - (me - 2))
y_new = x_old
for counter-clockwise rotation. me is the maximum extent of the block, i.e. 4 for I-blocks, 2 for O-blocks and 3 for all other blocks.
If you're doing this in python, cell-based instead of coordinate pairs it's very simple to rotate a nested list.
rotate = lambda tetrad: zip(*tetrad[::-1])
# S Tetrad
tetrad = rotate([[0,0,0,0], [0,0,0,0], [0,1,1,0], [1,1,0,0]])
If we assume that the central square of the tetromino has coordinates (x0, y0) which remains unchanged then the rotation of the other 3 squares in Java will look like this:
private void rotateClockwise()
{
if(rotatable > 0) //We don't rotate tetromino O. It doesn't have central square.
{
int i = y1 - y0;
y1 = (y0 + x1) - x0;
x1 = x0 - i;
i = y2 - y0;
y2 = (y0 + x2) - x0;
x2 = x0 - i;
i = y3 - y0;
y3 = (y0 + x3) - x0;
x3 = x0 - i;
}
}
private void rotateCounterClockwise()
{
if(rotatable > 0)
{
int i = y1 - y0;
y1 = (y0 - x1) + x0;
x1 = x0 + i;
i = y2 - y0;
y2 = (y0 - x2) + x0;
x2 = x0 + i;
i = y3 - y0;
y3 = (y0 - x3) + x0;
x3 = x0 + i;
}
}
for 3x3 sized tetris pieces
flip x and y of your piece
then swap the outer columns
that's what I figured out some time
I have used a shape position and set of four coordinates for the four points in all the shapes. Since it's in 2D space, you can easy apply a 2D rotational matrice to the points.
The points are divs so their css class is turned from off to on. (this is after clearing the css class of where they were last turn.)
If array size is 3*3 ,than the simplest way to rotate it for example in anti-clockwise direction is:
oldShapeMap[3][3] = {{1,1,0},
{0,1,0},
{0,1,1}};
bool newShapeMap[3][3] = {0};
int gridSize = 3;
for(int i=0;i<gridSize;i++)
for(int j=0;j<gridSize;j++)
newShapeMap[i][j] = oldShapeMap[j][(gridSize-1) - i];
/*newShapeMap now contain:
{{0,0,1},
{1,1,1},
{1,0,0}};
*/
Python:
pieces = [
[(0,0),(0,1),(0,2),(0,3)],
[(0,0),(0,1),(1,0),(1,1)],
[(1,0),(0,1),(1,1),(1,2)],
[(0,0),(0,1),(1,0),(2,0)],
[(0,0),(0,1),(1,1),(2,1)],
[(0,1),(1,0),(1,1),(2,0)]
]
def get_piece_dimensions(piece):
max_r = max_c = 0
for point in piece:
max_r = max(max_r, point[0])
max_c = max(max_c, point[1])
return max_r, max_c
def rotate_piece(piece):
max_r, max_c = get_piece_dimensions(piece)
new_piece = []
for r in range(max_r+1):
for c in range(max_c+1):
if (r,c) in piece:
new_piece.append((c, max_r-r))
return new_piece
In Ruby, at least, you can actually use matrices. Represent your piece shapes as nested arrays of arrays like [[0,1],[0,2],[0,3]]
require 'matrix'
shape = shape.map{|arr|(Matrix[arr] * Matrix[[0,-1],[1,0]]).to_a.flatten}
However, I agree that hard-coding the shapes is feasible since there are 7 shapes and 4 states for each = 28 lines and it will never be any more than that.
For more on this see my blog post at
https://content.pivotal.io/blog/the-simplest-thing-that-could-possibly-work-in-tetris and a completely working implementation (with minor bugs) at https://github.com/andrewfader/Tetronimo
In Java:
private static char[][] rotateMatrix(char[][] m) {
final int h = m.length;
final int w = m[0].length;
final char[][] t = new char[h][w];
for(int y = 0; y < h; y++) {
for(int x = 0; x < w; x++) {
t[w - x - 1][y] = m[y][x];
}
}
return t;
}
A simple Tetris implementation as a single-page application in Java:
https://github.com/vadimv/rsp-tetris