Computing tensor of Inertia in 2D - c++

I'm researching how to find the inertia for a 2D shape. The contour of this shape is meshed with several points, the x and y coordinate of each point is already known.
I know the expression of Ixx, Iyy and Ixy but the body has no mass. How do I proceed?

For whatever shape you have, you need to split it into triangles and handle each triangle separately. Then in the end combined the results using the following rules
Overall
% Combined total area of all triangles
total_area = SUM( area(i), i=1:n )
total_mass = SUM( mass(i), i=1:n )
% Combined centroid (center of mass) coordinates
combined_centroid_x = SUM( mass(i)*centroid_x(i), i=1:n)/total_mass
combined_centroid_y = SUM( mass(i)*centroid_y(i), i=1:n)/total_mass
% Each distance to triangle (squared)
centroid_distance_sq(i) = centroid_x(i)*centroid_x(i)+centroid_y(i)*centroid_y(i)
% Combined mass moment of inertia
combined_mmoi = SUM(mmoi(i)+mass(i)*centroid_distance_sq(i), i=1:n)
Now for each triangle.
Consider the three corner vertices with vector coordinates, points A, B and C
a=[ax,ay]
b=[bx,by]
c=[cx,cy]
and the following dot and cross product (scalar) combinations
a·a = ax*ax+ay*ay
b·b = bx*bx+by*by
c·c = cx*cx+cy*cy
a·b = ax*bx+ay*by
b·c = bx*cx+by*cy
c·a = cx*ax+cy*ay
a×b = ax*by-ay*bx
b×c = bx*cy-by*cx
c×a = cx*ay-cy*ax
The properties of the triangle are (with t(i) the thickness and rho the mass density)
area(i) = 1/2*ABS( a×b + b×c + c×a )
mass(i) = rho*t(i)*area(i)
centroid_x(i) = 1/3*(ax + bx + cx)
centroid_y(i) = 1/3*(ay + by + cy)
mmoi(i) = 1/6*mass(i)*( a·a + b·b + c·c + a·b + b·c + c·a )
By component the above are
area(i) = 1/2*ABS( ax*(by-cy)+ay*(cx-bx)+bx*cy-by*cx)
mmoi(i) = mass(i)/6*(ax^2+ax*(bx+cx)+bx^2+bx*cx+cx^2+ay^2+ay*(by+cy)+by^2+by*cy+cy^2)
Appendix
A little theory here. The area of each triangle is found using
Area = 1/2 * || (b-a) × (c-b) ||
where × is a vector cross product, and || .. || is vector norm (length function).
The triangle is parametrized by two variables t and s such that the double integral A = INT(INT(1,dx),dy) gives the total area
% position r(s,t) = [x,y]
[x,y] = [ax,ay] + t*[bx-ax, by-zy] + t*s*[cx-bx,cy-by]
% gradient directions along s and t
(dr/dt) = [bx-ax,by-ay] + s*[cx-bx,cy-by]
(dr/ds) = t*[cx-bx,cy-by]
% Integration area element
dA = || (dr/ds)×(dr/dt) || = (2*A*t)*ds*dt
%
% where A = 1/2*||(b-a)×(c-b)||
% Check that the integral returns the area
Area = INT( INT( 2*A*t,s=0..1), t=0..1) = 2*A*(1/2) = A
% Mass moment of inertia components
/ / / | y^2+z^2 -x*y -x*z |
I = 2*m*| | | t*| -x*y x^2+z^2 -y*z | dz ds dt
/ / / | -x*z -y*z x^2+y^2 |
% where [x,y] are defined from the parametrization

I want to slightly correct excellent John Alexiou answer:
Triangle mmoi algorithm expects corners to be defined relative to triangle center of mass (centroid). So subtract centroid from corners before calculating mmoi
Shape mmoi algorithm expects centroids of each single triangle to be defined relative to shape center of mass (combined centroid). So subtract combined centroid from each triangle centroid before calculating mmoi.
So result code would look like this:
public static float CalculateMMOI(Triangle[] triangles, float thickness, float density)
{
float[] mass = new float[triangles.Length];
float[] mmoi = new float[triangles.Length];
Vector2[] centroid = new Vector2[triangles.Length];
float combinedMass = 0;
float combinedMMOI = 0;
Vector2 combinedCentroid = new Vector2(0, 0);
for (var i = 0; i < triangles.Length; ++i)
{
mass[i] = triangles[i].CalculateMass(thickness, density);
mmoi[i] = triangles[i].CalculateMMOI(mass[i]);
centroid[i] = triangles[i].CalculateCentroid();
combinedMass += mass[i];
combinedCentroid += mass[i] * centroid[i];
}
combinedCentroid /= combinedMass;
for (var i = 0; i < triangles.Length; ++i) {
combinedMMOI += mmoi[i] + mass[i] * (centroid[i] - combinedCentroid).LengthSquared();
}
return combinedMMOI;
}
public struct Triangle
{
public Vector2 A, B, C;
public float CalculateMass(float thickness, float density)
{
var area = CalculateArea();
return area * thickness * density;
}
public float CalculateArea()
{
return 0.5f * Math.Abs(Vector2.Cross(A - B, A - C));
}
public float CalculateMMOI(float mass)
{
var centroid = CalculateCentroid()
var a = A - centroid;
var b = B - centroid;
var c = C - centroid;
var aa = Vector2.Dot(a, a);
var bb = Vector2.Dot(b, b);
var cc = Vector2.Dot(c, c);
var ab = Vector2.Dot(a, b);
var bc = Vector2.Dot(b, c);
var ca = Vector2.Dot(c, a);
return (aa + bb + cc + ab + bc + ca) * mass / 6f;
}
public Vector2 CalculateCentroid()
{
return (A + B + C) / 3f;
}
}
public struct Vector2
{
public float X, Y;
public float LengthSquared()
{
return X * X + Y * Y;
}
public static float Dot(Vector2 a, Vector2 b)
{
return a.X * b.X + a.Y * b.Y;
}
public static float Cross(Vector2 a, Vector2 b)
{
return a.X * b.Y - a.Y * b.X;
}
}

Related

How to generate geometry for part of a sphere

There are few algorithms to generate the points needed to render a sphere, but I couldn't not find a good algorithm nor to adapt a full sphere algorithm to generate the points for a part of the sphere.
Let's assume I have the min/max latitude, min/max longitude and the sphere radius. How do I generate a mesh to be able to render this part of the sphere?
I made hemisphere & part of hemisphere mesh. I edit songho Ahn's source code slightly and you can refer from following link :
http://www.songho.ca/opengl/gl_sphere.html
the main method of class Sphere are buildVertices*
You can get desired result from editing this method.
I made hemisphere like this :
void HemiSphere::buildVerticesSmooth()
{
...
float sectorStep = 2 *PI / sectorCount;
float stackStep = (PI / 2) / stackCount;
float sectorAngle, stackAngle;
for(int i = 0; i <= stackCount; ++i)
{
stackAngle = -PI / 2 + i * stackStep; // starting from -pi/2 to 0
xy = radius * cosf(stackAngle); // r * cos(u)
z = radius *( 1 + sinf(stackAngle) ); // r * sin(u)
...
and I also made semi-hemisphere which cut its floor so that it have even floor on the bottom. To do this, leave x y coordinate unchanged, just edit z coordinate to 0.
void HemiSphere::buildVerticesSmooth()
{
....
float sectorStep = 2 *PI / sectorCount;
float stackStep = (PI / 2) / stackCount;
float sectorAngle, stackAngle;
float d = sqrt(radius*radius - flat_radius*flat_radius)/radius;
for(int i = 0; i <= stackCount; ++i)
{
stackAngle = -PI / 2 + i * stackStep; // starting from -pi/2 to 0
xy = radius * cosf(stackAngle); // r * cos(u)
z = radius *( d + sinf(stackAngle) ); // r * sin(u)
if (z<0.0) z = 0.0;
....
Hope this helps.

3D Line Segment and Plane Intersection - Contd

After advice from krlzlx I have posted it as a new question.
From here:
3D Line Segment and Plane Intersection
I have a problem with this algorithm, I have implemented it like so:
template <class T>
class AnyCollision {
public:
std::pair<bool, T> operator()(Point3d &ray, Point3d &rayOrigin, Point3d &normal, Point3d &coord) const {
// get d value
float d = (normal.x * coord.x) + (normal.y * coord.y) + (normal.z * coord.z);
if (((normal.x * ray.x) + (normal.y * ray.y) + (normal.z * ray.z)) == 0) {
return std::make_pair(false, T());
}
// Compute the X value for the directed line ray intersecting the plane
float a = (d - ((normal.x * rayOrigin.x) + (normal.y * rayOrigin.y) + (normal.z * rayOrigin.z)) / ((normal.x * ray.x) + (normal.y * ray.y) + (normal.z * ray.z)));
// output contact point
float rayMagnitude = (sqrt(pow(ray.x, 2) + pow(ray.y, 2) + pow(ray.z, 2)));
Point3d rayNormalised((ray.x / rayMagnitude), (ray.y / rayMagnitude), (ray.z / rayMagnitude));
Point3d contact((rayOrigin.x + (rayNormalised.x * a)), (rayOrigin.y + (rayNormalised.y * a)), (rayOrigin.z + (rayNormalised.z * a))); //Make sure the ray vector is normalized
return std::make_pair(true, contact);
};
Point3d is defined as:
class Point3d {
public:
double x;
double y;
double z;
/**
* constructor
*
* 0 all elements
*/
Point3d() {
x = 0.0;
y = 0.0;
z = 0.0;
}
I am forced to use this structure, because in the larger system my component runs in it is defined like this and it cannot be changed.
My code compiles fine, but testing I get incorrect values for the point. The ratio of x, y, z is correct but the magnitude is wrong.
For example if:
rayOrigin.x = 0;
rayOrigin.y = 0;
rayOrigin.z = 0;
ray.x = 3;
ray.y = -5;
ray.z = 12;
normal.x = -3;
normal.y = 12;
normal.z = 0;
coord.x = 7;
coord.y = -5;
coord.z = 10;
I expect the point to be:
(0.63, 1.26, 1.89)
However, it is:
(3.52, -5.87, 14.09)
A magnitude of 5.09 too big.
And I also tested:
rayOrigin.x = 0;
rayOrigin.y = 0;
rayOrigin.z = 0;
ray.x = 2;
ray.y = 3;
ray.z = 3;
normal.x = 4;
normal.y = 1;
normal.z = 0;
p0.x = 2;
p0.y = 1;
p0.z = 5;
I expect the point to be:
(1.64, 2.45, 2.45)
However, it is:
(3.83761, 5.75642, 5.75642)
A magnitude of 2.34 too big?
Pseudocode (does not require vector normalization):
Diff = PlaneBaseCoordinate - RayOrigin
d = Normal.dot.Diff
e = Normal.dot.RayVector
if (e)
IntersectionPoint = RayOrigin + RayVector * d / e
otherwise
ray belongs to the plane or is parallel
Quick check:
Ray (0,0,0) (2,2,2) //to catch possible scale issues
Plane (0,1,0) (0,3,0) //plane y=1
Diff = (0,1,0)
d = 3
e = 6
IntersectionPoint = (0,0,0) + (2,2,2) * 3 / 6 = (1, 1, 1)

Bilinear interpolation in 2D transformation Qt

I'm currently working on 2D transformations (translation, scaling, shearing and rotation) in Qt. I have a problem with bilinear interpolation, which I want to use to cover the 'black pixels' in output image. I'm using matrix calculations to get new coordinates of pixels of input image. Then I use reverse matrix calculation to check which pixel of input image responds to output pixel. Result of that is some float number which I use to interpolation. I check the four neighbour points and calculate the value (color) of output pixel. I have checked my calculations 'by hand' and they seem to be good.
Can anyone find any bug in that code? (I cut out the parts of code which are responsible for interface such as sliders).
Geometric::Geometric(QWidget* parent) : QWidget(parent) {
resize(1000, 800);
displayLogoDefault = true;
a = shx = shy = x0 = y0 = 0;
scx = scy = 1;
tx = ty = 0;
x = 200, y = 200;
paintT = paintSc = paintR = paintShx = paintShy = false;
img = new QImage(600,600,QImage::Format_RGB32);
img2 = new QImage("logo.jpeg");
}
Geometric::~Geometric() {
delete img;
delete img2;
img = NULL;
img2 = NULL;
}
void Geometric::makeChange() {
displayLogoDefault = false;
// iteration through whole input image
for(int i = 0; i < img2->width(); i++) {
for(int j = 0; j < img2->height(); j++) {
// calculate new coordinates basing on given 2D transformations values
//I calculated that formula eariler by multiplying/adding matrixes
x = cos(a)*scx*(i-x0) - sin(a)*scy*(j-y0) + shx*sin(a)*scx*(i-x0) + shx*cos(a)*scy*(j-y0);
y = shy*(x) + sin(a)*scx*(i-x0) + cos(a)*scy*(j-y0);
// tx and ty goes for translation. scx and scy for scaling
// shx and shy for shearing and a is angle for rotation
x += (x0 + tx);
y += (y0 + ty);
if(x >= 0 && y >= 0 && x < img->width() && y < img->height()) {
// reverse matrix calculation formula to find proper pixel from input image
float tmx = x - x0 - tx;
float tmy = y - y0 - ty;
float recX = 1/scx * ( cos(-a)*( (tmx + shx*shy*tmx - shx*tmx) ) + sin(-a)*( shy*tmx - tmy ) ) + x0 ;
float recY = 1/scy * ( sin(-a)*(tmx + shx*shy*tmx - shx*tmx) - cos(-a)*(shy*tmx-tmy) ) + y0;
// here the interpolation starts. I calculate the color basing on four points from input image
// that points are taken from the reverse matrix calculation
float a = recX - floorf(recX);
float b = recY - floorf (recY);
if(recX + 1 > img2->width()) recX -= 1;
if(recY + 1 > img2->height()) recY -= 1;
QColor c1 = QColor(img2->pixel(recX, recY));
QColor c2 = QColor(img2->pixel(recX + 1, recY));
QColor c3 = QColor(img2->pixel(recX , recY + 1));
QColor c4 = QColor(img2->pixel(recX + 1, recY + 1));
float colR = b * ((1.0 - a) * (float)c3.red() + a * (float)c4.red()) + (1.0 - b) * ((1.0 - a) * (float)c1.red() + a * (float)c2.red());
float colG = b * ((1.0 - a) * (float)c3.green() + a * (float)c4.green()) + (1.0 - b) * ((1.0 - a) * (float)c1.green() + a * (float)c2.green());
float colB = b * ((1.0 - a) * (float)c3.blue() + a * (float)c4.blue()) + (1.0 - b) * ((1.0 - a) * (float)c1.blue() + a * (float)c2.blue());
if(colR > 255) colR = 255; if(colG > 255) colG = 255; if(colB > 255) colB = 255;
if(colR < 0 ) colR = 0; if(colG < 0 ) colG = 0; if(colB < 0 ) colB = 0;
paintPixel(x, y, colR, colG, colB);
}
}
}
// x0 and y0 are the starting point of image
x0 = abs(x-tx);
y0 = abs(y-ty);
repaint();
}
// function painting a pixel. It works directly on memory
void Geometric::paintPixel(int i, int j, int r, int g, int b) {
unsigned char *ptr = img->bits();
ptr[4 * (img->width() * j + i)] = b;
ptr[4 * (img->width() * j + i) + 1] = g;
ptr[4 * (img->width() * j + i) + 2] = r;
}
void Geometric::paintEvent(QPaintEvent*) {
QPainter p(this);
p.drawImage(0, 0, *img);
if (displayLogoDefault == true) p.drawImage(0, 0, *img2);
}

Create a 3D sphere and 3D box C++

I need to implement a tool to detect intersction between a 3D box and 3D Sphere in c++. Write now I find a way how to detect the intersection using that code.
inline float squared(float v) { return v * v; }
bool doesCubeIntersectSphere(vec3 C1, vec3 C2, vec3 S, float R)
{
float dist_squared = R * R;
/* assume C1 and C2 are element-wise sorted, if not, do that now */
if (S.X < C1.X) dist_squared -= squared(S.X - C1.X);
else if (S.X > C2.X) dist_squared -= squared(S.X - C2.X);
if (S.Y < C1.Y) dist_squared -= squared(S.Y - C1.Y);
else if (S.Y > C2.Y) dist_squared -= squared(S.Y - C2.Y);
if (S.Z < C1.Z) dist_squared -= squared(S.Z - C1.Z);
else if (S.Z > C2.Z) dist_squared -= squared(S.Z - C2.Z);
return dist_squared > 0;
}
What I need is an example of C++ code to create a 3D sphere using origin vector and a radius and
I need to create a 3D sphere using origin vector and a radius and a 3D box through its maximum and minimum corner vector.
I may be mistaken, but (assuming Axis-Aligned boxes):
The length of a vector from origin to corner C1 or C2 should be the radius r, right?
Explanation for my deriviation below: An Axis-Aligned-box with equal distance from center to all corners is a perfect cube. Translating such a cube to the origin puts two of the corners exactly at the line from the origin through the point {x=1,y=1,z=1}. Thus those two corners will have coordinates {d,d,d} and {-d, -d, -d}, where d is "distance" of the corner along the axises X,Y,Z. The distance to say the first corner is squaring and adding all components of the vector, and taking the square root, e.g:
|C1| = |{d,d,d}| = sqrt(d * d + d * d + d * d) = sqrt(3 * d * d)
Therefore solve:
r = sqrt(3 *d * d)
<=>
r * r = 3 * d * d
<=>
d = sqrt(r*r/3)
<=>
d = r/sqrt(3)
This needs to be translated back to the center of the Sphere, thus:
C1 = { S.x+d, S.y+d, S.z+d}
C2 = { S.x-d, S.y-d, S.z-d}
Your explanation is a little vague, so I made some assumptions. Perhaps I'm dead wrong. Anyway here is some non-tested code showing what I mean:
void makeCube(vec3 S, float R, vec3* C1, vec3* C2)
{
static const float sqrt_one_third = sqrtf(1.0f/3.0f);
float d = R * sqrt_one_third;
C1->X = S.X + d;
C1->Y = S.Y + d;
C1->Z = S.Z + d;
C2->X = S.X - d;
C2->Y = S.Y - d;
C2->Z = S.Z - d;
}

distance from given point to given ellipse

I have an ellipse, defined by Center Point, radiusX and radiusY, and I have a Point. I want to find the point on the ellipse that is closest to the given point. In the illustration below, that would be S1.
Now I already have code, but there is a logical error somewhere in it, and I seem to be unable to find it. I broke the problem down to the following code example:
#include <vector>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <math.h>
using namespace std;
void dostuff();
int main()
{
dostuff();
return 0;
}
typedef std::vector<cv::Point> vectorOfCvPoints;
void dostuff()
{
const double ellipseCenterX = 250;
const double ellipseCenterY = 250;
const double ellipseRadiusX = 150;
const double ellipseRadiusY = 100;
vectorOfCvPoints datapoints;
for (int i = 0; i < 360; i+=5)
{
double angle = i / 180.0 * CV_PI;
double x = ellipseRadiusX * cos(angle);
double y = ellipseRadiusY * sin(angle);
x *= 1.4;
y *= 1.4;
x += ellipseCenterX;
y += ellipseCenterY;
datapoints.push_back(cv::Point(x,y));
}
cv::Mat drawing = cv::Mat::zeros( 500, 500, CV_8UC1 );
for (int i = 0; i < datapoints.size(); i++)
{
const cv::Point & curPoint = datapoints[i];
const double curPointX = curPoint.x;
const double curPointY = curPoint.y * -1; //transform from image coordinates to geometric coordinates
double angleToEllipseCenter = atan2(curPointY - ellipseCenterY * -1, curPointX - ellipseCenterX); //ellipseCenterY * -1 for transformation to geometric coords (from image coords)
double nearestEllipseX = ellipseCenterX + ellipseRadiusX * cos(angleToEllipseCenter);
double nearestEllipseY = ellipseCenterY * -1 + ellipseRadiusY * sin(angleToEllipseCenter); //ellipseCenterY * -1 for transformation to geometric coords (from image coords)
cv::Point center(ellipseCenterX, ellipseCenterY);
cv::Size axes(ellipseRadiusX, ellipseRadiusY);
cv::ellipse(drawing, center, axes, 0, 0, 360, cv::Scalar(255));
cv::line(drawing, curPoint, cv::Point(nearestEllipseX,nearestEllipseY*-1), cv::Scalar(180));
}
cv::namedWindow( "ellipse", CV_WINDOW_AUTOSIZE );
cv::imshow( "ellipse", drawing );
cv::waitKey(0);
}
It produces the following image:
You can see that it actually finds "near" points on the ellipse, but it are not the "nearest" points. What I intentionally want is this: (excuse my poor drawing)
would you extent the lines in the last image, they would cross the center of the ellipse, but this is not the case for the lines in the previous image.
I hope you get the picture. Can anyone tell me what I am doing wrong?
Consider a bounding circle around the given point (c, d), which passes through the nearest point on the ellipse. From the diagram it is clear that the closest point is such that a line drawn from it to the given point must be perpendicular to the shared tangent of the ellipse and circle. Any other points would be outside the circle and so must be further away from the given point.
So the point you are looking for is not the intersection between the line and the ellipse, but the point (x, y) in the diagram.
Gradient of tangent:
Gradient of line:
Condition for perpedicular lines - product of gradients = -1:
When rearranged and substituted into the equation of your ellipse...
...this will give two nasty quartic (4th-degree polynomial) equations in terms of either x or y. AFAIK there are no general analytical (exact algebraic) methods to solve them. You could try an iterative method - look up the Newton-Raphson iterative root-finding algorithm.
Take a look at this very good paper on the subject:
http://www.spaceroots.org/documents/distance/distance-to-ellipse.pdf
Sorry for the incomplete answer - I totally blame the laws of mathematics and nature...
EDIT: oops, i seem to have a and b the wrong way round in the diagram xD
There is a relatively simple numerical method with better convergence than Newtons Method. I have a blog post about why it works http://wet-robots.ghost.io/simple-method-for-distance-to-ellipse/
This implementation works without any trig functions:
def solve(semi_major, semi_minor, p):
px = abs(p[0])
py = abs(p[1])
tx = 0.707
ty = 0.707
a = semi_major
b = semi_minor
for x in range(0, 3):
x = a * tx
y = b * ty
ex = (a*a - b*b) * tx**3 / a
ey = (b*b - a*a) * ty**3 / b
rx = x - ex
ry = y - ey
qx = px - ex
qy = py - ey
r = math.hypot(ry, rx)
q = math.hypot(qy, qx)
tx = min(1, max(0, (qx * r / q + ex) / a))
ty = min(1, max(0, (qy * r / q + ey) / b))
t = math.hypot(ty, tx)
tx /= t
ty /= t
return (math.copysign(a * tx, p[0]), math.copysign(b * ty, p[1]))
Credit to Adrian Stephens for the Trig-Free Optimization.
Here is the code translated to C# implemented from this paper to solve for the ellipse:
http://www.geometrictools.com/Documentation/DistancePointEllipseEllipsoid.pdf
Note that this code is untested - if you find any errors let me know.
//Pseudocode for robustly computing the closest ellipse point and distance to a query point. It
//is required that e0 >= e1 > 0, y0 >= 0, and y1 >= 0.
//e0,e1 = ellipse dimension 0 and 1, where 0 is greater and both are positive.
//y0,y1 = initial point on ellipse axis (center of ellipse is 0,0)
//x0,x1 = intersection point
double GetRoot ( double r0 , double z0 , double z1 , double g )
{
double n0 = r0*z0;
double s0 = z1 - 1;
double s1 = ( g < 0 ? 0 : Math.Sqrt(n0*n0+z1*z1) - 1 ) ;
double s = 0;
for ( int i = 0; i < maxIter; ++i ){
s = ( s0 + s1 ) / 2 ;
if ( s == s0 || s == s1 ) {break; }
double ratio0 = n0 /( s + r0 );
double ratio1 = z1 /( s + 1 );
g = ratio0*ratio0 + ratio1*ratio1 - 1 ;
if (g > 0) {s0 = s;} else if (g < 0) {s1 = s ;} else {break ;}
}
return s;
}
double DistancePointEllipse( double e0 , double e1 , double y0 , double y1 , out double x0 , out double x1)
{
double distance;
if ( y1 > 0){
if ( y0 > 0){
double z0 = y0 / e0;
double z1 = y1 / e1;
double g = z0*z0+z1*z1 - 1;
if ( g != 0){
double r0 = (e0/e1)*(e0/e1);
double sbar = GetRoot(r0 , z0 , z1 , g);
x0 = r0 * y0 /( sbar + r0 );
x1 = y1 /( sbar + 1 );
distance = Math.Sqrt( (x0-y0)*(x0-y0) + (x1-y1)*(x1-y1) );
}else{
x0 = y0;
x1 = y1;
distance = 0;
}
}
else // y0 == 0
x0 = 0 ; x1 = e1 ; distance = Math.Abs( y1 - e1 );
}else{ // y1 == 0
double numer0 = e0*y0 , denom0 = e0*e0 - e1*e1;
if ( numer0 < denom0 ){
double xde0 = numer0/denom0;
x0 = e0*xde0 ; x1 = e1*Math.Sqrt(1 - xde0*xde0 );
distance = Math.Sqrt( (x0-y0)*(x0-y0) + x1*x1 );
}else{
x0 = e0;
x1 = 0;
distance = Math.Abs( y0 - e0 );
}
}
return distance;
}
The following python code implements the equations described at "Distance from a Point to an Ellipse" and uses newton's method to find the roots and from that the closest point on the ellipse to the point.
Unfortunately, as can be seen from the example, it seems to only be accurate outside the ellipse. Within the ellipse weird things happen.
from math import sin, cos, atan2, pi, fabs
def ellipe_tan_dot(rx, ry, px, py, theta):
'''Dot product of the equation of the line formed by the point
with another point on the ellipse's boundary and the tangent of the ellipse
at that point on the boundary.
'''
return ((rx ** 2 - ry ** 2) * cos(theta) * sin(theta) -
px * rx * sin(theta) + py * ry * cos(theta))
def ellipe_tan_dot_derivative(rx, ry, px, py, theta):
'''The derivative of ellipe_tan_dot.
'''
return ((rx ** 2 - ry ** 2) * (cos(theta) ** 2 - sin(theta) ** 2) -
px * rx * cos(theta) - py * ry * sin(theta))
def estimate_distance(x, y, rx, ry, x0=0, y0=0, angle=0, error=1e-5):
'''Given a point (x, y), and an ellipse with major - minor axis (rx, ry),
its center at (x0, y0), and with a counter clockwise rotation of
`angle` degrees, will return the distance between the ellipse and the
closest point on the ellipses boundary.
'''
x -= x0
y -= y0
if angle:
# rotate the points onto an ellipse whose rx, and ry lay on the x, y
# axis
angle = -pi / 180. * angle
x, y = x * cos(angle) - y * sin(angle), x * sin(angle) + y * cos(angle)
theta = atan2(rx * y, ry * x)
while fabs(ellipe_tan_dot(rx, ry, x, y, theta)) > error:
theta -= ellipe_tan_dot(
rx, ry, x, y, theta) / \
ellipe_tan_dot_derivative(rx, ry, x, y, theta)
px, py = rx * cos(theta), ry * sin(theta)
return ((x - px) ** 2 + (y - py) ** 2) ** .5
Here's an example:
rx, ry = 12, 35 # major, minor ellipse axis
x0 = y0 = 50 # center point of the ellipse
angle = 45 # ellipse's rotation counter clockwise
sx, sy = s = 100, 100 # size of the canvas background
dist = np.zeros(s)
for x in range(sx):
for y in range(sy):
dist[x, y] = estimate_distance(x, y, rx, ry, x0, y0, angle)
plt.imshow(dist.T, extent=(0, sx, 0, sy), origin="lower")
plt.colorbar()
ax = plt.gca()
ellipse = Ellipse(xy=(x0, y0), width=2 * rx, height=2 * ry, angle=angle,
edgecolor='r', fc='None', linestyle='dashed')
ax.add_patch(ellipse)
plt.show()
Which generates an ellipse and the distance from the boundary of the ellipse as a heat map. As can be seen, at the boundary the distance is zero (deep blue).
Given an ellipse E in parametric form and a point P
the square of the distance between P and E(t) is
The minimum must satisfy
Using the trigonometric identities
and substituting
yields the following quartic equation:
Here's an example C function that solves the quartic directly and computes sin(t) and cos(t) for the nearest point on the ellipse:
void nearest(double a, double b, double x, double y, double *ecos_ret, double *esin_ret) {
double ax = fabs(a*x);
double by = fabs(b*y);
double r = b*b - a*a;
double c, d;
int switched = 0;
if (ax <= by) {
if (by == 0) {
if (r >= 0) { *ecos_ret = 1; *esin_ret = 0; }
else { *ecos_ret = 0; *esin_ret = 1; }
return;
}
c = (ax - r) / by;
d = (ax + r) / by;
} else {
c = (by + r) / ax;
d = (by - r) / ax;
switched = 1;
}
double cc = c*c;
double D0 = 12*(c*d + 1); // *-4
double D1 = 54*(d*d - cc); // *4
double D = D1*D1 + D0*D0*D0; // *16
double St;
if (D < 0) {
double t = sqrt(-D0); // *2
double phi = acos(D1 / (t*t*t));
St = 2*t*cos((1.0/3)*phi); // *2
} else {
double Q = cbrt(D1 + sqrt(D)); // *2
St = Q - D0 / Q; // *2
}
double p = 3*cc; // *-2
double SS = (1.0/3)*(p + St); // *4
double S = sqrt(SS); // *2
double q = 2*cc*c + 4*d; // *2
double l = sqrt(p - SS + q / S) - S - c; // *2
double ll = l*l; // *4
double ll4 = ll + 4; // *4
double esin = (4*l) / ll4;
double ecos = (4 - ll) / ll4;
if (switched) {
double t = esin;
esin = ecos;
ecos = t;
}
*ecos_ret = copysign(ecos, a*x);
*esin_ret = copysign(esin, b*y);
}
Try it online!
You just need to calculate the intersection of the line [P1,P0] to your elipse which is S1.
If the line equeation is:
and the elipse equesion is:
than the values of S1 will be:
Now you just need to calculate the distance between S1 to P1 , the formula (for A,B points) is:
I've solved the distance issue via focal points.
For every point on the ellipse
r1 + r2 = 2*a0
where
r1 - Euclidean distance from the given point to focal point 1
r2 - Euclidean distance from the given point to focal point 2
a0 - semimajor axis length
I can also calculate the r1 and r2 for any given point which gives me another ellipse that this point lies on that is concentric to the given ellipse. So the distance is
d = Abs((r1 + r2) / 2 - a0)
As propposed by user3235832
you shall solve quartic equation to find the normal to the ellipse (https://www.mathpages.com/home/kmath505/kmath505.htm). With good initial value only few iterations are needed (I use it myself). As an initial value I use S1 from your picture.
The fastest method I guess is
http://wwwf.imperial.ac.uk/~rn/distance2ellipse.pdf
Which has been mentioned also by Matt but as he found out the method doesn't work very well inside of ellipse.
The problem is the theta initialization.
I proposed an stable initialization:
Find the intersection of ellipse and horizontal line passing the point.
Find the other intersection using vertical line.
Choose the one that is closer the point.
Calculate the initial angle based on that point.
I got good results with no issue inside and outside:
As you can see in the following image it just iterated about 3 times to reach 1e-8. Close to axis it is 1 iteration.
The C++ code is here:
double initialAngle(double a, double b, double x, double y) {
auto abs_x = fabs(x);
auto abs_y = fabs(y);
bool isOutside = false;
if (abs_x > a || abs_y > b) isOutside = true;
double xd, yd;
if (!isOutside) {
xd = sqrt((1.0 - y * y / (b * b)) * (a * a));
if (abs_x > xd)
isOutside = true;
else {
yd = sqrt((1.0 - x * x / (a * a)) * (b * b));
if (abs_y > yd)
isOutside = true;
}
}
double t;
if (isOutside)
t = atan2(a * y, b * x); //The point is outside of ellipse
else {
//The point is inside
if (xd < yd) {
if (x < 0) xd = -xd;
t = atan2(y, xd);
}
else {
if (y < 0) yd = -yd;
t = atan2(yd, x);
}
}
return t;
}
double distanceToElipse(double a, double b, double x, double y, int maxIter = 10, double maxError = 1e-5) {
//std::cout <<"p="<< x << "," << y << std::endl;
auto a2mb2 = a * a - b * b;
double t = initialAngle(a, b, x, y);
auto ct = cos(t);
auto st = sin(t);
int i;
double err;
for (i = 0; i < maxIter; i++) {
auto f = a2mb2 * ct * st - x * a * st + y * b * ct;
auto fp = a2mb2 * (ct * ct - st * st) - x * a * ct - y * b * st;
auto t2 = t - f / fp;
err = fabs(t2 - t);
//std::cout << i + 1 << " " << err << std::endl;
t = t2;
ct = cos(t);
st = sin(t);
if (err < maxError) break;
}
auto dx = a * ct - x;
auto dy = b * st - y;
//std::cout << a * ct << "," << b * st << std::endl;
return sqrt(dx * dx + dy * dy);
}