int main() { //Tree t2(); //Tree t(5); //t.insert(3); //t.insert(7); //t.insert(1); //t.display(); std::cout << "***********" << "\n" ; double i = 3.6; Tree<double> newTree; std::cout << "inserting 3" << "\n" ; newTree.insert(i); std::cout << "inserting 4" << "\n" ; i = 4; newTree.insert(i); std::cout << "inserting 2" << "\n" ; i = 2; newTree.insert(i); std::cout << "inserting 7" << "\n" ; i = 7; newTree.insert(i); std::cout << "inserting 10" << "\n" ; i = 10; newTree.insert(i); std::cout << "inserting 8" << "\n" ; i = 8; newTree.insert(i); std::cout << "printing ..." << "\n" ; newTree.print(newTree.root); std::cout << "Second tree" << "\n"; Tree<double> new2(newTree); new2.print(new2.root); std::cout << "Third tree" << "\n"; Tree<double> new3(3); new3 = new2; new3.print(new3.root); return 0; }
void renderDepth(const WorldModel& wm, const ProjectionMatrix& P, const geo::Pose3D& sensor_pose, Result& res) { // Parameters double near_clip_z = -0.1; // - - - - - - - - - - - - - - - - - - - - - - - - - - - - const std::vector<Triangle>& triangles = wm.triangles(); const std::vector<geo::Vec3>& vertices = wm.vertices(); // transform points std::vector<geo::Vec3> vertices_t(vertices.size()); std::vector<geo::Vec2i> vertices_proj(vertices.size()); geo::Pose3D sensor_pose_inv = sensor_pose.inverse(); for(unsigned int i = 0; i < vertices.size(); ++i) { vertices_t[i] = sensor_pose_inv * vertices[i]; vertices_proj[i] = P.project3Dto2D(vertices_t[i]); } for(const auto& t : triangles) { const geo::Vec3& p1_3d = vertices_t[t.i1]; const geo::Vec3& p2_3d = vertices_t[t.i2]; const geo::Vec3& p3_3d = vertices_t[t.i3]; res.triangleHook(t, p1_3d, p2_3d, p3_3d); int n_verts_in = 0; bool v1_in = false; bool v2_in = false; bool v3_in = false; const geo::Vec3* vIn[3]; if (p1_3d.z < near_clip_z) { ++n_verts_in; v1_in = true; } if (p2_3d.z < near_clip_z) { ++n_verts_in; v2_in = true; } if (p3_3d.z < near_clip_z) { ++n_verts_in; v3_in = true; } if (n_verts_in == 1) { if (v1_in) { vIn[0] = &(p1_3d); vIn[1] = &(p2_3d); vIn[2] = &(p3_3d); } if (v2_in) { vIn[0] = &(p2_3d); vIn[1] = &(p3_3d); vIn[2] = &(p1_3d); } if (v3_in) { vIn[0] = &(p3_3d); vIn[1] = &(p1_3d); vIn[2] = &(p2_3d); } //Parametric line stuff // p = v0 + v01*t geo::Vec3 v01 = *vIn[1] - *vIn[0]; float t1 = ((near_clip_z - (*vIn[0]).z) / v01.z ); geo::Vec3 new2(vIn[0]->x + v01.x * t1, vIn[0]->y + v01.y * t1, near_clip_z); // Second vert point geo::Vec3 v02 = *vIn[2] - *vIn[0]; float t2 = ((near_clip_z - (*vIn[0]).z) / v02.z); geo::Vec3 new3(vIn[0]->x + v02.x * t2, vIn[0]->y + v02.y * t2, near_clip_z); drawTriangle(*vIn[0], new2, new3, P, res); } else if (n_verts_in == 2) { if (!v1_in) { vIn[0]=&(p2_3d); vIn[1]=&(p3_3d); vIn[2]=&(p1_3d); } if (!v2_in) { vIn[0]=&(p3_3d); vIn[1]=&(p1_3d); vIn[2]=&(p2_3d); } if (!v3_in) { vIn[0]=&(p1_3d); vIn[1]=&(p2_3d); vIn[2]=&(p3_3d); } //Parametric line stuff // p = v0 + v01*t geo::Vec3 v01 = *vIn[2] - *vIn[0]; float t1 = ((near_clip_z - (*vIn[0]).z)/v01.z ); geo::Vec3 new2((*vIn[0]).x + v01.x * t1,(*vIn[0]).y + v01.y * t1, near_clip_z); // Second point geo::Vec3 v02 = *vIn[2] - *vIn[1]; float t2 = ((near_clip_z - (*vIn[1]).z)/v02.z); geo::Vec3 new3((*vIn[1]).x + v02.x * t2, (*vIn[1]).y + v02.y * t2, near_clip_z); drawTriangle(*vIn[0], *vIn[1], new2, P, res); drawTriangle(new2, *vIn[1], new3, P, res); } else if (n_verts_in == 3) { const geo::Vec2i& p1_2d = vertices_proj[t.i1]; const geo::Vec2i& p2_2d = vertices_proj[t.i2]; const geo::Vec2i& p3_2d = vertices_proj[t.i3]; drawTriangle2D(geo::Vec3f(p1_2d.x, p1_2d.y, 1.0f / -p1_3d.z), geo::Vec3f(p2_2d.x, p2_2d.y, 1.0f / -p2_3d.z), geo::Vec3f(p3_2d.x, p3_2d.y, 1.0f / -p3_3d.z), res); } } }