Related
I'm having trouble to find the root of a parameterized quintic polynome. Background is: I want to find the parameter s_f such that for any given parameter d_f, the curvature of the polynomial is smaller than a threshold (yeah .. sounds complex, but the math is rather straight);
# define quintic polynomial (jerk-minimized trajectory)
# see http://courses.shadmehrlab.org/Shortcourse/minimumjerk.pdf
s = symbols('s', real=True, positive=True)
s_f = symbols('s_f', real=True, positive=True, nonzero=True)
d_0 = 0
d_f = symbols('d_f', real=True, positive=True, nonzero=True)
d_of_s = d_0 + (d_f - d_0) * ( 10*(s/s_f)**3 - 15*(s/s_f)**4 + 6*(s/s_f)**5 )
display(d_of_s)
# define curvature of d_of_s
# see https://en.wikipedia.org/wiki/Curvature#In_terms_of_a_general_parametrization
y = d_of_s
dy = diff(d_of_s, s)
ddy = diff(dy, s)
x = s
dx = diff(x, s) # evaluates to 1
ddx = diff(dx, s) # evaluates to 0
k = (dx*ddy - dy*ddx) / ((dx*dx + dy*dy)**Rational(3,2))
# the goal is to find s_f for any given d_f, such that k(s) < some_threshold
# strategy: find the roots of the derivative of k in the range of sā[0, s_f]
dk = diff(k, s)
dk = simplify(dk)
display(dk)
# now solve
res = solveset(dk, s, Interval(0, s_f).intersection(S.Reals))
display(res)
The function dk(s, d_f, s_f) has two root in the interval sā[0, s_f], however solveset returns this:
ConditionSet(s, Eq(5400*d_f**2*s**4*(-s**2 + 2*s*s_f - s_f**2)*(2*s**2 - 3*s*s_f + s_f**2)**2 + (900*d_f**2*s**4*(s**2 - 2*s*s_f + s_f**2)**2 + s_f**10)*(6*s**2 - 6*s*s_f + s_f**2), 0), Interval(0, s_f))
.. which is afaik equivalent to: I can't solve this, we got infinite number of results. Well, this is true for the function in general. limit(dk, s, -oo) and limit(dk, s, +oo) is zero. But since I stated the domain interval, why am I not getting the two roots I'm expecting? I'd also expect to get a more granular result:
set containing the roots for s < 0;
set containing the roots for s > s_f
the two roots when sā[0, s_f]
I started with solve() and a lot of different assumptions on my symbols. I get different results for different assumptions, but no combination seems to yield what I need. When I state no assumptions, I get back a set with a huge condition and 8 roots, that don't seem real or correct. In general, the constraints are:
- all symbols are real
- s_f > 0
- d_f > 0
- s ā [0, s_f] (domain range .. the polynomial is only evaluated in this interval)
I guess the problem is that I'm not setting up my solveset correctly:
how to specify that s_f and d_f are real? afaik the symbol
assumptions are ignored when using solveset?
how to specify intervals
and assumptions on other multivariate functions, i.e. other symbols
than the domain one?
This is what d_of_s look like for s_f = 1, d_f = 1
And this is what dk(s) looks like (I plotted outside the domain range to visualize the problem).
Substitute in the known values of d_f and s_f and use real_roots to find the real roots of the numerator of dk. Keep the ones that have a value in the range of interest:
>>> s_fi = 3
>>> [i for i in real_roots(dk.subs(d_f,2).subs(s_f, s_fi).as_numer_denom()[0])
... if 0 <= i.n(2) <= s_fi]
[CRootOf(800*s**10 - 12000*s**9 + 74000*s**8 - 240000*s**7 + 432000*s**6 - 410400*s**5
+ 162000*s**4 - 4374*s**2 + 13122*s - 6561, 1), CRootOf(800*s**10 - 12000*s**9 +
74000*s**8 - 240000*s**7 + 432000*s**6 - 410400*s**5 + 162000*s**4 - 4374*s**2 +
13122*s - 6561, 2)]
I kept the CRootOf instances because they can be computed to arbitrary precision, e.g. to 3 digits:
>>> [i.n(3) for i in _]
[0.433, 2.57]
I have two data files, each of them contain a big number of 3-dimensional points (file A stores approximately 50,000 points, file B stores approximately 500,000 points). My goal is to find for every point (a) in file A the point (b) in file B which has the smallest distance to (a). I store the points in two lists like this:
List A nodes:
(ID X Y Z)
[ ['478277', -107.0, 190.5674, 128.1634],
['478279', -107.0, 190.5674, 134.0172],
['478282', -107.0, 190.5674, 131.0903],
['478283', -107.0, 191.9798, 124.6807],
... ]
List B data:
(X Y Z Data)
[ [-28.102, 173.657, 229.744, 14.318],
[-28.265, 175.549, 227.824, 13.648],
[-27.695, 175.925, 227.133, 13.142],
...]
My first approach was to simply iterate through the first and second list with a nested loop and compute the distance between every points like this:
outfile = open(job[0] + '/' + output, 'wb');
dist_min = float(job[5]);
dist_max = float(job[6]);
dists = [];
for node in nodes:
shortest_distance = 1000.0;
shortest_data = 0.0;
for entry in data:
dist = math.sqrt((node[1] - entry[0])**2 + (node[2] - entry[1])**2 + (node[3] - entry[2])**2);
if (dist_min <= dist <= dist_max) and (dist < shortest_distance):
shortest_distance = dist;
shortest_data = entry[3];
outfile.write(node[0] + ', ' + str('%10.5f' % shortest_data + '\n'));
outfile.close();
I recognized that the amount of loops Python has to run is way too big (~25,000,000,000), so I had to fasten my code. I tried to first calculate all distances with list comprehensions but the code still is too slow:
p_x = [row[1] for row in nodes];
p_y = [row[2] for row in nodes];
p_z = [row[3] for row in nodes];
q_x = [row[0] for row in data];
q_y = [row[1] for row in data];
q_z = [row[2] for row in data];
dx = [[(px - qx) for px in p_x] for qx in q_x];
dy = [[(py - qy) for py in p_y] for qy in q_y];
dz = [[(pz - qz) for pz in p_z] for qz in q_z];
dx = [[dxxx * dxxx for dxxx in dxx] for dxx in dx];
dy = [[dyyy * dyyy for dyyy in dyy] for dyy in dy];
dz = [[dzzz * dzzz for dzzz in dzz] for dzz in dz];
D = [[(dx[i][j] + dy[i][j] + dz[i][j]) for j in range(len(dx[0]))] for i in range(len(dx))];
D = [[(DDD**(0.5)) for DDD in DD] for DD in D];
To be honest, at this point, I do not know which of the two approaches is better, anyway, none of the two possibilities seem feasible. I'm not even sure if it is possible to write a code which calculates all distances in an acceptable time. Is there even another way to solve my problem without calculating all distances?
Edit: I forgot to mention that I am running on Python 2.5.1 and am not allowed to install or add any new libraries...
Just in case someone is interrested in the solution:
I found a way to speed up the whole process by not calculating all distances:
I created a 3D-list, representing a grid in the given 3D space, divided in X, Y and Z in a given step size (e.g. (Max. - Min.) / 1,000). Then I iterated over every 3D point to put it into my grid. After that I iterated over the points of set A again, looking if there are points from B in the same cube, if not I would increase the search radius, so the process is looking in the adjacent 26 cubes for points. The radius is increasing until there is at least one point found. The resulting list is comparatively small and can be ordered in short time and the nearest point is found.
The processing time went down to a couple minutes and it is working fine.
p_x = [row[1] for row in nodes];
p_y = [row[2] for row in nodes];
p_z = [row[3] for row in nodes];
q_x = [row[0] for row in data];
q_y = [row[1] for row in data];
q_z = [row[2] for row in data];
min_x = min(p_x + q_x);
min_y = min(p_y + q_y);
min_z = min(p_z + q_z);
max_x = max(p_x + q_x);
max_y = max(p_y + q_y);
max_z = max(p_z + q_z);
max_n = max(max_x, max_y, max_z);
min_n = min(min_x, min_y, max_z);
gridcount = 1000;
step = (max_n - min_n) / gridcount;
ruler_x = [min_x + (i * step) for i in range(gridcount + 1)];
ruler_y = [min_y + (i * step) for i in range(gridcount + 1)];
ruler_z = [min_z + (i * step) for i in range(gridcount + 1)];
grid = [[[0 for i in range(gridcount)] for j in range(gridcount)] for k in range(gridcount)];
for node in nodes:
loc_x = self.abatemp_get_cell(node[1], ruler_x);
loc_y = self.abatemp_get_cell(node[2], ruler_y);
loc_z = self.abatemp_get_cell(node[3], ruler_z);
if grid[loc_x][loc_y][loc_z] is 0:
grid[loc_x][loc_y][loc_z] = [[node[1], node[2], node[3], node[0]]];
else:
grid[loc_x][loc_y][loc_z].append([node[1], node[2], node[3], node[0]]);
for entry in data:
loc_x = self.abatemp_get_cell(entry[0], ruler_x);
loc_y = self.abatemp_get_cell(entry[1], ruler_y);
loc_z = self.abatemp_get_cell(entry[2], ruler_z);
if grid[loc_x][loc_y][loc_z] is 0:
grid[loc_x][loc_y][loc_z] = [[entry[0], entry[1], entry[2], entry[3]]];
else:
grid[loc_x][loc_y][loc_z].append([entry[0], entry[1], entry[2], entry[3]]);
out = [];
outfile = open(job[0] + '/' + output, 'wb');
for node in nodes:
neighbours = [];
radius = -1;
loc_nx = self.abatemp_get_cell(node[1], ruler_x);
loc_ny = self.abatemp_get_cell(node[2], ruler_y);
loc_nz = self.abatemp_get_cell(node[3], ruler_z);
reloop = True;
while reloop:
if neighbours:
reloop = False;
radius += 1;
start_x = 0 if ((loc_nx - radius) < 0) else (loc_nx - radius);
start_y = 0 if ((loc_ny - radius) < 0) else (loc_ny - radius);
start_z = 0 if ((loc_nz - radius) < 0) else (loc_nz - radius);
end_x = (len(ruler_x) - 1) if ((loc_nx + radius + 1) > (len(ruler_x) - 1)) else (loc_nx + radius + 1);
end_y = (len(ruler_y) - 1) if ((loc_ny + radius + 1) > (len(ruler_y) - 1)) else (loc_ny + radius + 1);
end_z = (len(ruler_z) - 1) if ((loc_nz + radius + 1) > (len(ruler_z) - 1)) else (loc_nz + radius + 1);
for i in range(start_x, end_x):
for j in range(start_y, end_y):
for k in range(start_z, end_z):
if not grid[i][j][k] is 0:
for grid_entry in grid[i][j][k]:
if not isinstance(grid_entry[3], basestring):
neighbours.append(grid_entry);
dists = [];
for n in neighbours:
d = math.sqrt((node[1] - n[0])**2 + (node[2] - n[1])**2 + (node[3] - n[2])**2);
dists.append([d, n[3]]);
dists = sorted(dists);
outfile.write(node[0] + ', ' + str(dists[0][-1]) + '\n');
outfile.close();
Function to get the position of a point:
def abatemp_get_cell(self, n, ruler):
for i in range(len(ruler)):
if i >= len(ruler):
return False;
if ruler[i] <= n <= ruler[i + 1]:
return i;
The gridcount variable gives one the chance to fasten the process, with a small gridcount the process of sorting the points into the grid is very fast, but the lists of neighbours in the search loop gets bigger and more time is needed for this part of the process. With a big gridcount more time is needed at the beginning, however the loop runs faster.
The only issue I have now is the fact, that there are cases when the process found neighbours but there are other points, which are not yet found, but are closer to the point (see picture). So far I solved this issue by incrementing the search radius another time when there are already neigbours. And still then I have points which are closer but not in the neighbours list, although it's a very small amount (92 out of ~100,000). I could solve this problem by increment the radius two times after finding neighbours, but this solution seems not very smart. Maybe you guys have an idea...
This is the first working draft of the process, I think it will be possible to improve it even more, just to give you an idea of how it is working...
It took me a bit of thought but at the end I think I found a solution for you.
Your problem is not in the code you wrote but in the algorithm it implements.
There is an algorithm called Dijkstra's algorithm and here is the gist of it: https://en.wikipedia.org/wiki/Dijkstra%27s_algorithm .
Now what you need to do is to use this algorithm in a clever way:
create a node S (stand for source).
Now link edges from S to all the nodes in B group.
After you done that you should link edges from each point b in B to each point a in A.
You should set the cost of the links from the source to 0 and the other to the distance between 2 points (only in 3D).
Now if we will use Dijkstra's algorithm the output we will get would be the cost to travel from S to each point in the graph (we are only interested in the distance to points in group A).
So since the cost is 0 to each point b in B and S is only connected to points in B so the road to any point a in A must include a node in B (actually exactly one since the shortest distance between to points is a single line).
I am not sure if this will fasten your code but as far as I know, a way to solve this problem without calculating all distances does not exist and this algorithm is the best time complexity one could hope for.
take a look at this generic 3D data structure:
https://github.com/m4nh/skimap_ros
it has a very fast RadiusSearch feature just ready to be used. This solution (similar to Octree but faster) avoids to you to create the Regular Grid first (you don't have to fix MAX/MIN size along each axis) and you save a lot of memory
I am trying to create a binary tree from a lot of segments in 3d space sharing the same origin.
When merging two segments I want to have a specific angle between the lines to the child nodes.
The following image illustrates my problem. C shows the position of the parent node and A and B the child positions. N is the average vector of the vectors from C to A and C to B.
With a given angle, how can I determine point P?
Thanks for any help
P = C + t * ((A + B)/2 - C) t is unknown parameter
PA = A - P PA vector
PB = B - P PB vector
Tan(Fi) = (PA x PB) / (PA * PB) (cross product in the nominator, scalar product in the denominator)
Tan(Fi) * (PA.x*PB.x + PA.y*PB.y) = (PA.x*PB.y - PA.y*PB.x)
this is quadratic equation for t, after solving we will get two (for non-degenerate cases) possible positions of P point (the second one lies at other side of AB line)
Addition:
Let's ax = A.x - A point X-coordinate and so on,
abcx = (ax+bx)/2-cx, abcy = (ay+by)/2-cy
pax = ax-cx - t*abcx, pay = ay-cy - t*abcy
pbx = bx-cx - t*abcx, pby = by-cy - t*abcy
ff = Tan(Fi) , then
ff*(pax*pbx+pay*pby)-pax*pby+pay*pbx=0
ff*((ax-cx - t*abcx)*(bx-cx - t*abcx)+(ay-cy - t*abcy)*(by-cy - t*abcy)) -
- (ax-cx - t*abcx)*(by-cy - t*abcy) + (ay-cy - t*abcy)*(bx-cx - t*abcx) =
t^2 *(ff*(abcx^2+abcy^2)) +
t * (-2*ff*(abcx^2+abcy^2) + abcx*(by-ay) + abcy*(ax-bx) ) +
(ff*((ax-cx)*(bx-cx)+(ay-cy)*(by-cy)) - (ax-cx)*(by-cy)+(bx-cx)*(ay-cy)) =0
This is quadratic equation AA*t^2 + BB*t + CC = 0 with coefficients
AA = ff*(abcx^2+abcy^2)
BB = -2*ff*(abcx^2+abcy^2) + abcx*(by-ay) + abcy*(ax-bx)
CC = ff*((ax-cx)*(bx-cx)+(ay-cy)*(by-cy)) - (ax-cx)*(by-cy)+(bx-cx)*(ay-cy)
P.S. My answer is for 2d-case!
For 3d: It is probably simpler to use scalar product only (with vector lengths)
Cos(Fi) = (PA * PB) / (|PA| * |PB|)
Another solution could be using binary search on the vector N, whether P is close to C then the angle will be smaller and whether P is far from C then the angle will be bigger, being it suitable for a binary search.
I'm writing my own basic physic engine and now I come to a problem I can't solve. Probably because I don't how to google this problem.
So here is my problem. I hope this image can explain it:
Collision response
I have two objects. The gray one is fixed and don't move and the green one which falls from the top.
The green object has three vectors: a force, the acceleration and the velocity. It collides with the fixed gray object.
The real question is how can I get the rotation of the green object when it falls down?
It sounds like you may not have an understanding of the fundamental physics underlying rigid body dynamics. I say that only because you don't mention any of the terminology commonly used when talking about this kind of problem. You'll need to introduce the idea of orientation and angular velocity (the rotational analogs of position and linear velocity) to each dynamic body in the system, and compute all kinds of intermediate quantities like moment of inertia, angular acceleration, and torque.
Perhaps the best introductory reference for this is Chris Hecker's series of articles for Game Developer Magazine. Assuming you already have non-rotational dynamics (covered in part 1) and collision detection (not covered by this series) solved, you should begin with part 2 and proceed to part 3. They'll give you a solid foundation in the physics and mathematics necessary for implementing rotational collision response.
You do as described below once, when the objects collide.
Let us call the green rectangle "a", and the other one "b".
1.
First you need the rectangles "rotational mass", mass of inertia.
a.i = 4/3 * width * height * (width^2 + height^2) * a.density
2.
Then you need the vector pointing from the rectangle's center of mass (average position of all corners) to the contact position (where the rectangles collide), let us call it "r".
3.
Then you need to find the collision normal. This normal is the direction of an impulse being applied to a from b. The normal is a vector with length 1 unit. In your example the normal would probably point upwards. Let us call the normal vector "n".
4.
Now you will need the velocity of the contact point on a. If a is not rotating, the formula would be:
vp = a.vel
If a is rotating the formula would be:
vp = a.vel + cross(a.r_vel, r)
a.r_vel is a's rotational velocity given in radians and positive direction is counter clockwise.
cross() means cross product, the function is:
cross (v,i) = [-i * v.y , i * v.x]
The expanded formula would be:
vp = a.v + [-r * a.r_vel.y , r * a.r_vel.x]
5.
Now you need to calculate whether the objects are moving towards each other. Project the vp onto n.
vp_p = dot(vp, n)
dot (v1, v2) = v1.x * v2.x + v1.y * v2.y
vp_p is a scalar (a value, not a vector).
If vp_p is negative the obejcts are moving towards each other, if it is > 0 they are moving apart.
6.
Now you need to calculate the impulse to stop a from moving into b, the impulse is:
j = -vp_p / (
1/a.mass + cross(r,n)^2 / a.i
)
The cross product between two vectors are:
cross(v1,v2) = v1.x * v2.y - v1.y * v2.x
It returns a scalar.
Multiply the impulse with the normal to get the impulse vector:
jn = j * n
7.
Now you need to apply the impulse to a:
a.new_vel = a.old_vel + jn / a.mass;
a.new_r_vel = a.old_r_vel + cross(r,jn) / a.i;
If you want the collision to be fully elastic, you must multiply the impulse by 2. Let us call this multiplier "e". e needs to be between 1 and 2. 1 means no energy is conserved, 2 means all energy is conserved.
Example code:
var vp = a.vel + cross(a.r_vel, r);
var vp_p = dot(vp,n); // negative val = moving towards each other
if (vp_p >= 0) { // do they move apart?
return false;
}
// normal impulse
var j = - e * vp_p / (
1/a.mass + cross(r,n)^2 / a.i
);
var jn = j * n;
//
a.vel = a.vel + jn / a.mass;
a.r_vel = a.r_vel + cross(r,jn) / a.i;
If b is not static the algorithm will be slightly different:
a.r = vector pointing from a's center of mass to the contact position
var vp = a.vel + cross(a.r_vel, a.r) - b.vel - cross(b.r_vel, b.r);
var vp_p = dot(vp,n); // negative val = moving towards each other
if (vp_p >= 0) { // do they move apart?
return false;
}
// normal impulse
var j = - e * vp_p / (
1/a.mass + cross(a.r,n)^2 / a.i +
1/b.mass + cross(b.r,n)^2 / b.i
);
var jn = j * n;
//
a.vel = a.vel + jp / a.mass;
a.r_vel = a.r_vel + cross(a.r,jn) / a.i;
b.vel = b.vel - jp / b.mass;
b.r_vel = b.r_vel - cross(b.r,jn) / b.i;
How the formulas work / sources:
http://www.myphysicslab.com/collision.html#resting_contact
How do I map numbers, linearly, between a and b to go between c and d.
That is, I want numbers between 2 and 6 to map to numbers between 10 and 20... but I need the generalized case.
My brain is fried.
If your number X falls between A and B, and you would like Y to fall between C and D, you can apply the following linear transform:
Y = (X-A)/(B-A) * (D-C) + C
That should give you what you want, although your question is a little ambiguous, since you could also map the interval in the reverse direction. Just watch out for division by zero and you should be OK.
Divide to get the ratio between the sizes of the two ranges, then subtract the starting value of your inital range, multiply by the ratio and add the starting value of your second range. In other words,
R = (20 - 10) / (6 - 2)
y = (x - 2) * R + 10
This evenly spreads the numbers from the first range in the second range.
It would be nice to have this functionality in the java.lang.Math class, as this is such a widely required function and is available in other languages.
Here is a simple implementation:
final static double EPSILON = 1e-12;
public static double map(double valueCoord1,
double startCoord1, double endCoord1,
double startCoord2, double endCoord2) {
if (Math.abs(endCoord1 - startCoord1) < EPSILON) {
throw new ArithmeticException("/ 0");
}
double offset = startCoord2;
double ratio = (endCoord2 - startCoord2) / (endCoord1 - startCoord1);
return ratio * (valueCoord1 - startCoord1) + offset;
}
I am putting this code here as a reference for future myself and may be it will help someone.
As an aside, this is the same problem as the classic convert celcius to farenheit where you want to map a number range that equates 0 - 100 (C) to 32 - 212 (F).
https://rosettacode.org/wiki/Map_range
[a1, a2] => [b1, b2]
if s in range of [a1, a2]
then t which will be in range of [b1, b2]
t= b1 + ((s- a1) * (b2-b1))/ (a2-a1)
In addition to #PeterAllenWebb answer, if you would like to reverse back the result use the following:
reverseX = (B-A)*(Y-C)/(D-C) + A
Each unit interval on the first range takes up (d-c)/(b-a) "space" on the second range.
Pseudo:
var interval = (d-c)/(b-a)
for n = 0 to (b - a)
print c + n*interval
How you handle the rounding is up to you.
if your range from [a to b] and you want to map it in [c to d] where x is the value you want to map
use this formula (linear mapping)
double R = (d-c)/(b-a)
double y = c+(x*R)+R
return(y)
Where X is the number to map from A-B to C-D, and Y is the result:
Take the linear interpolation formula, lerp(a,b,m)=a+(m*(b-a)), and put C and D in place of a and b to get Y=C+(m*(D-C)). Then, in place of m, put (X-A)/(B-A) to get Y=C+(((X-A)/(B-A))*(D-C)). This is an okay map function, but it can be simplified. Take the (D-C) piece, and put it inside the dividend to get Y=C+(((X-A)*(D-C))/(B-A)). This gives us another piece we can simplify, (X-A)*(D-C), which equates to (X*D)-(X*C)-(A*D)+(A*C). Pop that in, and you get Y=C+(((X*D)-(X*C)-(A*D)+(A*C))/(B-A)). The next thing you need to do is add in the +C bit. To do that, you multiply C by (B-A) to get ((B*C)-(A*C)), and move it into the dividend to get Y=(((X*D)-(X*C)-(A*D)+(A*C)+(B*C)-(A*C))/(B-A)). This is redundant, containing both a +(A*C) and a -(A*C), which cancel each other out. Remove them, and you get a final result of: Y=((X*D)-(X*C)-(A*D)+(B*C))/(B-A)
TL;DR: The standard map function, Y=C+(((X-A)/(B-A))*(D-C)), can be simplified down to Y=((X*D)-(X*C)-(A*D)+(B*C))/(B-A)
int srcMin = 2, srcMax = 6;
int tgtMin = 10, tgtMax = 20;
int nb = srcMax - srcMin;
int range = tgtMax - tgtMin;
float rate = (float) range / (float) nb;
println(srcMin + " > " + tgtMin);
float stepF = tgtMin;
for (int i = 1; i < nb; i++)
{
stepF += rate;
println((srcMin + i) + " > " + (int) (stepF + 0.5) + " (" + stepF + ")");
}
println(srcMax + " > " + tgtMax);
With checks on divide by zero, of course.