Frustum culling - ошибка при оценке точки в плоскости усеченного конуса.

Я пытаюсь реализовать следующий код:

http://www.lighthouse3d.com/tutorials/view-frustum-culling/geometric-approach-implementation/

Мне нужно использовать отсечение усеченного конуса в моем графическом движке.

мои функции:

void frustum::updateCameraData()
{
    float _ratio = float(globalData::windowWidth/globalData::windowHeight);
    // store the information
    this->ratio = _ratio;
    this->angle = globalData::cameraAngle;
    this->nearD = globalData::zNear;
    this->farD = globalData::zFar;

    // compute width and height of the near and far m_frustumane sections
    tang = (float)tan(PI * globalData::cameraAngle * 0.5) ;
    nh = nearD * tang;
    nw = nh * _ratio;
    fh = farD  * tang;
    fw = fh * _ratio;

}

    void frustum::extractFrustum(vector3f camPosition, vector3f camDirection, vector3f camUp)
{
    cout << "camPosition = ";
    camPosition.print();
    cout << " camDirection = ";
    camDirection.print();
    cout << " camUp = ";
    camUp.print();
    cout << "\n";
    vector3f dir,nc,fc,X,Y,Z,Zn;


    // compute the Z axis of camera
    // this axis points in the opposite direction from
    // the looking direction
    Z = camPosition - camDirection;
    Z.Normalize();

    // X axis of camera with given "up" vector and Z axis
    X = camUp * Z;
    X.Normalize();

    // the real "up" vector is the cross product of Z and X
    Y = Z * X;

    // compute the centers of the near and far planes
    nc = camPosition - Z * nearD;
    fc = camPosition - Z * farD;

    // compute the 4 corners of the frustum on the near plane
    ntl = nc + Y * nh - X * nw;
    ntr = nc + Y * nh + X * nw;
    nbl = nc - Y * nh - X * nw;
    nbr = nc - Y * nh + X * nw;

    // compute the 4 corners of the frustum on the far plane
    ftl = fc + Y * fh - X * fw;
    ftr = fc + Y * fh + X * fw;
    fbl = fc - Y * fh - X * fw;
    fbr = fc - Y * fh + X * fw;



    Zn = vector3f(-Z.x,-Z.y,-Z.z);
    m_frustum[NEARP].setNormalAndPoint(Zn,nc);
    m_frustum[FARP].setNormalAndPoint(Z,fc);

    vector3f aux,normal;

    aux = (nc + Y*nh) - camPosition;
    aux.Normalize();
    normal = aux * X;
    Zn = nc+Y*nh;
    m_frustum[TOP].setNormalAndPoint(normal,Zn);

    aux = (nc - Y*nh) - camPosition;
    aux.Normalize();
    normal = X * aux;

    Zn = nc-Y*nh;
    m_frustum[BOTTOM].setNormalAndPoint(normal,Zn);

    aux = (nc - X*nw) - camPosition;
    aux.Normalize();
    normal = aux * Y;

    Zn = nc-X*nw;
    m_frustum[LEFT].setNormalAndPoint(normal,Zn);

    aux = (nc + X*nw) - camPosition;
    aux.Normalize();
    normal = Y * aux;

    Zn = nc+X*nw;
    m_frustum[RIGHT].setNormalAndPoint(normal,Zn);

}

bool frustum::pointInFrustum( float x, float y, float z )
{
    vector3f point = vector3f(x,y,z);

    for(int i=0; i < 6; i++)
    {
        cout << "m_frustum[i].distance(point) = "<< m_frustum[i].distance(point) <<" < 0\n";
        if (m_frustum[i].distance(point) < 0)
        {
            return false;
        }
    }
    return true;
}



/*this is used to calculate the distance of point to plane*/

float distance(vector3f &p) {

    return (d + normal.innerProduct(p));
}

однако я получаю нежелательные результаты (это мои отладочные сообщения):

   camPosition = (0, 15, 40) camDirection = (0, 0, 0) camUp = (0, 1, 0)

NO DRAWN: object0
posicion: -20,0,-20
/*object position*/ vector3f point = vector3f(-20,0,-20)
m_frustum[0].distance(point) = 14.7867 < 0
/*object position*/ vector3f point = vector3f(-20,0,-20)
m_frustum[1].distance(point) = 14.485 < 0
/*object position*/ vector3f point = vector3f(-20,0,-20)
m_frustum[2].distance(point) = 14.2216 < 0
/*object position*/ vector3f point = vector3f(-20,0,-20)
m_frustum[3].distance(point) = -15.0501 < 0
NO DRAWN: object1
posicion: 0,0,-20
/*object position*/ vector3f point = vector3f(0,0,-20)
m_frustum[0].distance(point) = 14.7867 < 0
/*object position*/ vector3f point = vector3f(0,0,-20)
m_frustum[1].distance(point) = 14.485 < 0
/*object position*/ vector3f point = vector3f(0,0,-20)
m_frustum[2].distance(point) = 14.2216 < 0
/*object position*/ vector3f point = vector3f(0,0,-20)
m_frustum[3].distance(point) = -15.0501 < 0
NO DRAWN: object2
posicion: 20,0,-7
/*object position*/ vector3f point = vector3f(20,0,-7)
m_frustum[0].distance(point) = 14.7867 < 0
/*object position*/ vector3f point = vector3f(20,0,-7)
m_frustum[1].distance(point) = 14.485 < 0
/*object position*/ vector3f point = vector3f(20,0,-7)
m_frustum[2].distance(point) = 14.2216 < 0
/*object position*/ vector3f point = vector3f(20,0,-7)
m_frustum[3].distance(point) = -15.0501 < 0
NO DRAWN: object3
posicion: -40,0,-20
/*object position*/ vector3f point = vector3f(-40,0,-20)
m_frustum[0].distance(point) = 14.7867 < 0
/*object position*/ vector3f point = vector3f(-40,0,-20)
m_frustum[1].distance(point) = 14.485 < 0
/*object position*/ vector3f point = vector3f(-40,0,-20)
m_frustum[2].distance(point) = 14.2216 < 0
/*object position*/ vector3f point = vector3f(-40,0,-20)
m_frustum[3].distance(point) = -15.0501 < 0

Ни один из объектов не прошел проверку (я тестирую с помощью pointInFrustrum (), используя точку в качестве позиции каждого объекта.

если я визуализирую сцену без отсечения усеченной пирамиды, все выглядит идеально, но при ее использовании ничто не проходит проверку. есть идеи где может быть ошибка? (код - это та ссылка, о которой я упомянул выше).

Все объекты находятся в поле зрения ...

Матрица управляется с помощью glm :: mat4;

РЕДАКТИРОВАТЬ:

После внесения исправлений, если я направлю камеру немного вниз, объекты будут нарисованы, но если я направлю камеру немного вверх, все объекты полностью исчезнут (на этот раз они не нарисованы). Вы знаете, что может происходить? Заранее спасибо.

если камера направлена ​​вниз:

если камера смотрит немного вверх: