bool Map::wrongFinishLineIntersection(int xFirst, int yFirst, int xSecond, int ySecond) const { if( intersectFinishLine(xFirst, yFirst, xSecond, ySecond) ) { int normalVectorCoordinatesX = rightFinishPoint.first - leftFinishPoint.first; int normalVectorCoordinatesY = rightFinishPoint.second - leftFinishPoint.second;/* if( area( map.GetFinishLine().first, playersCoordinates, map.GetFinishLine().second ) * area( map.GetFinishLine().first, normalVectorCoordinates, map.GetFinishLine().second ) >= 0 && area( map.GetFinishLine().first, playersPreviousCoordinates, map.GetFinishLine().second ) * area( map.GetFinishLine().first, normalVectorCoordinates, map.GetFinishLine().second ) < 0 )*/ return !( orientedArea( leftFinishPoint.first, leftFinishPoint.second, xSecond, ySecond, rightFinishPoint.first, rightFinishPoint.second ) * orientedArea( leftFinishPoint.first, leftFinishPoint.second, normalVectorCoordinatesX, normalVectorCoordinatesY, rightFinishPoint.first, rightFinishPoint.second ) >= 0 && orientedArea( leftFinishPoint.first, leftFinishPoint.second, xFirst, yFirst, rightFinishPoint.first, rightFinishPoint.second ) * orientedArea( leftFinishPoint.first, leftFinishPoint.second, normalVectorCoordinatesX, normalVectorCoordinatesY, rightFinishPoint.first, rightFinishPoint.second ) < 0); } return false; //if( intersectFinishLine( xFirst, yFirst, xSecond, ySecond ) ) { // return orientedArea( xSecond, ySecond, leftFinishPoint.first, leftFinishPoint.second, // rightFinishPoint.first, rightFinishPoint.second ) >= 0; //} //return false; }
bool Map::intersectFinishLine(int xFirst, int yFirst, int xSecond, int ySecond) const { return (intersect(xFirst, xSecond, leftFinishPoint.first, rightFinishPoint.first) && intersect(yFirst, ySecond, leftFinishPoint.second, rightFinishPoint.second) && (orientedArea(xFirst, yFirst, xSecond, ySecond, leftFinishPoint.first, leftFinishPoint.second) * orientedArea(xFirst, yFirst, xSecond, ySecond, rightFinishPoint.first, rightFinishPoint.second) <= 0) && (orientedArea(leftFinishPoint.first, leftFinishPoint.second, rightFinishPoint.first, rightFinishPoint.second, xFirst, yFirst) * orientedArea(leftFinishPoint.first, leftFinishPoint.second, rightFinishPoint.first, rightFinishPoint.second, xSecond, ySecond) <= 0)); }
bool Map::isIntersects( const std::pair<int, int>& firstPoint, const std::pair<int, int>& secondPoint, const std::pair<int, int>& thirdPoint, const std::pair<int, int>& fourthPoint ) const { return intersect( firstPoint.first, secondPoint.first, thirdPoint.first, fourthPoint.first ) && intersect( firstPoint.second, secondPoint.second, thirdPoint.second, fourthPoint.second ) && ( orientedArea( firstPoint.first, firstPoint.second, secondPoint.first, secondPoint.second, thirdPoint.first, thirdPoint.second ) * orientedArea( firstPoint.first, firstPoint.second, secondPoint.first, secondPoint.second, fourthPoint.first, fourthPoint.second )) <= 0 && ( orientedArea( thirdPoint.first, thirdPoint.second, fourthPoint.first, fourthPoint.second, firstPoint.first, firstPoint.second ) * orientedArea( thirdPoint.first, thirdPoint.second, fourthPoint.first, fourthPoint.second, secondPoint.first, secondPoint.second ) ) <= 0; }
bool Map::wrongFinishLineIntersection(int xFirst, int yFirst, int xSecond, int ySecond) const { if (intersectFinishLine(xFirst, yFirst, xSecond, ySecond)) { return (orientedArea(xFirst, yFirst, leftFinishPoint.first, leftFinishPoint.second, rightFinishPoint.first, rightFinishPoint.second) > 0); } return false; }
/* * The main routine. */ void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { const float * vertices = (float*) mxGetPr(prhs[0]); const int * faces = (int*) mxGetPr(prhs[1]); const int * vertexFaces = (int*) mxGetPr(prhs[2]); const int * vertexNbors = (int*) mxGetPr(prhs[3]); const int * vertexList_in = (int*) mxGetPr(prhs[4]); const float area_0 = (float) mxGetScalar(prhs[5]); const int numOfVertFaces = mxGetM(prhs[2]); const int numOfVertices = mxGetN(prhs[0]); const int numOfFaces = mxGetN(prhs[1]); const int numOfNbors = mxGetM(prhs[3]); const int size_list_in = mxGetM(prhs[4]); int f, scalar_dim, v, cur_face, size_list_out, N_in, vert, nv, cur_nbor, nbor_cnt; int * vertexList_out; const float * v0, * v1, * v2; int v0_ind, v1_ind, v2_ind, tmp_ind; float tmp_vec[3], v0v1[3], v0v2[3], tmp_grad[3], centroid[3]; float * faceArea, * energy, *gradient; int out_grad_dims[2]; bool * isVertexListed; int* flippedVertexList; out_grad_dims[0] = 3; out_grad_dims[1] = numOfVertices; scalar_dim = 1; size_list_out = 0; if (mxGetM(prhs[0]) != 3) mexErrMsgTxt("Vertices should be 3 dimensional"); if (mxGetM(prhs[1]) != 3) mexErrMsgTxt("Faces should have three vertices"); if (mxGetN(prhs[2]) != mxGetN(prhs[0])) mexErrMsgTxt("Vertices and VertexFaces should be the same number!"); if (!(faceArea = (float*) calloc(numOfFaces, sizeof(float)))) mexErrMsgTxt("Memory allocation error!"); if (!(isVertexListed = (bool*) calloc(numOfVertices, sizeof(bool)))) mexErrMsgTxt("Memory allocation error!"); if (!(flippedVertexList = (int*) calloc(numOfVertices, sizeof(int)))) mexErrMsgTxt("Memory allocation error!"); if(nrhs != 6) mexErrMsgTxt("Wrong number of inputs!"); plhs[0] = mxCreateNumericArray(1,&scalar_dim,mxSINGLE_CLASS, mxREAL); energy = (float *) mxGetData(plhs[0]); plhs[1] = mxCreateNumericArray(2, out_grad_dims, mxSINGLE_CLASS, mxREAL); gradient = (float *) mxGetData(plhs[1]); /* * First pass thru faces : determine folded faces * */ if (size_list_in == 0) N_in = numOfVertices; else N_in = size_list_in; for (vert = 0; vert < N_in; vert ++) { if (size_list_in == 0) v = vert; else /* Convert to C-style index*/ v = vertexList_in[vert] - 1; for (f = 0; f < numOfVertFaces; f++) { /* *Convert to C-style index! */ cur_face = vertexFaces[numOfVertFaces * v + f] - 1; if (cur_face < 0) f = numOfVertFaces; else { if (faceArea[cur_face] == 0.0) { /* *Notice the following are MATLAB-style indices */ v0_ind = faces[cur_face * 3]; v1_ind = faces[cur_face * 3 + 1]; v2_ind = faces[cur_face * 3 + 2]; v0 = &vertices[3*(v0_ind - 1)]; v1 = &vertices[3*(v1_ind - 1)]; v2 = &vertices[3*(v2_ind - 1)]; faceArea[cur_face] = orientedArea(v0, v1,v2, v0); if (isFlipped(faceArea[cur_face])) { energy[0] += faceArea[cur_face] + area_0; if (!isVertexListed[v0_ind - 1]) { flippedVertexList[size_list_out] = v0_ind; isVertexListed[v0_ind - 1] = true; size_list_out++; } if (!isVertexListed[v1_ind - 1]) { flippedVertexList[size_list_out] = v1_ind; isVertexListed[v1_ind - 1] = true; size_list_out++; } if (!isVertexListed[v2_ind - 1]) { flippedVertexList[size_list_out] = v2_ind; isVertexListed[v2_ind - 1] = true; size_list_out++; } } } } } } plhs[2] = mxCreateNumericArray(1,&size_list_out,mxINT32_CLASS, mxREAL); vertexList_out = (int*) mxGetData(plhs[2]); for (vert = 0; vert < size_list_out; vert++) { if (flippedVertexList[vert] <= 0) { break; } else { /* *Convert to C-style index!!! */ v = flippedVertexList[vert] - 1; /* *return Matlab-style index!! */ vertexList_out[vert] = flippedVertexList[vert]; nbor_cnt = 0; centroid[0] = 0.0; centroid[1] = 0.0; centroid[2] = 0.0; for (nv = 0; nv < numOfNbors; nv++) { /* *Convert to C-style index! */ cur_nbor = vertexNbors[numOfNbors * v + nv] - 1; if (cur_nbor < 0) nv = numOfNbors; else { centroid[0] += vertices[3*cur_nbor]; centroid[1] += vertices[3*cur_nbor + 1]; centroid[2] += vertices[3*cur_nbor + 2]; nbor_cnt++; } } gradient[3 * v] = -centroid[0]/nbor_cnt + vertices[3*v]; gradient[3 * v + 1] = -centroid[1]/nbor_cnt + vertices[3*v + 1]; gradient[3 * v + 2] = -centroid[2]/nbor_cnt + vertices[3*v + 2]; } } free(faceArea); free(flippedVertexList); free(isVertexListed); return; }
bool Map::IsOnRightSideOfFinishLine( int x, int y ) { return orientedArea( leftFinishPoint.first, leftFinishPoint.second, rightFinishPoint.first, rightFinishPoint.second, x, y ) > 0; }