//draw floor
void draw_floor(){
glPushMatrix();
stacks.push(nothing);
nothing = nothing * Mtranslate(0,0,-4);
nothing = nothing * Mscale(20,20,0.2);
draw_cube(1,c7,nothing);
//tree1
nothing = stacks.top();
nothing *=Mtranslate(5,5,-3);
draw_tree(nothing);
//tree2
nothing = stacks.top();
nothing *=Mtranslate(-5,5,-3);
nothing *=Mscale(0.8,0.8,0.8);
draw_tree(nothing);
//tree3
nothing = stacks.top();
nothing *=Mtranslate(0,5,-3);
nothing *=Mscale(0.4,0.4,0.4);
draw_tree(nothing);
nothing = stacks.top();
stacks.pop();
glPopMatrix();
}
void MonocularManhattanBnb::TransferBuilding(const SE3<>& new_pose,
double floor_z,
MatI& orients) {
// Compute scaling (TODO: actually use this)
DiagonalMatrix<3> Mscale(makeVector(1.0*orients.Cols()/pc->image_size()[0],
1.0*orients.Rows()/pc->image_size()[1],
1.0));
// Invert the pose
const SE3<> orig_inv = pc->pose().inverse();
orients.Fill(vert_axis);
for (ManhattanBuilding::ConstCnrIt left_cnr = soln.cnrs.begin();
successor(left_cnr) != soln.cnrs.end();
left_cnr++) {
ManhattanBuilding::ConstCnrIt right_cnr = successor(left_cnr);
//TITLE("Next corner");
int axis = OtherHorizAxis(left_cnr->right_axis);
// Compute vertices in the 3D camera frame
Vec3 bl = FloorPoint(left_cnr->right_floor, floor_z);
Vec3 br = FloorPoint(right_cnr->left_floor, floor_z);
Vec3 tl = CeilPoint(left_cnr->right_ceil, bl);
Vec3 tr = CeilPoint(right_cnr->left_ceil, br);
if (bl[2] < 0) {
bl = -bl;
br = -br;
tl = -tl;
tr = -tr;
}
// Compute vertices in the 3D global frame
Vec3 world_tl = orig_inv * tl;
Vec3 world_tr = orig_inv * tr;
Vec3 world_bl = orig_inv * bl;
Vec3 world_br = orig_inv * br;
// Compute the wall corners in the other camera
Vec3 ret_tl = new_pose * world_tl;
Vec3 ret_tr = new_pose * world_tr;
Vec3 ret_bl = new_pose * world_bl;
Vec3 ret_br = new_pose * world_br;
ClipToFront(ret_tr, ret_tl);
ClipToFront(ret_br, ret_bl);
// Compute the wall corners in the other image
Vec3 im_tl = pc->RetToIm(ret_tl);
Vec3 im_tr = pc->RetToIm(ret_tr);
Vec3 im_bl = pc->RetToIm(ret_bl);
Vec3 im_br = pc->RetToIm(ret_br);
// Build the polygon and fill
vector<Vec3 > poly;
poly.push_back(im_tl);
poly.push_back(im_bl);
poly.push_back(im_br);
poly.push_back(im_tr);
FillPolygon(poly, orients, axis);
}
}
//draw the main object
void draw_man(){
stacks.push(modelM);
// modelM *= Mtranslate(0,-ii,0);
modelM *= Mrotate(angle_whole,0,0,1);
// modelM *= Mtranslate(0,ii,0);
//modelM *= Mtranslate(0,-ii,0);
//modelM = modelM*Mrotate(angle_whole,0,0,1);
//modelM *= Mtranslate(0,ii,0);
//ii=0;
stacks.push(modelM);
//Head and nose
modelM = modelM*Mrotate(angle_head,0,0,1);
modelM = modelM*Mtranslate(0,0,5);
draw_sphere(1.5,12,12,c1,modelM);//head
modelM = modelM*Mtranslate(0,-2,0);
modelM = modelM*Mtranslate(0,1,0);
modelM = modelM*Mrotate(angle_nose,0,0,1);
modelM = modelM*Mtranslate(0,-1,0);
modelM = modelM*Mrotate(90,1,0,0);
draw_cylinder(0.2,0.1,1,12,c4,false,modelM);//nose
//draw_cube(1,c2,modelM);
//Left arm and body
modelM=stacks.top();
modelM = modelM * Mtranslate(0,0,2);
draw_cylinder(2,1,4,12,c6,false,modelM);//Body
modelM = modelM * Mtranslate(-2,0,1);
modelM = modelM*Mrotate(90,0,1,0);
modelM = modelM*Mtranslate(0,0,1);
modelM = modelM*Mrotate(angle_larm,0,1,0);
modelM = modelM*Mtranslate(0,0,-1);
draw_cylinder(0.5,0.8,2,12,c3,false,modelM);//L Arm
modelM = modelM * Mtranslate(0,0,-1);
draw_sphere(0.5,12,12,c1,modelM); // L connector
// modelM = modelM*Mtranslate(0,0,1);
modelM = modelM*Mrotate(angle_lhand,0,1,0);
// modelM = modelM*Mtranslate(0,0,-1);
modelM = modelM * Mtranslate(0,0,-1);
draw_cylinder(0.1,0.5,2,12,c5,false,modelM);
//Right arm
modelM=stacks.top();
modelM = modelM*Mtranslate(2,0,3);
modelM = modelM*Mrotate(-90,0,1,0);
modelM = modelM*Mtranslate(0,0,1);
modelM = modelM*Mrotate(angle_rarm,0,1,0);
modelM = modelM*Mtranslate(0,0,-1);
draw_cylinder(0.5,0.8,2,12,c3,false,modelM);//R Arm
modelM = modelM * Mtranslate(0,0,-1);
draw_sphere(0.5,12,12,c1,modelM); // R connector
// modelM = modelM*Mtranslate(0,0,1);
modelM = modelM*Mrotate(angle_rhand,0,1,0);
// modelM = modelM*Mtranslate(0,0,-1);
modelM = modelM * Mtranslate(0,0,-1);
draw_cylinder(0.1,0.5,2,12,c5,false,modelM);
//legs
modelM=stacks.top();
modelM = modelM*Mtranslate(1,0,-2);
modelM = modelM*Mscale(0.7,0.7,4);
draw_cube(1,c3,modelM);
modelM=stacks.top();
modelM = modelM*Mtranslate(-1,0,-2);
modelM = modelM*Mscale(0.7,0.7,4);
draw_cube(1,c3,modelM);
stacks.pop();
modelM=stacks.top();
stacks.pop();
}
