I had a question about tracking a spot on a turning circle. As you see in the image I am trying to calculate the x2 and only known parameters are θ1, L and x1. The challenge is to track that spot on each turn of circle which each step size is θ1.
The calculation which gives approximately correct answer is:
x2 = x1 - (L/2 - L/2 * cos(θ1))
Spot Tracking
The problem is as the circle turns x1 deviates more from the correct answer. Is there anyway to calculate θ2 as circle turns?
Hint:
The spot motion is described by
X = Xc + r cos Θ
Y = Yc + r sin Θ
Hence the angle seen from the origin,
φ = arctan((Yc + r sin Θ)/(Xc + r cos Θ)).
Notice that your problem is indeterminate, as the center of the circle is free to move at distance L of the origin, giving different intersections with the vertical at x1.
I'm trying to convert Twist to the left and right wheels' speed with the formula:
float speed_wish_right = (cmd_vel.angle*WHEEL_DIST)/2 + cmd_vel.speed;
float speed_wish_left = cmd_vel.speed*2-speed_wish_right;
Twist.angular is a vector [x, y, z] and so is Twist.linear. What do x, y, z mean in the vector and how can I get angles and speed out of the two vectors?
This is my callback function in Arduino
const int WHEEL_DIST = 16;
void velCallback(geometry_msgs::Twist vel) {
float linear = vel.linear.x;
float angle = vel.angular.z;
float speed_wish_right = (angle * WHEEL_DIST) / 2 + linear;
float speed_wish_left = linear * 2 - speed_wish_right;
motors.setSpeeds(speed_wish_left, speed_wish_right);
}
Consider that you are in some space, then there are 3 axes - x, y and z which are mutually perpendicular to each other and their point of intersection is called the origin (x = 0, y = 0, z = 0). This can be a frame of reference i.e. you can define various points and directions w.r.t. them.
The x, y, and z in Twist.linear are the linear velocities in x, y and z directions w.r.t. that frame of reference.
Similarly, the x, y, and z in Twist.angular are the angular velocities about the x, y and z directions respectively w.r.t. the same frame of reference.
Since you have a ground robot, most probably your angular velocity will be in z i.e. robot's turning speed. And your linear velocity will be mostly in x i.e. robot's moving straight speed. This is the case for the Turtlebot 2 at least.
I have this function which returns x and y position an just adding up degrees, it make objects to move around in circular movements like a satellite around a planet.
In my case it moves like an ellipse because I added +30 to dist.
-(CGPoint)circularMovement:(float)degrees moonDistance:(CGFloat)dist
{
if(degrees >=360)degrees = 0;
float x = _moon.position.x + (dist+30 + _moon.size.height/2) *cos(degrees);
float y = _moon.position.y + (dist + _moon.size.height/2) *sin(degrees);
CGPoint position= CGPointMake(x, y);
return position;
}
What I would like is to reverse this function, giving the x and y position of an object and getting back the dist value.
Is this possible?
If so, how would I go about achieving it?
If you have an origin and a target, the origin having the coordinates (x1, y1) and the target has the coordinates (x2, y2) the distance between them is found using the Pythagorean theorem.
The distance between the points is the square root of the difference between x2 and x1 plus the difference between y2 and y1.
In most languages this would look something like this:
x = x2 - x1;
y = y2 - y1;
distance = Math.SquareRoot(x * x + y * y);
Where Math is your language's math library.
float x = _moon.position.x + (dist+30 + _moon.size.height/2) *cos(degrees);
float y = _moon.position.y + (dist + _moon.size.height/2) *sin(degrees);
is the way you have originally calculated the values, so the inverse formula would be:
dist = ((y - _moon.position.y) / (sin(degrees))) - _moon.size.height/2
You could calculate it based on x as well, but there is no point, it is simpler based on y.
Given two rectangles with x, y, width, height in pixels and a rotation value in degrees -- how do I calculate the closest distance of their outlines toward each other?
Background: In a game written in Lua I'm randomly generating maps, but want to ensure certain rectangles aren't too close to each other -- this is needed because maps become unsolvable if the rectangles get into certain close-distance position, as a ball needs to pass between them. Speed isn't a huge issue as I don't have many rectangles and the map is just generated once per level. Previous links I found on StackOverflow are this and this
Many thanks in advance!
Not in Lua, a Python code based on M Katz's suggestion:
def rect_distance((x1, y1, x1b, y1b), (x2, y2, x2b, y2b)):
left = x2b < x1
right = x1b < x2
bottom = y2b < y1
top = y1b < y2
if top and left:
return dist((x1, y1b), (x2b, y2))
elif left and bottom:
return dist((x1, y1), (x2b, y2b))
elif bottom and right:
return dist((x1b, y1), (x2, y2b))
elif right and top:
return dist((x1b, y1b), (x2, y2))
elif left:
return x1 - x2b
elif right:
return x2 - x1b
elif bottom:
return y1 - y2b
elif top:
return y2 - y1b
else: # rectangles intersect
return 0.
where
dist is the euclidean distance between points
rect. 1 is formed by points (x1, y1) and (x1b, y1b)
rect. 2 is formed by points (x2, y2) and (x2b, y2b)
Edit: As OK points out, this solution assumes all the rectangles are upright. To make it work for rotated rectangles as the OP asks you'd also have to compute the distance from the corners of each rectangle to the closest side of the other rectangle. But you can avoid doing that computation in most cases if the point is above or below both end points of the line segment, and to the left or right of both line segments (in telephone positions 1, 3, 7, or 9 with respect to the line segment).
Agnius's answer relies on a DistanceBetweenLineSegments() function. Here is a case analysis that does not:
(1) Check if the rects intersect. If so, the distance between them is 0.
(2) If not, think of r2 as the center of a telephone key pad, #5.
(3) r1 may be fully in one of the extreme quadrants (#1, #3, #7, or #9). If so
the distance is the distance from one rect corner to another (e.g., if r1 is
in quadrant #1, the distance is the distance from the lower-right corner of
r1 to the upper-left corner of r2).
(4) Otherwise r1 is to the left, right, above, or below r2 and the distance is
the distance between the relevant sides (e.g., if r1 is above, the distance
is the distance between r1's low y and r2's high y).
Actually there is a fast mathematical solution.
Length(Max((0, 0), Abs(Center - otherCenter) - (Extent + otherExtent)))
Where Center = ((Maximum - Minimum) / 2) + Minimum and Extent = (Maximum - Minimum) / 2.
Basically the code above zero's axis which are overlapping and therefore the distance is always correct.
It's preferable to keep the rectangle in this format as it's preferable in many situations ( a.e. rotations are much easier ).
Pseudo-code:
distance_between_rectangles = some_scary_big_number;
For each edge1 in Rectangle1:
For each edge2 in Rectangle2:
distance = calculate shortest distance between edge1 and edge2
if (distance < distance_between_rectangles)
distance_between_rectangles = distance
There are many algorithms to solve this and Agnius algorithm works fine. However I prefer the below since it seems more intuitive (you can do it on a piece of paper) and they don't rely on finding the smallest distance between lines but rather the distance between a point and a line.
The hard part is implementing the mathematical functions to find the distance between a line and a point, and to find if a point is facing a line. You can solve all this with simple trigonometry though. I have below the methodologies to do this.
For polygons (triangles, rectangles, hexagons, etc.) in arbitrary angles
If polygons overlap, return 0
Draw a line between the centres of the two polygons.
Choose the intersecting edge from each polygon. (Here we reduce the problem)
Find the smallest distance from these two edges. (You could just loop through each 4 points and look for the smallest distance to the edge of the other shape).
These algorithms work as long as any two edges of the shape don't create angles more than 180 degrees. The reason is that if something is above 180 degrees then it means that the some corners are inflated inside, like in a star.
Smallest distance between an edge and a point
If point is not facing the face, then return the smallest of the two distances between the point and the edge cornerns.
Draw a triangle from the three points (edge's points plus the solo point).
We can easily get the distances between the three drawn lines with Pythagorean Theorem.
Get the area of the triangle with Heron's formula.
Calculate the height now with Area = 12⋅base⋅height with base being the edge's length.
Check to see if a point faces an edge
As before you make a triangle from an edge and a point. Now using the Cosine law you can find all the angles with just knowing the edge distances. As long as each angle from the edge to the point is below 90 degrees, the point is facing the edge.
I have an implementation in Python for all this here if you are interested.
This question depends on what kind of distance. Do you want, distance of centers, distance of edges or distance of closest corners?
I assume you mean the last one. If the X and Y values indicate the center of the rectangle then you can find each the corners by applying this trick
//Pseudo code
Vector2 BottomLeftCorner = new Vector2(width / 2, heigth / 2);
BottomLeftCorner = BottomLeftCorner * Matrix.CreateRotation(MathHelper.ToRadians(degrees));
//If LUA has no built in Vector/Matrix calculus search for "rotate Vector" on the web.
//this helps: http://www.kirupa.com/forum/archive/index.php/t-12181.html
BottomLeftCorner += new Vector2(X, Y); //add the origin so that we have to world position.
Do this for all corners of all rectangles, then just loop over all corners and calculate the distance (just abs(v1 - v2)).
I hope this helps you
I just wrote the code for that in n-dimensions. I couldn't find a general solution easily.
// considering a rectangle object that contains two points (min and max)
double distance(const rectangle& a, const rectangle& b) const {
// whatever type you are using for points
point_type closest_point;
for (size_t i = 0; i < b.dimensions(); ++i) {
closest_point[i] = b.min[i] > a.min[i] ? a.max[i] : a.min[i];
}
// use usual euclidian distance here
return distance(a, closest_point);
}
For calculating the distance between a rectangle and a point you can:
double distance(const rectangle& a, const point_type& p) const {
double dist = 0.0;
for (size_t i = 0; i < dimensions(); ++i) {
double di = std::max(std::max(a.min[i] - p[i], p[i] - a.max[i]), 0.0);
dist += di * di;
}
return sqrt(dist);
}
If you want to rotate one of the rectangles, you need to rotate the coordinate system.
If you want to rotate both rectangles, you can rotate the coordinate system for rectangle a. Then we have to change this line:
closest_point[i] = b.min[i] > a.min[i] ? a.max[i] : a.min[i];
because this considers there is only one candidate as the closest vertex in b. You have to change it to check the distance to all vertexes in b. It's always one of the vertexes.
See: https://i.stack.imgur.com/EKJmr.png
My approach to solving the problem:
Combine the two rectangles into one large rectangle
Subtract from the large rectangle the first rectangle and the second
rectangle
What is left after the subtraction is a rectangle between the two
rectangles, the diagonal of this rectangle is the distance between
the two rectangles.
Here is an example in C#
public static double GetRectDistance(this System.Drawing.Rectangle rect1, System.Drawing.Rectangle rect2)
{
if (rect1.IntersectsWith(rect2))
{
return 0;
}
var rectUnion = System.Drawing.Rectangle.Union(rect1, rect2);
rectUnion.Width -= rect1.Width + rect2.Width;
rectUnion.Width = Math.Max(0, rectUnion.Width);
rectUnion.Height -= rect1.Height + rect2.Height;
rectUnion.Height = Math.Max(0, rectUnion.Height);
return rectUnion.Diagonal();
}
public static double Diagonal(this System.Drawing.Rectangle rect)
{
return Math.Sqrt(rect.Height * rect.Height + rect.Width * rect.Width);
}
Please check this for Java, it has the constraint all rectangles are parallel, it returns 0 for all intersecting rectangles:
public static double findClosest(Rectangle rec1, Rectangle rec2) {
double x1, x2, y1, y2;
double w, h;
if (rec1.x > rec2.x) {
x1 = rec2.x; w = rec2.width; x2 = rec1.x;
} else {
x1 = rec1.x; w = rec1.width; x2 = rec2.x;
}
if (rec1.y > rec2.y) {
y1 = rec2.y; h = rec2.height; y2 = rec1.y;
} else {
y1 = rec1.y; h = rec1.height; y2 = rec2.y;
}
double a = Math.max(0, x2 - x1 - w);
double b = Math.max(0, y2 - y1 - h);
return Math.sqrt(a*a+b*b);
}
Another solution, which calculates a number of points on the rectangle and choses the pair with the smallest distance.
Pros: works for all polygons.
Cons: a little bit less accurate and slower.
import numpy as np
import math
POINTS_PER_LINE = 100
# get points on polygon outer lines
# format of polygons: ((x1, y1), (x2, y2), ...)
def get_points_on_polygon(poly, points_per_line=POINTS_PER_LINE):
all_res = []
for i in range(len(poly)):
a = poly[i]
if i == 0:
b = poly[-1]
else:
b = poly[i-1]
res = list(np.linspace(a, b, points_per_line))
all_res += res
return all_res
# compute minimum distance between two polygons
# format of polygons: ((x1, y1), (x2, y2), ...)
def min_poly_distance(poly1, poly2, points_per_line=POINTS_PER_LINE):
poly1_points = get_points_on_polygon(poly1, points_per_line=points_per_line)
poly2_points = get_points_on_polygon(poly2, points_per_line=points_per_line)
distance = min([math.sqrt((a[0] - b[0])**2 + (a[1] - b[1])**2) for a in poly1_points for b in poly2_points])
# slower
# distance = min([np.linalg.norm(a - b) for a in poly1_points for b in poly2_points])
return distance