I've been working on this for several weeks but have been unable to get my algorithm working properly and i'm at my wits end. Here's an illustration of what i have achieved:
If everything was working i would expect a perfect circle/oval at the end.
My sample points (in white) are recalculated every time a new control point (in yellow) is added. At 4 control points everything looks perfect, again as i add a 5th on top of the 1st things look alright, but then on the 6th it starts to go off too the side and on the 7th it jumps up to the origin!
Below I'll post my code, where calculateWeightForPointI contains the actual algorithm. And for reference- here is the information i'm trying to follow. I'd be so greatful if someone could take a look for me.
void updateCurve(const std::vector<glm::vec3>& controls, std::vector<glm::vec3>& samples)
{
int subCurveOrder = 4; // = k = I want to break my curve into to cubics
// De boor 1st attempt
if(controls.size() >= subCurveOrder)
{
createKnotVector(subCurveOrder, controls.size());
samples.clear();
for(int steps=0; steps<=20; steps++)
{
// use steps to get a 0-1 range value for progression along the curve
// then get that value into the range [k-1, n+1]
// k-1 = subCurveOrder-1
// n+1 = always the number of total control points
float t = ( steps / 20.0f ) * ( controls.size() - (subCurveOrder-1) ) + subCurveOrder-1;
glm::vec3 newPoint(0,0,0);
for(int i=1; i <= controls.size(); i++)
{
float weightForControl = calculateWeightForPointI(i, subCurveOrder, controls.size(), t);
newPoint += weightForControl * controls.at(i-1);
}
samples.push_back(newPoint);
}
}
}
//i = the weight we're looking for, i should go from 1 to n+1, where n+1 is equal to the total number of control points.
//k = curve order = power/degree +1. eg, to break whole curve into cubics use a curve order of 4
//cps = number of total control points
//t = current step/interp value
float calculateWeightForPointI( int i, int k, int cps, float t )
{
//test if we've reached the bottom of the recursive call
if( k == 1 )
{
if( t >= knot(i) && t < knot(i+1) )
return 1;
else
return 0;
}
float numeratorA = ( t - knot(i) );
float denominatorA = ( knot(i + k-1) - knot(i) );
float numeratorB = ( knot(i + k) - t );
float denominatorB = ( knot(i + k) - knot(i + 1) );
float subweightA = 0;
float subweightB = 0;
if( denominatorA != 0 )
subweightA = numeratorA / denominatorA * calculateWeightForPointI(i, k-1, cps, t);
if( denominatorB != 0 )
subweightB = numeratorB / denominatorB * calculateWeightForPointI(i+1, k-1, cps, t);
return subweightA + subweightB;
}
//returns the knot value at the passed in index
//if i = 1 and we want Xi then we have to remember to index with i-1
float knot(int indexForKnot)
{
// When getting the index for the knot function i remember to subtract 1 from i because of the difference caused by us counting from i=1 to n+1 and indexing a vector from 0
return knotVector.at(indexForKnot-1);
}
//calculate the whole knot vector
void createKnotVector(int curveOrderK, int numControlPoints)
{
int knotSize = curveOrderK + numControlPoints;
for(int count = 0; count < knotSize; count++)
{
knotVector.push_back(count);
}
}
Your algorithm seems to work for any inputs I tried it on. Your problem might be a that a control point is not where it is supposed to be, or that they haven't been initialized properly. It looks like there are two control-points, half the height below the bottom left corner.
Related
I know my title isn't very specific but that's because I have no idea where the problem comes from. I'm stuck with this problem since 2 or 3 hours and in theory everything should be working, but it's not.
This piece of code:
for ( int x = -1; x <= 1; x++ ) { //Iterate through the 8 neighbour cells plus the one indicated
for ( int y = -1; y <= 1; y++ ) {
neighbour = coords(locX + x, locY + y, width); //Get the cell index in the array
if (existsInOrtho(ortho, neighbour)) { //If the index exists in the array
if (ortho[neighbour] == 0) { //Cell is dead
cnt--; //Remove one from the number of alive neighbour cells
}
} else { //Cell is not in the zone
cnt--; //Remove one from the number of alive neighbour cells
}
}
}
Iterates through all the neighbour cells to get their value in the array (1 for alive, 0 for dead). The "coords" function, shown here:
int coords(int locX, int locY, int width)
{
int res = -1;
locX = locX - 1; //Remove one from both coordinates, since an index starts at 0 (and the zone starts at (1;1) )
locY = locY - 1;
res = locX * width + locY; //Small calculation to get the index of the pixel in the array
return res;
}
Gets the index of the cell in the array. But when I run the code, it doesn't work, the number of neighbour cells is not correct (it's like a cell is not counted every time there's some alive in the neighborhood). I tried decomposing everything manually, and it works, so I don't know what ruins everything in the final code... Here is the complete code. Sorry if I made any English mistake, it's not my native language.
This code ...
for ( int x = -1; x <= 1; x++ ) { //Iterate through the 8 neighbour cells plus the one indicated
for ( int y = -1; y <= 1; y++ ) {
Actually checks 9 cells. Perhaps you forgot that it checks (x,y) = (0,0). That would include the cell itself as well as its neighbours.
A simple fix is:
for ( int x = -1; x <= 1; x++ ) { //Iterate through the 8 neighbour cells plus the one indicated
for ( int y = -1; y <= 1; y++ ) {
if (x || y) {
Also, the simulate function (from your link) makes the common mistake of updating the value of the cell in the same array before processing state changes required for the cells beside it. The easiest fix is to keep two arrays -- two complete copies of the grid (two ortho arrays, in your code). When reading from orthoA, update orthoB. And then on the next generation, flip. Read from orthoB and write to orthoA.
Background
For a computer vision assignment I've been given the task of implementing RANSAC to fit a plane to a given set of points and filter that input list of points by the consensus model using Eigenvalue Decomposition.
I have spent days trying to tweak my code to achieve correct plane filtering behavior on an input set of test data. All you algorithm junkies, this one's for you.
My implementation uses a vector of a ROS data structure (Point32) as inputs, but this is transparent to the problem at hand.
What I've done
When I test for expected plane filtering behavior (correct elimination of outliers >95-99% of the time), I see in my implementation that I only eliminate outliers and extract the main plane of a test point cloud ~30-40% of the time. Other times, I filter a plane that ~somewhat~ fits the expected model, but leaves a lot of obvious outliers inside the consensus model. The fact that this works at all suggests that I'm doing some things right, and some things wrong.
I've tweaked my constants (distance threshold, max iterations, estimated % points fit) to London and back, and I only see small differences in the consensus model.
Implementation (long)
const float RANSAC_ESTIMATED_FIT_POINTS = .80f; // % points estimated to fit the model
const size_t RANSAC_MAX_ITER = 500; // max RANSAC iterations
const size_t RANDOM_MAX_TRIES = 100; // max RANSAC random point tries per iteration
const float RANSAC_THRESHOLD = 0.0000001f; // threshold to determine what constitutes a close point to a plane
/*
Helper to randomly select an item from a STL container, from stackoverflow.
*/
template <typename I>
I random_element(I begin, I end)
{
const unsigned long n = std::distance(begin, end);
const unsigned long divisor = ((long)RAND_MAX + 1) / n;
unsigned long k;
do { k = std::rand() / divisor; } while (k >= n);
std::advance(begin, k);
return begin;
}
bool run_RANSAC(const std::vector<Point32> all_points,
Vector3f *out_p0, Vector3f *out_n,
std::vector<Point32> *out_inlier_points)
{
for (size_t iterations = 0; iterations < RANSAC_MAX_ITER; iterations ++)
{
Point32 p1,p2,p3;
Vector3f v1;
Vector3f v2;
Vector3f n_hat; // keep track of the current plane model
Vector3f P0;
std::vector<Point32> points_agree; // list of points that agree with model within
bool found = false;
// try RANDOM_MAX_TRIES times to get random 3 points
for (size_t tries = 0; tries < RANDOM_MAX_TRIES; tries ++) // try to get unique random points 100 times
{
// get 3 random points
p1 = *random_element(all_points.begin(), all_points.end());
p2 = *random_element(all_points.begin(), all_points.end());
p3 = *random_element(all_points.begin(), all_points.end());
v1 = Vector3f (p2.x - p1.x,
p2.y - p1.y,
p2.z - p1.z ); //Vector P1P2
v2 = Vector3f (p3.x - p1.x,
p3.y - p1.y,
p3.z - p1.z); //Vector P1P3
if (std::abs(v1.dot(v2)) != 1.f) // dot product != 1 means we've found 3 nonlinear points
{
found = true;
break;
}
} // end try random element loop
if (!found) // could not find 3 random nonlinear points in 100 tries, go to next iteration
{
ROS_ERROR("run_RANSAC(): Could not find 3 random nonlinear points in %ld tries, going on to iteration %ld", RANDOM_MAX_TRIES, iterations + 1);
continue;
}
// nonlinear random points exist past here
// fit a plane to p1, p2, p3
Vector3f n = v1.cross(v2); // calculate normal of plane
n_hat = n / n.norm();
P0 = Vector3f(p1.x, p1.y, p1.z);
// at some point, the original p0, p1, p2 will be iterated over and added to agreed points
// loop over all points, find points that are inliers to plane
for (std::vector<Point32>::const_iterator it = all_points.begin();
it != all_points.end(); it++)
{
Vector3f M (it->x - P0.x(),
it->y - P0.y(),
it->z - P0.z()); // M = (P - P0)
float d = M.dot(n_hat); // calculate distance
if (d <= RANSAC_THRESHOLD)
{ // add to inlier points list
points_agree.push_back(*it);
}
} // end points loop
ROS_DEBUG("run_RANSAC() POINTS AGREED: %li=%f, RANSAC_ESTIMATED_FIT_POINTS: %f", points_agree.size(),
(float) points_agree.size() / all_points.size(), RANSAC_ESTIMATED_FIT_POINTS);
if (((float) points_agree.size()) / all_points.size() > RANSAC_ESTIMATED_FIT_POINTS)
{ // if points agree / total points > estimated % points fitting
// fit to points_agree.size() points
size_t n = points_agree.size();
Vector3f sum(0.0f, 0.0f, 0.0f);
for (std::vector<Point32>::iterator iter = points_agree.begin();
iter != points_agree.end(); iter++)
{
sum += Vector3f(iter->x, iter->y, iter->z);
}
Vector3f centroid = sum / n; // calculate centroid
Eigen::MatrixXf M(points_agree.size(), 3);
for (size_t row = 0; row < points_agree.size(); row++)
{ // build distance vector matrix
Vector3f point(points_agree[row].x,
points_agree[row].y,
points_agree[row].z);
for (size_t col = 0; col < 3; col ++)
{
M(row, col) = point(col) - centroid(col);
}
}
Matrix3f covariance_matrix = M.transpose() * M;
Eigen::EigenSolver<Matrix3f> eigen_solver;
eigen_solver.compute(covariance_matrix);
Vector3f eigen_values = eigen_solver.eigenvalues().real();
Matrix3f eigen_vectors = eigen_solver.eigenvectors().real();
// find eigenvalue that is closest to 0
size_t idx;
// find minimum eigenvalue, get index
float closest_eval = eigen_values.cwiseAbs().minCoeff(&idx);
// find corresponding eigenvector
Vector3f closest_evec = eigen_vectors.col(idx);
std::stringstream logstr;
logstr << "Closest eigenvalue : " << closest_eval << std::endl <<
"Corresponding eigenvector : " << std::endl << closest_evec << std::endl <<
"Centroid : " << std::endl << centroid;
ROS_DEBUG("run_RANSAC(): %s", logstr.str().c_str());
Vector3f all_fitted_n_hat = closest_evec / closest_evec.norm();
// invoke copy constructors for outbound
*out_n = Vector3f(all_fitted_n_hat);
*out_p0 = Vector3f(centroid);
*out_inlier_points = std::vector<Point32>(points_agree);
ROS_DEBUG("run_RANSAC():: Success, total_size: %li, inlier_size: %li, %% agreement %f",
all_points.size(), out_inlier_points->size(), (float) out_inlier_points->size() / all_points.size());
return true;
}
} // end iterations loop
return false;
}
Pseudocode from wikipedia for reference:
Given:
data – a set of observed data points
model – a model that can be fitted to data points
n – minimum number of data points required to fit the model
k – maximum number of iterations allowed in the algorithm
t – threshold value to determine when a data point fits a model
d – number of close data points required to assert that a model fits well to data
Return:
bestfit – model parameters which best fit the data (or nul if no good model is found)
iterations = 0
bestfit = nul
besterr = something really large
while iterations < k {
maybeinliers = n randomly selected values from data
maybemodel = model parameters fitted to maybeinliers
alsoinliers = empty set
for every point in data not in maybeinliers {
if point fits maybemodel with an error smaller than t
add point to alsoinliers
}
if the number of elements in alsoinliers is > d {
% this implies that we may have found a good model
% now test how good it is
bettermodel = model parameters fitted to all points in maybeinliers and alsoinliers
thiserr = a measure of how well model fits these points
if thiserr < besterr {
bestfit = bettermodel
besterr = thiserr
}
}
increment iterations
}
return bestfit
The only difference between my implementation and the wikipedia pseudocode is the following:
thiserr = a measure of how well model fits these points
if thiserr < besterr {
bestfit = bettermodel
besterr = thiserr
}
My guess is that I need to do something related to comparing the (closest_eval) with some sentinel value for the expected minimum eigenvalue corresponding to a normal for planes that tend to fit the model. However this was not covered in class and I have no idea where to start figuring out what's wrong.
Heh, it's funny how thinking about how to present the problem to others can actually solve the problem I'm having.
Solved by simply implementing this with a std::numeric_limits::max() starting best fit eigenvalue. This is because the best fit plane extracted on any n-th iteration of RANSAC is not guaranteed to be THE best fit plane and may have a huge error in consensus amongst each constituent point, so I need to converge on that for each iteration. Woops.
thiserr = a measure of how well model fits these points
if thiserr < besterr {
bestfit = bettermodel
besterr = thiserr
}
I am trying to get the left polygonal chain given a set of consecutive points. (NOTE: edges are non-intersecting.)
Image 1. Sample polygon and its bound.
What I did was:
Get the minY, maxY and minX. (Bound.)
Find the point that contains minY (or maxY) then save it as the first point.
Save any points until point with minY or maxY is found while checking for point with minX.
If the same Y is found first, save it as the new first point and repeat from #3.
If other Y is found first and the saved points has minX, this is the chain. Otherwise, save as the new first point and repeat from #3.
Image 2. The left chain of points.
But using this steps might give wrong result for some polygon, like this:
Since one point is (minX, maxY), either of the side will be returned.
EDIT:
With the idea of the left-bottom- and left-top-most points, here is the current code that I am using:
Get the min (left-bottom-most) and max (left-top-most) point.
std::vector<Coord> ret;
size_t i = 0;
Coord minCoord = poly[i];
Coord maxCoord = poly[i];
size_t minIdx = -1;
size_t maxIdx = -1;
size_t cnt = poly.size();
i++;
for (; i < cnt; i++)
{
Coord c = poly[i];
if (c.y < minCoord.y) // new bottom
{
minCoord = c;
minIdx = i;
}
else if (c.y == minCoord.y) // same bottom
{
if (c.x < minCoord.x) // left most
{
minCoord = c;
minIdx = i;
}
}
if (c.y > maxCoord.y) // new top
{
maxCoord = c;
maxIdx = i;
}
else if (c.y == maxCoord.y) // same top
{
if (c.x < maxCoord.x) // left most
{
maxCoord = c;
maxIdx = i;
}
}
}
Get the points connected to the max point.
i = maxIdx;
Coord mid = poly[i];
Coord ray1 = poly[(i + cnt - 1) % cnt];
Coord ray2 = poly[(i + 1) % cnt];
Get which has smallest angle. This will be the path we will follow.
double rad1 = Pts2Rad(mid, ray1);
double rad2 = Pts2Rad(mid, ray2);
int step = 1;
if (rad1 < rad2)
step = cnt - 1;
Save the points.
while (i != minIdx)
{
ret.push_back(poly[i]);
i = (i + step) % cnt;
}
ret.push_back(poly[minIdx]);
To be specific, I am assuming that no vertex is duplicated and define the "left chain" as the sequence of vertices from the original polygon loop that goes from the leftmost vertex in the top side of the bounding box, to the leftmost vertex in the bottom side of the bounding box. [In case the top and bottom sides coincide, these two vertices also coincide; I leave it to you what to return in this case.]
To obtain these, you can scan all vertices and keep the left-topmost so far and left-bottommost so far. Then compare to the next vertex. If above the left-topmost, becomes the new lef-topmost. If at the same level and to the left, becomes the new left-topmost. Similarly for the left-bottommost.
The problem to solve is finding the floating status of a floating body, given its weight and the center of gravity.
The function i use calculates the displaced volume and center of bouyance of the body given sinkage, heel and trim.
Where sinkage is a length unit and heel/trim is an angle limited to a value from -90 to 90.
The floating status is found when displaced volum is equal to weight and the center of gravity is in a vertical line with center of bouancy.
I have this implemeted as a non-linear Newton-Raphson root finding problem with 3 variables (sinkage, trim, heel) and 3 equations.
This method works, but needs good initial guesses. So I am hoping to find either a better approach for this, or a good method to find the initial values.
Below is the code for the newton and jacobian algorithm used for the Newton-Raphson iteration. The function volume takes the parameters sinkage, heel and trim. And returns volume, and the coordinates for center of bouyancy.
I also included the maxabs and GSolve2 algorithms, I belive these are taken from Numerical Recipies.
void jacobian(float x[], float weight, float vcg, float tcg, float lcg, float jac[][3], float f0[]) {
float h = 0.0001f;
float temp;
float j_volume, j_vcb, j_lcb, j_tcb;
float f1[3];
volume(x[0], x[1], x[2], j_volume, j_lcb, j_vcb, j_tcb);
f0[0] = j_volume-weight;
f0[1] = j_tcb-tcg;
f0[2] = j_lcb-lcg;
for (int i=0;i<3;i++) {
temp = x[i];
x[i] = temp + h;
volume(x[0], x[1], x[2], j_volume, j_lcb, j_vcb, j_tcb);
f1[0] = j_volume-weight;
f1[1] = j_tcb-tcg;
f1[2] = j_lcb-lcg;
x[i] = temp;
jac[0][i] = (f1[0]-f0[0])/h;
jac[1][i] = (f1[1]-f0[1])/h;
jac[2][i] = (f1[2]-f0[2])/h;
}
}
void newton(float weight, float vcg, float tcg, float lcg, float &sinkage, float &heel, float &trim) {
float x[3] = {10,1,1};
float accuracy = 0.000001f;
int ntryes = 30;
int i = 0;
float jac[3][3];
float max;
float f0[3];
float gauss_f0[3];
while (i < ntryes) {
jacobian(x, weight, vcg, tcg, lcg, jac, f0);
if (sqrt((f0[0]*f0[0]+f0[1]*f0[1]+f0[2]*f0[2])/2) < accuracy) {
break;
}
gauss_f0[0] = -f0[0];
gauss_f0[1] = -f0[1];
gauss_f0[2] = -f0[2];
GSolve2(jac, 3, gauss_f0);
x[0] = x[0]+gauss_f0[0];
x[1] = x[1]+gauss_f0[1];
x[2] = x[2]+gauss_f0[2];
// absmax(x) - Return absolute max value from an array
max = absmax(x);
if (max < 1) max = 1;
if (sqrt((gauss_f0[0]*gauss_f0[0]+gauss_f0[1]*gauss_f0[1]+gauss_f0[2]*gauss_f0[2])) < accuracy*max) {
x[0]=x2[0];
x[1]=x2[1];
x[2]=x2[2];
break;
}
i++;
}
sinkage = x[0];
heel = x[1];
trim = x[2];
}
int GSolve2(float a[][3],int n,float b[]) {
float x,sum,max,temp;
int i,j,k,p,m,pos;
int nn = n-1;
for (k=0;k<=n-1;k++)
{
/* pivot*/
max=fabs(a[k][k]);
pos=k;
for (p=k;p<n;p++){
if (max < fabs(a[p][k])){
max=fabs(a[p][k]);
pos=p;
}
}
if (ABS(a[k][pos]) < EPS) {
writeLog("Matrix is singular");
break;
}
if (pos != k) {
for(m=k;m<n;m++){
temp=a[pos][m];
a[pos][m]=a[k][m];
a[k][m]=temp;
}
}
/* convert to upper triangular form */
if ( fabs(a[k][k])>=1.e-6)
{
for (i=k+1;i<n;i++)
{
x = a[i][k]/a[k][k];
for (j=k+1;j<n;j++) a[i][j] = a[i][j] -a[k][j]*x;
b[i] = b[i] - b[k]*x;
}
}
else
{
writeLog("zero pivot found in line:%d",k);
return 0;
}
}
/* back substitution */
b[nn] = b[nn] / a[nn][nn];
for (i=n-2;i>=0;i--)
{
sum = b[i];
for (j=i+1;j<n;j++)
sum = sum - a[i][j]*b[j];
b[i] = sum/a[i][i];
}
return 0;
}
float absmax(float x[]) {
int i = 1;
int n = sizeof(x);
float max = x[0];
while (i < n) {
if (max < x[i]) {
max = x[i];
}
i++;
}
return max;
}
Have you considered some stochastic search methods to find the initial value and then fine-tuning with Newton Raphson? One possibility is evolutionary computation, you can use the Inspyred package. For a physical problem similar in many ways to the one you describe, look at this example: http://inspyred.github.com/tutorial.html#lunar-explorer
What about using a damped version of Newton's method? You could quite easily modify your implementation to make it. Think about Newton's method as finding a direction
d_k = f(x_k) / f'(x_k)
and updating the variable
x_k+1 = x_k - L_k d_k
In the usual Newton's method, L_k is always 1, but this might create overshoots or undershoots. So, let your method chose L_k. Suppose that your method usually overshoots. A possible strategy consists in taking the largest L_k in the set {1,1/2,1/4,1/8,... L_min} such that the condition
|f(x_k+1)| <= (1-L_k/2) |f(x_k)|
is satisfied (or L_min if none of the values satisfies this criteria).
With the same criteria, another possible strategy is to start with L_0=1 and if the criteria is not met, try with L_0/2 until it works (or until L_0 = L_min). Then for L_1, start with min(1, 2L_0) and do the same. Then start with L_2=min(1, 2L_1) and so on.
By the way: are you sure that your problem has a unique solution? I guess that the answer to this question depends on the shape of your object. If you have a rugby ball, there's one angle that you cannot fix. So if your shape is close to such an object, I would not be surprised that the problem is difficult to solve for that angle.
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.