C++ SbMatrix::multLeft方法代码示例

void
vpSimulator::moveInternalCamera(vpHomogeneousMatrix &cMf)
{

  SbMatrix matrix;
  SbRotation rotCam;
  SbMatrix rotX;
  rotX.setRotate (SbRotation (SbVec3f(1.0f, 0.0f, 0.0f), (float)M_PI));
  for(unsigned int i=0;i<4;i++)
    for(unsigned int j=0;j<4;j++)
      matrix[(int)j][(int)i]=(float)cMf[i][j];

  matrix= matrix.inverse();
  matrix.multLeft (rotX);
  rotCam.setValue(matrix);


  internalCamera->ref() ;
  internalCamera->orientation.setValue(rotCam);
  internalCamera->position.setValue(matrix[3][0],matrix[3][1],matrix[3][2]);
  internalCamera->unref() ;

  rotX.setRotate (SbRotation (SbVec3f(-1.0f, 0.0f, 0.0f), (float)M_PI));
  matrix.multLeft (rotX);
  rotCam.setValue(matrix);
  internalCameraPosition->ref() ;
  internalCameraPosition->rotation.setValue(rotCam);
  internalCameraPosition->translation.setValue(matrix[3][0],matrix[3][1],matrix[3][2]);
  internalCameraPosition->unref() ;
}

void
vpSimulator::getExternalCameraPosition(vpHomogeneousMatrix &cMf)
{
/*  SoCamera *camera ;
  camera  = this->externalView->getCamera() ;*/
  SoSFVec3f 	position = externalCamera->position ;

  // get the rotation
  SoSFRotation 	orientation = externalCamera->orientation;
  SbVec3f axis ;  float angle ;
  orientation.getValue(axis,angle) ;
  SbRotation rotation(axis,angle) ;

  // get the translation
  SbVec3f t ;
  t = position.getValue() ;

  SbMatrix matrix ;
  matrix.setRotate(rotation) ;

  vpHomogeneousMatrix fMc ;
  SbMatrix rotX;
  rotX.setRotate (SbRotation (SbVec3f(1.0f, 0.0f, 0.0f), (float)M_PI));
  matrix.multLeft (rotX);
  for(unsigned int i=0;i<4;i++)
    for(unsigned int j=0;j<4;j++)
      fMc[j][i]=matrix[(int)i][(int)j];
  fMc[0][3] = t[0] ;
  fMc[1][3] = t[1] ;
  fMc[2][3] = t[2] ;

  cMf = fMc.inverse() ;
}

// used to render shape and non-shape nodes (usually SoGroup or SoSeparator). 
void 
SoBoxSelectionRenderActionP::updateBbox(const SoPath * path)
{
    if (this->camerasearch == NULL) {
        this->camerasearch = new SoSearchAction;
    }

    // find camera used to render node
    this->camerasearch->setFind(SoSearchAction::TYPE);
    this->camerasearch->setInterest(SoSearchAction::LAST);
    this->camerasearch->setType(SoCamera::getClassTypeId());
    this->camerasearch->apply((SoPath*) path);
  
    if (!this->camerasearch->getPath()) {
        // if there is no camera there is no point rendering the bbox
        return;
    }
    this->localRoot->insertChild(this->camerasearch->getPath()->getTail(), 0);
    this->camerasearch->reset();
  
    if (this->bboxaction == NULL) {
        this->bboxaction = new SoGetBoundingBoxAction(SbViewportRegion(100, 100));
    }
    this->bboxaction->setViewportRegion(PUBLIC(this)->getViewportRegion());
    this->bboxaction->apply((SoPath*) path);
  
    SbXfBox3f & box = this->bboxaction->getXfBoundingBox();
  
    if (!box.isEmpty()) {
        // set cube size
        float x, y, z;
        box.getSize(x, y, z);
        this->cube->width  = x;
        this->cube->height  = y;
        this->cube->depth = z;
    
        SbMatrix transform = box.getTransform();
    
        // get center (in the local bbox coordinate system)
        SbVec3f center = box.SbBox3f::getCenter();
    
        // if center != (0,0,0), move the cube
        if (center != SbVec3f(0.0f, 0.0f, 0.0f)) {
            SbMatrix t;
            t.setTranslate(center);
            transform.multLeft(t);
        }
        this->xform->matrix = transform; 
    
        PUBLIC(this)->SoGLRenderAction::apply(this->localRoot);
    }
    // remove camera
    this->localRoot->removeChild(0);
}

/*! \COININTERNAL */
void
SoCenterballDragger::valueChangedCB(void *, SoDragger * d)
{
  SoCenterballDragger * thisp = static_cast<SoCenterballDragger *>(d);
  SbMatrix matrix = thisp->getMotionMatrix();
  SbVec3f t, s;
  SbRotation r, so;

  // Eliminate center variable of matrix
  if (thisp->savedcenter != SbVec3f(0.0f, 0.0f, 0.0f)) {
    SbMatrix trans;
    trans.setTranslate(thisp->savedcenter);
    matrix.multLeft(trans);
    trans.setTranslate(-(thisp->savedcenter));
    matrix.multRight(trans);
  }

  // Do an inverse rotation, using matrix with center eliminated
  // to obtain correct translation
  matrix.getTransform(t, r, s, so);
  SbMatrix rotmat;
  rotmat.setRotate(r);
  //SbMatrix tmp = matrix;
  matrix.multLeft(rotmat.inverse());

  // Update center of object if dragger has translated
  SbVec3f difftrans(matrix[3][0], matrix[3][1], matrix[3][2]);
  if (difftrans != SbVec3f(0.0f, 0.0f, 0.0f)) {
    thisp->centerFieldSensor->detach();
    thisp->center.setValue(thisp->savedcenter + difftrans);
    thisp->centerFieldSensor->attach(&thisp->center);
  }

  thisp->rotFieldSensor->detach();
  if (thisp->rotation.getValue() != r) {
    thisp->rotation = r;
  }
  thisp->rotFieldSensor->attach(&thisp->rotation);
}

SbBool
SbCylinder::intersect(const SbLine &line, SbVec3f &enter, SbVec3f &exit) const
//
////////////////////////////////////////////////////////////////////////
{
    // The intersection will actually be done on a radius 1 cylinder
    // aligned with the y axis, so we transform the line into that
    // space, then intersect, then transform the results back.

    // rotation to y axis
    SbRotation	rotToYAxis(axis.getDirection(), SbVec3f(0,1,0));
    SbMatrix	mtxToYAxis;
    mtxToYAxis.setRotate(rotToYAxis);

    // scale to unit space
    float	scaleFactor = 1.0f/radius;
    SbMatrix	toUnitCylSpace;
    toUnitCylSpace.setScale(SbVec3f(scaleFactor, scaleFactor, scaleFactor));
    toUnitCylSpace.multLeft(mtxToYAxis);

    // find the given line un-translated
    SbVec3f origin = line.getPosition();
    origin -= axis.getPosition();
    SbLine noTranslationLine(origin, origin + line.getDirection());

    // find the un-translated line in unit cylinder's space
    SbLine cylLine;
    toUnitCylSpace.multLineMatrix(noTranslationLine, cylLine);

    // find the intersection on the unit cylinder
    SbVec3f cylEnter, cylExit;
    SbBool intersected = unitCylinderIntersect(cylLine, cylEnter, cylExit);

    if (intersected) {
        // transform back to original space
        SbMatrix fromUnitCylSpace = toUnitCylSpace.inverse();

        fromUnitCylSpace.multVecMatrix(cylEnter, enter);
        enter += axis.getPosition();

        fromUnitCylSpace.multVecMatrix(cylExit, exit);
        exit += axis.getPosition();
    }

    return intersected;
}

int
main(int argc, char ** argv)
{
  SoDB::init();

  // FIXME: istty(stdin). 20010114 mortene.

  int valsread;
  SbMatrix rightmatrix = SbMatrix::identity();
  do {
    SbMat m;
    valsread = fscanf(stdin,
                      "%f %f %f %f "
                      "%f %f %f %f "
                      "%f %f %f %f "
                      "%f %f %f %f\n",
                      &m[0][0], &m[0][1], &m[0][2], &m[0][3],
                      &m[1][0], &m[1][1], &m[1][2], &m[1][3],
                      &m[2][0], &m[2][1], &m[2][2], &m[2][3],
                      &m[3][0], &m[3][1], &m[3][2], &m[3][3]);

    if (valsread == 16) {
      SbMatrix leftmatrix(m);
      rightmatrix.multLeft(leftmatrix);
      for (int i=0; i < 4; i++) {
        for (int j=0; j < 4; j++) {
          (void)fprintf(stdout, "%10.3f ", rightmatrix[i][j]);
        }
        (void)fprintf(stdout, "\n");
      }
      (void)fprintf(stdout, "\n");
    }
  } while (valsread == 16);

  if (valsread != EOF) {
    (void)fflush(stdout);
    (void)fprintf(stderr, "Input data format error!\n");
  }

  return 0;
}