Orthonormalize a Rotation Matrix

If you use a 3×3 R matrix to store the result of the multiplication of a series of rotation transformations, it could be the case that sometimes you end up with a matrix that is not orthonormal (i.e. det(R) != 1 and R.inv(R) != eye).

In such cases, you need to re-orthonormalize the rotation matrix, which can be done in either of the two following ways:

  1. Use the SVD decomposition as follows (MATLAB syntax):
  2. Alternatively, you can express the rotation matrix as a quaternion, normalize the quaternion, then convert the quaternion back to the rotation matrix form. In MATLAB, you could do this:

    Note that the above syntax requires MATLAB’s robotics toolbox.

Computing the Distance Between a 3D Point and a Plücker Line

In order to solve an optimization problem with the goal of reducing the distance between a bunch of 3D points and lines, I was looking for the correct way of finding the distance between 3D points and a Plucker line representation.

The Plucker line \(L\) passing through two lines \(A\) and \(B\) is defined as \(L = AB^T – BA^T\) (for more details refer to [1]). After a lot of looking, I found that there is a simple method for finding this distance in [2]. A direct quote from the paper:


A Plucker line \(L = (n, m)\) is described by a unit vector \(n\) and a
moment \(m\). This line representation allows to conveniently determine
the distance of a 3D point \(X\) to the line

$$d(X, L) = ||X \times n – m||_2$$

where \(\times\) denotes a cross product.


[1] Hartley, Richard, and Andrew Zisserman. Multiple view geometry in computer vision. Cambridge university press, 2003.

[2] Brox, Thomas, et al. “Combined region and motion-based 3D tracking of rigid and articulated objects.” IEEE Transactions on Pattern Analysis and Machine Intelligence 32.3 (2010): 402-415.

CGAL Point in Polyhedron Algorithm

The “point in polygon” or “point in polyhedron” is a classic computer graphics problem. The goal is to determine whether a given point is inside a polygon (in 2D) or a polyhedron (in 3D).

One solution to the problem is shooting a ray originating from the said point to an arbitrary direction and determine the number of intersections of the ray with the polygon or polyhedron. If the ray intersects the shape an odd number of times, then the point is inside the shape. Otherwise it is outside the shape.

There are problems associated with this approach. An important edge case is when the point is on the surface of the shape. Also, it is commonly advisable to shoot multiple rays instead of just one, and then do a majority voting.

Luckily, in CGAL the point in polyhedron test is very simple and you don’t have to worry about the edge cases! You can do the test using the Side_of_triangle_mesh class. A code snippet is shown below:


You can also determine whether the point is on the surface of the polyhedron or not.

3D Line Fitting in 5 Easy Steps with SVD

Least squares fit is used for 2D line fitting. In 3D space, the line is called 3D Orthogonal Distance Regression (ODR) line. The line can be easily found in 3D using SVD (singular value decomposition).

Assuming that we have a bunch of 3D points (x0, y0, z0) to (xn, yn, zn), the algorithm (in MATLAB) is as follows:


Intersection of a Ray and a Line Segment in 3D

This page contains methods for performing various intersection tests. Although it does not have an entry for ray vs. line segment intersection, I tried the suggested ray vs. ray intersection test (page 782 of Real-Time Rendering 3rd Edition) and it did not work in my case.

I looked around quite a bit and based on an adaptation of this answer, I finally found a method that works fine. Given a ray (with a start point, an end point and direction) and a line segment with the defined start and end points, we perform the 3D line intersection test. However, note that in various graphics APIs, there is always some error when converting screen points to lines (because there is no 1-1 mapping from screen pixels to exact 3D points in space). Therefore, when performing intersection tests, some degree of tolerance must be considered. This is especially important in the early rejection step of the intersection test algorithm. The early rejection test, checks to see whether the two 3D lines are co-planer. If they aren’t, then no intersection will be reported.

After finding the intersection point, we must check and see if this point lies between the start and end points of the line segment. This can be done by comparing the length of the line segment with the sum of distances of the intersection point from the start point and end point of the line segment respectively. If the length is “almost” equal, then it means that the original ray will intersect with the line segment.

Here is the pseudo-code of the described process (adapted from the original answer):