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