GeographicLib: moving a latLon coordinate and back creates an offset - c++

Using the following code (emplyoing GeographicLib) which is moving a coordinate and back again creates an offset to the original starting point.
The difference is growing with the distance of movement and depends on the Azimuth. The same is true for using both GeodesicExact and Geodesic.
What i want to achieve in the end is creating a latLon shape by moving the starting coordinate.
Is there are an exact/better way of doing this or do I miss something fundamental?
inline double distanceInMeters(const GeoCoords &_c1, const GeoCoords &_c2) {
GeodesicExact geod = geodWGS84(); // GeodesicExact::WGS84()
double meters;
geod.Inverse(_c1.Latitude(), _c1.Longitude(),
_c2.Latitude(), _c2.Longitude(),
meters);
return meters;
}
// move coord _byMeters in direction _azimuth
// inexact with horiz moves !!!
inline GeoCoords move(const GeoCoords &_coords, const double &_azimuth, const double &_byMeters) {
GeodesicExact geod = geodWGS84(); // GeodesicExact::WGS84()
double latOut, lngOut;
geod.Direct(_coords.Latitude(), _coords.Longitude(), _azimuth, _byMeters, latOut, lngOut);
return GeoCoords(latOut, lngOut);
}
inline void testDistanceMove() {
GeoCoords c(12.3456789, 12.3456789);
GeoCoords cc = c;
double dist = 123459998.6789; // meters
bool bHorz = true; // <-- creates some offset???
bool bVert = true; // almost exact
if (bHorz) cc = move(cc, Azimuth::WEST, dist); // 270.
if (bVert) cc = move(cc, Azimuth::SOUTH, dist); // 180
if (bHorz) cc = move(cc, Azimuth::EAST, dist); // 90.
if (bVert) cc = move(cc, Azimuth::NORTH, dist); // 0.
ofLogNotice((__func__)) << "c : " << toString(c);
ofLogNotice((__func__)) << "cc: " << toString(cc);
double diff = distanceInMeters(c, cc);
ofLogNotice((__func__)) << "diff: " << ofToString(diff, 12) << " m";
}

Simple notions of planar notions of geometry don't apply to a sphere on an ellipsoid. For example the sum of the interior angles of a quadrilateral is more than 360°. You would get approximate closure if the distance were small (order of 1km) and you're not close to a pole; however your distance is more than 3 times the circumference of the earth so all bets are off.
ADDENDUM
To help picture the issues, consider starting 1 meter south of the north pole and draw the 4 edges (successively west, south, east, and north) of distance 1 meter. Because 1 meter is much less than the radius of the earth, this is a planar problem. The polyline then looks like (the dashed lines are meridians)
The picture looks even more strange if you start within 1 meter of the south pole.

Related

Performing a cubic fit to a set of four points to extrapolate a "local" path, Or working alternatives?

Problem: Generate a extrapolated local path which provides path points ahead of the max FOV.
Situation: Having a car move round an unknown looped track of varying shape using a field of view which is limited so can only provide reliably 3 points ahead of the car and the car's current position. Note for more information the tack is defined by cone gates and the information provided about the locations of said gates is 2D (x,y).
Background: I have successfully generated a vector of mid points between gates however wish to generate an extrapolated path for the motion control algorithm to use. the format of this path needs to be a sequence of PathPoint (s) which contain (x,y velocity, gravity). note that gravity is just used to cap the maximum acceleration and is not important to the situation nor is velocity as the post is only concerned about generating respective (x,y) co-ordinates.
Attempted Solution Methodology: To fit two cubic functions for X positions and Y positions using the set of four points i.e f(x) and g(y). These functions should then be provided as the desired (f(x),g(y)) positions as we vary the look ahead distance to supply 20 path points.
Question: I do not believe this method to be correct both in theory and in implementation can anyone think of an easy/simple methodology to achieve the out come of having position in x axis and position in y axis to be the functions from the argument of overall distance from the car?
double PathPlanningClass::squared(double arg)
{
return arg*arg;
}
double PathPlanningClass::cubed(double arg)
{
return arg*arg*arg;
}
//https://eigen.tuxfamily.org/dox/group__TutorialLinearAlgebra.html
void PathPlanningClass::Coeffs()
{
Eigen::Matrix4f Aone;
Eigen::Vector4f bone;
Aone << _x, squared(_x), cubed(_x), _midpoints[0].getX(), squared(_midpoints[0].getX()), cubed(_midpoints[0].getX()), _midpoints[1].getX(), squared(_midpoints[1].getX()), cubed(_midpoints[1].getX()), _midpoints[_midpoints.size()-1].getX(), squared(_midpoints[_midpoints.size()-1].getX()), cubed(_midpoints[_midpoints.size()-1].getX());
bone << _y, _midpoints[0].getY(), _midpoints[1].getY(), _midpoints[_midpoints.size()-1].getY();
Eigen::Vector4f x = Aone.colPivHouseholderQr().solve(bone);
_Ax = x(1);
_Bx = x(2);
_Cx = x(3);
_Dx = x(4);
Eigen::Matrix4f Atwo;
Eigen::Vector4f btwo;
Atwo << _y, squared(_y), cubed(_y), _midpoints[0].getY(), squared(_midpoints[0].getY()), cubed(_midpoints[0].getY()), _midpoints[1].getY(), squared(_midpoints[1].getY()), cubed(_midpoints[1].getY()), _midpoints[_midpoints.size()-1].getY(), squared(_midpoints[_midpoints.size()-1].getY()), cubed(_midpoints[_midpoints.size()-1].getY());
btwo << _x, _midpoints[0].getX(), _midpoints[1].getX(), _midpoints[_midpoints.size()-1].getX();
Eigen::Vector4f y = Aone.colPivHouseholderQr().solve(bone);
_Ay = y(1);
_By = y(2);
_Cy = y(3);
_Dx = y(4);
return;
}
void PathPlanningClass::extrapolate()
{
// number of desired points
int numOfpoints = 20;
// distance to be extrapolated from car's location
double distance = 10;
// the argument for g(y) and f(x)
double arg = distance/numOfpoints;
for (int i = 0 ; i < numOfpoints; i++)
{
double farg = _Ax + _Bx*arg*i + _Cx*squared(arg*i) + _Dx*cubed(arg*i);
double garg = _Ay + _By*arg*i + _Cy*squared(arg*i) + _Dy*cubed(arg*i);
PathPoint newPoint(farg, garg, velocity(_x, _y, _yaw), 9.8);
_path.push_back(newPoint);
}
return;
}

Find Minimum area ellipse enclosing a set of points in c++

I have a set of 2D points. I need to find a minimum area ellipse enclosing all the points. Could someone give an idea of how the problem has to be tackled. For a circle it was simple. The largest distance between the center and the point. But for an ellipse its quite complicated which I do not know. I have to implement this in c++.
These don't go as far as giving you C++ code, but they include in-depth discussion of effective algorithms for what you need to do.
https://www.cs.cornell.edu/cv/OtherPdf/Ellipse.pdf
http://www.stsci.edu/~RAB/Backup%20Oct%2022%202011/f_3_CalculationForWFIRSTML/Gaertner%20&%20Schoenherr.pdf
The other answers here give approximation schemes or only provide links. We can do better.
Your question is addressed by the paper "Smallest Enclosing Ellipses -- Fast and Exact" by Gärtner and Schönherr (1997). The same authors provide a C++ implementation in their 1998 paper "Smallest Enclosing Ellipses -- An Exact and Generic Implementation in C++". This algorithm is implemented in a more usable form in CGAL here.
However, CGAL only provides the general equation for the ellipse, so we use a few transforms to get a parametric equation suitable for plotting.
All this is included in the implementation below.
Using WebPlotDigitizer to extract your data while choosing arbitrary values for the lengths of the axes, but preserving their aspect ratio, gives:
-1.1314123177813773 4.316368664322679
1.345680085331649 5.1848164974519015
2.2148682495160603 3.9139687117291504
0.9938150357523803 3.2732678860664475
-0.24524315569075128 3.0455750009876343
-1.4493153715482157 2.4049282977126376
0.356472958558844 0.0699802473037554
2.8166270295895384 0.9211630387547896
3.7889384901038987 -0.8484766720657362
1.3457654169794182 -1.6996053411290646
2.9287101489353287 -3.1919219373444463
0.8080480385572635 -3.990389523169913
0.46847074625686425 -4.008682890214516
-1.6521060324734327 -4.8415723146209455
Fitting this using the program below gives:
a = 3.36286
b = 5.51152
cx = 0.474112
cy = -0.239756
theta = -0.0979706
We can then plot this with gnuplot
set parametric
plot "points" pt 7 ps 2, [0:2*pi] a*cos(t)*cos(theta) - b*sin(t)*sin(theta) + cx, a*cos(t)*sin(theta) + b*sin(t)*cos(theta) +
cy lw 2
to get
Implementation
The code below does this:
// Compile with clang++ -DBOOST_ALL_NO_LIB -DCGAL_USE_GMPXX=1 -O2 -g -DNDEBUG -Wall -Wextra -pedantic -march=native -frounding-math main.cpp -lgmpxx -lmpfr -lgmp
#include <CGAL/Cartesian.h>
#include <CGAL/Min_ellipse_2.h>
#include <CGAL/Min_ellipse_2_traits_2.h>
#include <CGAL/Exact_rational.h>
#include <cassert>
#include <cmath>
#include <fstream>
#include <iostream>
#include <string>
#include <vector>
typedef CGAL::Exact_rational NT;
typedef CGAL::Cartesian<NT> K;
typedef CGAL::Point_2<K> Point;
typedef CGAL::Min_ellipse_2_traits_2<K> Traits;
typedef CGAL::Min_ellipse_2<Traits> Min_ellipse;
struct EllipseCanonicalEquation {
double semimajor; // Length of semi-major axis
double semiminor; // Length of semi-minor axis
double cx; // x-coordinate of center
double cy; // y-coordinate of center
double theta; // Rotation angle
};
std::vector<Point> read_points_from_file(const std::string &filename){
std::vector<Point> ret;
std::ifstream fin(filename);
float x,y;
while(fin>>x>>y){
std::cout<<x<<" "<<y<<std::endl;
ret.emplace_back(x, y);
}
return ret;
}
// Uses "Smallest Enclosing Ellipses -- An Exact and Generic Implementation in C++"
// under the hood.
EllipseCanonicalEquation get_min_area_ellipse_from_points(const std::vector<Point> &pts){
// Compute minimum ellipse using randomization for speed
Min_ellipse me2(pts.data(), pts.data()+pts.size(), true);
std::cout << "done." << std::endl;
// If it's degenerate, the ellipse is a line or a point
assert(!me2.is_degenerate());
// Get coefficients for the equation
// r*x^2 + s*y^2 + t*x*y + u*x + v*y + w = 0
double r, s, t, u, v, w;
me2.ellipse().double_coefficients(r, s, t, u, v, w);
// Convert from CGAL's coefficients to Wikipedia's coefficients
// A*x^2 + B*x*y + C*y^2 + D*x + E*y + F = 0
const double A = r;
const double B = t;
const double C = s;
const double D = u;
const double E = v;
const double F = w;
// Get the canonical form parameters
// Using equations from https://en.wikipedia.org/wiki/Ellipse#General_ellipse
const auto a = -std::sqrt(2*(A*E*E+C*D*D-B*D*E+(B*B-4*A*C)*F)*((A+C)+std::sqrt((A-C)*(A-C)+B*B)))/(B*B-4*A*C);
const auto b = -std::sqrt(2*(A*E*E+C*D*D-B*D*E+(B*B-4*A*C)*F)*((A+C)-std::sqrt((A-C)*(A-C)+B*B)))/(B*B-4*A*C);
const auto cx = (2*C*D-B*E)/(B*B-4*A*C);
const auto cy = (2*A*E-B*D)/(B*B-4*A*C);
double theta;
if(B!=0){
theta = std::atan(1/B*(C-A-std::sqrt((A-C)*(A-C)+B*B)));
} else if(A<C){
theta = 0;
} else { //A>C
theta = M_PI;
}
return EllipseCanonicalEquation{a, b, cx, cy, theta};
}
int main(int argc, char** argv){
if(argc!=2){
std::cerr<<"Provide name of input containing a list of x,y points"<<std::endl;
std::cerr<<"Syntax: "<<argv[0]<<" <Filename>"<<std::endl;
return -1;
}
const auto pts = read_points_from_file(argv[1]);
const auto eq = get_min_area_ellipse_from_points(pts);
// Convert canonical equation for rotated ellipse to parametric based on:
// https://math.stackexchange.com/a/2647450/14493
std::cout << "Ellipse has the parametric equation " << std::endl;
std::cout << "x(t) = a*cos(t)*cos(theta) - b*sin(t)*sin(theta) + cx"<<std::endl;
std::cout << "y(t) = a*cos(t)*sin(theta) + b*sin(t)*cos(theta) + cy"<<std::endl;
std::cout << "with" << std::endl;
std::cout << "a = " << eq.semimajor << std::endl;
std::cout << "b = " << eq.semiminor << std::endl;
std::cout << "cx = " << eq.cx << std::endl;
std::cout << "cy = " << eq.cy << std::endl;
std::cout << "theta = " << eq.theta << std::endl;
return 0;
}
Not sure if I can prove it, but it seems to me that the optimal solution would be characterized by tangenting (at least) 3 of the points, while all the other points are inside the ellipse (think about it!). So if nothing else, you should be able to brute force it by checking all ~n^3 triplets of points and checking if they define a solution. Should be possible to improve on that by removing all points that would have to be strictly inside any surrounding ellipse, but I'm not sure how that could be done. Maybe by sorting the points by x and y coordinates and then doing something fancy.
Not a complete solution, but it's a start.
EDIT:
Unfortunately 3 points aren't enough to define an ellipse. But perhaps if you restrict it to the ellipse of the smallest area tangenting 3 points?
as Rory Daulton suggest you need to clearly specify the constraints of solution and removal of any will greatly complicates things. For starters assume this for now:
it is 2D problem
ellipse is axis aligned
center is arbitrary instead of (0,0)
I would attack this as standard genere and test problem with approximation search (which is hybrid between binary search and linear search) to speed it up (but you can also try brute force from start so you see if it works).
compute constraints of solution
To limit the search you need to find approximate placement position and size of the ellipse. For that you can use out-scribed circle for your points. It is clear that ellipse area will be less or equal to the circle and placement will be near by. The circle does not have to be necessarily the smallest one possible so we can use for example this:
find bounding box of the points
let the circle be centered to that bounding box and with radius be the max distance from its center to any of the points.
This will be O(n) complexity where n is number of your points.
search "all" the possible ellipses and remember best solution
so we need to find ellipse center (x0,y0) and semi-axises rx,ry while area = M_PI*rx*ry is minimal. With approximation search each variable has factor of O(log(m)) and each iteration need to test validity which is O(n) so final complexity would be O(n.log^4(m)) where m is average number of possible variations of each search parameter (dependent on accuracy and search constraints). With simple brute search it would be O(n.m^4) which is really scary especially for floating point where m can be really big.
To speed this up we know that the area of ellipse will be less then or equal to area of found circle so we can ignore all the bigger ellipses. The constrains to rx,ry can be derived from the aspect ratio of the bounding box +/- some reserve.
Here simple small C++ example using that approx class from link above:
//---------------------------------------------------------------------------
// input points
const int n=15; // number of random points to test
float pnt[n][2];
// debug bounding box
float box_x0,box_y0,box_x1,box_y1;
// debug outscribed circle
float circle_x,circle_y,circle_r;
// solution ellipse
float ellipse_x,ellipse_y,ellipse_rx,ellipse_ry;
//---------------------------------------------------------------------------
void compute(float x0,float y0,float x1,float y1) // cal with bounding box where you want your points will be generated
{
int i;
float x,y;
// generate n random 2D points inside defined area
Randomize();
for (i=0;i<n;i++)
{
pnt[i][0]=x0+(x1-x0)*Random();
pnt[i][1]=y0+(y1-y0)*Random();
}
// compute bounding box
x0=pnt[0][0]; x1=x0;
y0=pnt[0][1]; y1=y0;
for (i=0;i<n;i++)
{
x=pnt[i][0]; if (x0>x) x0=x; if (x1<x) x1=x;
y=pnt[i][1]; if (y0>y) y0=y; if (y1<y) y1=y;
}
box_x0=x0; box_x1=x1;
box_y0=y0; box_y1=y1;
// "outscribed" circle
circle_x=0.5*(x0+x1);
circle_y=0.5*(y0+y1);
circle_r=0.0;
for (i=0;i<n;i++)
{
x=pnt[i][0]-circle_x; x*=x;
y=pnt[i][1]-circle_y; y*=y; x+=y;
if (circle_r<x) circle_r=x;
}
circle_r=sqrt(circle_r);
// smallest area ellipse
int N;
double m,e,step,area;
approx ax,ay,aa,ab;
N=3; // number of recursions each one improves accuracy with factor 10
area=circle_r*circle_r; // solution will not be bigger that this
step=((x1-x0)+(y1-y0))*0.05; // initial position/size step for the search as 1/10 of avg bounding box size
for (ax.init( x0, x1,step,N,&e);!ax.done;ax.step()) // search x0
for (ay.init( y0, y1,step,N,&e);!ay.done;ay.step()) // search y0
for (aa.init(0.5*(x1-x0),2.0*circle_r,step,N,&e);!aa.done;aa.step()) // search rx
for (ab.init(0.5*(y1-y0),2.0*circle_r,step,N,&e);!ab.done;ab.step()) // search ry
{
e=aa.a*ab.a;
// is ellipse outscribed?
if (aa.a>=ab.a)
{
m=aa.a/ab.a; // convert to circle of radius rx
for (i=0;i<n;i++)
{
x=(pnt[i][0]-ax.a); x*=x;
y=(pnt[i][1]-ay.a)*m; y*=y;
// throw away this ellipse if not
if (x+y>aa.a*aa.a) { e=2.0*area; break; }
}
}
else{
m=ab.a/aa.a; // convert to circle of radius ry
for (i=0;i<n;i++)
{
x=(pnt[i][0]-ax.a)*m; x*=x;
y=(pnt[i][1]-ay.a); y*=y;
// throw away this ellipse if not
if (x+y>ab.a*ab.a) { e=2.0*area; break; }
}
}
}
ellipse_x =ax.aa;
ellipse_y =ay.aa;
ellipse_rx=aa.aa;
ellipse_ry=ab.aa;
}
//---------------------------------------------------------------------------
Even this simple example with only 15 points took around 2 seconds to compute. You can improve performance by adding heuristics like test only areas lower then circle_r^2 etc, or better select solution area with some math rule. If you use brute force instead of approximation search that expect the computation time could be even minutes or more hence the O(scary)...
Beware this example will not work for any aspect ratio of the points as I hardcoded the upper bound for rx,ry to 2.0*circle_r which may not be enough. Instead you can compute the upper bound from aspect ratio of the points and or condition that rx*ry<=circle_r^2...
There are also other ("faster") methods for example variation of CCD (cyclic coordinate descend) can be used. But such methods usually can not guarantee that optimal solution will be found or any at all ...
Here overview of the example output:
The dots are individual points from pnt[n], the gray dashed stuff are bounding box and used out-scribed circle. The green ellipse is found solution.
Code for MVEE (minimal volume enclosing ellipse) can be found here, and works even for non-centered and rotated ellipses:
https://github.com/chrislarson1/MVEE
My related code:
bool _mvee(const std::vector<cv::Point> & contour, cv::RotatedRect & ellipse, const float epsilon, const float lmc) {
std::vector<cv::Point> hull;
cv::convexHull(contour, hull);
mvee::Mvee B;
std::vector<std::vector<double>> X;
// speedup: the mve-ellipse on the convex hull should be the same theoretically as the one on the entire contour
for (const auto &points : hull) {
std::vector<double> p = {double(points.x), double(points.y)};
X.push_back(p); // speedup: the mve-ellipse on part of the points (e.g. one every 4) should be similar
}
B.compute(X, epsilon, lmc); // <-- call to the MVEE algorithm
cv::Point2d center(B.centroid()[0], B.centroid()[1]);
cv::Size2d size(B.radii()[0] * 2, B.radii()[1] * 2);
float angle = asin(B.pose()[1][0]) * 180 / CV_PI;
if (B.pose()[0][0] < 0) angle *= -1;
ellipse = cv::RotatedRect(center, size, angle);
if (std::isnan(ellipse.size.height)) {
LOG_ERR("pupil with nan size");
return false;
}
return true;
}

Angle of a line with relative to another line

Although this is an easy question, I might be thinking in a wrong way so could not find the correct way.
Imagine I have a line, say Line 1, starting from m_StartPoint1 and ending at m_EndPoint1. I want to draw another line, say Line 2, starting from m_EndPoint1 and having a constant angle of alpha with Line 1. Basically my goal is to draw an arrow.
I am using the following code to calculate the x,y coordinates of Line 2.
const float ARROW_ANGLE=-PI/8.0;
wxPoint p;
p.x=m_EndPoint.x+ARROW_LENGTH*sin(ARROW_ANGLE);
p.y=m_EndPoint.y+ARROW_LENGTH*cos(ARROW_ANGLE);
m_ArrowHead1=new CLine(m_EndPoint,p,color,PenWidth); //Draws a line from m_EndPoint to p
This calculation works well when the angle of Line 1 is less than 90 (in degrees). However, when the angle of line 1 changes the arrow does not show correctly. Basically the user should be able to draw Line 1 as he/she wants and arrow line should show correctly regardless of the angle of Line 1.
I have expressed line 1 as vector and got its angle through the following code:
class CVector2D
{
wxPoint m_StartPoint, m_EndPoint;
public:
CVector2D():m_StartPoint(),m_EndPoint() {}
CVector2D(wxPoint p1, wxPoint p2):m_StartPoint(p1),m_EndPoint(p2) {}
float GetSlope(void)
{
return float(m_EndPoint.y-m_StartPoint.y)/float(m_EndPoint.x-m_StartPoint.x);
}
float GetSlopeAngleInRadians()
{
/*Will return the angle of the vector in radians
* The angle is the counterclockwise rotation therefore it is negative
*/
float slope=GetSlope();
float InRadians=atan2(float(m_EndPoint.y-m_StartPoint.y),float(m_EndPoint.x-m_StartPoint.x));
if(InRadians<=0) return InRadians;
return -(2*PI-InRadians);
}
};
And then I tried to calculate with the following code:
CVector2D vector(m_StartPoint,m_EndPoint);
float vector_angle=vector.GetSlopeAngleInRadians();
float total_angle=vector_angle+ARROW_ANGLE;
wxPoint p;
p.x=m_EndPoint.x+ARROW_LENGTH*cos(total_angle);
p.y=m_EndPoint.y+ARROW_LENGTH*sin(total_angle);
m_ArrowHead1=new CLine(m_EndPoint,p,color,PenWidth);
However, this code did not work either. Any ideas will be appreciated.
The difficulty here is perhaps more about math than programming. You need however to be careful to handle the singularities.
How do you guard against division by zero at 90 degrees in:
float GetSlope(void)
{
return float(m_EndPoint.y-m_StartPoint.y)/float(m_EndPoint.x-m_StartPoint.x);
}
I recommend dot product based math, see:
How to find out if the angle between two vectors is external or internal?
PS
Why not use double instead of float?
I think, you are overcomplicating slightly. std::atan2 returns an angle within [-PI,PI] range already. All you have to do is to add correctly the delta angle. If I understood your problem correctly, you are looking for something like this (code on ideone.com):
#include <iostream>
#include <cmath>
namespace so
{
struct _point_2d_
{
float x{};
float y{};
};
struct _line_2d_
{
_point_2d_ start{};
_point_2d_ end{};
};
_line_2d_ create_arrow(float _angle_delta, float _length, _line_2d_ const & _base)
{
float angle_arrow_{std::atan2(_base.end.y - _base.start.y, _base.end.x - _base.start.x) + _angle_delta};
_line_2d_ line_arrow_{};
line_arrow_.start.x = _base.end.x;
line_arrow_.start.y = _base.end.y;
line_arrow_.end.x = line_arrow_.start.x + _length * std::cos(angle_arrow_);
line_arrow_.end.y = line_arrow_.start.y + _length * std::sin(angle_arrow_);
return (line_arrow_);
}
}
int main()
{
so::_line_2d_ base_{};
float angle_delta_{0.5236f};
float length_{1.0f};
base_.start.x = 1.0f;
base_.start.y = 1.0f;
base_.end.x = -1.0f;
base_.end.y = 1.0f;
so::_line_2d_ arrow_counter{so::create_arrow(3.1416f - angle_delta_, length_, base_)};
std::cout << "(" << arrow_counter.start.x << ", " << arrow_counter.start.y << ") - (" << arrow_counter.end.x << ", "
<< arrow_counter.end.y << ")" << std::endl;
so::_line_2d_ arrow_clock{so::create_arrow(-3.1416f + angle_delta_, length_, base_)};
std::cout << "(" << arrow_clock.start.x << ", " << arrow_clock.start.y << ") - (" << arrow_clock.end.x << ", "
<< arrow_clock.end.y << ")" << std::endl;
return (0);
}
Program output:
(-1, 1) - (-0.133971, 0.500006)
(-1, 1) - (-0.133972, 1.49999)
first, to draw a line of length ARROW_LENGTH from m_EndPoint to p you should use this (I think there is an error in your formula):
wxPoint p;
p.x=m_EndPoint.x+ARROW_LENGTH*cos(ARROW_ANGLE);
p.y=m_EndPoint.y+ARROW_LENGTH*sin(ARROW_ANGLE);
m_ArrowHead1=new CLine(m_EndPoint,p,color,PenWidth);
Then to draw a line with relative angle add the angle
new_angle = ARROW_ANGLE + ANGLE_BETWEEN_LINES
But:
be careful to take into consideration periodicity of trigonometric functions. If you get a slope as you said:
float GetSlope(void)
{
return float(m_EndPoint.y-m_StartPoint.y)/float(m_EndPoint.x-m_StartPoint.x);
}
remember that this is tan(angle) and tangent function is not an injection so here you have lost information whether your angle is i.e. in first or third quadrant. To use described formula for drawing a line you have to get correct value of angle from this (first or third quarter argument) - you can use
std::atan2()
Compute arc tangent with two parameters. Returns the principal value of
the arc tangent of y/x, expressed in radians. Notice that despite the
sign ambiguity, this function determines with certainty in which
quadrant the angle falls. It takes a fractional argument. To compute the value, the function takes into account the sign of both arguments in order to determine the quadrant.
and then you can use this value to calculate new_angle. So it might looks like this:
float GetSlope(void)
{
return std::atan2(m_EndPoint.y-m_StartPoint.y, m_EndPoint.x-m_StartPoint.x);
}
wxPoint p;
p.x=m_EndPoint.x+ARROW_LENGTH*cos(GetSlope()+ANGLE_BETWEEN_LINES);
p.y=m_EndPoint.y+ARROW_LENGTH*sin(GetSlope()+ANGLE_BETWEEN_LINES);
m_ArrowHead1=new CLine(m_EndPoint,p,color,PenWidth);
now, because of atan2 you don't have to handle PI/2 and 3/2PI case individually.
I have solved the question using the following code:
CVector2D vector(m_StartPoint,m_EndPoint);
float vector_angle=vector.GetSlopeAngleInRadians();
float total_angle=vector_angle+ARROW_ANGLE; //ARROW_ANGLE=-PI/8, so is negative too
wxPoint p;
p.x=m_EndPoint.x-ARROW_LENGTH*cos(total_angle);
p.y=m_EndPoint.y-ARROW_LENGTH*sin(total_angle);
m_ArrowHead1=new CLine(m_EndPoint,p,color,PenWidth);
total_angle=vector_angle-ARROW_ANGLE;
p.x=m_EndPoint.x-ARROW_LENGTH*cos(total_angle);
p.y=m_EndPoint.y-ARROW_LENGTH*sin(total_angle);
m_ArrowHead2=new CLine(m_EndPoint,p,color,PenWidth);
It now draws a perfect arrow regardless of the way I draw it. Thanks all for your ideas.

Intersection problems with ray-sphere intersection

I'm writing a simple ray tracer and to keep it simple for now I've decided to just have spheres in my scene. I am at a stage now where I merely want to confirm that my rays are intersecting a sphere in the scene properly, nothing else. I've created a Ray and Sphere class and then a function in my main file which goes through each pixel to see if there's an intersection (relevant code will be posted below). The problem is that the whole intersection with the sphere is acting rather strangely. If I create a sphere with center (0, 0, -20) and a radius of 1 then I get only one intersection which is always at the very first pixel of what would be my image (upper-left corner). Once I reach a radius of 15 I suddenly get three intersections in the upper-left region. A radius of 18 gives me six intersections and once I reach a radius of 20+ I suddenly get an intersection for EACH pixel so something is acting as it's not supposed to do.
I was suspicious that my ray-sphere intersection code might be at fault here but having looked through it and looked through the net for more information most solutions describe the very same approach I use so I assume it shouldn't(!) be at fault here. So...I am not exactly sure what I am doing wrong, it could be my intersection code or it could be something else causing the problems. I just can't seem to find it. Could it be that I am thinking wrong when giving values for the sphere and rays? Below is relevant code
Sphere class:
Sphere::Sphere(glm::vec3 center, float radius)
: m_center(center), m_radius(radius), m_radiusSquared(radius*radius)
{
}
//Sphere-ray intersection. Equation: (P-C)^2 - R^2 = 0, P = o+t*d
//(P-C)^2 - R^2 => (o+t*d-C)^2-R^2 => o^2+(td)^2+C^2+2td(o-C)-2oC-R^2
//=> at^2+bt+c, a = d*d, b = 2d(o-C), c = (o-C)^2-R^2
//o = ray origin, d = ray direction, C = sphere center, R = sphere radius
bool Sphere::intersection(Ray& ray) const
{
//Squared distance between ray origin and sphere center
float squaredDist = glm::dot(ray.origin()-m_center, ray.origin()-m_center);
//If the distance is less than the squared radius of the sphere...
if(squaredDist <= m_radiusSquared)
{
//Point is in sphere, consider as no intersection existing
//std::cout << "Point inside sphere..." << std::endl;
return false;
}
//Will hold solution to quadratic equation
float t0, t1;
//Calculating the coefficients of the quadratic equation
float a = glm::dot(ray.direction(),ray.direction()); // a = d*d
float b = 2.0f*glm::dot(ray.direction(),ray.origin()-m_center); // b = 2d(o-C)
float c = glm::dot(ray.origin()-m_center, ray.origin()-m_center) - m_radiusSquared; // c = (o-C)^2-R^2
//Calculate discriminant
float disc = (b*b)-(4.0f*a*c);
if(disc < 0) //If discriminant is negative no intersection happens
{
//std::cout << "No intersection with sphere..." << std::endl;
return false;
}
else //If discriminant is positive one or two intersections (two solutions) exists
{
float sqrt_disc = glm::sqrt(disc);
t0 = (-b - sqrt_disc) / (2.0f * a);
t1 = (-b + sqrt_disc) / (2.0f * a);
}
//If the second intersection has a negative value then the intersections
//happen behind the ray origin which is not considered. Otherwise t0 is
//the intersection to be considered
if(t1<0)
{
//std::cout << "No intersection with sphere..." << std::endl;
return false;
}
else
{
//std::cout << "Intersection with sphere..." << std::endl;
return true;
}
}
Program:
#include "Sphere.h"
#include "Ray.h"
void renderScene(const Sphere& s);
const int imageWidth = 400;
const int imageHeight = 400;
int main()
{
//Create sphere with center in (0, 0, -20) and with radius 10
Sphere testSphere(glm::vec3(0.0f, 0.0f, -20.0f), 10.0f);
renderScene(testSphere);
return 0;
}
//Shoots rays through each pixel and check if there's an intersection with
//a given sphere. If an intersection exists then the counter is increased.
void renderScene(const Sphere& s)
{
//Ray r(origin, direction)
Ray r(glm::vec3(0.0f), glm::vec3(0.0f));
//Will hold the total amount of intersections
int counter = 0;
//Loops through each pixel...
for(int y=0; y<imageHeight; y++)
{
for(int x=0; x<imageWidth; x++)
{
//Change ray direction for each pixel being processed
r.setDirection(glm::vec3(((x-imageWidth/2)/(float)imageWidth), ((imageHeight/2-y)/(float)imageHeight), -1.0f));
//If current ray intersects sphere...
if(s.intersection(r))
{
//Increase counter
counter++;
}
}
}
std::cout << counter << std::endl;
}
Your second solution (t1) to the quadratic equation is wrong in the case disc > 0, where you need something like:
float sqrt_disc = glm::sqrt(disc);
t0 = (-b - sqrt_disc) / (2 * a);
t1 = (-b + sqrt_disc) / (2 * a);
I think it's best to write out the equation in this form rather than turning the division by 2 into a multiplication by 0.5, because the more the code resembles the mathematics, the easier it is to check.
A few other minor comments:
It seemed confusing to re-use the name disc for sqrt(disc), so I used a new variable name above.
You don't need to test for t0 > t1, since you know that both a and sqrt_disc are positive, and so t1 is always greater than t0.
If the ray origin is inside the sphere, it's possible for t0 to be negative and t1 to be positive. You don't seem to handle this case.
You don't need a special case for disc == 0, as the general case computes the same values as the special case. (And the fewer special cases you have, the easier it is to check your code.)
If I understand your code correctly, you might want to try:
r.setDirection(glm::vec3(((x-imageWidth/2)/(float)imageWidth),
((imageHeight/2-y)/(float)imageHeight),
-1.0f));
Right now, you've positioned the camera one unit away from the screen, but the rays can shoot as much as 400 units to the right and down. This is a very broad field of view. Also, your rays are only sweeping one octent of space. This is why you only get a handful of pixels in the upper-left corner of the screen. The code I wrote above should rectify that.

How can a find a face containing a predefined point when i have a planar graph embedded on a plane

I have a planar graph embedded on a plane (plane graph ) and want to search its faces.
The graph is not connected but consists of several connected graphs, which are not separately adressable (e.g. a subgraph can be contained in the face of another graph)
I want to find the polygons (faces) which include a certain 2d point.
The polygons are formed by the faces of the graphs. As the number of faces is quite big I would like to avoid to determine them beforehand.
What is the general complexity of such a search and what c++ library/ coding approach can I use to accomplish it.
Updated to clarify: I am refering to a graph in the xy plane here
You pose an interesting challenge. A relatively simple solution is possible if the polygon happens always to be convex, because in that case one need only ask whether the point of interest lies on the same flank (whether left or right) of all the polygon's sides. Though I know of no especially simple solution for the general case, the following code does seem to work for any, arbitrary polygon, as inspired indirectly by Cauchy's famous integral formula.
One need not be familiar with Cauchy to follow the code, for comments within the code explain the technique.
#include <vector>
#include <cstddef>
#include <cstdlib>
#include <cmath>
#include <iostream>
// This program takes its data from the standard input
// stream like this:
//
// 1.2 0.5
// -0.1 -0.2
// 2.7 -0.3
// 2.5 2.9
// 0.1 2.8
//
// Given such input, the program answers whether the
// point (1.2, 0.5) does not lie within the polygon
// whose vertices, in sequence, are (-0.1, -0.2),
// (2.7, -0.3), (2.5, 2.9) and (0.1, 2.8). Naturally,
// the program wants at least three vertices, so it
// requires the input of at least eight numbers (where
// the example has four vertices and thus ten numbers).
//
// This code lacks really robust error handling, which
// could however be added without too much trouble.
// Also, its function angle_swept() could be shortened
// at cost to readability; but this is not done here,
// since the function is already hard enough to grasp as
// it stands.
//
//
const double TWOPI = 8.0 * atan2(1.0, 1.0); // two times pi, or 360 deg
namespace {
struct Point {
double x;
double y;
Point(const double x0 = 0.0, const double y0 = 0.0)
: x(x0), y(y0) {}
};
// As it happens, for the present code's purpose,
// a Point and a Vector want exactly the same
// members and operations; thus, make the one a
// synonym for the other.
typedef Point Vector;
std::istream &operator>>(std::istream &ist, Point &point) {
double x1, y1;
if(ist >> x1 >> y1) point = Point(x1, y1);
return ist;
}
// Calculate the vector from one point to another.
Vector operator-(const Point &point2, const Point &point1) {
return Vector(point2.x - point1.x, point2.y - point1.y);
}
// Calculate the dot product of two Vectors.
// Overload the "*" operator for this purpose.
double operator*(const Vector &vector1, const Vector &vector2) {
return vector1.x*vector2.x + vector1.y*vector2.y;
}
// Calculate the (two-dimensional) cross product of two Vectors.
// Overload the "%" operator for this purpose.
double operator%(const Vector &vector1, const Vector &vector2) {
return vector1.x*vector2.y - vector1.y*vector2.x;
}
// Calculate a Vector's magnitude or length.
double abs(const Vector &vector) {
return std::sqrt(vector.x*vector.x + vector.y*vector.y);
}
// Normalize a vector to unit length.
Vector unit(const Vector &vector) {
const double abs1 = abs(vector);
return Vector(vector.x/abs1, vector.y/abs1);
}
// Imagine standing in the plane at the point of
// interest, facing toward a vertex. Then imagine
// turning to face the next vertex without leaving
// the point. Answer this question: through what
// angle did you just turn, measured in radians?
double angle_swept(
const Point &point, const Point &vertex1, const Point &vertex2
) {
const Vector unit1 = unit(vertex1 - point);
const Vector unit2 = unit(vertex2 - point);
const double dot_product = unit1 * unit2;
const double cross_product = unit1 % unit2;
// (Here we must be careful. Either the dot
// product or the cross product could in theory
// be used to extract the angle but, in
// practice, either the one or the other may be
// numerically problematical. Use whichever
// delivers the better accuracy.)
return (fabs(dot_product) <= fabs(cross_product)) ? (
(cross_product >= 0.0) ? (
// The angle lies between 45 and 135 degrees.
acos(dot_product)
) : (
// The angle lies between -45 and -135 degrees.
-acos(dot_product)
)
) : (
(dot_product >= 0.0) ? (
// The angle lies between -45 and 45 degrees.
asin(cross_product)
) : (
// The angle lies between 135 and 180 degrees
// or between -135 and -180 degrees.
((cross_product >= 0.0) ? TWOPI/2.0 : -TWOPI/2.0)
- asin(cross_product)
)
);
}
}
int main(const int, char **const argv) {
// Read the x and y coordinates of the point of
// interest, followed by the x and y coordinates of
// each vertex in sequence, from std. input.
// Observe that whether the sequence of vertices
// runs clockwise or counterclockwise does
// not matter.
Point point;
std::vector<Point> vertex;
std::cin >> point;
{
Point point1;
while (std::cin >> point1) vertex.push_back(point1);
}
if (vertex.size() < 3) {
std::cerr << argv[0]
<< ": a polygon wants at least three vertices\n";
std::exit(1);
}
// Standing as it were at the point of interest,
// turn to face each vertex in sequence. Keep
// track of the total angle through which you
// have turned.
double cumulative_angle_swept = 0.0;
for (size_t i = 0; i < vertex.size(); ++i) {
// In an N-sided polygon, vertex N is again
// vertex 0. Since j==N is out of range,
// if i==N-1, then let j=0. Otherwise,
// let j=i+1.
const size_t j = (i+1) % vertex.size();
cumulative_angle_swept +=
angle_swept(point, vertex[i], vertex[j]);
}
// Judge the point of interest to lie within the
// polygon if you have turned a complete circuit.
const bool does_the_point_lie_within_the_polygon =
fabs(cumulative_angle_swept) >= TWOPI/2.0;
// Output.
std::cout
<< "The angle swept by the polygon's vertices about the point\n"
<< "of interest is " << cumulative_angle_swept << " radians ("
<< ((360.0/TWOPI)*cumulative_angle_swept) << " degrees).\n"
<< "Therefore, the point lies "
<< (
does_the_point_lie_within_the_polygon
? "within" : "outside of"
)
<< " the polygon.\n";
return !does_the_point_lie_within_the_polygon;
}
Of course, the above code is just something I wrote, because your challenge was interesting and I wanted to see if I could meet it. If your application is important, then you should both test and review the code, and please revise back here any bugs you find. I have tested the code against two or three cases, and it seems to work, but for important duty it would want more exhaustive testing.
Good luck with your application.