From 580c54100580bcf5de3e176248276dd207dde46b Mon Sep 17 00:00:00 2001 From: wmayer Date: Tue, 29 Jul 2014 23:01:29 +0200 Subject: [PATCH] + Add class CoordinateSystem and use it to compute placement of plane from fit --- src/Base/CMakeLists.txt | 2 + src/Base/CoordinateSystem.cpp | 147 +++++++++++++++++++++ src/Base/CoordinateSystem.h | 104 +++++++++++++++ src/Mod/ReverseEngineering/Gui/Command.cpp | 28 ++-- 4 files changed, 270 insertions(+), 11 deletions(-) create mode 100644 src/Base/CoordinateSystem.cpp create mode 100644 src/Base/CoordinateSystem.h diff --git a/src/Base/CMakeLists.txt b/src/Base/CMakeLists.txt index 83b6ae503..1eaa8d4ca 100644 --- a/src/Base/CMakeLists.txt +++ b/src/Base/CMakeLists.txt @@ -182,6 +182,7 @@ SET(FreeCADBase_CPP_SRCS BoundBoxPyImp.cpp Builder3D.cpp Console.cpp + CoordinateSystem.cpp Debugger.cpp Exception.cpp Factory.cpp @@ -235,6 +236,7 @@ SET(FreeCADBase_HPP_SRCS BoundBox.h Builder3D.h Console.h + CoordinateSystem.h Debugger.h Exception.h Factory.h diff --git a/src/Base/CoordinateSystem.cpp b/src/Base/CoordinateSystem.cpp new file mode 100644 index 000000000..afef0d863 --- /dev/null +++ b/src/Base/CoordinateSystem.cpp @@ -0,0 +1,147 @@ +/*************************************************************************** + * Copyright (c) 2014 Werner Mayer * + * * + * This file is part of the FreeCAD CAx development system. * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of the GNU Library General Public * + * License as published by the Free Software Foundation; either * + * version 2 of the License, or (at your option) any later version. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Library General Public License for more details. * + * * + * You should have received a copy of the GNU Library General Public * + * License along with this library; see the file COPYING.LIB. If not, * + * write to the Free Software Foundation, Inc., 51 Franklin Street, * + * Fifth Floor, Boston, MA 02110-1301, USA * + * * + ***************************************************************************/ + + +#include "PreCompiled.h" +#ifndef _PreComp_ +# include +#endif + +#include "CoordinateSystem.h" +#include "Exception.h" + +using namespace Base; + +CoordinateSystem::CoordinateSystem() + : axis(Vector3d(), Vector3d(0,0,1)), xdir(1,0,0), ydir(0,1,0) +{ +} + +CoordinateSystem::~CoordinateSystem() +{ +} + +void CoordinateSystem::setAxes(const Axis& v, const Vector3d& xd) +{ + if (xd.Sqr() < FLT_EPSILON) + throw Base::Exception("Direction is null vector"); + Vector3d yd = v.getDirection() % xd; + if (yd.Sqr() < FLT_EPSILON) + throw Base::Exception("Direction is parallel to Z direction"); + ydir = yd; + xdir = ydir % v.getDirection(); + axis = v; +} + +void CoordinateSystem::setAxes(const Vector3d& n, const Vector3d& xd) +{ + if (xd.Sqr() < FLT_EPSILON) + throw Base::Exception("Direction is null vector"); + Vector3d yd = n % xd; + if (yd.Sqr() < FLT_EPSILON) + throw Base::Exception("Direction is parallel to Z direction"); + ydir = yd; + xdir = ydir % n; + axis.setDirection(n); +} + +void CoordinateSystem::setAxis(const Axis& v) +{ + setAxes(v, xdir); +} + +void CoordinateSystem::setXDirection(const Vector3d& dir) +{ + Vector3d yd = axis.getDirection() % dir; + if (yd.Sqr() < FLT_EPSILON) + throw Base::Exception("Direction is parallel to Z direction"); + ydir = yd; + xdir = ydir % axis.getDirection(); +} + +void CoordinateSystem::setYDirection(const Vector3d& dir) +{ + Vector3d xd = dir & axis.getDirection(); + if (xd.Sqr() < FLT_EPSILON) + throw Base::Exception("Direction is parallel to Z direction"); + xdir = xd; + ydir = axis.getDirection() % xdir; +} + +void CoordinateSystem::setZDirection(const Vector3d& dir) +{ + setAxes(dir, xdir); +} + +Placement CoordinateSystem::displacement(const CoordinateSystem& cs) const +{ + // align the Z axes + Base::Rotation rotZ(getZDirection(), cs.getZDirection()); + + // align the X axes + Base::Vector3d xd = xdir; + rotZ.multVec(xd,xd); + Base::Rotation rotX(xd, cs.getXDirection()); + + // the transformed base point + Vector3d mov = axis.getBase(); + rotZ.multVec(mov,mov); + rotX.multVec(mov,mov); + mov = cs.getPosition() - mov; + + Base::Rotation rot; + rot = rotX * rotZ; + + return Placement(mov, rot); +} + +void CoordinateSystem::transformTo(Vector3d& p) +{ + return p.TransformToCoordinateSystem(axis.getBase(), xdir, ydir); +} + +void CoordinateSystem::transform(const Placement& p) +{ + axis *= p; + p.getRotation().multVec(this->xdir, this->xdir); + p.getRotation().multVec(this->ydir, this->ydir); +} + +void CoordinateSystem::transform(const Rotation& r) +{ + Vector3d zdir = axis.getDirection(); + r.multVec(zdir, zdir); + axis.setDirection(zdir); + r.multVec(this->xdir, this->xdir); + r.multVec(this->ydir, this->ydir); +} + +void CoordinateSystem::setPlacement(const Placement& p) +{ + Vector3d zdir(0,0,1); + p.getRotation().multVec(zdir, zdir); + axis.setBase(p.getPosition()); + axis.setDirection(zdir); + + p.getRotation().multVec(Vector3d(1,0,0), this->xdir); + p.getRotation().multVec(Vector3d(0,1,0), this->ydir); +} diff --git a/src/Base/CoordinateSystem.h b/src/Base/CoordinateSystem.h new file mode 100644 index 000000000..456c5ed4c --- /dev/null +++ b/src/Base/CoordinateSystem.h @@ -0,0 +1,104 @@ +/*************************************************************************** + * Copyright (c) 2014 Werner Mayer * + * * + * This file is part of the FreeCAD CAx development system. * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of the GNU Library General Public * + * License as published by the Free Software Foundation; either * + * version 2 of the License, or (at your option) any later version. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Library General Public License for more details. * + * * + * You should have received a copy of the GNU Library General Public * + * License along with this library; see the file COPYING.LIB. If not, * + * write to the Free Software Foundation, Inc., 51 Franklin Street, * + * Fifth Floor, Boston, MA 02110-1301, USA * + * * + ***************************************************************************/ + + +#ifndef BASE_COORDINATESYSTEM_H +#define BASE_COORDINATESYSTEM_H + +#include "Axis.h" + +namespace Base { +/** + * Describes a right-handed coordinate system in 3D space. + \author Werner Mayer + */ +class BaseExport CoordinateSystem +{ +public: + /** Construct a default coordinate system with position in (0,0,0), + * with X axis (1,0,0), with Y axis (0,1,0) and Z axis (0,0,1) + */ + CoordinateSystem(); + ~CoordinateSystem(); + + /** Sets the main axis. X and Y dir are adjusted accordingly. + * The main axis \a v must not be parallel to the X axis + */ + void setAxis(const Axis& v); + /** Sets the main axis. X and Y dir are adjusted accordingly. + * The main axis must not be parallel to \a xd + */ + void setAxes(const Axis&, const Vector3d& xd); + /** Sets the main axis. X and Y dir are adjusted accordingly. + * The main axis \a n must not be parallel to \a xd + */ + void setAxes(const Vector3d& n, const Vector3d& xd); + inline const Axis& getAxis() const + { return axis; } + + /** The passed vector must not be parallel to the main axis */ + void setXDirection(const Vector3d&); + inline const Vector3d& getXDirection() const + { return xdir; } + + /** The passed vector must not be parallel to the main axis */ + void setYDirection(const Vector3d&); + inline const Vector3d& getYDirection() const + { return ydir; } + + /** Sets the main axis. X and Y dir are adjusted accordingly. + * The main axis must not be parallel to the X axis + */ + void setZDirection(const Vector3d&); + inline const Vector3d& getZDirection() const + { return axis.getDirection(); } + inline void setPosition(const Vector3d& p) + { axis.setBase(p); } + inline const Vector3d& getPosition() const + { return axis.getBase(); } + + /** This computes the displacement from this coordinate system to the + * given coordinate system \a cs + */ + Placement displacement(const CoordinateSystem& cs) const; + + /** Transform the point \a p to be in this coordinate system */ + void transformTo(Vector3d& p); + + /** Apply the placement \a p to the coordinate system. */ + void transform(const Placement& p); + + /** Apply the rotation \a r to the coordinate system. */ + void transform(const Rotation& r); + + /** Set the placement \a p to the coordinate system. */ + void setPlacement(const Placement& p); + +private: + Axis axis; + Vector3d xdir; + Vector3d ydir; +}; + +} + +#endif // BASE_COORDINATESYSTEM_H diff --git a/src/Mod/ReverseEngineering/Gui/Command.cpp b/src/Mod/ReverseEngineering/Gui/Command.cpp index 30991d150..28584e2a0 100644 --- a/src/Mod/ReverseEngineering/Gui/Command.cpp +++ b/src/Mod/ReverseEngineering/Gui/Command.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include "../App/ApproxSurface.h" @@ -148,7 +149,7 @@ void CmdApproxPlane::activated(int iMsg) Base::Vector3f dirV = fit.GetDirV(); Base::Vector3f norm = fit.GetNormal(); - // if the dot product of the refernce with the plane normal is negative + // if the dot product of the reference with the plane normal is negative // a flip must be done if (refNormal * norm < 0) { norm = -norm; @@ -157,25 +158,30 @@ void CmdApproxPlane::activated(int iMsg) float width, length; fit.Dimension(width, length); + // move to the corner point base = base - (0.5f * length * dirU + 0.5f * width * dirV); + Base::CoordinateSystem cs; + cs.setPosition(Base::convertTo(base)); + cs.setAxes(Base::convertTo(norm), + Base::convertTo(dirU)); + Base::Placement pm = Base::CoordinateSystem().displacement(cs); + double q0, q1, q2, q3; + pm.getRotation().getValue(q0, q1, q2, q3); + Base::Console().Log("RMS value for plane fit with %ld points: %.4f\n", aData.size(), sigma); Base::Console().Log(" Plane base(%.4f, %.4f, %.4f)\n", base.x, base.y, base.z); Base::Console().Log(" Plane normal(%.4f, %.4f, %.4f)\n", norm.x, norm.y, norm.z); std::stringstream str; - str << "import Part" << std::endl; str << "from FreeCAD import Base" << std::endl; - str << "App.ActiveDocument.addObject('Part::Feature','Plane_fit').Shape=" - << "Part.makePlane(" - << width << ", " << length << ", " - << "Base.Vector(" - << base.x << ", " << base.y << ", " << base.z << "), " - << "Base.Vector(" - << norm.x << ", " << norm.y << ", " << norm.z << "), " - << "Base.Vector(" - << dirU.x << ", " << dirU.y << ", " << dirU.z << "))" << std::endl; + str << "App.ActiveDocument.addObject('Part::Plane','Plane_fit')" << std::endl; + str << "App.ActiveDocument.ActiveObject.Length = " << length << std::endl; + str << "App.ActiveDocument.ActiveObject.Width = " << width << std::endl; + str << "App.ActiveDocument.ActiveObject.Placement = Base.Placement(" + << "Base.Vector(" << base.x << "," << base.y << "," << base.z << ")," + << "Base.Rotation(" << q0 << "," << q1 << "," << q2 << "," << q3 << "))" << std::endl; openCommand("Fit plane"); doCommand(Gui::Command::Doc, str.str().c_str());