geoData mapload(char* path,VisiLibity::Environment & mapEnv,VisiLibity::Visibility_Graph & visGraph,float clearDist )
{
std::ifstream in(path);
std::string s;
std::string s1="";
while(getline(in, s)) { // Discards newline char
s1=s1+s+"\n";
}
std::vector<char> xml_copy(s1.begin(), s1.end());
xml_copy.push_back('\0');
rapidxml::xml_document<> doc; // character type defaults to char
doc.parse<0>(&xml_copy[0]); // 0 means default parse flags
//std::cout << "Name of my first node is: " << doc.first_node()->name() << "\n";
rapidxml::xml_node<char> *n1;
n1=doc.first_node("svg");
n1=n1->first_node("g")->first_node("path");
//std::vector < VisiLibity::Point > poly_temp;
//geoData vertices_temp(10,poly_temp);
geoData vertices_temp;
int i=0;
while (n1)
{
vertices_temp.push_back(loadPath(n1->first_attribute("d")->value()));
n1=n1->next_sibling("path");
i++;
}
Polygon_2 mapEnvOB;
for (int j=0;j<vertices_temp[0].size();j++)
{
Point_2 nextVert= Point_2(vertices_temp[0][j][0],vertices_temp[0][j][1]);
mapEnvOB.push_back(nextVert);
};
Polygon_set_2 S;
mapEnvOB.reverse_orientation();
S.insert(mapEnvOB);
for (int k=1;k<vertices_temp.size();k++)
{
Polygon_2 holeCGAL;
for (int j=0;j<vertices_temp[k].size();j++)
{
holeCGAL.push_back(Point_2(vertices_temp[k][j][0],vertices_temp[k][j][1]));
};
//holeCGAL.reverse_orientation();
S.difference(holeCGAL);
holeCGAL.clear();
};
std::list<Polygon_with_holes_2> res;
std::list<Polygon_with_holes_2>::const_iterator it;
S.polygons_with_holes (std::back_inserter (res));
std::vector<VisiLibity::Polygon> envPolys;
for (it = res.begin(); it != res.end(); ++it)
{
if(CGAL::ON_BOUNDED_SIDE==CGAL::bounded_side_2(it->outer_boundary().vertices_begin(),it->outer_boundary().vertices_end(),Point_2(guest1.pos.x(),guest1.pos.y()),Kernel()))
{
envPolys.push_back(ConvertPolygonCGAL2Vis(it->outer_boundary()));
Polygon_with_holes_2::Hole_const_iterator hi;
for (hi=it->holes_begin();hi!=it->holes_end();++hi)
{
envPolys.push_back(ConvertPolygonCGAL2Vis(*hi));
};
break;
};
}
for (int i=0;i<envPolys.size();i++){
//envPolys.push_back(VisiLibity::Polygon(vertices_temp[i]));
//i=0;
envPolys[i].eliminate_redundant_vertices(0.0001);
VisiLibity::Point cm=envPolys[i].centroid();
for (int j=0;j<envPolys[i].n();j++)
{
if (j<envPolys[i].n()-1){
VisiLibity::Point n1=clearDist*normal(envPolys[i][j+1]-envPolys[i][j]);
envPolys[i][j]=envPolys[i][j]+n1;
envPolys[i][j+1]=envPolys[i][j+1]+n1;
}
}
VisiLibity::Point norm1=clearDist*normal(envPolys[i][0]-envPolys[i][envPolys[i].n()-1]);
envPolys[i][0]=envPolys[i][0]+norm1;
envPolys[i][envPolys[i].n()-1]=envPolys[i][envPolys[i].n()-1]+norm1;
};
mapEnv = *(new VisiLibity::Environment(envPolys));
mapEnv.enforce_standard_form();
visGraph = *(new VisiLibity::Visibility_Graph(mapEnv,0.00000001));
return vertices_temp;
};
void CKWResearchWorkDoc::OnHelpTest()
{
// TODO: Add your command handler code here
vector<Point_3> SamplePoints;
GeometryAlgorithm::SampleCircle(Point_3(0,0,0),0.3,20,SamplePoints);
FILE* pfile=fopen("circle0.contour","w");
for (unsigned int i=0;i<SamplePoints.size();i++)
{
fprintf(pfile,"%.3f %.3f %.3f\n",SamplePoints.at(i).x(),SamplePoints.at(i).y(),SamplePoints.at(i).z());
}
fclose(pfile);
Polygon_2 PolyTest;
PolyTest.push_back(Point_2(0,0));
PolyTest.push_back(Point_2(1,1));
PolyTest.push_back(Point_2(2,0));
PolyTest.push_back(Point_2(2,2));
PolyTest.push_back(Point_2(0,2));
Point_2 ResultPoint;
bool bResult=GeometryAlgorithm::GetArbiPointInPolygon(PolyTest,ResultPoint);
DBWindowWrite("result point: %f %f\n",ResultPoint.x(),ResultPoint.y());
SparseMatrix LHMatrix(2);
LHMatrix.m = 3;
LHMatrix[0][0] = 2;
LHMatrix[0][1] = -1;
LHMatrix[0][2] = 1;
LHMatrix[1][1] = 1;
LHMatrix[1][2] = 1;
SparseMatrix LHMatrixAT(LHMatrix.NCols());
CMath MathCompute;
MathCompute.TAUCSFactorize(LHMatrix,LHMatrixAT);
vector<vector<double>> RightMatB,ResultMat;
vector<double> BRow;
BRow.push_back(5);BRow.push_back(3);
RightMatB.push_back(BRow);
BRow.clear();
BRow.push_back(10);BRow.push_back(1);
RightMatB.push_back(BRow);
BRow.clear();
MathCompute.TAUCSComputeLSE(LHMatrixAT,RightMatB,ResultMat);
return;
int iVer=this->Mesh.size_of_vertices();
int iEdge=this->Mesh.size_of_halfedges();
int iFacet=this->Mesh.size_of_facets();
Polygon_2 BoundingPolygon0;
Polygon_2 BoundingPolygon1;
bool bSimple0=BoundingPolygon0.is_simple();
bool bSimple1=BoundingPolygon1.is_simple();
bool bConvex0=BoundingPolygon0.is_convex();
bool bConvex1=BoundingPolygon1.is_convex();
bool bOrien0=BoundingPolygon0.is_clockwise_oriented();
bool bOrien1=BoundingPolygon1.is_clockwise_oriented();
BoundingPolygon0.reverse_orientation();
BoundingPolygon1.reverse_orientation();
//float?
bool bIntersect=CGAL::do_intersect(BoundingPolygon0,BoundingPolygon1);
bool bCW=BoundingPolygon0.is_clockwise_oriented();
bool bConvex=BoundingPolygon0.is_convex();
Plane_3 plane(1,1,1,0);
vector<vector<Point_3>> IntersectCurves;
int iNum=GeometryAlgorithm::GetMeshPlaneIntersection(this->Mesh,plane,IntersectCurves);
CMath CMathTest;
CMathTest.testTAUCS();
// Vector_3 vec0(Point_3(0,0,1),Point_3(0,0,0));
Vector_3 vec0(Point_3(0,0,0),Point_3(0,0,1));
// Vector_3 vec0(0,0,1);
// Vector_3 vec1(0,-1,-1);
Vector_3 vec1(Point_3(0,0,0),Point_3(0,-1,-1));
double dAngle=GeometryAlgorithm::GetAngleBetweenTwoVectors3d(vec0,vec1);
vector<double> number;
number.push_back(2);
number.push_back(4);
number.push_back(4);
number.push_back(4);
number.push_back(5);
number.push_back(5);
number.push_back(7);
number.push_back(9);
double dderi=GeometryAlgorithm::GetDerivation(number);
Point_3 center(0,0,0);
Sphere_3 sphere(center,1);
Line_3 line(Point_3(0,2,1),Point_3(0,2,-1));
vector<Point_3> points;
GeometryAlgorithm::GetLineSphereIntersection(line,sphere,points);
vector<Point_3> Group0,Group1;
vector<Int_Int_Pair> GroupResult;
Group0.push_back(Point_3(3,3,2));
Group0.push_back(Point_3(5,3,2));
Group0.push_back(Point_3(2,3,2));
Group0.push_back(Point_3(6,3,2));
Group0.push_back(Point_3(4,3,2));
Group0.push_back(Point_3(1,3,2));
Group1.push_back(Point_3(3,4,2));
Group1.push_back(Point_3(1,4,2));
Group1.push_back(Point_3(6,4,2));
Group1.push_back(Point_3(2,4,2));
Group1.push_back(Point_3(5,4,2));
Group1.push_back(Point_3(4,4,2));
vector<Point_3> vecMidPoint;
GeometryAlgorithm::GroupNearestPoints(Group0,Group1,GroupResult,vecMidPoint);//
vector<Point_3> OriginalCurve;
OriginalCurve.push_back(Point_3(-1,0,2));
OriginalCurve.push_back(Point_3(1,0,2));
OriginalCurve.push_back(Point_3(1,0,0));
OriginalCurve.push_back(Point_3(-1,0,0));
vector<Point_3> NewCurve=OriginalCurve;
vector<int> HandleInd;
HandleInd.push_back(0);
HandleInd.push_back(1);
vector<Point_3> NewPos;
NewPos.push_back(Point_3(-1,0,3));
NewPos.push_back(Point_3(1,0,3));
// CCurveDeform::ClosedCurveNaiveLaplacianDeform(NewCurve,HandleInd,NewPos,1);
vector<vector<float>> MatrixA,MatrixB,Result;
vector<float> CurrentRow;
CurrentRow.push_back(2);CurrentRow.push_back(0);CurrentRow.push_back(1);
MatrixA.push_back(CurrentRow);
CurrentRow.clear();
CurrentRow.push_back(3);CurrentRow.push_back(1);CurrentRow.push_back(2);
MatrixA.push_back(CurrentRow);
CurrentRow.clear();
CurrentRow.push_back(0);CurrentRow.push_back(1);CurrentRow.push_back(0);
MatrixA.push_back(CurrentRow);
CurrentRow.clear();
CurrentRow.push_back(1);CurrentRow.push_back(3);CurrentRow.push_back(0);
MatrixB.push_back(CurrentRow);
CurrentRow.clear();
CurrentRow.push_back(2);CurrentRow.push_back(0);CurrentRow.push_back(2);
MatrixB.push_back(CurrentRow);
CurrentRow.clear();
CurrentRow.push_back(1);CurrentRow.push_back(0);CurrentRow.push_back(1);
MatrixB.push_back(CurrentRow);
CurrentRow.clear();
int iANonZeroSize=0;
for (unsigned int i=0;i<MatrixA.size();i++)
{
for (unsigned int j=0;j<MatrixA.front().size();j++)
{
if (MatrixA.at(i).at(j)!=0)
{
iANonZeroSize++;
}
}
}
KW_SparseMatrix A(MatrixA.size(),MatrixA.front().size(),iANonZeroSize);
for (unsigned int i=0;i<MatrixA.size();i++)
{
for (unsigned int j=0;j<MatrixA.front().size();j++)
{
if (MatrixA.at(i).at(j)!=0)
{
A.fput(i,j,MatrixA.at(i).at(j));
}
}
}
int iBNonZeroSize=0;
for (unsigned int i=0;i<MatrixB.size();i++)
{
for (unsigned int j=0;j<MatrixB.front().size();j++)
{
if (MatrixB.at(i).at(j)!=0)
{
iBNonZeroSize++;
}
}
}
KW_SparseMatrix B(MatrixB.size(),MatrixB.front().size(),iBNonZeroSize);
for (unsigned int i=0;i<MatrixB.size();i++)
{
for (unsigned int j=0;j<MatrixB.front().size();j++)
{
if (MatrixB.at(i).at(j)!=0)
{
B.fput(i,j,MatrixB.at(i).at(j));
}
}
}
KW_SparseMatrix C(A.m,B.n,0);
KW_SparseMatrix::KW_multiply(A,B,C);
int iIndex=0;
Result.clear();
for (int i=0;i<C.m;i++)
{
vector<float> CurrentRow;
for (int j=0;j<C.n;j++)
{
iIndex=0;
while (iIndex<C.vol)
{
if (C.indx[iIndex]==i&&C.jndx[iIndex]==j)
{
break;
}
iIndex++;
}
if (iIndex!=C.vol)
{
CurrentRow.push_back(C.array[iIndex]);
}
else
{
CurrentRow.push_back(0);
}
}
Result.push_back(CurrentRow);
}
}
void triangulate(const Polygon& polygon, Triangle_iter triangles,
const boost::unordered_map<Point_3, boost::unordered_set<Segment_3_undirected> >& point2edges)
{
typedef CGAL::Triangulation_vertex_base_2<Kernel> Vb;
typedef CGAL::Triangulation_vertex_base_with_info_2<bool, Kernel, Vb> Info;
typedef CGAL::Constrained_triangulation_face_base_2<Kernel> Fb;
typedef CGAL::Triangulation_data_structure_2<Info,Fb> TDS;
typedef CGAL::Exact_predicates_tag Itag;
typedef CGAL::Constrained_Delaunay_triangulation_2<Kernel, TDS, Itag> CDT;
static log4cplus::Logger tlogger = log4cplus::Logger::getInstance("polygon_utils");
LOG4CPLUS_TRACE(tlogger, "Triangulating " << pp(polygon));
if (polygon.size() < 3) return;
Polygon p = polygon;
bool vertical = is_vertical(p);
if (vertical)
{
LOG4CPLUS_TRACE(tlogger, "Polygon is vertical. Rotating.");
p = yz_swap_neg(p);
}
bool reverse = !p.is_counterclockwise_oriented();
// THIS IS BAD, BAD, BAD!
{
typename Polygon::Vertex_circulator start = p.vertices_circulator();
typename Polygon::Vertex_circulator c = start;
typename Polygon::Vertex_circulator n = c;
typename Polygon::Vertex_circulator prev = c;
++n;
--prev;
Polygon_2 newp;
do
{
if (!CGAL::collinear(*prev, *c, *n))
newp.push_back(*c);
++prev;
++c;
++n;
} while (c != start);
p = newp;
}
CDT cdt;
typename Polygon::Vertex_circulator start = p.vertices_circulator();
typename Polygon::Vertex_circulator c = start;
typename Polygon::Vertex_circulator n = c;
do
{
cdt.insert_constraint(*c, *n);
++c;
++n;
} while (c != start);
// Loop through the triangulation and store the vertices of each triangle
for (CDT::Finite_faces_iterator ffi = cdt.finite_faces_begin();
ffi != cdt.finite_faces_end();
++ffi)
{
Triangle t;
Point_3 center = centroid(ffi->vertex(0)->point(), ffi->vertex(1)->point(), ffi->vertex(2)->point());
if (p.has_on_bounded_side(center) &&
is_legal(ffi->vertex(0)->point(), ffi->vertex(1)->point(), ffi->vertex(2)->point(), point2edges))
{
for (int i = 0; i < 3; ++i)
{
int idx = reverse ? 2-i : i;
if (!vertical)
t[idx] = ffi->vertex(i)->point();
else
t[idx] = yz_swap_pos(ffi->vertex(i)->point());
}
LOG4CPLUS_TRACE(tlogger, "Adding tile: " << pp_tri(t));
*triangles = t;
++triangles;
}
}
}
int alpha_shape(vertex_t *vertices, size_t count, double alpha,
vertex_t **res, size_t *res_count, char **err_msg) {
try {
std::list<Point> points;
{
std::vector<Point> pv;
for (std::size_t j = 0; j < count; ++j) {
Point p(vertices[j].x, vertices[j].y);
pv.push_back(p);
}
std::sort(pv.begin(), pv.end(),
[](const Point &e1, const Point &e2)->bool {
return e2.y() < e1.y();
});
std::stable_sort(pv.begin(), pv.end(),
[](const Point &e1, const Point &e2)->bool {
return e2.x() < e1.x();
});
pv.erase(std::unique(pv.begin(), pv.end()), pv.end());
if (pv.size() != count && pv.size() < 3) {
*err_msg = strdup("After eliminating duplicated points, less than 3 points remain!!. Alpha shape calculation needs at least 3 vertices.");
return -1;
}
points.insert(points.begin(), pv.begin(), pv.end());
}
Alpha_shape_2 A(points.begin(), points.end(),
coord_type(10000),
Alpha_shape_2::REGULARIZED);
std::vector<Segment> segments;
// std::vector<Segment> result;
// Alpha_shape_2::Alpha_shape_vertices_iterator vit;
// Alpha_shape_2::Vertex_handle vertex;
// Alpha_shape_2::Alpha_shape_edges_iterator eit;
// Alpha_shape_2::Edge edge;
// Alpha_shape_2::Face_iterator fit;
// Alpha_shape_2::Face_handle face;
if (alpha <= 0.0) {
alpha = *A.find_optimal_alpha(1);
}
A.set_alpha(alpha);
alpha_edges(A, std::back_inserter(segments));
// Segment s = segments.at(0);
// find_next_edge(s, segments, result);
if (segments.empty()) {
*res = NULL;
*res_count = 0;
} else {
std::set<int> unusedIndexes;
for (unsigned int i = 0; i < segments.size(); i++) {
unusedIndexes.insert(i);
}
std::vector<Polygon_2> rings;
Polygon_2 ring;
ring.push_back(segments.at(0).source());
rings.push_back(ring);
unusedIndexes.erase(0);
find_next_edge(segments.at(0), segments, unusedIndexes, rings);
size_t result_count = 0;
for (unsigned int i = 0; i < rings.size(); i++) {
Polygon_2 ring = rings.at(i);
result_count += ring.size();
}
result_count += rings.size() - 1;
*res = (vertex_t *) malloc(sizeof(vertex_t) * result_count);
*res_count = result_count;
int idx = 0;
for (unsigned int i = 0; i < rings.size(); i++) {
if (i > 0) {
(*res)[idx].x = DBL_MAX;
(*res)[idx].y = DBL_MAX;
idx++;
}
Polygon_2 ring = rings.at(i);
for (unsigned int j = 0; j < ring.size(); j++) {
Point point = ring.vertex(j);
(*res)[idx].x = point.x();
(*res)[idx].y = point.y();
idx++;
}
}
}
*err_msg = NULL;
return EXIT_SUCCESS;
} catch ( ... ) {
*err_msg = strdup("Caught unknown exception!");
}
return -1;
}