+ Add class CoordinateSystem and use it to compute placement of plane from fit

This commit is contained in:
wmayer 2014-07-29 23:01:29 +02:00
parent 65c974ed2d
commit 580c541005
4 changed files with 270 additions and 11 deletions

View File

@ -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

View 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
View 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

View File

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