Frustum Culling

#include "testApp.h"

class Plane {
public:
    ofVec3f p0,p1,p2;
    void set(ofVec3f pt0, ofVec3f pt1, ofVec3f pt2) {
        p0 = pt0; p1 = pt1; p2 = pt2;
        
    }
    float distance(ofVec3f point) {
        
        ofVec3f v = p1 - p0;
        ofVec3f u = p2 - p0;
        ofVec3f n = v.cross(u);
        n.normalize();
        
        float A = n.x;
        float B = n.y;
        float C = n.z;
        float D = - n.dot(p1);
        
        return (A * point.x) + (B * point.y) + (C * point.z) + D;
    }
    void draw() {
    }
};


int                 selectedCamera;
ofCamera            cameraA;
ofEasyCam           cameraB;
vector  <ofCamera*> cams;
vector  <ofVec3f>   points;

float mNear		= 20.0f;
float mFar		= 1500.0f;

//--------------------------------------------------------------
void testApp::setup() {
    
    ofBackground(90);
    ofEnableDepthTest();
    
    selectedCamera = 1;
    cams.push_back(&cameraA);
    cams.push_back(&cameraB);
    
    
    //cameraA.setNearClip(mNear);
    //cameraA.setFarClip(mFar);

    cameraA.setupPerspective(true, 20.0, mNear, mFar, ofVec2f(0, 0));
    
    //true, 60, mNear, mFar);
    cameraA.setPosition(ofVec3f() + (100 * cameraA.getZAxis()));
    cameraB.setDistance(600);
    
    int nPts = 50000;
    int div  = 400;
    for (int i=0; i<nPts; i++) {
        ofVec3f p(ofRandom(-div, div), ofRandom(-div, div), ofRandom(-div, div));
        points.push_back(p);
    }
    
}

//--------------------------------------------------------------
void testApp::update(){

    cameraA.rotateAround(0.2, ofVec3f(0, 1, 0), ofVec3f());
    cameraA.lookAt(ofVec3f());
    
}

//--------------------------------------------------------------
void drawBox(ofPoint tr, ofPoint tl, ofPoint br, ofPoint bl) {
    ofLine(tl, tr);
    ofLine(tr, br);
    ofLine(br, bl);
    ofLine(bl, tl);
}


//--------------------------------------------------------------
bool pointInFrustum(ofVec3f &p, Plane pl[6]) {
    for(int i=0; i < 6; i++) {
        if (pl[i].distance(p) < 0) return false;
    }
    return true;
}

//--------------------------------------------------------------
void testApp::draw(){
        
    cams[selectedCamera]->begin();
 
    ofRectangle viewport = ofGetCurrentViewport();
    float halfFov = PI * cameraA.getFov() / 360;
	float theTan = tanf(halfFov);
    
    float ratio     = viewport.width/viewport.height;
    float Hnear     = 2 * tan(cameraA.getFov() / 2.0) * -cameraA.getNearClip();// * theTan;
	float Wnear     = Hnear * ratio;
    
    float Hfar      = 2 * tan(cameraA.getFov() / 2.0) * -cameraA.getFarClip();// * theTan;
	float Wfar      = Hfar * ratio;
    
    ofNoFill();
    ofSetColor(255);
    ofDrawBox(0, 0, 0, 10, 10, 10);


    ofVec3f p  = cameraA.getPosition();
    ofVec3f l  = cameraA.getLookAtDir();
    ofVec3f u  = cameraA.getUpDir();
    
    ofVec3f fc = p + l * cameraA.getFarClip();
	ofVec3f nc = p + l * cameraA.getNearClip();

    ofSetColor(255);
    ofLine(nc, fc);
    
    
    ofVec3f up  = cameraA.getUpDir();
    ofVec3f right = cameraA.getXAxis();
    
    ofVec3f ftl = fc + (up * Hfar/2) - (right * Wfar/2);
    ofVec3f ftr = fc + (up * Hfar/2) + (right * Wfar/2);
	ofVec3f fbl = fc - (up * Hfar/2) - (right * Wfar/2);
	ofVec3f fbr = fc - (up * Hfar/2) + (right * Wfar/2);
    
    ofVec3f ntl = nc + (up * Hnear/2) - (right * Wnear/2);
	ofVec3f ntr = nc + (up * Hnear/2) + (right * Wnear/2);
	ofVec3f nbl = nc - (up * Hnear/2) - (right * Wnear/2);
	ofVec3f nbr = nc - (up * Hnear/2) + (right * Wnear/2);
    
    drawBox(ftl, ftr, fbl, fbr);
    drawBox(ntl, ntr, nbl, nbr);

    
    ofLine(ftl, ntl);
    ofLine(ftr, ntr);
    ofLine(fbl, nbl);
    ofLine(fbr, nbr);

    Plane pl[6];
    pl[0].set(ntr,ntl,ftl);
    pl[1].set(nbl,nbr,fbr);
    pl[2].set(ntl,nbl,fbl);
    pl[3].set(nbr,ntr,fbr);
    pl[4].set(ntl,ntr,nbr);
    pl[5].set(ftr,ftl,fbl);

    
    ofSetColor(255, 0, 0);
    for(int i=0; i<6; i++) {
        pl[i].draw();
    }
    
    
    for(int i=0; i<points.size(); i++) {
        if(pointInFrustum(points[i], pl)) {
            ofSetColor(255, 0, 0);
            ofCircle(points[i], 2);
        }
    }
    
    
    
    ofSetColor(255);
    // draw cameras
    for(int i=0; i<cams.size(); i++) {
        cams[i]->draw();
    }
    
    cams[selectedCamera]->end();
 
    string info = "";
    info += "Aspect "+ofToString(ratio) + "\n";
    info += "FOV "+ofToString(cameraA.getFov()) + "\n";
    info += "NC "+ofToString(cameraA.getNearClip()) + "\n";
    info += "FC "+ofToString(cameraA.getFarClip()) + "\n";
    
    info += "Wnear " + ofToString(Wnear)  + "\n";
    info += "Hnear " + ofToString(Hnear)  + "\n";
   
    info += "Wfar " + ofToString(Wfar)  + "\n";
    info += "Hfar " + ofToString(Hfar)  + "\n";
    
    ofDrawBitmapString(info, 20, 20);
}

//--------------------------------------------------------------
void testApp::keyPressed(int key){

    if(key == '1') selectedCamera = 0;
    if(key == '2') selectedCamera = 1;
    
    if(key == OF_KEY_DOWN) cameraB.setDistance(cameraB.getDistance()+2);
    if(key == OF_KEY_UP)   cameraB.setDistance(cameraB.getDistance()-2);

    if(key == OF_KEY_RIGHT)  cameraA.move(ofVec3f(1, 0, 0));
    if(key == OF_KEY_LEFT)   cameraA.move(ofVec3f(-1, 0, 0));

}

//--------------------------------------------------------------
void testApp::keyReleased(int key){

}

//--------------------------------------------------------------
void testApp::mouseMoved(int x, int y ){

}

//--------------------------------------------------------------
void testApp::mouseDragged(int x, int y, int button){

}

//--------------------------------------------------------------
void testApp::mousePressed(int x, int y, int button){

}

//--------------------------------------------------------------
void testApp::mouseReleased(int x, int y, int button){

}

//--------------------------------------------------------------
void testApp::windowResized(int w, int h){

}

//--------------------------------------------------------------
void testApp::gotMessage(ofMessage msg){

}

//--------------------------------------------------------------
void testApp::dragEvent(ofDragInfo dragInfo){ 

}