void SpatialStatistics(const PointSet &points, int npoints, Statistics *stats) {
#ifdef PSA_HAS_CGAL
std::vector<Point_2> cgalPoints(npoints);
for (int i = 0; i < npoints; ++i)
cgalPoints[i] = Point_2(points[i].x, points[i].y);
Delaunay dt(cgalPoints, true);
dt.GetStatistics(stats);
#else
Distances(points, npoints, &stats->mindist, &stats->avgmindist);
stats->orientorder = 0;
#endif
}
void test(){
std::vector<Point_2> points;
points.push_back(Point_2(0,0));
points.push_back(Point_2(1,1));
points.push_back(Point_2(0,1));
DT dt2;
//insert points into the triangulation
dt2.insert(points.begin(),points.end());
//construct a rectangle
Iso_rectangle_2 bbox(-1,-1,2,2);
Cropped_voronoi_from_delaunay vor(bbox);
//extract the cropped Voronoi diagram
dt2.draw_dual(vor);
//print the cropped Voronoi diagram as segments
std::copy(vor.m_cropped_vd.begin(),vor.m_cropped_vd.end(),
std::ostream_iterator<Segment_2>(std::cout,"\n"));
}
void debug_func()
{
Angle ang1( Point_2( 1 , 0 ) , Point_2( 0 , 0 ) , Point_2( -1 , 0 ) );
Angle ang2( Point_2( -1 , 0 ) , Point_2( 0 , 0 ) , Point_2( 0 , -1 ) );
Angle ang3( Point_2( 0 , -1 ) , Point_2( 0 , 0 ) , Point_2( 1 , 0 ) );
Angle ang4 = ( ang1 + ang2 ) + ang3;
std::cout << "ang1 = "<< ang1.getVal() << std::endl;
std::cout << "ang2 = "<< ang2.getVal() << std::endl;
std::cout << "ang3 = "<< ang3.getVal() << std::endl;
std::cout << "ang4 = "<< ang4.getVal() << std::endl;
std::cout << "rotation of ang4 is: R = "<< ang4.getR() << std::endl;
system("pause");
/*//debug
if ( !tempL.isValid() )
{
std::cout<<"tempL problem\n";
system ("pause");
}*/
}
bool is_selected (qglviewer::Vec& p)
{
if (domain_rectangle.xmin () < p.x &&
p.x < domain_rectangle.xmax () &&
domain_rectangle.ymin () < p.y &&
p.y < domain_rectangle.ymax ())
{
if (rectangle)
return true;
if (domain_freeform.has_on_bounded_side (Point_2 (p.x, p.y)))
return true;
}
return false;
}
QList<Vector2D> Voronoi_Diagram::calculate(QList<Vector2D> in_points)
{
//consider some points
std::vector<Point_2> points;
for(int i=0;i<in_points.size();i++)
{
points.push_back(Point_2(in_points.at(i).x,in_points.at(i).y));
}
Delaunay_triangulation_2 dt2;
//insert points into the triangulation
dt2.insert(points.begin(),points.end());
//construct a rectangle
Iso_rectangle_2 bbox(Field::MinX,Field::MinY,Field::MaxX,Field::MaxY);
Cropped_voronoi_from_delaunay vor(bbox);
//extract the cropped Voronoi diagram
dt2.draw_dual(vor);
//print the cropped Voronoi diagram as segments
std::stringstream ss;
QList<Vector2D> out;
wm->voronoi.clear();
for(int i=0;i<vor.m_cropped_vd.size();i++)
{
Segment_2 tmp = vor.m_cropped_vd.at(i);
Point_2 start = tmp.vertex(0) , end = tmp.vertex(1);
Vector2D first(floor(start.x()),floor(start.y())) , second(floor(end.x()),floor(end.y()));
if( !out.contains(first) )
{
if( wm->kn->IsInsideField(first) )
out.append(first);
}
if( !out.contains(second) )
{
if( wm->kn->IsInsideField(second) )
out.append(second);
}
Segment2D seg(first,second);
wm->voronoi.append(seg);
}
return out;
}
TEST_F(ch_chanFixture, CheckSubHulls){
Point_2 points[8];
int i =0;
points[i++] = Point_2(0,0);
points[i++] = Point_2(0,1);
points[i++] = Point_2(0,0.1);
points[i++] = Point_2(1,0);
points[i++] = Point_2(1,0);
points[i++] = Point_2(1,1);
points[i++] = Point_2(1,0.1);
points[i++] = Point_2(2,0);
vector<vector< Point_2 > > hulls = ch_chan::GetSubHulls(points,points+8,4);
int actual = hulls[0].size();
int expected = 4;
EXPECT_EQ(actual,expected);
actual = hulls[1].size();
expected = 4;
EXPECT_EQ(actual,expected);
}
void ApplyObjects(Pmwx& ioMap)
{
if (gFAAObs.empty()) return;
Point_2 sw, ne;
CalcBoundingBox(ioMap, sw, ne);
// ioMap.Index();
int placed = 0;
// CGAL::Arr_landmarks_point_location<Arrangement_2> locator(gMap);
CGAL::Arr_walk_along_line_point_location<Arrangement_2> locator(gMap);
for (FAAObsTable::iterator i = gFAAObs.begin(); i != gFAAObs.end(); ++i)
{
if (i->second.kind != NO_VALUE)
{
Point_2 loc = Point_2(i->second.lon, i->second.lat);
DebugAssert(CGAL::is_valid(gMap));
CGAL::Object obj = locator.locate(loc);
Face_const_handle ff;
if(CGAL::assign(ff,obj))
{
Face_handle f = ioMap.non_const_handle(ff);
GISPointFeature_t feat;
feat.mFeatType = i->second.kind;
feat.mLocation = loc;
if (i->second.agl != DEM_NO_DATA)
feat.mParams[pf_Height] = i->second.agl;
feat.mInstantiated = false;
f->data().mPointFeatures.push_back(feat);
++placed;
#if 0
printf("Placed %s at %lf, %lf\n",
FetchTokenString(i->second.kind), i->second.lon, i->second.lat);
#endif
// if (v.size() > 1)
// fprintf(stderr,"WARNING (%d,%d): Point feature %lf, %lf matches multiple areas.\n",gMapWest, gMapSouth, CGAL::to_double(loc.x()), CGAL::to_double(loc.y()));
}
}
}
printf("Placed %d objects.\n", placed);
}
Obstacle
SSLPathPlanner::getGoalAsObstacle (ssl::FIELD_SIDES left_or_right)
{
Obstacle obstacle;
obstacle.type = ssl::GOAL;
Rectancle_2 rectangle_upper;
Rectancle_2 rectangle_bottom;
if (left_or_right == ssl::LEFT)
{
rectangle_upper = Rectancle_2 (Point_2 (-3.050, 0.35), Point_2 (-3.025, 0.37));
rectangle_bottom = Rectancle_2 (Point_2 (-3.050, -0.37), Point_2 (-3.025, -0.35));
}
else
{
rectangle_upper = Rectancle_2 (Point_2 (3.025, 0.35), Point_2 (3.050, 0.37));
rectangle_bottom = Rectancle_2 (Point_2 (3.025, -0.37), Point_2 (3.050, -0.35));
}
Point upper_points[] = {rectangle_upper.vertex (0), rectangle_upper.vertex (1), rectangle_upper.vertex (2),
rectangle_upper.vertex (3)};
Point lower_points[] = {rectangle_bottom.vertex (0), rectangle_bottom.vertex (1), rectangle_bottom.vertex (2),
rectangle_bottom.vertex (3)};
// Point upper_points[] = {Point (ssl::math::sign ((int8_t)left_or_right) * 3.050, 0.35),
// Point (ssl::math::sign ((int8_t)left_or_right) * 3.050, 0.37),
// Point (ssl::math::sign ((int8_t)left_or_right) * 3.025, 0.37),
// Point (ssl::math::sign ((int8_t)left_or_right) * 3.025, 0.35)};
//
// Point lower_points[] = {Point (ssl::math::sign ((int8_t)left_or_right) * 3.025, -0.35),
// Point (ssl::math::sign ((int8_t)left_or_right) * 3.025, -0.37),
// Point (ssl::math::sign ((int8_t)left_or_right) * 3.050, -0.35),
// Point (ssl::math::sign ((int8_t)left_or_right) * 3.050, -0.37)};
obstacle.polygons.push_back (Polygon_2 (upper_points, upper_points + 4));
obstacle.polygons.push_back (Polygon_2 (lower_points, lower_points + 4));
return obstacle;
}
Polygon_2 CollisionDetector::translate_polygon_to(const Polygon_2& poly, const Point_2& new_ref_pt) const
{
m_translate_helper.resize(0);
const Point_2 &ref = *poly.left_vertex();
std::pair<double,double> diff(
//Vector_2 diff(
CGAL::to_double(ref.x().exact()) - CGAL::to_double(new_ref_pt.x().exact()),
CGAL::to_double(ref.y().exact()) - CGAL::to_double(new_ref_pt.y().exact())
);
for (Polygon_2::Vertex_const_iterator it = poly.vertices_begin(), it_end = poly.vertices_end(); it != it_end; ++it)
{
m_translate_helper.push_back( Point_2(CGAL::to_double(it->x()) + diff.first, CGAL::to_double(it->y()) + diff.second ) );
//translated.push_back( (*it) + diff );
}
Polygon_2 new_poly(m_translate_helper.begin(),m_translate_helper.end());
return new_poly;
}
Obstacle
SSLPathPlanner::getRobotAsObstacle (uint8_t team, uint8_t id, const geometry_msgs::Vector3& vel,
const geometry_msgs::Vector3& acc)
{
Obstacle obstacle = getRobotAsObstacle (team, id);
//assuming linear velocity in one control cycle which is about 20ms.
tf::Vector3 x_curr;
tf::vector3MsgToTF (getRobotPosition (team, id), x_curr);
tf::Vector3 v_curr;
tf::vector3MsgToTF (vel, v_curr);
tf::Vector3 acc_cmd;
tf::vector3MsgToTF (acc, acc_cmd);
tf::Vector3 x_next = x_curr + v_curr * ssl::config::TIME_STEP_SEC + 0.5 * acc_cmd * ssl::config::TIME_STEP_SEC
* ssl::config::TIME_STEP_SEC;
obstacle.circles.push_back (Circle_2 (Point_2 (x_next.x (), x_next.y ()), ssl::config::ROBOT_BOUNDING_RADIUS));
//TODO fill in between these circles with a rectangular polygon
// skipping this for now since, a robot in 20ms can move at most 6cm while moving with 3m/s
return obstacle;
}
Point_2 Origin::operator-(const Vector_2& v) const {return Point_2( CGAL::ORIGIN-v.get_data() ); }
PwhPtr CstmCGAL::applyOffset(double offset, const Polygon_with_holes_2& poly) {
// This code is inspired from the CGAL example Straight_skeleton_2/Low_level_API
// As the offset can only produce an interior polygon, we need to produce a frame
// that encloses the polygon and is big enough so that the offset of the contour
// does not interfere with the one ot the polygon. See CGAL doc page for more info
boost::optional<double> margin = CGAL::compute_outer_frame_margin(
poly.outer_boundary().vertices_begin(),poly.outer_boundary().vertices_end(),offset);
if ( margin ) {
CGAL::Bbox_2 bbox = CGAL::bbox_2(poly.outer_boundary().vertices_begin(),poly.outer_boundary().vertices_end());
double fxmin = bbox.xmin() - *margin ;
double fxmax = bbox.xmax() + *margin ;
double fymin = bbox.ymin() - *margin ;
double fymax = bbox.ymax() + *margin ;
// Create the rectangular frame
Point_2 frame[4]= { Point_2(fxmin,fymin)
, Point_2(fxmax,fymin)
, Point_2(fxmax,fymax)
, Point_2(fxmin,fymax)
} ;
SsBuilder ssb ;
ssb.enter_contour(frame,frame+4);
// We have to revert the orientation of the polygon
std::vector<Point_2> outerBoundary = std::vector<Point_2>(
poly.outer_boundary().vertices_begin(),poly.outer_boundary().vertices_end());
ssb.enter_contour(outerBoundary.rbegin(), outerBoundary.rend());
SsPtr ss = ssb.construct_skeleton();
if ( ss ) {
std::vector<Polygon_2Ptr> offset_contours ;
OffsetBuilder ob(*ss);
ob.construct_offset_contours(offset, std::back_inserter(offset_contours));
// Locate the offset contour that corresponds to the frame
// That must be the outmost offset contour, which in turn must be the one
// with the largest unsigned area.
std::vector<Polygon_2Ptr>::iterator f = offset_contours.end();
double lLargestArea = 0.0 ;
for (std::vector<Polygon_2Ptr>::iterator i = offset_contours.begin(); i != offset_contours.end(); ++ i) {
double lArea = CGAL_NTS abs( (*i)->area() ) ; //Take abs() as Polygon_2::area() is signed.
if ( lArea > lLargestArea ) {
f = i ;
lLargestArea = lArea ;
}
}
offset_contours.erase(f);
// Construct result polygon
std::vector<Point_2> newOuterBoundary = std::vector<Point_2>(
offset_contours.front()->vertices_begin(), offset_contours.front()->vertices_end());
Polygon_with_holes_2 result = Polygon_with_holes_2(Polygon_2(newOuterBoundary.rbegin(), newOuterBoundary.rend()));
// We have to handle the holes separately
for (auto it = poly.holes_begin() ; it != poly.holes_end() ; it++) {
std::vector<Point_2> hole = std::vector<Point_2>(it->vertices_begin(),it->vertices_end());
std::vector<PwhPtr> holeOffsets =
CGAL::create_interior_skeleton_and_offset_polygons_with_holes_2(offset,
Polygon_with_holes_2(Polygon_2(hole.begin(), hole.end())));
for (auto it2 = holeOffsets.begin() ; it2 != holeOffsets.end() ; it++) {
std::vector<Point_2> revertNewHoles = std::vector<Point_2>(
(*it2)->outer_boundary().vertices_begin(),(*it2)->outer_boundary().vertices_end());
result.add_hole(Polygon_2(revertNewHoles.rbegin(), revertNewHoles.rend()));
}
}
return boost::make_shared<Polygon_with_holes_2>(result);
}
}
return NULL;
}
TEST_F(ch_chanFixture, FindLeftmostHull){
Point_2 points[16];
int i =0;
//Single Point
points[i++] = Point_2(1,0);
points[i++] = Point_2(1,0);
points[i++] = Point_2(1,0);
points[i++] = Point_2(1,0);
//Two Points
points[i++] = Point_2(1,0);
points[i++] = Point_2(1,0);
points[i++] = Point_2(1,1);
points[i++] = Point_2(1,1);
//Square
points[i++] = Point_2(0,0);
points[i++] = Point_2(0,-1);
points[i++] = Point_2(-1,0.32); // This guy is the leftmost!!!
points[i++] = Point_2(-0.9,-1);
//Triangle with point in middle
points[i++] = Point_2(1,0);
points[i++] = Point_2(1,1);
points[i++] = Point_2(1,0.1);
points[i++] = Point_2(2,0);
vector<vector< Point_2 > > hulls = ch_chan::GetSubHulls(points,points+i,4);
int hIndex, pIndex;
ch_chan::FindLeftmostHull(hulls, hIndex, pIndex);
int expected = 2;
int actual = hIndex;
EXPECT_EQ(expected, actual)<<"hIndex";
expected = 0.32;
actual = hulls[hIndex][pIndex].y();
EXPECT_EQ(expected, actual)<<"pIndex";
}
TEST_F(ch_chanFixture, NextPair){
vector<Point_2> square;
//square
square.push_back(Point_2(0,0));
square.push_back(Point_2(1,-0.1));
square.push_back(Point_2(1,1.2));
square.push_back(Point_2(0.1,1));
square.push_back(square[0]);
vector<Point_2> triangle;
//Triangle
triangle.push_back(Point_2(2,-0.2));
triangle.push_back(Point_2(3,0));
triangle.push_back(Point_2(2,1.1));
triangle.push_back(triangle[0]);
vector<vector<Point_2> > hulls;
hulls.push_back(square);
hulls.push_back(triangle);
int hIndex = 0;
int pIndex = 0;
int counter = 1;
ch_chan::NextPair(hulls,hIndex,pIndex);
int actualH = hIndex;
int expectedH = 0;
int actualP = pIndex;
int expectedP = 1;
EXPECT_EQ(expectedH,actualH)<<"Point("<<counter<<"): h";
EXPECT_EQ(expectedP,actualP)<<"Point("<<counter<<"): p";
counter++;
ch_chan::NextPair(hulls,hIndex,pIndex);
actualH = hIndex;
expectedH = 1;
actualP = pIndex;
expectedP = 0;
EXPECT_EQ(expectedH,actualH)<<"Point("<<counter<<"): h";
EXPECT_EQ(expectedP,actualP)<<"Point("<<counter<<"): p";
counter++;
ch_chan::NextPair(hulls,hIndex,pIndex);
actualH = hIndex;
expectedH = 1;
actualP = pIndex;
expectedP = 1;
EXPECT_EQ(expectedH,actualH)<<"Point("<<counter<<"): h";
EXPECT_EQ(expectedP,actualP)<<"Point("<<counter<<"): p";
counter++;
ch_chan::NextPair(hulls,hIndex,pIndex);
actualH = hIndex;
expectedH = 1;
actualP = pIndex;
expectedP = 2;
EXPECT_EQ(expectedH,actualH)<<"Point("<<counter<<"): h";
EXPECT_EQ(expectedP,actualP)<<"Point("<<counter<<"): p";
counter++;
ch_chan::NextPair(hulls,hIndex,pIndex);
actualH = hIndex;
expectedH = 0;
actualP = pIndex;
expectedP = 2;
EXPECT_EQ(expectedH,actualH)<<"Point("<<counter<<"): h";
EXPECT_EQ(expectedP,actualP)<<"Point("<<counter<<"): p";
counter++;
ch_chan::NextPair(hulls,hIndex,pIndex);
actualH = hIndex;
expectedH = 0;
actualP = pIndex;
expectedP = 3;
EXPECT_EQ(expectedH,actualH)<<"Point("<<counter<<"): h";
EXPECT_EQ(expectedP,actualP)<<"Point("<<counter<<"): p";
counter++;
}
bool LocalPlanner::local_planner_two_robot(const Point_2& start_r1,
const Point_2& target_r1,
const Point_2& start_r2,
const Point_2& target_r2,
double eps ) const
{
//robot 1 calc
double x1_r1 = CGAL::to_double(start_r1.x());
double y1_r1 = CGAL::to_double(start_r1.y());
double x2_r1 = CGAL::to_double(target_r1.x());
double y2_r1 = CGAL::to_double(target_r1.y()) ;
double distance_r1 = sqrt(pow(x1_r1 - x2_r1, 2) + pow(y1_r1 - y2_r1,2));
double step_size = eps;
// calculate how many steps are required for first robot
int step_num_r1 = floor((distance_r1 - step_size) / step_size);
double vx_r1 = x2_r1 - x1_r1;
double vy_r1 = y2_r1 - y1_r1;
//robot 2 calc
double x1_r2 = CGAL::to_double(start_r2.x());
double y1_r2 = CGAL::to_double(start_r2.y());
double x2_r2 = CGAL::to_double(target_r2.x());
double y2_r2 = CGAL::to_double(target_r2.y()) ;
double distance_r2 = sqrt(pow(x1_r2 - x2_r2, 2) + pow(y1_r2 - y2_r2,2));
// calculate how many steps are required for second robot
int step_num_r2 = floor((distance_r2 - step_size) / step_size);
double vx_r2 = x2_r2 - x1_r2;
double vy_r2 = y2_r2 - y1_r2;
//select the largest number of steps of both robot for loop iteration check
double step_num = step_num_r1 > step_num_r2 ? step_num_r1 : step_num_r2;
double step_size_r1 = distance_r1/step_num;
double step_size_r2 = distance_r2/step_num;
std::vector<double> coords;
for (int i = 1; i <= step_num; ++i)
{
// generate a configuration for every step
double offset_r1 = (i * step_size_r1) / (distance_r1 - step_size_r1);
double currx_r1 = x1_r1 + vx_r1 * offset_r1;
double curry_r1 = y1_r1 + vy_r1 * offset_r1;
double offset_r2 = (i * step_size_r2) / (distance_r2 - step_size_r2);
double currx_r2 = x1_r2 + vx_r2 * offset_r2;
double curry_r2 = y1_r2 + vy_r2 * offset_r2;
if( !m_cd.valid_conf( Point_2(currx_r1,curry_r1),Point_2(currx_r2,curry_r2) ) ) return false;
// If an intermidiate configuration is invalid, return false
//if (!m_cd.one_robot_valid_conf(currentPos,obstacles)) return false;
}
// GREAT SUCCESS!
return true;
}
CollisionDetector::CollisionDetector(Polygon_2 robot1, Polygon_2 robot2, Obstacles* obs,double eps)
: approx_robot1(robot1)
, approx_robot2(robot2)
, m_obs(obs)
, m_translate_helper()
, m_minus_r1(flip(robot1))
, m_minus_r2(flip(robot2))
, m_epsilon(eps)
{
Polygon_2 enlarger;
int nOfEdges = 8;
double radius = eps/2;
double dAlpha = (360.0/nOfEdges)*CGAL_PI/180;
for (int i = nOfEdges; i>0; i--) {
double alpha = (i + 0.5)*dAlpha;
double x = (radius/cos(dAlpha/2)) * cos(alpha) * 1.05;
double y = (radius/cos(dAlpha/2)) * sin(alpha) * 1.05;
enlarger.push_back(Point_2(x, y));
}
// Compute the Minkowski sum using the decomposition approach.
CGAL::Small_side_angle_bisector_decomposition_2<Kernel> ssab_decomp;
Polygon_with_holes_2 pwh1 = minkowski_sum_2 (robot1, enlarger, ssab_decomp);
Polygon_with_holes_2 pwh2 = minkowski_sum_2 (robot2, enlarger, ssab_decomp);
approx_robot1 = pwh1.outer_boundary();
approx_robot2 = pwh2.outer_boundary();
Polygon_with_holes_2 pwhF1 = minkowski_sum_2 (m_minus_r1, enlarger, ssab_decomp);
Polygon_with_holes_2 pwhF2 = minkowski_sum_2 (m_minus_r2, enlarger, ssab_decomp);
m_minus_r1_en = pwhF1.outer_boundary();
m_minus_r2_en = pwhF2.outer_boundary();
Polygon_set_2 ps;
if (!m_obs->empty())
{
for (Obstacles::iterator Oiter = m_obs->begin(); Oiter != m_obs->end(); Oiter++)
{
// For every obstacle calculate its Minkowski sum with a "robot"
Polygon_with_holes_2 poly_wh1 = minkowski_sum_2 (*Oiter, m_minus_r1_en, ssab_decomp);
Polygon_with_holes_2 poly_wh2 = minkowski_sum_2 (*Oiter, m_minus_r2_en, ssab_decomp);
// Add the result to the polygon set
m_r1_poly_set.join(poly_wh1);
m_r2_poly_set.join(poly_wh2);
}
}
Polygon_with_holes_2 r1_r2 = minkowski_sum_2 (approx_robot1, m_minus_r2_en, ssab_decomp);
Polygon_2 for_print = r1_r2.outer_boundary();
for(Polygon_2::Vertex_const_iterator it = for_print.vertices_begin(); it != for_print.vertices_end(); ++it)
{
std::cout << "(" << it->x() << "," << it->y() << "),";
}
std::cout << std::endl;
m_r1_min_r2.join(r1_r2);
Polygon_with_holes_2 r2_r1 = minkowski_sum_2 (approx_robot2, m_minus_r1_en, ssab_decomp);
m_r2_min_r1.join(r2_r1);
}
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);
}
}
// this method overrides the virtual method 'draw()' of Qt_widget_layer
void Qt_layer_show_ch::draw()
{
widget->lock(); // widget has to be locked before drawing
RasterOp old_rasterop = widget->rasterOp();
widget->get_painter().setRasterOp(XorROP);
widget->setFilled (true);
widget->setFillColor (CGAL::RED);
*widget << CGAL::BLACK;
std::list<Polygon_with_holes> red_pgns_list;
myWin->red_set.polygons_with_holes(std::back_inserter(red_pgns_list));
std::list<Polygon_with_holes>::iterator itpgn1 = red_pgns_list.begin();
while (itpgn1 != red_pgns_list.end())
{
const Polygon_with_holes& pgn_with_hole = *itpgn1;
const Polygon_2& outer_boundary = pgn_with_hole.outer_boundary();
if (outer_boundary.is_empty())
{
// no boundary -> unbounded polygon
Iso_rectangle rect(Point_2(widget->x_min(), widget->y_min()),
Point_2(widget->x_max(), widget->y_max()));
*widget << rect;
}
else
*widget << outer_boundary;
for(Hole_const_iterator hit = pgn_with_hole.holes_begin();
hit != pgn_with_hole.holes_end();
++hit)
{
*widget << *hit;
}
++itpgn1;
}
widget->setFilled (true);
widget->setFillColor (CGAL::BLUE);
std::list<Polygon_with_holes> blue_pgns_list;
myWin->blue_set.polygons_with_holes(std::back_inserter(blue_pgns_list));
std::list<Polygon_with_holes>::iterator itpgn2 = blue_pgns_list.begin();
while (itpgn2 != blue_pgns_list.end())
{
const Polygon_with_holes& pgn_with_hole = *itpgn2;
const Polygon_2& outer_boundary = pgn_with_hole.outer_boundary();
if (outer_boundary.is_empty())
{
// no boundary -> unbounded polygon
Iso_rectangle rect(Point_2(widget->x_min(), widget->y_min()),
Point_2(widget->x_max(), widget->y_max()));
*widget << rect;
}
else
{
*widget << outer_boundary;
}
for(Hole_const_iterator hit = pgn_with_hole.holes_begin();
hit != pgn_with_hole.holes_end();
++hit)
{
*widget << *hit;
}
++itpgn2;
}
widget->get_painter().setRasterOp(old_rasterop);
widget->setFilled (false);
widget->unlock(); // widget have to be unlocked when finished drawing
}
void SubSelectIpelet::protected_run(int fn)
{
if (fn==2) {
show_help();
return;
}
std::list<Circle_2> cir_list;
std::list<Polygon_2> pol_list;
Iso_rectangle_2 bbox=
read_active_objects(
CGAL::dispatch_or_drop_output<Polygon_2,Circle_2>(
std::back_inserter(pol_list),
std::back_inserter(cir_list)
)
);
if (fn==0 && pol_list.size()!=2){
print_error_message("You must select exactly two polygons");
return;
}
std::list<double> r_offsets;
for (std::list<Circle_2>::iterator it=cir_list.begin();it!=cir_list.end();++it)
r_offsets.push_back(sqrt(CGAL::to_double(it->squared_radius())));
IpeMatrix tfm (1,0,0,1,-CGAL::to_double(bbox.min().x()),-CGAL::to_double(bbox.min().y()));
for (std::list<Polygon_2>::iterator it=pol_list.begin();it!=pol_list.end();++it)
if(!it->is_simple()){
print_error_message("Polygon(s) must be simple");
}
if (fn==0){
Polygon_2 polygon1=*pol_list.begin();
Polygon_2 polygon2=*++pol_list.begin();
Polygon_with_holes_2 sum = minkowski_sum_2 (polygon1, polygon2);
std::list<Point_2> LP;
for (Polygon_2::iterator it=sum.outer_boundary().vertices_begin();it!= sum.outer_boundary().vertices_end();++it)
LP.push_back(*it);
draw_polyline_in_ipe(LP.begin(),LP.end(),true,false,false);
for (Polygon_with_holes_2::Hole_const_iterator poly_it = sum.holes_begin(); poly_it != sum.holes_end();
++poly_it){
LP.clear();
for (Polygon_2::iterator it=poly_it->vertices_begin();it!= poly_it->vertices_end();++it)
LP.push_back(*it);
draw_polyline_in_ipe(LP.begin(),LP.end(),true,false,false);
}
create_polygon_with_holes(true);
transform_selected_objects_(tfm);
}
else{
if (r_offsets.size()==0)
r_offsets.push_back(10);
for (std::list<Polygon_2>::iterator it_pol=pol_list.begin();it_pol!=pol_list.end();++it_pol){
for(std::list<double>::iterator it=r_offsets.begin();it!=r_offsets.end();++it){
Offset_polygon_with_holes_2 offset=approximated_offset_2 (*it_pol, *it, 0.0001);
std::list<Segment_2> LS;
for( Offset_polygon_2::Curve_iterator itt=offset.outer_boundary().curves_begin();
itt!=offset.outer_boundary().curves_end();++itt){
Point_2 S=Point_2(CGAL::to_double(itt->source().x()),CGAL::to_double(itt->source().y()));
Point_2 T=Point_2(CGAL::to_double(itt->target().x()),CGAL::to_double(itt->target().y()));
if (itt->is_linear ())
LS.push_back(Segment_2(S,T));
if (itt->is_circular())
draw_in_ipe(Circular_arc_2(itt->supporting_circle(),S,T,itt->supporting_circle().orientation()));
}
draw_in_ipe(LS.begin(),LS.end());
}
}
}
}
TEST_F(ch_chanFixture, CheckSubHullsOddNumberOfPoints){
Point_2 points[16];
int i =0;
//Single Point
points[i++] = Point_2(1,0);
points[i++] = Point_2(1,0);
points[i++] = Point_2(1,0);
points[i++] = Point_2(1,0);
//Two Points
points[i++] = Point_2(1,0);
points[i++] = Point_2(1,0);
points[i++] = Point_2(1,1);
points[i++] = Point_2(1,1);
//Triangle with point in middle
points[i++] = Point_2(1,0);
points[i++] = Point_2(1,1);
points[i++] = Point_2(1,0.1);
points[i++] = Point_2(2,0);
//Square
points[i++] = Point_2(0,0);
points[i++] = Point_2(0,1);
points[i++] = Point_2(1,0);
points[i++] = Point_2(1,1);
vector<vector< Point_2 > > hulls = ch_chan::GetSubHulls(points,points+i,4);
for(int i =0;i<4;i++){
int expected = i+2;
int actual = hulls[i].size();
EXPECT_EQ(expected, actual);
}
}
inline Point_2 RawCoordToCoord(const RawCoordPair& p) { return Point_2(atof(p.second.c_str())/* / 1000000.0*/, atof(p.first.c_str())/* / 1000000.0*/); }
int main(int argc , char* argv[])
{
if (argc < 7) {
std::cerr << "usage: test <input file> <output folder> "
"<format (wkt | geojson | sql)> <weight city> <weight town> <weight village>" << std::endl;
exit(1);
}
char* input = argv[1];
char* outdir = argv[2];
char* format = argv[3];
char* swc = argv[4];
char* swt = argv[5];
char* swv = argv[6];
std::ifstream ifs(input);
assert( ifs );
double wc, wt, wv;
std::istringstream stmc, stmt, stmv;
stmc.str(swc);
stmc >> wc;
stmt.str(swt);
stmt >> wt;
stmv.str(swv);
stmv >> wv;
std::cerr << "using weights: city: " << wc << ", town: " << wt << ", village: " << wv << std::endl;
/*
* prepare data
*/
// we use a ScalingFactor(SF) here to stretch input values at the
// beginning, and divide by SF in the end. This is used because the
// point-generation of the hyperbola class is using some arbitrary
// internal decision thresholds to decide how many points to generate for
// a certain part of the curve. Rule of thumb is: the higher SF the more
// detail is used in approximation of the hyperbolas.
double SF = 4000;
// read in sites from input file
SiteList sites = readSites(ifs, SF, wc, wt, wv);
printSites(sites);
// calculate bounding box of all input sites (and extend it a little).
// Extension is important, because we later add artificial sites which are
// actually mirrored on the bounds of this rectangle. If we did not extend
// some points would lie on the boundary of the bounding box and so would
// their artificial clones. This would complicate the whole stuff a lot :)
Iso_rectangle_2 crect = extend(boundingBox(sites), 0.1*SF);
std::cerr << "rect: " << crect << std::endl;
// a number of artificial sites
SiteList artificialSites = createArtificialSites(sites, crect);
/*
* create Apollonius graph
*/
Apollonius_graph ag;
SiteList::iterator itr;
// add all original sites to the apollonius graph
for (itr = sites.begin(); itr != sites.end(); ++itr) {
Site_2 site = *itr;
ag.insert(site);
}
// add all artificial sites to the apollonius graph
for (itr = artificialSites.begin(); itr != artificialSites.end(); ++itr) {
Site_2 site = *itr;
ag.insert(site);
}
// validate the Apollonius graph
assert( ag.is_valid(true, 1) );
std::cerr << std::endl;
/*
* create polygons from cells
*/
// we want an identifier for each vertex within the iteration.
// this is a loop iteration counter
int vertexIndex = 0;
// for each vertex in the apollonius graph (this are the sites)
for (All_vertices_iterator viter = ag.all_vertices_begin ();
viter != ag.all_vertices_end(); ++viter) {
// get the corresponding site
Site_2 site = viter->site();
Point_2 point = site.point();
// ignore artifical sites, detect them by their position
if (!CGAL::do_intersect(crect, point)) {
continue;
}
std::cerr << "vertex " << ++vertexIndex << std::endl;
// we than circulate all incident edges. By obtaining the respective
// dual of each edge, we get access to the objects forming the boundary
// of each voronoi cell in a proper order.
Edge_circulator ecirc = ag.incident_edges(viter), done(ecirc);
// this is where we store the polylines
std::vector<PointList> polylines;
// for each incident edge
do {
// the program may fail in certain situations without this test.
// acutally !is_infinite(edge) is a precondition in ag.dual(edge).
if (ag.is_infinite(*ecirc)) {
continue;
}
// NOTE: for this to work, we had to make public the dual function in ApolloniusGraph
// change line 542 in "Apollonius_graph_2.h" from "private:" to "public:"
Object_2 o = ag.dual(*ecirc);
handleDual(o, crect, polylines);
} while(++ecirc != done);
PointList polygon = buildPolygon(site, polylines);
for (int i = 0; i < polygon.size(); i++) {
Point_2& p = polygon.at(i);
p = Point_2(p.x()/SF, p.y()/SF);
}
if(std::string(format) == "geojson") {
writeGeoJSON(site, polygon, outdir);
} else if(std::string(format) == "sql") {
writeSQL(site, polygon, outdir);
} else {
writeWKT(site, polygon, outdir);
}
// check each point
for (int i = 0; i < polygon.size(); i++) {
Point_2 p = polygon.at(i);
if (p.x() > crect.xmax()/SF || p.x() < crect.xmin()/SF || p.y() > crect.ymax()/SF || p.y() < crect.ymin()/SF) {
std::cerr << "out of bounds" << std::endl;
}
}
}
}