Waypoint generator between 2 points on 2d map - c++

I'm now trying to generate straight sync points on my 2D map path.
In other words I want to spilt distance between point A on map for example X : 301 Y : 679 to point B X : 360 Y : 630, every 8 unit of distance is passed.
Every 8 distance unit calculated by sqrt(pow(a_x - b_x, 2), pow(a_y - b_y, 2)). I want to obtain next coordinates on map, like by a a_x + distance and b_y + distance.
I tried to do it but it didn't work, x axis doesnt change propertly.
Here is my code :
float base_x = active->getX();
float base_y = active->getY();
float destx = incoming_packet.get()->getFloat(4);
float desty = incoming_packet.get()->getFloat(8);
float distance = active->distance(destx, desty); // calculated by sqrt(pow(curent character x pos - destx, 2), pow(current character y pos - desty, 2))
float delta_X = active->vDistance(base_x, destx); // calculated by sqrt(pow(base_x - destx, 2))
float delta_Y = active->vDistance(base_y, desty); // calculated by sqrt(pow(base_y - desty, 2))
float cosa = delta_X / distance;
float sina = delta_Y / distance;
int mapx = 1;
int mapy = 1;
if(distance > 8)///active sync
{
for(float dist = 8; dist < distance;dist+=8)
{
base_x += mapx * (cosa * 8);
base_y += mapy * (sina * 8);
BOOST_LOG_TRIVIAL(debug) << "[ACTIVESYNC]NEXT SYNC ACK X : " << base_x << " Y : " << base_y;
}
}
What I'm doing here wrong ?

"cosa" (and cosb) are apparently dimensionless. (i.e. meters / meters)
"mapx" (and "mapy") are also dimensionless.
Note that in your for loop, base_x, base_y describe a point on the map.
And the 2 interesting computations in that loop
base_x += mapx * (cosa * 8);
base_y += mapy * (sina * 8);
become meaningless by attempting to add dimensionless numbers to a point. It might be ok to multiply by dimensionless numbers, but it is unreasonable to add dimensionless numbers to map points.
I suggest cosa and cosb be changed to represent the distance (i.e. meters) for each step.
float cosa = delta_X / 8; // size of steps in x direction
float sina = delta_Y / 8; // size of steps in y direction
Now the for loop can add 8 steps of cosa and sina appropriately to describe the path way points, and cosa and sina both have appropriate dimensions for the next computation.
The for loop can simplify to :
for(int step = 0; step < 8; step += 1)
{
base_x += (mapx * cosa);
base_y += (mapy * sina);
// remove or adapt the following
std::cout << std::setw(10) << std::left << (step+1) << std::setw(10)
<< base_x << std::setw(10) << base_y << std::endl;
// you were using:
//BOOST_LOG_TRIVIAL(debug) << "[ACTIVESYNC]NEXT SYNC ACK X : "
// << base_x << " Y : " << base_y;
}
My dummy code outputs:
base x/y 301 679
dest x/y 360 630
delta x/y 59 -49
step_x = 7.375
step_y = -6.125
step base_x base_y
0 301 679
1 308.375 672.875
2 315.75 666.75
3 323.125 660.625
4 330.5 654.5
5 337.875 648.375
6 345.25 642.25
7 352.625 636.125
8 360 630
Do these way points look more what you are looking for?

Related

How to determine the axis of rotation from a set of point clouds

I have a number of point clouds taken from an kinect-like instrument that is mounted on a tripod and then rotated. How do I determine the rotation axis accurately? I'm using c++ PCL and Eigen.
I can match the point clouds together with ICP and run a global registration (SLAM or ELCH) to get combined point cloud but for a number of reasons I would like to be able to determine the axis accurately and force the registrations to respect this rotation.
One issue that is related to this problem is the origin of my instrument. I can measure the distance to the rotation axis from the external dimensions of the device fairly accurately but I don't know exactly where is the origin in relation the extremities of the device. Solving this issue could help me to locate the origin too.
There are two methods that I'm considering.
One is to take the transformation matrices of the registered point clouds and extract the translation vectors that represent the locations where the transformation would project the internal origin in the current position. To the set of points acquired this way I could try to fit a circle and the center point would represent a vector from the origin to the rotation axis and the normal direction of the circle would be the direction of the axis.
The other option is to determine the rotation axis directly from any single rotation matrix, but the vector to the rotation axis seems volatile.
Any better solutions or insights on the issue?
You need to calculate the major axis oft the tensor of inertia. https://en.m.wikipedia.org/wiki/Moment_of_inertia.
All points can be considered to have the same mass. Then you can use the Steiner approach.
THIS IS NOT AN ANSWER TO THE ORIGINAL QUESTION BUT CLARIFICATION TO A DISCUSSION ABOUT DEFINING THE ROTATION AXIS BETWEEN TWO POSES
Deepfreeze. Here is an Octave script to demonstrate what we discussed in the chat. I know it might be bad practice to post an answer when not presenting one, but I hope this will give you an insight on what I was trying to explain about the relationship between the translation vector and the point on the rotation axis (t_i in your notation).
rotation_axis_unit = [0,0,1]' % parallel to z axis
angle = 1 /180 *pi() % one degree
% a rotaion matrix of one degree rotation
R = [cos(angle) -sin(angle) 0 ; sin(angle) cos(angle) 0 ; 0 0 1 ];
% the point around wich to rotate
axis_point = [-10,0,0]' % a point
% just a point used to demonstrate that all points form a circular path
test_point = [10,5,0]';
%containers for plotting
path_origin = zeros(360,3);
path_test_point = zeros(360,3);
path_v = zeros(360,3);
origin = [0,0,0]';
% updating rotation matrix
R_i = R;
M1 = [R,R*-axis_point.+axis_point;[0,0,0,1]];
% go around a full circle
for i=1:360
% v = the last column of M. Created from axis_point.
% -axis_point is the vector from axis_point to origin which is being rotated
% then a correction is applied to center it around the axis point
v = R_i * -axis_point .+ axis_point;
% building 4x4 transformation matrix
M = [R_i, v;[0,0,0,1]];
% M could also be built M_i = M1 * M_i, rotating the previous M by one degree around axis_point
% rotatin testing point and saving it
test_point_i = M * [test_point;1];
path_test_point(i,:) = test_point_i(1:3,1)';
% saving the translation part of M
path_v(i,:) = v';
% rotating origin point and saving it
origin_i = test_point_i = M * [origin;1];
path_origin(i,:) = origin_i(1:3,1)';
R_i = R * R_i ;
end
figure(1)
% plot test point path, just to show it forms a circular path, center and axis_point
scatter3(path_test_point(:,1), path_test_point(:,2), path_test_point(:,3), 5,'r')
hold on
% plotting origin path, circular, center at axis_point
scatter3(path_origin(:,1), path_origin(:,2), path_origin(:,3), 7,'r')
hold on
% plotting translation vectors, identical to origin path, (if invisible rotate so that you are watching it from z axis direction)
scatter3(path_v(:,1), path_v(:,2), path_v(:,3), 1, 'black');
hold on
% plots for visual analysis
scatter3(0,0,0,5,'b') % origin
hold on
scatter3(axis_point(1), axis_point(2), axis_point(3), 5, 'g') % axis point, center of the circles
hold on
scatter3(test_point(1), test_point(2), test_point(3), 5, 'black') % test point origin
hold off
% what does this demonstrate?
% it shows that that the ralationship between a 4x4
% transformation matrix and axis_angle notation plus point on the rotation axis
% M = [ R, v,; [0,0,0,1]] = [ R_i , R_i * -c + c; [ 0, 0, 0, 1] ]
%
% where c equals axis_point ( = perpendicular vector from origin to the axis of rotation )
% pay attention to path_v and path_origin
% they are identical
% path_v was extracted from the 4x4 transformation matrix M
% and path_origin was created by rotating the origin point by M
%--> v = R_i * -.c +.c
% Notice that since M describes a rotation of alpha angles around an
% axis that goes through c
% and its translation vector lies on a circle whose center
% is at the rotation_axis and radius is the distance from that
% point to origin ->
%
% M * M will describe a rotation of 2 x alpha angles around the same axis
% Therefore you can easily create more points that lay on that circle
% by multiplying M by itself and extracting the translation vector
%
% c can then be solved by normal circle fit algorithms.
%------------------------------------------------------------
% CAUTION !!!
% this applies perfectly when the transformation matrices have been created so
% that the translation is perfectly orthogonal to the rotation axis
% in real world matrices the translation will not be orthogonal
% therefore the points will not travel on a circular path but on a helix and this needs to be
% dealt with when solving the center of rotation.
An option is to place a chessboard at ~1 [m]. Use the kinect camera to make images for different rotations, where the hole chessboard is still visible. Fit the chessboard using OpenCV.
The goal is to find the xyz coordinates of the chessboard for the different orientations. Use your camera api functions to determine the xyz coordinates of the chessboard or do the following:
Determine camera intrinsics of camera 1 and 2. (use both color and IR images for kinect).
Determine camera extrinsics (camera 2 [R,t] wrt camera 1)
Use the values to calculate projection matrices
Use the projection matrices to triangulate the points of the chessboard to get coordinates in [X,Y,Z] wrt camera1 coordinate system.
Each group of chessboard points is called [x_i]. Now we can write the equation:
Update:
This equation can be solved with a non-linear solver, I used ceres-solver.
#include "ceres/ceres.h"
#include "ceres/rotation.h"
#include "glog/logging.h"
#include "opencv2/opencv.hpp"
#include "csv.h"
#include "Eigen/Eigen"
using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solver;
using ceres::Solve;
struct AxisRotationError {
AxisRotationError(double observed_x0, double observed_y0, double observed_z0, double observed_x1, double observed_y1, double observed_z1)
: observed_x0(observed_x0), observed_y0(observed_y0), observed_z0(observed_z0), observed_x1(observed_x1), observed_y1(observed_y1), observed_z1(observed_z1) {}
template <typename T>
bool operator()(const T* const axis, const T* const angle, const T* const trans, T* residuals) const {
//bool operator()(const T* const axis, const T* const trans, T* residuals) const {
// Normalize axis
T a[3];
T k = axis[0] * axis[0] + axis[1] * axis[1] + axis[2] * axis[2];
a[0] = axis[0] / sqrt(k);
a[1] = axis[1] / sqrt(k);
a[2] = axis[2] / sqrt(k);
// Define quaternion from axis and angle. Convert angle to radians
T pi = T(3.14159265359);
//T angle[1] = {T(10.0)};
T quaternion[4] = { cos((angle[0]*pi / 180.0) / 2.0),
a[0] * sin((angle[0] * pi / 180.0) / 2.0),
a[1] * sin((angle[0] * pi / 180.0) / 2.0),
a[2] * sin((angle[0] * pi / 180.0) / 2.0) };
// Define transformation
T t[3] = { trans[0], trans[1], trans[2] };
// Calculate predicted positions
T observedPoint0[3] = { T(observed_x0), T(observed_y0), T(observed_z0)};
T point[3]; point[0] = observedPoint0[0] - t[0]; point[1] = observedPoint0[1] - t[1]; point[2] = observedPoint0[2] - t[2];
T rotatedPoint[3];
ceres::QuaternionRotatePoint(quaternion, point, rotatedPoint);
T predicted_x = rotatedPoint[0] + t[0];
T predicted_y = rotatedPoint[1] + t[1];
T predicted_z = rotatedPoint[2] + t[2];
// The error is the difference between the predicted and observed position.
residuals[0] = predicted_x - T(observed_x1);
residuals[1] = predicted_y - T(observed_y1);
residuals[2] = predicted_z - T(observed_z1);
return true;
}
// Factory to hide the construction of the CostFunction object from
// the client code.
static ceres::CostFunction* Create(const double observed_x0, const double observed_y0, const double observed_z0,
const double observed_x1, const double observed_y1, const double observed_z1) {
// Define AutoDiffCostFunction. <AxisRotationError, #residuals, #dim axis, #dim angle, #dim trans
return (new ceres::AutoDiffCostFunction<AxisRotationError, 3, 3, 1,3>(
new AxisRotationError(observed_x0, observed_y0, observed_z0, observed_x1, observed_y1, observed_z1)));
}
double observed_x0;
double observed_y0;
double observed_z0;
double observed_x1;
double observed_y1;
double observed_z1;
};
int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]);
// Load points.csv into cv::Mat's
// 216 rows with (x0, y0, z0, x1, y1, z1)
// [x1,y1,z1] = R* [x0-tx,y0-ty,z0-tz] + [tx,ty,tz]
// The xyz coordinates are points on a chessboard, where the chessboard
// is rotated for 4x. Each chessboard has 54 xyz points. So 4x 54,
// gives the 216 rows of observations.
// The chessboard is located at [0,0,1], as the camera_0 is located
// at [-0.1,0,0], the t should become [0.1,0,1.0].
// The chessboard is rotated around axis [0.0,1.0,0.0]
io::CSVReader<6> in("points.csv");
float x0, y0, z0, x1, y1, z1;
// The observations
cv::Mat x_0(216, 3, CV_32F);
cv::Mat x_1(216, 3, CV_32F);
for (int rowNr = 0; rowNr < 216; rowNr++){
if (in.read_row(x0, y0, z0, x1, y1, z1))
{
x_0.at<float>(rowNr, 0) = x0;
x_0.at<float>(rowNr, 1) = y0;
x_0.at<float>(rowNr, 2) = z0;
x_1.at<float>(rowNr, 0) = x1;
x_1.at<float>(rowNr, 1) = y1;
x_1.at<float>(rowNr, 2) = z1;
}
}
std::cout << x_0(cv::Rect(0, 0, 2, 5)) << std::endl;
// The variable to solve for with its initial value. It will be
// mutated in place by the solver.
int numObservations = 216;
double axis[3] = { 0.0, 1.0, 0.0 };
double* pAxis; pAxis = axis;
double angles[4] = { 10.0, 10.0, 10.0, 10.0 };
double* pAngles; pAngles = angles;
double t[3] = { 0.0, 0.0, 1.0,};
double* pT; pT = t;
bool FLAGS_robustify = true;
// Build the problem.
Problem problem;
// Set up the only cost function (also known as residual). This uses
// auto-differentiation to obtain the derivative (jacobian).
for (int i = 0; i < numObservations; ++i) {
ceres::CostFunction* cost_function =
AxisRotationError::Create(
x_0.at<float>(i, 0), x_0.at<float>(i, 1), x_0.at<float>(i, 2),
x_1.at<float>(i, 0), x_1.at<float>(i, 1), x_1.at<float>(i, 2));
//std::cout << "pAngles: " << pAngles[i / 54] << ", " << i / 54 << std::endl;
ceres::LossFunction* loss_function = FLAGS_robustify ? new ceres::HuberLoss(0.001) : NULL;
//ceres::LossFunction* loss_function = FLAGS_robustify ? new ceres::CauchyLoss(0.002) : NULL;
problem.AddResidualBlock(cost_function, loss_function, pAxis, &pAngles[i/54], pT);
//problem.AddResidualBlock(cost_function, loss_function, pAxis, pT);
}
// Run the solver!
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
//options.linear_solver_type = ceres::DENSE_QR;
options.minimizer_progress_to_stdout = true;
options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;
options.num_threads = 4;
options.use_nonmonotonic_steps = false;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
//std::cout << summary.FullReport() << "\n";
std::cout << summary.BriefReport() << "\n";
// Normalize axis
double k = axis[0] * axis[0] + axis[1] * axis[1] + axis[2] * axis[2];
axis[0] = axis[0] / sqrt(k);
axis[1] = axis[1] / sqrt(k);
axis[2] = axis[2] / sqrt(k);
// Plot results
std::cout << "axis: [ " << axis[0] << "," << axis[1] << "," << axis[2] << " ]" << std::endl;
std::cout << "t: [ " << t[0] << "," << t[1] << "," << t[2] << " ]" << std::endl;
std::cout << "angles: [ " << angles[0] << "," << angles[1] << "," << angles[2] << "," << angles[3] << " ]" << std::endl;
return 0;
}
The result I've got:
iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time
0 3.632073e-003 0.00e+000 3.76e-002 0.00e+000 0.00e+000 1.00e+004 0 4.30e-004 7.57e-004
1 3.787837e-005 3.59e-003 2.10e-003 1.17e-001 1.92e+000 3.00e+004 1 7.43e-004 8.55e-003
2 3.756202e-005 3.16e-007 1.73e-003 5.49e-001 1.61e-001 2.29e+004 1 5.35e-004 1.13e-002
3 3.589147e-005 1.67e-006 2.90e-004 9.19e-002 9.77e-001 6.87e+004 1 5.96e-004 1.46e-002
4 3.584281e-005 4.87e-008 1.38e-005 2.70e-002 1.00e+000 2.06e+005 1 4.99e-004 1.73e-002
5 3.584268e-005 1.35e-010 1.02e-007 1.63e-003 1.01e+000 6.18e+005 1 6.32e-004 2.01e-002
Ceres Solver Report: Iterations: 6, Initial cost: 3.632073e-003, Final cost: 3.584268e-005, Termination: CONVERGENCE
axis: [ 0.00119037,0.999908,-0.0134817 ]
t: [ 0.0993185,-0.0080394,1.00236 ]
angles: [ 9.90614,9.94415,9.93216,10.1119 ]
The angles result is quite nice 10 degrees. These can even be fixed for my case, as I know the rotation very accurately from my rotation stage. There is a small difference in the t and axis. This is cause by inaccuracies in my virtual stereoCamera simulation. My chessboard squares are not exactly square and the dimensions are also a little off....
My simulation scripts, images, results: blender_simulation.zip

Find a new angle of the ball when it bounces (using coordinates)

I have a game where a ball is bouncing off walls. It's on a coordinate plane. I want there to be some small amount of randomness when it bounces to keep the game more interesting. How would I do this while keeping the ball at a constant speed the whole time? Right now my code means it only bounces at right angles.
The top left corner of the window is 0,0 and the bottom right is winW,winH (set at 800,800 right now).
ball.cpp snippet
pos.x = start.x;
pos.y = start.y;
speed.x = .4f; // the f indicates that it's per frame.
speed.y = .4f;
void Ball::hitLeftRight() {
speed.x = -speed.x;
}
void Ball::hitTopBottom() {
speed.y = -speed.y;
}
void Ball::reset() {
// for a new level in game
pos.x = start.x;
pos.y = start.y;
}
void Ball::update() {
// called every frame
pos.y += speed.y;
pos.x += speed.x;
ballShape.setPosition(pos);
}
You could add a random angle to the ball in addition to just flipping the velocities. By using the <random> header and generating values between 0 and 2 * pi you can add a random velocity in any direction. Or as in the example below, limiting it to -22.5 to 22.5 degrees. Or pi / 8.0 radians.
Random Angle
You could of course tweak those values based on the angle of impact, but that is implementation specific. Below is an example on how you could generate such numbers:
#include <random>
#include <cmath>
#include <iostream>
int main() {
constexpr double pi = 3.14159;
constexpr double bounceSpeed = 5.0;
std::random_device seed;
std::mt19937 generator(seed());
// Numbers between - pi / 2 and pi / 8 gives angles between -22.5 and 22.5
std::uniform_real_distribution<double> random(-pi / 8.0, pi / 8.0);
double deltaX = cos(random(generator)) * bounceSpeed;
double deltaY = sin(random(generator)) * bounceSpeed;
std::cout << deltaX << "\n" << deltaY << "\n";
return 0;
}
Afterwards, you could add deltaX and deltaY to your respective x and y velocities.
Plain Random
Or if you're satisfied with just any purely random velocity:
// Generate random double in range [min, max]
double uniform(double min, double max) {
static std::random_device seed;
static std::mt19937 generator(seed());
std::uniform_real_distribution<double> random(min, max);
return random(generator);
}
Call that function twice with the velocity range you desire, and add that to the x and y of your ball.
Keeping Speed
To keep the speed of the ball you could normalize the velocity vector and then multiply it by the desired speed of you ball after adding the random velocity.
To normalize, divide by length:
#include <cmath>
#include <iostream>
int main() {
constexpr double speed = 4.0;
double x = 3.0;
double y = 4.0;
double length = sqrt(x * x + y * y);
x /= length;
y /= length;
x *= speed;
y *= speed;
std::cout << x << "\n" << y << "\n";
return 0;
}
Then multiply by speed to keep it consistent.
To start, have a set speed for the ball as a float. Then have a velocity which you move by every update with x and y members like what you have now. Calculate how far you move x and y using your speed, your angle of movement, and some trigonometry. (Remember to convert degrees to radians)
float Speed = 10;
float Angle = 45;
Velocity.x = cos(Angle * 3.14159 / 180) * Speed;
Velocity.y = sin(Angle * 3.14159 / 180) * Speed;
Whenever you encounter a collision, you can then recalculate your angle add your new random angle and recalculate your velocity. (Again converting degrees to radians).
//if (collision)
Angle = atan2(Velocity.y * 3.14159 / 180, Velocity.x * 3.14159 / 180);
Angle += rand() % 90 + 1; // Could also be subtracting here
Velocity.x = cos(Angle * 3.14159 / 180) * Speed;
Velocity.y = sin(Angle * 3.14159 / 180) * Speed;
Add or subtract from the angle based on what side you have struck and from what angle.
void HitTop()
{
if (Velocity.x > 0)
//Subtract random angle
else
//Add random angle
}
Do this for all sides.
Let your border has arbitrary form (not only rectangle). If border normal (unit) vector in bouncing points is
n = (n.x, n.y)
then speed vector after bouncing changes:
dot = speed.x * n.x + speed.y * n.y
//after reflection
newspeed.x = speed.x - 2 * dot * n.x
newspeed.y = speed.y - 2 * dot * n.y
To add some randomness, just rotate normal by small random angle:
df = GaussianRandom(Mean = 0, Sigma = Pi / 30) //arbitrary parameter
//seems in c++ it is std::normal_distribution
n'.x = n.x * Cos(df) - n.y * Sin(df)
n'.y = n.x * Sin(df) + n.y * Cos(df)
and use n' to calculate reflection. Note that this approach preserves speed magnitude.

How to stretch points?

Let say I've 5 points, where p0 and p4 are fixed with values 0.0 and 4.0:
0 | 1.0 | 2.0 | 3.0 | 4
The points in the middle can change, but they must stretch the others once moving.
So for a stretch "to right", it must enlarge the prev values around the moving point and press the next ones between the moving point and the last point, keeping the proportions between each points.
I've write this code which move the 3° point to 2.5 from its original 2.0 x-position:
const int numPoints = 5;
double points[numPoints] = { 0.0, 1.0, 2.0, 3.0, 4.0 };
int stretchedPoint = 2;
double prevX = points[stretchedPoint];
points[stretchedPoint] = 2.5;
std::cout<< points[0];
for (int prevPoint = 1; prevPoint < numPoints - 1; prevPoint++) {
// prev points
if (prevPoint < stretchedPoint) {
double ratio = points[stretchedPoint] / prevX;
points[prevPoint] *= ratio;
// next points
} else if (prevPoint > stretchedPoint) {
double ratio = (points[numPoints - 1] - prevX) / (points[numPoints - 1] - points[stretchedPoint]);
points[prevPoint] *= ratio;
}
std::cout << " | " << points[prevPoint];
}
std::cout << " | " << points[numPoints - 1];
which give to me right result for prev points:
0 | 1.25 | 2.5 | 0.76 | 4
but when I try to apply the "same-wrapped-math" for the next points, I get a non-proportional scaling, which give weird results (4?)
Can anyone help me?
You forgot about non-zero starting point
points[prevPoint] = points[stretchedPoint] + ratio * (points[prevPoint] - prevX)
Note that same logic should be applied to previos points, if start value is non-zero
In general, to apply linear interpolation for initial X0..X1 interval and final X0new..X1new interval, one have to use
(Xnew - X0new) / (X1new - X0new) = (X - X0) / (X1 - X0)
so
XNew = X0new + (X1new - X0new) * (X - X0) / (X1 - X0)
What you did on the left side of the point (and which is working) can be rewritten somehow like this:
// double ratio = (points[stretchedPoint] - 0) / (prevX - 0);
// points[prevPoint] = 0 + ratio * (points[prevPoint] - 0);
To achieve exactly the dual on the right side, it should be:
} else if (prevPoint > stretchedPoint) {
double ratio = (points[numPoints - 1] - points[stretchedPoint]) /
(points[numPoints - 1] - prevX);
points[prevPoint] = points[numPoints - 1] -
ratio * (points[numPoints-1] - points[prevPoint]);
}

Finding Cordinates of Cap of an arrow

I have to draw an arrow. I have a head point and a tail point now i need to draw a triangular arrow cap. A triangle whose length is of size 5.How can i find coordinates of the end points of triangle. One thing is we have angle of 45.so if we can rotate the vector by 45 to obtain it.
` int x1=arrowStart.X;
int y1=arrowStart.Y;
int x2=arrowend.X;
int y2=arrowend.Y;
PointF arrowPoint=arrowend;
double arrowlength=sqrt(pow(x1-x2,2)+pow(y1-y2,2));
int ArrowMultiplier=1;
double arrowangle=atan2(y1-y2,x1-x2);
double pointx,pointy;
if(x1>x2)
{
pointx=x1 - (cos(arrowangle) * (arrowlength-3 * ArrowMultiplier ));
}
else
{
pointx = cos(arrowangle) * (arrowlength-3 * ArrowMultiplier ) + x1;
}
if (y1 > y2)
{
pointy = y1 - (sin(arrowangle) * (arrowlength -3 * ArrowMultiplier));
}
else
{
pointy = (sin(arrowangle) * (arrowlength-3 * ArrowMultiplier )) + y1;
}
PointF arrowPointBack(pointx,pointy);
double angleB = atan2((3 * ArrowMultiplier), (arrowlength - (3 * ArrowMultiplier)));
double angleC = (3.14) * (90 - (arrowangle * (180 /3.14)) - (angleB * (180 / 3.14))) / 180;
double secondaryLength = (3 * ArrowMultiplier)/sin(angleB);
if (x1 > x2)
{
pointx = x1 - (sin(angleC) * secondaryLength);
}
else
{
pointx = (sin(angleC) * secondaryLength) + x1;
}
if (y1 > y2)
{
pointy = y1 - (cos(angleC) * secondaryLength);
}
else
{
pointy = (cos(angleC) * secondaryLength) + y1;
}
PointF arrowPointLeft((float)pointx, (float)pointy);
angleC = arrowangle - angleB;
if (x1 > x2)
{
pointx = x1 - (cos(angleC) * secondaryLength);
}
else
{
pointx = (cos(angleC) * secondaryLength) +x1;
}
if (y1 > y2)
{
pointy =y1 - (sin(angleC) * secondaryLength);
}
else
{
pointy = (sin(angleC) * secondaryLength) + y1;
}
PointF arrowPointRight((float)pointx,(float)pointy);
PointF arrowPoints[4];
arrowPoints[0] = arrowPoint;
arrowPoints[1] = arrowPointLeft;
//arrowPoints[2] = arrowPointBack;
arrowPoints[2] = arrowPointRight;
arrowPoints[3] = arrowPoint;
`
Right, I suppose I should break it down for you:
First, you need to calculate the angle that the arrow sits at. This can be achieved with the inverse tangent function:
atan(diff_y, diff_x)
where diff_y and diff_x are the difference between the x and y values of your two end-points.
You can then add the desired angle of the arrow-head to this angle and use sin and cos to calculate the x and y values of the first of the extra points of the arrow-head.
new_x = head_x - 5 * cos (angle + pi/4)
new_y = head_y + 5 * sin (angle + pi/4)
for the other point, you do the same, but with a subtraction of the difference in angle.
new_x = head_x - 5 * cos (angle - pi/4)
new_y = head_y + 5 * sin (angle - pi/4)
You then have all the points you need.
I did this for fun (sue me, I was bored) and came up with this:
#include <math.h>
#include <iostream>
const double arrow_head_length = 3;
const double PI = 3.14159265;
const double arrow_head_angle = PI/6;
//returns the angle between two points, with coordinate1 describing the centre of the circle, with the angle progressing clockwise
double angle_between_points( std::pair<double,double> coordinate1, std::pair<double,double> coordinate2)
{
return atan2(coordinate2.second - coordinate1.second, coordinate1.first - coordinate2.first);
}
//calculate the position of a new point [displacement] away from an original point at an angle of [angle]
std::pair<double,double> displacement_angle_offset(std::pair<double,double> coordinate_base, double displacement, double angle)
{
return std::make_pair
(
coordinate_base.first - displacement * cos(angle),
coordinate_base.second + displacement * sin(angle)
);
}
int main()
{
std::pair<double,double> arrow_tail( 0, 0);
std::pair<double,double> arrow_head( 15,-15);
//find the angle of the arrow
double angle = angle_between_points(arrow_head, arrow_tail);
//calculate the new positions
std::pair<double,double> head_point_1 = displacement_angle_offset(arrow_head, arrow_head_length, angle + arrow_head_angle);
std::pair<double,double> head_point_2 = displacement_angle_offset(arrow_head, arrow_head_length, angle - arrow_head_angle);
//output the points in order: tail->head->point1->point2->head so if you follow them it draws the arrow
std::cout << arrow_tail.first << ',' << arrow_tail.second << '\n'
<< arrow_head.first << ',' << arrow_head.second << '\n'
<< head_point_1.first << ',' << head_point_1.second << '\n'
<< head_point_2.first << ',' << head_point_2.second << '\n'
<< arrow_head.first << ',' << arrow_head.second << std::endl;
}
The output can be saved as a .csv and loaded into excel for example, where you can use it to draw a connected scatter-graph that will form the shape of the arrow.
If this is homework, then before you do anything with it, make sure you know exactly how it works. That includes knowing the answers to questions like:
when calculating the angle, why does the code do point2_y-point1_y but point1_x-point2_x?
what direction is angle 0?
why does the angle increase going clockwise and not anti-clockwise?
why are there 5 outputs when only 4 points are needed?
what is the significance of PI/6 in the code? It isn't == 45 degrees. Why would this angle be better?
Also note that this question and answer will now pop up in a google search.
Working example: http://ideone.com/D4IwOy
You can paste the output into any graphing tool (such as this one) or save as a .csv and open in excel/spreadsheet of choice and plot a scatter graph to see the arrow coordinates. Note that it (annoyingly) doesn't keep the x and y scales equal so will stretch arrows like this one:
3,7
24,15
21.0381,15.4768
22.1061,12.6734
24,15

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);
}