void CylinderTest::collisionPoint() {
    Shapes::Cylinder3D cylinder({-1.0f, -1.0f, 0.0f}, {1.0f, 1.0f, 0.0f}, 2.0f);
    Shapes::Point3D point({2.0f, 0.0f, 0.0f});
    Shapes::Point3D point1({1.0f, 3.1f, 0.0f});
    Shapes::Point3D point2({2.9f, -1.0f, 0.0f});

    VERIFY_COLLIDES(cylinder, point);
    VERIFY_COLLIDES(cylinder, point1);
    VERIFY_NOT_COLLIDES(cylinder, point2);
}

static bool ClipToRect( long & x1, long & y1, long & x2, long & y2,
                        const WRect & clip )
//-----------------------------------------------------------------
// Cohen-Sutherland Clipping Algorithm from _Fundamentals of Interactive
// Computer Graphics_, page 148.
{
    PointCode   point1( x1, y1, CL_Inside );
    PointCode   point2( x2, y2, CL_Inside );
    PointCode * p1;
    PointCode * p2;
    PointCode * tmp;
    long        top = clip.y();
    long        left = clip.x();
    long        bottom = clip.y() + clip.h();
    long        right = clip.x() + clip.w();

    p1 = & point1;
    p2 = & point2;

    while( 1 ) {
        CalcOut( *p1, top, left, bottom, right );
        CalcOut( *p2, top, left, bottom, right );

        if( p1->code == CL_Inside && p2->code == CL_Inside ) {
            return TRUE;                                // trivial acceptance
        }
        if( (p1->code & p2->code) != 0 ) {
            return FALSE;                               // trivial rejection
        }

        if( p1->code == 0 ) {   // p1 inside -- swap so p1 outside
            tmp = p1;
            p1 = p2;
            p2 = tmp;
        }

        // perform a subdivision; move p1 to the intersection point.
        // use the formula y = y1 + slope * (x - x1),
        //                 x = x1 + (y - y1) / slope.

        if( p1->code & CL_Above ) {                 // divide at top
            p1->x += ((p2->x - p1->x) * (top - p1->y)) / (p2->y - p1->y);
            p1->y = top;
        } else if( p1->code & CL_Below ) {          // divide at bottom of
            p1->x += ((p2->x - p1->x) * (bottom - p1->y)) / (p2->y - p1->y);
            p1->y = bottom;
        } else if( p1->code & CL_Right ) {          // divide at right
            p1->y += ((p2->y - p1->y) * (right - p1->x)) / (p2->x - p1->x);
            p1->x = right;
        } else if( p1->code & CL_Left ) {           // divide at left
            p1->y += ((p2->y - p1->y) * (left - p1->x)) / (p2->x - p1->x);
            p1->x = left;
        }
    }
}

int main(){
    int N = 3;
    CGAL::Timer cost;
    std::vector<Point_d> points;
    
   std::vector<double> point;
   point.push_back(6);
   point.push_back(6);
   point.push_back(6);
    
   Point_d point1(3, point.begin(), point.end());
   std::cout << point1[1] << std::endl;
  // Point_d point1(1,3,5);
   Point_d point2(4,8,10);
   Point_d point3(2,7,9);

   // Point_d point(1,2,3);


    points.push_back(point1);
    points.push_back(point2);
    points.push_back(point3);
   
   
//    D Dt(d);
//  //  CGAL_assertion(Dt.empty());
//   
//    // insert the points in the triangulation
//    cost.reset();cost.start();
//    std::cout << "  Delaunay triangulation of "<<N<<" points in dim "<<d<< std::flush;
//    std::vector<Point_d>::iterator it;
//    for(it = points.begin(); it!= points.end(); ++it){
//	Dt.insert(*it); 
//    }
//    std::list<Simplex_handle> NL = Dt.all_simplices(D::NEAREST);
//    std::cout << " done in "<<cost.time()<<" seconds." << std::endl;
//    CGAL_assertion(Dt.is_valid() );
//    CGAL_assertion(!Dt.empty());
// 
//   
//    Vertex_handle v = Dt.nearest_neighbor(point);
//    Simplex_handle s = Dt.simplex(v);    
//     
//    std::vector<Point_d> Simplex_vertices;
//    for(int j=0; j<=d; ++j){
// 	  Vertex_handle vertex = Dt.vertex_of_simplex(s,j);
//       	  Simplex_vertices.push_back(Dt.associated_point(vertex));
//     }
//    
//    std::vector<K::FT> coords;
//    K::Barycentric_coordinates_d BaryCoords;
//    BaryCoords(Simplex_vertices.begin(), Simplex_vertices.end(),point,std::inserter(coords, coords.begin()));
//    std::cout << coords[0] << std::endl; 
//   return 0;
}

// track object
void ObjectTrackingClass::track(cv::Mat& image, // output image
                                cv::Mat& image1, // previous frame
                                cv::Mat& image2, // next frame
                                std::vector<cv::Point2f>& points1, // previous points 
                                std::vector<cv::Point2f>& points2, // next points
                                std::vector<uchar>& status, // status array
                                std::vector<float>& err) // error array
{
    // tracking code
    cv::calcOpticalFlowPyrLK(image1,
                             image2,
                             points1,
                             points2,
                             status,
                             err,
                             winSize,
                             maxLevel,
                             termcrit,
                             flags,
                             minEigThreshold);
    
    // work out maximum X,Y keypoint values in the next_points keypoint vector
    cv::Point2f min(FLT_MAX, FLT_MAX);
    cv::Point2f max(FLT_MIN, FLT_MIN);
    
    // refactor the points array to remove points lost due to tracking error
    size_t i, k;
    for( i = k = 0; i < points2.size(); i++ )
    {
        if( !status[i] )
            continue;
        
        points2[k++] = points2[i];
        
        // find keypoints at the extremes
        min.x = Min(min.x, points2[i].x);
        min.y = Min(min.y, points2[i].y);
        max.x = Max(max.x, points2[i].x);
        max.y = Max(max.y, points2[i].y);
        
        // draw points
        cv::circle( image, points2[i], 3, cv::Scalar(0,255,0), -1, 8);
    }
    points2.resize(k);
    
    // Draw lines between the extreme points (square)
    cv::Point2f point0(min.x, min.y);
    cv::Point2f point1(max.x, min.y);
    cv::Point2f point2(max.x, max.y);
    cv::Point2f point3(min.x, max.y);
    cv::line( image, point0, point1, cv::Scalar( 0, 255, 0 ), 4 );
    cv::line( image, point1, point2, cv::Scalar( 0, 255, 0 ), 4 );
    cv::line( image, point2, point3, cv::Scalar( 0, 255, 0 ), 4 );
    cv::line( image, point3, point0, cv::Scalar( 0, 255, 0 ), 4 );
}

//-----------------------------------------------------------------------------
void D2DDrawContext::drawPoint (const CPoint &point, const CColor& color)
{
    saveGlobalState ();
    setLineWidth (1);
    setFrameColor (color);
    CPoint point2 (point);
    point2.h++;
    moveTo (point);
    lineTo (point2);
    restoreGlobalState ();
}

      void TestEndPosition()
      {
         osg::Vec3 point1(0.0f, 0.0f, 0.0f);
         osg::Vec3 point2(10.0f, 10.0f, 10.0f);
         mIsector->SetStartPosition(point1);
         mIsector->SetEndPosition(point2);

         CPPUNIT_ASSERT(mIsector->GetDirection() == (point2 - point1));
         CPPUNIT_ASSERT_DOUBLES_EQUAL( mIsector->GetLength(), mIsector->GetDirection().length(), 0.001f );
         
      }

point2 point2::rotate_relative_to(double angle, point2 const& p) const
{
	double ox = p.x;
	double oy = p.y;
	
	double v1 = sin(angle);
	double v2 = cos(angle);
	double v3 = -oy + y;
	double v4 = -ox + x;
	return point2(ox - v1*v3 + v2*v4, oy + v2*v3 + v1*v4);
}

bool RTRBoundingBox::contain(const RTRBoundingBox& other) const
{
    for (int i = 0; i < 3; i++)
    {
        if (point1(i)>other.point1(i) || point2(i) < other.point2(i))
        {
            return false;
        }
    }
    return true;
}

FX_BOOL CPWL_EditCtrl::IsWndHorV()
{
    CPDF_Matrix mt = GetWindowMatrix();
    CPDF_Point point1(0,1);
    CPDF_Point point2(1,1);

    mt.Transform(point1.x, point1.y);
    mt.Transform(point2.x, point2.y);

    return point2.y == point1.y;
}

//-----------------------------------------------------------------------------
void CGDrawContext::drawPoint (const CPoint &point, const CColor& color)
{
    saveGlobalState ();

    setLineWidth (1);
    setFrameColor (color);
    CPoint point2 (point);
    point2.x++;
    COffscreenContext::drawLine (point, point2);

    restoreGlobalState ();
}

			// gradient influence of vertex vx,vt to point y
			double vertice_gradient(int vx, int vy, double x, double y) const
			{
				// distance
				auto dx = x - vx;
				auto dy = y - vy;

				vx = (vx == W) ? 0 : vx;
				vy = (vy == H) ? 0 : vy;

				const gradient &g = m_matrix[point2(vx, vy)];
				return dx * g.x + dy * g.y;
			}

void drawTree2D(const std::vector<std::shared_ptr<Node<dim>>> &nodes, cv::Mat &image, Eigen::Vector3i colorNode,
                Eigen::Vector3i colorEdge, int thickness) {
    static_assert(dim == 2, "Dimension has to be 2");
    for (auto &elem : nodes) {
        cv::Point point(elem->getValue(0), elem->getValue(1));
        // cv::circle(image, point, 3, cv::Scalar(colorNode[0], colorNode[1], colorNode[2]), 1);
        if (elem->getParentNode() != nullptr) {
            cv::Point point2(elem->getParentNode()->getValue(0), elem->getParentNode()->getValue(1));
            cv::line(image, point, point2, cv::Scalar(colorEdge[0], colorEdge[1], colorEdge[2]), thickness);
        }
    }
}

int main(int argc, char **argv) {
  std::cout
      << "¸.·´¯`·.¸¸.·´ BioFractalTree Version 0 Revision 1 `·.¸¸.·´¯`·.¸\n";

  // set up a bcurve for testing early curve / surface viz algs
  vector<double> coords0 = { 0.0, 0.0, 0.0 };
  vector<double> coords1 = { 0.0, 0.3, 0.3 };
  vector<double> coords2 = { 0.3, 0.0, 0.6 };
  vector<double> coords3 = { 0.0, 0.0, 1.0 };

  std::unique_ptr<point> point0(new point(coords0, 0));
  std::unique_ptr<point> point1(new point(coords1, 1));
  std::unique_ptr<point> point2(new point(coords2, 2));
  std::unique_ptr<point> point3(new point(coords3, 3));

  vector<point> curve0 = { *point0, *point1, *point2, *point3 };
  std::unique_ptr<bcurve> thisCurve(new bcurve(curve0, 20));

  // we set up a static Bernstein basis set for a given resolution
  std::unique_ptr<bBasis> bBasis_20(new bBasis(20));
  // this sets up a 4x21 vector of vector<double>
  // 19 internal points plus the two endpoints

  // we can return / output the basis set
  vector<vector<double> > returnBasis;
  bBasis_20->getBasis(returnBasis);
  //    std::cout << "Basis Set Output: \n";
  //    std::cout << "  size : " << returnBasis.at(0).size();
  for (int j = 0; j < 4; j++) {
    //        std::cout << "\n  b[" << j << "] : ";
    for (int i = 0; i < returnBasis.at(j).size(); i++) {
      //    std::cout << returnBasis.at(j).at(i) << " ";
    }
  }
  //    std::cout << "\n";

  // now we can try getting some points on the bcurve
  vector<point> returnPoints;
  thisCurve->getPointsOnCurve(returnPoints, returnBasis, 4);
  // only returnPoints gets modified by the above function
  // it returns 21 points along the curve, including endpoints
  std::cout << "points on curve : \n";
  for (int i = 0; i < returnPoints.size(); i++) {
    vector<double> tempCoord;
    returnPoints.at(i).getCoord(tempCoord);
    for (int j = 0; j < 3; j++) {
      std::cout << tempCoord.at(j) << " ";
    }
    std::cout << "\n";
  }

  return 0;
}

// TODO: Uncomment and fix, see issue #339
TEST(PathConstructor, TestConstructPath) {
    /* origin of OccupancyGrid */
    // initialize origin of occupancy grid
    geometry_msgs::Pose origin =
    PathFinderTestUtils::constructPose(3.0, 3.0, 0.0);

    /* mapMetaData of OccupancyGrid */
    // initialize mapMetaData
    nav_msgs::MapMetaData map_meta_data;
    map_meta_data.resolution = 2.0;
    map_meta_data.width      = 2;
    map_meta_data.height     = 2;
    // add origin to mapMetaData
    map_meta_data.origin = origin;

    /* OccupancyGridAdapter */
    std::shared_ptr<OccupancyGridAdapter> occupancy_grid_adapter_ptr(
    new OccupancyGridAdapter(map_meta_data));

    /* first point in path*/
    AStar::GridPoint point1(-99 - sqrt(3), -99 - 1);

    /* second point in path*/
    AStar::GridPoint point2(-99, -99);

    /* add points to path */
    std::stack<AStar::GridPoint> points;
    points.push(point1);
    points.push(point2);

    PathConstructor path_constructor(occupancy_grid_adapter_ptr);
    nav_msgs::Path path = path_constructor.constructPath(points);

    tf::Quaternion q1;
    tf::quaternionMsgToTF(path.poses[0].pose.orientation, q1);

    /* we lose resolution by converting a point into a grid, so allow more
    error
     */
    EXPECT_NEAR(q1.getAngle(), M_PI + M_PI / 6, 0.5);
    EXPECT_FLOAT_EQ(
    path.poses[0].pose.position.x,
    occupancy_grid_adapter_ptr->convertFromGridToMapPoint(point2).x);
    EXPECT_FLOAT_EQ(
    path.poses[0].pose.position.y,
    occupancy_grid_adapter_ptr->convertFromGridToMapPoint(point2).y);
    EXPECT_FLOAT_EQ(
    path.poses[1].pose.position.x,
    occupancy_grid_adapter_ptr->convertFromGridToMapPoint(point1).x);
    EXPECT_FLOAT_EQ(
    path.poses[1].pose.position.y,
    occupancy_grid_adapter_ptr->convertFromGridToMapPoint(point1).y);
}

void AxisAlignedBoxTest::collisionPoint() {
    Physics::AxisAlignedBox3D box({-1.0f, -2.0f, -3.0f}, {1.0f, 2.0f, 3.0f});
    Physics::Point3D point1({-1.5f, -1.0f, 2.0f});
    Physics::Point3D point2({0.5f, 1.0f, -2.5f});

    randomTransformation(box);
    randomTransformation(point1);
    randomTransformation(point2);

    VERIFY_NOT_COLLIDES(box, point1);
    VERIFY_COLLIDES(box, point2);
}

void SphereTest::collisionPoint() {
    Physics::Sphere3D sphere({1.0f, 2.0f, 3.0f}, 2.0f);
    Physics::Point3D point({1.0f, 3.0f, 3.0f});
    Physics::Point3D point2({1.0f, 3.0f, 1.0f});

    randomTransformation(sphere);
    randomTransformation(point);
    randomTransformation(point2);

    VERIFY_COLLIDES(sphere, point);
    VERIFY_NOT_COLLIDES(sphere, point2);
}

Line CohenShutterlandAlgorithm::getLineInView(Line line,Point viewPortStart,Point viewPortEnd) {
	
	Point linePosition = line.getPosition();
	int x0 = line.getBeginPoint().x + linePosition.x;
	int y0 = line.getBeginPoint().y + linePosition.y;
	int x1 = line.getEndPoint().x + linePosition.x;
	int y1 = line.getEndPoint().y + linePosition.y;

	Point point1(x0,y0);
	Point point2(x1,y1);

	
	int outcode1 = computeOutcode(point1,viewPortStart,viewPortEnd);
	int outcode2 = computeOutcode(point2,viewPortStart,viewPortEnd);
	while(true) {

		int outcodeOut = outcode1 ? outcode1 : outcode2;
		if (!(outcode1|outcode2)){
			return Line(Point(x0,y0),Point(x1,y1),line.getColor());	
		}	
		else if (outcode1 & outcode2){
			return Line(Point(0,0),Point(0,0),Color(0,0,0,0));
		}
		else {
			int x,y;
			if ( outcodeOut & TOP ) {
				x = x0 + (x1-x0) * (viewPortStart.y - y0) / (y1-y0) ;
				y = viewPortStart.y;
			} else if (outcodeOut & BOTTOM) {
				x = x0 + (x1-x0) * (viewPortEnd.y - y0) / (y1-y0);
				y = viewPortEnd.y;
			} else if (outcodeOut & LEFT ) {
				y = y0 + (y1-y0) * (viewPortStart.x - x0) / (x1-x0);
				x = viewPortStart.x;
			} else if (outcodeOut & RIGHT) {
				y = y0 + (y1-y0) * (viewPortEnd.x - x0 ) / (x1-x0);
				x = viewPortEnd.x;
			}
			
			if (outcodeOut == outcode1) {
				x0 = x;
				y0 = y;
				outcode1 = computeOutcode(Point(x0,y0),viewPortStart,viewPortEnd);
			}
			else {
				x1 = x;
				y1 = y;
				outcode2 = computeOutcode(Point(x1,y1),viewPortStart,viewPortEnd);
			}
		}
	}
}

int main()
{
    bg::model::point<double, 2, bg::cs::cartesian> point1;
    bg::model::point<double, 3, bg::cs::cartesian> point2(1.0, 2.0, 3.0); /*< Construct, assigning three coordinates >*/
    point1.set<0>(1.0); /*< Set a coordinate. [*Note]: prefer using `bg::set<0>(point1, 1.0);` >*/
    point1.set<1>(2.0);

    double x = point1.get<0>(); /*< Get a coordinate. [*Note]: prefer using `x = bg::get<0>(point1);` >*/
    double y = point1.get<1>();

    std::cout << x << ", " << y << std::endl;
    return 0;
}

// Check that the base point scalar multiple routine matches the generic base point routine
void base_point_test(MinTLS_NamedCurve curve)
{
    size_t const key_sz = mintls_ecdh_scalar_size(curve);
    size_t const pt_sz  = mintls_ecdh_point_size(curve);

    // Scalar representing 1 (in big endian)
    std::vector<uint8_t> one(key_sz,0);
    one[key_sz-1] = 1;

    // Extract out the base point
    std::vector<uint8_t> base_point(pt_sz);
    ASSERT_EQ(
        mintls_ecdh_base_scalar_mult(
            curve,          // (I) Curve
            &one[0],        // (I) Private Key
            one.size(),     // (I) Private Key size
            &base_point[0]  // (O) Public Key
        ),
        0
    );

    // Computer scalar multiple in two ways
    std::vector<uint8_t> scalar(key_sz,0);
    mintls_random(&scalar[0], key_sz);

    std::vector<uint8_t> point1(pt_sz);
    std::vector<uint8_t> point2(pt_sz);
    ASSERT_EQ(
        mintls_ecdh_base_scalar_mult(
            curve,          // (I) Curve
            &scalar[0],     // (I) Private Key
            scalar.size(),  // (I) Private Key Size
            &point1[0]      // (O) Public Key
        ),
        0
    );

    ASSERT_EQ(
        mintls_ecdh_scalar_mult(
            curve,              // (I) Curve
            &scalar[0],         // (I) Scalar (big endian using [5] 4.3.3)
            scalar.size(),      // (I) Scalar size
            &base_point[0],     // (I) Base point (uncompressed using [5] 4.3.6)
            base_point.size(),  // (I) Base point size
            &point2[0]          // (O) Point (uncompressed using [5] 4.3.6)
        ),
        0
    );

    EXPECT_EQ(point1,point2);
}

        void testOperatorMultiply()
        {
            FTPoint point1(1.0f, 2.0f, 3.0f);
            FTPoint point2(1.0f, 2.0f, 3.0f);

            FTPoint point3(2.0f, 4.0f, 6.0f);
            FTPoint point4 = point1 * 2.0;

            CPPUNIT_ASSERT(point4 == point3);

            point4 = 2.0 * point2;

            CPPUNIT_ASSERT(point4 == point3);
        }