Related
I have checked out this question, but the answer is very large for me:
How to know if a line intersects a plane in C#? - Basic 2D geometry
Is there any .NET method to know if a line defined by two points intersects a rectangle?
public bool Intersects(Point a, Point b, Rectangle r)
{
// return true if the line intersects the rectangle
// false otherwise
}
Thanks in advance.
public static bool LineIntersectsRect(Point p1, Point p2, Rectangle r)
{
return LineIntersectsLine(p1, p2, new Point(r.X, r.Y), new Point(r.X + r.Width, r.Y)) ||
LineIntersectsLine(p1, p2, new Point(r.X + r.Width, r.Y), new Point(r.X + r.Width, r.Y + r.Height)) ||
LineIntersectsLine(p1, p2, new Point(r.X + r.Width, r.Y + r.Height), new Point(r.X, r.Y + r.Height)) ||
LineIntersectsLine(p1, p2, new Point(r.X, r.Y + r.Height), new Point(r.X, r.Y)) ||
(r.Contains(p1) && r.Contains(p2));
}
private static bool LineIntersectsLine(Point l1p1, Point l1p2, Point l2p1, Point l2p2)
{
float q = (l1p1.Y - l2p1.Y) * (l2p2.X - l2p1.X) - (l1p1.X - l2p1.X) * (l2p2.Y - l2p1.Y);
float d = (l1p2.X - l1p1.X) * (l2p2.Y - l2p1.Y) - (l1p2.Y - l1p1.Y) * (l2p2.X - l2p1.X);
if( d == 0 )
{
return false;
}
float r = q / d;
q = (l1p1.Y - l2p1.Y) * (l1p2.X - l1p1.X) - (l1p1.X - l2p1.X) * (l1p2.Y - l1p1.Y);
float s = q / d;
if( r < 0 || r > 1 || s < 0 || s > 1 )
{
return false;
}
return true;
}
Unfortunately the wrong answer has been voted up. It is much to expensive to compute the actual intersection points, you only need comparisons. The keyword to look for is "Line Clipping" (http://en.wikipedia.org/wiki/Line_clipping). Wikipedia recommends the Cohen-Sutherland algorithm (http://en.wikipedia.org/wiki/Cohen%E2%80%93Sutherland) when you want fast rejects, which is probably the most common scenario. There is a C++-implementation on the wikipedia page. If you are not interested in actually clipping the line, you can skip most of it.
The answer of #Johann looks very similar to that algorithm, but I didn't look at it in detail.
Brute force algorithm...
First check if the rect is to the left or right of the line endpoints:
Establish the leftmost and rightmost X values of the line endpoints: XMIN and XMAX
If Rect.Left > XMAX, then no intersection.
If Rect.Right < XMIN, then no intersection.
Then, if the above wasn't enough to rule out intersection, check if the rect is above or below the line endpoints:
Establish the topmost and bottommost Y values of the line endpoints: YMAX and YMIN
If Rect.Bottom > YMAX, then no intersection.
If Rect.Top < YMIN, then no intersection.
Then, if the above wasn't enough to rule out intersection, you need to check the equation of the line, y = m * x + b, to see if the rect is above the line:
Establish the line's Y-value at Rect.Left and Rect.Right: LINEYRECTLEFT and LINEYRECTRIGHT
If Rect.Bottom > LINEYRECTRIGHT && Rect.Bottom > LINEYRECTLEFT, then no intersection.
Then, if the above wasn't enough to rule out intersection, you need to check if the rect is below the line:
If Rect.Top < LINEYRECTRIGHT && Rect.Top < LINEYRECTLEFT, then no intersection.
Then, if you get here:
Intersection.
N.B. I'm sure there's a more elegant algebraic solution, but performing these steps geometrically with pen and paper is easy to follow.
Some untested and uncompiled code to go with that:
public struct Line
{
public int XMin { get { ... } }
public int XMax { get { ... } }
public int YMin { get { ... } }
public int YMax { get { ... } }
public Line(Point a, Point b) { ... }
public float CalculateYForX(int x) { ... }
}
public bool Intersects(Point a, Point b, Rectangle r)
{
var line = new Line(a, b);
if (r.Left > line.XMax || r.Right < line.XMin)
{
return false;
}
if (r.Top < line.YMin || r.Bottom > line.YMax)
{
return false;
}
var yAtRectLeft = line.CalculateYForX(r.Left);
var yAtRectRight = line.CalculateYForX(r.Right);
if (r.Bottom > yAtRectLeft && r.Bottom > yAtRectRight)
{
return false;
}
if (r.Top < yAtRectLeft && r.Top < yAtRectRight)
{
return false;
}
return true;
}
This code has better performance:
public static bool SegmentIntersectRectangle(
double rectangleMinX,
double rectangleMinY,
double rectangleMaxX,
double rectangleMaxY,
double p1X,
double p1Y,
double p2X,
double p2Y)
{
// Find min and max X for the segment
double minX = p1X;
double maxX = p2X;
if (p1X > p2X)
{
minX = p2X;
maxX = p1X;
}
// Find the intersection of the segment's and rectangle's x-projections
if (maxX > rectangleMaxX)
{
maxX = rectangleMaxX;
}
if (minX < rectangleMinX)
{
minX = rectangleMinX;
}
if (minX > maxX) // If their projections do not intersect return false
{
return false;
}
// Find corresponding min and max Y for min and max X we found before
double minY = p1Y;
double maxY = p2Y;
double dx = p2X - p1X;
if (Math.Abs(dx) > 0.0000001)
{
double a = (p2Y - p1Y)/dx;
double b = p1Y - a*p1X;
minY = a*minX + b;
maxY = a*maxX + b;
}
if (minY > maxY)
{
double tmp = maxY;
maxY = minY;
minY = tmp;
}
// Find the intersection of the segment's and rectangle's y-projections
if (maxY > rectangleMaxY)
{
maxY = rectangleMaxY;
}
if (minY < rectangleMinY)
{
minY = rectangleMinY;
}
if (minY > maxY) // If Y-projections do not intersect return false
{
return false;
}
return true;
}
You can also check how it's work in JS demo: http://jsfiddle.net/77eej/2/
If you have two Points and Rect you can call this function like that:
public static bool LineIntersectsRect(Point p1, Point p2, Rect r)
{
return SegmentIntersectRectangle(r.X, r.Y, r.X + r.Width, r.Y + r.Height, p1.X, p1.Y, p2.X, p2.Y);
}
I took HABJAN's solution, which worked well, and converted it to Objective-C. The Objective-C code is as follows:
bool LineIntersectsLine(CGPoint l1p1, CGPoint l1p2, CGPoint l2p1, CGPoint l2p2)
{
CGFloat q = (l1p1.y - l2p1.y) * (l2p2.x - l2p1.x) - (l1p1.x - l2p1.x) * (l2p2.y - l2p1.y);
CGFloat d = (l1p2.x - l1p1.x) * (l2p2.y - l2p1.y) - (l1p2.y - l1p1.y) * (l2p2.x - l2p1.x);
if( d == 0 )
{
return false;
}
float r = q / d;
q = (l1p1.y - l2p1.y) * (l1p2.x - l1p1.x) - (l1p1.x - l2p1.x) * (l1p2.y - l1p1.y);
float s = q / d;
if( r < 0 || r > 1 || s < 0 || s > 1 )
{
return false;
}
return true;
}
bool LineIntersectsRect(CGPoint p1, CGPoint p2, CGRect r)
{
return LineIntersectsLine(p1, p2, CGPointMake(r.origin.x, r.origin.y), CGPointMake(r.origin.x + r.size.width, r.origin.y)) ||
LineIntersectsLine(p1, p2, CGPointMake(r.origin.x + r.size.width, r.origin.y), CGPointMake(r.origin.x + r.size.width, r.origin.y + r.size.height)) ||
LineIntersectsLine(p1, p2, CGPointMake(r.origin.x + r.size.width, r.origin.y + r.size.height), CGPointMake(r.origin.x, r.origin.y + r.size.height)) ||
LineIntersectsLine(p1, p2, CGPointMake(r.origin.x, r.origin.y + r.size.height), CGPointMake(r.origin.x, r.origin.y)) ||
(CGRectContainsPoint(r, p1) && CGRectContainsPoint(r, p2));
}
Many thanks HABJAN. I will note that at first I wrote my own routine which checked each point along the gradient, and I did everything I could do to maximize performance, but this was immediately far faster.
For Unity (inverts y!). This takes care of division by zero problem that other approaches here have:
using System;
using UnityEngine;
namespace Util {
public static class Math2D {
public static bool Intersects(Vector2 a, Vector2 b, Rect r) {
var minX = Math.Min(a.x, b.x);
var maxX = Math.Max(a.x, b.x);
var minY = Math.Min(a.y, b.y);
var maxY = Math.Max(a.y, b.y);
if (r.xMin > maxX || r.xMax < minX) {
return false;
}
if (r.yMin > maxY || r.yMax < minY) {
return false;
}
if (r.xMin < minX && maxX < r.xMax) {
return true;
}
if (r.yMin < minY && maxY < r.yMax) {
return true;
}
Func<float, float> yForX = x => a.y - (x - a.x) * ((a.y - b.y) / (b.x - a.x));
var yAtRectLeft = yForX(r.xMin);
var yAtRectRight = yForX(r.xMax);
if (r.yMax < yAtRectLeft && r.yMax < yAtRectRight) {
return false;
}
if (r.yMin > yAtRectLeft && r.yMin > yAtRectRight) {
return false;
}
return true;
}
}
}
The simplest computational geometry technique is to just walk through the segments of the polygon and see if it intersects with any of them, as it then must also intersect the polygon.
The only caveat of this method (and most of CG) is that we have to be careful about edge cases. What if the line crosses the rectangle at a point - do we count that as intersection or not? Be careful in your implementation.
Edit: The typical tool for the line-intersects-segment calculation is a LeftOf(Ray, Point) test, which returns if the point is the to the left of the ray. Given a line l (which we use as a ray) and a segment containing points a and b, the line intersects the segment if one point is to the left and one point is not:
(LeftOf(l,a) && !LeftOf(l,b)) || (LeftOf(l,b) && !LeftOf(l,a))
Again, you need to watch out for edge-cases, when the point is on the line, but depends how you wish to actually define intersection.
There is no simple predefined .NET method you can call to accomplish that. However, using the Win32 API, there is a pretty easy way to do this (easy in the sense of implementation, performance is not the strong point): LineDDA
BOOL LineDDA(int nXStart,int nYStart,int nXEnd,int nYEnd,LINEDDAPROC lpLineFunc,LPARAM lpData)
This functions calls the callback function for every pixel of the line to be drawn. In this function, you can check if the pixel is within your rectangle - if you find one, then it intersects.
As I said, this is not the fastest solution, but pretty easy to implement. To use it in C#, you will of course need to DllImport it from gdi32.dll.
[DllImport("gdi32.dll")] public static extern int LineDDA(int n1,int n2,int n3,int n4,int lpLineDDAProc,int lParam);
I have line that is defined as two points.
start = (xs,ys)
end = (xe, ye)
Drawing function that I'm using Only accepts lines that are fully in screen coordinates.
Screen size is (xSize, ySize).
Top left corner is (0,0). Bottom right corner is (xSize, ySize).
Some other funcions gives me line that that is defined for example as start(-50, -15) end(5000, 200). So it's ends are outside of screen size.
In C++
struct Vec2
{
int x, y
};
Vec2 start, end //This is all little bit pseudo code
Vec2 screenSize;//You can access coordinates like start.x end.y
How can I calculate new start and endt that is at the screen edge, not outside screen.
I know how to do it on paper. But I can't transfer it to c++.
On paper I'm sershing for point that belongs to edge and line. But it is to much calculations for c++.
Can you help?
There are many line clipping algorithms like:
Cohen–Sutherland wikipedia page with implementation
Liang–Barsky wikipedia page
Nicholl–Lee–Nicholl (NLN)
and many more. see Line Clipping on wikipedia
[EDIT1]
See below figure:
there are 3 kinds of start point:
sx > 0 and sy < 0 (red line)
sx < 0 and sy > 0 (yellow line)
sx < 0 and sy < 0 (green and violet lines)
In situations 1 and 2 simply find Xintersect and Yintersect respectively and choose them as new start point.
As you can see, there are 2 kinds of lines in situation 3. In this situation find Xintersect and Yintersect and choose the intersect point near the end point which is the point that has minimum distance to endPoint.
min(distance(Xintersect, endPoint), distance(Yintersect, endPoint))
[EDIT2]
// Liang-Barsky function by Daniel White # http://www.skytopia.com/project/articles/compsci/clipping.html
// This function inputs 8 numbers, and outputs 4 new numbers (plus a boolean value to say whether the clipped line is drawn at all).
//
bool LiangBarsky (double edgeLeft, double edgeRight, double edgeBottom, double edgeTop, // Define the x/y clipping values for the border.
double x0src, double y0src, double x1src, double y1src, // Define the start and end points of the line.
double &x0clip, double &y0clip, double &x1clip, double &y1clip) // The output values, so declare these outside.
{
double t0 = 0.0; double t1 = 1.0;
double xdelta = x1src-x0src;
double ydelta = y1src-y0src;
double p,q,r;
for(int edge=0; edge<4; edge++) { // Traverse through left, right, bottom, top edges.
if (edge==0) { p = -xdelta; q = -(edgeLeft-x0src); }
if (edge==1) { p = xdelta; q = (edgeRight-x0src); }
if (edge==2) { p = -ydelta; q = -(edgeBottom-y0src);}
if (edge==3) { p = ydelta; q = (edgeTop-y0src); }
r = q/p;
if(p==0 && q<0) return false; // Don't draw line at all. (parallel line outside)
if(p<0) {
if(r>t1) return false; // Don't draw line at all.
else if(r>t0) t0=r; // Line is clipped!
} else if(p>0) {
if(r<t0) return false; // Don't draw line at all.
else if(r<t1) t1=r; // Line is clipped!
}
}
x0clip = x0src + t0*xdelta;
y0clip = y0src + t0*ydelta;
x1clip = x0src + t1*xdelta;
y1clip = y0src + t1*ydelta;
return true; // (clipped) line is drawn
}
Here is a function I wrote. It cycles through all 4 planes (left, top, right, bottom) and clips each point by the plane.
// Clips a line segment to an axis-aligned rectangle
// Returns true if clipping is successful
// Returns false if line segment lies outside the rectangle
bool clipLineToRect(int a[2], int b[2],
int xmin, int ymin, int xmax, int ymax)
{
int mins[2] = {xmin, ymin};
int maxs[2] = {xmax, ymax};
int normals[2] = {1, -1};
for (int axis=0; axis<2; axis++) {
for (int plane=0; plane<2; plane++) {
// Check both points
for (int pt=1; pt<=2; pt++) {
int* pt1 = pt==1 ? a : b;
int* pt2 = pt==1 ? b : a;
// If both points are outside the same plane, the line is
// outside the rectangle
if ( (a[0]<xmin && b[0]<xmin) || (a[0]>xmax && b[0]>xmax) ||
(a[1]<ymin && b[1]<ymin) || (a[1]>ymax && b[1]>ymax)) {
return false;
}
const int n = normals[plane];
if ( (n==1 && pt1[axis]<mins[axis]) || // check left/top plane
(n==-1 && pt1[axis]>maxs[axis]) ) { // check right/bottom plane
// Calculate interpolation factor t using ratio of signed distance
// of each point from the plane
const float p = (n==1) ? mins[axis] : maxs[axis];
const float q1 = pt1[axis];
const float q2 = pt2[axis];
const float d1 = n * (q1-p);
const float d2 = n * (q2-p);
const float t = d1 / (d1-d2);
// t should always be between 0 and 1
if (t<0 || t >1) {
return false;
}
// Interpolate to find the new point
pt1[0] = (int)(pt1[0] + (pt2[0] - pt1[0]) * t );
pt1[1] = (int)(pt1[1] + (pt2[1] - pt1[1]) * t );
}
}
}
}
return true;
}
Example Usage:
void testClipLineToRect()
{
int screenWidth = 320;
int screenHeight = 240;
int xmin=0;
int ymin=0;
int xmax=screenWidth-1;
int ymax=screenHeight-1;
int a[2] = {-10, 10};
int b[2] = {300, 250};
printf("Before clipping:\n\ta={%d, %d}\n\tb=[%d, %d]\n",
a[0], a[1], b[0], b[1]);
if (clipLineToRect(a, b, xmin, ymin, xmax, ymax)) {
printf("After clipping:\n\ta={%d, %d}\n\tb=[%d, %d]\n",
a[0], a[1], b[0], b[1]);
}
else {
printf("clipLineToRect returned false\n");
}
}
Output:
Before clipping:
a={-10, 10}
b=[300, 250]
After clipping:
a={0, 17}
b=[285, 239]
I have created an open plane area with thin cylinders on it like pucks, they bounce around the area and have collision detection for some larger cylinders also placed on the plane. I am trying to get them to now head towards a set point on the plane using a steering method.
The steering works for works for avoiding the obstacles by calculating distance from obstacle then calculating angle between direction travelling and the direction of the obstacle, using the calculation of the distance from obstacle when the puck is too close it steers left or right based on the calculated angle. The same technique reversed fails to work for steering towards a point, I have tried using both acos and atan2 to calculate the angle between direction travelling and target direction and from outputs believe this bit is right but when I try to use that calculation to determine when to steer towards the target I get unexpected results. Sometimes random turning?
#include "Puck.h"
#include <iostream>
#include <fstream>
using namespace std;
#include <math.h>
ofstream fout("danna.txt");
#ifndef M_PI
#define M_PI 3.1415
#endif
class TranslateCB : public osg::NodeCallback
{
public:
TranslateCB() : _dx( 0. ), _dy( 0. ), _dirx(1), _diry(0), _inc(0.1), _theta(0) {}
TranslateCB(Puck** pp, Obstacle** ob, int count, double r, double x, double y) : _dx( 0. ), _dy( 0. ),
_dirx(2.0*rand()/RAND_MAX-1), _diry(2.0*rand()/RAND_MAX-1), _inc(0.3), _theta(0)
{
obstacles = ob;
ob_count = count;
_radius = r;
_x = x;
_y = y;
puckH = pp;
}
virtual void operator()( osg::Node* node,osg::NodeVisitor* nv )
{
osg::MatrixTransform* mt =
dynamic_cast<osg::MatrixTransform*>( node );
osg::Matrix mR, mT;
mT.makeTranslate( _dx , _dy, 0. );
mt->setMatrix( mT );
double ob_dirx;
double ob_diry;
double ob_dist;
double centerX=0, centerY =0;
_theta = 0;
double min = 4;
// location that I am trying to get the pucks to head towards
centerX = 1;
centerY = 5;
double tDirx = (_x+_dx) - centerX;
double tDiry = (_y+_dy) - centerY;
double tDist = sqrt(tDirx*tDirx+tDiry*tDiry); //distance to target location
// normalizing my target direction
tDirx = tDirx/tDist;
tDiry = tDiry/tDist;
double hDist = sqrt(_dirx*_dirx + _diry*_diry); //distance to next heading
_dirx= _dirx/hDist;
_diry= _diry/hDist;
double cAngle = acos(_dirx*tDirx+_diry*tDiry); //using inverse of cos to calculate angle between directions
double tAngle = atan2(centerY - (_y+_dy),centerX - (_x+_dx)); // using inverse of tan to calculate angle between directions
double tMin = tDist*sin(cAngle);
//if statement used to define when to apply steering direction
if(tMin > 3)
{
if(tDist < 1){ _theta = 0; } //puck is inside target location, so keep travelling straight
if(cAngle > M_PI/2){ _theta = -0.1; } //turn left
else{ _theta = 0.1; } //turn right
}
else{ _theta = 0; }
////// The collision detection for the obstacles that works on the same princables that I am using above
for(int i = 0; i < ob_count; i++)
{
ob_dirx = (_x+_dx) - obstacles[i]->x;
ob_diry = (_y+_dy) - obstacles[i]->y;
ob_dist = sqrt(ob_dirx*ob_dirx+ob_diry*ob_diry);
if (ob_dist < 3) {
//normalise directions
double ob_norm = sqrt(ob_dirx*ob_dirx+ob_diry*ob_diry);
ob_dirx = (ob_dirx)/ob_norm;
ob_diry = (ob_diry)/ob_norm;
double norm = sqrt(_dirx*_dirx+_diry*_diry);
_dirx = (_dirx)/norm;
_diry = (_diry)/norm;
//calculate angle between direction travelling, and direction to obstacle
double angle = acos(_dirx*ob_dirx + _diry*ob_diry);
//calculate closest distance between puck and obstacle if continues on same path
double min_dist = ob_dist*sin(angle);
if(min_dist < _radius + obstacles[i]->radius && ob_dist < min+obstacles[i]->radius)
{
min = ob_dist;
if(ob_dist < _radius + obstacles[i]->radius){ _theta = 0; }
else if(angle <= M_PI/2){ _theta = -0.3; }
else{ _theta = 0.3; }
}
}
}
//change direction accordingly
_dirx = _dirx*cos(_theta) + _diry*sin(_theta);
_diry = _diry*cos(_theta) - _dirx*sin(_theta);
_dx += _inc*_dirx;
if((_x+_dx > 20 && _dirx > 0) || (_x+_dx < -20 && _dirx < 0))
{
_dirx = -_dirx;
_diry += (0.2*rand()/RAND_MAX-0.1); //add randomness to bounce
}
_dy += _inc*_diry;
if((_y+_dy > 20 && _diry > 0) || (_y+_dy < -20 && _diry < 0))
{
_diry = -_diry;
_dirx += (0.2*rand()/RAND_MAX-0.1); //add randomness to bounce
}
traverse( node, nv );
}
private:
double _dx,_dy;
double _dirx,_diry;
double _inc;
double _theta;
double _radius;
double _x,_y;
Obstacle** obstacles;
Puck** puckH;
int ob_count;
};
Puck::Puck()
{
}
void Puck::createBoids (Puck** pucks, Group *root, Obstacle** obstacles, int count, double xx, double yy)
{
// geometry
radius = 0.2;
x = xx;
y = yy;
ob_count = count;
Cylinder *shape=new Cylinder(Vec3(x,y,0),radius,0.1);
ShapeDrawable *draw=new ShapeDrawable(shape);
draw->setColor(Vec4(1,0,0,1));
Geode *geode=new Geode();
geode->addDrawable(draw);
// transformation
MatrixTransform *T=new MatrixTransform();
TranslateCB *tcb = new TranslateCB(pucks, obstacles,ob_count,radius,x,y);
T->setUpdateCallback(tcb);
T->addChild(geode);
root->addChild(T);
}
any help would be amazing!
The problem here is that the technique that "works" for avoiding obstacles will always occur when the puck is heading towards the obstacle. This special condition makes both the direction of the puck and the direction of the obstacle in adjacent quadrants.
When attempting to make the pucks steer towards the obstacle however, the technique breaks down because the puck most likely will be heading away from the obstacle, no longer having the condition that the target and direction vectors are in adjacent quadrants.
The correct way to determine the steering direction is to rotate the target vector by an angle that would make the the direction vector point straight up in the quadrants (0, 1). Now that the target vector is relative to the direction vector (0, 1) looking at the x component of the target vector will determine the steering direction. If the x component of the target vector is negative, the puck must turn left to steer towards the target (increase the angle). If the x component of the target vector is positive, the puck must turn right to steer towards the target (decrease the angle).
Consider the following snippet written in python to verify this, it should still be easy to read for you to grasp the concept:
from math import *
dirX = 0.0
dirY = 0.0
targX = 1.0
targY = 0.0
def dir():
global dirX, dirY, targX, targY
# get magnitiude of direction
mag1 = sqrt(dirX*dirX + dirY*dirY)
if mag1 != 0:
# normalize direction vector
normX = dirX / mag1
normY = dirY / mag1
# get magnitude of target vector
mag2 = sqrt(targX*targX + targY*targY)
if mag2 != 0:
# normalize target vector
targX = targX / mag2
targY = targY / mag2
# find the angle need to rotate the dir vector to (0, 1)
rotateAngle = (pi/2.0) - atan2(normY, normX)
# rotate targ vector by that angle (we only care about the x component)
relTargX = cos(rotateAngle) * normX + sin(rotateAngle) * normY
# if the target vector's x is negative
if relTargX < 0:
# turn left
print "Left!"
# otherwise the target vector is 0 or positive
else:
# turn right
print "Right!"
def out():
global dirX, dirY, targX, targY
# function just prints values to the screen
print "dir(%f, %f) targ(%f, %f)" % (dirX, dirY, targX, targY)
# for values 0 to 360
for i in range(360):
# pretend this is the pucks direction
dirX = sin(radians(i))
dirY = cos(radians(i))
# print dir and target vectors to screen
out()
# print the direction to turn
dir()
I suppose I could've written this in C++, but compared to running a python prompt it's a royal pain. It is as readable as any pseudo code I could've written and the concepts will work regardless of language.
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.
I am trying to implement the Winding Number Algorithm to test if a point is within another polygon. Although the results from my algorithm are wrong and not consistent. I have been working on this for ages now and it has become a bit of a pain!
I have basically converted pseudo code from notes and websites, such as, softsurfer.com
I successfully detect if my player and building object bounding boxes overlap. I return the result to a struct, (BoxResult) which lets me know if there has been a collision and returns the box which it has collided with (Below)
struct BoxResult{
bool collide;
Building returned;
};
void buildingCollision(){
int wn = 0; //winding number count
BoxResult detect = boxDetection(); //detect if any bounding boxes intersect
if(detect.collide){ //If a bounding box has collided, excute Winding Number Algorithm.
for(int i = 0; i < player.getXSize(); i++){
Point p;
p.x = player.getXi(i);
p.y = player.getYi(i);
wn = windingNum(detect.returned,p);
cout << wn << endl;
//Continue code to figure out rebound reaction
}
}
}
I then test for a collision between the building and the player (Below). I have tried 5 different attempts and hours of debugging to understand where the error is occuring, however I am implementing the most ineffienct method which just uses maths (Below).
int windingNum(Building & b, Point & p){
int result = 0; //Winding number is one, if point is in poly
float total; //Counts the total angle between different vertexs
double wn;
for(int i = 0; i <= b.getXSize()-1;i++){
float acs, nom, modPV, modPV1, denom, angle;
if(i == 3){
//Create the different points PVi . PVi+1
Point PV, PV1;
PV.x = (b.getXi(i) + wx) * p.x;
PV.y = (b.getYi(i) + wy) * p.y;
PV1.x = (b.getXi(0) + wx) * p.x;
PV1.y = (b.getYi(0) + wy) * p.y;
modPV = sqrt( (PV.x * PV.x) + (PV.y * PV.y)); //Get the modulus of PV
modPV1 = sqrt( (PV1.x * PV1.x) + (PV1.y * PV1.y)); //Get modulus of PV1
nom = (PV1.x * PV.x) + (PV1.y * PV.y); //Dot product of PV and PV1
denom = modPV * modPV1; //denomintor of winding number equation
angle = nom / denom;
acs = acos(angle) * 180/PI; //find the angle between the different points
total = total + acs; //add this angle, to the total angle count
}
if(i < 3){
//Create the different points PVi . PVi+1
Point PV, PV1;
PV.x = (b.getXi(i) + wx) * p.x;
PV.y = (b.getYi(i) + wy) * p.y;
PV1.x = (b.getXi(i+1) +wx) * p.x;
PV1.y = (b.getYi(i+1) +wy) * p.y;
modPV = sqrt((PV.x * PV.x) + (PV.y * PV.y)); //Get the modulus of PV
modPV1 = sqrt((PV1.x * PV1.x) + (PV1.y * PV1.y)); //Get modulus of PV1
nom = (PV1.x * PV.x) + (PV1.y * PV.y); //Dot product of PV and PV1
denom = modPV * modPV1; //denomintor of winding number equation
angle = nom / denom;
acs = acos(angle) * 180/PI; //find the angle between the different points
total = total + acs; //add this angle, to the total angle count
}
}
wn = total;
if(wn < 360){
result = 0;}
if(wn == 360){
result = 1;}
return result;
}
For reasons I do not understand acs = acos(angle) always returns 1.#IND000.
Btw so you know, I am just testing the algorithm against another square, hence the two if statements if i == 3 and if i < 3.
Also incase you need to know these, wy and wx are the world co-ordinates which are translated. Thus moving the player around the world e.g. to move the player forward everything is translated by a minus number for wy.
Further, a Building object would look something like the following struct below:
struct Building {
vector<float> x; //vector storing x co-ords
vector<float> y; //vector storing y co-ords
float ymax, ymin, xmax, xmin //values for bounding box
vector<int> polygons; //stores the number points per polygon (not relevant to the problem)
}
If anyone can help I would amazingly grateful! I just wish I could see where it is all going wrong! (Something I am sure all programmers have said in there time lol) Thanks for readings...
The two lines calculating the modulus of PV and PV1 are incorrect. They should be
modPV = sqrt(PV.x * PV.x + PV.y * PV.y );
modPV1 = sqrt(PV1.x * PV1.x + PV1.y * PV1.y);
Does that fix the problem?
I probably don't understand your problem/question, but there's a simple and robust point in polygon test available here: PNPOLY.
As regards your implementation of the crossing number algorithm the first obvious mistake is that you are not looping over all the sides. You are one short. You should loop up to i < n and then define i plus one as
int ip1 = ( i + 1 ) % n;
This applies to the code in your original question too of course to save you having to have two copies of the code.
The second one is that
rem = cn % 1;
has no effect. The code on softsurfer is fine i.e.
rem = (cn&1);
It is trying to detect if cn is odd or even by testing if the last bit is set. If you want to the same test using the modulo operator % then you should write it as
rem = cn % 2;
as that assigns the remainder on division by two of cn to rem.
I haven't looked beyond that to see if there are any more issues.
I have given up with the winding number code, it really has got me! If anyone does find the solution I would still be amazingly grateful. I am now trying with point in poly detection using the crossing number algorithm. I kept the pesudo code in the comments, again from softsurfer....
int cn_PnPoly( Point P, Building & b, int n )
{
int cn = 0; // the crossing number counter
int rem = 0;
vector<float>x;
vector<float>y;
x.swap(b.getX());
y.swap(b.getY());
//// loop through all edges of the polygon
//for (int i=0; i<n; i++) { // edge from V[i] to V[i+1]
// if (((V[i].y <= P.y) && (V[i+1].y > P.y)) // an upward crossing
// || ((V[i].y > P.y) && (V[i+1].y <= P.y))) { // a downward crossing
// // compute the actual edge-ray intersect x-coordinate
// float vt = (float)(P.y - V[i].y) / (V[i+1].y - V[i].y);
// if (P.x < V[i].x + vt * (V[i+1].x - V[i].x)) // P.x < intersect
// ++cn; // a valid crossing of y=P.y right of P.x
// }
//}
//return (cn&1); // 0 if even (out), and 1 if odd (in)
// loop through all edges of the polygon
for (int i=0; i<n-1; i++) { // edge from V[i] to V[i+1]
if (((y.at(i) <= P.y) && (y.at(i+1) > P.y)) // an upward crossing
|| ((y.at(i) > P.y) && (y.at(i+1) <= P.y))) { // a downward crossing
// compute the actual edge-ray intersect x-coordinate
float vt = (float)(P.y - y.at(i)) / (y.at(i+1) - y.at(i));
if (P.x < x.at(i) + vt * (x.at(i+1) - x.at(i))) // P.x < intersect
++cn; // a valid crossing of y=P.y right of P.x
}
}
rem = cn % 1;
return (rem); // 0 if even (out), and 1 if odd (in)
}
Again this always returns zero, I am unsure why!?! Have I converted the algorithm incorrectly? Does it matter which direction the points are tested (i.e. clockwise, anti-clockwise)?
I have tried implementing PNPOLY as audris suggests. However this gives some funny results.
Below is the orginal C code, then below that is my conversion of that for my app...
Original C code...
int pnpoly(int nvert, float *vertx, float *verty, float testx, float testy)
{
int i, j, c = 0;
for (i = 0, j = nvert-1; i < nvert; j = i++) {
if ( ((verty[i]>testy) != (verty[j]>testy)) &&
(testx < (vertx[j]-vertx[i]) * (testy-verty[i]) / (verty[j]-verty[i]) + vertx[i]) )
c = !c;
}
return c;
}
My code....
Where wx and wy are the global co-ordinates.
int pnpoly(int nvert, vector<float> vertx, vector<float> verty, float testx, float testy)
{
int i, j, c = 0;
for (i = 0, j = nvert-1; i < nvert; j = i++) {
if ( (( (verty.at(i)+wy) > testy) != ( (verty.at(j)+wy) >testy)) &&
(testx < ((vertx.at(j)+wx) - (vertx.at(i)+wx) ) * (testy- (verty.at(i)+wy) ) / ( (verty.at(j)+wy) - (verty.at(i)+wy)) + (vertx.at(i)+wx)) )
c++;
}
return c;
}
I am testing the player object, against a 2D square building. This also returns strange results, when I hit bottom line (xmin,ymin to xmax,ymin) it works fine. If I hit ethier of the sides (xmin,ymin to xmin,ymax or xmax,ymin to xmax,ymax) it returns 1 only if the player is so far in its past the orgin point. Also on side (xmin,ymin to xmin,ymax) where the player enters the bounding box the algorithm returns 2 despite to hitting the polygon. On the top side, (xmin,ymax to xmax,ymax) it returns 1 only if the player is totally in the polygon.
Also i pass two vectors x and y which are from the Building object, and the vector size as int nvert. Could any of this be to do with the heading of the player object? How is the accounted for within the algorithm?
Hi have done as Troubadour has suggested concerning the crossing number algorithm and made several changes, however the if statement never returns true for some reason. I post of the new code is below. Btw thanks again for everyones replies :-)
int cn_PnPoly( Point P, Building & b, int n )
{
int cn = 0; // the crossing number counter
int rem = 0;
vector<float>x;
vector<float>y;
x.swap(b.getX());
y.swap(b.getY());
//// loop through all edges of the polygon
//for (int i=0; i<n; i++) { // edge from V[i] to V[i+1]
// if (((V[i].y <= P.y) && (V[i+1].y > P.y)) // an upward crossing
// || ((V[i].y > P.y) && (V[i+1].y <= P.y))) { // a downward crossing
// // compute the actual edge-ray intersect x-coordinate
// float vt = (float)(P.y - V[i].y) / (V[i+1].y - V[i].y);
// if (P.x < V[i].x + vt * (V[i+1].x - V[i].x)) // P.x < intersect
// ++cn; // a valid crossing of y=P.y right of P.x
// }
//}
//return (cn&1); // 0 if even (out), and 1 if odd (in)
// loop through all edges of the polygon
for (int i=0; i<n; i++) { // edge from V[i] to V[i+1]
int ip1 = (i +1) %n;
if (((y.at(i) <= P.y) && (y.at(ip1) > P.y)) // an upward crossing
|| ((y.at(i) > P.y) && (y.at(ip1) <= P.y))) { // a downward crossing
// compute the actual edge-ray intersect x-coordinate
float vt = (float)(P.y - y.at(i)) / (y.at(ip1) - y.at(i));
if (P.x < x.at(i) + vt * (x.at(ip1) - x.at(i))) // P.x < intersect
++cn; // a valid crossing of y=P.y right of P.x
}
}
rem = (cn&1);
return (rem); // 0 if even (out), and 1 if odd (in)
}
Below I corrected the code, I forgot to add the world co-ords into account. Yet another silly silly error...
int cn_PnPoly( Point P, Building & b, int n )
{
int cn = 0; // the crossing number counter
int rem = 0;
vector<float>x;
vector<float>y;
x.swap(b.getX());
y.swap(b.getY());
// loop through all edges of the polygon
for (int i=0; i<n; i++) { // edge from V[i] to V[i+1]
int ip1 = (i +1) %n;
if ((( (y.at(i)+wy) <= P.y) && ( (y.at(ip1)+wy) > P.y)) // an upward crossing
|| (( (y.at(i)+wy) > P.y) && ( (y.at(ip1)+wy) <= P.y))) { // a downward crossing
// compute the actual edge-ray intersect x-coordinate
float vt = (float)(P.y - (y.at(i)+wy) ) / ( (y.at(ip1)+wy) - (y.at(i)+wy) );
if (P.x < (x.at(i)+wx) + vt * ( (x.at(ip1)+wx) - (x.at(i)+wx) )) // P.x < intersect
++cn; // a valid crossing of y=P.y right of P.x
}
}
rem = (cn&1);
return (rem); // 0 if even (out), and 1 if odd (in)
}
Although this works to detect when a point is in a polygon, it does not take into account the current heading of the player.
If this doesn't make sense, in the 2D game I move the world map around the player by translating all the polygons by the world co-ordinates. These are wx and wy in the game.
Also I rotate the player about a heading varriable.
These are figured out within the draw function, however the collision detection function does not take the heading into account. To do this do I symply multiply the x and y co-ord given by the Building object by the heading? Unfortunately I am not very good at geometry.