C++ VRTransformPtr::getWorldMatrix方法代码示例

    void snap(Matrix& m) {
        if (csys) C = csys->getWorldMatrix();

        if (orientation == POINT) {
            MatrixLookAt(m, snapP, snapP+Vec3f(prim_o.getPosition()), prim_o.getDirection());
            m.multLeft(C);
        }
    }

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;
}

 Vec3f local(Vec3f p) {
     if (csys) {
         C = csys->getWorldMatrix();
         C.invert();
         Pnt3f pL;
         C.mult(p,pL);
         return Vec3f(pL);
     } else return 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);
}

void VRSnappingEngine::addObject(VRTransformPtr obj, float weight) {
    objects[obj] = obj->getWorldMatrix();
    Vec3f p = obj->getWorldPosition();
    positions->add(p[0], p[1], p[2], obj.get());
}