We Keep Coding

Why is angle of point (X, Y point) w.r.t origin after rotation is different than before rotation in Python?

I have two doubts. I have X and Y coordinates which I have listed below. I also have plotted coordinates as shown in picture below.
x = [0, 1, 1, 0, 0, 1]
y = [1, 1, 2, 2, 3, 3]
Now, I have decided to rotate the geometry in clockwise. Therefore, I have rotate all points at 45 degree (+ve) using below formula.
x_dash = x[i] * math.cos(theta) + y[i] * math.sin(theta)
y_dash = -x[i] * math.sin(theta) + y[i] * math.cos(theta)
After using above code (Formula), I got below results which shows new coordinate points after 45 degree clockwise rotation and after plotting, I got below plot.
x_dash = [0.8509035245341184, 1.3762255133518482, 2.2271290378859665, 1.7018070490682369, 2.552710573602355, 3.078032562420085]
y_dash = [0.5253219888177297, -0.3255815357163887, 0.19974045310134103, 1.0506439776354595, 1.575965966453189, 0.7250624419190707]
My questions:
(1) if I take two coordinates (X and Y) of one point and if I find an angle using
theta = np.degrees(np.arctan2(y, x)),
I did not get 45 degree.
For example:
np.degrees(np.arctan2(0.5253219888177297, 0.8509035245341184))
Result: 31.68992191129556
However, when I found an angle of 1st point before rotation. np.degrees(np.arctan2(1, 0)), I got 90.0.
I would like to know the reason that why there is a differece between the angle of same point before and after the rotation.
(2) If I have a rotated geometry like in the 2nd picture and I do not know the angle of rotation. What should I do make that geometry without rotation (like in the first picture).
Kinldy help me with these questions.

By default, math.sin() and math.cos() assumes that the arguments are in radians. So, the code considers the angle of rotation as 45 radians, and not 45 degrees.
You can define theta as:
theta = numpy.radians(45)
Hope this clarifies everything.

Fit Curve-Spline to 3D Point Cloud

Objective
I have a 3D facet model (e.g. .off file) which can for example look like a pipe/tube (see example picture). The goal is to derive an approximate spline (best case combination of lines and splines) which represents the 3D skeleton of this tube using python.
State of the art
Stackoverflow posts in same field:
how-to-fit-a-line-through-a-3d-pointcloud
General:
Fitting-Spline-Curves-through-Set-of-Unorganized Point Cloud
Skeletons from point cloud
Powercrust / NN-Crust? I read about those, however I cant find a python implementation and maybe I am too dumb to implement it on my own. As far as I understood, the basis is delaunay/voronoi which I know already. However i dont know any further steps
My Approach (so far)
Starting from the example facet model (Image 1), I used a python package to convert the 3d model to a point cloud (Image 2). This point cloud can be used for a voxelized representation (Image 3). Consequently, these three types of data are my starting point.
Basically, this problem does not seem too complicated to me, however I am missing a starting logic. Most of the research papers overcomplicate this for various further-reaching tasks.
One idea would be to do a PCA to derive major axes of the component, and scan along these axes. However, this doesnt appear to lead to good results in a performant way.
Another idea would be to use the voxelized grid and detect a path due to voxel adjacencies.
Another idea would be to use KD-Tree to evaluate closest points to detect the correct planes for defining the spline direction via their plane normals.
An approach that I tried was to select N random points from the pointcloud and search for all neighbors within a radius (cKDTree.query_ball_point). I calculated the center of all neighboring points. This leads to the result in image 4. The result seems good as first approach, however it is more or less a tuning of the radius parameter.
Image 1:
Image 2:
Image 3:
Image 4:

Delaunay/Voronoi methods can be used for this problem, since the medial axis is a sub-graph of the Voronoi graph
(see for example this paper by Attali, Boissonnat and Edelsbrunner).
In the following I will demonstrate the methods on an example of points sampled from a quarter torus surface with small radius 10 and large radius 100 (the medial path/skeleton starts at point (100, 0, 0) and ends at (0, 100, 0)).
The Voronoi graph is the dual of the 3D Delaunay tetrahedralization (I will from now on use the term triangulation for this).
Computing the Delaunay triangulation can be done using scipy's scipy.spatial.Delaunay package.
Below is a figure of the sample points (200 in this example) and their full Delaunay triangulation
(the triangulation was plotted with the function from here).
The Voronoi vertex corresponding to a Delaunay tetrahedron is the center of the circumscribing sphere of the tetrahedron.
The following is a function that computes these Delaunay centers, it is an extension to the 2D function from my previous answer here.
def compute_delaunay_tetra_circumcenters(dt):
"""
Compute the centers of the circumscribing circle of each tetrahedron in the Delaunay triangulation.
:param dt: the Delaunay triangulation
:return: array of xyz points
"""
simp_pts = dt.points[dt.simplices]
# (n, 4, 3) array of tetrahedra points where simp_pts[i, j, :] holds the j'th 3D point (of four) of the i'th tetrahedron
assert simp_pts.shape[1] == 4 and simp_pts.shape[2] == 3
# finding the circumcenter (x, y, z) of a simplex defined by four points:
# (x-x0)**2 + (y-y0)**2 + (z-z0)**2 = (x-x1)**2 + (y-y1)**2 + (z-z1)**2
# (x-x0)**2 + (y-y0)**2 + (z-z0)**2 = (x-x2)**2 + (y-y2)**2 + (z-z2)**2
# (x-x0)**2 + (y-y0)**2 + (z-z0)**2 = (x-x3)**2 + (y-y3)**2 + (z-z3)**2
# becomes three linear equations (squares are canceled):
# 2(x1-x0)*x + 2(y1-y0)*y + 2(z1-z0)*y = (x1**2 + y1**2 + z1**2) - (x0**2 + y0**2 + z0**2)
# 2(x2-x0)*x + 2(y2-y0)*y + 2(z2-z0)*y = (x2**2 + y2**2 + z2**2) - (x0**2 + y0**2 + z0**2)
# 2(x3-x0)*x + 2(y3-y0)*y + 2(z3-z0)*y = (x3**2 + y3**2 + z3**2) - (x0**2 + y0**2 + z0**2)
# building the 3x3 matrix of the linear equations
a = 2 * (simp_pts[:, 1, 0] - simp_pts[:, 0, 0])
b = 2 * (simp_pts[:, 1, 1] - simp_pts[:, 0, 1])
c = 2 * (simp_pts[:, 1, 2] - simp_pts[:, 0, 2])
d = 2 * (simp_pts[:, 2, 0] - simp_pts[:, 0, 0])
e = 2 * (simp_pts[:, 2, 1] - simp_pts[:, 0, 1])
f = 2 * (simp_pts[:, 2, 2] - simp_pts[:, 0, 2])
g = 2 * (simp_pts[:, 3, 0] - simp_pts[:, 0, 0])
h = 2 * (simp_pts[:, 3, 1] - simp_pts[:, 0, 1])
i = 2 * (simp_pts[:, 3, 2] - simp_pts[:, 0, 2])
v1 = (simp_pts[:, 1, 0] ** 2 + simp_pts[:, 1, 1] ** 2 + simp_pts[:, 1, 2] ** 2) - (simp_pts[:, 0, 0] ** 2 + simp_pts[:, 0, 1] ** 2 + simp_pts[:, 0, 2] ** 2)
v2 = (simp_pts[:, 2, 0] ** 2 + simp_pts[:, 2, 1] ** 2 + simp_pts[:, 2, 2] ** 2) - (simp_pts[:, 0, 0] ** 2 + simp_pts[:, 0, 1] ** 2 + simp_pts[:, 0, 2] ** 2)
v3 = (simp_pts[:, 3, 0] ** 2 + simp_pts[:, 3, 1] ** 2 + simp_pts[:, 3, 2] ** 2) - (simp_pts[:, 0, 0] ** 2 + simp_pts[:, 0, 1] ** 2 + simp_pts[:, 0, 2] ** 2)
# solve a 3x3 system by inversion (see https://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_3_%C3%97_3_matrices)
A = e*i - f*h
B = -(d*i - f*g)
C = d*h - e*g
D = -(b*i - c*h)
E = a*i - c*g
F = -(a*h - b*g)
G = b*f - c*e
H = -(a*f - c*d)
I = a*e - b*d
det = a*A + b*B + c*C
# multiplying inv*[v1, v2, v3] to get solution point (x, y, z)
x = (A*v1 + D*v2 + G*v3) / det
y = (B*v1 + E*v2 + H*v3) / det
z = (C*v1 + F*v2 + I*v3) / det
return (np.vstack((x, y, z))).T
We would like to filter out tetrahedra that are outside of the original surface (see for example the long tetrahedra in the figure above).
This might be done by testing the tetrahedra against the original surface.
However, a simpler way, which is very suited for the input of a tube/pipe surface is
to filter out tetrahedra that have a large circumscribing radius.
This is what the alpha-shape algorithm does.
This is easily done within our context since the radius is just the distance between the center and any of the tetrahedron points.
The following figure shows the Delaunay triangulation after filtering out tetrahedra with radius greater than 20.
We can now use these building blocks to build the Voronoi sub-graph of the tetrahedra that pass the radius condition.
The function below does this using the connectivity information in the Delaunay triangulation to construct the Voronoi sub-graph, represented as an edge list.
def compute_voronoi_vertices_and_edges(points, r_thresh=np.inf):
"""
Compute (finite) Voronoi edges and vertices of a set of points.
:param points: input points.
:param r_thresh: radius value for filtering out vertices corresponding to
Delaunay tetrahedrons with large radii of circumscribing sphere (alpha-shape condition).
:return: array of xyz Voronoi vertex points and an edge list.
"""
dt = Delaunay(points)
xyz_centers = compute_delaunay_tetra_circumcenters(dt)
# filtering tetrahedrons that have radius > thresh
simp_pts_0 = dt.points[dt.simplices[:, 0]]
radii = np.linalg.norm(xyz_centers - simp_pts_0, axis=1)
is_in = radii < r_thresh
# build an edge list from (filtered) tetrahedrons neighbor relations
edge_lst = []
for i in range(len(dt.neighbors)):
if not is_in[i]:
continue # i is an outside tetra
for j in dt.neighbors[i]:
if j != -1 and is_in[j]:
edge_lst.append((i, j))
return xyz_centers, edge_lst
The result is still not sufficient as can be seen in the figure below, where the sub-graph edges are the black line segments.
The reason is that 3D Delaunay triangulations are notorious for having thin tetrahedra
(so-called slivers, needles and caps in this paper by Shewchuk),
which cause the outer "spikes" in the results.
While there are general methods to remove these unwanted spikes
(see, for example, Amenta and Bern),
in the context of a tube surface there is a simple solution.
The path we are looking for can be computed as the shortest Euclidean path in the graph starting at the closest point to the start of the tube and ending at the closest point to the end.
The following code does this using a networkx graph with weights set to the lengths of the edges.
# get closest vertex to start and end points
xyz_centers, edge_lst = compute_voronoi_vertices_and_edges(pts, r_thresh=20.)
kdt = cKDTree(xyz_centers)
dist0, idx0 = kdt.query(np.array([100., 0, 0]))
dist1, idx1 = kdt.query(np.array([0, 100., 0]))
# compute shortest weighted path
edge_lengths = [np.linalg.norm(xyz_centers[e[0], :] - xyz_centers[e[1], :]) for e in edge_lst]
g = nx.Graph((i, j, {'weight': dist}) for (i, j), dist in zip(edge_lst, edge_lengths))
path_s = nx.shortest_path(g,source=idx0,target=idx1, weight='weight')
The figure below shows the results for the original 200 points.
And this is the results for a denser sample of 1000 points.
Now you can pass an approximating spline - interpolating or least-square fit, through the path points.
You can use scipy.interpolate.UnivariateSpline as suggested in the link
or scipy.interpolate.splrep as done here or any other standard spline implementation.

Shortest path through ordered circular waypoints

I am trying to implement an algorithm which computes the shortest path and and its associated distance from a current position to a goal through an ordered list of waypoints in a 2d plane. A waypoint is defined by its center coordinates (x, y) and its radius r. The shortest path have to intersect each waypoint circumference at least once. This is different from other path optimization problems because I already know the order in which the waypoints have to be crossed.
In the simple case, consecutive waypoints are distinct and not aligned and this can be solved using consecutive angle bisections. The tricky cases are :
when three or more consecutive waypoints have the same center but different radii
when consecutive waypoints are aligned such that a straight line passes through all of them
Here is a stripped down version of my Python implementation, which does not handle aligned waypoints, and handles badly concentric consecutive waypoints. I adapted it because it normally uses latitudes and longitudes, not points in the euclidean space.
def optimize(position, waypoints):
# current position is on the shortest path, cumulative distance starts at zero
shortest_path = [position.center]
optimized_distance = 0
# if only one waypoint left, go in a straight line
if len(waypoints) == 1:
shortest_path.append(waypoints[-1].center)
optimized_distance += distance(position.center, waypoints[-1].center)
else:
# consider the last optimized point (one) and the next two waypoints (two, three)
for two, three in zip(waypoints[:], waypoints[1:]):
one = fast_waypoints[-1]
in_heading = get_heading(two.center, one.center)
in_distance = distance(one.center, two.center)
out_distance = distance(two.center, three.center)
# two next waypoints are concentric
if out_distance == 0:
next_target, nb_concentric = find_next_not_concentric(two, waypoints)
out_heading = get_heading(two.center, next_target.center)
angle = out_heading - in_heading
leg_distance = two.radius
leg_heading = in_heading + (0.5/nb_concentric) * angle
else:
out_heading = get_heading(two.center, three.center)
angle = out_heading - in_heading
leg_heading = in_heading + 0.5 * angle
leg_distance = (2 * in_distance * out_distance * math.cos(math.radians(angle * 0.5))) / (in_distance + out_distance)
best_leg_distance = min(leg_distance, two.radius)
next_best = get_offset(two.center, leg_heading, min_leg_distance)
shortest_path.append(next_best.center)
optimized_distance += distance(one.center, next_best.center)
return optimized_distance, shortest_path
I can see how to test for the different corner cases but I think this approach is bad, because there may be other corner cases I haven't thought of. Another approach would be to discretize the waypoints circumferences and apply a shortest path algorithm such as A*, but that would be highly inefficient.
So here is my question : Is there a more concise approach to this problem ?

For the record, I implemented a solution using Quasi-Newton methods, and described it in this short article. The main work is summarized below.
import numpy as np
from scipy.optimize import minimize
# objective function definition
def tasklen(θ, x, y, r):
x_proj = x + r*np.sin(θ)
y_proj = y + r*np.cos(θ)
dists = np.sqrt(np.power(np.diff(x_proj), 2) + np.power(np.diff(y_proj), 2))
return dists.sum()
# center coordinates and radii of turnpoints
X = np.array([0, 5, 0, 7, 12, 12]).astype(float)
Y = np.array([0, 0, 4, 7, 0, 5]).astype(float)
R = np.array([0, 2, 1, 2, 1, 0]).astype(float)
# first initialization vector is an array of zeros
init_vector = np.zeros(R.shape).astype(float)
# using scipy's solvers to minimize the objective function
result = minimize(tasklen, init_vector, args=(X, Y, R), tol=10e-5)

I would do it like this:
For each circle in order, pick any point on the circumference, and route the path through these points.
For each circle, move the point along the circumference in the direction that makes the total path length smaller.
Repeat 2. until no further improvement can be done.

Finding the Closest Points between Two Cubic Splines with Python and Numpy

I'm looking for a way to analyze two cubic splines and find the point where they come the closest to each other. I've seen a lot of solutions and posts but I've been unable to implement the methods suggested. I know that the closest point will be one of the end-points of the two curves or a point where the first derivative of both curves is equal. Checking the end points is easy. Finding the points where the first derivatives match is hard.
Given:
Curve 0 is B(t) (red)
Curve 1 is C(s) (blue)
A candidate for closest point is where:
B'(t) = C'(s)
The first derivative of each curve takes the following form:
Where the a, b, c coefficients are formed from the control points of the curves:
a=P1-P0
b=P2-P1
c=P3-P2
Taking the 4 control points for each cubic spline I can get each curve's parametric sections into a matrix form that can be expressed with Numpy with the following Python code:
def test_closest_points():
# Control Points for the two qubic splines.
spline_0 = [(1,28), (58,93), (113,95), (239,32)]
spline_1 = [(58, 241), (26,76), (225,83), (211,205)]
first_derivative_matrix = np.array([[3, -6, 3], [-6, 6, 0], [3, 0, 0]])
spline_0_x_A = spline_0[1][0] - spline_0[0][0]
spline_0_x_B = spline_0[2][0] - spline_0[1][0]
spline_0_x_C = spline_0[3][0] - spline_0[2][0]
spline_0_y_A = spline_0[1][1] - spline_0[0][1]
spline_0_y_B = spline_0[2][1] - spline_0[1][1]
spline_0_y_C = spline_0[3][1] - spline_0[2][1]
spline_1_x_A = spline_1[1][0] - spline_1[0][0]
spline_1_x_B = spline_1[2][0] - spline_1[1][0]
spline_1_x_C = spline_1[3][0] - spline_1[2][0]
spline_1_y_A = spline_1[1][1] - spline_1[0][1]
spline_1_y_B = spline_1[2][1] - spline_1[1][1]
spline_1_y_C = spline_1[3][1] - spline_1[2][1]
spline_0_first_derivative_x_coefficients = np.array([[spline_0_x_A], [spline_0_x_B], [spline_0_x_C]])
spline_0_first_derivative_y_coefficients = np.array([[spline_0_y_A], [spline_0_y_B], [spline_0_y_C]])
spline_1_first_derivative_x_coefficients = np.array([[spline_1_x_A], [spline_1_x_B], [spline_1_x_C]])
spline_1_first_derivative_y_coefficients = np.array([[spline_1_y_A], [spline_1_y_B], [spline_1_y_C]])
# Show All te matrix values
print 'first_derivative_matrix:'
print first_derivative_matrix
print
print 'spline_0_first_derivative_x_coefficients:'
print spline_0_first_derivative_x_coefficients
print
print 'spline_0_first_derivative_y_coefficients:'
print spline_0_first_derivative_y_coefficients
print
print 'spline_1_first_derivative_x_coefficients:'
print spline_1_first_derivative_x_coefficients
print
print 'spline_1_first_derivative_y_coefficients:'
print spline_1_first_derivative_y_coefficients
print
# Now taking B(t) as spline_0 and C(s) as spline_1, I need to find the values of t and s where B'(t) = C'(s)
This post has some good high-level advice but I'm unsure how to implement a solution in python that can find the correct values for t and s that have matching first derivatives (slopes). The B'(t) - C'(s) = 0 problem seems like a matter of finding roots. Any advice on how to do it with python and Numpy would be greatly appreciated.

Using Numpy assumes that the problem should be solved numerically. Without loss of generality we can treat that 0<s<=1 and 0<t<=1. You can use SciPy package to solve the problem numerically, e.g.
from scipy.optimize import minimize
import numpy as np
def B(t):
"""Assumed for simplicity: 0 < t <= 1
"""
return np.sin(6.28 * t), np.cos(6.28 * t)
def C(s):
"""0 < s <= 1
"""
return 10 + np.sin(3.14 * s), 10 + np.cos(3.14 * s)
def Q(x):
"""Distance function to be minimized
"""
b = B(x[0])
c = C(x[1])
return (b[0] - c[0]) ** 2 + (b[1] - c[1]) ** 2
res = minimize(Q, (0.5, 0.5))
print("B-Point: ", B(res.x[0]))
print("C-Point: ", C(res.x[1]))
B-Point: (0.7071067518175205, 0.7071068105555733)
C-Point: (9.292893243165555, 9.29289319446135)
This is example for two circles (one circle and arc). This will likely work with splines.

Your assumption of B'(t) = C'(s) is too strong.
Derivatives have direction and magnitude. Directions must coincide in the candidate points, but magnitudes might differ.
To find points with the same derivative slopes and the closest distance you can solve equation system (of course, high power :( )
yb'(t) * xc'(u) - yc'(t) * xb'(u) = 0 //vector product of (anti)collinear vectors is zero
((xb(t) - xc(u))^2 + (xb(t) - xc(u))^2)' = 0 //distance derivative

You can use the function fmin also:
import numpy as np
import matplotlib.pylab as plt
from scipy.optimize import fmin
def BCubic(t, P0, P1, P2, P3):
a=P1-P0
b=P2-P1
c=P3-P2
return a*3*(1-t)**2 + b*6*(1-t)*t + c*3*t**2
def B(t):
return BCubic(t,4,2,3,1)
def C(t):
return BCubic(t,1,4,3,4)
def f(t):
# L1 or manhattan distance
return abs(B(t) - C(t))
init = 0 # 2
tmin = fmin(f,np.array([init]))
#Optimization terminated successfully.
#Current function value: 2.750000
# Iterations: 23
# Function evaluations: 46
print(tmin)
# [0.5833125]
tmin = tmin[0]
t = np.linspace(0, 2, 100)
plt.plot(t, B(t), label='B')
plt.plot(t, C(t), label='C')
plt.plot(t, abs(B(t)-C(t)), label='|B-C|')
plt.plot(tmin, B(tmin), 'r.', markersize=12, label='min')
plt.axvline(x=tmin, linestyle='--', color='k')
plt.legend()
plt.show()

Finding the distance of points to an axis

I have an array of points in 3D Cartesian space:
P = np.random.random((10, 3))
Now I'd like to find their distances to a given axis and on that given axis
Ax_support = array([3, 2, 1])
Ax_direction = array([1, 2, 3])
I've found a solution that first finds the vector from each point that is perpendicular to the direction vector... I feel however, that it is really complicated and that for such a standard problem there would be a numpy or scipy routine already out there (as there is to find the distance between points in scipy.spatial.distance)

I would be surprised to see such an operation among the standard operations of numpy/scipy. What you are looking for is the projection distance onto your line. Start by subtracting Ax_support:
P_centered = P - Ax_support
The points on the line through 0 with direction Ax_direction with the shortest distance in the L2 sense to each element of P_centered is given by
P_projected = P_centered.dot(np.linalg.pinv(
Ax_direction[np.newaxis, :])).dot(Ax_direction[np.newaxis, :])
Thus the formula you are looking for is
distances = np.sqrt(((P_centered - P_projected) ** 2).sum(axis=1))
Yes, this is exactly what you propose, in a vectorized way of doing things, so it should be pretty fast for reasonably many data points.
Note: If anybody knows a built-in function for this, I'd be very interested!

It's been bothering me that something like this does not exist, so I added haggis.math.segment_distance to the library of utilities I maintain, haggis.
One catch is that this function expects a line defined by two points rather than a support and direction. You can supply as many points and lines as you want, as long as the dimensions broadcast. The usual formats are many points projected on one line (as you have), or one point projected on many lines.
distances = haggis.math.segment_distance(
P, Ax_support, Ax_support + Ax_direction,
axis=1, segment=False)
Here is a reproducible example:
np.random.seed(0xBEEF)
P = np.random.random((10,3))
Ax_support = np.array([3, 2, 1])
Ax_direction = np.array([1, 2, 3])
d, t = haggis.math.segment_distance(
P, Ax_support, Ax_support + Ax_direction,
axis=1, return_t=True, segment=False)
return_t returns the location of the normal points along the line as a ratio of the line length from the support (i.e., Ax_support + t * Ax_direction is the location of the projected points).
>>> d
array([2.08730838, 2.73314321, 2.1075711 , 2.5672012 , 1.96132443,
2.53325436, 2.15278454, 2.77763701, 2.50545181, 2.75187883])
>>> t
array([-0.47585462, -0.60843258, -0.46755277, -0.4273361 , -0.53393468,
-0.58564737, -0.38732655, -0.53212317, -0.54956548, -0.41748691])
This allows you to make the following plot:
fig, ax = plt.subplots(subplot_kw={'projection': '3d'})
ax.plot(*Ax_support, 'ko')
ax.plot(*Ax_support + Ax_direction, 'ko')
start = min(t.min(), 0)
end = max(t.max(), 1)
margin = 0.1 * (end - start)
start -= margin
end += margin
ax.plot(*Ax_support[:, None] + [start, end] * Ax_direction[:, None], 'k--')
for s, te in zip(P, t):
pt = Ax_support + te * Ax_direction
ax.plot(*np.stack((s, pt), axis=-1), 'k-')
ax.plot(*pt, 'ko', markerfacecolor='w')
ax.plot(*P.T, 'ko', markerfacecolor='r')
plt.show()