void OSG::loadXYZ(string path, VRTransformPtr res) {
    cout << "load xyz pointcloud " << path << endl;
    res->setName(path);

    try {

        VRGeoData data;
        vector<float> vertex = vector<float>(6);
        int i=0;

        ifstream file(path);
        while (file >> vertex[i]) {
            i++;
            if (i >= 6) {
                i = 0;
                data.pushVert(Pnt3d(vertex[0], vertex[1], vertex[2]), Vec3d(0,1,0), Color3f(vertex[3]/255.0, vertex[4]/255.0, vertex[5]/255.0));
                data.pushPoint();
            }
        }

        if (data.size()) {
            cout << "  assemble geometry.. " << endl;
            auto geo = data.asGeometry("points");
            res->addChild(geo);
        }
    }
    catch (std::exception& ex) { cerr << "Got an std::exception, what=" << ex.what() << endl; return; }
    catch (...) { cerr << "Got an unknown exception" << endl; return; }
}

void VRRecorder::capture() {
    auto v = view.lock();
    if (!v) {
        v = VRSetup::getCurrent()->getView(viewID);
        view = v;
    }
    if (!v) return;
    if (frameLimitReached()) return;

    //int ts = VRGlobals::get()->CURRENT_FRAME;
    VRFrame* f = new VRFrame();
    captures.push_back(f);
    f->capture = v->grab();
    f->timestamp = getTime()*1e-3;

    VRTransformPtr t = v->getCamera();
    if (t == 0) return;
    f->f = t->getFrom();
    f->a = t->getAt();
    f->u = t->getUp();

    f->width = f->capture->getImage()->getWidth();
    f->height = f->capture->getImage()->getHeight();

    if (!frame) initFrame();

    f->transcode(frame, codec_context, sws_context, captures.size()-1);
}

void VRRecorder::setTransform(VRTransformPtr t, int f) {
    if (f >= (int)captures.size() || f < 0) return;
    VRFrame* fr = captures[f];
    cout << "setTransform " << t->getName() << " " << fr->f << endl;
    t->setFrom(fr->f);
    t->setAt(fr->a);
    t->setUp(fr->u);
}

shared_ptr<FContainer> FLogistics::addContainer(VRTransformPtr t) {
    if (t == 0) return 0;
    auto c = shared_ptr<FContainer>(new FContainer());
    t = static_pointer_cast<VRTransform>(t->duplicate(true));
    t->setVisible(true);
    t->setPersistency(0);
    c->setTransformation(t);
    objects[c->getID()] = c;
    return c;
}

void FLogistics::fillContainer(shared_ptr<FContainer> c, int N, VRTransformPtr t) {
    for (int i=0; i<N; i++) {
        auto p = addProduct();
        t = static_pointer_cast<VRTransform>(t->duplicate(true));
        t->setVisible(true);
        t->setPersistency(0);
        p->setTransformation(t);
        c->add( p );
    }
}

void VRSnappingEngine::update() {
    for (auto dev : VRSetupManager::getCurrent()->getDevices()) { // get dragged objects
        VRTransformPtr obj = dev.second->getDraggedObject();
        VRTransformPtr gobj = dev.second->getDraggedGhost();
        if (obj == 0 || gobj == 0) continue;
        if (objects.count(obj) == 0) continue;

        Matrix m = gobj->getWorldMatrix();
        Vec3f p = Vec3f(m[3]);

        bool lastEvent = event->snap;
        event->snap = 0;

        for (auto ri : rules) {
            Rule* r = ri.second;
            if (r->csys == obj) continue;

            if (anchors.count(obj)) {
                for (auto a : anchors[obj]) {
                    Matrix maL = a->getMatrix();
                    Matrix maW = m; maW.mult(maL);
                    Vec3f pa = Vec3f(maW[3]);
                    Vec3f paL = r->local( Vec3f(maW[3]) );
                    Vec3f psnap = r->getSnapPoint(pa);
                    float D = (psnap-paL).length(); // check distance
                    //cout << "dist " << D << " " << pa[1] << " " << paL[1] << " " << psnap[1] << endl;
                    if (!r->inRange(D)) continue;

                    r->snap(m);
                    maL.invert();
                    m.mult(maL);
                    event->set(obj, r->csys, m, dev.second, 1);
                    break;
                }
            } else {
                Vec3f p2 = r->getSnapPoint(p);
                float D = (p2-p).length(); // check distance
                if (!r->inRange(D)) continue;
                r->snap(m);
                event->set(obj, r->csys, m, dev.second, 1);
            }
        }

        obj->setWorldMatrix(m);
        if (lastEvent != event->snap) {
            if (event->snap) snapSignal->trigger<EventSnap>(event);
            else if (obj == event->o1) snapSignal->trigger<EventSnap>(event);
        }
    }

    // update geo
    if (!hintGeo->isVisible()) return;
}

// called from VRTransform::apply_constraints
void VRConstraint::apply(VRTransformPtr obj, VRObjectPtr parent, bool force) {
    if (!active || obj->getPhysics()->isPhysicalized()) return;
    auto now = VRGlobals::CURRENT_FRAME;
    if (apply_time_stamp == now && !force) return;
    apply_time_stamp = now;

    if (local) parent = obj->getParent(true);
    if (auto r = Referential.lock()) parent = r;

    Matrix4d J;
    if (parent) J = parent->getMatrixTo(obj);
    else J = obj->getWorldMatrix();
    J.mult(refMatrixB);
    J.multLeft(refMatrixAI);

    for (int i=0; i<3; i++) { // translation
        if (min[i] > max[i]) continue; // free
        if (min[i] > J[3][i]) J[3][i] = min[i]; // lower bound
        if (max[i] < J[3][i]) J[3][i] = max[i]; // upper bound
    }

    Vec3d angles = VRTransform::computeEulerAngles(J);

    auto sign = [](float a) {
        return a<0?-1:1;
    };

    // TODO: this is not correct, for example [180, 20, 180], corresponds to [0, 160, 0], and not [0, 20, 0] !!
    //  this tries to fix it somewhat, but its not clean!
    if ( abs(angles[0]) > Pi*0.5 && abs(angles[2]) > Pi*0.5) {
        angles[0] -= sign(angles[0])*Pi;
        angles[2] -= sign(angles[2])*Pi;
        angles[1] = Pi - angles[1];
    }

    Vec3d angleDiff;
    for (int i=3; i<6; i++) { // rotation
        if (min[i] > max[i]) continue; // free
        float a = angles[i-3];
        float d1 = min[i]-a; while(d1 > Pi) d1 -= 2*Pi; while(d1 < -Pi) d1 += 2*Pi;
        float d2 = max[i]-a; while(d2 > Pi) d2 -= 2*Pi; while(d2 < -Pi) d2 += 2*Pi;
        if (d1 > 0 && abs(d1) <= abs(d2)) angleDiff[i-3] = d1; // lower bound
        if (d2 < 0 && abs(d2) <= abs(d1)) angleDiff[i-3] = d2; // upper bound
    }
    VRTransform::applyEulerAngles(J, angles + angleDiff);

    J.multLeft(refMatrixA);
    J.mult(refMatrixBI);
    obj->setMatrixTo(J, parent);
}

int path::addPoint(VRTransformPtr t) {
    OSG::Matrix m = t->getWorldMatrix();
    Vec3f p = Vec3f(m[3]);
    Vec3f d = Vec3f(m[2]);
    Vec3f u = Vec3f(m[1]);
    pnt pn(p, d, Vec3f(1,1,1), u);
    points.push_back(pn);
    return points.size() - 1;
}

bool FObject::move(OSG::PathPtr p, float dx) {
    VRTransformPtr trans = getTransformation();
    if (trans == 0) return true;

    bool done = false;
    t += dx;
    if (t >= 1) { t = 1; done = true; }

    Matrix4d m;
    Vec3d dir, up, pos;
    pos = p->getPosition(t);
    p->getOrientation(t, dir, up);

    MatrixLookAt( m, pos, pos+dir, up );
    trans->setWorldMatrix(m);

    if (done) t = 0;
    return done;
}

PyObject* VRScriptManager::loadGeometry(VRScriptManager* self, PyObject *args, PyObject *kwargs) {
    const char* path = "";
    int ignoreCache = 0;
    const char* preset = "OSG";
    const char* parent = "";

    const char* kwlist[] = {"path", "cached", "preset", "parent", NULL};
    string format = "s|iss:loadGeometry";
    if (! PyArg_ParseTupleAndKeywords(args, kwargs, format.c_str(), (char**)kwlist, &path, &ignoreCache, &preset, &parent)) return NULL;

    VRObjectPtr prnt = VRSceneManager::getCurrent()->getRoot()->find( parent );

    VRTransformPtr obj = VRImport::get()->load( path, prnt, ignoreCache, preset);
    if (obj == 0) {
        VRGuiManager::get()->printInfo("Warning: " + string(path) + " not loaded!\n");
        Py_RETURN_NONE;
    }
    obj->setPersistency(0);
    return VRPyTypeCaster::cast(obj);
}

void VRRecorder::capture() {
    auto v = view.lock();
    if (!v) {
        v = VRSetupManager::getCurrent()->getView(viewID);
        view = v;
    }
    if (!v) return;
    if (frameLimitReached()) return;

    //int ts = VRGlobals::get()->CURRENT_FRAME;
    VRFrame* f = new VRFrame();
    captures.push_back(f);
    f->capture = v->grab();
    f->timestamp = glutGet(GLUT_ELAPSED_TIME);

    VRTransformPtr t = v->getCamera();
    if (t == 0) return;
    f->f = t->getFrom();
    f->a = t->getAt();
    f->u = t->getUp();
}

void VRMaterial::setClipPlane(bool active, Vec4f equation, VRTransformPtr beacon) {
    for (int i=0; i<getNPasses(); i++) {
        auto md = mats[i];
        if (md->clipChunk == 0) {
            md->clipChunk = ClipPlaneChunk::create();
            md->mat->addChunk(md->clipChunk);
        }

        md->clipChunk->setEquation(equation);
        md->clipChunk->setEnable  (active);
        if (beacon) md->clipChunk->setBeacon(beacon->getNode());
    }
}

void virtuose::attachTransform(VRTransformPtr trans)
{
    if(vc == 0) return;

    isAttached = true;
    attached = trans;
    VRPhysics* o = trans->getPhysics();
    btMatrix3x3 t = o->getInertiaTensor();
    float inertia[9] {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
    Matrix3ToArray(t,inertia);
    cout<<"\n "<<"\n "<<inertia[0] << "    " <<inertia[1] <<  "    " <<inertia[2] << "\n "<<inertia[3] <<  "    " <<inertia[4] <<  "    " <<inertia[5] << "\n "<<inertia[6] << "    " <<inertia[7] <<"    " << inertia[8]<<"\n ";
    CHECK(virtAttachVO(vc, o->getMass(), inertia));

}

void virtuose::attachTransform(VRTransformPtr trans) {
    //if(!connected()) return;
    isAttached = true;
    attached = trans;
    VRPhysics* o = trans->getPhysics();
    btMatrix3x3 t = o->getInertiaTensor();
    Vec9 inertia;
    Matrix3ToArray(t,inertia.data);
    print(inertia, 9);
    //cout<<"\n virtuose::attachTransform:\n " << inertia[0] << "    " <<inertia[1] <<  "    " <<inertia[2] << "\n "<<inertia[3] <<  "    " <<inertia[4] <<  "    " <<inertia[5] << "\n "<<inertia[6] << "    " <<inertia[7] <<"    " << inertia[8]<<"\n ";
    cout<<"\n virtuose::attachTransform:\n ";
    interface.setObject<Vec9>("inertia", inertia);
    interface.setObject<float>("mass", o->getMass());
    interface.setObject<bool>("doAttach", true);
    //CHECK(virtAttachVO(vc, o->getMass(), inertia));
}

void loadVtk_old(string path, VRTransformPtr res) {
    cout << "load VTK file " << path << endl;
    ifstream file(path.c_str());
    string line;

    auto next = [&]() -> string& {
        getline(file, line);
        return line;
    };

    VTKProject project;
    project.version = splitString( next() )[4];
    project.title = next();
    project.format = next();
    project.dataset = splitString( next() )[1];

    VRGeoData geo; // build geometry

    if (project.dataset == "STRUCTURED_POINTS") {
        auto r = splitString( next() ); Vec3i dims = toValue<Vec3i>( r[1] + " " + r[2] + " " + r[3] );
             r = splitString( next() ); Vec3d p0 = toValue<Vec3d>( r[1] + " " + r[2] + " " + r[3] );
             r = splitString( next() ); Vec3d d = toValue<Vec3d>( r[1] + " " + r[2] + " " + r[3] );

        for (int k=0; k<dims[2]; k++) {
            for (int j=0; j<dims[1]; j++) {
                for (int i=0; i<dims[0]; i++) {
                    geo.pushVert(p0 + Vec3d(d[0]*i, d[1]*j, d[2]*k) );
                    geo.pushPoint();
                }
            }
        }
    }

    if (project.dataset == "STRUCTURED_GRID") {
        auto r = splitString( next() ); Vec3i dims = toValue<Vec3i>( r[1] + " " + r[2] + " " + r[3] );
             r = splitString( next() ); int N = toInt(r[1]); string type = r[2]; // points

        vector<Vec3d> points;
        for (int i=0; i<N; i++) {
            Vec3d p = toValue<Vec3d>( next() );
            points.push_back(p);
            geo.pushVert(p);
            geo.pushPoint();
        }
    }

    if (project.dataset == "RECTILINEAR_GRID") {
        ;
    }

    if (project.dataset == "UNSTRUCTURED_GRID") {
        ;
    }

    if (project.dataset == "POLYDATA") {
        auto r = splitString( next() ); int N = toInt(r[1]); string type = r[2]; // points
        for (int i=0; i<N; i++) geo.pushVert( toValue<Vec3d>( next() ) );

        while (next() != "") {
            r = splitString( line );
            string type = r[0];
            N = toInt(r[1]);
            //int size = toInt(r[2]);
            for (int i=0; i<N; i++) { // for each primitive
                r = splitString( next() );
                int Ni = toInt(r[0]); // length of primitive
                cout << line << "  " << Ni << endl;
                //if (Ni == 2) geo.pushLine(toInt(r[1]), toInt(r[2]));
                if (Ni == 3) geo.pushTri(toInt(r[1]), toInt(r[2]), toInt(r[3]));
                if (Ni == 4) geo.pushQuad(toInt(r[1]), toInt(r[2]), toInt(r[3]), toInt(r[4]));
            }
        }
    }

    if (project.dataset == "FIELD") {
        ;
    }

    // parsing finished
    cout << project.toString() << endl;
    file.close();

    auto m = VRMaterial::create(project.title + "_mat");
    m->setLit(0);
    m->setDiffuse(Color3f(0.3,0.7,1.0));

    VRGeometryPtr g = geo.asGeometry(project.title);
    g->setMaterial(m);
    //g->updateNormals();
    res->addChild( g );
}

void VRHaptic::applyTransformation(VRTransformPtr t) {
    if (!v->connected()) return;
    t->setMatrix(v->getPose());
}

void OSG::loadE57(string path, VRTransformPtr res) {
    cout << "load e57 pointcloud " << path << endl;
    res->setName(path);

    try {
        ImageFile imf(path, "r"); // Read file from disk

        StructureNode root = imf.root();
        if (!root.isDefined("/data3D")) { cout << "File doesn't contain 3D images" << endl; return; }

        e57::Node n = root.get("/data3D");
        if (n.type() != E57_VECTOR) { cout << "bad file" << endl; return; }

        VectorNode data3D(n);
        int64_t scanCount = data3D.childCount(); // number of scans in file
        cout << " file read succefully, it contains " << scanCount << " scans" << endl;

        for (int i = 0; i < scanCount; i++) {
            StructureNode scan(data3D.get(i));
            string sname = scan.pathName();

            CompressedVectorNode points( scan.get("points") );
            string pname = points.pathName();
            auto cN = points.childCount();
            cout << "  scan " << i << " contains " << cN << " points\n";

            StructureNode proto(points.prototype());
            bool hasPos = (proto.isDefined("cartesianX") && proto.isDefined("cartesianY") && proto.isDefined("cartesianZ"));
            bool hasCol = (proto.isDefined("colorRed") && proto.isDefined("colorGreen") && proto.isDefined("colorBlue"));
            if (!hasPos) continue;

            if (hasCol) cout << "   scan has colors\n";
            else cout << "   scan has no colors\n";

            for (int i=0; i<proto.childCount(); i++) {
                auto child = proto.get(i);
                cout << "    scan data: " << child.pathName() << endl;
            }

            vector<SourceDestBuffer> destBuffers;
            const int N = 4;
            double x[N]; destBuffers.push_back(SourceDestBuffer(imf, "cartesianX", x, N, true));
            double y[N]; destBuffers.push_back(SourceDestBuffer(imf, "cartesianY", y, N, true));
            double z[N]; destBuffers.push_back(SourceDestBuffer(imf, "cartesianZ", z, N, true));
            double r[N];
            double g[N];
            double b[N];
            if (hasCol) {
                destBuffers.push_back(SourceDestBuffer(imf, "colorRed", r, N, true));
                destBuffers.push_back(SourceDestBuffer(imf, "colorGreen", g, N, true));
                destBuffers.push_back(SourceDestBuffer(imf, "colorBlue", b, N, true));
            }

            int Nchunk = 1e6; // separate in chunks because of tcmalloc large alloc issues
            VRGeoData data;
            unsigned int gotCount = 0;
            CompressedVectorReader reader = points.reader(destBuffers);
            do {
                if (data.size() > Nchunk) {
                    cout << "  assemble geometry.. " << endl;
                    auto geo = data.asGeometry(pname);
                    res->addChild(geo);
                    data = VRGeoData();
                }

                gotCount = reader.read();
                for (unsigned j=0; j < gotCount; j++) {
                    int v;
                    if (hasCol) v = data.pushVert(Pnt3d(x[j], y[j], z[j]), Vec3d(0,1,0), Color3f(r[j]/255.0, g[j]/255.0, b[j]/255.0));
                    else v = data.pushVert(Pnt3d(x[j], y[j], z[j]), Vec3d(0,1,0));
                    data.pushPoint(v);
                }
            } while(gotCount);
            reader.close();

            if (data.size()) {
                cout << "  assemble geometry.. " << endl;
                auto geo = data.asGeometry(pname);
                res->addChild(geo);
            }
        }

        imf.close();
    }
    catch (E57Exception& ex) { ex.report(__FILE__, __LINE__, __FUNCTION__); return; }
    catch (std::exception& ex) { cerr << "Got an std::exception, what=" << ex.what() << endl; return; }
    catch (...) { cerr << "Got an unknown exception" << endl; return; }
}

void VRMillingWorkPiece::setCuttingTool(VRTransformPtr geo) {
    tool = geo;
    toolPose = geo->getWorldPose();
}

void FObject::setTransformation(VRTransformPtr t) {
    transform = t;
    t->set_orientation_mode(true);
    if (metaData) metaData->switchParent(t);
}

VRTransformPtr VRFactory::loadVRML(string path) { // wrl filepath
    ifstream file(path);
    if (!file.is_open()) { cout << "file " << path << " not found" << endl; return 0; }

    // get file size
    file.seekg(0, ios_base::end);
    size_t fileSize = file.tellg();
    file.seekg(0, ios_base::beg);
    VRProgress prog("load VRML " + path, fileSize);

    int state = 0;
    map<int, string> states;
    states[0] = "Transform "; // 0
    states[1] = "diffuseColor "; // 6
    states[2] = "coord "; // 21 +2
    states[3] = "normal "; // x +2
    states[4] = "coordIndex "; // x +1
    states[5] = "colorIndex "; // x +1
    states[6] = "normalIndex "; // x +1

    Vec3f color;
    Vec3f last_col(-1,-1,-1);

    Geo geo;

    Pnt3f v;
    Vec3f n;
    int i;

    //vector<VRGeometryPtr> geos;
    vector<Geo> geos;
    map<Vec3f, VRMaterialPtr> mats;
    bool new_obj = true;
    bool new_color = true;
    int li = 0;

    string line;
    while ( getline(file, line) ) {
        prog.update( line.size() );
        li++;

        for (auto d : states) {
            //if ( line[d.second.size()-1] != ' ') continue; // optimization
            if ( line.compare(0, d.second.size(), d.second) == 0) {
                //if (state != d.first) cout << "got on line " << li << ": " << states[d.first] << " instead of: " << states[state] << endl;
                switch (d.first) {
                    case 0: break;
                    case 1:
                        new_obj = true;
                        if (line.size() > 12) color = toVec3f( line.substr(12) );
                        if (mats.count(color) == 0) {
                            mats[color] = VRMaterial::create("fmat");
                            mats[color]->setDiffuse(color);
                        }

                        if (color != last_col) {
                            new_color = true;
                            last_col = color;
                        }
                        break;
                    case 2:
                        geo.updateN();
                        break;
                    case 3: break;
                    case 4: break;
                    case 5: break;
                }
                state = d.first+1;
                if (state == 7) state = 0;
                break;
            }
        }

        if (line[0] != ' ') continue;
        if (state == 6) continue; // skip color indices

        stringstream ss(line);
        switch (state) {
            case 3:
                while(ss >> v[0] && ss >> v[1] && ss >> v[2] && ss.get()) {
                    if (!new_color && new_obj) new_obj = !geo.inBB(v); // strange artifacts!!
                    geo.updateBB(v);

                    if (new_obj) {
                        new_obj = false;
                        new_color = false;
                        geo.init(geos, mats[color]);
                    }

                    geo.pos->addValue(v);
                }
                break;
            case 4:
                while(ss >> n[0] && ss >> n[1] && ss >> n[2] && ss.get()) geo.norms->addValue( n );
                break;
            case 5:
                while(ss >> i && ss.get()) if (i >= 0) geo.inds_p->addValue( geo.Np + i );
                break;
            case 0:
                while(ss >> i && ss.get()) if (i >= 0) geo.inds_n->addValue( geo.Nn + i );
//.........这里部分代码省略.........