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

void ViewProviderFemConstraintFixed::updateData(const App::Property* prop)
{
    // Gets called whenever a property of the attached object changes
    Fem::ConstraintFixed* pcConstraint = static_cast<Fem::ConstraintFixed*>(this->getObject());

#ifdef USE_MULTIPLE_COPY
    if (pShapeSep->getNumChildren() == 0) {
        // Set up the nodes
        SoMultipleCopy* cp = new SoMultipleCopy();
        cp->matrix.setNum(0);
        cp->addChild((SoNode*)createFixed(HEIGHT, WIDTH));
        pShapeSep->addChild(cp);
    }
#endif

    if (strcmp(prop->getName(),"Points") == 0) {
        const std::vector<Base::Vector3d>& points = pcConstraint->Points.getValues();
        const std::vector<Base::Vector3d>& normals = pcConstraint->Normals.getValues();
        if (points.size() != normals.size())
            return;
        std::vector<Base::Vector3d>::const_iterator n = normals.begin();

#ifdef USE_MULTIPLE_COPY
        SoMultipleCopy* cp = static_cast<SoMultipleCopy*>(pShapeSep->getChild(0));
        cp->matrix.setNum(points.size());
        SbMatrix* matrices = cp->matrix.startEditing();
        int idx = 0;
#else
        // Note: Points and Normals are always updated together
        pShapeSep->removeAllChildren();
#endif

        for (std::vector<Base::Vector3d>::const_iterator p = points.begin(); p != points.end(); p++) {
            SbVec3f base(p->x, p->y, p->z);
            SbVec3f dir(n->x, n->y, n->z);
            SbRotation rot(SbVec3f(0,-1,0), dir);
#ifdef USE_MULTIPLE_COPY
            SbMatrix m;
            m.setTransform(base, rot, SbVec3f(1,1,1));
            matrices[idx] = m;
            idx++;
#else
            SoSeparator* sep = new SoSeparator();
            createPlacement(sep, base, rot);
            createFixed(sep, HEIGHT, WIDTH);
            pShapeSep->addChild(sep);
#endif
            n++;
        }
#ifdef USE_MULTIPLE_COPY
        cp->matrix.finishEditing();
#endif
    }

    ViewProviderFemConstraint::updateData(prop);
}

SbMatrix tgf::MatrixFromSoTransform( SoTransform* const & soTransform )
{
	SbMatrix sbMatrix;
	sbMatrix.setTransform( 	soTransform->translation.getValue(),
							soTransform->rotation.getValue(),
						    soTransform->scaleFactor.getValue(),
						    soTransform->scaleOrientation.getValue(),
						    soTransform->center.getValue() );

	return sbMatrix;

}

/*! \COININTERNAL */
void
SoRotateSphericalDragger::fieldSensorCB(void *d, SoSensor *)
{
  assert(d);
  SoRotateSphericalDragger * thisp = THISP(d);
  SbMatrix matrix = thisp->getMotionMatrix();
  SbVec3f trans, scale;
  SbRotation rot, scaleOrient;
  matrix.getTransform(trans, rot, scale, scaleOrient);
  matrix.setTransform(trans, thisp->rotation.getValue(), scale, scaleOrient);
  thisp->setMotionMatrix(matrix);
}

SbMatrix ManipWidget::filteredMatrix(const SbMatrix &mat, ComponentMask components)
{
	SbVec3f xlat, scaleFactor;
	SbRotation rot, scaleOrient;
	mat.getTransform(xlat, rot, scaleFactor, scaleOrient);

	SbMatrix ret;
	ret.setTransform(
				components & COMPONENT_XLAT ? xlat : SbVec3f(0, 0, 0),
				components & COMPONENT_ROT ? rot : SbRotation(),
				components & COMPONENT_SCALE ? scaleFactor : SbVec3f(1, 1, 1),
				components & COMPONENT_SCALE ? scaleOrient : SbRotation()
				);
	return ret;
}

/*! \COININTERNAL */
void
SoScale2Dragger::fieldSensorCB(void * d, SoSensor *)
{
  assert(d);
  SoScale2Dragger * thisp = THISP(d);
  SbMatrix matrix = thisp->getMotionMatrix();

  SbVec3f t, s;
  SbRotation r, so;

  matrix.getTransform(t, r, s, so);
  s = thisp->scaleFactor.getValue();
  matrix.setTransform(t, r, s, so);
  thisp->setMotionMatrix(matrix);
}

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


}

  const SbOctTree * getOctTree(void) {
    if (this->octtree == NULL) {
      const SbOctTreeFuncs funcs = {
        NULL /* ptinsidefunc */,
        PrimitiveData::insideboxfunc,
        NULL /* insidespherefunc */,
        NULL /* insideplanesfunc */
      };

      SbBox3f b = this->getBoundingBox();
      // Add a 1% slack to the bounding box, to avoid problems in
      // SbOctTree due to floating point inaccuracies (see assert() in
      // SbOctTree::addItem()).
      //
      // This may be just a temporary hack -- see the FIXME at the
      // same place as mentioned above.
      SbMatrix m;
      m.setTransform(SbVec3f(0, 0, 0), // translation
                     SbRotation::identity(), // rotation
                     SbVec3f(1.01f, 1.01f, 1.01f), // scalefactor
                     SbRotation::identity(), // scaleorientation
                     SbVec3f(b.getCenter())); // center
      b.transform(m);

      this->octtree = new SbOctTree(b, funcs);

      if (ida_debug()) {
        SoDebugError::postInfo("PrimitiveData::getOctTree",
                               "made new octtree for PrimitiveData %p", this);
      }

      for (unsigned int k = 0; k < this->numTriangles(); k++) {
        SbTri3f * t = this->getTriangle(k);
        this->octtree->addItem(t);
      }
    }
    return this->octtree;
  }

// Documented in superclass.
void
SoComposeMatrix::evaluate()
{
  int numTranslation = this->translation.getNum();
  int numRotation = this->rotation.getNum();
  int numScaleFactor = this->scaleFactor.getNum();
  int numScaleOrientation = this->scaleOrientation.getNum();
  int numCenter = this->center.getNum();

  int numOut = numTranslation > numRotation? numTranslation:numRotation;
  int numOut2 =
    numScaleFactor>numScaleOrientation?numScaleFactor:numScaleOrientation;
  numOut2 = numOut2>numCenter?numOut2:numCenter;
  numOut = numOut>numOut2?numOut:numOut2;

  SO_ENGINE_OUTPUT(matrix,SoMFMatrix,setNum(numOut));

  int i;

  for (i=0;i<numOut;i++) {
    const SbVec3f translationVal=
      i<numTranslation?this->translation[i]:this->translation[numTranslation-1];
    const SbVec3f scaleFactorVal=
      i<numScaleFactor?this->scaleFactor[i]:this->scaleFactor[numScaleFactor-1];
    const SbVec3f centerVal=i<numCenter?this->center[i]:this->center[numCenter-1];
    const SbRotation rotationVal=
      i<numRotation?this->rotation[i]:this->rotation[numRotation-1];
    const SbRotation scaleOrientationVal=
      i<numScaleOrientation?
      this->scaleOrientation[i]:this->scaleOrientation[numScaleOrientation-1];

    SbMatrix mat;
    mat.setTransform(translationVal,rotationVal,scaleFactorVal,
                     scaleOrientationVal,centerVal);
    SO_ENGINE_OUTPUT(matrix,SoMFMatrix,set1Value(i,mat));
  }
}

//.........这里部分代码省略.........
        origin[1] = t[1];
        origin[2] = t[2];
    }

    axisX[0] = model[0][0];
    axisX[1] = model[0][1];
    axisX[2] = model[0][2];

    axisY[0] = model[1][0];
    axisY[1] = model[1][1];
    axisY[2] = model[1][2];

    axisZ[0] = model[2][0];
    axisZ[1] = model[2][1];
    axisZ[2] = model[2][2];

    SbVec3f vert[8];
    vert[0] = origin;
    vert[1] = origin + axisX;
    vert[2] = origin + axisX + axisY;
    vert[3] = origin + axisY;

    vert[4] = vert[0] + axisZ;
    vert[5] = vert[1] + axisZ;
    vert[6] = vert[2] + axisZ;
    vert[7] = vert[3] + axisZ;

    mpr.getTransform ( t, r, s, so ) ;
    axisX[0] = mpr[0][0];
    axisX[1] = mpr[0][1];
    axisX[2] = mpr[0][2];
    axisX.normalize();

    axisY[0] = mpr[1][0];
    axisY[1] = mpr[1][1];
    axisY[2] = mpr[1][2];
    axisY.normalize();

    SbPlane plane(t, t+axisX, t+axisY);

    vector<SbVec3f> pts;
    pts.clear();
    SbVec3f pt;
    for (int i=0; i<12; ++i)
    {
        if ( intersect(vert[edge[i][0]], vert[edge[i][1]], plane, pt) )
        {
            pts.push_back(pt);
        }
    }

    for (int i=0; i<8; ++i)
    {
        if ( isOnPlane(vert[i], plane) )
        {
            pts.push_back(vert[i]);
        }
    }

    if ( pts.size()<3 ) return false;

    SbVec3f u;
    SbVec2f minv(numeric_limits<float>::max(), numeric_limits<float>::max());
    SbVec2f maxv(-numeric_limits<float>::max(), -numeric_limits<float>::max());
    float tmp;
    for (size_t i=0; i<pts.size(); ++i)
    {
        u = pts[i] - t;
        tmp = u.dot(axisX);
        if ( minv[0]>tmp ) minv[0] = tmp;
        if ( maxv[0]<tmp ) maxv[0] = tmp;

        tmp = u.dot(axisY);
        if ( minv[1]>tmp ) minv[1] = tmp;
        if ( maxv[1]<tmp ) maxv[1] = tmp;
    }

    SbVec2f center = 0.5f*(minv+maxv);

    origin = t + center[0]*axisX + center[1]*axisY;

    if ( !XipGeomUtils::isOnPlane(origin, mpr) )
    {
        XipDebug::output("Error: new center is not on the plane");
    }

    SbVec3f pt0 = t + minv[0]*axisX + minv[1]*axisY;
    SbVec3f pt1 = t + maxv[0]*axisX + minv[1]*axisY;
    SbVec3f pt2 = t + maxv[0]*axisX + maxv[1]*axisY;

    float scale = (pt1 - pt0).length();
    u = pt2 - pt1;
    if ( scale<u.length() ) scale = u.length();
    scale *= 1.01f;

    newMpr.setTransform(origin, r, SbVec3f(scale, scale, scale), so);
    //XipDebug::output("%f --> %f", s[0], scale);

    return true;
}

void ViewProviderFemConstraintFluidBoundary::updateData(const App::Property* prop)
{
    // Gets called whenever a property of the attached object changes
    Fem::ConstraintFluidBoundary* pcConstraint = static_cast<Fem::ConstraintFluidBoundary*>(this->getObject());
    float scaledwidth = WIDTH * pcConstraint->Scale.getValue(); //OvG: Calculate scaled values once only
    float scaledheight = HEIGHT * pcConstraint->Scale.getValue();

    float scaledheadradius = ARROWHEADRADIUS * pcConstraint->Scale.getValue(); //OvG: Calculate scaled values once only
    float scaledlength = ARROWLENGTH * pcConstraint->Scale.getValue();

    std::string boundaryType = pcConstraint->BoundaryType.getValueAsString();
    if (strcmp(prop->getName(),"BoundaryType") == 0)
    {
        if (boundaryType == "wall")
        {
            FaceColor.setValue(0.0,1.0,1.0);
        }
        else if (boundaryType == "interface")
        {
            FaceColor.setValue(0.0,1.0,0.0);
        }
        else if (boundaryType == "freestream")
        {
            FaceColor.setValue(1.0,1.0,0.0);
        }
        else if(boundaryType == "inlet")
        {
            FaceColor.setValue(1.0,0.0,0.0);
        }
        else //(boundaryType == "outlet")
        {
            FaceColor.setValue(0.0,0.0,1.0);
        }
    }
        
    if (boundaryType == "inlet" || boundaryType == "outlet"){
#ifdef USE_MULTIPLE_COPY
        //OvG: need access to cp for scaling
        SoMultipleCopy* cp = new SoMultipleCopy();
        if (pShapeSep->getNumChildren() == 0) {
            // Set up the nodes
            cp->matrix.setNum(0);
            cp->addChild((SoNode*)createArrow(scaledlength , scaledheadradius)); //OvG: Scaling
            pShapeSep->addChild(cp);
        }
#endif

        if (strcmp(prop->getName(),"Points") == 0) {
            const std::vector<Base::Vector3d>& points = pcConstraint->Points.getValues();

#ifdef USE_MULTIPLE_COPY
            cp = static_cast<SoMultipleCopy*>(pShapeSep->getChild(0));
            cp->matrix.setNum(points.size());
            SbMatrix* matrices = cp->matrix.startEditing();
            int idx = 0;
#else
            // Redraw all arrows
            pShapeSep->removeAllChildren();
#endif
            // This should always point outside of the solid
            Base::Vector3d normal = pcConstraint->NormalDirection.getValue();

            // Get default direction (on first call to method)
            Base::Vector3d forceDirection = pcConstraint->DirectionVector.getValue();
            if (forceDirection.Length() < Precision::Confusion())
                forceDirection = normal;

            SbVec3f dir(forceDirection.x, forceDirection.y, forceDirection.z);
            SbRotation rot(SbVec3f(0,1,0), dir);

            for (std::vector<Base::Vector3d>::const_iterator p = points.begin(); p != points.end(); p++) {
                SbVec3f base(p->x, p->y, p->z);
                if (forceDirection.GetAngle(normal) < M_PI_2) // Move arrow so it doesn't disappear inside the solid
                    base = base + dir * scaledlength; //OvG: Scaling
#ifdef USE_MULTIPLE_COPY
                SbMatrix m;
                m.setTransform(base, rot, SbVec3f(1,1,1));
                matrices[idx] = m;
                idx++;
#else
                SoSeparator* sep = new SoSeparator();
                createPlacement(sep, base, rot);
                createArrow(sep, scaledlength, scaledheadradius); //OvG: Scaling
                pShapeSep->addChild(sep);
#endif
            }
#ifdef USE_MULTIPLE_COPY
            cp->matrix.finishEditing();
#endif
        }
        else if (strcmp(prop->getName(),"DirectionVector") == 0) { // Note: "Reversed" also triggers "DirectionVector"
            // Re-orient all arrows
            Base::Vector3d normal = pcConstraint->NormalDirection.getValue();
            Base::Vector3d forceDirection = pcConstraint->DirectionVector.getValue();
            if (forceDirection.Length() < Precision::Confusion())
                forceDirection = normal;

            SbVec3f dir(forceDirection.x, forceDirection.y, forceDirection.z);
            SbRotation rot(SbVec3f(0,1,0), dir);

//.........这里部分代码省略.........

void SoXipImageAttributes::evaluate()
{
	SoXipDataImage *imgData = image.getValue();

	if (imgData)
	{
		SbXipImage *img = imgData->get();
		if (img)
		{
			SO_ENGINE_OUTPUT(modelMatrix, SoSFMatrix, setValue(img->getModelMatrix()));
			SO_ENGINE_OUTPUT(bitsStored, SoSFShort, setValue(img->getBitsStored()));

			SO_ENGINE_OUTPUT(width,  SoSFShort, setValue(img->getDimStored()[0]));
			SO_ENGINE_OUTPUT(height, SoSFShort, setValue(img->getDimStored()[1]));
			SO_ENGINE_OUTPUT(depth,  SoSFShort, setValue(img->getDimStored()[2]));

			SbMatrix modelMat = img->getModelMatrix();

			SbVec3f t, s;
			SbRotation r, so;
			modelMat.getTransform(t, r, s, so);
			modelMat.multVecMatrix(SbVec3f(0.5, 0.5, 0.5), t);

			// scale MPR model matrix always to max. individual dimension by default
			float maxScale = s[0] > s[1] ? s[0] : s[1] > s[2] ? s[1] : s[2];
//			modelMat.setTransform(t, r, SbVec3f(maxScale, maxScale, maxScale), so);

			// when using get/setTransform, the rotation is derived from normal vector
			// but for gantry tilt, we need to compute normal from row and column vector
			SbVec3f rot[3];
			rot[0] = SbVec3f(modelMat[0][0], modelMat[0][1], modelMat[0][2]);
			rot[1] = SbVec3f(modelMat[1][0], modelMat[1][1], modelMat[1][2]);
			rot[2] = rot[0].cross(rot[1]);

			rot[0].normalize();
			rot[1].normalize();
			rot[2].normalize();
			rot[0] *= maxScale;
			rot[1] *= maxScale;
			rot[2] *= maxScale;

			modelMat = SbMatrix(
				rot[0][0], rot[0][1], rot[0][2], 0,
				rot[1][0], rot[1][1], rot[1][2], 0,
				rot[2][0], rot[2][1], rot[2][2], 0,
				t[0], t[1], t[2], 1);

			// update engine outputs
			SbMatrix tmp = SbMatrix::identity();

			// flip default viewing direction
			tmp.setRotate(SbRotation(SbVec3f(1, 0, 0), M_PI));

			SbMatrix defOrient = tmp * modelMat;

			// adjust so plane falls onto original plane
			defOrient.getTransform(t, r, s, so);
			SbVec3f object;
			modelMat = img->getModelMatrix();
			modelMat.inverse().multVecMatrix(t, object);

			object[0] = int(object[0] * img->getDimStored()[0] + 0.5);
			object[1] = int(object[1] * img->getDimStored()[1] + 0.5);
			object[2] = int(object[2] * img->getDimStored()[2] + 0.5);

			object[0] /= img->getDimStored()[0];
			object[1] /= img->getDimStored()[1];
			object[2] /= img->getDimStored()[2];

			modelMat.multVecMatrix(object, t);
			defOrient.setTransform(t, r, s, so);

			SO_ENGINE_OUTPUT(defaultOrientation, SoSFMatrix, setValue(defOrient));

			SbMatrix ortho1, ortho2, ortho3;
			int which = XipGeomUtils::orthoOrientations(defOrient, ortho1, ortho2, ortho3);
			SO_ENGINE_OUTPUT(orthoScanOrientation, SoSFShort, setValue(which));

			SO_ENGINE_OUTPUT(orthoOrientation1, SoSFMatrix, setValue(ortho1));
			SO_ENGINE_OUTPUT(orthoOrientation2, SoSFMatrix, setValue(ortho2));
			SO_ENGINE_OUTPUT(orthoOrientation3, SoSFMatrix, setValue(ortho3));

			defOrient.getTransform(t, r, s, so);
			SO_ENGINE_OUTPUT(defaultCenter, SoSFVec3f, setValue(t));

			return;
		}
	}

	SO_ENGINE_OUTPUT(modelMatrix, SoSFMatrix, setValue(SbMatrix::identity()));
	SO_ENGINE_OUTPUT(bitsStored, SoSFShort, setValue(0));

	SO_ENGINE_OUTPUT(width,  SoSFShort, setValue(0));
	SO_ENGINE_OUTPUT(height, SoSFShort, setValue(0));
	SO_ENGINE_OUTPUT(depth,  SoSFShort, setValue(0));

	SbMatrix rot1, rot2;
	SO_ENGINE_OUTPUT(defaultOrientation, SoSFMatrix, setValue(SbMatrix::identity()));
	SO_ENGINE_OUTPUT(orthoScanOrientation, SoSFShort, setValue(0));
	SO_ENGINE_OUTPUT(orthoOrientation1, SoSFMatrix, setValue(SbMatrix::identity()));
//.........这里部分代码省略.........

void ViewProviderFemConstraintPressure::updateData(const App::Property* prop)
{
    // Gets called whenever a property of the attached object changes
    Fem::ConstraintPressure* pcConstraint = static_cast<Fem::ConstraintPressure*>(this->getObject());
    float scaledheadradius = ARROWHEADRADIUS * pcConstraint->Scale.getValue(); //OvG: Calculate scaled values once only
    float scaledlength = ARROWLENGTH * pcConstraint->Scale.getValue();

#ifdef USE_MULTIPLE_COPY
    //OvG: always need access to cp for scaling
    SoMultipleCopy* cp = new SoMultipleCopy();
    if (pShapeSep->getNumChildren() == 0) {
        // Set up the nodes
        cp->matrix.setNum(0);
        cp->addChild((SoNode*)createArrow(scaledlength , scaledheadradius)); //OvG: Scaling
        pShapeSep->addChild(cp);
    }
#endif

    if (strcmp(prop->getName(),"Points") == 0) {
        const std::vector<Base::Vector3d>& points = pcConstraint->Points.getValues();
        const std::vector<Base::Vector3d>& normals = pcConstraint->Normals.getValues();
        if (points.size() != normals.size()) {
            return;
        }
        std::vector<Base::Vector3d>::const_iterator n = normals.begin();

#ifdef USE_MULTIPLE_COPY      
        cp = static_cast<SoMultipleCopy*>(pShapeSep->getChild(0)); //OvG: Use top cp
        cp->matrix.setNum(points.size());
        SbMatrix* matrices = cp->matrix.startEditing();
        int idx = 0;
#else
        // Redraw all arrows
        pShapeSep->removeAllChildren();
#endif

        for (std::vector<Base::Vector3d>::const_iterator p = points.begin(); p != points.end(); p++) {
            SbVec3f base(p->x, p->y, p->z);
            SbVec3f dir(n->x, n->y, n->z);
            double rev;
            if (pcConstraint->Reversed.getValue()) {
                base = base + dir * scaledlength; //OvG: Scaling
                rev = 1;
            } else {
                rev = -1;
            }
            SbRotation rot(SbVec3f(0, rev, 0), dir);
#ifdef USE_MULTIPLE_COPY
            SbMatrix m;
            m.setTransform(base, rot, SbVec3f(1,1,1));
            matrices[idx] = m;
            idx++;
#else
            SoSeparator* sep = new SoSeparator();
            createPlacement(sep, base, rot);
            createArrow(sep, scaledlength , scaledheadradius); //OvG: Scaling
            pShapeSep->addChild(sep);
#endif
            n++;
        }
#ifdef USE_MULTIPLE_COPY
        cp->matrix.finishEditing();
#endif
    }

    ViewProviderFemConstraint::updateData(prop);
}

//.........这里部分代码省略.........
		}
		// Axis 5
		searchAction.setName("FREECAD_AXIS5");
		searchAction.setInterest(SoSearchAction::FIRST);
		searchAction.setSearchingAll(FALSE);
		searchAction.apply(pcRobotRoot);
		path = searchAction.getPath();
		if(path){
			SoNode* node = path->getTail();
			std::string typeName = (const char*)node->getTypeId().getName();
			if (!node || node->getTypeId() != SoVRMLTransform::getClassTypeId())
				throw; // should not happen
			Axis5Node = static_cast<SoVRMLTransform *>(node);
		}
		// Axis 6
		searchAction.setName("FREECAD_AXIS6");
		searchAction.setInterest(SoSearchAction::FIRST);
		searchAction.setSearchingAll(FALSE);
		searchAction.apply(pcRobotRoot);
		path = searchAction.getPath();
		if(path){
			SoNode* node = path->getTail();
			std::string typeName = (const char*)node->getTypeId().getName();
			if (!node || node->getTypeId() != SoVRMLTransform::getClassTypeId())
				throw; // should not happen
			Axis6Node = static_cast<SoVRMLTransform *>(node);
		}
		if(Axis1Node)
			Axis1Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis1.getValue()*(M_PI/180));
		if(Axis2Node)
			Axis2Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis2.getValue()*(M_PI/180));
		if(Axis3Node)
			Axis3Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis3.getValue()*(M_PI/180));
		if(Axis4Node)
			Axis4Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis4.getValue()*(M_PI/180));
		if(Axis5Node)
			Axis5Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis5.getValue()*(M_PI/180));
		if(Axis6Node)
			Axis6Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis6.getValue()*(M_PI/180));
    }else if (prop == &robObj->Axis1) {
        if(Axis1Node){
			Axis1Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis1.getValue()*(M_PI/180));
            if(toolShape)
                toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
        }
    }else if (prop == &robObj->Axis2) {
        if(Axis2Node){
			Axis2Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis2.getValue()*(M_PI/180));
            if(toolShape)
                toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
        }
    }else if (prop == &robObj->Axis3) {
        if(Axis3Node){
			Axis3Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis3.getValue()*(M_PI/180));
            if(toolShape)
                toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
        }
    }else if (prop == &robObj->Axis4) {
        if(Axis4Node){
			Axis4Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis4.getValue()*(M_PI/180));
            if(toolShape)
                toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
        }
    }else if (prop == &robObj->Axis5) {
        if(Axis5Node){
			Axis5Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis5.getValue()*(M_PI/180));
            if(toolShape)
                toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
        }
    }else if (prop == &robObj->Axis6) {
        if(Axis6Node){
			Axis6Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis6.getValue()*(M_PI/180));
            if(toolShape)
                toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
        }
	}else if (prop == &robObj->Tcp) {
        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)
                       );
        if(pcDragger)
            pcDragger->setMotionMatrix(M);
        if(toolShape)
            toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
		//pcTcpTransform->translation = SbVec3f(loc.getPosition().x,loc.getPosition().y,loc.getPosition().z);
		//pcTcpTransform->rotation = SbRotation(loc.getRotation()[0],loc.getRotation()[1],loc.getRotation()[2],loc.getRotation()[3]);
	}else if (prop == &robObj->ToolShape) {
        App::DocumentObject* o = robObj->ToolShape.getValue<App::DocumentObject*>();

        if(o && (o->isDerivedFrom(Part::Feature::getClassTypeId()) || o->isDerivedFrom(App::VRMLObject::getClassTypeId())) ){
            //Part::Feature *p = dynamic_cast<Part::Feature *>(o);
            toolShape = Gui::Application::Instance->getViewProvider(o);
            toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
        }else
            toolShape = 0;
 	}

}

// Execute full set of intersection detection operations on all the
// primitives that has been souped up from the scene graph.
void
SoIntersectionDetectionAction::PImpl::doIntersectionTesting(void)
{
  if (this->callbacks.empty()) {
    SoDebugError::postWarning("SoIntersectionDetectionAction::PImpl::doIntersectionTesting",
                              "intersection testing invoked, but no callbacks set up");
    return;
  }

  delete this->traverser;
  this->traverser = NULL;

  if (ida_debug()) {
    SoDebugError::postInfo("SoIntersectionDetectionAction::PImpl::doIntersectionTesting",
                           "total number of shapedata items == %d",
                           this->shapedata.getLength());

  }

  const SbOctTreeFuncs funcs = {
    NULL /* ptinsidefunc */,
    shapeinsideboxfunc,
    NULL /* insidespherefunc */,
    NULL /* insideplanesfunc */
  };

  SbBox3f b = this->fullxfbbox.project();
  // Add a 1% slack to the bounding box, to avoid problems in
  // SbOctTree due to floating point inaccuracies (see assert() in
  // SbOctTree::addItem()).
  //
  // This may be just a temporary hack -- see the FIXME at the
  // same place.
  SbMatrix m;
  m.setTransform(SbVec3f(0, 0, 0), // translation
                 SbRotation::identity(), // rotation
                 SbVec3f(1.01f, 1.01f, 1.01f), // scalefactor
                 SbRotation::identity(), // scaleorientation
                 SbVec3f(b.getCenter())); // center
  b.transform(m);

  SbOctTree shapetree(b, funcs);
  for (int k = 0; k < this->shapedata.getLength(); k++) {
    ShapeData * shape = this->shapedata[k];
    if (shape->xfbbox.isEmpty()) { continue; }
    shapetree.addItem(shape);
  }

  if (ida_debug()) { shapetree.debugTree(stderr); }

  // For debugging.
  unsigned int nrshapeshapeisects = 0;
  unsigned int nrselfisects = 0;

  const float theepsilon = this->getEpsilon();

  for (int i = 0; i < this->shapedata.getLength(); i++) {
    ShapeData * shape1 = this->shapedata[i];

    // If the shape has no geometry, immediately skip to next
    // iteration of for-loop.
    if (shape1->xfbbox.isEmpty()) { continue; }

    // Remove shapes from octtree as we iterate over the full set, to
    // avoid self-intersection and to avoid checks against other
    // shapes happening both ways.
    shapetree.removeItem(shape1);

    // FIXME: shouldn't we also invoke the filter-callback here? 20030403 mortene.
    if (this->internalsenabled) {
      nrselfisects++;
      SbBool cont;
      this->doInternalPrimitiveIntersectionTesting(shape1->getPrimitives(), cont);
      if (!cont) { goto done; }
    }

    SbBox3f shapebbox = shape1->xfbbox.project();
    if (theepsilon > 0.0f) {
      const SbVec3f e(theepsilon, theepsilon, theepsilon);
      // Extend bbox in all 6 directions with the epsilon value.
      shapebbox.getMin() -= e;
      shapebbox.getMax() += e;
    }
    SbList<void*> candidateshapes;
    shapetree.findItems(shapebbox, candidateshapes);

    if (ida_debug()) {
      SoDebugError::postInfo("SoIntersectionDetectionAction::PImpl::doIntersectionTesting",
                             "shape %d intersects %d other shapes",
                             i, candidateshapes.getLength());

      // debug, dump to .iv-file the "master" shape bbox given by i,
      // plus ditto for all intersected shapes
#if 0
      if (i == 4) {
        SoSeparator * root = new SoSeparator;
        root->ref();

//.........这里部分代码省略.........

void doClipping(SbVec3f trans, SbRotation rot)
{
  SbMatrix mat;
  SbVec3f normal;

  mat.setTransform(trans, rot, SbVec3f(1,1,1));
  mat.multDirMatrix(SbVec3f(0, -1, 0), normal);
  SbPlane plane(normal, trans);

  const float coords[][3] = {
    {-5,-5,-5},
    {5,-5,-5},
    {5,5,-5},
    {-5,5,-5},
    {-5,-5,5},
    {5,-5,5},
    {5,5,5},
    {-5,5,5}
  };
  const int indices[] = {
    0,3,2,1,-1,
    0,1,5,4,-1,
    2,6,5,1,-1,
    3,7,6,2,-1,
    3,0,4,7,-1,
    7,4,5,6,-1
  };

  // Clip box against plane

  SbClip clip;
  SoMFVec3f * globalVerts = 
    (SoMFVec3f *)SoDB::getGlobalField(SbName("globalVerts"));
  SoMFVec3f * globalTVerts = 
    (SoMFVec3f *)SoDB::getGlobalField(SbName("globalTVerts"));
  SoMFInt32 * globalnv = 
    (SoMFInt32 *)SoDB::getGlobalField(SbName("globalnv"));
  globalVerts->startEditing();
  globalVerts->setNum(0);
  globalTVerts->startEditing();
  globalTVerts->setNum(0);
  globalnv->startEditing();
  globalnv->setNum(0);
  int i;
  for (i = 0;i<6*5;i++) {
    if (indices[i] == -1) {
      clip.clip(plane);
      int numVerts = clip.getNumVertices();
      if (numVerts > 0) {
        for (int j = 0;j<numVerts;j++) {
          SbVec3f v;
          clip.getVertex(j, v);
          globalVerts->set1Value(globalVerts->getNum(), v);
          v += SbVec3f(5, 5, 5);
          v /= 10.0;
          globalTVerts->set1Value(globalTVerts->getNum(), v);
        }
        globalnv->set1Value(globalnv->getNum(), numVerts);
      }
      clip.reset();
    }
    else clip.addVertex(coords[indices[i]]);
  }
  globalVerts->finishEditing();
  globalTVerts->finishEditing();
  globalnv->finishEditing();

  // Close hole in clipped box by clipping against all 6 planes
  
  const SbVec3f planecoords[] = {
    SbVec3f(-10,0,-10),
    SbVec3f(10,0,-10),
    SbVec3f(10,0,10),
    SbVec3f(-10,0,10)
  };

  
  clip.reset();
  for (i = 0;i<4;i++) {
    SbVec3f v;
    mat.multVecMatrix(planecoords[i], v);
    clip.addVertex(v);
  }
  for (i = 0;i<6*5;i+=5) {
    SbPlane p(coords[indices[i+2]],
              coords[indices[i+1]],
              coords[indices[i]]);
    clip.clip(p);
  }
  int numVerts = clip.getNumVertices();
  SoMFVec3f * planeVerts = 
    (SoMFVec3f *)SoDB::getGlobalField(SbName("planeVerts"));
  SoMFVec3f * planeTVerts = 
    (SoMFVec3f *)SoDB::getGlobalField(SbName("planeTVerts"));
  planeVerts->startEditing();
  planeVerts->setNum(0);
  planeTVerts->startEditing();
  planeTVerts->setNum(0);
  for (i = 0;i<numVerts;i++) {
    SbVec3f v;
//.........这里部分代码省略.........