void ofCube(const vec3f& Center,float WidthHeightDepth) { ofPushMatrix(); ofTranslate( Center ); ofScale( WidthHeightDepth/2.f, WidthHeightDepth/2.f, WidthHeightDepth/2.f ); // front/back top/bottom left/right vec3f FTL( -1, -1, -1 ); vec3f FTR( 1, -1, -1 ); vec3f FBR( 1, 1, -1 ); vec3f FBL( -1, 1, -1 ); vec3f BTL( -1, -1, 1 ); vec3f BTR( 1, -1, 1 ); vec3f BBR( 1, 1, 1 ); vec3f BBL( -1, 1, 1 ); // front ofTriangle( FBL, FTL, FTR ); ofTriangle( FTR, FBR, FBL ); // back ofTriangle( BBL, BTL, BTR ); ofTriangle( BTR, BBR, BBL ); // left ofTriangle( BBL, BTL, FTL ); ofTriangle( FTL, FBL, BBL ); // right ofTriangle( FBR, FTR, BTR ); ofTriangle( BTR, BBR, FBR ); // top ofTriangle( FTR, FTL, BTL ); ofTriangle( BTL, BTR, FTR ); // bottom ofTriangle( FBR, FBL, BBL ); ofTriangle( BBL, BBR, FBR ); ofPopMatrix(); }
osg::ref_ptr<osg::Group> BuildFrustumNode(std::string const &name, osg::Camera const * camera, Frustum & frustum, double near_dist, double far_dist) { // Projection and ModelView matrices osg::Matrixd proj; osg::Matrixd mv; osg::Matrixd vm; osg::Vec3d eye(0,0,0); if (camera) { proj = camera->getProjectionMatrix(); mv = camera->getViewMatrix(); vm = camera->getViewMatrix(); osg::Vec3d vpt,up; camera->getViewMatrixAsLookAt(eye,vpt,up); } else { // return empty group if camera is invalid osg::ref_ptr<osg::Group> gp = new osg::Group; return gp; } osg::Matrixd const mv_inv = osg::Matrixd::inverse( mv ); // Get near and far from the Projection matrix. double near = proj(3,2) / (proj(2,2)-1.0); double far = proj(3,2) / (1.0+proj(2,2)); if(near_dist > 0.0) { near = near_dist; } if(far_dist > 0.0) { far = far_dist; } // Get the sides of the near plane. const double nLeft = near * (proj(2,0)-1.0) / proj(0,0); const double nRight = near * (1.0+proj(2,0)) / proj(0,0); const double nTop = near * (1.0+proj(2,1)) / proj(1,1); const double nBottom = near * (proj(2,1)-1.0) / proj(1,1); // Get the sides of the far plane. const double fLeft = far * (proj(2,0)-1.0) / proj(0,0); const double fRight = far * (1.0+proj(2,0)) / proj(0,0); const double fTop = far * (1.0+proj(2,1)) / proj(1,1); const double fBottom = far * (proj(2,1)-1.0) / proj(1,1); // Our vertex array needs only 9 vertices: The origin, and the // eight corners of the near and far planes. osg::ref_ptr<osg::Vec3dArray> v = new osg::Vec3dArray; v->resize( 21 ); // near and far are negated because the opengl // camera is at (0,0,0) with the view dirn pointing // down the -Z axis osg::Vec3d NBL(nLeft,nBottom,-near); NBL = NBL * mv_inv; osg::Vec3d NBR(nRight,nBottom,-near); NBR = NBR *mv_inv; osg::Vec3d NTR(nRight,nTop,-near); NTR = NTR * mv_inv; osg::Vec3d NTL(nLeft,nTop,-near); NTL = NTL * mv_inv; osg::Vec3d FBL(fLeft, fBottom, -far); FBL = FBL * mv_inv; osg::Vec3d FBR(fRight, fBottom, -far); FBR = FBR * mv_inv; osg::Vec3d FTR(fRight, fTop, -far); FTR = FTR * mv_inv; osg::Vec3d FTL(fLeft, fTop, -far); FTL = FTL* mv_inv; // get the normals for the frustum planes osg::Vec3d p_left = (NBL+FTL)*0.5; osg::Vec3d d_left = (FTL-NTL)^(NBL-NTL); d_left.normalize(); osg::Vec3d p_right = (NBR+FTR)*0.5; osg::Vec3d d_right = (NTR-FTR)^(FBR-FTR); d_right.normalize(); osg::Vec3d p_top = (NTL+FTR)*0.5; osg::Vec3d d_top = (FTR-NTR)^(NTL-NTR); d_top.normalize(); osg::Vec3d p_btm = (NBL+FBR)*0.5; osg::Vec3d d_btm = (FBL-NBL)^(NBR-NBL); d_btm.normalize(); osg::Vec3d p_near = (NBL+NTR)*0.5; osg::Vec3d d_near = (NTL-NTR)^(NBR-NTR); d_near.normalize(); osg::Vec3d p_far = (FBL+FTR)*0.5; osg::Vec3d d_far = (FTR-FBL)^(FBL-FTL); d_far.normalize(); // save { frustum.list_planes[0].n = d_left; frustum.list_planes[0].p = p_left; frustum.list_planes[0].d = d_left*p_left; frustum.list_planes[1].n = d_btm; frustum.list_planes[1].p = p_btm; frustum.list_planes[1].d = d_btm*p_btm; frustum.list_planes[2].n = d_right; frustum.list_planes[2].p = p_right; frustum.list_planes[2].d = d_right*p_right; frustum.list_planes[3].n = d_top; frustum.list_planes[3].p = p_top; frustum.list_planes[3].d = d_top*p_top; frustum.list_planes[4].n = d_near; frustum.list_planes[4].p = p_near; frustum.list_planes[4].d = d_near*p_near; frustum.list_planes[5].n = d_far; frustum.list_planes[5].p = p_far; frustum.list_planes[5].d = d_far*p_far; // TODO/Note: The magnitude of these edges // should be similar to the magnitude of the // edges of any geometry used in the SAT! // near edges frustum.list_edges[0].dirn_ab = NTL-NBL; // left frustum.list_edges[0].a = NBL; frustum.list_edges[1].dirn_ab = NBL-NBR; // btm frustum.list_edges[1].a = NBR; frustum.list_edges[2].dirn_ab = NBR-NTR; // right frustum.list_edges[2].a = NTR; frustum.list_edges[3].dirn_ab = NTR-NTL; // top frustum.list_edges[3].a = NTL; // side edges frustum.list_edges[4].dirn_ab = FTL-NTL; // tl frustum.list_edges[4].a = NTL; // tl frustum.list_edges[5].dirn_ab = FBL-NBL; frustum.list_edges[5].a = NBL; frustum.list_edges[6].dirn_ab = FBR-NBR; frustum.list_edges[6].a = NBR; frustum.list_edges[7].dirn_ab = FTR-NTR; frustum.list_edges[7].a = NTR; // far edges frustum.list_edges[8].dirn_ab = FTL-FBL; // left frustum.list_edges[8].a = FBL; frustum.list_edges[9].dirn_ab = FBL-FBR; // btm frustum.list_edges[9].a = FBR; frustum.list_edges[10].dirn_ab = FBR-FTR; // right frustum.list_edges[10].a = FTR; frustum.list_edges[11].dirn_ab = FTR-FTL; // top frustum.list_edges[11].a = FTL; // frustum vx frustum.list_vx[0] = NBL; frustum.list_vx[1] = NBR; frustum.list_vx[2] = NTR; frustum.list_vx[3] = NTL; frustum.list_vx[4] = FBL; frustum.list_vx[5] = FBR; frustum.list_vx[6] = FTR; frustum.list_vx[7] = FTL; // pyramid vx frustum.list_pyr_vx[0] = &(frustum.eye); frustum.list_pyr_vx[1] = &(frustum.list_vx[4]); frustum.list_pyr_vx[2] = &(frustum.list_vx[5]); frustum.list_pyr_vx[3] = &(frustum.list_vx[6]); frustum.list_pyr_vx[4] = &(frustum.list_vx[7]); // eye frustum.eye = eye; } // get a length to show the normals double const normal_length = (FTR-FBR).length()*0.5; d_left *= normal_length; d_right *= normal_length; d_top *= normal_length; d_btm *= normal_length; d_near *= normal_length; d_far *= normal_length; v->at(0).set(0.,0.,0.); v->at(0) = v->at(0) * mv_inv; v->at(1) = NBL; v->at(2) = NBR; v->at(3) = NTR; v->at(4) = NTL; v->at(5) = FBL; v->at(6) = FBR; v->at(7) = FTR; v->at(8) = FTL; v->at(9) = p_left; v->at(10) = p_left+d_left; v->at(11) = p_right; v->at(12) = p_right+d_right; v->at(13) = p_top; v->at(14) = p_top+d_top; v->at(15) = p_btm; v->at(16) = p_btm+d_btm; v->at(17) = p_near; v->at(18) = p_near+d_near; v->at(19) = p_far; v->at(20) = p_far+d_far; osg::ref_ptr<osg::Geometry> geom = new osg::Geometry; geom->setUseDisplayList( false ); geom->setVertexArray( v ); osg::ref_ptr<osg::Vec4Array> c = new osg::Vec4Array; // c->push_back(osg::Vec4(0.5,0.5,0.5,0.5)); c->push_back(osg::Vec4(1,1,1,1)); geom->setColorArray( c, osg::Array::BIND_OVERALL ); GLushort idxLines[8] = { 0, 5, 0, 6, 0, 7, 0, 8 }; GLushort idxLoops0[4] = { 1, 2, 3, 4 }; GLushort idxLoops1[4] = { 5, 6, 7, 8 }; // GLushort idxNormals[12] = { // 9,10,11,12,13,14,15,16,17,18,19,20 }; geom->addPrimitiveSet( new osg::DrawElementsUShort( osg::PrimitiveSet::LINES, 8, idxLines ) ); geom->addPrimitiveSet( new osg::DrawElementsUShort( osg::PrimitiveSet::LINE_LOOP, 4, idxLoops0 ) ); geom->addPrimitiveSet( new osg::DrawElementsUShort( osg::PrimitiveSet::LINE_LOOP, 4, idxLoops1 ) ); // geom->addPrimitiveSet( new osg::DrawElementsUShort( osg::PrimitiveSet::LINES, 12, idxNormals ) ); osg::ref_ptr<osg::Geode> geode = new osg::Geode; geode->addDrawable( geom ); // Create parent MatrixTransform to transform the view volume by // the inverse ModelView matrix. osg::ref_ptr<osg::Group> gp = new osg::Group; gp->setName(name); gp->addChild(geode); return gp; }
osg::ref_ptr<osg::Group> BuildFrustumNode(osg::Camera * camera) { // Projection and ModelView matrices osg::Matrixd proj; osg::Matrixd mv; osg::Matrixd vm; if (camera) { proj = camera->getProjectionMatrix(); mv = camera->getViewMatrix(); vm = camera->getViewMatrix(); } else { // Create some kind of reasonable default Projection matrix. proj.makePerspective( 30., 1., 1., 10. ); // leave mv as identity } osg::Matrixd const mv_inv = osg::Matrixd::inverse( mv ); // Get near and far from the Projection matrix. const double near = proj(3,2) / (proj(2,2)-1.0); const double far = proj(3,2) / (1.0+proj(2,2)); // Get the sides of the near plane. const double nLeft = near * (proj(2,0)-1.0) / proj(0,0); const double nRight = near * (1.0+proj(2,0)) / proj(0,0); const double nTop = near * (1.0+proj(2,1)) / proj(1,1); const double nBottom = near * (proj(2,1)-1.0) / proj(1,1); // Get the sides of the far plane. const double fLeft = far * (proj(2,0)-1.0) / proj(0,0); const double fRight = far * (1.0+proj(2,0)) / proj(0,0); const double fTop = far * (1.0+proj(2,1)) / proj(1,1); const double fBottom = far * (proj(2,1)-1.0) / proj(1,1); // Our vertex array needs only 9 vertices: The origin, and the // eight corners of the near and far planes. osg::ref_ptr<osg::Vec3dArray> v = new osg::Vec3dArray; v->resize( 21 ); // near and far are negated because the opengl // camera is at (0,0,0) with the view dirn pointing // down the -Z axis osg::Vec3d NBL(nLeft,nBottom,-near); NBL = NBL * mv_inv; osg::Vec3d NBR(nRight,nBottom,-near); NBR = NBR *mv_inv; osg::Vec3d NTR(nRight,nTop,-near); NTR = NTR * mv_inv; osg::Vec3d NTL(nLeft,nTop,-near); NTL = NTL * mv_inv; osg::Vec3d FBL(fLeft, fBottom, -far); FBL = FBL * mv_inv; osg::Vec3d FBR(fRight, fBottom, -far); FBR = FBR * mv_inv; osg::Vec3d FTR(fRight, fTop, -far); FTR = FTR * mv_inv; osg::Vec3d FTL(fLeft, fTop, -far); FTL = FTL* mv_inv; // get the normals for the frustum planes osg::Vec3d p_left = NBL+ (FTL-NBL)*0.5; osg::Vec3d d_left = (FTL-NTL)^(NBL-NTL); d_left.normalize(); osg::Vec3d p_right = (NBR+FTR)*0.5; osg::Vec3d d_right = (NTR-FTR)^(FBR-FTR); d_right.normalize(); osg::Vec3d p_top = (NTL+FTR)*0.5; osg::Vec3d d_top = (FTR-NTR)^(NTL-NTR); d_top.normalize(); osg::Vec3d p_btm = (NBL+FBR)*0.5; osg::Vec3d d_btm = (FBL-NBL)^(NBR-NBL); d_btm.normalize(); osg::Vec3d p_near = (NBL+NTR)*0.5; osg::Vec3d d_near = (NTL-NTR)^(NBR-NTR); d_near.normalize(); osg::Vec3d p_far = (FBL+FTR)*0.5; osg::Vec3d d_far = (FTR-FBL)^(FBL-FTL); d_far.normalize(); // save { g_list_frustum_plane_pts[0] = p_left; g_list_frustum_plane_pts[1] = p_right; g_list_frustum_plane_pts[2] = p_top; g_list_frustum_plane_pts[3] = p_btm; g_list_frustum_plane_pts[4] = p_near; g_list_frustum_plane_pts[5] = p_far; g_list_frustum_plane_norms[0] = d_left; g_list_frustum_plane_norms[1] = d_right; g_list_frustum_plane_norms[2] = d_top; g_list_frustum_plane_norms[3] = d_btm; g_list_frustum_plane_norms[4] = d_near; g_list_frustum_plane_norms[5] = d_far; // TODO/Note: The magnitude of these edges // should be similar to the magnitude of the // edges of any geometry used in the SAT! // near edges g_list_frustum_edges[0] = NTL-NBL; // left g_list_frustum_edges[1] = NBL-NBR; g_list_frustum_edges[2] = NBR-NTR; g_list_frustum_edges[3] = NTR-NTL; // side edges g_list_frustum_edges[4] = FTL-NTL; // tl g_list_frustum_edges[5] = FBL-NBL; g_list_frustum_edges[6] = FBR-NBR; g_list_frustum_edges[7] = FTR-NTR; // far edges // (assume symmetric frustum so these // arent needed) // g_list_frustum_edges[8] = FTL-FBL; // left // g_list_frustum_edges[9] = FBL-FBR; // g_list_frustum_edges[10] = FBR-FTR; // g_list_frustum_edges[11] = FTR-FTL; // frustum vx g_list_frustum_vx[0] = NBL; g_list_frustum_vx[1] = NBR; g_list_frustum_vx[2] = NTR; g_list_frustum_vx[3] = NTL; g_list_frustum_vx[4] = FBL; g_list_frustum_vx[5] = FBR; g_list_frustum_vx[6] = FTR; g_list_frustum_vx[7] = FTL; } // get a length to show the normals double const normal_length = (FTR-FBR).length()*0.5; d_left *= normal_length; d_right *= normal_length; d_top *= normal_length; d_btm *= normal_length; d_near *= normal_length; d_far *= normal_length; v->at(0).set(0.,0.,0.); v->at(0) = v->at(0) * mv_inv; v->at(1) = NBL; v->at(2) = NBR; v->at(3) = NTR; v->at(4) = NTL; v->at(5) = FBL; v->at(6) = FBR; v->at(7) = FTR; v->at(8) = FTL; v->at(9) = p_left; v->at(10) = p_left+d_left; v->at(11) = p_right; v->at(12) = p_right+d_right; v->at(13) = p_top; v->at(14) = p_top+d_top; v->at(15) = p_btm; v->at(16) = p_btm+d_btm; v->at(17) = p_near; v->at(18) = p_near+d_near; v->at(19) = p_far; v->at(20) = p_far+d_far; osg::ref_ptr<osg::Geometry> geom = new osg::Geometry; geom->setUseDisplayList( false ); geom->setVertexArray( v ); osg::ref_ptr<osg::Vec4Array> c = new osg::Vec4Array; c->push_back(osg::Vec4(0.5,0.5,0.5,0.5)); geom->setColorArray( c, osg::Array::BIND_OVERALL ); GLushort idxLines[8] = { 0, 5, 0, 6, 0, 7, 0, 8 }; GLushort idxLoops0[4] = { 1, 2, 3, 4 }; GLushort idxLoops1[4] = { 5, 6, 7, 8 }; // GLushort idxNormals[12] = { // 9,10,11,12,13,14,15,16,17,18,19,20 }; geom->addPrimitiveSet( new osg::DrawElementsUShort( osg::PrimitiveSet::LINES, 8, idxLines ) ); geom->addPrimitiveSet( new osg::DrawElementsUShort( osg::PrimitiveSet::LINE_LOOP, 4, idxLoops0 ) ); geom->addPrimitiveSet( new osg::DrawElementsUShort( osg::PrimitiveSet::LINE_LOOP, 4, idxLoops1 ) ); // geom->addPrimitiveSet( new osg::DrawElementsUShort( osg::PrimitiveSet::LINES, 12, idxNormals ) ); osg::ref_ptr<osg::Geode> geode = new osg::Geode; geode->addDrawable( geom ); // Create parent MatrixTransform to transform the view volume by // the inverse ModelView matrix. osg::ref_ptr<osg::Group> gp = new osg::Group; gp->setName("frustum"); gp->addChild(geode); return gp; }