Lowest Common Ancestor of a Binary Tree, with Parent Pointer

int getHeight(Node* p)
{
    int height = 0;
    while (p)
    {
        height++;
        p = p->parent;
    }
    return height;
}

Node* LCA(Node* p, Node *q)
{
    int h1 = getHeight(p);
    int h2 = getHeight(q);
    if (h1 > h2)
    {
        swap(h1, h2);
        swap(p, q);
    }
    for (int h = 0; h < h2-h1; h++)
    {
        q = q->parent;
    }
    while (p && q)
    {
        if (p == q)
            return p;
        p = p->parent;
        q = q->parent;
    }
    return NULL;
}