void bios_hook_ints()
{
/* setup new base memory size */
p16(0x0413)[0] -= d16(bdat->bd_size / 1024);
/* hook bios interrupts */
bdat->hook_ints();
}
static void boot_from_partition(prt_inf *prt, int n_mount)
{
dc_partition_io(prt, pv(0x7C00), 1, 0, 1);
/* check MBR signature */
if (p16(0x7C00+510)[0] != 0xAA55) {
puts("partition unbootable\n");
} else {
bios_jump_boot(prt->hdd->dos_numb, n_mount);
}
}
static int scan_hdd_off(int hdd_n, u64 off, u64 ext_off)
{
u8 d_mbr[SECTOR_SIZE];
int found = 0;
pt_ent *pt;
int i;
u64 pt_off;
u64 ex_off;
do
{
/* read MBR */
if (btab->p_hdd_io(hdd_n, d_mbr, 1, off + ext_off, 1) == 0) {
break;
}
/* check MBR signature */
if (p16(d_mbr+510)[0] != 0xAA55) {
break;
}
for (pt = pv(d_mbr + 446), i = 4; i; i--, pt++)
{
if (pt->prt_size == 0) {
continue;
}
pt_off = pt->start_sect;
if ( (pt->os == 5) || (pt->os == 0x0F) )
{
if (ext_off == 0) {
ex_off = pt_off;
pt_off = 0;
} else {
ex_off = ext_off;
}
found += scan_hdd_off(hdd_n, pt_off, ex_off);
} else
{
if (n_parts < PART_MAX)
{
p_parts[n_parts].hdd_n = hdd_n;
p_parts[n_parts].begin = pt_off + off + ext_off;
p_parts[n_parts].size = pt->prt_size;
p_parts[n_parts].flags = (pt->active == 0x80 ? PT_ACTIVE : 0) |
(ext_off != 0 ? PT_EXTENDED : 0);
n_parts++, found++;
}
}
}
} while (0);
return found;
}
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);
}
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);
}
int main(int argc, char** argv)
{
// parse command
unsigned int pos = 1;
unsigned int mask = (argc >= 2) ? 0 : 0xFFFF;
while(pos < argc)
{
unsigned int sceneNumber = atoi(argv[pos++]);
mask |= 1 << sceneNumber;
}
// Initialize
phong p32(32.0f);
phong p64(64.0f);
phong p16(16.0f);
diffuse d;
constantAlbedo red(color(1.0f, 0.0f, 0.0f));
constantAlbedo green(color(0.0f, 1.0f, 0.0f));
constantAlbedo blue(color(0.0f, 0.0f, 1.0f));
constantAlbedo white(color(1.0f, 1.0f, 1.0f));
std::vector<triangle> sphere = createSphere( vec3d(0.0f, 0.0f, 0.0f), 1.0f, 16, 16 );
std::vector<triangle> plane = createPlane( vec3d(-1.0f, -1.0f, 0.0f), vec3d(0.0f, 2.0f, 0.0f), vec3d(2.0f, 0.0f, 0.0f) );
material redMaterial;
redMaterial.addComponent( reflectanceComponent(&red, &d) );
redMaterial.addComponent( reflectanceComponent(&white, &p32) );
material greenMaterial;
greenMaterial.addComponent( reflectanceComponent(&green, &d) );
greenMaterial.addComponent( reflectanceComponent(&white, &p64) );
material blueMaterial;
blueMaterial.addComponent( reflectanceComponent(&blue, &d) );
blueMaterial.addComponent( reflectanceComponent(&white, &p16) );
material whiteMaterial;
whiteMaterial.addComponent( reflectanceComponent(&white, &d) );
triangleMesh redSphere(sphere, redMaterial);
triangleMesh greenSphere(sphere, greenMaterial);
triangleMesh blueSphere(sphere, blueMaterial);
triangleMesh whitePlane(plane, whiteMaterial);
sceneGraphObject redSphereObject(redSphere);
sceneGraphObject greenSphereObject(greenSphere);
sceneGraphObject blueSphereObject(blueSphere);
sceneGraphObject whitePlaneObject(whitePlane);
camera cam( vec3d(0.0f, 0.0f, 5.0f),
vec3d(0.0f, 0.0f, -1.0f),
vec3d(0.0f, 1.0f, 0.0f),
35.0f * M_PI / 180.0f,
512, 512 );
directionalLightsource ls( color(1.0f, 1.0f, 1.0f), vec3d(0.0f, -1.0f, -1.0f) );
directionalLightsource frontal( color(1.0f, 1.0f, 1.0f), vec3d(0.0f, 0.0f, -1.0f) );
/////////////////////////////////////////////////////
if((mask & 2) != 0)
{
sceneGraphNode sg1;
sg1.addChildNode(redSphereObject);
std::cout << "Generating Image 1." << std::endl;
image result1 = generateImage(cam, sg1, ls);
result1.save("hw4-result1.ppm");
}
/////////////////////////////////////////////////////
if((mask & 4) != 0)
{
sceneGraphNode sg2node1(translation3d(vec3d(+1.0f, 0.0f, 0.0f)));
sg2node1.addChildNode(redSphereObject);
sceneGraphNode sg2node2(translation3d(vec3d(-1.0f, 0.0f, 0.0f)));
sg2node2.addChildNode(blueSphereObject);
sceneGraphNode sg2;
sg2.addChildNode(sg2node1);
sg2.addChildNode(sg2node2);
std::cout << "Generating Image 2." << std::endl;
image result2 = generateImage(cam, sg2, ls);
result2.save("hw4-result2.ppm");
}
/////////////////////////////////////////////////////
if((mask & 8) != 0)
{
sceneGraphNode sg3node1(scale3d(2.0f, 0.5f, 0.5f));
sg3node1.addChildNode(greenSphereObject);
sceneGraphNode sg3;
sg3.addChildNode(sg3node1);
std::cout << "Generating Image 3." << std::endl;
image result3 = generateImage(cam, sg3, ls);
result3.save("hw4-result3.ppm");
}
/////////////////////////////////////////////////////
if((mask & 16) != 0)
{
sceneGraphNode sg2node1(translation3d(vec3d(+1.0f, 0.0f, 0.0f)));
sg2node1.addChildNode(redSphereObject);
sceneGraphNode sg2node2(translation3d(vec3d(-1.0f, 0.0f, 0.0f)));
sg2node2.addChildNode(blueSphereObject);
sceneGraphNode sg2;
sg2.addChildNode(sg2node1);
sg2.addChildNode(sg2node2);
sceneGraphNode sg4node1(rotationY3d(M_PI));
sg4node1.addChildNode(sg2);
sceneGraphNode sg4;
sg4.addChildNode(sg4node1);
std::cout << "Generating Image 4." << std::endl;
image result4 = generateImage(cam, sg4, ls);
result4.save("hw4-result4.ppm");
}
/////////////////////////////////////////////////////
if((mask & 32) != 0)
{
sceneGraphNode sg5node0(scale3d(0.5f, 0.5f, 0.5f));
sceneGraphNode sg5node1(rotationY3d(0.5 * M_PI));
sceneGraphNode sg5node2(rotationX3d(0.5 * M_PI));
sceneGraphNode sg5node3(translation3d(vec3d(0.0f, 2.0f, 0.0f)));
sceneGraphNode sg5node4(rotationX3d(0.5 * M_PI));
sceneGraphNode sg5node5(translation3d(vec3d(0.0f, 2.0f, 0.0f)));
sceneGraphNode sg5node6(rotationX3d(0.5 * M_PI));
sceneGraphNode sg5node7(translation3d(vec3d(0.0f, 2.0f, 0.0f)));
sceneGraphNode sg5node8(rotationX3d(0.5 * M_PI));
sceneGraphNode sg5node9(translation3d(vec3d(0.0f, 2.0f, 0.0f)));
sceneGraphNode sg5;
sg5.addChildNode(sg5node0);
sg5node0.addChildNode(sg5node1);
sg5node1.addChildNode(sg5node2);
sg5node2.addChildNode(sg5node3);
sg5node3.addChildNode(redSphereObject);
sg5node3.addChildNode(sg5node4);
sg5node4.addChildNode(sg5node5);
sg5node5.addChildNode(greenSphereObject);
sg5node5.addChildNode(sg5node6);
sg5node6.addChildNode(sg5node7);
sg5node7.addChildNode(blueSphereObject);
sg5node7.addChildNode(sg5node8);
sg5node8.addChildNode(sg5node9);
sg5node9.addChildNode(blueSphereObject);
std::cout << "Generating Image 5." << std::endl;
image result5 = generateImage(cam, sg5, ls);
result5.save("hw4-result5.ppm");
}
/////////////////////////////////////////////////////
if((mask & 64) != 0)
{
sceneGraphNode cubeside( translation3d(vec3d(0.0f, 0.0f, 1.0f)) );
cubeside.addChildNode(whitePlaneObject);
sceneGraphNode cubeNode1( rotationY3d(0.0f / 2.0f * M_PI) );
cubeNode1.addChildNode(cubeside);
sceneGraphNode cubeNode2( rotationY3d(1.0f / 2.0f * M_PI) );
cubeNode2.addChildNode(cubeside);
sceneGraphNode cubeNode3( rotationY3d(2.0f / 2.0f * M_PI) );
cubeNode3.addChildNode(cubeside);
sceneGraphNode cubeNode4( rotationY3d(3.0f / 2.0f * M_PI) );
cubeNode4.addChildNode(cubeside);
sceneGraphNode cubeNode5( rotationX3d(0.5f * M_PI) );
cubeNode5.addChildNode(cubeside);
sceneGraphNode cubeNode6( rotationX3d(-0.5f * M_PI) );
cubeNode6.addChildNode(cubeside);
sceneGraphNode cube;
cube.addChildNode(cubeNode1);
cube.addChildNode(cubeNode2);
cube.addChildNode(cubeNode3);
cube.addChildNode(cubeNode4);
cube.addChildNode(cubeNode5);
cube.addChildNode(cubeNode6);
sceneGraphNode sg6( rotation3d(0.25*M_PI, vec3d(1.0f, 0.75f, 0.5f)) );
sg6.addChildNode(cube);
std::cout << "Generating Image 6." << std::endl;
image result6 = generateImage(cam, sg6, frontal);
result6.save("hw4-result6.ppm");
}
// Done.
return 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]);
}
