3D coords to 2D screen coords

stock Float:MatrixMultiply(Float:mat1[4][4], Float:mat2[4][4])
{
    new Float:matRet[4][4];
    for (new i = 0; i < 4; i++)
        for (new j = 0; j < 4; j++)
            for (new k = 0; k < 4; k++)
                matRet[i][j] += mat1[i][k] * mat2[k][j];
    return matRet;
}

stock Float:MatrixMultiplyVector(Float:mat[4][4], Float:vec[4])
{
    new Float:vTmp[4];
    for (new i = 0; i < 4; i++)
        for (new k = 0; k < 4; k++)
            vTmp[i] += vec[k] * mat[k][i];
    return vTmp;
}

stock calcPosInScreen(playerid, nScreenW, nScreenH, Float:fObjectPos[3])
{
    new str[256];

    new Float:matProj[4][4],
        Float:matView[4][4],
        Float:matWorld[4][4],
        Float:matRet[4][4];

    for (new i = 0; i < 4; i++)
        for (new j = 0; j < 4; j++)
            matProj[i][j] = 0.0;

    for (new i = 0; i < 4; i++)
        for (new j = 0; j < 4; j++)
            matView[i][j] = 0.0;

    for (new i = 0; i < 4; i++)
        for (new j = 0; j < 4; j++)
            matWorld[i][j] = 0.0;

    for (new i = 0; i < 4; i++)
        for (new j = 0; j < 4; j++)
            matRet[i][j] = 0.0;

    new Float:fYScale = 1.0 / floattan(27.0, degrees),
        Float:fZFar = 10.0,
        Float:fZNear = 0.5;

    new bWideScreen = 0;

    matProj[0][0] = fYScale * nScreenH / nScreenW * 1.2;
    matProj[1][1] = fYScale;

    if (bWideScreen)
        matProj[1][1] *= 1.33;

    matProj[2][2] = fZFar / (fZFar - fZNear);
    matProj[3][2] = -fZNear * fZFar / (fZFar - fZNear);
    matProj[2][3] = 1;


    new Float:fCamPos[3],
        Float:fCamLookAt[3];
    GetPlayerCameraPos(playerid, fCamPos[0], fCamPos[1], fCamPos[2]);
    GetPlayerCameraFrontVector(playerid, fCamLookAt[0], fCamLookAt[1], fCamLookAt[2]);

    new Float:fYaw = atan2(fCamLookAt[0], fCamLookAt[1]),
        Float:fPitch = acos(fCamLookAt[2]) + 270.0,
        Float:fRoll = 0.0;

    if (fYaw < 0.0)
        fYaw += 360.0;

    fYaw = 90.0;
    fPitch = 0.0;

    fCamPos[0] = -0.1;
    fCamPos[1] = 0.0;
    fCamPos[2] = fHeight;

    new Float:fSinYaw = floatsin(fYaw, degrees),
        Float:fCosYaw = floatcos(fYaw, degrees),
        Float:fSinPitch = floatsin(fPitch, degrees),
        Float:fCosPitch = floatcos(fPitch, degrees),
        Float:fSinRoll = floatsin(fRoll, degrees),
        Float:fCosRoll = floatcos(fRoll, degrees);

    matView[0][0] = fCosYaw * fCosRoll + fSinYaw * fSinPitch * fSinRoll;
    matView[0][1] = -fCosYaw * fSinRoll + fSinYaw * fSinPitch * fCosRoll;
    matView[0][2] = fSinYaw * fCosPitch;

    matView[1][0] = fSinRoll * fCosPitch;
    matView[1][1] = fCosRoll * fCosPitch;
    matView[1][2] = -fSinPitch;

    matView[2][0] = -fSinYaw * fCosRoll + fCosYaw * fSinPitch * fSinRoll;
    matView[2][1] = fSinRoll * fSinYaw + fCosYaw * fSinPitch * fCosRoll;
    matView[2][2] = fCosYaw * fCosPitch;

    matView[3][3] = 1.0;
    matView[3][0] = -(fCamPos[0] * matView[0][0] + fCamPos[2] * matView[1][0] + fCamPos[1] * matView[2][0]);
    matView[3][1] = -(fCamPos[0] * matView[0][1] + fCamPos[2] * matView[1][1] + fCamPos[1] * matView[2][1]);
    matView[3][2] = -(fCamPos[0] * matView[0][2] + fCamPos[2] * matView[1][2] + fCamPos[1] * matView[2][2]);


    matWorld[0][0] = 1.0;
    matWorld[1][1] = 1.0;
    matWorld[2][2] = 1.0;
    matWorld[3][3] = 1.0;
    //matWorld[3][0] = fObjectPos[0];
    //matWorld[3][1] = fObjectPos[2];
    //matWorld[3][2] = fObjectPos[1];

    matRet = MatrixMultiply(matView, matProj);

    matRet = MatrixMultiply(matWorld, matRet);

    new Float:fNewVec[4];
    fNewVec[0] = fObjectPos[0];
    fNewVec[1] = fObjectPos[2];
    fNewVec[2] = fObjectPos[1];
    fNewVec[3] = 1.0;
    fNewVec = MatrixMultiplyVector(matRet, fNewVec);

    fNewVec[0] /= fNewVec[3];
    fNewVec[1] /= fNewVec[3];
    fNewVec[2] /= fNewVec[3];

    if (fNewVec[0] < -1.0 || fNewVec[0] > 1.0 || fNewVec[1] < -1.0 || fNewVec[1] > 1.0 || fNewVec[2] < fZNear || fNewVec[2] > fZFar)
        format(str, 256, "Object out of screen");
    else
    {
        fNewVec[0] = (fNewVec[0] + 1) * nScreenW / 2;
        fNewVec[1] = nScreenH / 2 - fNewVec[1] * nScreenH / 2;
        format(str, 256, "%0.5f %0.5f %0.5f", fNewVec[0], fNewVec[1], fNewVec[2]);
    }
    SendClientMessage(playerid, -1, str);
}