PyObject* MatrixPy::scale(PyObject * args) { double x,y,z; Base::Vector3d vec; PyObject *pcVecObj; if (PyArg_ParseTuple(args, "ddd", &x,&y,&z)) { // convert args: Python->C vec.x = x; vec.y = y; vec.z = z; } else if (PyArg_ParseTuple(args, "O!:three floats or a vector is needed", &PyTuple_Type, &pcVecObj)) { vec = getVectorFromTuple<double>(pcVecObj); // clears the error from the first PyArg_ParseTuple()6 PyErr_Clear(); } else if (PyArg_ParseTuple(args, "O!:three floats or a vector is needed", &(Base::VectorPy::Type), &pcVecObj)) { // convert args: Python->C Base::VectorPy *pcObject = static_cast<Base::VectorPy*>(pcVecObj); Base::Vector3d* val = pcObject->getVectorPtr(); vec.Set(val->x,val->y,val->z); // clears the error from the first PyArg_ParseTuple()6 PyErr_Clear(); } else return NULL; PY_TRY { getMatrixPtr()->scale(vec); } PY_CATCH; Py_Return; }
/// utility non-class member functions //! gets a coordinate system that matches view system used in 3D with +Z up (or +Y up if neccessary) //! used for individual views, but not secondary views in projection groups gp_Ax2 TechDrawGeometry::getViewAxis(const Base::Vector3d origin, const Base::Vector3d& direction, const bool flip) { gp_Pnt inputCenter(origin.x,origin.y,origin.z); Base::Vector3d stdZ(0.0,0.0,1.0); Base::Vector3d flipDirection(direction.x,-direction.y,direction.z); if (!flip) { flipDirection = Base::Vector3d(direction.x,direction.y,direction.z); } Base::Vector3d cross = flipDirection; //special cases if (flipDirection == stdZ) { cross = Base::Vector3d(1.0,0.0,0.0); } else if (flipDirection == (stdZ * -1.0)) { cross = Base::Vector3d(1.0,0.0,0.0); } else { cross.Normalize(); cross = cross.Cross(stdZ); } gp_Ax2 viewAxis; viewAxis = gp_Ax2(inputCenter, gp_Dir(flipDirection.x, flipDirection.y, flipDirection.z), // gp_Dir(1.0, 1.0, 0.0)); gp_Dir(cross.x, cross.y, cross.z)); return viewAxis; }
PyObject* MatrixPy::transform(PyObject * args) { Base::Vector3d vec; Matrix4D mat; PyObject *pcVecObj,*pcMatObj; if (PyArg_ParseTuple(args, "O!O!: a transform point (Vector) and a transform matrix (Matrix) is needed", &(Base::VectorPy::Type), &pcVecObj, &(MatrixPy::Type), &pcMatObj) ) { // convert args: Python->C Base::VectorPy *pcObject = static_cast<Base::VectorPy*>(pcVecObj); Base::Vector3d* val = pcObject->getVectorPtr(); vec.Set(val->x,val->y,val->z); mat = *(static_cast<MatrixPy*>(pcMatObj)->getMatrixPtr()); // clears the error from the first PyArg_ParseTuple()6 PyErr_Clear(); } else return NULL; // NULL triggers exception PY_TRY { getMatrixPtr()->transform(vec,mat); } PY_CATCH; Py_Return; }
void TrajectoryVisualization::setColor(const base::Vector3d& color) { { boost::mutex::scoped_lock lockit(this->updateMutex); this->color = osg::Vec4(color.x(), color.y(), color.z(), 1.0); } emit propertyChanged("Color"); setDirty(); }
Extrusion::ExtrusionParameters Extrusion::computeFinalParameters() { Extrusion::ExtrusionParameters result; Base::Vector3d dir; switch(this->DirMode.getValue()){ case dmCustom: dir = this->Dir.getValue(); break; case dmEdge:{ bool fetched; Base::Vector3d base; fetched = fetchAxisLink(this->DirLink, base, dir); if (! fetched) throw Base::Exception("DirMode is set to use edge, but no edge is linked."); this->Dir.setValue(dir); }break; case dmNormal: dir = calculateShapeNormal(this->Base); this->Dir.setValue(dir); break; default: throw Base::ValueError("Unexpected enum value"); } if(dir.Length() < Precision::Confusion()) throw Base::ValueError("Direction is zero-length"); result.dir = gp_Dir(dir.x, dir.y, dir.z); if (this->Reversed.getValue()) result.dir.Reverse(); result.lengthFwd = this->LengthFwd.getValue(); result.lengthRev = this->LengthRev.getValue(); if(fabs(result.lengthFwd) < Precision::Confusion() && fabs(result.lengthRev) < Precision::Confusion() ){ result.lengthFwd = dir.Length(); } if (this->Symmetric.getValue()){ result.lengthRev = result.lengthFwd * 0.5; result.lengthFwd = result.lengthFwd * 0.5; } if (fabs(result.lengthFwd + result.lengthRev) < Precision::Confusion()) throw Base::ValueError("Total length of extrusion is zero."); result.solid = this->Solid.getValue(); result.taperAngleFwd = this->TaperAngle.getValue() * M_PI / 180.0; if (fabs(result.taperAngleFwd) > M_PI * 0.5 - Precision::Angular() ) throw Base::ValueError("Magnitude of taper angle matches or exceeds 90 degrees. That is too much."); result.taperAngleRev = this->TaperAngleRev.getValue() * M_PI / 180.0; if (fabs(result.taperAngleRev) > M_PI * 0.5 - Precision::Angular() ) throw Base::ValueError("Magnitude of taper angle matches or exceeds 90 degrees. That is too much."); result.faceMakerClass = this->FaceMakerClass.getValue(); return result; }
Base::Vector3d MeshObject::getPointNormal(unsigned long index) const { std::vector<Base::Vector3f> temp = _kernel.CalcVertexNormals(); Base::Vector3d normal = transformToOutside(temp[index]); // the normal is a vector, hence we must not apply the translation part // of the transformation to the vector normal.x -= _Mtrx[0][3]; normal.y -= _Mtrx[1][3]; normal.z -= _Mtrx[2][3]; normal.Normalize(); return normal; }
void TrajectoryVisualization::updateDataIntern( const base::Vector3d& data ) { if(doClear) { points.clear(); doClear = false; } Point p; p.point = osg::Vec3(data.x(), data.y(), data.z()); p.color = color; points.push_back(p); while(points.size() > max_number_of_points) points.pop_front(); }
base::Vector3d SplineBase::poseError(base::Vector3d _position, double _heading, double _guess) { double param = findOneClosestPoint(_position.data(), _guess, getGeometricResolution()); // Returns the error [distance error, orientation error, parameter] return base::Vector3d(distanceError(_position, param), headingError(_heading, param), param); }
void LocationWidget::setDirection(const Base::Vector3d& dir) { if (dir.Length() < Base::Vector3d::epsilon()) { return; } // check if the user-defined direction is already there for (int i=0; i<dValue->count()-1; i++) { QVariant data = dValue->itemData (i); if (data.canConvert<Base::Vector3d>()) { const Base::Vector3d val = data.value<Base::Vector3d>(); if (val == dir) { dValue->setCurrentIndex(i); return; } } } // add a new item before the very last item QString display = QString::fromLatin1("(%1,%2,%3)") .arg(dir.x) .arg(dir.y) .arg(dir.z); dValue->insertItem(dValue->count()-1, display, QVariant::fromValue<Base::Vector3d>(dir)); dValue->setCurrentIndex(dValue->count()-2); }
Base::Matrix4D MeshObject::getEigenSystem(Base::Vector3d& v) const { MeshCore::MeshEigensystem cMeshEval(_kernel); cMeshEval.Evaluate(); Base::Vector3f uvw = cMeshEval.GetBoundings(); v.Set(uvw.x, uvw.y, uvw.z); return cMeshEval.Transform(); }
void LocationWidget::on_direction_activated(int index) { // last item is selected to define direction by user if (index+1 == dValue->count()) { bool ok; Base::Vector3d dir = this->getUserDirection(&ok); if (ok) { if (dir.Length() < Base::Vector3d::epsilon()) { QMessageBox::critical(this, LocationDialog::tr("Wrong direction"), LocationDialog::tr("Direction must not be the null vector")); return; } setDirection(dir); } } }
//! calculate the section Normal/Projection Direction given baseView projection direction and section name Base::Vector3d DrawViewSection::getSectionVector (const std::string sectionName) { Base::Vector3d result; Base::Vector3d stdX(1.0,0.0,0.0); Base::Vector3d stdY(0.0,1.0,0.0); Base::Vector3d stdZ(0.0,0.0,1.0); double adjustAngle = 0.0; if (getBaseDPGI() != nullptr) { adjustAngle = getBaseDPGI()->getRotateAngle(); } Base::Vector3d view = getBaseDVP()->Direction.getValue(); view.Normalize(); Base::Vector3d left = view.Cross(stdZ); left.Normalize(); Base::Vector3d up = view.Cross(left); up.Normalize(); double dot = view.Dot(stdZ); if (sectionName == "Up") { result = up; if (DrawUtil::fpCompare(dot,1.0)) { //view = stdZ result = (-1.0 * stdY); } else if (DrawUtil::fpCompare(dot,-1.0)) { //view = -stdZ result = stdY; } } else if (sectionName == "Down") { result = up * -1.0; if (DrawUtil::fpCompare(dot,1.0)) { //view = stdZ result = stdY; } else if (DrawUtil::fpCompare(dot, -1.0)) { //view = -stdZ result = (-1.0 * stdY); } } else if (sectionName == "Left") { result = left * -1.0; if (DrawUtil::fpCompare(fabs(dot),1.0)) { //view = +/- stdZ result = stdX; } } else if (sectionName == "Right") { result = left; if (DrawUtil::fpCompare(fabs(dot),1.0)) { result = -1.0 * stdX; } } else { Base::Console().Log("Error - DVS::getSectionVector - bad sectionName: %s\n",sectionName.c_str()); result = stdZ; } Base::Vector3d adjResult = DrawUtil::vecRotate(result,adjustAngle,view); return adjResult; }
void ConstraintForce::onChanged(const App::Property* prop) { // Note: If we call this at the end, then the arrows are not oriented correctly initially // because the NormalDirection has not been calculated yet Constraint::onChanged(prop); if (prop == &References) { std::vector<Base::Vector3d> points; std::vector<Base::Vector3d> normals; if (getPoints(points, normals)) { Points.setValues(points); // We don't use the normals because all arrows should have the same direction } } else if (prop == &Direction) { Base::Vector3d direction = getDirection(Direction); if (direction.Length() < Precision::Confusion()) return; naturalDirectionVector = direction; if (Reversed.getValue()) direction = -direction; DirectionVector.setValue(direction); } else if (prop == &Reversed) { // if the direction is invalid try to compute it again if (naturalDirectionVector.Length() < Precision::Confusion()) { naturalDirectionVector = getDirection(Direction); } if (naturalDirectionVector.Length() >= Precision::Confusion()) { if (Reversed.getValue() && (DirectionVector.getValue() == naturalDirectionVector)) { DirectionVector.setValue(-naturalDirectionVector); } else if (!Reversed.getValue() && (DirectionVector.getValue() != naturalDirectionVector)) { DirectionVector.setValue(naturalDirectionVector); } } } else if (prop == &NormalDirection) { // Set a default direction if no direction reference has been given if (Direction.getValue() == NULL) { Base::Vector3d direction = NormalDirection.getValue(); if (Reversed.getValue()) direction = -direction; DirectionVector.setValue(direction); naturalDirectionVector = direction; } } }
bool Extrusion::fetchAxisLink(const App::PropertyLinkSub& axisLink, Base::Vector3d& basepoint, Base::Vector3d& dir) { if (!axisLink.getValue()) return false; if (!axisLink.getValue()->isDerivedFrom(Part::Feature::getClassTypeId())) throw Base::TypeError("AxisLink has no OCC shape"); Part::Feature* linked = static_cast<Part::Feature*>(axisLink.getValue()); TopoDS_Shape axEdge; if (axisLink.getSubValues().size() > 0 && axisLink.getSubValues()[0].length() > 0){ axEdge = linked->Shape.getShape().getSubShape(axisLink.getSubValues()[0].c_str()); } else { axEdge = linked->Shape.getValue(); } if (axEdge.IsNull()) throw Base::ValueError("DirLink shape is null"); if (axEdge.ShapeType() != TopAbs_EDGE) throw Base::TypeError("DirLink shape is not an edge"); BRepAdaptor_Curve crv(TopoDS::Edge(axEdge)); gp_Pnt startpoint; gp_Pnt endpoint; if (crv.GetType() == GeomAbs_Line){ startpoint = crv.Value(crv.FirstParameter()); endpoint = crv.Value(crv.LastParameter()); if (axEdge.Orientation() == TopAbs_REVERSED) std::swap(startpoint, endpoint); } else { throw Base::TypeError("DirLink edge is not a line."); } basepoint.Set(startpoint.X(), startpoint.Y(), startpoint.Z()); gp_Vec vec = gp_Vec(startpoint, endpoint); dir.Set(vec.X(), vec.Y(), vec.Z()); return true; }
PyObject* MeshPointPy::move(PyObject *args) { if (!getMeshPointPtr()->isBound()) PyErr_SetString(Base::BaseExceptionFreeCADError, "This object is not bounded to a mesh, so no topological operation is possible!"); double x=0.0,y=0.0,z=0.0; PyObject *object; Base::Vector3d vec; if (PyArg_ParseTuple(args, "ddd", &x,&y,&z)) { vec.Set(x,y,z); } else if (PyArg_ParseTuple(args,"O!",&(Base::VectorPy::Type), &object)) { PyErr_Clear(); // set by PyArg_ParseTuple() // Note: must be static_cast, not reinterpret_cast vec = *(static_cast<Base::VectorPy*>(object)->getVectorPtr()); } else { return 0; } getMeshPointPtr()->Mesh->movePoint(getMeshPointPtr()->Index,vec); Py_Return; }
void ConstraintGear::onChanged(const App::Property* prop) { ConstraintBearing::onChanged(prop); if (prop == &Direction) { Base::Vector3d direction = getDirection(Direction); if (direction.Length() < Precision::Confusion()) return; naturalDirectionVector = direction; if (Reversed.getValue()) direction = -direction; DirectionVector.setValue(direction); DirectionVector.touch(); } else if (prop == &Reversed) { if (Reversed.getValue() && (DirectionVector.getValue() == naturalDirectionVector)) { DirectionVector.setValue(-naturalDirectionVector); DirectionVector.touch(); } else if (!Reversed.getValue() && (DirectionVector.getValue() != naturalDirectionVector)) { DirectionVector.setValue(naturalDirectionVector); DirectionVector.touch(); } } // The computation for the force angle is simpler in the ViewProvider directly }
bool ViewProviderMirror::setEdit(int ModNum) { if (ModNum == ViewProvider::Default) { // get the properties from the mirror feature Part::Mirroring* mf = static_cast<Part::Mirroring*>(getObject()); Base::BoundBox3d bbox = mf->Shape.getBoundingBox(); float len = (float)bbox.CalcDiagonalLength(); Base::Vector3d base = mf->Base.getValue(); Base::Vector3d norm = mf->Normal.getValue(); Base::Vector3d cent = bbox.GetCenter(); base = cent.ProjToPlane(base, norm); // setup the graph for editing the mirror plane SoTransform* trans = new SoTransform; SbRotation rot(SbVec3f(0,0,1), SbVec3f(norm.x,norm.y,norm.z)); trans->rotation.setValue(rot); trans->translation.setValue(base.x,base.y,base.z); trans->center.setValue(0.0f,0.0f,0.0f); SoMaterial* color = new SoMaterial(); color->diffuseColor.setValue(0,0,1); color->transparency.setValue(0.5); SoCoordinate3* points = new SoCoordinate3(); points->point.setNum(4); points->point.set1Value(0, -len/2,-len/2,0); points->point.set1Value(1, len/2,-len/2,0); points->point.set1Value(2, len/2, len/2,0); points->point.set1Value(3, -len/2, len/2,0); SoFaceSet* face = new SoFaceSet(); pcEditNode->addChild(trans); pcEditNode->addChild(color); pcEditNode->addChild(points); pcEditNode->addChild(face); // Now we replace the SoTransform node by a manipulator // Note: Even SoCenterballManip inherits from SoTransform // we cannot use it directly (in above code) because the // translation and center fields are overridden. SoSearchAction sa; sa.setInterest(SoSearchAction::FIRST); sa.setSearchingAll(FALSE); sa.setNode(trans); sa.apply(pcEditNode); SoPath * path = sa.getPath(); if (path) { SoCenterballManip * manip = new SoCenterballManip; manip->replaceNode(path); SoDragger* dragger = manip->getDragger(); dragger->addStartCallback(dragStartCallback, this); dragger->addFinishCallback(dragFinishCallback, this); dragger->addMotionCallback(dragMotionCallback, this); } pcRoot->addChild(pcEditNode); } else { ViewProviderPart::setEdit(ModNum); } return true; }
int DrawSketchHandler::seekAutoConstraint(std::vector<AutoConstraint> &suggestedConstraints, const Base::Vector2D& Pos, const Base::Vector2D& Dir, AutoConstraint::TargetType type) { suggestedConstraints.clear(); if (!sketchgui->Autoconstraints.getValue()) return 0; // If Autoconstraints property is not set quit Base::Vector3d hitShapeDir = Base::Vector3d(0,0,0); // direction of hit shape (if it is a line, the direction of the line) // Get Preselection int preSelPnt = sketchgui->getPreselectPoint(); int preSelCrv = sketchgui->getPreselectCurve(); int preSelCrs = sketchgui->getPreselectCross(); int GeoId = Constraint::GeoUndef; Sketcher::PointPos PosId = Sketcher::none; if (preSelPnt != -1) sketchgui->getSketchObject()->getGeoVertexIndex(preSelPnt, GeoId, PosId); else if (preSelCrv != -1){ GeoId = preSelCrv; const Part::Geometry *geom = sketchgui->getSketchObject()->getGeometry(GeoId); if(geom->getTypeId() == Part::GeomLineSegment::getClassTypeId()){ const Part::GeomLineSegment *line = static_cast<const Part::GeomLineSegment *>(geom); hitShapeDir= line->getEndPoint()-line->getStartPoint(); } } else if (preSelCrs == 0) { // root point GeoId = -1; PosId = Sketcher::start; } else if (preSelCrs == 1){ // x axis GeoId = -1; hitShapeDir = Base::Vector3d(1,0,0); } else if (preSelCrs == 2){ // y axis GeoId = -2; hitShapeDir = Base::Vector3d(0,1,0); } if (GeoId != Constraint::GeoUndef) { // Currently only considers objects in current Sketcher AutoConstraint constr; constr.Type = Sketcher::None; constr.GeoId = GeoId; constr.PosId = PosId; if (type == AutoConstraint::VERTEX && PosId != Sketcher::none) constr.Type = Sketcher::Coincident; else if (type == AutoConstraint::CURVE && PosId != Sketcher::none) constr.Type = Sketcher::PointOnObject; else if (type == AutoConstraint::VERTEX && PosId == Sketcher::none) constr.Type = Sketcher::PointOnObject; else if (type == AutoConstraint::CURVE && PosId == Sketcher::none) constr.Type = Sketcher::Tangent; if(constr.Type == Sketcher::Tangent && Dir.Length() > 1e-8 && hitShapeDir.Length() > 1e-8) { // We are hitting a line and have hitting vector information Base::Vector3d dir3d = Base::Vector3d(Dir.fX,Dir.fY,0); double cosangle=dir3d.Normalize()*hitShapeDir.Normalize(); // the angle between the line and the hitting direction are over around 6 degrees (it is substantially parallel) // or if it is an sketch axis (that can not move to accomodate to the shape), then only if it is around 6 degrees with the normal (around 84 degrees) if (fabs(cosangle) < 0.995f || ((GeoId==-1 || GeoId==-2) && fabs(cosangle) < 0.1)) suggestedConstraints.push_back(constr); return suggestedConstraints.size(); } if (constr.Type != Sketcher::None) suggestedConstraints.push_back(constr); } if (Dir.Length() < 1e-8 || type == AutoConstraint::CURVE) // Direction not set so return; return suggestedConstraints.size(); // Suggest vertical and horizontal constraints // Number of Degree of deviation from horizontal or vertical lines const double angleDev = 2; const double angleDevRad = angleDev * M_PI / 180.; AutoConstraint constr; constr.Type = Sketcher::None; constr.GeoId = Constraint::GeoUndef; constr.PosId = Sketcher::none; double angle = std::abs(atan2(Dir.fY, Dir.fX)); if (angle < angleDevRad || (M_PI - angle) < angleDevRad ) // Suggest horizontal constraint constr.Type = Sketcher::Horizontal; else if (std::abs(angle - M_PI_2) < angleDevRad) // Suggest vertical constraint constr.Type = Sketcher::Vertical; if (constr.Type != Sketcher::None) suggestedConstraints.push_back(constr); // Find if there are tangent constraints (currently arcs and circles) int tangId = Constraint::GeoUndef; // Do not consider if distance is more than that. // Decrease this value when a candidate is found. double tangDeviation = 0.1 * sketchgui->getScaleFactor(); // Get geometry list const std::vector<Part::Geometry *> geomlist = sketchgui->getSketchObject()->getCompleteGeometry(); Base::Vector3d tmpPos(Pos.fX, Pos.fY, 0.f); // Current cursor point Base::Vector3d tmpDir(Dir.fX, Dir.fY, 0.f); // Direction of line Base::Vector3d tmpStart(Pos.fX-Dir.fX, Pos.fY-Dir.fY, 0.f); // Start point // Iterate through geometry int i = 0; for (std::vector<Part::Geometry *>::const_iterator it=geomlist.begin(); it != geomlist.end(); ++it, i++) { if ((*it)->getTypeId() == Part::GeomCircle::getClassTypeId()) { const Part::GeomCircle *circle = static_cast<const Part::GeomCircle *>((*it)); Base::Vector3d center = circle->getCenter(); double radius = circle->getRadius(); // ignore if no touch (use dot product) if(tmpDir * (center-tmpPos) > 0 || tmpDir * (center-tmpStart) < 0) continue; Base::Vector3d projPnt(0.f, 0.f, 0.f); projPnt = projPnt.ProjectToLine(center - tmpPos, tmpDir); double projDist = std::abs(projPnt.Length() - radius); // Find if nearest if (projDist < tangDeviation) { tangId = i; tangDeviation = projDist; } } else if ((*it)->getTypeId() == Part::GeomEllipse::getClassTypeId()) { const Part::GeomEllipse *ellipse = static_cast<const Part::GeomEllipse *>((*it)); Base::Vector3d center = ellipse->getCenter(); double a = ellipse->getMajorRadius(); double b = ellipse->getMinorRadius(); Base::Vector3d majdir = ellipse->getMajorAxisDir(); double cf = sqrt(a*a - b*b); Base::Vector3d focus1P = center + cf * majdir; Base::Vector3d focus2P = center - cf * majdir; Base::Vector3d norm = Base::Vector3d(Dir.fY,-Dir.fX).Normalize(); double distancetoline = norm*(tmpPos - focus1P); // distance focus1 to line Base::Vector3d focus1PMirrored = focus1P + 2*distancetoline*norm; // mirror of focus1 with respect to the line double error = fabs((focus1PMirrored-focus2P).Length() - 2*a); if ( error< tangDeviation) { tangId = i; tangDeviation = error; } } else if ((*it)->getTypeId() == Part::GeomArcOfCircle::getClassTypeId()) { const Part::GeomArcOfCircle *arc = static_cast<const Part::GeomArcOfCircle *>((*it)); Base::Vector3d center = arc->getCenter(); double radius = arc->getRadius(); // ignore if no touch (use dot product) if(tmpDir * (center-tmpPos) > 0 || tmpDir * (center-tmpStart) < 0) continue; Base::Vector3d projPnt(0.f, 0.f, 0.f); projPnt = projPnt.ProjectToLine(center - tmpPos, tmpDir); double projDist = std::abs(projPnt.Length() - radius); if (projDist < tangDeviation) { double startAngle, endAngle; arc->getRange(startAngle, endAngle, /*emulateCCW=*/true); double angle = atan2(projPnt.y, projPnt.x); while(angle < startAngle) angle += 2*D_PI; // Bring it to range of arc // if the point is on correct side of arc if (angle <= endAngle) { // Now need to check only one side tangId = i; tangDeviation = projDist; } } } else if ((*it)->getTypeId() == Part::GeomArcOfEllipse::getClassTypeId()) { const Part::GeomArcOfEllipse *aoe = static_cast<const Part::GeomArcOfEllipse *>((*it)); Base::Vector3d center = aoe->getCenter(); double a = aoe->getMajorRadius(); double b = aoe->getMinorRadius(); Base::Vector3d majdir = aoe->getMajorAxisDir(); double cf = sqrt(a*a - b*b); Base::Vector3d focus1P = center + cf * majdir; Base::Vector3d focus2P = center - cf * majdir; Base::Vector3d norm = Base::Vector3d(Dir.fY,-Dir.fX).Normalize(); double distancetoline = norm*(tmpPos - focus1P); // distance focus1 to line Base::Vector3d focus1PMirrored = focus1P + 2*distancetoline*norm; // mirror of focus1 with respect to the line double error = fabs((focus1PMirrored-focus2P).Length() - 2*a); if ( error< tangDeviation ) { tangId = i; tangDeviation = error; } if (error < tangDeviation) { double startAngle, endAngle; aoe->getRange(startAngle, endAngle, /*emulateCCW=*/true); double angle = Base::fmod( atan2(-aoe->getMajorRadius()*((tmpPos.x-center.x)*majdir.y-(tmpPos.y-center.y)*majdir.x), aoe->getMinorRadius()*((tmpPos.x-center.x)*majdir.x+(tmpPos.y-center.y)*majdir.y) )- startAngle, 2.f*M_PI); while(angle < startAngle) angle += 2*D_PI; // Bring it to range of arc // if the point is on correct side of arc if (angle <= endAngle) { // Now need to check only one side tangId = i; tangDeviation = error; } } } } if (tangId != Constraint::GeoUndef) { if (tangId > getHighestCurveIndex()) // external Geometry tangId = getHighestCurveIndex() - tangId; // Suggest vertical constraint constr.Type = Tangent; constr.GeoId = tangId; constr.PosId = Sketcher::none; suggestedConstraints.push_back(constr); } return suggestedConstraints.size(); }
// Methods for distances (edge length, two points, edge and a point double Measurement::length() const { int numRefs = References3D.getSize(); if(!numRefs || measureType == Invalid) { throw Base::Exception("Measurement - length - Invalid References3D Provided"); } double result = 0.0; const std::vector<App::DocumentObject*> &objects = References3D.getValues(); const std::vector<std::string> &subElements = References3D.getSubValues(); if(measureType == Points || measureType == PointToEdge || measureType == PointToSurface) { Base::Vector3d diff = this->delta(); //return diff.Length(); result = diff.Length(); } else if(measureType == Edges) { double length = 0.f; // Iterate through edges and calculate each length std::vector<App::DocumentObject*>::const_iterator obj = objects.begin(); std::vector<std::string>::const_iterator subEl = subElements.begin(); for (;obj != objects.end(); ++obj, ++subEl) { //const Part::Feature *refObj = static_cast<const Part::Feature*>((*obj)); //const Part::TopoShape& refShape = refObj->Shape.getShape(); // Get the length of one edge TopoDS_Shape shape = getShape(*obj, (*subEl).c_str()); const TopoDS_Edge& edge = TopoDS::Edge(shape); BRepAdaptor_Curve curve(edge); switch(curve.GetType()) { case GeomAbs_Line : { gp_Pnt P1 = curve.Value(curve.FirstParameter()); gp_Pnt P2 = curve.Value(curve.LastParameter()); gp_XYZ diff = P2.XYZ() - P1.XYZ(); length += diff.Modulus(); } break; case GeomAbs_Circle : { double u = curve.FirstParameter(); double v = curve.LastParameter(); double radius = curve.Circle().Radius(); if (u > v) // if arc is reversed std::swap(u, v); double range = v-u; length += radius * range; } break; case GeomAbs_Ellipse: case GeomAbs_BSplineCurve: case GeomAbs_Hyperbola: case GeomAbs_BezierCurve: { length += GCPnts_AbscissaPoint::Length(curve); } break; default: { throw Base::Exception("Measurement - length - Curve type not currently handled"); } } } result = length; //return length; } return result; }
Py::Object makeFilletArc(const Py::Tuple& args) { PyObject *pM1; PyObject *pP; PyObject *pQ; PyObject *pN; double r2; int ccw; if (!PyArg_ParseTuple(args.ptr(), "O!O!O!O!di", &(Base::VectorPy::Type), &pM1, &(Base::VectorPy::Type), &pP, &(Base::VectorPy::Type), &pQ, &(Base::VectorPy::Type), &pN, &r2, &ccw)) throw Py::Exception(); Base::Vector3d M1 = Py::Vector(pM1, false).toVector(); Base::Vector3d P = Py::Vector(pP, false).toVector(); Base::Vector3d Q = Py::Vector(pQ, false).toVector(); Base::Vector3d N = Py::Vector(pN, false).toVector(); Base::Vector3d u = Q - P; Base::Vector3d v = P - M1; Base::Vector3d b; if (ccw) b = u % N; else b = N % u; b.Normalize(); double uu = u * u; double uv = u * v; double r1 = v.Length(); // distinguish between internal and external fillets r2 *= Base::sgn(uv); double cc = 2.0 * r2 * (b * v - r1); double d = uv * uv - uu * cc; if (d < 0) { throw Py::RuntimeError("Unable to calculate intersection points"); } double t; double t1 = (-uv + sqrt(d)) / uu; double t2 = (-uv - sqrt(d)) / uu; if (fabs(t1) < fabs(t2)) t = t1; else t = t2; Base::Vector3d M2 = P + (u*t) + (b*r2); Base::Vector3d S1 = (r2 * M1 + r1 * M2)/(r1+r2); Base::Vector3d S2 = M2 - (b*r2); Py::Tuple tuple(3); tuple.setItem(0, Py::Vector(S1)); tuple.setItem(1, Py::Vector(S2)); tuple.setItem(2, Py::Vector(M2)); return tuple; }
void ViewProviderFemConstraintForce::updateData(const App::Property* prop) { // Gets called whenever a property of the attached object changes Fem::ConstraintForce* pcConstraint = static_cast<Fem::ConstraintForce*>(this->getObject()); /* // This has a HUGE performance penalty as opposed to separate nodes for every symbol // The problem seems to be SoCone if (pShapeSep->getNumChildren() == 0) { // Set up the nodes SoMultipleCopy* cp = new SoMultipleCopy(); cp->ref(); cp->matrix.setNum(0); cp->addChild((SoNode*)createArrow(ARROWLENGTH, ARROWHEADRADIUS)); pShapeSep->addChild(cp); } */ if (strcmp(prop->getName(),"Points") == 0) { // Redraw all arrows pShapeSep->removeAllChildren(); // 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); const std::vector<Base::Vector3d>& points = pcConstraint->Points.getValues(); /* SoMultipleCopy* cp = static_cast<SoMultipleCopy*>(pShapeSep->getChild(0)); cp->matrix.setNum(points.size()); int idx = 0;*/ 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 * ARROWLENGTH; /* SbMatrix m; m.setTransform(base, rot, SbVec3f(1,1,1)); cp->matrix.set1Value(idx, m); idx++; */ SoSeparator* sep = new SoSeparator(); createPlacement(sep, base, rot); createArrow(sep, ARROWLENGTH, ARROWHEADRADIUS); pShapeSep->addChild(sep); } } 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); const std::vector<Base::Vector3d>& points = pcConstraint->Points.getValues(); /* SoMultipleCopy* cp = static_cast<SoMultipleCopy*>(pShapeSep->getChild(0)); cp->matrix.setNum(points.size()); */ int idx = 0; 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) base = base + dir * ARROWLENGTH; /* SbMatrix m; m.setTransform(base, rot, SbVec3f(1,1,1)); cp->matrix.set1Value(idx, m);*/ SoSeparator* sep = static_cast<SoSeparator*>(pShapeSep->getChild(idx)); updatePlacement(sep, 0, base, rot); updateArrow(sep, 2, ARROWLENGTH, ARROWHEADRADIUS); idx++; } } ViewProviderFemConstraint::updateData(prop); }
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); const std::vector<Base::Vector3d>& points = pcConstraint->Points.getValues(); #ifdef USE_MULTIPLE_COPY SoMultipleCopy* cp = static_cast<SoMultipleCopy*>(pShapeSep->getChild(0)); cp->matrix.setNum(points.size()); SbMatrix* matrices = cp->matrix.startEditing(); #endif int idx = 0; 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) base = base + dir * scaledlength; //OvG: Scaling #ifdef USE_MULTIPLE_COPY SbMatrix m; m.setTransform(base, rot, SbVec3f(1,1,1)); matrices[idx] = m; #else SoSeparator* sep = static_cast<SoSeparator*>(pShapeSep->getChild(idx)); updatePlacement(sep, 0, base, rot); updateArrow(sep, 2, scaledlength, scaledheadradius); //OvG: Scaling #endif idx++; } #ifdef USE_MULTIPLE_COPY cp->matrix.finishEditing(); #endif } } else{// not inlet or outlet boundary type #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*)createFixed(scaledheight, scaledwidth)); //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)); 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, scaledheight, scaledwidth); //OvG: Scaling pShapeSep->addChild(sep); #endif n++; } #ifdef USE_MULTIPLE_COPY cp->matrix.finishEditing(); #endif } } ViewProviderFemConstraint::updateData(prop); }
bool TraversabilityGrassfire::determineDrivePlane(base::Vector3d startPos, bool searchSourunding) { size_t startX; size_t startY; if(!mlsGrid->toGrid(startPos, startX, startY, mlsGrid->getEnvironment()->getRootNode())) return false; //make shure temp maps have correct size bestPatchMap.resize(boost::extents[mlsGrid->getCellSizeY()][mlsGrid->getCellSizeX()]); visited.resize(boost::extents[mlsGrid->getCellSizeY()][mlsGrid->getCellSizeX()]); trData->resize(boost::extents[mlsGrid->getCellSizeY()][mlsGrid->getCellSizeX()]); //fill them with defautl values SurfacePatch *emptyPatch = NULL; //Note passing directly NULL to fill makes the compiler cry.... std::fill(bestPatchMap.data(), bestPatchMap.data() + bestPatchMap.num_elements(), emptyPatch); std::fill(visited.data(), visited.data() + visited.num_elements(), false); std::fill(trData->data(), trData->data() + trData->num_elements(), UNKNOWN); //init probability with zero TraversabilityGrid::ArrayType *probabilityArray = &(trGrid->getGridData(TraversabilityGrid::PROBABILITY)); std::fill(probabilityArray->data(), probabilityArray->data() + probabilityArray->num_elements(), 0); double bestHeightDiff = std::numeric_limits< double >::max(); SurfacePatch *bestMatchingPatch = NULL; size_t correctedStartX = startX; size_t correctedStartY = startY; //search the sourounding of the start pos for a start patch for(int i = 0; i < 10; i++) { for(int yi = -i; yi <= i; yi++) { for(int xi = -i; xi <= i; xi++) { //only check the 'outer rim' if(abs(yi) != i && abs(xi) != i) continue; size_t newX = startX + xi; size_t newY = startY + yi; if(newX < mlsGrid->getCellSizeX() && newY < mlsGrid->getCellSizeY()) { bool isObstacle; //look for patch with best height SurfacePatch *curPatch = getNearestPatchWhereRobotFits(newX, newY, startPos.z(), isObstacle); if(curPatch) { double curHeightDiff = fabs(startPos.z() - curPatch->getMean() + curPatch->getStdev()); if(curHeightDiff < bestHeightDiff) { bestMatchingPatch = curPatch; bestHeightDiff = curHeightDiff; correctedStartX = newX; correctedStartY = newY; } } } } } if(bestMatchingPatch) break; } if(!bestMatchingPatch) { //we are screwed, can't start the grassfire return false; } //recuse to surounding patches addNeightboursToSearchList(correctedStartX, correctedStartY, bestMatchingPatch); return true; }
void QGIViewPart::drawSectionLine(TechDraw::DrawViewSection* viewSection, bool b) { TechDraw::DrawViewPart *viewPart = static_cast<TechDraw::DrawViewPart *>(getViewObject()); if (!viewPart || !viewSection) { return; } if (b) { QGISectionLine* sectionLine = new QGISectionLine(); addToGroup(sectionLine); sectionLine->setSymbol(const_cast<char*>(viewSection->SectionSymbol.getValue())); //TODO: handle oblique section lines? //find smallest internal angle(normalDir,get?Dir()) and use -1*get?Dir() +/- angle //Base::Vector3d normalDir = viewSection->SectionNormal.getValue(); Base::Vector3d arrowDir(0,1,0); //for drawing only, not geom Base::Vector3d lineDir(1,0,0); bool horiz = false; if (viewSection->SectionDirection.isValue("Right")) { arrowDir = Base::Vector3d(1,0,0); lineDir = Base::Vector3d(0,1,0); } else if (viewSection->SectionDirection.isValue("Left")) { arrowDir = Base::Vector3d(-1,0,0); lineDir = Base::Vector3d(0,-1,0); } else if (viewSection->SectionDirection.isValue("Up")) { arrowDir = Base::Vector3d(0,1,0); lineDir = Base::Vector3d(1,0,0); horiz = true; } else if (viewSection->SectionDirection.isValue("Down")) { arrowDir = Base::Vector3d(0,-1,0); lineDir = Base::Vector3d(-1,0,0); horiz = true; } sectionLine->setDirection(arrowDir.x,arrowDir.y); Base::Vector3d org = viewSection->SectionOrigin.getValue(); double scale = viewPart->Scale.getValue(); Base::Vector3d pOrg = scale * viewPart->projectPoint(org); //pOrg.y = -1 * pOrg.y; //now project pOrg onto arrowDir Base::Vector3d displace; displace.ProjectToLine(pOrg, arrowDir); Base::Vector3d offset = pOrg + displace; sectionLine->setPos(offset.x,offset.y); double sectionSpan; double sectionFudge = 10.0; double xVal, yVal; if (horiz) { sectionSpan = m_border->rect().width() + sectionFudge; xVal = sectionSpan / 2.0; yVal = 0.0; } else { sectionSpan = (m_border->rect().height() - m_label->boundingRect().height()) + sectionFudge; xVal = 0.0; yVal = sectionSpan / 2.0; } sectionLine->setBounds(-xVal,-yVal,xVal,yVal); sectionLine->setWidth(viewPart->LineWidth.getValue()); //TODO: add fudge to make sectionLine thinner than reg lines? sectionLine->setFont(m_font,6.0); sectionLine->setZValue(ZVALUE::SECTIONLINE); sectionLine->draw(); } }