示例#1
0
TEST(test5, tests)
{
   cg::point_2 p11(0, 0);
   cg::point_2 p12(10, 0);
   cg::point_2 p13(10, 10);
   cg::point_2 p2(-10, -10);
   cg::triangle_2d tr(p11, p12, p13);
   ASSERT_EQ(point_inside_triangle(tr, p2), false);
}
示例#2
0
TEST(test8, tests)
{
   cg::point_2 p11(10, 10);
   cg::point_2 p12(0, 0);
   cg::point_2 p13(20, 5);
   cg::point_2 p2(6, 5);
   cg::triangle_2d tr(p11, p12, p13);
   ASSERT_EQ(point_inside_triangle(tr, p2), true);
}
TEST(test7, tests)
{
   cg::point_2 p11(20, 10);
   cg::point_2 p12(10, 20);
   cg::point_2 p13(20, 00);
   cg::point_2 p21(0, 100);
   cg::point_2 p22(40, 20);
   cg::triangle_2d tr(p11, p12, p13);
   cg::segment_2d ss(p21, p22);
   ASSERT_EQ(triangle_segment_intersect(tr, ss), false);
}
TEST(test1, tests)
{
   cg::point_2 p11(0, 0);
   cg::point_2 p12(10, 0);
   cg::point_2 p13(10, 10);
   cg::point_2 p21(3, 3);
   cg::point_2 p22(3, 100);
   cg::triangle_2d tr(p11, p12, p13);
   cg::segment_2d ss(p21, p22);
   ASSERT_EQ(triangle_segment_intersect(tr, ss), true);
}
示例#5
0
文件: Paint2.cpp 项目: kyushu-pc/kpc
int WINAPI WinMain(HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nCmdShow) {
	ChangeWindowMode(TRUE);
	if (DxLib_Init() == -1)
		return -1;

	p13();


	DxLib_End();
	return 0;
}
示例#6
0
int main()
{
	int i,j,f;
	printf("判断R是否为等价关系\n");
	printf("输入元素为1或0的二维数组:");
	for(i=0;i<N;i++)
		for(j=0;j<N;j++)
			scanf("%d",&M[i][j]);
		for(i=0;i<N;i++)
		{
			for(j=0;j<N;j++)
				printf("%4d",M[i][j]);
			printf("\n");
		}
		f=p13(M);
		if(f == 0)
		{
			printf("关系R不是等价关系\n");
			goto end;
		}
		printf("关系R是等价关系\n");
end:
		return 0;
}
std::list<WCIntersectionResult> __WILDCAT_NAMESPACE__::GeometricIntersection(WCGeometricLine *left,
	WCGeometricLine *right, const WPFloat &tol, unsigned const int &flags) {
	std::list<WCIntersectionResult> results;
	//Check if self intersecting
	if (left == right) return results;

	//Evaluate both lines at both ends
	WCVector4 p1 = left->Begin();
	WCVector4 p2 = left->End();
	WCVector4 p3 = right->Begin();
	WCVector4 p4 = right->End();
	
	//Get length vectors
	WCVector4 p21(p2 - p1);
	WCVector4 p43(p4 - p3);
	WPFloat dist;
	
	//Check for parallel
	WCVector4 cross = p43.CrossProduct(p21);
	WPFloat denom = cross.Magnitude();
	WPFloat onePlusTol = 1.0 + tol;
	if (denom < 0.001) {
		//Determine perpendicular distance
		WPFloat p21mag = p21.Magnitude();
		WCVector4 p13(p1 - p3);
		WCVector4 num = p21.CrossProduct(p13);
		dist = num.Magnitude() / p21mag;
		//If outside of tolerance, no intersection
		if (dist <= tol) {
			//Project all four points onto opposite lines
			p21 /= p21mag;
			WPFloat p43mag = p43.Magnitude();
			p43 /= p43mag;
			WCVector4 p31(p3 - p1);
			WCVector4 p41(p4 - p1);
			WCVector4 p23(p2 - p3);
			WPFloat p1Proj = p43.DotProduct(p13) / p43mag;
			WPFloat p2Proj = p43.DotProduct(p23) / p43mag;
			WPFloat p3Proj = p21.DotProduct(p31) / p21mag;
			WPFloat p4Proj = p21.DotProduct(p41) / p21mag;

			//See if each point is in 0.0 to 1.0 +- tolerance
			bool p1In = (p1Proj > -tol) && (p1Proj < onePlusTol);
			bool p2In = (p2Proj > -tol) && (p2Proj < onePlusTol);
			bool p3In = (p3Proj > -tol) && (p3Proj < onePlusTol);
			bool p4In = (p4Proj > -tol) && (p4Proj < onePlusTol);
			//If none are in then no overlap, return
			if ( p1In || p2In || p3In || p4In) {
				//Bounds check all of the projections
				p1Proj = STDMAX(0.0, STDMIN(1.0, p1Proj));
				p2Proj = STDMAX(0.0, STDMIN(1.0, p2Proj));
				p3Proj = STDMAX(0.0, STDMIN(1.0, p3Proj));
				p4Proj = STDMAX(0.0, STDMIN(1.0, p4Proj));
				//Create hit result
				WCIntersectionResult hit;
				hit.type = IntersectLine;
				WCVector4 pStart, pEnd;
				//Case 1 - Left is completely within Right
				if (p1In && p2In) {
					//Set the start and end points
					pStart = p1;
					pEnd = p2;
					//Set the param values
					hit.leftParam.I( 0.0 );
					hit.leftParam.J( 1.0 );
					hit.rightParam.I( STDMIN(p1Proj,p2Proj) );
					hit.rightParam.J( STDMAX(p1Proj,p2Proj) );
					//Set boundary values
					hit.leftBoundary = true;
					hit.rightBoundary = false;
				}
				//Case 2 - Right is completely within Left
				else if (p3In && p4In) {
					//Set the start and end points
					pStart = p3;
					pEnd = p4;
					//Set the param values
					hit.leftParam.I( STDMIN(p3Proj,p4Proj) );
					hit.leftParam.J( STDMAX(p3Proj,p4Proj) );
					hit.rightParam.I( 0.0 );
					hit.rightParam.J( 1.0 );
					//Set boundary values
					hit.leftBoundary = false;
					hit.rightBoundary = true;
				}
				//Remaining Cases
				else {
					//Simple sets
					if (p1In) { hit.leftParam.I(0.0); pStart = p1; }
					if (p2In) { hit.leftParam.J(1.0); pStart = p2; }
					if (p3In) { hit.rightParam.I(0.0); pEnd = p3; }
					if (p4In) { hit.rightParam.J(1.0); pEnd = p4; }
					//Double sets
					if (p1In && p3In) { hit.leftParam.J(p3Proj); hit.rightParam.J(p1Proj); }
					if (p1In && p4In) { hit.leftParam.J(p4Proj); hit.rightParam.I(p1Proj); }
					if (p2In && p3In) { hit.leftParam.I(p3Proj); hit.rightParam.J(p2Proj); }
					if (p2In && p4In) { hit.leftParam.I(p4Proj); hit.rightParam.I(p2Proj); }
					
					//Check to see if intersection is a point (think of two lines end to end)
					if (pStart.Distance(pEnd) < tol) hit.type = IntersectPoint;

					//This is not the best way to check for CULL BOUNDARY, but if pStart and pEnd distance < tol, must be end-to-end
					if ((flags & INTERSECT_CULL_BOUNDARY) && (hit.type == IntersectPoint))
						return results;

					//Set boundary values
					hit.leftBoundary = true;
					hit.rightBoundary = true;
				}
				//Overlap forces boundarys to false

				//See if genObj
				if ((hit.type == IntersectPoint) && (flags & INTERSECT_GEN_POINTS)) {
					//Create new point
					WCGeometricPoint *newPoint = new WCGeometricPoint(pStart);
					//Add to result struct
					hit.object = newPoint;					
				}
				else if ((hit.type == IntersectLine) && (flags & INTERSECT_GEN_LINES)) {
					//Create new line
					WCGeometricLine *newLine = new WCGeometricLine(pStart, pEnd);
					//Add to result struct
					hit.object = newLine;
				}
				else hit.object = NULL;
				//Add the hit to the list
				results.push_back(hit);
			}
		}
	}
	//Non-Parallel Case
	else {
		//Many dot products
		WPFloat d121 = p1.DotProduct(p21);			// L
		WPFloat d2121 = p21.DotProduct(p21);		// M
		WPFloat d321 = p3.DotProduct(p21);			// N
		WPFloat d4321 = p43.DotProduct(p21);		// O
		WPFloat d143 = p1.DotProduct(p43);			// Q
		WPFloat d343 = p3.DotProduct(p43);			// R
		WPFloat d4343 = p43.DotProduct(p43);		// S
		denom = (d4343 * d2121) - (d4321 * d4321);
		//What does this correspond to?
		if (denom == 0.0) {
			CLOGGER_WARN(WCLogManager::RootLogger(), "GeometricIntersection::LineLine -  Denominator == 0.0.");
		}
		//Otherwise
		else {
			//Calculate parametric intersection values
			WPFloat numer = d4343 * (d321 - d121) + d4321 * (d143 - d343);
			WPFloat mua = numer / denom;
			WPFloat mub = (d143 + d4321 * mua - d343) / d4343;
			//Make sure mua and mub are (0.0 +- tol, 1.0 +- tol)
			if ( (mua < onePlusTol) && (mua > -tol) && (mub < onePlusTol) && (mub > -tol) ) {
				//Bound mua and mub [0, 1]
				mua = STDMAX(0.0, STDMIN(1.0, mua));
				mub = STDMAX(0.0, STDMIN(1.0, mub));
				//Calculate points and distance between them
				WCVector4 pointOnFirst = p1 + p21 * mua;
				WCVector4 pointOnSecond = p3 + p43 * mub;
				//Make sure lines are not > tol apart
				dist = pointOnFirst.Distance(pointOnSecond);
				if (dist < tol) {
					//Create intersection result
					WCIntersectionResult hit;
					hit.type = IntersectPoint;
					hit.leftParam.I(mua);
					hit.rightParam.I(mub);
					hit.leftBoundary = (fabs(mua) < tol) || (fabs(mua-1.0) < tol);
					hit.rightBoundary = (fabs(mub) < tol) || (fabs(mub-1.0) < tol);

					//Check for culling end-point intersections
					if (!(flags & INTERSECT_CULL_BOUNDARY) || (!hit.leftBoundary && !hit.rightBoundary)) {
						//See if genObj
						if (flags & INTERSECT_GEN_POINTS) {
							//Create new point
							WCGeometricPoint *newPoint = new WCGeometricPoint(pointOnFirst);
							//Add to result struct
							hit.object = newPoint;
						}
						else hit.object = NULL;
						//Add the intersection to the list
//						std::cout << hit << pointOnFirst << std::endl;
						results.push_back(hit);
					}
				}
			}
		}
	}
	//Return result
	return results;
}
TEST(FloatRectTest, SquaredDistanceToTest)
{

    //
    //  O--x
    //  |
    //  y
    //
    //     FloatRect.x()   FloatRect.maxX()
    //            |          |
    //        1   |    2     |  3
    //      ======+==========+======   --FloatRect.y()
    //        4   |    5(in) |  6
    //      ======+==========+======   --FloatRect.maxY()
    //        7   |    8     |  9
    //

    FloatRect r1(100, 100, 250, 150);

    // `1` case
    FloatPoint p1(80, 80);
    EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p1), 800.f);

    FloatPoint p2(-10, -10);
    EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p2), 24200.f);

    FloatPoint p3(80, -10);
    EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p3), 12500.f);

    // `2` case
    FloatPoint p4(110, 80);
    EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p4), 400.f);

    FloatPoint p5(150, 0);
    EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p5), 10000.f);

    FloatPoint p6(180, -10);
    EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p6), 12100.f);

    // `3` case
    FloatPoint p7(400, 80);
    EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p7), 2900.f);

    FloatPoint p8(360, -10);
    EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p8), 12200.f);

    // `4` case
    FloatPoint p9(80, 110);
    EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p9), 400.f);

    FloatPoint p10(-10, 180);
    EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p10), 12100.f);

    // `5`(& In) case
    FloatPoint p11(100, 100);
    EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p11), 0.f);

    FloatPoint p12(150, 100);
    EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p12), 0.f);

    FloatPoint p13(350, 100);
    EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p13), 0.f);

    FloatPoint p14(350, 150);
    EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p14), 0.f);

    FloatPoint p15(350, 250);
    EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p15), 0.f);

    FloatPoint p16(150, 250);
    EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p16), 0.f);

    FloatPoint p17(100, 250);
    EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p17), 0.f);

    FloatPoint p18(100, 150);
    EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p18), 0.f);

    FloatPoint p19(150, 150);
    EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p19), 0.f);

    // `6` case
    FloatPoint p20(380, 150);
    EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p20), 900.f);

    // `7` case
    FloatPoint p21(80, 280);
    EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p21), 1300.f);

    FloatPoint p22(-10, 300);
    EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p22), 14600.f);

    // `8` case
    FloatPoint p23(180, 300);
    EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p23), 2500.f);

    // `9` case
    FloatPoint p24(450, 450);
    EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p24), 50000.f);
}
示例#9
0
cv::Mat TestProjection::test(double userX, double userY, double userZ, 
        const char* filename) {

    //Coordinates of the projection in the real world
    /*cv::Point3f p11(-480, 735, -420);
    cv::Point3f p12(0, 935, 0);
    cv::Point3f p13(0, 220, 0);
    cv::Point3f p14(-480, 240, -420);
    Plane3d proj1(p11, p12, p13, p14);

    cv::Point3f p21(0, 935, 0);
    cv::Point3f p22(480, 735, -420);
    cv::Point3f p23(480, 240, -420);
    cv::Point3f p24(0, 220, 0);
    Plane3d proj2(p21, p22, p23, p24);*/

    cv::Point3f p11(-590, 725, -350);
    cv::Point3f p12(0, 955, 0);
    cv::Point3f p13(0, 200, 0);
    cv::Point3f p14(-590, 227, -350);
    Plane3d proj1(p11, p12, p13, p14);

    cv::Point3f p21(0, 955, 0);
    cv::Point3f p22(567, 755, -350);
    cv::Point3f p23(567, 227, -350);
    cv::Point3f p24(0, 200, 0);
    Plane3d proj2(p21, p22, p23, p24);

    std::vector<Plane3d> planes;
    planes.push_back(proj1);
    planes.push_back(proj2);

    Projection proj(planes);

    //    proj.print();

    //Create the user with the obtained projection coordinates
    User u(proj);

    //Update his position
    u.updatePosition(userX, userY, userZ);
    //    u.print();

    //Create the distorted-corrected plane pairs, using the projections
    //on the user's view plane
    //Plane 1
    //****************************************************************************************************
    Plane2d p1 = u.getProjectedPlanes().at(0).to2d();
    Plane2d p2(cv::Point2f(0, 0), cv::Point2f(480, 0), cv::Point2f(480, 540), cv::Point2f(0, 540));
//    Plane2d p2(cv::Point2f(0, 0), cv::Point2f(230, 0), cv::Point2f(230, 520), cv::Point2f(0, 520));
//    Plane2d p2(cv::Point2f(0, 0), cv::Point2f(270, 0), cv::Point2f(270, 405), cv::Point2f(0, 405));
    //****************************************************************************************************
    //Invert the plane y coordinates
    Plane2d inv1 = p1.yInverted();
    //Move it so that it's closer to the target plane
    cv::Vec2f dist = pjs::distance(inv1, p2);
    Plane2d pp1(cv::Point2f(inv1.getPoint(0).x - dist[0], inv1.getPoint(0).y - dist[1]),
            cv::Point2f(inv1.getPoint(1).x - dist[0], inv1.getPoint(1).y - dist[1]),
            cv::Point2f(inv1.getPoint(2).x - dist[0], inv1.getPoint(2).y - dist[1]),
            cv::Point2f(inv1.getPoint(3).x - dist[0], inv1.getPoint(3).y - dist[1]));

    //Plane 2
    //****************************************************************************************************
    Plane2d p3 = u.getProjectedPlanes().at(1).to2d();
    Plane2d p4(cv::Point2f(0, 0), cv::Point2f(480, 0), cv::Point2f(480, 540), cv::Point2f(0, 540));
//    Plane2d p4(cv::Point2f(0, 0), cv::Point2f(230, 0), cv::Point2f(230, 520), cv::Point2f(0, 520));
//    Plane2d p4(cv::Point2f(0, 0), cv::Point2f(270, 0), cv::Point2f(270, 405), cv::Point2f(0, 405));
    //****************************************************************************************************
    //Invert the plane y coordinates
    Plane2d inv2 = p3.yInverted();
    //Move it so that it's closer to the target plane
    dist = pjs::distance(inv2, p4);
    Plane2d pp3(cv::Point2f(inv2.getPoint(0).x - dist[0], inv2.getPoint(0).y - dist[1]),
            cv::Point2f(inv2.getPoint(1).x - dist[0], inv2.getPoint(1).y - dist[1]),
            cv::Point2f(inv2.getPoint(2).x - dist[0], inv2.getPoint(2).y - dist[1]),
            cv::Point2f(inv2.getPoint(3).x - dist[0], inv2.getPoint(3).y - dist[1]));



    //***********************
    //Load the target image
    //***********************    
    cv::Mat img = cv::imread(filename, CV_LOAD_IMAGE_COLOR);
    if (!img.data) {
        std::cout << " --(!) Error reading image" << std::endl;
        throw std::exception();
    }

    //Helper object
    Utils utils;

    //Divide the image in two
    //    std::vector<cv::Mat> images = utils.divideImageInTwo(img);

    //Build the surfaces with their reference planes and their corresponding
    //image
    Surface s1(pp1, p2);
    Surface s2(pp3, p4);

    std::vector<Surface*> surfaces;
    surfaces.push_back(&s1);
    surfaces.push_back(&s2);

    int originX;
    int padding;
    int screenWidth = 1280;
    int screenHeight = 800;
    //TODO recursive position correction
    int width1 = s1.getWidth();
    int width2 = s2.getWidth();
    int diffW = width1 - width2;
    if (diffW < 0) {
        originX = screenWidth / 2 - width1;
        padding = 0;
    } else {
        originX = 0 + screenWidth / 2 - width1;
        padding = 0;
    }

    //1st position correction
    cv::Point2f origin(originX, 0);
    s1.correctBBPosition(origin);
    cv::Point2f s1ur = s1.getUpperRightCorner();    
    s2.correctPosition(s1ur);

    cv::Point2f upperLeft = s2.getUpperLeftCorner();
    cv::Point2f upperRight = s2.getUpperRightCorner();
    double topY;
    if (upperLeft.y < upperRight.y) {
        topY = upperLeft.y;
    } else {
        topY = upperRight.y;
    }
    cv::Size size = utils.getFinalSize(surfaces);
    int diffH = screenHeight - size.height;
    //2nd position correction if necessary (if second plane is still outside)
    if (!topY < 0) {
        topY = 0;
    }
    cv::Point2f newOrigin(originX, -topY + diffH / 2);
    s1.correctBBPosition(newOrigin);
    s1ur = s1.getUpperRightCorner();
    s2.correctPosition(s1ur);

    //    cv::Size size = utils.getFinalSize(surfaces);
    size.width += padding;

    size.width = std::max(screenWidth, size.width);
    size.height = screenHeight;

    cv::Size sizeS1(size.width / 2, size.height);

    s1.setSize(sizeS1);
    s2.setSize(size);

    std::vector<cv::Mat> images = utils.divideImageInTwo(img);

    s1.setImage(images.at(0));
    s2.setImage(images.at(1));

    s1.applyHomography();
    s2.applyHomography();
    //        s1.addTransparency();
    //        s2.addTransparency();

    cv::Mat finalImage = utils.getImageFromSurfaces(surfaces);

    surfaces.clear();

    return finalImage;
}
示例#10
0
GeometryTools::IntersectionStatus inPlaneLineIntersect(
    double x1, double y1,
    double x2, double y2,
    double x3, double y3,
    double x4, double y4,
    double l1NormalizedTolerance, double l2NormalizedTolerance,
    double *x, double *y, double* fractionAlongLine1, double* fractionAlongLine2)
{
   double mua, mub;
   double denom, numera, numerb;

   denom  = (y4-y3) * (x2-x1) - (x4-x3) * (y2-y1);
   numera = (x4-x3) * (y1-y3) - (y4-y3) * (x1-x3);
   numerb = (x2-x1) * (y1-y3) - (y2-y1) * (x1-x3);

    double EPS = 1e-40;

   // Are the line coincident? 
   if (fabs(numera) < EPS && fabs(numerb) < EPS && fabs(denom) < EPS) 
   {
#if 0
       *x = 0;
       *y = 0;
       *fractionAlongLine1 = 0;
       *fractionAlongLine2 = 0;
       return GeometryTools::LINES_OVERLAP;
#else
       cvf::Vec3d p12(x2-x1, y2-y1, 0);
       cvf::Vec3d p13(x3-x1, y3-y1, 0);
       cvf::Vec3d p34(x4-x3, y4-y3, 0);

       double length12 = p12.length();
       double length34 = p34.length();

       // Check if the p1 p2 line is a point
      
       if (length12 < EPS )
       {
           cvf::Vec3d p34(x4-x3, y4-y3, 0);
           *x = x1;
           *y = y1;
           *fractionAlongLine1 = 1;
           *fractionAlongLine2 = p13.length()/p34.length();
           return GeometryTools::LINES_OVERLAP;
       }

       cvf::Vec3d p14(x4-x1, y4-y1, 0);
       cvf::Vec3d p32(x2-x3, y2-y3, 0);

       cvf::Vec3d e12 = p12.getNormalized(); 
       double normDist13 =  e12*p13 / length12;
       double normDist14 =  e12*p14 / length12;

       // Check if both points on the p3 p4 line is outside line p1 p2.
       if( (normDist13 < 0 - l1NormalizedTolerance && normDist14 < 0-l1NormalizedTolerance )|| (normDist13 > 1 +l1NormalizedTolerance  && normDist14 > 1+l1NormalizedTolerance ) ) 
      {
           *x = 0;
           *y = 0;
           *fractionAlongLine1 = 0;
           *fractionAlongLine2 = 0;
          return GeometryTools::NO_INTERSECTION;
       }

       double normDist32 =  e12*p32 / length34;
       //double normDist31 = -e12*p13 / length34;

       // Set up fractions along lines to the edge2 vertex actually touching edge 1.
       /// if two, select the one furthest from the start
       bool pt3IsInside = false;
       bool pt4IsInside = false;
       if ((0.0 - l1NormalizedTolerance) <= normDist13 && normDist13 <= (1.0 +l1NormalizedTolerance) ) pt3IsInside = true;
       if ((0.0 - l1NormalizedTolerance) <= normDist14 && normDist14 <= (1.0 +l1NormalizedTolerance) ) pt4IsInside = true;

       if (pt3IsInside && !pt4IsInside)
       {
           *fractionAlongLine1 = normDist13;
           *fractionAlongLine2 = 0.0;
           *x = x3;
           *y = y3;
       }
       else if (pt4IsInside && !pt3IsInside)
       {
           *fractionAlongLine1 = normDist14;
           *fractionAlongLine2 = 1.0;
           *x = x4;
           *y = y4;
       }
       else if (pt3IsInside && pt4IsInside)
       {
           // Return edge 2 vertex furthest along edge 1
           if (normDist13 <= normDist14)
           {
               *fractionAlongLine1 =  normDist14 ;
               *fractionAlongLine2 =  1.0;
               *x = x4;
               *y = y4;
           }
           else
           {
               *fractionAlongLine1 = normDist13;
               *fractionAlongLine2 =  0.0;
               *x = x3;
               *y = y3;
           }
      }
       else // both outside on each side
       {
           // Return End of edge 1
            *fractionAlongLine1 = 1.0;
            *fractionAlongLine2 = normDist32;
            *x = x2;
            *y = y2;
       } 
  
      return GeometryTools::LINES_OVERLAP;
#endif
   }

   /* Are the line parallel */
   if (fabs(denom) < EPS) {
      *x = 0;
      *y = 0;
      *fractionAlongLine1 = 0;
      *fractionAlongLine2 = 0;

      return GeometryTools::NO_INTERSECTION;
   }

   /* Is the intersection along the the segments */
   mua = numera / denom;
   mub = numerb / denom;

   *x = x1 + mua * (x2 - x1);
   *y = y1 + mua * (y2 - y1);
   *fractionAlongLine1 = mua;
   *fractionAlongLine2 = mub;

   if (mua < 0 - l1NormalizedTolerance || 1 + l1NormalizedTolerance < mua  || mub < 0 - l2NormalizedTolerance ||  1 + l2NormalizedTolerance < mub) 
   {
      return GeometryTools::LINES_INTERSECT_OUTSIDE;
   }
   else if (fabs(mua) < l1NormalizedTolerance || fabs(1-mua) < l1NormalizedTolerance || 
            fabs(mub) < l2NormalizedTolerance || fabs(1-mub) < l2NormalizedTolerance )
   {
       if (fabs(mua)   < l1NormalizedTolerance) *fractionAlongLine1 = 0;
       if (fabs(1-mua) < l1NormalizedTolerance) *fractionAlongLine1 = 1;
       if (fabs(mub)   < l2NormalizedTolerance) *fractionAlongLine2 = 0;
       if (fabs(1-mub) < l2NormalizedTolerance) *fractionAlongLine2 = 1;
       return GeometryTools::LINES_TOUCH;
   }
   else
   {
       return GeometryTools::LINES_CROSSES;
   }
}
示例#11
0
int main()
{
  Point p1(-253.357, -123.36);
  Point p2(-190.03, 216.606);
  Point p3(-343.349, 286.6);
  Point p4(141.604, 279.934);
  Point p5(276.591, -46.7012);
  Point p6(251.593, -263.347);
  Point p7(-3.38184, -343.339);
  Point p8(-380.012, -173.355);
  Point p9(-98.3726, 39.957);
  Point p10(133.271, 124.949);
  Point p11(289.923, 301.598);
  Point p12(421.577, 23.292);
  Point p13(79.9434, -93.3633);
  Point p14(-40.0449, 366.592);
  Point p15(311.587, 374.924);
  Point p16(431.576, 214.94);
  Point p17(426.576, -131.693);
  Point p18(-265.023, -285.011);
  Point p19(369.915, 89.9521);
  Point p20(368.249, -15.0376);
  Point p21(484.904, 18.2925);
  Point p22(-411.675, 283.267);
  Point p23(-250.024, 124.949);
  Point p24(-80.041, -78.3647);
  Point p25(-360.014, 31.6245);
  Point p26(-305.019, 356.593);
  
  // built Delaunay triangulation
  PS.insert(p1); PS.insert(p2); PS.insert(p3); PS.insert(p4);
  PS.insert(p5); PS.insert(p6); PS.insert(p7); PS.insert(p8);  
  PS.insert(p9); PS.insert(p10); PS.insert(p11); PS.insert(p12);
  PS.insert(p13); PS.insert(p14); PS.insert(p15); PS.insert(p16);  
  PS.insert(p17); PS.insert(p18); PS.insert(p19); PS.insert(p20);
  PS.insert(p21); PS.insert(p22); PS.insert(p23); PS.insert(p24);
  PS.insert(p25); PS.insert(p26);
  
  std::list<Vertex_handle> LV;
  
  bool correct = true;
  
  // circle emptiness check
  Circle cs1(Point(-23.3799, 108.284), 1124.78);
  check_empty checker(cs1);
  
  CGAL::range_search(PS,cs1,std::back_inserter(LV),checker,true);
   
  if (checker.get_result()) {
    std::cout << "circle not empty !\n";
    std::cout <<  "this is an error !\n"; correct=false;
  }
  else std::cout << "circle was empty !\n";  
  
  Circle cs2(Point(-255.024, -100.029), 23551);
  check_empty checker2(cs2);
  
  CGAL::range_search(PS,cs2,std::back_inserter(LV),checker2,true);
   
  if (checker2.get_result()) std::cout << "circle not empty !\n";
  else {
    std::cout << "circle was empty !\n";   
    std::cout <<  "this is an error !\n"; correct=false;
  } 
  
  // triangle check
  Triangle t1(Point(-21.7134, -123.36), Point(84.9429, 74.9536), Point(209.931, -161.69)); 
  Triangle t2(Point(-61.7095, 164.945), Point(-88.3735, 101.618), Point(49.9463, 101.618));
  
  check_empty_triangle tchecker1(t1);
  CGAL::range_search(PS,t1.vertex(0),t1.vertex(1),t1.vertex(2),std::back_inserter(LV),tchecker1,true);
   
  if (tchecker1.get_result()) std::cout << "triangle not empty !\n";
  else {
    std::cout << "triangle was empty !\n";   
    std::cout <<  "this is an error !\n"; correct=false;
  }
  
  check_empty_triangle tchecker2(t2);
  CGAL::range_search(PS,t2.vertex(0),t2.vertex(1),t2.vertex(2),std::back_inserter(LV),tchecker2,true);
   
  if (tchecker2.get_result()) {
     std::cout << "triangle not empty !\n";
     std::cout <<  "this is an error !\n"; correct=false;
  }
  else std::cout << "triangle was empty !\n";   
  
  // rectangle check
  Rectangle_2 r1(-290.021, -175.022, -125.037, -35.0356);       
  Rectangle_2 r2(-48.3774, 136.614, -23.3799, 251.603);   

  check_empty_rectangle rchecker1(r1);
  CGAL::range_search(PS,r1.vertex(0),r1.vertex(1),r1.vertex(2),r1.vertex(3),std::back_inserter(LV),rchecker1,true);
   
  if (rchecker1.get_result()) std::cout << "rectangle not empty !\n";
  else {
    std::cout << "rectangle was empty !\n";
    std::cout <<  "this is an error !\n"; correct=false;
  }
   
  check_empty_rectangle rchecker2(r2);
  CGAL::range_search(PS,r2.vertex(0),r2.vertex(1),r2.vertex(2),r2.vertex(3),std::back_inserter(LV),rchecker2,true);
   
  if (rchecker2.get_result()) {
    std::cout << "rectangle not empty !\n";
    std::cout <<  "this is an error !\n"; correct=false;
  }
  else std::cout << "rectangle was empty !\n";
 
  if (correct) return 0;
  
  return 1;
}
示例#12
0
void CContainers::prepareMemBuffers()
{
	memout=new CMemoryBuffer();
	std::pair<std::string,CMemoryBuffer*> p("!data",memout);
	memmap.insert(p);

	memout_words=new CMemoryBuffer();
	std::pair<std::string,CMemoryBuffer*> p2("!!!words",memout_words);
	memmap.insert(p2);

	memout_letters=new CMemoryBuffer();
	std::pair<std::string,CMemoryBuffer*> p3("!!letters",memout_letters);
	memmap.insert(p3);

	memout_num=new CMemoryBuffer();
	std::pair<std::string,CMemoryBuffer*> p4("!num",memout_num);
	memmap.insert(p4);

	memout_year=new CMemoryBuffer();
	std::pair<std::string,CMemoryBuffer*> p5("!year",memout_year);
	memmap.insert(p5);

	memout_date=new CMemoryBuffer();
	std::pair<std::string,CMemoryBuffer*> p6("!date",memout_date);
	memmap.insert(p6);

	memout_words2=new CMemoryBuffer();
	std::pair<std::string,CMemoryBuffer*> p7("!!!words2",memout_words2);
	memmap.insert(p7);

	memout_words3=new CMemoryBuffer();
	std::pair<std::string,CMemoryBuffer*> p8("!!!words3",memout_words3);
	memmap.insert(p8);

	memout_words4=new CMemoryBuffer();
	std::pair<std::string,CMemoryBuffer*> p9("!!!words4",memout_words4);
	memmap.insert(p9);

	memout_pages=new CMemoryBuffer();
	std::pair<std::string,CMemoryBuffer*> p10("!pages",memout_pages);
	memmap.insert(p10);

	memout_num2=new CMemoryBuffer();
	std::pair<std::string,CMemoryBuffer*> p11("!num2",memout_num2);
	memmap.insert(p11);

	memout_num3=new CMemoryBuffer();
	std::pair<std::string,CMemoryBuffer*> p12("!num3",memout_num3);
	memmap.insert(p12);

	memout_num4=new CMemoryBuffer();
	std::pair<std::string,CMemoryBuffer*> p13("!num4",memout_num4);
	memmap.insert(p13);

	memout_remain=new CMemoryBuffer();
	std::pair<std::string,CMemoryBuffer*> p14("!remain",memout_remain);
	memmap.insert(p14);

	memout_date2=new CMemoryBuffer();
	std::pair<std::string,CMemoryBuffer*> p15("!date2",memout_date2);
	memmap.insert(p15);

	memout_date3=new CMemoryBuffer();
	std::pair<std::string,CMemoryBuffer*> p16("!date3",memout_date3);
	memmap.insert(p16);

	memout_num2b=new CMemoryBuffer();
	std::pair<std::string,CMemoryBuffer*> p17("!num2b",memout_num2b);
	memmap.insert(p17);

	memout_num3b=new CMemoryBuffer();
	std::pair<std::string,CMemoryBuffer*> p18("!num3b",memout_num3b);
	memmap.insert(p18);

	memout_num4b=new CMemoryBuffer();
	std::pair<std::string,CMemoryBuffer*> p19("!num4b",memout_num4b);
	memmap.insert(p19);

	memout_numb=new CMemoryBuffer();
	std::pair<std::string,CMemoryBuffer*> p20("!numb",memout_numb);
	memmap.insert(p20);

	memout_num2c=new CMemoryBuffer();
	std::pair<std::string,CMemoryBuffer*> p21("!num2c",memout_num2c);
	memmap.insert(p21);

	memout_num3c=new CMemoryBuffer();
	std::pair<std::string,CMemoryBuffer*> p22("!num3c",memout_num3c);
	memmap.insert(p22);

	memout_num4c=new CMemoryBuffer();
	std::pair<std::string,CMemoryBuffer*> p23("!num4c",memout_num4c);
	memmap.insert(p23);

	memout_numc=new CMemoryBuffer();
	std::pair<std::string,CMemoryBuffer*> p24("!numc",memout_numc);
	memmap.insert(p24);

	memout_time=new CMemoryBuffer();
	std::pair<std::string,CMemoryBuffer*> p25("!time",memout_time);
	memmap.insert(p25);

	memout_remain2=new CMemoryBuffer();
	std::pair<std::string,CMemoryBuffer*> p26("!remain2",memout_remain2);
	memmap.insert(p26);

	memout_ip=new CMemoryBuffer();
	std::pair<std::string,CMemoryBuffer*> p27("!ip",memout_ip);
	memmap.insert(p27);

	memout_hm=new CMemoryBuffer();
	std::pair<std::string,CMemoryBuffer*> p28("!hm",memout_hm);
	memmap.insert(p28);

	memout_hms=new CMemoryBuffer();
	std::pair<std::string,CMemoryBuffer*> p29("!hms",memout_hms);
	memmap.insert(p29);
}
示例#13
0
int main(void){

	TriangleCache::TriangleCache TC(50,2);

	sm::Point p11(10,10);
	sm::Point p12(20,20);
	sm::Point p13(30,30);
	sm::Point p21(40,40);
	sm::Point p22(50,50);
	sm::Point p23(60,60);

	Triangle t1(&p11, &p12, &p13);
	Triangle t2(&p21, &p22, &p23);
	GNode gn1(&t1);
	GNode gn2(&t2);

	GNode gn3(&t2);
	GNode gn4(&t2);
	GNode gn5(&t1);
	GNode gn6(&t1);

	const GNode ** path1 = (const GNode **)calloc(3, sizeof(GNode *));
	path1[0] = &gn3;
	path1[1] = &gn4;
	path1[2] = 0;
	const GNode ** path = (const GNode **)calloc(3, sizeof(GNode *));
	path[0] = &gn1;
	path[1] = &gn2;
	path[2] = 0;
	const GNode ** path2 = (const GNode **)calloc(4, sizeof(GNode *));
	path2[0] = &gn5;
	path2[1] = &gn3;
	path2[2] = &gn6;
	path2[3] = 0;
	bool f = false;

	debugGREEN("TEST FIND WHEN THERE IS NOTHING\n");
	const GNode **r = TC.getPath( &gn1, &gn2, f);
	ASSERT(!r);
	debugRED("OK... \n");

	debugGREEN("TEST ADD AND SEARCH ONE ELEMENT\n");
	TC.addPath( &gn1, &gn2, path);
	const GNode **ret = TC.getPath( &gn1, &gn2, f);
	ASSERT(ret);
	ASSERT(f);
	ASSERT(&gn1 == ret[0]);
	ASSERT(&gn2 == ret[1]);
	debugRED("OK... \n");

	debugGREEN("TEST SEARCH THE SAME ELEMENT BUT IN REVERSE ORDER\n");
	ASSERT(TC.getPath( &gn2, &gn1, f));
	ASSERT(!f);
	debugRED("OK... \n");

	debugGREEN("TEST ADD 500000 MORE ELEMENTS gn2-gn1 and look for gn1-gn2 and gn2-gn1\n");
	for (int i = 0; i < 50000; i++){
		TC.addPath( &gn2, &gn1, path);
	}
	ASSERT(TC.getPath( &gn1, &gn2, f));
	ASSERT(TC.getPath( &gn2, &gn1, f));
	debugRED("OK... \n");

	debugGREEN("TEST ADD path, then path1, then path2, and then find path 1\n");
	TC.addPath( &gn1, &gn2, path);
	TC.addPath( &gn3, &gn4, path1);
	TC.addPath( &gn5, &gn6, path2);
	ASSERT(!TC.getPath( &gn1, &gn2, f));
	ASSERT(!TC.getPath( &gn2, &gn1, f));
	ASSERT(TC.getPath( &gn4, &gn3, f));
	ASSERT(TC.getPath( &gn3, &gn4, f));
	ASSERT(TC.getPath( &gn5, &gn6, f));
	ASSERT(TC.getPath( &gn6, &gn5, f));
	debugRED("OK... \n");

	debugGREEN("TEST CLEAR CACHE\n");
	TC.clear();
	ASSERT(!TC.getPath( &gn1, &gn2, f));
	ASSERT(!TC.getPath( &gn2, &gn1, f));
	ASSERT(!TC.getPath( &gn4, &gn3, f));
	ASSERT(!TC.getPath( &gn3, &gn4, f));
	ASSERT(!TC.getPath( &gn5, &gn6, f));
	ASSERT(!TC.getPath( &gn6, &gn5, f));
	debugRED("OK... \n");

	debugGREEN("TEST REFRESHING\n");
	TC.addPath( &gn2, &gn1, path);
	TC.addPath( &gn3, &gn4, path1);
	TC.addPath( &gn2, &gn1, path);
	TC.addPath( &gn5, &gn6, path2);
	ASSERT(!TC.getPath( &gn4, &gn3, f));
	ASSERT(TC.getPath( &gn1, &gn2, f));
	ASSERT(TC.getPath( &gn5, &gn6, f));
	debugRED("OK... \n");

	debugBLUE("END TESTS (ALL OK)\n");

	free(path);
	free(path1);
	free(path2);

	return 0;
}
short orient3d_triangle  (const GenericPointT& p1, 
			  const GenericPointT& p2, 
			  const GenericPointT& p3, 
			  const GenericPointT& ptn ) 
{

  // possible bounding box check
  // [RH] TODO
  //

  typedef GenericPointT Vector_t;

  Vector_t p12(p2 - p1);
  Vector_t p13(p3 - p1);
  Vector_t useNormal = p12.ex(p13);
  useNormal.normalize();
        
  short ori1, ori2;

  // Test line p1 - p2
  GenericPointT temppoint (p1[0] + useNormal[0],
			   p1[1] + useNormal[1],
			   p1[2] + useNormal[2]);
  SwkMatrix3x3<GenericPointT, double>  swk_matrix1 (p1, p2,  temppoint, ptn);
  SwkMatrix3x3<GenericPointT, double>  swk_matrix2 (p1, p2,  temppoint, p3);
  ori1 = swk_matrix1.orient3D_short();
  ori2 = swk_matrix2.orient3D_short();
  //std::cout << "p12: ori1: " << ori1 << std::endl;
  //std::cout << "p12: ori2: " << ori2 << std::endl;
  if (ori1 == RH_ON || ori2 == RH_ON)
    return RH_ON;
  if (ori1 != ori2)
    return (RH_OUT);

  // Test line p1 - p3
  SwkMatrix3x3<GenericPointT, double>  swk_matrix3 (p1, p3,  temppoint, ptn);
  SwkMatrix3x3<GenericPointT, double>  swk_matrix4 (p1, p3,  temppoint, p2);
  ori1 = swk_matrix3.orient3D_short();
  ori2 = swk_matrix4.orient3D_short();
  //std::cout << "p13: ori1: " << ori1 << std::endl;
  //std::cout << "p13: ori2: " << ori2 << std::endl;
  if (ori1 == RH_ON || ori2 == RH_ON)
    return RH_ON;
  if (ori1 != ori2)
    return (RH_OUT);



  // Test line p2 - p3
  GenericPointT temppoint2 (p2[0] + useNormal[0],
			    p2[1] + useNormal[1],
			    p2[2] + useNormal[2]);
  SwkMatrix3x3<GenericPointT, double>  swk_matrix5 (p2, p3,  temppoint2, ptn);
  SwkMatrix3x3<GenericPointT, double>  swk_matrix6 (p2, p3,  temppoint2, p1);
  ori1 = swk_matrix5.orient3D_short();
  ori2 = swk_matrix6.orient3D_short();
  //std::cout << "p23: ori1: " << ori1 << std::endl;
  //std::cout << "p23: ori2: " << ori2 << std::endl;
  if (ori1 == RH_ON || ori2 == RH_ON)
    return RH_ON;
  if (ori1 != ori2)
    return (RH_OUT);

  
  return (RH_IN);
	
}
示例#15
0
void BuildTreeDoubleYShape(Node *node[], Tree &tree)
{
    const KDL::Vector unitx(1,0,0);
    const KDL::Vector unity(0,1,0);
    const KDL::Vector unitz(0,0,1);
    const KDL::Vector unit1(sqrt(14.0)/8.0, 1.0/8.0, 7.0/8.0);
    const KDL::Vector zero = KDL::Vector::Zero();
    KDL::Vector p0(0.0f, -1.5f, 0.0f);
    KDL::Vector p1(0.0f, -1.0f, 0.0f);
    KDL::Vector p2(0.0f, -0.5f, 0.0f);
    KDL::Vector p3(0.5f*Root2Inv, -0.5+0.5*Root2Inv, 0.0f);
    KDL::Vector p4(0.5f*Root2Inv+0.5f*HalfRoot3, -0.5+0.5*Root2Inv+0.5f*0.5, 0.0f);
    KDL::Vector p5(0.5f*Root2Inv+1.0f*HalfRoot3, -0.5+0.5*Root2Inv+1.0f*0.5, 0.0f);
    KDL::Vector p6(0.5f*Root2Inv+1.5f*HalfRoot3, -0.5+0.5*Root2Inv+1.5f*0.5, 0.0f);
    KDL::Vector p7(0.5f*Root2Inv+0.5f*HalfRoot3, -0.5+0.5*Root2Inv+0.5f*HalfRoot3, 0.0f);
    KDL::Vector p8(0.5f*Root2Inv+1.0f*HalfRoot3, -0.5+0.5*Root2Inv+1.0f*HalfRoot3, 0.0f);
    KDL::Vector p9(0.5f*Root2Inv+1.5f*HalfRoot3, -0.5+0.5*Root2Inv+1.5f*HalfRoot3, 0.0f);
    KDL::Vector p10(-0.5f*Root2Inv, -0.5+0.5*Root2Inv, 0.0f);
    KDL::Vector p11(-0.5f*Root2Inv-0.5f*HalfRoot3, -0.5+0.5*Root2Inv+0.5f*HalfRoot3, 0.0f);
    KDL::Vector p12(-0.5f*Root2Inv-1.0f*HalfRoot3, -0.5+0.5*Root2Inv+1.0f*HalfRoot3, 0.0f);
    KDL::Vector p13(-0.5f*Root2Inv-1.5f*HalfRoot3, -0.5+0.5*Root2Inv+1.5f*HalfRoot3, 0.0f);
    KDL::Vector p14(-0.5f*Root2Inv-0.5f*HalfRoot3, -0.5+0.5*Root2Inv+0.5f*0.5, 0.0f);
    KDL::Vector p15(-0.5f*Root2Inv-1.0f*HalfRoot3, -0.5+0.5*Root2Inv+1.0f*0.5, 0.0f);
    KDL::Vector p16(-0.5f*Root2Inv-1.5f*HalfRoot3, -0.5+0.5*Root2Inv+1.5f*0.5, 0.0f);
    
    node[0] = new Node(p0, unit1, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
    tree.InsertRoot(node[0]);
    
    node[1] = new Node(p1, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
    tree.InsertLeftChild(node[0], node[1]);
    
    node[2] = new Node(p1, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
    tree.InsertLeftChild(node[1], node[2]);
    
    node[3] = new Node(p2, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
    tree.InsertLeftChild(node[2], node[3]);
    
    node[4] = new Node(p2, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
    tree.InsertRightSibling(node[3], node[4]);
    
    node[5] = new Node(p3, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
    tree.InsertLeftChild(node[3], node[5]);
    
    node[6] = new Node(p3, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
    tree.InsertRightSibling(node[5], node[6]);
    
    node[7] = new Node(p3, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
    tree.InsertLeftChild(node[5], node[7]);
    
    node[8] = new Node(p4, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
    tree.InsertLeftChild(node[7], node[8]);
    
    node[9] = new Node(p5, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
    tree.InsertLeftChild(node[8], node[9]);
    
    node[10] = new Node(p5, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
    tree.InsertLeftChild(node[9], node[10]);
    
    node[11] = new Node(p6, zero, 0.08, EFFECTOR);
    tree.InsertLeftChild(node[10], node[11]);
    
    node[12] = new Node(p3, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
    tree.InsertLeftChild(node[6], node[12]);
    
    node[13] = new Node(p7, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
    tree.InsertLeftChild(node[12], node[13]);
    
    node[14] = new Node(p8, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
    tree.InsertLeftChild(node[13], node[14]);
    
    node[15] = new Node(p8, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
    tree.InsertLeftChild(node[14], node[15]);
    
    node[16] = new Node(p9, zero, 0.08, EFFECTOR);
    tree.InsertLeftChild(node[15], node[16]);
    
    node[17] = new Node(p10, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
    tree.InsertLeftChild(node[4], node[17]);
    
    node[18] = new Node(p10, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
    tree.InsertLeftChild(node[17], node[18]);
    
    node[19] = new Node(p10, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
    tree.InsertRightSibling(node[17], node[19]);
    
    node[20] = new Node(p11, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
    tree.InsertLeftChild(node[18], node[20]);
    
    node[21] = new Node(p12, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
    tree.InsertLeftChild(node[20], node[21]);
    
    node[22] = new Node(p12, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
    tree.InsertLeftChild(node[21], node[22]);
    
    node[23] = new Node(p13, zero, 0.08, EFFECTOR);
    tree.InsertLeftChild(node[22], node[23]);
    
    node[24] = new Node(p10, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
    tree.InsertLeftChild(node[19], node[24]);
    
    node[25] = new Node(p14, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
    tree.InsertLeftChild(node[24], node[25]);
    
    node[26] = new Node(p15, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
    tree.InsertLeftChild(node[25], node[26]);
    
    node[27] = new Node(p15, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
    tree.InsertLeftChild(node[26], node[27]);
    
    node[28] = new Node(p16, zero, 0.08, EFFECTOR);
    tree.InsertLeftChild(node[27], node[28]);
}