C++ Matrix4d::mult方法代码示例

void VRMolecule::rotateBond(int a, int b, float f) {
    if (atoms.count(a) == 0) return;
    if (atoms.count(b) == 0) return;
    VRAtom* A = atoms[a];
    VRAtom* B = atoms[b];

	uint now = VRGlobals::CURRENT_FRAME + rand();
    A->recFlag = now;

    Vec3d p1 = Vec3d( A->getTransformation()[3] );
    Vec3d p2 = Vec3d( B->getTransformation()[3] );
    Vec3d dir = p2-p1;
    Quaterniond q(dir, f);
    Matrix4d R;
    R.setRotate(q);

    Matrix4d T;
    T[3] = B->getTransformation()[3];
    Matrix4d _T;
    T.inverse(_T);
    T.mult(R);
    T.mult(_T);

    //cout << "ROTATE bound " << a << "-" << b << " around " << dir << " with " << f << endl;

    B->propagateTransformation(T, now);
    updateGeo();
}

void VRAtom::computePositions() {
    computeGeo();

    string g = geo;
    if (AtomicStructures.count(geo) == 0) { cout << "Error: " << geo << " is invalid!\n"; return; }

    vector<Matrix4d> structure = AtomicStructures[geo];
    for (auto& b : bonds) {
        if (b.first >= (int)structure.size()) break;
        if (b.second.extra) continue;

        Matrix4d T = transformation;
        Matrix4d S = structure[b.first];

        VRAtom* a = b.second.atom2;
        if (a == 0) { // duplets
            float r = 0.5*b.second.atom1->getParams().radius;
            S[3] *= r; S[3][3] = 1;
            T.mult(S);
            T.mult(Pnt3d(1,0,0)*r, b.second.p1);
            T.mult(Pnt3d(-1,-0,0)*r, b.second.p2);
            continue;
        }

        if (a->ID <= ID) continue;
        T.mult(S);
        a->transformation = T;
    }
}

// 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 VRMolecule::substitute(int a, VRMoleculePtr m, int b) {
    if (atoms.count(a) == 0) return;
    if (m->atoms.count(b) == 0) return;

    Matrix4d am = atoms[a]->getTransformation();
    Matrix4d bm = m->atoms[b]->getTransformation();

    map<int, VRBond> bondsA = atoms[a]->getBonds();
    map<int, VRBond> bondsB = m->atoms[b]->getBonds();
    if (bondsA.count(0) == 0) return;
    if (bondsB.count(0) == 0) return;

    VRAtom* A = bondsA[0].atom2;
    VRAtom* B = bondsB[0].atom2;
    int Ai = A->getID();
    int Bi = B->getID();
    remAtom(a);
    m->remAtom(b);

    if (atoms.count(Ai) == 0) { cout << "AA\n"; return; }
    if (m->atoms.count(Bi) == 0) { cout << "BB\n"; return; }

    // copy atoms
    for (auto at : m->atoms) {
        int ID = getID();
        at.second->setID(ID);
        atoms[ID] = at.second;
    }
    m->set(m->getDefinition());

    // attach molecules
    A->append(B, 1, true);

    // transform new atoms
	uint now = VRGlobals::CURRENT_FRAME + rand();
    A->recFlag = now;
    bm.invert();
    Matrix4d Bm = B->getTransformation();
    bm.mult(Bm);
    bm.setTranslate(Vec3d(0,0,0));
    am.mult(bm);
    MatrixLookAt( bm, Vec3d(0,0,0), Vec3d(0,0,1), Vec3d(0,-1,0) );
    bm.mult(am);
    bm[3] = am[3];
    B->propagateTransformation(bm, now);

    updateGeo();
}

void VRAtom::propagateTransformation(Matrix4d& T, uint flag, bool self) {
    if (flag == recFlag) return;
    recFlag = flag;

    if (self) {
        Matrix4d m = T;
        m.mult(transformation);
        transformation = m;
    }

    for (auto& b : bonds) {
        if (b.second.atom2 == 0) { // duplet
            T.mult(b.second.p1, b.second.p1);
            T.mult(b.second.p2, b.second.p2);
        }
        else b.second.atom2->propagateTransformation(T, flag);
    }
}

vruiMatrix *SGVruiMatrix::preTranslated(double x, double y, double z, vruiMatrix *matrix)
{
    Matrix4d translate;
    translate.setIdentity();
    translate.setTranslate(x, y, z);
    SGVruiMatrix *osgVruiMatrix = dynamic_cast<SGVruiMatrix *>(matrix);
    translate.mult(osgVruiMatrix->matrix);
    this->matrix = translate;
    return this;
}

void VRMolecule::setLocalOrigin(int ID) {
    if (atoms.count(ID) == 0) return;

	uint now = VRGlobals::CURRENT_FRAME + rand();
    Matrix4d m = atoms[ID]->getTransformation();
    m.invert();

    Matrix4d im;
    MatrixLookAt( im, Vec3d(0,0,0), Vec3d(0,0,1), Vec3d(0,1,0) );
    im.mult(m);

    atoms[ID]->propagateTransformation(im, now);
}

void frustum::computeProfile() {
    profile.clear();
    Matrix4d m = trans.asMatrix();
    m.invert();

    for (auto d : directions) {
        Vec3d p;
        m.mult(d,p);
        profile.addPoint( Vec2d(p[0], p[1]) );
    }

    if (!profile.isCCW()) profile.reverseOrder();
}