335 lines
9.3 KiB
C++
335 lines
9.3 KiB
C++
/***************************************************************************
|
|
* Copyright (c) 2013 Stefan Tröger <stefantroeger@gmx.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., 59 Temple Place, *
|
|
* Suite 330, Boston, MA 02111-1307, USA *
|
|
* *
|
|
***************************************************************************/
|
|
|
|
|
|
#ifndef SOLVER_H
|
|
#define SOLVER_H
|
|
|
|
#include <Base/Placement.h>
|
|
|
|
#include <gp_Pnt.hxx>
|
|
#include <gp_Lin.hxx>
|
|
#include <gp_Pln.hxx>
|
|
#include <gp_Cylinder.hxx>
|
|
#include <Inventor/SbVec3f.h>
|
|
|
|
#include "opendcm/core.hpp"
|
|
#include "opendcm/module3d.hpp"
|
|
#include "opendcm/modulepart.hpp"
|
|
|
|
struct gp_pnt_accessor {
|
|
|
|
template<typename Scalar, int ID, typename T>
|
|
Scalar get(T& t) {
|
|
switch(ID) {
|
|
case 0:
|
|
return t.X();
|
|
case 1:
|
|
return t.Y();
|
|
case 2:
|
|
return t.Z();
|
|
default:
|
|
return 0;
|
|
};
|
|
};
|
|
template<typename Scalar, int ID, typename T>
|
|
void set(Scalar value, T& t) {
|
|
switch(ID) {
|
|
case 0:
|
|
t.SetX(value);
|
|
break;
|
|
case 1:
|
|
t.SetY(value);
|
|
break;
|
|
case 2:
|
|
t.SetZ(value);
|
|
break;
|
|
};
|
|
};
|
|
};
|
|
|
|
struct gp_lin_accessor {
|
|
|
|
template<typename Scalar, int ID, typename T>
|
|
Scalar get(T& t) {
|
|
switch(ID) {
|
|
case 0:
|
|
return t.Location().X();
|
|
case 1:
|
|
return t.Location().Y();
|
|
case 2:
|
|
return t.Location().Z();
|
|
case 3:
|
|
return t.Direction().X();
|
|
case 4:
|
|
return t.Direction().Y();
|
|
case 5:
|
|
return t.Direction().Z();
|
|
default:
|
|
return 0;
|
|
};
|
|
};
|
|
template<typename Scalar, int ID, typename T>
|
|
void set(Scalar value, T& t) {
|
|
gp_Pnt p = t.Location();
|
|
gp_Dir d = t.Direction();
|
|
switch(ID) {
|
|
case 0:
|
|
p.SetX(value);
|
|
break;
|
|
case 1:
|
|
p.SetY(value);
|
|
break;
|
|
case 2:
|
|
p.SetZ(value);
|
|
case 3:
|
|
d.SetX(value);
|
|
break;
|
|
case 4:
|
|
d.SetY(value);
|
|
break;
|
|
case 5:
|
|
d.SetZ(value);
|
|
break;
|
|
};
|
|
t.SetLocation(p);
|
|
t.SetDirection(d);
|
|
};
|
|
};
|
|
|
|
struct gp_pln_accessor {
|
|
|
|
template<typename Scalar, int ID, typename T>
|
|
Scalar get(T& t) {
|
|
switch(ID) {
|
|
case 0:
|
|
return t.Axis().Location().X();
|
|
case 1:
|
|
return t.Axis().Location().Y();
|
|
case 2:
|
|
return t.Axis().Location().Z();
|
|
case 3:
|
|
return t.Axis().Direction().X();
|
|
case 4:
|
|
return t.Axis().Direction().Y();
|
|
case 5:
|
|
return t.Axis().Direction().Z();
|
|
default:
|
|
return 0;
|
|
};
|
|
};
|
|
template<typename Scalar, int ID, typename T>
|
|
void set(Scalar value, T& t) {
|
|
gp_Pnt p = t.Axis().Location();
|
|
gp_Dir d = t.Axis().Direction();
|
|
switch(ID) {
|
|
case 0:
|
|
p.SetX(value);
|
|
break;
|
|
case 1:
|
|
p.SetY(value);
|
|
break;
|
|
case 2:
|
|
p.SetZ(value);
|
|
case 3:
|
|
d.SetX(value);
|
|
break;
|
|
case 4:
|
|
d.SetY(value);
|
|
break;
|
|
case 5:
|
|
d.SetZ(value);
|
|
break;
|
|
};
|
|
t.SetAxis(gp_Ax1(p,d));
|
|
};
|
|
};
|
|
|
|
struct gp_cylinder_accessor {
|
|
|
|
template<typename Scalar, int ID, typename T>
|
|
Scalar get(T& t) {
|
|
switch(ID) {
|
|
case 0:
|
|
return t.Axis().Location().X();
|
|
case 1:
|
|
return t.Axis().Location().Y();
|
|
case 2:
|
|
return t.Axis().Location().Z();
|
|
case 3:
|
|
return t.Axis().Direction().X();
|
|
case 4:
|
|
return t.Axis().Direction().Y();
|
|
case 5:
|
|
return t.Axis().Direction().Z();
|
|
case 6:
|
|
return t.Radius();
|
|
default:
|
|
return 0;
|
|
};
|
|
};
|
|
template<typename Scalar, int ID, typename T>
|
|
void set(Scalar value, T& t) {
|
|
gp_Pnt p = t.Axis().Location();
|
|
gp_Dir d = t.Axis().Direction();
|
|
switch(ID) {
|
|
case 0:
|
|
p.SetX(value);
|
|
break;
|
|
case 1:
|
|
p.SetY(value);
|
|
break;
|
|
case 2:
|
|
p.SetZ(value);
|
|
case 3:
|
|
d.SetX(value);
|
|
break;
|
|
case 4:
|
|
d.SetY(value);
|
|
break;
|
|
case 5:
|
|
d.SetZ(value);
|
|
break;
|
|
case 6:
|
|
t.SetRadius(value);
|
|
};
|
|
t.SetAxis(gp_Ax1(p,d));
|
|
};
|
|
};
|
|
|
|
struct placement_accessor {
|
|
|
|
double q0, q1, q2, q3;
|
|
Base::Vector3d vec;
|
|
|
|
template<typename Scalar, int ID, typename T>
|
|
Scalar get(T& t) {
|
|
t.getRotation().getValue(q0,q1,q2,q3);
|
|
switch(ID) {
|
|
case 0:
|
|
return q3;
|
|
case 1:
|
|
return q0;
|
|
case 2:
|
|
return q1;
|
|
case 3:
|
|
return q2;
|
|
case 4:
|
|
return t.getPosition()[0];
|
|
case 5:
|
|
return t.getPosition()[1];
|
|
case 6:
|
|
return t.getPosition()[2];
|
|
default:
|
|
return 0;
|
|
};
|
|
};
|
|
template<typename Scalar, int ID, typename T>
|
|
void set(Scalar value, T& t) {
|
|
switch(ID) {
|
|
case 0:
|
|
q3 = value;
|
|
break;
|
|
case 1:
|
|
q0 = value;
|
|
break;
|
|
case 2:
|
|
q1 = value;
|
|
break;
|
|
case 3:
|
|
q2 = value;
|
|
break;
|
|
case 4:
|
|
vec[0] = value;
|
|
break;
|
|
case 5:
|
|
vec[1] = value;
|
|
break;
|
|
case 6:
|
|
vec[2] = value;
|
|
break;
|
|
};
|
|
};
|
|
|
|
template<typename T>
|
|
void finalize(T& t) {
|
|
//need to do it at once as setting every value step by step would always normalize the rotation and
|
|
//therefor give a false value
|
|
Base::Rotation rot(q0,q1,q2,q3);
|
|
t.setRotation(rot);
|
|
t.setPosition(vec);
|
|
};
|
|
};
|
|
|
|
//geometry_traits for opencascade
|
|
namespace dcm {
|
|
template<>
|
|
struct geometry_traits<gp_Pnt> {
|
|
typedef tag::point3D tag;
|
|
typedef modell::XYZ modell;
|
|
typedef gp_pnt_accessor accessor;
|
|
};
|
|
template<>
|
|
struct geometry_traits<gp_Lin> {
|
|
typedef tag::line3D tag;
|
|
typedef modell::XYZ2 modell;
|
|
typedef gp_lin_accessor accessor;
|
|
};
|
|
template<>
|
|
struct geometry_traits<gp_Pln> {
|
|
typedef tag::plane3D tag;
|
|
typedef modell::XYZ2 modell;
|
|
typedef gp_pln_accessor accessor;
|
|
};
|
|
template<>
|
|
struct geometry_traits<gp_Cylinder> {
|
|
typedef tag::cylinder3D tag;
|
|
typedef modell::XYZ2P modell;
|
|
typedef gp_cylinder_accessor accessor;
|
|
};
|
|
template<>
|
|
struct geometry_traits<Base::Placement> {
|
|
typedef tag::part tag;
|
|
typedef modell::quaternion_wxyz_vec3 modell;
|
|
typedef placement_accessor accessor;
|
|
};
|
|
template<>
|
|
struct geometry_traits<SbVec3f> {
|
|
typedef tag::point3D tag;
|
|
typedef modell::XYZ modell;
|
|
typedef orderd_bracket_accessor accessor;
|
|
};
|
|
}
|
|
|
|
|
|
//our constraint solving system
|
|
typedef dcm::Kernel<double> Kernel;
|
|
typedef dcm::Module3D< mpl::vector5< gp_Pnt, gp_Lin, gp_Pln, gp_Cylinder, SbVec3f>, std::string > Module3D;
|
|
typedef dcm::ModulePart< mpl::vector1< Base::Placement >, std::string > ModulePart;
|
|
typedef dcm::System<Kernel, Module3D, ModulePart> Solver;
|
|
|
|
typedef typename ModulePart::type<Solver>::Part Part3D;
|
|
typedef typename Module3D::type<Solver>::Geometry3D Geometry3D;
|
|
typedef typename Module3D::type<Solver>::Constraint3D Constraint3D;
|
|
|
|
|
|
#endif //SOLVER_H
|