void WindowHermite::paintGL()
{
    glClear(GL_COLOR_BUFFER_BIT);

    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();

    hermite(point3(-1,0,0), point3(1,0,0), point3(4,4,0), point3(4,4,0));

    glFlush();
}

// Plane class for frustum planes (or any plane...)
Plane::Plane(vec3 normal, point3 point) {
	this->normal = normal != NULL ? normal : vec3(0, 0, 0);
	this->point = point != NULL ? point : point3(0, 0, 0);
	this->d = NULL;
	this->computeParameters();

}

void MyPrimitive::GenerateTube(float a_fOuterRadius, float a_fInnerRadius, float a_fHeight, int a_nSubdivisions, vector3 a_v3Color)
{
	if (a_nSubdivisions < 3)
		a_nSubdivisions = 3;
	if (a_nSubdivisions > 360)
		a_nSubdivisions = 360;

	Release();
	Init();

	for (int i = 0; i < a_nSubdivisions; i++)
	{
		float ang = (2 * PI) / a_nSubdivisions;

		vector3 point0(a_fOuterRadius*cos((i + 1)*ang), a_fHeight, a_fOuterRadius*sin((i + 1)*ang)); //1
		vector3 point1(a_fOuterRadius*cos(i*ang), 0, a_fOuterRadius*sin(i*ang)); //2
		vector3 point2(a_fOuterRadius*cos(i*ang), a_fHeight, a_fOuterRadius*sin(i*ang)); //0
		vector3 point3(a_fOuterRadius*cos((i + 1)*ang), 0, a_fOuterRadius*sin((i + 1)*ang)); //3

		vector3 point4(a_fInnerRadius*cos((i + 1)*ang), a_fHeight, a_fInnerRadius*sin((i + 1)*ang)); //1
		vector3 point5(a_fInnerRadius*cos(i*ang), 0, a_fInnerRadius*sin(i*ang)); //2
		vector3 point6(a_fInnerRadius*cos(i*ang), a_fHeight, a_fInnerRadius*sin(i*ang)); //0
		vector3 point7(a_fInnerRadius*cos((i + 1)*ang), 0, a_fInnerRadius*sin((i + 1)*ang)); //3

		AddQuad(point4, point0, point6, point2); //Top
		AddQuad(point0, point3, point2, point1); //Outer
		AddQuad(point7, point5, point3, point1); //Bottom
		AddQuad(point5, point7, point6,point4 ); // Inner
		
	}

	//Your code ends here
	CompileObject(a_v3Color);
}

point3 point3::cross(const point3 &u, const point3 &v)
{
	double x = u.y*v.z - u.z*v.y;
	double y = u.z*v.x - u.x*v.z;
	double z = u.x*v.y - u.y*v.x;
	return point3(x, y, z);
}

BaseIF* makeVane(const Real&     thick,
                 const RealVect& normal,
                 const Real&     innerRadius,
                 const Real&     outerRadius,
                 const Real&     offset,
                 const Real&     height)
{
  RealVect zero(D_DECL(0.0,0.0,0.0));
  RealVect xAxis(D_DECL(1.0,0.0,0.0));
  bool inside = true;

  Vector<BaseIF*> vaneParts;

  // Each side of the vane (infinite)
  RealVect normal1(normal);
  RealVect point1(D_DECL(offset+height/2.0,-thick/2.0,0.0));
  PlaneIF plane1(normal1,point1,inside);

  vaneParts.push_back(&plane1);

  RealVect normal2(-normal);
  RealVect point2(D_DECL(offset+height/2.0,thick/2.0,0.0));
  PlaneIF plane2(normal2,point2,inside);

  vaneParts.push_back(&plane2);

  // Make sure we only get something to the right of the origin
  RealVect normal3(D_DECL(0.0,0.0,1.0));
  RealVect point3(D_DECL(0.0,0.0,0.0));
  PlaneIF plane3(normal3,point3,inside);

  vaneParts.push_back(&plane3);

  // Cut off the top and bottom
  RealVect normal4(D_DECL(1.0,0.0,0.0));
  RealVect point4(D_DECL(offset,0.0,0.0));
  PlaneIF plane4(normal4,point4,inside);

  vaneParts.push_back(&plane4);

  RealVect normal5(D_DECL(-1.0,0.0,0.0));
  RealVect point5(D_DECL(offset+height,0.0,0.0));
  PlaneIF plane5(normal5,point5,inside);

  vaneParts.push_back(&plane5);

  // The outside of the inner cylinder
  TiltedCylinderIF inner(innerRadius,xAxis,zero,!inside);

  vaneParts.push_back(&inner);

  // The inside of the outer cylinder
  TiltedCylinderIF outer(outerRadius,xAxis,zero,inside);

  vaneParts.push_back(&outer);

  IntersectionIF* vane = new IntersectionIF(vaneParts);

  return vane;
}

Grid::Grid(Mesh& m, float size) : mesh(m), voxelSize(size)
{
	float minX = FLT_MAX, maxX = FLT_MIN, minY = FLT_MAX, maxY = FLT_MIN, minZ = FLT_MAX, maxZ = FLT_MIN;
	for (int i = 0; i < m.points.size(); ++i) {
		minX = std::fmin(minX, m.points.at(i).x);
		maxX = std::fmax(maxX, m.points.at(i).x);
		minY = std::fmin(minY, m.points.at(i).y);
		maxY = std::fmax(maxY, m.points.at(i).y);
		minZ = std::fmin(minZ, m.points.at(i).z);
		maxZ = std::fmax(maxZ, m.points.at(i).z);
	}
	float coef = 1 / voxelSize;

	nbX = (std::abs(maxX - minX))*coef + 1;
	nbY = (std::abs(maxY - minY))*coef + 1;
	nbZ = (std::abs(maxZ - minZ))*coef + 1;

	base = point3(minX, minY, minZ);

	float dif = voxelSize * 0.5;

	for (float i = 0; i < nbX; ++i) {
		for (float j = 0; j < nbY; ++j) {
			for (float k = 0; k < nbZ; ++k) {
				for (int o = 0; o < m.points.size(); ++o) {
					int x = (m.points.at(o).x - base.x) * coef;
					int y = (m.points.at(o).y - base.z) * coef;
					int z = (m.points.at(o).z - base.z) * coef;
					voxels[x*nbX*nbY + y*nbY + z].push_back(o);
				}
			}
		}
	}
}

void MyPrimitive::GenerateTorus(float a_fOuterRadius, float a_fInnerRadius, int a_nSubdivisionsA, int a_nSubdivisionsB, vector3 a_v3Color)
{
	if (a_fOuterRadius <= a_fInnerRadius + 0.1f)
		return;

	if (a_nSubdivisionsA < 3)
		a_nSubdivisionsA = 3;
	if (a_nSubdivisionsA > 25)
		a_nSubdivisionsA = 25;

	if (a_nSubdivisionsB < 3)
		a_nSubdivisionsB = 3;
	if (a_nSubdivisionsB > 25)
		a_nSubdivisionsB = 25;

	Release();
	Init();

	//Your code starts here
	float fValue = 0.5f;
	//3--2
	//|  |
	//0--1
	vector3 point0(-fValue, -fValue, fValue); //0
	vector3 point1(fValue, -fValue, fValue); //1
	vector3 point2(fValue, fValue, fValue); //2
	vector3 point3(-fValue, fValue, fValue); //3

	AddQuad(point0, point1, point3, point2);

	//Your code ends here
	CompileObject(a_v3Color);
}

void NormalQuantifier::build (UInt32 numberSubdivisions)
{
    UInt32 index = 0;
    UInt32 nN    = ((1 << (2 * numberSubdivisions)) * 8);
    
    _numberSubdivisions = numberSubdivisions;

    _normalTable.resize(nN);
  
    if(_numberSubdivisions != 0) 
    {
        for(UInt32 octant = 0; octant < 8; octant++) 
        {
            Int32 xoctant = (octant & 4)>0?-1:1;
            Int32 yoctant = (octant & 2)>0?-1:1;
            Int32 zoctant = (octant & 1)>0?-1:1;
            
            Vec3f point1(0.f * xoctant, 0.f * yoctant, 1.f * zoctant);
            Vec3f point2(1.f * xoctant, 0.f * yoctant, 0.f * zoctant);
            Vec3f point3(0.f * xoctant, 1.f * yoctant, 0.f * zoctant);
            
            subdivide(point1, point2, point3, _numberSubdivisions+1, index);
        }
        
        if(index != nN)
        {
            FFATAL(("NormalQuantifier::build() index missmatch!\n"));
        }
        else
        {
            FLOG(("NormalQuantifier init: %d subdivision, %d normal\n",
                  _numberSubdivisions, _normalTable.size()));
        }
    }  
}

void Rectangle3DOverlay::render(RenderArgs* args) {
    if (!_visible) {
        return; // do nothing if we're not visible
    }

    float alpha = getAlpha();
    xColor color = getColor();
    const float MAX_COLOR = 255.0f;
    glm::vec4 rectangleColor(color.red / MAX_COLOR, color.green / MAX_COLOR, color.blue / MAX_COLOR, alpha);

    glm::vec3 position = getPosition();
    glm::vec2 dimensions = getDimensions();
    glm::vec2 halfDimensions = dimensions * 0.5f;
    glm::quat rotation = getRotation();

    auto batch = args->_batch;

    if (batch) {
        Transform transform;
        transform.setTranslation(position);
        transform.setRotation(rotation);

        batch->setModelTransform(transform);
        auto geometryCache = DependencyManager::get<GeometryCache>();

        if (getIsSolid()) {
            glm::vec3 topLeft(-halfDimensions.x, -halfDimensions.y, 0.0f);
            glm::vec3 bottomRight(halfDimensions.x, halfDimensions.y, 0.0f);
            geometryCache->bindSimpleProgram(*batch);
            geometryCache->renderQuad(*batch, topLeft, bottomRight, rectangleColor);
        } else {
            geometryCache->bindSimpleProgram(*batch, false, false, false, true, true);
            if (getIsDashedLine()) {
                glm::vec3 point1(-halfDimensions.x, -halfDimensions.y, 0.0f);
                glm::vec3 point2(halfDimensions.x, -halfDimensions.y, 0.0f);
                glm::vec3 point3(halfDimensions.x, halfDimensions.y, 0.0f);
                glm::vec3 point4(-halfDimensions.x, halfDimensions.y, 0.0f);

                geometryCache->renderDashedLine(*batch, point1, point2, rectangleColor);
                geometryCache->renderDashedLine(*batch, point2, point3, rectangleColor);
                geometryCache->renderDashedLine(*batch, point3, point4, rectangleColor);
                geometryCache->renderDashedLine(*batch, point4, point1, rectangleColor);
            } else {
                if (halfDimensions != _previousHalfDimensions) {
                    QVector<glm::vec3> border;
                    border << glm::vec3(-halfDimensions.x, -halfDimensions.y, 0.0f);
                    border << glm::vec3(halfDimensions.x, -halfDimensions.y, 0.0f);
                    border << glm::vec3(halfDimensions.x, halfDimensions.y, 0.0f);
                    border << glm::vec3(-halfDimensions.x, halfDimensions.y, 0.0f);
                    border << glm::vec3(-halfDimensions.x, -halfDimensions.y, 0.0f);
                    geometryCache->updateVertices(_geometryCacheID, border, rectangleColor);

                    _previousHalfDimensions = halfDimensions;
                }
                geometryCache->renderVertices(*batch, gpu::LINE_STRIP, _geometryCacheID);
            }
        }
    }
}

    plane(double _a,double _b,double _c,double _d)
    {
    	//ax+by+cz+d=0
    	o=point3(_a,_b,_c);
		if (dblcmp(_a)!=0)
		{
			a=point3((-_d-_c-_b)/_a,1,1);
		}
		else if (dblcmp(_b)!=0)
		{
			a=point3(1,(-_d-_c-_a)/_b,1);
		}
		else if (dblcmp(_c)!=0)
		{
			a=point3(1,1,(-_d-_a-_b)/_c);
		}
    }

void Checkbox::RenderCheck(Rect& rectArea)
{
    Rect rectBox = rectArea;

    int iBoxMargin = int(rectArea.Height() * 0.2);
    int iBoxMarginRight = int(rectArea.Height() * 0.8 * 0.75);

    rectBox.Left(rectBox.Left() + iBoxMargin);
    rectBox.Right(rectBox.Left() + iBoxMarginRight);
    rectBox.Top(rectBox.Top() + iBoxMargin);
    rectBox.Bottom(rectBox.Bottom() - iBoxMargin);

    OpenGL::PushedAttributes attr(GL_ENABLE_BIT | GL_LINE_BIT | GL_POINT_BIT);

    glDisable(GL_TEXTURE_2D);
    glEnable(GL_LINE_SMOOTH);
    glEnable(GL_POINT_SMOOTH);

    Color cBox = GetAttrAsColor(CheckboxAttr::BoxColor);
    glColor4ubv(cBox.m_color);
    glLineWidth(2.0);
    glBegin(GL_LINE_LOOP);
    glVertex2i(rectBox.Left(), rectBox.Bottom());
    glVertex2i(rectBox.Right(), rectBox.Bottom());
    glVertex2i(rectBox.Right(), rectBox.Top());
    glVertex2i(rectBox.Left(), rectBox.Top());
    glEnd();

    if (IsChecked())
    {
        double iCheckUnit = rectBox.Height() * 0.1;

        Point point1(int(rectBox.Left() + 3 * iCheckUnit), int(rectBox.Bottom() - 6 * iCheckUnit));
        Point point2(int(rectBox.Left() + 6 * iCheckUnit), int(rectBox.Bottom() - 3 * iCheckUnit));
        Point point3(int(rectBox.Left() + 11* iCheckUnit), int(rectBox.Bottom() - 11* iCheckUnit));

        Color cCheck = GetAttrAsColor(CheckboxAttr::CheckColor);
        glColor4ubv(cCheck.m_color);
        glLineWidth(4.0);
        glBegin(GL_LINES);
        glVertex2i(point1.X(), point1.Y());
        glVertex2i(point2.X(), point2.Y());
        glVertex2i(point2.X(), point2.Y());
        glVertex2i(point3.X(), point3.Y());
        glEnd();

        glPointSize(3.0);
        glBegin(GL_POINTS);
        glVertex2i(point1.X(), point1.Y());
        glVertex2i(point2.X(), point2.Y());
        glVertex2i(point3.X(), point3.Y());
        glEnd();
    }

    // adjust rect for size of checkbox
    int iOffset = int(rectArea.Height() * 1.1);
    rectArea.Left(rectArea.Left() + iOffset);
}

	// out: optimized model-shape
	// in: GRAY img
	// in: evtl zusaetzlicher param um scale-level/iter auszuwaehlen
	// calculates shape updates (deltaShape) for one or more iter/scales and returns...
	// assume we get a col-vec.
	cv::Mat optimize(cv::Mat modelShape, cv::Mat image) {

		for (int cascadeStep = 0; cascadeStep < model.getNumCascadeSteps(); ++cascadeStep) {
			//feature_current = obtain_features(double(TestImg), New_Shape, 'HOG', hogScale);

			vector<cv::Point2f> points;
			for (int i = 0; i < model.getNumLandmarks(); ++i) { // in case of HOG, need integers?
				points.emplace_back(cv::Point2f(modelShape.at<float>(i), modelShape.at<float>(i + model.getNumLandmarks())));
			}
			Mat currentFeatures;
			double dynamicFaceSizeDistance = 0.0;
			if (true) { // adaptive
				// dynamic face-size:
				cv::Vec2f point1(modelShape.at<float>(8), modelShape.at<float>(8 + model.getNumLandmarks())); // reye_ic
				cv::Vec2f point2(modelShape.at<float>(9), modelShape.at<float>(9 + model.getNumLandmarks())); // leye_ic
				cv::Vec2f anchor1 = (point1 + point2) / 2.0f;
				cv::Vec2f point3(modelShape.at<float>(11), modelShape.at<float>(11 + model.getNumLandmarks())); // rmouth_oc
				cv::Vec2f point4(modelShape.at<float>(12), modelShape.at<float>(12 + model.getNumLandmarks())); // lmouth_oc
				cv::Vec2f anchor2 = (point3 + point4) / 2.0f;
				// dynamic window-size:
				// From the paper: patch size $ S_p(d) $ of the d-th regressor is $ S_p(d) = S_f / ( K * (1 + e^(d-D)) ) $
				// D = numCascades (e.g. D=5, d goes from 1 to 5 (Matlab convention))
				// K = fixed value for shrinking
				// S_f = the size of the face estimated from the previous updated shape s^(d-1).
				// For S_f, can use the IED, EMD, or max(IED, EMD). We use the EMD.
				dynamicFaceSizeDistance = cv::norm(anchor1 - anchor2);
				double windowSize = dynamicFaceSizeDistance / 2.0; // shrink value
				double windowSizeHalf = windowSize / 2.0;
				windowSizeHalf = std::round(windowSizeHalf * (1 / (1 + exp((cascadeStep + 1) - model.getNumCascadeSteps())))); // this is (step - numStages), numStages is 5 and step goes from 1 to 5. Because our step goes from 0 to 4, we add 1.
				int NUM_CELL = 3; // think about if this should go in the descriptorExtractor or not. Is it Hog specific?
				int windowSizeHalfi = static_cast<int>(windowSizeHalf) + NUM_CELL - (static_cast<int>(windowSizeHalf) % NUM_CELL); // make sure it's divisible by 3. However, this is not needed and not a good way
				
				currentFeatures = model.getDescriptorExtractor(cascadeStep)->getDescriptors(image, points, windowSizeHalfi);
			}
			else { // non-adaptive, the descriptorExtractor has all necessary params
				currentFeatures = model.getDescriptorExtractor(cascadeStep)->getDescriptors(image, points);
			}
			currentFeatures = currentFeatures.reshape(0, currentFeatures.cols * model.getNumLandmarks()).t();

			//delta_shape = AAM.RF(1).Regressor(hogScale).A(1:end - 1, : )' * feature_current + AAM.RF(1).Regressor(hogScale).A(end,:)';
			Mat regressorData = model.getRegressorData(cascadeStep);
			//Mat deltaShape = regressorData.rowRange(0, regressorData.rows - 1).t() * currentFeatures + regressorData.row(regressorData.rows - 1).t();
			Mat deltaShape = currentFeatures * regressorData.rowRange(0, regressorData.rows - 1) + regressorData.row(regressorData.rows - 1);
			if (true) { // adaptive
				modelShape = modelShape + deltaShape.t() * dynamicFaceSizeDistance;
			}
			else {
				modelShape = modelShape + deltaShape.t();
			}
			
			/*
			for (int i = 0; i < m.getNumLandmarks(); ++i) {
			cv::circle(landmarksImage, Point2f(modelShape.at<float>(i, 0), modelShape.at<float>(i + m.getNumLandmarks(), 0)), 6 - hogScale, Scalar(51.0f*(float)hogScale, 51.0f*(float)hogScale, 0.0f));
			}*/
		}

		return modelShape;
	};

    void TestSameChastePoints()
    {
        ChastePoint<3> point1(4,5,6);
        ChastePoint<3> point2(4,5,6);
        ChastePoint<3> point3(12,5,6);

        TS_ASSERT(point1.IsSamePoint(point2));
        TS_ASSERT(!point1.IsSamePoint(point3));
    }

/* initialisation d'OpenGL*/
static void init()
{
	glClearColor(0.0, 0.0, 0.0, 0.0);
	
	// Initialisation des points de contrôles
	// On choisit de les intialiser selon une ligne
	/*for (int i = 0; i < Ordre; i++)
	{
     TabPC[i] = point3(i,i,i);
    }//*/
 
	TabPC[0] = point3(-3,-2,0);
	TabPC[1] = point3(-2,1,0);
	TabPC[2] = point3(0,1.5,0);
	TabPC[3] = point3(1,0,0);
	TabPC[4] = point3(-1, -1, 0);

}

point3 matrix4::GetAxis( int axis ) const
{
	return point3
	(
		m[axis][0],
		m[axis][1],
		m[axis][2]
	);
}

        void testOperatorNotEqual()
        {
            FTPoint point1(1.0f, 2.0f, 3.0f);
            FTPoint point2(1.0f, 2.0f, 3.0f);
            FTPoint point3(-1.0f, 2.3f, 23.0f);

            CPPUNIT_ASSERT(!(point1 != point1));
            CPPUNIT_ASSERT(!(point1 != point2));
            CPPUNIT_ASSERT(point1 != point3);
        }

        void testOperatorPlus()
        {
            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 + point2;

            CPPUNIT_ASSERT(point4 == point3);
        }

        void testOperatorPlusEquals()
        {
            FTPoint point1(1.0f, 2.0f, 3.0f);
            FTPoint point2(-2.0f, 21.0f, 0.0f);
            FTPoint point3(-1.0f, 23.0f, 3.0f);

            point1 += point2;

            CPPUNIT_ASSERT(point1 == point3);
        }

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