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