C++ Placement::getPosition方法代码示例

Command Command::transform(const Base::Placement other)
{
    Base::Placement plac = getPlacement();
    plac *= other;
    double xval, yval, zval, aval, bval, cval;
    xval = plac.getPosition().x;
    yval = plac.getPosition().y;
    zval = plac.getPosition().z;
    plac.getRotation().getYawPitchRoll(aval,bval,cval);
    Command c = Command();
    c.Name = Name;
    for(std::map<std::string,double>::const_iterator i = Parameters.begin(); i != Parameters.end(); ++i) {
        std::string k = i->first;
        double v = i->second;
        if (k == "X")
            v = xval;
        if (k == "Y")
            v = yval;
        if (k == "Z")
            v = zval;
        if (k == "A")
            v = aval;
        if (k == "B")
            v = bval;
        if (k == "C")
            v = cval;
        c.Parameters[k] = v;
    }
    return c;
}

void Command::setFromPlacement (const Base::Placement &plac)
{
    Name = "G1";
    Parameters.clear();
    std::string x = "X";
    std::string y = "Y";
    std::string z = "Z";
    std::string a = "A";
    std::string b = "B";
    std::string c = "C";
    double xval, yval, zval, aval, bval, cval;
    xval = plac.getPosition().x;
    yval = plac.getPosition().y;
    zval = plac.getPosition().z;
    plac.getRotation().getYawPitchRoll(aval,bval,cval);
    if (xval != 0.0)
        Parameters[x] = xval;
    if (yval != 0.0)
        Parameters[y] = yval;
    if (zval != 0.0)
        Parameters[z] = zval;
    if (aval != 0.0)
        Parameters[a] = aval;
    if (bval != 0.0)
        Parameters[b] = bval;
    if (cval != 0.0)
        Parameters[c] = cval;
}

TopLoc_Location FeatureReference::getLocation() const
{
    Base::Placement pl = this->Placement.getValue();
    Base::Rotation rot(pl.getRotation());
    Base::Vector3d axis;
    double angle;
    rot.getValue(axis, angle);
    gp_Trsf trf;
    trf.SetRotation(gp_Ax1(gp_Pnt(), gp_Dir(axis.x, axis.y, axis.z)), angle);
    trf.SetTranslationPart(gp_Vec(pl.getPosition().x,pl.getPosition().y,pl.getPosition().z));
    return TopLoc_Location(trf);
}

void ViewProviderDragger::updateTransform(const Base::Placement& from, SoTransform* to)
{
  float q0 = (float)from.getRotation().getValue()[0];
  float q1 = (float)from.getRotation().getValue()[1];
  float q2 = (float)from.getRotation().getValue()[2];
  float q3 = (float)from.getRotation().getValue()[3];
  float px = (float)from.getPosition().x;
  float py = (float)from.getPosition().y;
  float pz = (float)from.getPosition().z;
  to->rotation.setValue(q0,q1,q2,q3);
  to->translation.setValue(px,py,pz);
  to->center.setValue(0.0f,0.0f,0.0f);
  to->scaleFactor.setValue(1.0f,1.0f,1.0f);
}

void TaskRobot6Axis::viewTool(const Base::Placement pos)
{
    double A,B,C;
    pos.getRotation().getYawPitchRoll(A,B,C);

    QString result = QString::fromAscii("Tool:( %1, %2, %3, %4, %5, %6 )")
        .arg(Base::UnitsApi::toDblWithUserPrefs(Base::Length,pos.getPosition().x),0,'f',1)
        .arg(Base::UnitsApi::toDblWithUserPrefs(Base::Length,pos.getPosition().y),0,'f',1)
        .arg(Base::UnitsApi::toDblWithUserPrefs(Base::Length,pos.getPosition().z),0,'f',1)
        .arg(Base::UnitsApi::toDblWithUserPrefs(Base::Angle,A),0,'f',1)
        .arg(Base::UnitsApi::toDblWithUserPrefs(Base::Angle,B),0,'f',1)
        .arg(Base::UnitsApi::toDblWithUserPrefs(Base::Angle,C),0,'f',1);

    ui->label_Tool->setText(result);
}

void Placement::setPlacementData(const Base::Placement& p)
{
    signalMapper->blockSignals(true);
    ui->xPos->setValue(Base::Quantity(p.getPosition().x, Base::Unit::Length));
    ui->yPos->setValue(Base::Quantity(p.getPosition().y, Base::Unit::Length));
    ui->zPos->setValue(Base::Quantity(p.getPosition().z, Base::Unit::Length));

    double Y,P,R;
    p.getRotation().getYawPitchRoll(Y,P,R);
    ui->yawAngle->setValue(Base::Quantity(Y, Base::Unit::Angle));
    ui->pitchAngle->setValue(Base::Quantity(P, Base::Unit::Angle));
    ui->rollAngle->setValue(Base::Quantity(R, Base::Unit::Angle));

    // check if the user-defined direction is already there
    bool newitem = true;
    double angle;
    Base::Vector3d axis;
    p.getRotation().getValue(axis, angle);
    ui->angle->setValue(Base::Quantity(angle*180.0/D_PI, Base::Unit::Angle));
    Base::Vector3d dir(axis.x,axis.y,axis.z);
    for (int i=0; i<ui->direction->count()-1; i++) {
        QVariant data = ui->direction->itemData (i);
        if (data.canConvert<Base::Vector3d>()) {
            const Base::Vector3d val = data.value<Base::Vector3d>();
            if (val == dir) {
                ui->direction->setCurrentIndex(i);
                newitem = false;
                break;
            }
        }
    }

    if (newitem) {
        // add a new item before the very last item
        QString display = QString::fromAscii("(%1,%2,%3)")
            .arg(dir.x)
            .arg(dir.y)
            .arg(dir.z);
        ui->direction->insertItem(ui->direction->count()-1, display,
            QVariant::fromValue<Base::Vector3d>(dir));
        ui->direction->setCurrentIndex(ui->direction->count()-2);
    }
    signalMapper->blockSignals(false);
}

void ViewProviderRobotObject::DraggerMotionCallback(SoDragger *dragger)
{
    float q0, q1, q2, q3;

    Robot::RobotObject* robObj = static_cast<Robot::RobotObject*>(pcObject);
    Base::Placement Tcp = robObj->Tcp.getValue();
    const SbMatrix & M = dragger->getMotionMatrix ();
    SbVec3f    translation;
    SbRotation rotation;
    SbVec3f    scaleFactor;
    SbRotation scaleOrientation;
    SbVec3f    center(Tcp.getPosition().x,Tcp.getPosition().y,Tcp.getPosition().z);
    M.getTransform(translation, rotation, scaleFactor, scaleOrientation);
    rotation.getValue(q0, q1, q2, q3);
    //Base::Console().Message("M %f %f %f\n", M[3][0], M[3][1], M[3][2]);
    Base::Rotation rot(q0, q1, q2, q3);
    Base::Vector3d pos(translation[0],translation[1],translation[2]);
    robObj->Tcp.setValue(Base::Placement(pos,rot));
}

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


}

void TaskDatumParameters::updateSuperplacementUI()
{
    Part::Datum* pcDatum = static_cast<Part::Datum*>(DatumView->getObject());
    Base::Placement pl = pcDatum->superPlacement.getValue();
    Base::Vector3d pos = pl.getPosition();
    Base::Rotation rot = pl.getRotation();
    double yaw, pitch, roll;
    rot.getYawPitchRoll(yaw, pitch, roll);

    bool bBlock = true;
    ui->superplacementX->blockSignals(bBlock);
    ui->superplacementY->blockSignals(bBlock);
    ui->superplacementZ->blockSignals(bBlock);
    ui->superplacementYaw->blockSignals(bBlock);
    ui->superplacementPitch->blockSignals(bBlock);
    ui->superplacementRoll->blockSignals(bBlock);

    ui->superplacementX->setValue(Base::Quantity(pos.x,Base::Unit::Length));
    ui->superplacementY->setValue(Base::Quantity(pos.y,Base::Unit::Length));
    ui->superplacementZ->setValue(Base::Quantity(pos.z,Base::Unit::Length));
    ui->superplacementYaw->setValue(yaw);
    ui->superplacementPitch->setValue(pitch);
    ui->superplacementRoll->setValue(roll);

    auto expressions = pcDatum->ExpressionEngine.getExpressions();
    bool bRotationBound = false;
    bRotationBound = bRotationBound ||
            expressions.find(App::ObjectIdentifier::parse(pcDatum,std::string("superPlacement.Rotation.Angle"))) != expressions.end();
    bRotationBound = bRotationBound ||
            expressions.find(App::ObjectIdentifier::parse(pcDatum,std::string("superPlacement.Rotation.Axis.x"))) != expressions.end();
    bRotationBound = bRotationBound ||
            expressions.find(App::ObjectIdentifier::parse(pcDatum,std::string("superPlacement.Rotation.Axis.y"))) != expressions.end();
    bRotationBound = bRotationBound ||
            expressions.find(App::ObjectIdentifier::parse(pcDatum,std::string("superPlacement.Rotation.Axis.z"))) != expressions.end();

    ui->superplacementYaw->setEnabled(!bRotationBound);
    ui->superplacementPitch->setEnabled(!bRotationBound);
    ui->superplacementRoll->setEnabled(!bRotationBound);

    QString tooltip = bRotationBound ? tr("Not editable because rotation part of superplacement is bound by expressions.") : QString();
    ui->superplacementYaw->setToolTip(tooltip);
    ui->superplacementPitch->setToolTip(tooltip);
    ui->superplacementRoll->setToolTip(tooltip);

    bBlock = false;
    ui->superplacementX->blockSignals(bBlock);
    ui->superplacementY->blockSignals(bBlock);
    ui->superplacementZ->blockSignals(bBlock);
    ui->superplacementYaw->blockSignals(bBlock);
    ui->superplacementPitch->blockSignals(bBlock);
    ui->superplacementRoll->blockSignals(bBlock);
}

void CmdRobotSetDefaultOrientation::activated(int iMsg)
{
    // create placement dialog 
    Gui::Dialog::Placement *Dlg = new Gui::Dialog::Placement();
    Base::Placement place;
    Dlg->setPlacement(place);
    if(Dlg->exec() == QDialog::Accepted ){
        place = Dlg->getPlacement();
        Base::Rotation rot = place.getRotation();
        Base::Vector3d disp = place.getPosition();
        doCommand(Doc,"_DefOrientation = FreeCAD.Rotation(%f,%f,%f,%f)",rot[0],rot[1],rot[2],rot[3]);
        doCommand(Doc,"_DefDisplacement = FreeCAD.Vector(%f,%f,%f)",disp[0],disp[1],disp[2]);
    }
      
}

PyObject* GeometryPy::rotate(PyObject *args)
{
    PyObject* o;
    if (!PyArg_ParseTuple(args, "O!", &(Base::PlacementPy::Type),&o))
        return 0;

    Base::Placement* plm = static_cast<Base::PlacementPy*>(o)->getPlacementPtr();
    Base::Rotation rot(plm->getRotation());
    Base::Vector3d pnt, dir;
    double angle;

    rot.getValue(dir, angle);
    pnt = plm->getPosition();
    
    gp_Ax1 ax1(gp_Pnt(pnt.x,pnt.y,pnt.z), gp_Dir(dir.x,dir.y,dir.z));
    getGeometryPtr()->handle()->Rotate(ax1, angle);
    Py_Return;
}

void TaskDatumParameters::onSuperplacementChanged(double /*val*/, int idx)
{
    Part::Datum* pcDatum = static_cast<Part::Datum*>(DatumView->getObject());
    Base::Placement pl = pcDatum->superPlacement.getValue();

    Base::Vector3d pos = pl.getPosition();
    if (idx == 0) {
        pos.x = ui->superplacementX->value().getValueAs(Base::Quantity::MilliMetre);
    }
    if (idx == 1) {
        pos.y = ui->superplacementY->value().getValueAs(Base::Quantity::MilliMetre);
    }
    if (idx == 2) {
        pos.z = ui->superplacementZ->value().getValueAs(Base::Quantity::MilliMetre);
    }
    if (idx >= 0  && idx <= 2){
        pl.setPosition(pos);
    }

    Base::Rotation rot = pl.getRotation();
    double yaw, pitch, roll;
    rot.getYawPitchRoll(yaw, pitch, roll);
    if (idx == 3) {
        yaw = ui->superplacementYaw->value().getValueAs(Base::Quantity::Degree);
    }
    if (idx == 4) {
        pitch = ui->superplacementPitch->value().getValueAs(Base::Quantity::Degree);
    }
    if (idx == 5) {
        roll = ui->superplacementRoll->value().getValueAs(Base::Quantity::Degree);
    }
    if (idx >= 3  &&  idx <= 5){
        rot.setYawPitchRoll(yaw,pitch,roll);
        pl.setRotation(rot);
    }

    pcDatum->superPlacement.setValue(pl);
    updatePreview();
}

void Placement::applyPlacement(const Base::Placement& p, bool incremental, bool data)
{
    Gui::Document* document = Application::Instance->activeDocument();
    if (!document) return;

    std::vector<App::DocumentObject*> sel = Gui::Selection().getObjectsOfType
        (App::DocumentObject::getClassTypeId(), document->getDocument()->getName());
    if (!sel.empty()) {
        if (data) {
            document->openCommand("Placement");
            for (std::vector<App::DocumentObject*>::iterator it=sel.begin();it!=sel.end();++it) {
                std::map<std::string,App::Property*> props;
                (*it)->getPropertyMap(props);
                // search for the placement property
                std::map<std::string,App::Property*>::iterator jt;
                jt = std::find_if(props.begin(), props.end(), find_placement(this->propertyName));
                if (jt != props.end()) {
                    Base::Placement cur = static_cast<App::PropertyPlacement*>(jt->second)->getValue();
                    if (incremental)
                        cur = p * cur;
                    else
                        cur = p;

                    Base::Vector3d pos = cur.getPosition();
                    const Base::Rotation& rt = cur.getRotation();
                    QString cmd = QString::fromAscii(
                        "App.getDocument(\"%1\").%2.Placement="
                        "App.Placement("
                        "App.Vector(%3,%4,%5),"
                        "App.Rotation(%6,%7,%8,%9))\n")
                        .arg(QLatin1String((*it)->getDocument()->getName()))
                        .arg(QLatin1String((*it)->getNameInDocument()))
                        .arg(pos.x,0,'g',6)
                        .arg(pos.y,0,'g',6)
                        .arg(pos.z,0,'g',6)
                        .arg(rt[0],0,'g',6)
                        .arg(rt[1],0,'g',6)
                        .arg(rt[2],0,'g',6)
                        .arg(rt[3],0,'g',6);
                    Application::Instance->runPythonCode((const char*)cmd.toAscii());
                }
            }

            document->commitCommand();
            try {
                document->getDocument()->recompute();
            }
            catch (...) {
            }
        }
        // apply transformation only on view matrix not on placement property
        else {
            for (std::vector<App::DocumentObject*>::iterator it=sel.begin();it!=sel.end();++it) {
                std::map<std::string,App::Property*> props;
                (*it)->getPropertyMap(props);
                // search for the placement property
                std::map<std::string,App::Property*>::iterator jt;
                jt = std::find_if(props.begin(), props.end(), find_placement(this->propertyName));
                if (jt != props.end()) {
                    Base::Placement cur = static_cast<App::PropertyPlacement*>(jt->second)->getValue();
                    if (incremental)
                        cur = p * cur;
                    else
                        cur = p;

                    Gui::ViewProvider* vp = document->getViewProvider(*it);
                    if (vp) vp->setTransformation(cur.toMatrix());
                }
            }
        }
    }
    else {
        Base::Console().Warning("No object selected.\n");
    }
}

App::DocumentObjectExecReturn *FeatureShape::execute(void)
{
    TopoDS_Shape shape = Shape.getValue();
    if (!shape.IsNull()) {
        if (shape.ShapeType() == TopAbs_WIRE) {
            Path::Toolpath result;
            bool first = true;
            Base::Placement last;
            
            TopExp_Explorer ExpEdges (shape,TopAbs_EDGE);
            while (ExpEdges.More()) {
                const TopoDS_Edge& edge = TopoDS::Edge(ExpEdges.Current());
                TopExp_Explorer ExpVerts(edge,TopAbs_VERTEX);
                bool vfirst = true;
                while (ExpVerts.More()) {
                    const TopoDS_Vertex& vert = TopoDS::Vertex(ExpVerts.Current());
                    gp_Pnt pnt = BRep_Tool::Pnt(vert);
                    Base::Placement tpl;
                    tpl.setPosition(Base::Vector3d(pnt.X(),pnt.Y(),pnt.Z()));
                    if (first) {
                        // add first point as a G0 move
                        Path::Command cmd;
                        std::ostringstream ctxt;
                        ctxt << "G0 X" << tpl.getPosition().x << " Y" << tpl.getPosition().y << " Z" << tpl.getPosition().z;
                        cmd.setFromGCode(ctxt.str());
                        result.addCommand(cmd);
                        first = false;
                        vfirst = false;
                    } else {
                        if (vfirst)
                            vfirst = false;
                        else {
                            Path::Command cmd;
                            cmd.setFromPlacement(tpl);
                
                            // write arc data if needed
                            BRepAdaptor_Curve adapt(edge);
                            if (adapt.GetType() == GeomAbs_Circle) {
                                gp_Circ circ = adapt.Circle();
                                gp_Pnt c = circ.Location();
                                bool clockwise = false;
                                gp_Dir n = circ.Axis().Direction();
                                if (n.Z() < 0)
                                    clockwise = true;
                                Base::Vector3d center = Base::Vector3d(c.X(),c.Y(),c.Z());
                                // center coords must be relative to last point
                                center -= last.getPosition();
                                cmd.setCenter(center,clockwise);
                            }
                            result.addCommand(cmd);
                        }
                    }
                    ExpVerts.Next();
                    last = tpl;
                }
                ExpEdges.Next();
            }
            
            Path.setValue(result);
        }
    }
    return App::DocumentObject::StdReturn;
}

bool TaskDlgDatumParameters::accept()
{
    std::string name = DatumView->getObject()->getNameInDocument();
    Part::Datum* pcDatum = static_cast<Part::Datum*>(DatumView->getObject());
    auto pcActiveBody = PartDesignGui::getBodyFor(pcDatum, false);
    auto pcActivePart = PartDesignGui::getPartFor(pcActiveBody, false);
    std::vector<App::DocumentObject*> copies;

    //see if we are able to assign a mode
    if (parameter->getActiveMapMode() == mmDeactivated) {
        QMessageBox msg;
        msg.setWindowTitle(tr("Incompatible reference set"));
        msg.setText(tr("There is no attachment mode that fits the current set"
        " of references. If you choose to continue, the feature will remain where"
        " it is now, and will not be moved as the references change."
        " Continue?"));
        msg.addButton(QMessageBox::Yes);
        auto btNo =  msg.addButton(QMessageBox::No);
        msg.setDefaultButton(btNo);
        msg.setIcon(QMessageBox::Warning);
        msg.exec();
        if (msg.clickedButton() == btNo)
            return false;
    }

    //see what to do with external references
    //check the prerequisites for the selected objects
    //the user has to decide which option we should take if external references are used
    bool ext = false;
    for(App::DocumentObject* obj : pcDatum->Support.getValues()) {
        if(!pcActiveBody->hasFeature(obj) && !pcActiveBody->getOrigin()->hasObject(obj))
            ext = true;
    }
    if(ext) {
        // TODO rewrite this to be shared with CmdPartDesignNewSketch::activated() (2015-10-20, Fat-Zer)
        QDialog* dia = new QDialog;
        Ui_Dialog dlg;
        dlg.setupUi(dia);
        dia->setModal(true);
        int result = dia->exec();
        if(result == QDialog::DialogCode::Rejected)
            return false;
        else if(!dlg.radioXRef->isChecked()) {

            std::vector<App::DocumentObject*> objs;
            std::vector<std::string> subs = pcDatum->Support.getSubValues();
            int index = 0;
            for(App::DocumentObject* obj : pcDatum->Support.getValues()) {

                if(!pcActiveBody->hasFeature(obj) && !pcActiveBody->getOrigin()->hasObject(obj)) {
                    objs.push_back(PartDesignGui::TaskFeaturePick::makeCopy(obj, subs[index], dlg.radioIndependent->isChecked()));
                    copies.push_back(objs.back());
                    subs[index] = "";
                }
                else
                    objs.push_back(obj);

                index++;
            }

            pcDatum->Support.setValues(objs, subs);
        }
    }

    try {
        //DeepSOIC: changed this to heavily rely on dialog constantly updating feature properties
        if (pcDatum->superPlacement.isTouched()){
            Base::Placement plm = pcDatum->superPlacement.getValue();
            double yaw, pitch, roll;
            plm.getRotation().getYawPitchRoll(yaw,pitch,roll);
            Gui::Command::doCommand(Gui::Command::Doc,"App.ActiveDocument.%s.superPlacement = App.Placement(App.Vector(%.10f, %.10f, %.10f),  App.Rotation(%.10f, %.10f, %.10f))",
                                    name.c_str(),
                                    plm.getPosition().x, plm.getPosition().y, plm.getPosition().z,
                                    yaw, pitch, roll);
        }

        Gui::Command::doCommand(Gui::Command::Doc,"App.ActiveDocument.%s.MapReversed = %s", name.c_str(), pcDatum->MapReversed.getValue() ? "True" : "False");

        Gui::Command::doCommand(Gui::Command::Doc,"App.ActiveDocument.%s.Support = %s", name.c_str(), pcDatum->Support.getPyReprString().c_str());

        Gui::Command::doCommand(Gui::Command::Doc,"App.ActiveDocument.%s.MapMode = '%s'", name.c_str(), AttachEngine::getModeName(eMapMode(pcDatum->MapMode.getValue())).c_str());

        Gui::Command::doCommand(Gui::Command::Doc,"App.ActiveDocument.recompute()");
        if (!DatumView->getObject()->isValid())
            throw Base::Exception(DatumView->getObject()->getStatusString());
        Gui::Command::doCommand(Gui::Command::Gui, "Gui.activeDocument().resetEdit()");
        Gui::Command::commitCommand();

        //we need to add the copied features to the body after the command action, as otherwise freecad crashs unexplainable
        for(auto obj : copies) {
            if(pcActiveBody)
                pcActiveBody->addFeature(obj);
            else if (pcActivePart)
                pcActivePart->addObject(obj);
        }
    }
    catch (const Base::Exception& e) {
        QMessageBox::warning(parameter, tr("Datum dialog: Input error"), QString::fromLatin1(e.what()));
        return false;
    }
//.........这里部分代码省略.........