3D envelope

#include <unordered_set>
#include <unordered_map>
#include <fstream>
#include <iostream>
#include <chrono>
#include <memory>

using namespace std;

static string IN_FILE = "points_uniq.txt";
static double NORM_ZERO = 1e-9;
static double ZERO = 1e-15;

class Point3D {

public:
    int id;

private:
    double x;
    double y;
    double z;

public:
    Point3D() : Point3D(0, 0, 0) {}
    Point3D(int iid, double ix, double iy, double iz) : id(iid), x(ix), y(iy), z(iz) {}

    Point3D(double x, double y, double z) :Point3D(-1, x, y, z) {}

    bool operator== (const Point3D & x) const {
        return id == x.id;
    }

    Point3D neg() const {
        return Point3D(-x, -y, -z);
    }

    static Point3D add(initializer_list<Point3D> pts) {
        double x = 0;
        double y = 0;
        double z = 0;
        for (Point3D pt : pts) {
            x += pt.x;
            y += pt.y;
            z += pt.z;
        }
        return Point3D(x, y, z);
    }

    static Point3D sub(const Point3D a, const Point3D b) {
        return Point3D(a.x - b.x, a.y - b.y, a.z - b.z);
    }

    Point3D mul(double d) const {
        return Point3D(x * d, y * d, z * d);
    }

    static double scal(const Point3D a, const Point3D b) {
        return a.x * b.x + a.y * b.y + a.z * b.z;
    }

    static Point3D prod(const Point3D a, const Point3D b) {
        double x = a.y * b.z - a.z * b.y;
        double y = a.z * b.x - a.x * b.z;
        double z = a.x * b.y - a.y * b.x;

        return Point3D(x, y, z);
    }

    double size() const {
        double d = scal(*this, *this);
        return sqrt(d);
    }

    Point3D norm() const {
        double d = size();
        if (d < ZERO)
            return *this;
        return mul(1 / d);
    }
};

class Line {

    Point3D a;
    Point3D b;
    bool mark;

public:
    Line(Point3D ia, Point3D ib): a(ia), b(ib), mark(false) {}

    void doMark() {
        mark = !mark;
    }

    bool isMark() const {
        return mark;
    }

    Point3D beg() const {
        return a;
    }

    Point3D end() const {
        return b;
    }

    Point3D beg(bool rev) const {
        return rev ? b : a;
    }

    Point3D end(bool rev) const {
        return rev ? a : b;
    }

    Point3D dir(bool rev) const {
        Point3D d = Point3D::sub(b, a);
        return rev ? d.neg() : d;
    }

    void addIDs(unordered_set<int> & pts) const {
        pts.insert(a.id);
        pts.insert(b.id);
    }
};

class Triangle {

    Point3D norm;
    shared_ptr<Line> a;
    shared_ptr<Line> b;
    shared_ptr<Line> c;
    bool arev;
    bool brev;
    bool crev;

public:
    Triangle(Point3D a, Point3D b, Point3D c, Point3D * p) : Triangle(make_shared<Line>(a, b), make_shared<Line>(b, c), make_shared<Line>(c, a), p) {}

    Triangle(shared_ptr<Line> a, shared_ptr<Line> b, shared_ptr<Line> c, Point3D * p) : Triangle(a, b, c, p, false, false) {}

    Triangle(shared_ptr<Line> ia, shared_ptr<Line> ib, shared_ptr<Line> ic, Point3D * p, bool iarev, bool icrev) : a(ia), b(ib), c(ic), arev(iarev), crev(icrev), norm(0, 0, 0) {
        brev = a->end(arev) == b->end();

        bool ba = a->beg(arev) == c->end(crev);
        bool bb = b->beg(brev) == a->end(arev);
        bool bc = c->beg(crev) == b->end(brev);
        if (!(ba && bb && bc))
            cout << "failed ori\n";

        Point3D da = a->dir(arev);
        Point3D dc = c->dir(!crev);

        Point3D n = Point3D::prod(da, dc);
        if (p != nullptr) {
            double d = Point3D::scal(n, Point3D::sub(*p, a->beg()));
            if (d < 0)
                n = n.mul(-1);
            if (fabs(d) < NORM_ZERO)
                cout << "inv norm " << d << "\n";
        }
        norm = n.norm();
    }

    bool isValid() const {
        return Point3D::scal(norm, norm) > 1e-5;
    }

    double getOri(const Point3D p) const {
        auto diff = Point3D::sub(p, a->beg());
        auto sc = Point3D::scal(norm, diff);
        return sc;
    }

    void marklines() {
        a->doMark();
        b->doMark();
        c->doMark();
    }

    void addMarked(list<shared_ptr<Line>> & bord) {
        if (a->isMark())
            bord.push_back(a);
        if (b->isMark())
            bord.push_back(b);
        if (c->isMark())
            bord.push_back(c);
    }

    void addIDs(unordered_set<int> & pts) {
        a->addIDs(pts);
        b->addIDs(pts);
        c->addIDs(pts);
    }
};

class Envelope {

    Point3D in;
    list<shared_ptr<Triangle>> tri;

public:
    Envelope(Point3D iin, shared_ptr<Triangle> tr) : in(iin) {
        tri.push_back(tr);
    }

    void addPoint(Point3D pt) {
        for (auto tr = tri.begin(); tr != tri.end();)
        {
            if ((*tr)->getOri(pt) < NORM_ZERO) {
                tr = tri.erase(tr);
                continue;
            }
            (*tr)->marklines();
            tr++;
        }

        list<shared_ptr<Line>> bord;
        for (auto tr : tri)
            tr->addMarked(bord);

        unordered_map<int, shared_ptr<Line>> map;
        for (auto lin : bord) {
            Point3D b = lin->beg(false);
            Point3D e = lin->end(false);

            shared_ptr<Line> x = map[b.id];
            if (x == nullptr) {
                x = make_shared<Line>(pt, b);
                map[b.id] = x;
            }
            bool xrev = x->end() == pt;

            shared_ptr<Line> z = map[e.id];
            if (z == nullptr) {
                z = make_shared<Line>(e, pt);
                map[e.id] = z;
            }
            bool zrev = z->beg() == pt;

            shared_ptr<Triangle> tr = make_shared<Triangle>(x, lin, z, &in, xrev, zrev);
            if (tr->isValid())
                tri.push_back(tr);
            else
                cout << "inv triangle" << "\n";
            lin->doMark();
        }
    }

    size_t getPtCnt() {
        unordered_set<int> pts;
        for (auto tr : tri)
            tr->addIDs(pts);

        return pts.size();
    }

    size_t getTriCnt() {
        return tri.size();
    }

};

static int ptCnt;

Point3D getPoint(istream & sc) {
    double x, y, z;
    sc >> x;
    sc >> y;
    sc >> z;
    if (sc.eof())
        return Point3D();
    return Point3D(++ptCnt, x, y, z);
}

Envelope * initEnvelope(istream & sc) {
    Point3D a = getPoint(sc);
    Point3D b = getPoint(sc);

    vector<Point3D> pts;

    Point3D c(0,0,0);

    unique_ptr<Triangle> tr = nullptr;
    while (!sc.eof()) {
        c = getPoint(sc);
        tr = make_unique<Triangle>(a, b, c, nullptr);
        if (tr->isValid())
            break;
        pts.push_back(c);
    }

    // TODO: if !tr valid
    Point3D d(0,0,0);
    while (!sc.eof()) {
        d = getPoint(sc);
        double n = tr->getOri(d);
        if (fabs(n) > NORM_ZERO)
            break;
        pts.push_back(d);
    }

    Point3D in = Point3D::add({ a, b, c, d }).mul(1 / 4.0);
    Envelope * env = new Envelope(in, make_shared<Triangle>(a, b, c, &in));
    env->addPoint(d);

    for each (Point3D pt in pts)
        env->addPoint(pt);
    return env;
}

using namespace std::chrono;

int main(char ** arg) {
    ptCnt = 0;

    for (int i = 0; i < 20; i++) {
        ifstream sc(IN_FILE);

        auto start = high_resolution_clock::now();
        Envelope * env = initEnvelope(sc);

        while (!sc.eof()) {
            Point3D pt = getPoint(sc);
            if (pt.id >= 0)
                env->addPoint(pt);
        }
        auto end = high_resolution_clock::now();
        sc.close();

        auto diff = duration_cast<duration<double>>(end - start);

        cout << "time: " << diff.count() << "\n";
        cout << "poly cnt: " << env->getTriCnt() << " pt cnt: " << ptCnt << " pts: " << env->getPtCnt() << "\n";

        delete env;
    }
    return 0;
}