+ Add class CoordinateSystem and use it to compute placement of plane from fit
This commit is contained in:
parent
65c974ed2d
commit
580c541005
|
@ -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
|
||||
|
|
147
src/Base/CoordinateSystem.cpp
Normal file
147
src/Base/CoordinateSystem.cpp
Normal file
|
@ -0,0 +1,147 @@
|
|||
/***************************************************************************
|
||||
* Copyright (c) 2014 Werner Mayer <wmayer[at]users.sourceforge.net> *
|
||||
* *
|
||||
* 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 <cfloat>
|
||||
#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);
|
||||
}
|
104
src/Base/CoordinateSystem.h
Normal file
104
src/Base/CoordinateSystem.h
Normal file
|
@ -0,0 +1,104 @@
|
|||
/***************************************************************************
|
||||
* Copyright (c) 2014 Werner Mayer <wmayer[at]users.sourceforge.net> *
|
||||
* *
|
||||
* 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
|
|
@ -37,6 +37,7 @@
|
|||
#include <Gui/MainWindow.h>
|
||||
#include <Gui/FileDialog.h>
|
||||
#include <Gui/Selection.h>
|
||||
#include <Base/CoordinateSystem.h>
|
||||
|
||||
#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::Vector3d>(base));
|
||||
cs.setAxes(Base::convertTo<Base::Vector3d>(norm),
|
||||
Base::convertTo<Base::Vector3d>(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());
|
||||
|
|
Loading…
Reference in New Issue
Block a user