C++: Camera Math - Pitch, RotateY -- Confirm If Correct

[Halsafar]

Check this out and let me know if this is correct:

I am having my mind blown trying to get a proper 3rd person camera setup.
I got this far, and I can get it to move and follow something but its all messed up and at times goes into weird and crazy positions. Sometimes it even just locks. So I figure might as well see if the math I am using is correct, if so then it is obviously my implimentation which needs work.

Thanks.

// Rotates camera's eye around the world Y-axis
// Using your standard Y-axis rotation matrix, we'll rotate around the Y-axis
// ca == cosf(angle)    sa == sinf(angle)
// [ ca 0 -sa ]
// [ 0  1   0 ]
// [ sa 0  ca ]
void Camera::RotateY(USINT angle, const D3DVECTOR &vTarget)
{
    float xxx=0, zzz=0;
    float sinAng=0, cosAng=0;
    
    // Get sin()/cos() of angle
    /*angle *= D3D.ToRad
    sinAng = sinf(angle);
    cosAng = cosf(angle);*/
    cosAng = m_VectorCircle[angle].u;
    sinAng = m_VectorCircle[angle].v;
    
    // First translate to the world's origin
    m_Eye -= vTarget;
    
    // Save off forward components for computation?????????
    xxx = m_Eye.x;
    zzz = m_Eye.z;
    
    // Rotate forward vector
    m_Eye.x = xxx * cosAng + zzz * sinAng;
    m_Eye.z = xxx * -sinAng + zzz * cosAng;
    
    // Translate back
    m_Eye += vTarget;
    
    SetTarget(vTarget); // Build the camera's axes 
}


void Camera::Pitch(USINT angle, const D3DVECTOR &vTarget)
{
    // Compute the sin and cos of the angle 
    //float sinAng = sinf(angle);
    //float cosAng = cosf(angle);
    float cosAng = m_VectorCircle[angle].u;
    float sinAng = m_VectorCircle[angle].v;

    m_Eye -= vTarget; // Translate to world's origin

    // Calculate new eye X component
    float xxx = (m_Right.x * m_Right.x * (1.0f - cosAng) + cosAng) * m_Eye.x +
                (m_Right.x * m_Right.y * (1.0f - cosAng) + m_Right.z * sinAng) * m_Eye.y +
                (m_Right.x * m_Right.z * (1.0f - cosAng) - m_Right.y * sinAng) * m_Eye.z;

    // Calculate new Y component       
    float yyy = (m_Right.x * m_Right.y * (1.0f - cosAng) - m_Right.z * sinAng) * m_Eye.x +
                (m_Right.y * m_Right.y * (1.0f - cosAng) + cosAng) * m_Eye.y +
                (m_Right.y * m_Right.z * (1.0f - cosAng) + m_Right.x * sinAng) * m_Eye.z;

    // Calculate new Z component
    float zzz = (m_Right.x * m_Right.z * (1.0f - cosAng) + m_Right.y * sinAng) * m_Eye.x +
                (m_Right.y * m_Right.z * (1.0f - cosAng) - m_Right.x * sinAng) * m_Eye.y +
                (m_Right.z * m_Right.z * (1.0f - cosAng) + cosAng) * m_Eye.z;

    m_Eye = D3DVECTOR(xxx, yyy, zzz); // Set the eye
    m_Eye += vTarget; // Translate back
    
    // Get vector from target to eye
    D3DVECTOR Vec = m_Eye - vTarget;
    //Vec.normalize();
    normalize(Vec);
    
    // Position eye so that's always "kCamDist" from target
    m_Eye = vTarget + (Vec * m_Away);
    
    SetTarget(vTarget); // Build the camera's axes
}
    
// Builds the camera's axes by using the eye, target, and world's up vector
void Camera::SetTarget(const D3DVECTOR &PlayerPos)
{
    // Compute new forward vector (line of sight vector)
    m_Forward = PlayerPos - m_Eye;
    //forward.normalize();
    normalize(m_Forward);
    
    // Compute new right vector
    //m_Right = kWorldUp.crossProduct(forward);
    m_Right = CrossProduct(m_Up, m_Forward);
    //m_Right.normalize();
    normalize(m_Right);
    
    // Compute new camera up vector
    //m_Up = forward.crossProduct(right);
    m_Up = CrossProduct(m_Forward, m_Right);
}



// Normalizes the CVector (ie makes it a unit vector)
void normalize(D3DVECTOR &v)
{
    //assert(!TEqual(magnitude(v), 0.0f, .001f)); // Make sure length isn't zero
    
    float oneOverLen = 1.0f / magnitude(v);
    
    v.x *= oneOverLen;
    v.y *= oneOverLen;
    v.z *= oneOverLen;
}

// Checks for two floats to be equal within a certain tolerance
bool TEqual(float a, float b, float t)
{
    return ((a > b - t) && (a < b + t));
}

// Returns the magnitude (ie length or norm) of the CVector
float magnitude(D3DVECTOR &v)
{
    return sqrtf((v.x * v.x) + (v.y * v.y) + (v.z * v.z));
}