Matrix4d virtuose::getPose(float f[7]) {
Matrix4d m;
Vec3d pos(f[1], f[2], f[0]);
Quaterniond q;
q.setValue(f[4], f[5], f[3]);
m.setRotate(q);
m.setTranslate(pos);
return m;
}
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 FContainer::add(shared_ptr<FProduct> p) {
auto t = p->getTransformation();
t->hide();
products.push_back(p);
setMetaData("Nb: " + toString(products.size()));
Matrix4d wm;
wm = t->getWorldMatrix();
t->switchParent(getTransformation());
wm.setTranslate(getTransformation()->getWorldPosition());
t->setWorldMatrix(wm);
t->updateChange();
}
void FNode::set(shared_ptr<FObject> o) {
object = o;
if (o == 0) { setState(FREE); return; }
if (o->getType() == FObject::CONTAINER) setState(CONTAINER);
if (o->getType() == FObject::PRODUCT) setState(PRODUCT);
if (o->getTransformation() == 0) return;
auto t = o->getTransformation();
Matrix4d wm;
wm = t->getWorldMatrix();
t->switchParent(getTransform());
wm.setTranslate(getTransform()->getWorldPosition());
t->setWorldMatrix(wm);
t->updateChange();
}
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();
}
