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

OSG_BASE_DLLMAPPING bool MatrixLookAt(OSG::Matrix &result,
                                      OSG::Pnt3f   from, 
                                      OSG::Pnt3f   at, 
                                      OSG::Vec3f   up    )
{
    Vec3f view;
    Vec3f right;
    Vec3f newup;
    Vec3f tmp;

    view = from - at;
    view.normalize();

    right = up.cross(view);

    if(right.dot(right) < TypeTraits<Real>::getDefaultEps())
    {
        return true;
    }

    right.normalize();

    newup = view.cross(right);

    result.setIdentity ();
    result.setTranslate(from[0], from[1], from[2]);

    Matrix tmpm;

    tmpm.setValue(right, newup, view);

    result.mult(tmpm);

    return false;
}

btCollisionShape* VRPhysics::getConvexShape() {
    btConvexHullShape* shape = new btConvexHullShape();

    OSG::Matrix m;
    OSG::Matrix M = vr_obj->getWorldMatrix();
    M.invert();

    vector<OSG::VRObject*> geos = vr_obj->getObjectListByType("Geometry");
	for (auto g : geos) {
        OSG::VRGeometry* geo = (OSG::VRGeometry*)g;
        if (geo == 0) continue;
        if (geo->getMesh() == 0) continue;
        OSG::GeoVectorPropertyRecPtr pos = geo->getMesh()->getPositions();
        if (pos == 0) continue;

        if (geo != vr_obj) {
            m = geo->getWorldMatrix();
            m.multLeft(M);
        }

		for (unsigned int i = 0; i<pos->size(); i++) {
            OSG::Pnt3f p;
            pos->getValue(p,i);
            if (geo != vr_obj) m.mult(p,p);
            for (int i=0; i<3; i++) p[i] *= scale[i];
            shape->addPoint(btVector3(p[0], p[1], p[2]));
        }
    }

    shape->setMargin(collisionMargin);

    //cout << "\nConstruct Convex shape for " << vr_obj->getName() << endl;
    return shape;
}

OSG::Matrix VRPhysics::fromBTTransform(const btTransform t, OSG::Vec3f scale) {
    OSG::Matrix m = fromBTTransform(t);

    // apply scale
    OSG::Matrix s;
    s.setScale(scale);
    m.mult(s);
    return m;
}

/*! The mouseMove is called by the viewer when the mouse is moved in the
    viewer and this handle is the active one.

    \param x the x-pos of the mouse (pixel)
    \param y the y-pos of the mouse (pixel)
 */
void PlaneMoveManipulator::mouseMove(const Int16 x,
                            const Int16 y)
{
    SLOG << "==============================" << endLog;
    SLOG << "PlaneMoveManipulator::mouseMove() enter x=" << x << " y=" << y << endLog;

    // get the beacon's core (must be ComponentTransform) and it's center
    if( getTarget() == NULL )
    {
        SWARNING << "Handle has no target.\n";
        return;
        
    }
    // get transformation of beacon
    Transform *t = dynamic_cast<Transform *>(getTarget()->getCore());

    if( t == NULL )
    {
        SWARNING << "handled object has no parent transform!\n";
        return;
    }

    Vec3f      translation;       // for matrix decomposition
    Quaternion rotation;
    Vec3f      scaleFactor;
    Quaternion scaleOrientation;

    t->getMatrix().getTransform(translation, rotation, scaleFactor,
                                scaleOrientation);

    OSG::Line viewray;
    getViewport()->getCamera()->calcViewRay(viewray, x, y, *getViewport());

    SLOG << "Manipulator::mouseMove(): viewray: " << viewray << endLog;

    // Get manipulator axes into world space
    OSG::Matrix tm = getTarget()->getToWorld();
    
    Vec3f rot_axis;
    tm.multFull(Vec3f(0,1,0), rot_axis);

    Plane pl(rot_axis, getClickPoint());

    Pnt3f plpoint;
    
    if (pl.intersect(viewray, plpoint) == true) // Ignore moving out of the plane...
    {
        SLOG << "Manipulator::mouseMove(): plpoint: " << plpoint << endLog;
         
        Vec3f      trans = getBaseTranslation();
        Quaternion rot   = getBaseRotation();

        // Get manipulator axes into world space
        Vec3f xp,zp;

        tm.multFull(Vec3f(1,0,0), xp);
        tm.multFull(Vec3f(0,0,1), zp);
        
        if (getActiveSubHandle() == getHandleXNode())
        {
            Line xaxis(getClickPoint(), xp);
            Line zaxis(getClickPoint(), zp);

            Real32 fx = xaxis.getClosestPointT(plpoint);
            Real32 fz = zaxis.getClosestPointT(plpoint);

            SLOG << "Manipulator::mouseMove(): xaxis: " << xaxis << " zaxis: " << zaxis <<endLog;
            SLOG << "Manipulator::mouseMove(): fx: " << fx << " fz: " << fz <<endLog;
        
            // Alternative: transform hitpoint into manip space
            OSG::Matrix m = getTarget()->getToWorld();
            m.invert();
            
            Pnt3f mpoint;
            m.mult(plpoint, mpoint);
            
            SLOG << "Manipulator::mouseMove(): mpoint:" << mpoint << endLog;

            trans = trans + xp * fx + zp * fz;
        }
        else if (getActiveSubHandle() == getHandleZNode())
        {
            Pnt3f wcenter;
            
            tm.multFull(Pnt3f(0,getLength()[1],0), wcenter);
            
            Vec3f vclick, vcurrent;
            
            vclick = getClickPoint() - wcenter;
            vcurrent = plpoint - wcenter;
            
            vclick.normalize();
            vcurrent.normalize();
            
//.........这里部分代码省略.........