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