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