C++ SbMatrix类代码示例

void MicrotubuleTransformOperation::undo() {
    SbMatrix invMat = mMat.inverse();
    SpatialGraphSelection::Iterator iter(mSelection);
    iter.vertices.reset();
    int vNum = iter.vertices.nextSelected();
    while (vNum != -1) {
        McVec3f c = graph->getVertexCoords(vNum);
        SbVec3f t(c.x, c.y, c.z);
        SbVec3f res;
        invMat.multVecMatrix(t, res);
        graph->setVertexCoords(vNum, McVec3f(res[0], res[1], res[2]));
        vNum = iter.vertices.nextSelected();
    }

    iter.edges.reset();
    int eNum = iter.edges.nextSelected();
    while (eNum != -1) {
        McDArray<McVec3f> points = graph->getEdgePoints(eNum);
        for (int p = 0; p < points.size(); p++) {
            SbVec3f t(points[p].x, points[p].y, points[p].z);
            SbVec3f res;
            invMat.multVecMatrix(t, res);
            points[p] = McVec3f(res[0], res[1], res[2]);
        }
        graph->setEdgePoints(eNum, points);
        eNum = iter.edges.nextSelected();
    }
    int numPoints = mSelection.getNumSelectedPoints();
    for (int i = 0; i < numPoints; ++i) {
        SpatialGraphPoint p = mSelection.getSelectedPoint(i);
        McDArray<McVec3f> points = graph->getEdgePoints(p.edgeNum);
        SbVec3f t(points[p.pointNum].x, points[p.pointNum].y,
                  points[p.pointNum].z);
        SbVec3f res;
        invMat.multVecMatrix(t, res);
        points[p.pointNum] = McVec3f(res[0], res[1], res[2]);
        graph->setEdgePoints(p.edgeNum, points);
    }

    // update the transform parameters
    for (int i = 0; i < mTransParams.size(); ++i) {
        appendTransform(mTransParams[i], invMat);
    }
}

void ViewProviderRobotObject::setDragger()
{
    assert(pcDragger==0);
    pcDragger = new SoJackDragger();
    pcDragger->addMotionCallback(sDraggerMotionCallback,this);
    pcTcpRoot->addChild(pcDragger);

    // set the actual TCP position
    Robot::RobotObject* robObj = static_cast<Robot::RobotObject*>(pcObject);
    Base::Placement loc = robObj->Tcp.getValue();
    SbMatrix  M;
    M.setTransform(SbVec3f(loc.getPosition().x,loc.getPosition().y,loc.getPosition().z),
                   SbRotation(loc.getRotation()[0],loc.getRotation()[1],loc.getRotation()[2],loc.getRotation()[3]),
                   SbVec3f(150,150,150)
                   );
    pcDragger->setMotionMatrix(M);


}

/*! \COININTERNAL */
void
SoHandleBoxDragger::valueChangedCB(void * COIN_UNUSED_ARG(f), SoDragger * d)
{
  SoHandleBoxDragger * thisp = static_cast<SoHandleBoxDragger *>(d);
  SbMatrix matrix = thisp->getMotionMatrix();
  SbVec3f trans, scale;
  SbRotation rot, scaleOrient;
  matrix.getTransform(trans, rot, scale, scaleOrient);

  thisp->translFieldSensor->detach();
  if (thisp->translation.getValue() != trans)
    thisp->translation = trans;
  thisp->translFieldSensor->attach(&thisp->translation);

  thisp->scaleFieldSensor->detach();
  if (thisp->scaleFactor.getValue() != scale)
    thisp->scaleFactor = scale;
  thisp->scaleFieldSensor->attach(&thisp->scaleFactor);
}

void
SoSurroundScale::doAction(SoAction *action)
//
////////////////////////////////////////////////////////////////////////
{
    SoState	*state = action->getState();

    SbMatrix theCtm = SoModelMatrixElement::get(state);

    if (cacheOK == FALSE )
	updateMySurroundParams( action, theCtm.inverse() );

    if (doTranslations)
	SoModelMatrixElement::translateBy(state, this, cachedTranslation );
    else
	cachedTranslation.setValue(0,0,0);

    SoModelMatrixElement::scaleBy(state, this, cachedScale );
}

SoXipNeheBoxGenerator::SoXipNeheBoxGenerator()
{
  SO_NODE_CONSTRUCTOR(SoXipNeheBoxGenerator);
  
  SbRotation pitch(SbVec3f(1, 0, 0), 0);  // rotation around X
  SbMatrix pitchM;

  SbRotation yaw(SbVec3f(0, 1, 0), 0);    // rotation around Y
  SbMatrix yawM;

  SbMatrix transM = SbMatrix::identity(); // translation
  SbMatrix compM = SbMatrix::identity();
 
  float xrot = 0;
  float yrot = 0;

  for (int yloop = 1; yloop < 6 /* number of rows */ ; yloop++)
	{
		for (int xloop = 0; xloop < yloop; xloop++)
		{
		
      // another magic formula from Nehe for the translation...
		  transM.setTranslate(SbVec3f(1.4f+(float(xloop)*2.8f)-(float(yloop)*1.4f),((6.0f-float(yloop))*2.4f)-7.0f,-20.0f));
			
      pitch.setValue(SbVec3f(1, 0, 0), (M_PI/180) * (45.0f-(2.0f*yloop)+ xrot));
      pitch.getValue(pitchM);
			
      yaw.setValue(SbVec3f(0,1,0), (M_PI/180) * (45.0f + yrot));
      yaw.getValue(yawM);

      compM = yawM * pitchM * transM;

			SoXipNeheBox* neheBox = new SoXipNeheBox();
      neheBox->transform.setValue(compM);
      neheBox->topColor.setValue(topCol[yloop - 1]);
      neheBox->boxColor.setValue(boxCol[yloop - 1]);
      this->addChild(neheBox);
    }
	}

  
}

void
SbMatrix::setTransform(const SbVec3f &translation,
		 const SbRotation &rotation,
		 const SbVec3f &scaleFactor,
		 const SbRotation &scaleOrientation,
		 const SbVec3f &center)
{
#define TRANSLATE(vec)		m.setTranslate(vec), multLeft(m)
#define ROTATE(rot)		rot.getValue(m), multLeft(m)
    SbMatrix m;

    makeIdentity();
    
    if (translation != SbVec3f(0,0,0))
	TRANSLATE(translation);

    if (center != SbVec3f(0,0,0))
	TRANSLATE(center);

    if (rotation != SbRotation(0,0,0,1))
	ROTATE(rotation);

    if (scaleFactor != SbVec3f(1,1,1)) {
	SbRotation so = scaleOrientation;
	if (so != SbRotation(0,0,0,1))
	    ROTATE(so);
	
	m.setScale(scaleFactor);
	multLeft(m);

	if (so != SbRotation(0,0,0,1)) {
	    so.invert();
	    ROTATE(so);
	}
    }

    if (center != SbVec3f(0,0,0))
	TRANSLATE(-center);

#undef TRANSLATE
#undef ROTATE
}

void SoFCCSysDragger::idleCB(void *data, SoSensor *)
{
    SoFCCSysDragger *sudoThis = reinterpret_cast<SoFCCSysDragger *>(data);
    SoField* field = sudoThis->cameraSensor.getAttachedField();
    if (field)
    {
        SoCamera* camera = static_cast<SoCamera*>(field->getContainer());
        SbMatrix localToWorld = sudoThis->getLocalToWorldMatrix();
        SbVec3f origin;
        localToWorld.multVecMatrix(SbVec3f(0.0, 0.0, 0.0), origin);

        SbViewVolume viewVolume = camera->getViewVolume();
        float radius = sudoThis->draggerSize.getValue() / 2.0;
        float localScale = viewVolume.getWorldToScreenScale(origin, radius);
        SbVec3f scaleVector(localScale, localScale, localScale);
        SoScale *localScaleNode = SO_GET_ANY_PART(sudoThis, "scaleNode", SoScale);
        localScaleNode->scaleFactor.setValue(scaleVector);
        sudoThis->autoScaleResult.setValue(localScale);
    }
}

SbPlane XipGeomUtils::planeFromMatrix(const SbMatrix &m)
{
    SbVec3f translation, scaleFactor, normal;
    SbRotation rotation, scaleOrientation;
    m.getTransform(translation, rotation, scaleFactor, scaleOrientation);

    normal = SbVec3f(m[2][0], m[2][1], m[2][2]);
    normal.normalize();

    return SbPlane(normal, translation);
}

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

void TranslateRadialDragger::
valueChangedCB(
  void *,
  SoDragger *inDragger)
{
  TranslateRadialDragger *myself = (TranslateRadialDragger *) inDragger;

  // Get translation by decomposing motionMatrix.
  SbMatrix motMat = myself->getMotionMatrix();
  SbVec3f trans, scale;
  SbRotation rot, scaleOrient;
  motMat.getTransform(trans, rot, scale, scaleOrient);

  // Set "translation", disconnecting sensor while doing so.
  myself->fieldSensor->detach();
  if(myself->translation.getValue() != trans)
  {
    myself->translation = trans;
  }
  myself->fieldSensor->attach(&(myself->translation));
}

void SoFCCSysDragger::valueChangedCB(void *, SoDragger *d)
{
    SoFCCSysDragger *sudoThis = dynamic_cast<SoFCCSysDragger *>(d);
    assert(sudoThis);
    SbMatrix matrix = sudoThis->getMotionMatrix();

    //all this just to get the translation?
    SbVec3f localTranslation, scaleDummy;
    SbRotation localRotation, scaleOrientationDummy;
    matrix.getTransform(localTranslation, localRotation, scaleDummy, scaleOrientationDummy);

    sudoThis->translationSensor.detach();
    if (sudoThis->translation.getValue() != localTranslation)
        sudoThis->translation = localTranslation;
    sudoThis->translationSensor.attach(&sudoThis->translation);

    sudoThis->rotationSensor.detach();
    if (sudoThis->rotation.getValue() != localRotation)
        sudoThis->rotation = localRotation;
    sudoThis->rotationSensor.attach(&sudoThis->rotation);
}

void RDragger::dragStart()
{
    SoSwitch *sw;
    sw = SO_GET_ANY_PART(this, "rotatorSwitch", SoSwitch);
    SoInteractionKit::setSwitchValue(sw, 1);

    projector.setViewVolume(this->getViewVolume());
    projector.setWorkingSpace(this->getLocalToWorldMatrix());
    projector.setPlane(SbPlane(SbVec3f(0.0, 0.0, 1.0), 0.0));

    SbVec3f hitPoint;
    if (!projector.tryProject(getNormalizedLocaterPosition(), 0.0, hitPoint))
        return;
    hitPoint.normalize();

    SbMatrix localToWorld = getLocalToWorldMatrix();
    localToWorld.multVecMatrix(hitPoint, hitPoint);
    setStartingPoint((hitPoint));

    rotationIncrementCount.setValue(0);
}

/*!
  Should be called after motion matrix has been updated by a child
  dragger.
*/
void
SoCenterballDragger::transferCenterDraggerMotion(SoDragger * childdragger)
{
  if (coin_assert_cast<SoNode *>(childdragger) == XCenterChanger.getValue() ||
      coin_assert_cast<SoNode *>(childdragger) == YCenterChanger.getValue() ||
      coin_assert_cast<SoNode *>(childdragger) == ZCenterChanger.getValue()) {
    // translate part of matrix should not change. Move motion
    // into center instead.

    SbVec3f transl;
    SbMatrix matrix = this->getMotionMatrix();
    transl[0] = matrix[3][0];
    transl[1] = matrix[3][1];
    transl[2] = matrix[3][2];

    SbVec3f difftransl = transl - this->savedtransl;
    { // consider rotation before translating
      SbRotation rot = this->rotation.getValue();
      SbMatrix tmp;
      tmp.setRotate(rot.inverse());
      tmp.multVecMatrix(difftransl, difftransl);
    }

    this->centerFieldSensor->detach();
    this->center = difftransl + this->savedcenter;
    this->centerFieldSensor->attach(&this->center);

    matrix[3][0] = this->savedtransl[0];
    matrix[3][1] = this->savedtransl[1];
    matrix[3][2] = this->savedtransl[2];

    SbBool oldval = this->enableValueChangedCallbacks(FALSE);
    this->setMotionMatrix(matrix);
    this->enableValueChangedCallbacks(oldval);

    SoMatrixTransform *mt = SO_GET_ANY_PART(this, "translateToCenter", SoMatrixTransform);
    matrix.setTranslate(this->center.getValue());
    mt->matrix = matrix;
  }
}

bool
XipGeomUtils::isIntersect(const SbMatrix &mpr, const SbMatrix &model)
{
    SbVec3f s, origin;
    SbRotation r, so;
    model.getTransform(origin, r, s, so);

    SbVec3f axisX, axisY, axisZ;
    axisX.setValue(model[0][0], model[0][1], model[0][2]);
    axisY.setValue(model[1][0], model[1][1], model[1][2]);
    axisZ.setValue(model[2][0], model[2][1], model[2][2]);

    SbVec3f pt0[12], pt1[12];
    pt0[0] = origin;
    pt1[0] = origin + axisX;
    pt0[1] = origin + axisX;
    pt1[1] = origin + axisX + axisY;
    pt0[2] = origin + axisX + axisY;
    pt1[2] = origin + axisY;
    pt0[3] = origin + axisY;
    pt1[3] = origin;

    pt0[4] = origin + axisZ;
    pt1[4] = origin + axisX + axisZ;
    pt0[5] = origin + axisX + axisZ;
    pt1[5] = origin + axisX + axisY + axisZ;
    pt0[6] = origin + axisX + axisY + axisZ;
    pt1[6] = origin + axisY + axisZ;
    pt0[7] = origin + axisY + axisZ;
    pt1[7] = origin + axisZ;

    pt0[8] = origin;
    pt1[8] = pt0[8] + axisZ;
    pt0[9] = origin + axisX;
    pt1[9] = pt0[9] + axisZ;
    pt0[10] = origin + axisX + axisY;
    pt1[10] = pt0[10] + axisZ;
    pt0[11] = origin + axisY;
    pt1[11] = pt0[11] + axisZ;

    axisX.setValue(mpr[0][0], mpr[0][1], mpr[0][2]);
    axisY.setValue(mpr[1][0], mpr[1][1], mpr[1][2]);
    origin.setValue(mpr[3][0], mpr[3][1], mpr[3][2]);
    SbPlane plane(origin, origin+axisX, origin+axisY);

    for (int i=0; i<12; ++i)
    {
        if ( isIntersect(pt0[i], pt1[i], plane) ) return true;
    }

    return false;
}

void 
SoXipOverlayTransformBoxManip::scale( SoHandleEventAction* action )
{
	// Extract the rotation matrix from the view matrix
	SbMatrix rotationMatrix = extractRotationMatrix( mXBoundingBox.getInverse() );

	// Get the two control points involved in the computation of the scale
	// matrix (the one initially picked by the user and its opposit control
	// point.
	const SbVec3f& cp3d = mControlPointsCoords->point[ mControlPointId ];
	const SbVec3f& opp_cp3d = mControlPointsCoords->point[ (mControlPointId + 4) % 8 ];

	// Get the projection in the world of the current mouse position 
	SbVec3f p3d;
	getPoint( action, p3d );

	// Project the control points and the mouse world position on a xy plane
	SbVec3f p2d, cp2d, opp_cp2d;
	rotationMatrix.multVecMatrix( cp3d, cp2d );
	rotationMatrix.multVecMatrix( opp_cp3d, opp_cp2d );
	rotationMatrix.multVecMatrix( p3d, p2d );

	SbVec3f refVec = cp2d - opp_cp2d;
	SbVec3f movVec = p2d - opp_cp2d;	

	SbVec3f scaleVec(1, 1, 1);
	for( int i = 0; i < 3; ++ i )
	{
		if( fabs( refVec[i] ) > 0.001 )
			scaleVec[i] = movVec[i] / refVec[i];
	}

	// Compute the 2D scale matrix
	SbMatrix scaleMatrix2D;
	scaleMatrix2D.setScale( scaleVec );

	// Use the opposit control point to center the shape (scale only regarding
	// the picked control point)
	SbMatrix centerMatrix;
	centerMatrix.setTranslate( opp_cp3d );

	mTransformationMatrix =  centerMatrix.inverse()		// Translate a point to the origin
		* rotationMatrix								// Rotate the plane to a xy aligned plane
		* scaleMatrix2D									// Scale the point in this plane
		* rotationMatrix.inverse()						// Rotate back to initial plane
		* centerMatrix;									// Undo the first translation

	transform( mActionNode );
}