How can I sort an array of points/vectors by counter-clockwise increasing angle from a given axis vector?
For example:
If 0 is the axis vector I would expect the sorted array to be in the order 2, 3, 1.
I'm reasonably sure it's possible to do this with cross products, a custom comparator, and std::sort().
Yes, you can do it with a custom comparator based on the cross-product. The only problem is that a naive comparator won't have the transitivity property. So an extra step is needed, to prevent angles either side of the reference from being considered close.
This will be MUCH faster than anything involving trig. There's not even any need to normalize first.
Here's the comparator:
class angle_sort
{
point m_origin;
point m_dreference;
// z-coordinate of cross-product, aka determinant
static double xp(point a, point b) { return a.x * b.y - a.y * b.x; }
public:
angle_sort(const point origin, const point reference) : m_origin(origin), m_dreference(reference - origin) {}
bool operator()(const point a, const point b) const
{
const point da = a - m_origin, db = b - m_origin;
const double detb = xp(m_dreference, db);
// nothing is less than zero degrees
if (detb == 0 && db.x * m_dreference.x + db.y * m_dreference.y >= 0) return false;
const double deta = xp(m_dreference, da);
// zero degrees is less than anything else
if (deta == 0 && da.x * m_dreference.x + da.y * m_dreference.y >= 0) return true;
if (deta * detb >= 0) {
// both on same side of reference, compare to each other
return xp(da, db) > 0;
}
// vectors "less than" zero degrees are actually large, near 2 pi
return deta > 0;
}
};
Demo: http://ideone.com/YjmaN
Most straightforward, but possibly not the optimal way is to shift the cartesian coordinates to be relative to center point and then convert them to polar coordinates. Then just subtract the angle of the "starting vector" modulo 360, and finally sort by angle.
Or, you could make a custom comparator for just handling all the possible slopes and configurations, but I think the polar coordinates are little more transparent.
#include <iostream>
#include <cmath>
#include <algorithm>
using namespace std;
struct Point {
static double base_angle;
static void set_base_angle(double angle){
base_angle = angle;
}
double x;
double y;
Point(double x, double y):x(x),y(y){}
double Angle(Point o = Point(0.0, 0.0)){
double dx = x - o.x;
double dy = y - o.y;
double r = sqrt(dx * dx + dy * dy);
double angle = atan2(dy , dx);
angle -= base_angle;
if(angle < 0) angle += M_PI * 2;
return angle;
}
};
double Point::base_angle = 0;
ostream& operator<<(ostream& os, Point& p){
return os << "Point(" << p.x << "," << p.y << ")";
}
bool comp(Point a, Point b){
return a.Angle() < b.Angle();
}
int main(){
Point p[] = { Point(-4., -4.), Point(-6., 3.), Point(2., -4.), Point(1., 5.) };
Point::set_base_angle(p[0].Angle());
sort(p, p + 4, comp);
Point::set_base_angle(0.0);
for(int i = 0;i< 4;++i){
cout << p[i] << " angle:" << p[i].Angle() << endl;
}
}
DEMO
Point(-4,-4) angle:3.92699
Point(2,-4) angle:5.17604
Point(1,5) angle:1.3734
Point(-6,3) angle:2.67795
Assuming they are all the same length and have the same origin, you can sort on
struct sorter {
operator()(point a, point b) const {
if (a.y > 0) { //a between 0 and 180
if (b.y < 0) //b between 180 and 360
return false;
return a.x < b.x;
} else { // a between 180 and 360
if (b.y > 0) //b between 0 and 180
return true;
return a.x > b.x;
}
}
//for comparison you don't need exact angles, simply relative.
}
This will quickly sort them from 0->360 degress. Then you find your vector 0 (at position N), and std::rotate the results left N elements. (Thanks TomSirgedas!)
This is an example of how I went about solving this. It converts to polar to get the angle and then is used to compare them. You should be able to use this in a sort function like so:
std::sort(vectors.begin(), vectors.end(), VectorComp(centerPoint));
Below is the code for comparing
struct VectorComp : std::binary_function<sf::Vector2f, sf::Vector2f, bool>
{
sf::Vector2f M;
IntersectComp(sf::Vector2f v) : M(v) {}
bool operator() ( sf::Vector2f o1, sf::Vector2f o2)
{
float ang1 = atan( ((o1.y - M.y)/(o1.x - M.x) ) * M_PI / 180);
float ang2 = atan( (o2.y - M.y)/(o2.x - M.x) * M_PI / 180);
if(ang1 < ang2) return true;
else if (ang1 > ang2) return false;
return true;
}
};
It uses sfml library but you can switch any vector/point class instead of sf::Vector2f. M would be the center point. It works great if your looking to draw a triangle fan of some sort.
You should first normalize each vector, so each point is in (cos(t_n), sin(t_n)) format.
Then calculating the cos and sin of the angles between each points and you reference point. Of course:
cos(t_n-t_0)=cos(t_n)cos(t_0)+sin(t_n)sin(t_0) (this is equivalent to dot product)
sin(t_n-t_0)=sin(t_n)cos(t_0)-cos(t_n)sin(t_0)
Only based on both values, you can determine the exact angles (-pi to pi) between points and reference point. If just using dot product, clockwise and counter-clockwise of same angle have same values. One you determine the angle, sort them.
I know this question is quite old, and the accepted answer helped me get to this, still I think I have a more elegant solution which also covers equality (so returns -1 for lowerThan, 0 for equals, and 1 for greaterThan).
It is based on the division of the plane to 2 halves, one from the positive ref axis (inclusive) to the negative ref axis (exclusive), and the other is its complement.
Inside each half, comparison can be done by right hand rule (cross product sign), or in other words - sign of sine of angle between the 2 vectors.
If the 2 points come from different halves, then the comparison is trivial and is done between the halves themselves.
For an adequately uniform distribution, this test should perform on average 4 comparisons, 1 subtraction, and 1 multiplication, besides the 4 subtractions done with ref, that in my opinion should be precalculated.
int compareAngles(Point const & A, Point const & B, Point const & ref = Point(0,0)) {
typedef decltype(Point::x) T; // for generality. this would not appear in real code.
const T sinA = A.y - ref.y; // |A-ref|.sin(angle between A and positive ref-axis)
const T sinB = B.y - ref.y; // |B-ref|.sin(angle between B and positive ref-axis)
const T cosA = A.x - ref.x; // |A-ref|.cos(angle between A and positive ref-axis)
const T cosB = B.x - ref.x; // |B-ref|.cos(angle between B and positive ref-axis)
bool hA = ( (sinA < 0) || ((sinA == 0) && (cosA < 0)) ); // 0 for [0,180). 1 for [180,360).
bool hB = ( (sinB < 0) || ((sinB == 0) && (cosB < 0)) ); // 0 for [0,180). 1 for [180,360).
if (hA == hB) {
// |A-ref|.|B-ref|.sin(angle going from (B-ref) to (A-ref))
T sinBA = sinA * cosB - sinB * cosA;
// if T is int, or return value is changed to T, it can be just "return sinBA;"
return ((sinBA > 0) ? 1 : ((sinBA < 0) ? (-1) : 0));
}
return (hA - hB);
}
If S is an array of PointF, and mid is the PointF in the centre:
S = S.OrderBy(s => -Math.Atan2((s.Y - mid.Y), (s.X - mid.X))).ToArray();
will sort the list in order of rotation around mid, starting at the point closest to (-inf,0) and go ccw (clockwise if you leave out the negative sign before Math).
Related
I've been attempting, for months now, to write a function to return the minimum translation needed to be applied to a line segment in order to separate it from an polygon in which intersects. I'm using the separating axis theorem and it seems I'm able to calculate the magnitude correctly however, the direction returned is sometimes wrong. Yet, when the returned translation is incorrect, the inverse is always correct.
In the pictures below, the yellow line is the one used in calculations, the purple line is the yellow line + translation and the red line is the yellow line minus the translation. As you can see either the purple or the red line is correct in different positions but I'm not sure under what conditions to return which line.
So my question is: On what condition does the translation actually need to be flipped so that my function always returns a translation with the correct direction?
const Projection Polygon::Project(const Axis &a) const
{
float min = a.Dot(GetPoint(0));
float max = min;
for (unsigned i = 1; i < GetPointCount(); i++)
{
float prj = a.Dot(GetPoint(i));
if (prj < min)
min = prj;
else if (prj > max)
max = prj;
}
return Projection(min, max);
}
const Projection Segment::Project(const Axis &a) const
{
const float dot0 = a.Dot(GetPoint(0));
const float dot1 = a.Dot(GetPoint(1));
return Projection(std::min(dot0, dot1), std::max(dot0, dot1));
}
const float Projection::GetOverlap(const Projection &p) const
{
// x = min & y = max
return std::min(y - p.x, p.y - x);
}
const Vector2 Segment::GetTranslation(const Polygon &p) const
{
float Overlap = std::numeric_limits<float>::infinity();
Axis smallest;
Vector2 translation;
AxesVec axes(p.GetAxes());
axes.push_back(GetAxis());
for (auto && axis : axes)
{
const Projection pA = p.Project(axis);
const Projection pB = Project(axis);
if (pA.IsOverlap(pB))
{
const float o = pA.GetOverlap(pB);
if (o < Overlap)
{
Overlap = o;
smallest = axis;
}
}
}
translation = smallest * (Overlap + 1);
return translation;
}
The trouble is that your GetOverlap function returns the magnitude of the overlap, but not the sense (left or right).
You could change it to this:
if(y - p.x < p.y - x)
return y - p.x;
else
return x - p.y;
Then in GetTranslation:
const float o = pA.GetOverlap(pB);
if (abs(o) < abs(Overlap))
{
...
}
if (Overlap > 0)
++Overlap;
else
--Overlap;
translation = smallest * Overlap;
I have a set of points that I'm trying to sort in ccw order or cw order from their angle. I want the points to be sorted in a way that they could form a polygon with no splits in its region or intersections. This is difficult because in most cases, it would be a concave polygon.
point centroid;
int main( int argc, char** argv )
{
// I read a set of points into a struct point array: points[n]
// Find centroid
double sx = 0; double sy = 0;
for (int i = 0; i < n; i++)
{
sx += points[i].x;
sy += points[i].y;
}
centroid.x = sx/n;
centroid.y = sy/n;
// sort points using in polar order using centroid as reference
std::qsort(&points, n, sizeof(point), polarOrder);
}
// -1 ccw, 1 cw, 0 collinear
int orientation(point a, point b, point c)
{
double area2 = (b.x-a.x)*(c.y-a.y) - (b.y-a.y)*(c.x-a.x);
if (area2 < 0) return -1;
else if (area2 > 0) return +1;
else return 0;
}
// compare other points relative to polar angle they make with this point
// (where the polar angle is between 0 and 2pi)
int polarOrder(const void *vp1, const void *vp2)
{
point *p1 = (point *)vp1;
point *p2 = (point *)vp2;
// translation
double dx1 = p1->x - centroid.x;
double dy1 = p1->y - centroid.y;
double dx2 = p2->x - centroid.x;
double dy2 = p2->y - centroid.y;
if (dy1 >= 0 && dy2 < 0) { return -1; } // p1 above and p2 below
else if (dy2 >= 0 && dy1 < 0) { return 1; } // p1 below and p2 above
else if (dy1 == 0 && dy2 ==0) { // 3-collinear and horizontal
if (dx1 >= 0 && dx2 < 0) { return -1; }
else if (dx2 >= 0 && dx1 < 0) { return 1; }
else { return 0; }
}
else return -orientation(centroid,*p1,*p2); // both above or below
}
It looks like the points are sorted accurately(pink) until they "cave" in, in which case the algorithm skips over these points then continues.. Can anyone point me into the right direction to sort the points so that they form the polygon I'm looking for?
Raw Point Plot - Blue, Pink Points - Sorted
Point List: http://pastebin.com/N0Wdn2sm (You can ignore the 3rd component, since all these points lie on the same plane.)
The code below (sorry it's C rather than C++) sorts correctly as you wish with atan2.
The problem with your code may be that it attempts to use the included angle between the two vectors being compared. This is doomed to fail. The array is not circular. It has a first and a final element. With respect to the centroid, sorting an array requires a total polar order: a range of angles such that each point corresponds to a unique angle regardless of the other point. The angles are the total polar order, and comparing them as scalars provides the sort comparison function.
In this manner, the algorithm you proposed is guaranteed to produce a star-shaped polyline. It may oscillate wildly between different radii (...which your data do! Is this what you meant by "caved in"? If so, it's a feature of your algorithm and data, not an implementation error), and points corresponding to exactly the same angle might produce edges that coincide (lie directly on top of each other), but the edges won't cross.
I believe that your choice of centroid as the polar origin is sufficient to guarantee that connecting the ends of the polyline generated as above will produce a full star-shaped polygon, however, I don't have a proof.
Result plotted with Excel
Note you can guess from the nearly radial edges where the centroid is! This is the "star shape" I referred to above.
To illustrate this is really a star-shaped polygon, here is a zoom in to the confusing lower left corner:
If you want a polygon that is "nicer" in some sense, you will need a fancier (probably much fancier) algorithm, e.g. the Delaunay triangulation-based ones others have referred to.
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
struct point {
double x, y;
};
void print(FILE *f, struct point *p) {
fprintf(f, "%f,%f\n", p->x, p->y);
}
// Return polar angle of p with respect to origin o
double to_angle(const struct point *p, const struct point *o) {
return atan2(p->y - o->y, p->x - o->x);
}
void find_centroid(struct point *c, struct point *pts, int n_pts) {
double x = 0, y = 0;
for (int i = 0; i < n_pts; i++) {
x += pts[i].x;
y += pts[i].y;
}
c->x = x / n_pts;
c->y = y / n_pts;
}
static struct point centroid[1];
int by_polar_angle(const void *va, const void *vb) {
double theta_a = to_angle(va, centroid);
double theta_b = to_angle(vb, centroid);
return theta_a < theta_b ? -1 : theta_a > theta_b ? 1 : 0;
}
void sort_by_polar_angle(struct point *pts, int n_pts) {
find_centroid(centroid, pts, n_pts);
qsort(pts, n_pts, sizeof pts[0], by_polar_angle);
}
int main(void) {
FILE *f = fopen("data.txt", "r");
if (!f) return 1;
struct point pts[10000];
int n_pts, n_read;
for (n_pts = 0;
(n_read = fscanf(f, "%lf%lf%*f", &pts[n_pts].x, &pts[n_pts].y)) != EOF;
++n_pts)
if (n_read != 2) return 2;
fclose(f);
sort_by_polar_angle(pts, n_pts);
for (int i = 0; i < n_pts; i++)
print(stdout, pts + i);
return 0;
}
Well, first and foremost, I see centroid declared as a local variable in main. Yet inside polarOrder you are also accessing some centroid variable.
Judging by the code you posted, that second centroid is a file-scope variable that you never initialized to any specific value. Hence the meaningless results from your comparison function.
The second strange detail in your code is that you do return -orientation(centroid,*p1,*p2) if both points are above or below. Since orientation returns -1 for CCW and +1 for CW, it should be just return orientation(centroid,*p1,*p2). Why did you feel the need to negate the result of orientation?
Your original points don't appear form a convex polygon, so simply ordering them by angle around a fixed centroid will not necessarily result in a clean polygon. This is a non-trivial problem, you may want to research Delaunay triangulation and/or gift wrapping algorithms, although both would have to be modified because your polygon is concave. The answer here is an interesting example of a modified gift wrapping algorithm for concave polygons. There is also a C++ library called PCL that may do what you need.
But...if you really do want to do a polar sort, your sorting functions seem more complex than necessary. I would sort using atan2 first, then optimize it later once you get the result you want if necessary. Here is an example using lambda functions:
#include <algorithm>
#include <math.h>
#include <vector>
int main()
{
struct point
{
double x;
double y;
};
std::vector< point > points;
point centroid;
// fill in your data...
auto sort_predicate = [¢roid] (const point& a, const point& b) -> bool {
return atan2 (a.x - centroid.x, a.y - centroid.y) <
atan2 (b.x - centroid.x, b.y - centroid.y);
};
std::sort (points.begin(), points.end(), sort_predicate);
}
I'm making a application for school in which I have to click a particular object.
EDIT: This is being made in 2D
I have a rectangle, I rotate this rectangle by X.
The rotation of the rectangle has made my rectangles (x,y,width,height) become a new rectangle around the rotated rectangle.
http://i.stack.imgur.com/MejMA.png
(excuse me for my terrible paint skills)
The Black lines describe the rotated rectangle, the red lines are my new rectangle.
I need to find out if my mouse is within the black rectangle or not. Whatever rotation I do I already have a function for getting the (X,Y) for each corner of the black rectangle.
Now I'm trying to implement this Check if point is within triangle (The same side technique).
So I can either check if my mouse is within each triangle or if theres a way to check if my mouse is in the rotated rectangle that would be even better.
I practically understand everything written in the triangle document, but I simply don't have the math skills to calculate the cross product and the dot product of the 2 cross products.
This is supposed to be the cross product:
a × b = |a| |b| sin(θ) n
|a| is the magnitude (length) of vector a
|b| is the magnitude (length) of vector b
θ is the angle between a and b
n is the unit vector at right angles to both a and b
But how do I calculate the unit vector to both a and b?
And how do I get the magnitude of a vector?
EDIT:
I forgot to ask for the calculation of the dotproduct between 2 cross products.
function SameSide(p1,p2, a,b)
cp1 = CrossProduct(b-a, p1-a)
cp2 = CrossProduct(b-a, p2-a)
if DotProduct(cp1, cp2) >= 0 then return true
else return false
Thank you everyone for your help I think I got the hang of it now, I wish I could accept multiple answers.
If you are having to carry out loads of check, I would shy away from using square root functions: they are computationally expensive. for comparison purposes, just multiply everything by itself and you can bypass the square rooting:
magnitude of vector = length of vector
If vector is defined as float[3] length can be calculated as follows:
double magnitude = sqrt( a[0]*a[0] + a[1]*a[1] + a[2]*a[2] );
However that is expensive computationally so I would use
double magnitudeSquared = a[0]*a[0] + a[1]*a[1] + a[2]*a[2];
Then modify any comparative calculations to use the squared version of the distance or magnitude and it will be more performant.
For the cross product, please forgive me if this maths is shaky, it has been a couple of years since I wrote functions for this (code re-use is great but terrible for remembering things):
double c[3];
c[0] = ( a[1]*b[2] - a[2]*b[1] );
c[1] = ( a[2]*b[0] - a[0]*b[2] );
c[2] = ( a[0]*b[1] - a[1]*b[0] );
To simplify it all I would put a vec3d in a class of its own, with a very simple representation being:
class vec3d
{
public:
float x, y, z;
vec3d crossProduct(vec3d secondVector)
{
vec3d retval;
retval.x = (this.y * secondVector.z)-(secondVector.y * this.z);
retval.y = -(this.x * secondVector.z)+(secondVector.x * this.z);
retval.z = (this.x * secondVector.y)-(this.y * secondVector.x);
return retval;
}
// to get the unit vector divide by a vectors length...
void normalise() // this will make the vector into a 1 unit long variant of itself, or a unit vector
{
if(fabs(x) > 0.0001){
x= x / this.magnitude();
}
if(fabs(y) > 0.0001){
y= y / this.magnitude();
}
if(fabs(z) > 0.0001){
z = / this.magnitude();
}
}
double magnitude()
{
return sqrt((x*x) + (y*y) + (z*z));
}
double magnitudeSquared()
{
return ((x*x) + (y*y) + (z*z));
}
};
A fuller implementation of a vec3d class can be had from one of my old 2nd year coding excercises: .h file and .cpp file.
And here is a minimalist 2d implementation (doing this off the top of my head so forgive the terse code please, and let me know if there are errors):
vec2d.h
#ifndef VEC2D_H
#define VEC2D_H
#include <iostream>
using namespace std;
class Vec2D {
private:
double x, y;
public:
Vec2D(); // default, takes no args
Vec2D(double, double); // user can specify init values
void setX(double);
void setY(double);
double getX() const;
double getY() const;
double getMagnitude() const;
double getMagnitudeSquared() const;
double getMagnitude2() const;
Vec2D normalize() const;
double crossProduct(Vec2D secondVector);
Vec2D crossProduct(Vec2D secondVector);
friend Vec2D operator+(const Vec2D&, const Vec2D&);
friend ostream &operator<<(ostream&, const Vec2D&);
};
double dotProduct(const Vec2D, const Vec2D);
#endif
vec2d.cpp
#include <iostream>
#include <cmath>
using namespace std;
#include "Vec2D.h"
// Constructors
Vec2D::Vec2D() { x = y = 0.0; }
Vec2D::Vec2D(double a, double b) { x = a; y = b; }
// Mutators
void Vec2D::setX(double a) { x = a; }
void Vec2D::setY(double a) { y = a; }
// Accessors
double Vec2D::getX() const { return x; }
double Vec2D::getY() const { return y; }
double Vec2D::getMagnitude() const { return sqrt((x*x) + (y*y)); }
double Vec2D::getMagnitudeSquared() const { return ((x*x) + (y*y)); }
double Vec2D::getMagnitude2 const { return getMagnitudeSquared(); }
double Vec2d::crossProduct(Vec2D secondVector) { return ((this.x * secondVector.getY())-(this.y * secondVector.getX()));}
Vec2D crossProduct(Vec2D secondVector) {return new Vec2D(this.y,-(this.x));}
Vec2D Vec2D::normalize() const { return Vec2D(x/getMagnitude(), y/getMagnitude());}
Vec2D operator+(const Vec2D& a, const Vec2D& b) { return Vec2D(a.x + b.x, a.y + b.y);}
ostream& operator<<(ostream& output, const Vec2D& a) { output << "(" << a.x << ", " << a.y << ")" << endl; return output;}
double dotProduct(const Vec2D a, const Vec2D b) { return a.getX() * b.getX() + a.getY() * b.getY();}
Check if a point is inside a triangle described by three vectors:
float calculateSign(Vec2D v1, Vec2D v2, Vec2D v3)
{
return (v1.getX() - v3.getX()) * (v2.getY() - v3.getY()) - (v2.getX() - v3.getX()) * (v1.getY() - v3.getY());
}
bool isPointInsideTriangle(Vec2D point2d, Vec2D v1, Vec2D v2, Vec2D v3)
{
bool b1, b2, b3;
// the < 0.0f is arbitrary, could have just as easily been > (would have flipped the results but would compare the same)
b1 = calculateSign(point2d, v1, v2) < 0.0f;
b2 = calculateSign(point2d, v2, v3) < 0.0f;
b3 = calculateSign(point2d, v3, v1) < 0.0f;
return ((b1 == b2) && (b2 == b3));
}
In the code above if calculateSign is in the triangle you will get a true returned :)
Hope this helps, let me know if you need more info or a fuller vec3d or 2d class and I can post:)
Addendum
I have added in a small 2d-vector class, to show the differences in the 2d and 3d ones.
The magnitude of a vector is its length. In C++, if you have a vector represented as a double[3], you would calculate the length via
#include <math.h>
double a_length = sqrt( a[0]*a[0] + a[1]*a[1] + a[2]*a[2] );
However, I understand what you actually want is the cross product? In that case, you may want to calculate it directly. The result is a vector, i.e. c = a x b.
You code it like this for example:
double c[3];
c[0] = ( a[2]*b[3] - a[3]*b[2] );
c[1] = ( a[3]*b[1] - a[1]*b[3] );
c[2] = ( a[1]*b[2] - a[2]*b[1] );
You can calculate the magnitude of vector by sqrt(x*x + y*y). Also you can calculate the crossproduct simpler: a x b = a.x * b.y - a.y * b.x. Checking that a point is inside triangle can be done by counting the areas for all 4 triangles. For example a is the area of the source triangle, b,c,d are areas of other ones. If b + c + d = a then the point is inside. Counting the area of triangle is simple: we have vectors a, b that are vertexes of triangle. The area of triangle then is (a x b) / 2
One simple way without getting into vectors is to check for area.
For example ,lets say you have a rectangle with corners A,B,C,D. and point P.
first calculate the area of rectangle, simply find height and width of the rectangle and multiply.
B D
| /
| /
|/____ C
A
For calculating the height,width take one point lets say A, find its distance from all other three points i.e AB,AC,AD 1st and 2nd minimum will be width,and height, max will be diagonal length.
Now store the points from which you get the height, width, lets says those points are B,C.
So now you know how rectangle looks, i.e
B _____ D
| |
|_____|
A C
Then calculate the sum of area of triangles ACP,ABP,BDP,CDP (use heros formula to compute area of rectangle), if it equals to the area of rectangle, point P is inside else outside the rectangle.
I have a path of points that represent the outline of a polygon. The path is constructed from pixels.
This means all points are very very close to each other, but I've ensured they are all unique.
Right now I'm checking if 3 points are collinear, and if they are, I remove the middle one.
I check if they are collinear using dot product. I observed however that many of my dot products are 0.0f. What could be wrong?
void ImagePolygon::computeOptimized()
{
m_optimized = m_hull;
m_optimized.erase(
std::unique(m_optimized.begin(),
m_optimized.end()),
m_optimized.end());
int first = 0;
int second = 1;
std::vector<int> removeList;
for(int i = 2; i < m_optimized.size(); ++i)
{
second = i - 1;
first = i - 2;
if(isColinear(m_optimized[i - 2],m_optimized[i - 1],m_optimized[i]))
{
m_optimized.erase(m_optimized.begin() + i - 1);
removeList.push_back(i - 1);
}
}
std::sort(removeList.rbegin(),removeList.rend());
for(int i = 0; i < removeList.size(); ++i)
{
m_optimized.erase(m_optimized.begin() + removeList[i]);
}
}
bool ImagePolygon::isColinear( const b2Vec2& a, const b2Vec2& b, const b2Vec2& c ) const
{
b2Vec2 vec1 = b2Vec2(b.x - a.x, b.y - a.y);
vec1.Normalize();
b2Vec2 vec2 = b2Vec2(c.x - b.x, c.y - b.y);
vec2.Normalize();
float dotProduct = vec1.x * vec2.x + vec1.y * vec2.y;
//test value
return abs(dotProduct) > 0.00001f;
}
The major problem is that I'm getting a lot of 0 dot products when I should not so therefore no matter where I set the threshold the path is not optimized as much as it should be.
Thanks
float32 Normalize()
{
float32 length = Length();
if (length < b2_epsilon)
{
return 0.0f;
}
float32 invLength = 1.0f / length;
x *= invLength;
y *= invLength;
return length;
}
You want the 2x2 determinant vec1.x * vec2.y - vec1.y * vec2.x instead of the dot product. The determinant is zero iff the points are collinear, whereas the dot product is zero iff the points form a right angle.
This:
return abs(dotProduct) > 0.00001f;
is actually telling you whether your vectors are (not) perpendicular, not whether they are parallel. Check if it's close to 1 rather than close to 0 for parallel.
You should not increment index in case the element is deleted. You are skipping some values. Try the following:
for(int i = 2; i < m_optimized.size();) {
second = i - 1;
first = i - 2;
if (isColinear(m_optimized[i - 2],m_optimized[i - 1],m_optimized[i])) {
m_optimized.erase(m_optimized.begin() + i - 1);
removeList.push_back(i - 1);
} else i++;
}
Also I can not understand the purpose of the removeList. You erase some points inside of the main loop and try to erase the same points in the subsidiary loop. It seems to be an error. BTW, there is no reason to sort removeList due to the way it was constructed.
My brain has been melting over a line segment-vs-cylinder intersection routine I've been working on.
/// Line segment VS <cylinder>
// - cylinder (A, B, r) (start point, end point, radius)
// - line has starting point (x0, y0, z0) and ending point (x0+ux, y0+uy, z0+uz) ((ux, uy, uz) is "direction")
// => start = (x0, y0, z0)
// dir = (ux, uy, uz)
// A
// B
// r
// optimize? (= don't care for t > 1)
// <= t = "time" of intersection
// norm = surface normal of intersection point
void CollisionExecuter::cylinderVSline(const Ogre::Vector3& start, const Ogre::Vector3& dir, const Ogre::Vector3& A, const Ogre::Vector3& B, const double r,
const bool optimize, double& t, Ogre::Vector3& normal) {
t = NaN;
// Solution : http://www.gamedev.net/community/forums/topic.asp?topic_id=467789
double cxmin, cymin, czmin, cxmax, cymax, czmax;
if (A.z < B.z) { czmin = A.z - r; czmax = B.z + r; } else { czmin = B.z - r; czmax = A.z + r; }
if (A.y < B.y) { cymin = A.y - r; cymax = B.y + r; } else { cymin = B.y - r; cymax = A.y + r; }
if (A.x < B.x) { cxmin = A.x - r; cxmax = B.x + r; } else { cxmin = B.x - r; cxmax = A.x + r; }
if (optimize) {
if (start.z >= czmax && (start.z + dir.z) > czmax) return;
if (start.z <= czmin && (start.z + dir.z) < czmin) return;
if (start.y >= cymax && (start.y + dir.y) > cymax) return;
if (start.y <= cymin && (start.y + dir.y) < cymin) return;
if (start.x >= cxmax && (start.x + dir.x) > cxmax) return;
if (start.x <= cxmin && (start.x + dir.x) < cxmin) return;
}
Ogre::Vector3 AB = B - A;
Ogre::Vector3 AO = start - A;
Ogre::Vector3 AOxAB = AO.crossProduct(AB);
Ogre::Vector3 VxAB = dir.crossProduct(AB);
double ab2 = AB.dotProduct(AB);
double a = VxAB.dotProduct(VxAB);
double b = 2 * VxAB.dotProduct(AOxAB);
double c = AOxAB.dotProduct(AOxAB) - (r*r * ab2);
double d = b * b - 4 * a * c;
if (d < 0) return;
double time = (-b - sqrt(d)) / (2 * a);
if (time < 0) return;
Ogre::Vector3 intersection = start + dir * time; /// intersection point
Ogre::Vector3 projection = A + (AB.dotProduct(intersection - A) / ab2) * AB; /// intersection projected onto cylinder axis
if ((projection - A).length() + (B - projection).length() > AB.length()) return; /// THIS IS THE SLOW SAFE WAY
//if (projection.z > czmax - r || projection.z < czmin + r ||
// projection.y > cymax - r || projection.y < cymin + r ||
// projection.x > cxmax - r || projection.x < cxmin + r ) return; /// THIS IS THE FASTER BUGGY WAY
normal = (intersection - projection);
normal.normalise();
t = time; /// at last
}
I have thought of this way to speed up the discovery of whether the projection of the intersection point lies inside the cylinder's length. However, it doesn't work and I can't really get it because it seems so logical :
if the projected point's x, y or z co-ordinates are not within the cylinder's limits, it should be considered outside. It seems though that this doesn't work in practice.
Any help would be greatly appreciated!
Cheers,
Bill Kotsias
Edit : It seems that the problems rise with boundary-cases, i.e when the cylinder is parallel to one of the axis. Rounding errors come into the equation and the "optimization" stops working correctly.
Maybe, if the logic is correct, the problems will go away by inserting a bit of tolerance like :
if (projection.z > czmax - r + 0.001 || projection.z < czmin + r - 0.001 || ... etc...
The cylinder is circular, right? You could transform coordinates so that the center line of the cylinder functions as the Z axis. Then you have a 2D problem of intersecting a line with a circle. The intersection points will be in terms of a parameter going from 0 to 1 along the length of the line, so you can calculate their positions in that coordinate system and compare to the top and bottom of the cylinder.
You should be able to do it all in closed form. No tolerances. And sure, you will get singularities and imaginary solutions. You seem to have thought of all this, so I guess I'm not sure what the question is.
This is what I use, it may help:
bool d3RayCylinderIntersection(const DCylinder &cylinder,const DVector3 &org,const DVector3 &dir,float &lambda,DVector3 &normal,DVector3 &newPosition)
// Ray and cylinder intersection
// If hit, returns true and the intersection point in 'newPosition' with a normal and distance along
// the ray ('lambda')
{
DVector3 RC;
float d;
float t,s;
DVector3 n,D,O;
float ln;
float in,out;
RC=org; RC.Subtract(&cylinder.position);
n.Cross(&dir,&cylinder.axis);
ln=n.Length();
// Parallel? (?)
if((ln<D3_EPSILON)&&(ln>-D3_EPSILON))
return false;
n.Normalize();
d=fabs(RC.Dot(n));
if (d<=cylinder.radius)
{
O.Cross(&RC,&cylinder.axis);
//TVector::cross(RC,cylinder._Axis,O);
t=-O.Dot(n)/ln;
//TVector::cross(n,cylinder._Axis,O);
O.Cross(&n,&cylinder.axis);
O.Normalize();
s=fabs( sqrtf(cylinder.radius*cylinder.radius-d*d) / dir.Dot(O) );
in=t-s;
out=t+s;
if (in<-D3_EPSILON)
{
if(out<-D3_EPSILON)
return false;
else lambda=out;
} else if(out<-D3_EPSILON)
{
lambda=in;
} else if(in<out)
{
lambda=in;
} else
{
lambda=out;
}
// Calculate intersection point
newPosition=org;
newPosition.x+=dir.x*lambda;
newPosition.y+=dir.y*lambda;
newPosition.z+=dir.z*lambda;
DVector3 HB;
HB=newPosition;
HB.Subtract(&cylinder.position);
float scale=HB.Dot(&cylinder.axis);
normal.x=HB.x-cylinder.axis.x*scale;
normal.y=HB.y-cylinder.axis.y*scale;
normal.z=HB.z-cylinder.axis.z*scale;
normal.Normalize();
return true;
}
return false;
}
Have you thought about it this way?
A cylinder is essentially a "fat" line segment so a way to do this would be to find the closest point on line segment (the cylinder's center line) to line segment (the line segment you are testing for intersection).
From there, you check the distance between this closest point and the other line segment, and compare it to the radius.
At this point, you have a "Pill vs Line Segment" test, but you could probably do some plane tests to "chop off" the caps on the pill to make a cylinder.
Shooting from the hip a bit though so hope it helps (:
Mike's answer is good. For any tricky shape you're best off finding the transformation matrix T that maps it into a nice upright version, in this case an outright cylinder with radius 1. height 1, would do the job nicely. Figure out your new line in this new space, perform the calculation, convert back.
However, if you are looking to optimise (and it sounds like you are), there is probably loads you can do.
For example, you can calculate the shortest distance between two lines -- probably using the dot product rule -- imagine joining two lines by a thread. Then if this thread is the shortest of all possible threads, then it will be perpendicular to both lines, so Thread.LineA = Thread.LineB = 0
If the shortest distance is greater than the radius of the cylinder, it is a miss.
You could define the locus of the cylinder using x,y,z, and thrash the whole thing out as some horrible quadratic equation, and optimise by calculating the discriminant first, and returning no-hit if this is negative.
To define the locus, take any point P=(x,y,z). drop it as a perpendicular on to the centre line of your cylinder, and look at its magnitude squared. if that equals R^2 that point is in.
Then you throw your line {s = U + lamda*V} into that mess, and you would end up with some butt ugly quadratic in lamda. but that would probably be faster than fiddling matrices, unless you can get the hardware to do it (I'm guessing OpenGL has some function to get the hardware to do this superfast).
It all depends on how much optimisation you want; personally I would go with Mike's answer unless there was a really good reason not to.
PS You might get more help if you explain the technique you use rather than just dumping code, leaving it to the reader to figure out what you're doing.