add opendcm constraint solver
This commit is contained in:
Stefan Tröger
committed by
Stefan Tröger
Stefan Tröger
parent
d50f7f1787
commit
02bc130c42
149
src/Mod/Assembly/App/opendcm/module3d/angle.hpp
Normal file
149
src/Mod/Assembly/App/opendcm/module3d/angle.hpp
Normal file
@@ -0,0 +1,149 @@
|
||||
/*
|
||||
openDCM, dimensional constraint manager
|
||||
Copyright (C) 2012 Stefan Troeger <stefantroeger@gmx.net>
|
||||
|
||||
This library is free software; you can redistribute it and/or modify
|
||||
it under the terms of the GNU Lesser General Public License as published by
|
||||
the Free Software Foundation; either version 2.1 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 Lesser General Public License for more detemplate tails.
|
||||
|
||||
You should have received a copy of the GNU Lesser General Public License along
|
||||
with this library; if not, write to the Free Software Foundation, Inc.,
|
||||
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
|
||||
*/
|
||||
|
||||
#ifndef GCM_ANGLE_HPP
|
||||
#define GCM_ANGLE_HPP
|
||||
|
||||
#include "geometry.hpp"
|
||||
|
||||
namespace dcm {
|
||||
|
||||
//the calculations( same as we always calculate directions we can outsource the work to this functions)
|
||||
namespace angle_detail {
|
||||
|
||||
template<typename Kernel, typename T>
|
||||
inline typename Kernel::number_type calc(T d1,
|
||||
T d2,
|
||||
typename Kernel::number_type angle) {
|
||||
|
||||
return d1.dot(d2) / (d1.norm()*d2.norm()) - angle;
|
||||
};
|
||||
|
||||
|
||||
template<typename Kernel, typename T>
|
||||
inline typename Kernel::number_type calcGradFirst(T d1,
|
||||
T d2,
|
||||
T dd1) {
|
||||
|
||||
typename Kernel::number_type norm = d1.norm()*d2.norm();
|
||||
return dd1.dot(d2)/norm - (d1.dot(d2) * (d1.dot(dd1)/d1.norm())*d2.norm()) / std::pow(norm,2);
|
||||
};
|
||||
|
||||
template<typename Kernel, typename T>
|
||||
inline typename Kernel::number_type calcGradSecond(T d1,
|
||||
T d2,
|
||||
T dd2) {
|
||||
|
||||
typename Kernel::number_type norm = d1.norm()*d2.norm();
|
||||
return d1.dot(dd2)/norm - (d1.dot(d2) * (d2.dot(dd2)/d2.norm())*d1.norm()) / std::pow(norm,2);
|
||||
};
|
||||
|
||||
template<typename Kernel, typename T>
|
||||
inline void calcGradFirstComp(T d1,
|
||||
T d2,
|
||||
T grad) {
|
||||
|
||||
typename Kernel::number_type norm = d1.norm()*d2.norm();
|
||||
grad = d2/norm - (d1.dot(d2)/std::pow(norm,2))*d1/d1.norm();
|
||||
};
|
||||
|
||||
template<typename Kernel, typename T>
|
||||
inline void calcGradSecondComp(T d1,
|
||||
T d2,
|
||||
T grad) {
|
||||
|
||||
typename Kernel::number_type norm = d1.norm()*d2.norm();
|
||||
grad = d1/norm - (d1.dot(d2)/std::pow(norm,2))*d2/d2.norm();
|
||||
};
|
||||
|
||||
}
|
||||
/*
|
||||
template< typename Kernel, typename Tag1, typename Tag2 >
|
||||
struct Angle3D {
|
||||
|
||||
typedef typename Kernel::number_type Scalar;
|
||||
typedef typename Kernel::VectorMap Vector;
|
||||
Scalar m_angle;
|
||||
|
||||
Angle3D(Scalar d = 0.) : m_angle(std::cos(d)) {};
|
||||
|
||||
//template definition
|
||||
void setScale(Scalar scale){
|
||||
assert(false);
|
||||
};
|
||||
Scalar calculate(Vector& param1, Vector& param2) {
|
||||
assert(false);
|
||||
};
|
||||
Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) {
|
||||
assert(false);
|
||||
};
|
||||
Scalar calculateGradientSecond(Vector& param1, Vector& param2, Vector& dparam2) {
|
||||
assert(false);
|
||||
};
|
||||
void calculateGradientFirstComplete(Vector& param1, Vector& param2, Vector& gradient) {
|
||||
assert(false);
|
||||
};
|
||||
void calculateGradientSecondComplete(Vector& param1, Vector& param2, Vector& gradient) {
|
||||
assert(false);
|
||||
};
|
||||
};
|
||||
|
||||
template< typename Kernel >
|
||||
struct Angle3D< Kernel, tag::line3D, tag::line3D > {
|
||||
|
||||
typedef typename Kernel::number_type Scalar;
|
||||
typedef typename Kernel::VectorMap Vector;
|
||||
|
||||
Scalar m_angle;
|
||||
|
||||
Angle3D(Scalar d = 0.) : m_angle(std::cos(d)) {};
|
||||
|
||||
Scalar getEquationScaling(typename Kernel::Vector& local1, typename Kernel::Vector& local2) {
|
||||
return 1.;
|
||||
}
|
||||
|
||||
//template definition
|
||||
Scalar calculate(Vector& param1, Vector& param2) {
|
||||
return angle::calc<Kernel>(param1.template tail<3>(), param2.template tail<3>(), m_angle);
|
||||
};
|
||||
Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) {
|
||||
return angle::calcGradFirst<Kernel>(param1.template tail<3>(), param2.template tail<3>(), dparam1.template tail<3>());
|
||||
};
|
||||
Scalar calculateGradientSecond(Vector& param1, Vector& param2, Vector& dparam2) {
|
||||
return angle::calcGradSecond<Kernel>(param1.template tail<3>(), param2.template tail<3>(), dparam2.template tail<3>());
|
||||
};
|
||||
void calculateGradientFirstComplete(Vector& param1, Vector& param2, Vector& gradient) {
|
||||
gradient.template head<3>().setZero();
|
||||
angle::calcGradFirstComp<Kernel>(param1.template tail<3>(), param2.template tail<3>(), gradient.template tail<3>());
|
||||
};
|
||||
void calculateGradientSecondComplete(Vector& param1, Vector& param2, Vector& gradient) {
|
||||
gradient.template head<3>().setZero();
|
||||
angle::calcGradSecondComp<Kernel>(param1.template tail<3>(), param2.template tail<3>(), gradient.template tail<3>());
|
||||
};
|
||||
};
|
||||
|
||||
//planes like lines have the direction as segment 3-5, so we can use the same implementations
|
||||
template< typename Kernel >
|
||||
struct Angle3D< Kernel, tag::plane3D, tag::plane3D > : public Angle3D<Kernel, tag::line3D, tag::line3D> {};
|
||||
template< typename Kernel >
|
||||
struct Angle3D< Kernel, tag::line3D, tag::plane3D > : public Angle3D<Kernel, tag::line3D, tag::line3D> {};
|
||||
*/
|
||||
}
|
||||
|
||||
#endif //GCM_ANGLE_HPP
|
||||
770
src/Mod/Assembly/App/opendcm/module3d/clustermath.hpp
Normal file
770
src/Mod/Assembly/App/opendcm/module3d/clustermath.hpp
Normal file
@@ -0,0 +1,770 @@
|
||||
/*
|
||||
openDCM, dimensional constraint manager
|
||||
Copyright (C) 2012 Stefan Troeger <stefantroeger@gmx.net>
|
||||
|
||||
This library is free software; you can redistribute it and/or modify
|
||||
it under the terms of the GNU Lesser General Public License as published by
|
||||
the Free Software Foundation; either version 2.1 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 Lesser General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU Lesser General Public License along
|
||||
with this library; if not, write to the Free Software Foundation, Inc.,
|
||||
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
|
||||
*/
|
||||
|
||||
#ifndef GCM_CLUSTERMATH_H
|
||||
#define GCM_CLUSTERMATH_H
|
||||
|
||||
#include <vector>
|
||||
#include <cmath>
|
||||
|
||||
#include <Eigen/StdVector>
|
||||
#include <boost/noncopyable.hpp>
|
||||
#include <opendcm/core/logging.hpp>
|
||||
#include "defines.hpp"
|
||||
|
||||
#define MAXFAKTOR 1.2 //the maximal distance allowd by a point normed to the cluster size
|
||||
#define MINFAKTOR 0.8 //the minimal distance allowd by a point normed to the cluster size
|
||||
#define SKALEFAKTOR 1. //the faktor by which the biggest size is multiplied to get the scale value
|
||||
#define NQFAKTOR 0.5 //the faktor by which the norm quaternion is multiplied with to get the RealScalar
|
||||
//norm quaternion to generate the unit quaternion
|
||||
|
||||
namespace dcm {
|
||||
namespace details {
|
||||
|
||||
enum Scalemode {
|
||||
one,
|
||||
two,
|
||||
three,
|
||||
multiple_inrange,
|
||||
multiple_outrange
|
||||
};
|
||||
|
||||
template<typename Sys>
|
||||
struct ClusterMath {
|
||||
|
||||
public:
|
||||
typedef typename system_traits<Sys>::Kernel Kernel;
|
||||
typedef typename system_traits<Sys>::Cluster Cluster;
|
||||
typedef typename system_traits<Sys>::template getModule<m3d>::type module3d;
|
||||
typedef typename module3d::Geometry3D Geometry3D;
|
||||
typedef boost::shared_ptr<Geometry3D> Geom;
|
||||
typedef typename module3d::math_prop math_prop;
|
||||
typedef typename module3d::fix_prop fix_prop;
|
||||
|
||||
typedef typename Kernel::number_type Scalar;
|
||||
|
||||
typename Kernel::Transform3D m_transform, m_ssrTransform, m_resetTransform;
|
||||
typename Kernel::DiffTransform3D m_diffTrans;
|
||||
typename Kernel::Vector3Map m_normQ;
|
||||
typename Kernel::Quaternion m_resetQuaternion;
|
||||
|
||||
int m_offset;
|
||||
bool init, fix;
|
||||
std::vector<Geom> m_geometry;
|
||||
|
||||
typename Kernel::Vector3Map m_translation;
|
||||
//shift scale stuff
|
||||
typename Kernel::Vector3 midpoint, m_shift, scale_dir, maxm, minm, max, fixtrans;
|
||||
Scalemode mode;
|
||||
Scalar m_scale;
|
||||
|
||||
typedef std::vector<typename Kernel::Vector3, Eigen::aligned_allocator<typename Kernel::Vector3> > Vec;
|
||||
Vec m_points, m_pseudo;
|
||||
|
||||
#ifdef USE_LOGGING
|
||||
src::logger log;
|
||||
#endif
|
||||
|
||||
public:
|
||||
ClusterMath();
|
||||
|
||||
void setParameterOffset(int offset);
|
||||
int getParameterOffset();
|
||||
|
||||
typename Kernel::Vector3Map& getNormQuaternionMap();
|
||||
typename Kernel::Vector3Map& getTranslationMap();
|
||||
void initMaps();
|
||||
void initFixMaps();
|
||||
|
||||
typename Kernel::Transform3D& getTransform();
|
||||
void mapsToTransform(typename Kernel::Transform3D& trans);
|
||||
void transformToMaps(typename Kernel::Transform3D& trans);
|
||||
|
||||
void finishCalculation();
|
||||
void finishFixCalculation();
|
||||
|
||||
void resetClusterRotation(typename Kernel::Transform3D& trans);
|
||||
|
||||
void calcDiffTransform(typename Kernel::DiffTransform3D& trans);
|
||||
void recalculate();
|
||||
|
||||
void addGeometry(Geom g);
|
||||
void clearGeometry();
|
||||
std::vector<Geom>& getGeometry();
|
||||
|
||||
struct map_downstream {
|
||||
|
||||
details::ClusterMath<Sys>& m_clusterMath;
|
||||
typename Kernel::Transform3D m_transform;
|
||||
bool m_isFixed;
|
||||
|
||||
map_downstream(details::ClusterMath<Sys>& cm, bool fix);
|
||||
|
||||
void operator()(Geom g);
|
||||
void operator()(boost::shared_ptr<Cluster> c);
|
||||
};
|
||||
|
||||
void mapClusterDownstreamGeometry(boost::shared_ptr<Cluster> cluster);
|
||||
|
||||
//Calculate the scale of the cluster. Therefore the midpoint is calculated and the scale is
|
||||
// defined as the max distance between the midpoint and the points.
|
||||
Scalar calculateClusterScale();
|
||||
|
||||
void applyClusterScale(Scalar scale, bool isFixed);
|
||||
|
||||
private:
|
||||
Scalar calcOnePoint(const typename Kernel::Vector3& p);
|
||||
|
||||
Scalar calcTwoPoints(const typename Kernel::Vector3& p1, const typename Kernel::Vector3& p2);
|
||||
|
||||
Scalar calcThreePoints(const typename Kernel::Vector3& p1,
|
||||
const typename Kernel::Vector3& p2, const typename Kernel::Vector3& p3);
|
||||
};
|
||||
|
||||
|
||||
/*****************************************************************************************************************/
|
||||
/*****************************************************************************************************************/
|
||||
/*****************************************************************************************************************/
|
||||
/*****************************************************************************************************************/
|
||||
|
||||
|
||||
|
||||
template<typename Sys>
|
||||
ClusterMath<Sys>::ClusterMath() : m_normQ(NULL), m_translation(NULL), init(false) {
|
||||
|
||||
m_resetTransform = typename Kernel::Quaternion(1,1,1,1);
|
||||
m_shift.setZero();
|
||||
|
||||
#ifdef USE_LOGGING
|
||||
log.add_attribute("Tag", attrs::constant< std::string >("Clustermath3D"));
|
||||
#endif
|
||||
};
|
||||
|
||||
template<typename Sys>
|
||||
void ClusterMath<Sys>::setParameterOffset(int offset) {
|
||||
m_offset = offset;
|
||||
};
|
||||
|
||||
template<typename Sys>
|
||||
int ClusterMath<Sys>::getParameterOffset() {
|
||||
return m_offset;
|
||||
};
|
||||
|
||||
template<typename Sys>
|
||||
typename ClusterMath<Sys>::Kernel::Vector3Map& ClusterMath<Sys>::getNormQuaternionMap() {
|
||||
return m_normQ;
|
||||
};
|
||||
|
||||
template<typename Sys>
|
||||
typename ClusterMath<Sys>::Kernel::Vector3Map& ClusterMath<Sys>::getTranslationMap() {
|
||||
return m_translation;
|
||||
};
|
||||
|
||||
template<typename Sys>
|
||||
void ClusterMath<Sys>::initMaps() {
|
||||
|
||||
transformToMaps(m_transform);
|
||||
init = true;
|
||||
midpoint.setZero();
|
||||
m_shift.setZero();
|
||||
m_ssrTransform.setIdentity();
|
||||
m_diffTrans = m_transform;
|
||||
fix=false;
|
||||
#ifdef USE_LOGGING
|
||||
BOOST_LOG(log) << "Init transform: "<<m_transform;
|
||||
#endif
|
||||
};
|
||||
|
||||
template<typename Sys>
|
||||
void ClusterMath<Sys>::initFixMaps() {
|
||||
//when fixed no maps exist
|
||||
new(&m_translation) typename Kernel::Vector3Map(&fixtrans(0));
|
||||
m_translation = m_transform.translation().vector();
|
||||
init = true;
|
||||
midpoint.setZero();
|
||||
m_shift.setZero();
|
||||
m_ssrTransform.setIdentity();
|
||||
m_diffTrans = m_transform;
|
||||
fix=true;
|
||||
#ifdef USE_LOGGING
|
||||
BOOST_LOG(log) << "Init fix transform: "<<m_transform;
|
||||
#endif
|
||||
};
|
||||
|
||||
template<typename Sys>
|
||||
typename ClusterMath<Sys>::Kernel::Transform3D& ClusterMath<Sys>::getTransform() {
|
||||
return m_transform;
|
||||
};
|
||||
|
||||
template<typename Sys>
|
||||
void ClusterMath<Sys>::mapsToTransform(typename ClusterMath<Sys>::Kernel::Transform3D& trans) {
|
||||
//add scale only after possible reset
|
||||
typename Kernel::Transform3D::Scaling scale(m_transform.scaling());
|
||||
trans = m_diffTrans;
|
||||
trans *= scale;
|
||||
};
|
||||
|
||||
template<typename Sys>
|
||||
void ClusterMath<Sys>::transformToMaps(typename ClusterMath<Sys>::Kernel::Transform3D& trans) {
|
||||
|
||||
const typename Kernel::Quaternion& m_quaternion = trans.rotation();
|
||||
if(m_quaternion.w() < 1.) {
|
||||
Scalar s = std::acos(m_quaternion.w())/std::sin(std::acos(m_quaternion.w()));
|
||||
m_normQ = m_quaternion.vec()*s;
|
||||
m_normQ /= NQFAKTOR;
|
||||
} else {
|
||||
m_normQ.setZero();
|
||||
}
|
||||
m_translation = trans.translation().vector();
|
||||
};
|
||||
|
||||
template<typename Sys>
|
||||
void ClusterMath<Sys>::finishCalculation() {
|
||||
|
||||
mapsToTransform(m_transform);
|
||||
init=false;
|
||||
|
||||
#ifdef USE_LOGGING
|
||||
BOOST_LOG(log) << "Finish calculation";
|
||||
#endif
|
||||
|
||||
m_transform = m_ssrTransform*m_transform;
|
||||
|
||||
//scale all geometries back to the original size
|
||||
m_diffTrans *= typename Kernel::Transform3D::Scaling(1./m_ssrTransform.scaling().factor());
|
||||
typedef typename std::vector<Geom>::iterator iter;
|
||||
for(iter it = m_geometry.begin(); it != m_geometry.end(); it++)
|
||||
(*it)->recalculate(m_diffTrans);
|
||||
#ifdef USE_LOGGING
|
||||
BOOST_LOG(log) << "Finish transform:"<<std::endl<<m_transform;
|
||||
#endif
|
||||
};
|
||||
|
||||
template<typename Sys>
|
||||
void ClusterMath<Sys>::finishFixCalculation() {
|
||||
#ifdef USE_LOGGING
|
||||
BOOST_LOG(log) << "Finish fix calculation";
|
||||
#endif
|
||||
typedef typename std::vector<Geom>::iterator iter;
|
||||
m_transform *= m_ssrTransform.inverse();
|
||||
typename Kernel::DiffTransform3D diff(m_transform);
|
||||
for(iter it = m_geometry.begin(); it != m_geometry.end(); it++)
|
||||
(*it)->recalculate(diff);
|
||||
#ifdef USE_LOGGING
|
||||
BOOST_LOG(log) << "Finish fix transform:"<<std::endl<<m_transform;
|
||||
#endif
|
||||
};
|
||||
|
||||
template<typename Sys>
|
||||
void ClusterMath<Sys>::resetClusterRotation(typename ClusterMath<Sys>::Kernel::Transform3D& trans) {
|
||||
|
||||
#ifdef USE_LOGGING
|
||||
BOOST_LOG(log) << "Reset cluster rotation:"<<std::endl<<m_diffTrans;
|
||||
#endif
|
||||
|
||||
trans = m_resetTransform.inverse()*trans;
|
||||
m_ssrTransform *= m_resetTransform;
|
||||
m_transform = m_resetTransform.inverse()*m_transform;
|
||||
|
||||
//apply the needed transformation to all geometries local values
|
||||
typedef typename std::vector<Geom>::iterator iter;
|
||||
for(iter it = m_geometry.begin(); it != m_geometry.end(); it++) {
|
||||
(*it)->transform(m_resetTransform);
|
||||
};
|
||||
};
|
||||
|
||||
template<typename Sys>
|
||||
void ClusterMath<Sys>::calcDiffTransform(typename ClusterMath<Sys>::Kernel::DiffTransform3D& trans) {
|
||||
|
||||
Scalar norm = m_normQ.norm();
|
||||
trans.setIdentity();
|
||||
|
||||
if(norm < 0.1) {
|
||||
if(norm == 0) {
|
||||
trans *= typename Kernel::Transform3D::Translation(m_translation);
|
||||
resetClusterRotation(trans);
|
||||
} else {
|
||||
const Scalar fac = std::sin(NQFAKTOR*norm)/norm;
|
||||
trans = typename Kernel::Quaternion(std::cos(NQFAKTOR*norm), m_normQ(0)*fac, m_normQ(1)*fac,m_normQ(2)*fac);
|
||||
trans *= typename Kernel::Transform3D::Translation(m_translation);
|
||||
resetClusterRotation(trans);
|
||||
}
|
||||
transformToMaps(trans);
|
||||
return;
|
||||
}
|
||||
|
||||
const Scalar fac = std::sin(NQFAKTOR*norm)/norm;
|
||||
trans = typename Kernel::Quaternion(std::cos(NQFAKTOR*norm), m_normQ(0)*fac, m_normQ(1)*fac, m_normQ(2)*fac);
|
||||
trans *= typename Kernel::Transform3D::Translation(m_translation);
|
||||
};
|
||||
|
||||
template<typename Sys>
|
||||
void ClusterMath<Sys>::recalculate() {
|
||||
|
||||
|
||||
calcDiffTransform(m_diffTrans);
|
||||
|
||||
const typename Kernel::Quaternion Q = m_diffTrans.rotation();
|
||||
|
||||
// now calculate the gradient quaternions and calculate the diff rotation matrices
|
||||
// m_normQ = (a,b,c)
|
||||
// n = ||m_normQ||
|
||||
//
|
||||
// Q = (a/n sin(n), b/n sin(n), c/n sin(n), cos(n))
|
||||
//
|
||||
|
||||
//n=||m_normQ||, sn = sin(n)/n, sn3 = sin(n)/n^3, cn = cos(n)/n, divn = 1/n;
|
||||
const Scalar n = m_normQ.norm();
|
||||
const Scalar sn = std::sin(NQFAKTOR*n)/n;
|
||||
const Scalar mul = (NQFAKTOR*std::cos(NQFAKTOR*n)-sn)/std::pow(n,2);
|
||||
|
||||
//dxa = dQx/da
|
||||
const Scalar dxa = sn + std::pow(m_normQ(0),2)*mul;
|
||||
const Scalar dxb = m_normQ(0)*m_normQ(1)*mul;
|
||||
const Scalar dxc = m_normQ(0)*m_normQ(2)*mul;
|
||||
|
||||
const Scalar dya = m_normQ(1)*m_normQ(0)*mul;
|
||||
const Scalar dyb = sn + std::pow(m_normQ(1),2)*mul;
|
||||
const Scalar dyc = m_normQ(1)*m_normQ(2)*mul;
|
||||
|
||||
const Scalar dza = m_normQ(2)*m_normQ(0)*mul;
|
||||
const Scalar dzb = m_normQ(2)*m_normQ(1)*mul;
|
||||
const Scalar dzc = sn + std::pow(m_normQ(2),2)*mul;
|
||||
|
||||
const Scalar dwa = -sn*NQFAKTOR*m_normQ(0);
|
||||
const Scalar dwb = -sn*NQFAKTOR*m_normQ(1);
|
||||
const Scalar dwc = -sn*NQFAKTOR*m_normQ(2);
|
||||
|
||||
//write in the diffrot matrix, starting with dQ/dx
|
||||
m_diffTrans.at(0,0) = -4.0*(Q.y()*dya+Q.z()*dza);
|
||||
m_diffTrans.at(0,1) = -2.0*(Q.w()*dza+dwa*Q.z())+2.0*(Q.x()*dya+dxa*Q.y());
|
||||
m_diffTrans.at(0,2) = 2.0*(dwa*Q.y()+Q.w()*dya)+2.0*(dxa*Q.z()+Q.x()*dza);
|
||||
m_diffTrans.at(1,0) = 2.0*(Q.w()*dza+dwa*Q.z())+2.0*(Q.x()*dya+dxa*Q.y());
|
||||
m_diffTrans.at(1,1) = -4.0*(Q.x()*dxa+Q.z()*dza);
|
||||
m_diffTrans.at(1,2) = -2.0*(dwa*Q.x()+Q.w()*dxa)+2.0*(dya*Q.z()+Q.y()*dza);
|
||||
m_diffTrans.at(2,0) = -2.0*(dwa*Q.y()+Q.w()*dya)+2.0*(dxa*Q.z()+Q.x()*dza);
|
||||
m_diffTrans.at(2,1) = 2.0*(dwa*Q.x()+Q.w()*dxa)+2.0*(dya*Q.z()+Q.y()*dza);
|
||||
m_diffTrans.at(2,2) = -4.0*(Q.x()*dxa+Q.y()*dya);
|
||||
|
||||
//dQ/dy
|
||||
m_diffTrans.at(0,3) = -4.0*(Q.y()*dyb+Q.z()*dzb);
|
||||
m_diffTrans.at(0,4) = -2.0*(Q.w()*dzb+dwb*Q.z())+2.0*(Q.x()*dyb+dxb*Q.y());
|
||||
m_diffTrans.at(0,5) = 2.0*(dwb*Q.y()+Q.w()*dyb)+2.0*(dxb*Q.z()+Q.x()*dzb);
|
||||
m_diffTrans.at(1,3) = 2.0*(Q.w()*dzb+dwb*Q.z())+2.0*(Q.x()*dyb+dxb*Q.y());
|
||||
m_diffTrans.at(1,4) = -4.0*(Q.x()*dxb+Q.z()*dzb);
|
||||
m_diffTrans.at(1,5) = -2.0*(dwb*Q.x()+Q.w()*dxb)+2.0*(dyb*Q.z()+Q.y()*dzb);
|
||||
m_diffTrans.at(2,3) = -2.0*(dwb*Q.y()+Q.w()*dyb)+2.0*(dxb*Q.z()+Q.x()*dzb);
|
||||
m_diffTrans.at(2,4) = 2.0*(dwb*Q.x()+Q.w()*dxb)+2.0*(dyb*Q.z()+Q.y()*dzb);
|
||||
m_diffTrans.at(2,5) = -4.0*(Q.x()*dxb+Q.y()*dyb);
|
||||
|
||||
//dQ/dz
|
||||
m_diffTrans.at(0,6) = -4.0*(Q.y()*dyc+Q.z()*dzc);
|
||||
m_diffTrans.at(0,7) = -2.0*(Q.w()*dzc+dwc*Q.z())+2.0*(Q.x()*dyc+dxc*Q.y());
|
||||
m_diffTrans.at(0,8) = 2.0*(dwc*Q.y()+Q.w()*dyc)+2.0*(dxc*Q.z()+Q.x()*dzc);
|
||||
m_diffTrans.at(1,6) = 2.0*(Q.w()*dzc+dwc*Q.z())+2.0*(Q.x()*dyc+dxc*Q.y());
|
||||
m_diffTrans.at(1,7) = -4.0*(Q.x()*dxc+Q.z()*dzc);
|
||||
m_diffTrans.at(1,8) = -2.0*(dwc*Q.x()+Q.w()*dxc)+2.0*(dyc*Q.z()+Q.y()*dzc);
|
||||
m_diffTrans.at(2,6) = -2.0*(dwc*Q.y()+Q.w()*dyc)+2.0*(dxc*Q.z()+Q.x()*dzc);
|
||||
m_diffTrans.at(2,7) = 2.0*(dwc*Q.x()+Q.w()*dxc)+2.0*(dyc*Q.z()+Q.y()*dzc);
|
||||
m_diffTrans.at(2,8) = -4.0*(Q.x()*dxc+Q.y()*dyc);
|
||||
|
||||
//recalculate all geometries
|
||||
typedef typename std::vector<Geom>::iterator iter;
|
||||
for(iter it = m_geometry.begin(); it != m_geometry.end(); it++)
|
||||
(*it)->recalculate(m_diffTrans);
|
||||
};
|
||||
|
||||
template<typename Sys>
|
||||
void ClusterMath<Sys>::addGeometry(Geom g) {
|
||||
m_geometry.push_back(g);
|
||||
};
|
||||
|
||||
template<typename Sys>
|
||||
void ClusterMath<Sys>::clearGeometry() {
|
||||
m_geometry.clear();
|
||||
};
|
||||
|
||||
template<typename Sys>
|
||||
std::vector<typename ClusterMath<Sys>::Geom>& ClusterMath<Sys>::getGeometry() {
|
||||
return m_geometry;
|
||||
};
|
||||
|
||||
template<typename Sys>
|
||||
ClusterMath<Sys>::map_downstream::map_downstream(details::ClusterMath<Sys>& cm, bool fix)
|
||||
: m_clusterMath(cm), m_isFixed(fix) {
|
||||
m_transform = m_clusterMath.getTransform();
|
||||
};
|
||||
|
||||
template<typename Sys>
|
||||
void ClusterMath<Sys>::map_downstream::operator()(Geom g) {
|
||||
//allow iteration over all maped geometries
|
||||
m_clusterMath.addGeometry(g);
|
||||
//set the offsets so that geometry knows where it is in the parameter map
|
||||
g->m_offset = m_clusterMath.getParameterOffset();
|
||||
//position and offset of the parameters must be set to the clusters values
|
||||
g->setClusterMode(true, m_isFixed);
|
||||
//calculate the appropriate local values
|
||||
typename Kernel::Transform3D trans = m_transform.inverse();
|
||||
g->transform(trans);
|
||||
};
|
||||
|
||||
template<typename Sys>
|
||||
void ClusterMath<Sys>::map_downstream::operator()(boost::shared_ptr<Cluster> c) {
|
||||
m_transform *= c->template getClusterProperty<math_prop>().getTransform();
|
||||
};
|
||||
|
||||
|
||||
template<typename Sys>
|
||||
void ClusterMath<Sys>::mapClusterDownstreamGeometry(boost::shared_ptr<Cluster> cluster) {
|
||||
|
||||
#ifdef USE_LOGGING
|
||||
BOOST_LOG(log) << "Map downstream geometry";
|
||||
#endif
|
||||
|
||||
map_downstream down(cluster->template getClusterProperty<math_prop>(),
|
||||
cluster->template getClusterProperty<fix_prop>());
|
||||
cluster->template for_each<Geometry3D>(down, true);
|
||||
//TODO: if one subcluster is fixed the hole cluster should be too, as there are no
|
||||
// dof's remaining between parts and so nothing can be moved when one part is fixed.
|
||||
};
|
||||
|
||||
template<typename Sys>
|
||||
typename ClusterMath<Sys>::Scalar ClusterMath<Sys>::calculateClusterScale() {
|
||||
|
||||
#ifdef USE_LOGGING
|
||||
BOOST_LOG(log) << "Calculate cluster scale with transform scale: "<<m_transform.scaling().factor();
|
||||
#endif
|
||||
//ensure the usage of the right transformation
|
||||
if(!fix)
|
||||
mapsToTransform(m_transform);
|
||||
#ifdef USE_LOGGING
|
||||
BOOST_LOG(log) << "Calculate cluster scale sec transform scale: "<<m_transform.scaling().factor();
|
||||
#endif
|
||||
|
||||
//collect all points together
|
||||
m_points.clear();
|
||||
typename Kernel::Transform3D trans(m_transform.rotation(), m_transform.translation());
|
||||
trans.invert();
|
||||
typedef typename Vec::iterator iter;
|
||||
for(iter it = m_pseudo.begin(); it != m_pseudo.end(); it++) {
|
||||
m_points.push_back(trans*(*it));
|
||||
}
|
||||
typedef typename std::vector<Geom>::iterator g_iter;
|
||||
for(g_iter it = m_geometry.begin(); it != m_geometry.end(); it++)
|
||||
m_points.push_back((*it)->getPoint());
|
||||
|
||||
//start scale calculation
|
||||
if(m_points.empty()) assert(false); //TODO: Throw
|
||||
else if(m_points.size() == 1) {
|
||||
const typename Kernel::Vector3 p = m_points[0];
|
||||
return calcOnePoint(p);
|
||||
} else if(m_points.size() == 2) {
|
||||
const typename Kernel::Vector3 p1 = m_points[0];
|
||||
const typename Kernel::Vector3 p2 = m_points[1];
|
||||
|
||||
if(Kernel::isSame((p1-p2).norm(), 0.))
|
||||
return calcOnePoint(p1);
|
||||
|
||||
return calcTwoPoints(p1, p2);
|
||||
} else if(m_points.size() == 3) {
|
||||
|
||||
const typename Kernel::Vector3 p1 = m_points[0];
|
||||
const typename Kernel::Vector3 p2 = m_points[1];
|
||||
const typename Kernel::Vector3 p3 = m_points[2];
|
||||
|
||||
const typename Kernel::Vector3 d = p2-p1;
|
||||
const typename Kernel::Vector3 e = p3-p1;
|
||||
|
||||
if(Kernel::isSame(d.norm(), 0.)) {
|
||||
|
||||
if(Kernel::isSame(e.norm(), 0.))
|
||||
return calcOnePoint(p1);
|
||||
|
||||
return calcTwoPoints(p1, p3);
|
||||
} else if(Kernel::isSame(e.norm(), 0.)) {
|
||||
return calcTwoPoints(p1, p2);
|
||||
} else if(!Kernel::isSame((d/d.norm() - e/e.norm()).norm(), 0.) &&
|
||||
!Kernel::isSame((d/d.norm() + e/e.norm()).norm(), 0.)) {
|
||||
return calcThreePoints(p1, p2, p3);
|
||||
}
|
||||
//three points on a line need to be treaded as multiple points
|
||||
}
|
||||
|
||||
//more than 3 points dont have a exakt solution. we search for a midpoint from which all points
|
||||
//are at least MAXFAKTOR*scale away, but not closer than MINFAKTOR*scale
|
||||
|
||||
//get the bonding box to get the center of points
|
||||
Scalar xmin=1e10, xmax=1e-10, ymin=1e10, ymax=1e-10, zmin=1e10, zmax=1e-10;
|
||||
for(iter it = m_points.begin(); it != m_points.end(); it++) {
|
||||
typename Kernel::Vector3 v = (*it);
|
||||
xmin = (v(0)<xmin) ? v(0) : xmin;
|
||||
xmax = (v(0)<xmax) ? xmax : v(0);
|
||||
ymin = (v(1)<ymin) ? v(1) : ymin;
|
||||
ymax = (v(1)<ymax) ? ymax : v(1);
|
||||
zmin = (v(2)<zmin) ? v(2) : zmin;
|
||||
zmax = (v(2)<zmax) ? zmax : v(2);
|
||||
};
|
||||
//now calculate the midpoint
|
||||
midpoint << xmin+xmax, ymin+ymax, zmin+zmax;
|
||||
midpoint /= 2.;
|
||||
|
||||
|
||||
//get the scale direction an the resulting nearest point indexes
|
||||
double xh = xmax-xmin;
|
||||
double yh = ymax-ymin;
|
||||
double zh = zmax-zmin;
|
||||
int i1, i2, i3;
|
||||
if((xh<=yh) && (xh<=zh)) {
|
||||
i1=1;
|
||||
i2=2;
|
||||
i3=0;
|
||||
} else if((yh<xh) && (yh<zh)) {
|
||||
i1=0;
|
||||
i2=2;
|
||||
i3=1;
|
||||
} else {
|
||||
i1=0;
|
||||
i2=1;
|
||||
i3=2;
|
||||
}
|
||||
scale_dir.setZero();
|
||||
scale_dir(i3) = 1;
|
||||
max = Eigen::Vector3d(xmin,ymin,zmin);
|
||||
m_scale = (midpoint-max).norm()/MAXFAKTOR;
|
||||
mode = multiple_inrange;
|
||||
|
||||
maxm = max-midpoint;
|
||||
Scalar minscale = 1e10;
|
||||
|
||||
//get the closest point
|
||||
for(iter it = m_points.begin(); it != m_points.end(); it++) {
|
||||
|
||||
const Eigen::Vector3d point = (*it)-midpoint;
|
||||
if(point.norm()<MINFAKTOR*m_scale) {
|
||||
|
||||
const double h = std::abs(point(i3)-maxm(i3));
|
||||
const double k = std::pow(MINFAKTOR/MAXFAKTOR,2);
|
||||
double q = std::pow(point(i1),2) + std::pow(point(i2),2);
|
||||
q -= (std::pow(maxm(i1),2) + std::pow(maxm(i2),2) + std::pow(h,2))*k;
|
||||
q /= 1.-k;
|
||||
|
||||
const double p = h*k/(1.-k);
|
||||
|
||||
if(std::pow(p,2)<q) assert(false);
|
||||
|
||||
midpoint(i3) += p + std::sqrt(std::pow(p,2)-q);
|
||||
maxm = max-midpoint;
|
||||
m_scale = maxm.norm()/MAXFAKTOR;
|
||||
|
||||
mode = multiple_outrange;
|
||||
minm = (*it)-midpoint;
|
||||
|
||||
it = m_points.begin();
|
||||
} else if(point.norm()<minscale) {
|
||||
minscale = point.norm();
|
||||
}
|
||||
}
|
||||
|
||||
if(mode==multiple_inrange) {
|
||||
//we are in the range, let's get the perfect balanced scale value
|
||||
m_scale = (minscale+maxm.norm())/2.;
|
||||
}
|
||||
return m_scale;
|
||||
};
|
||||
|
||||
template<typename Sys>
|
||||
void ClusterMath<Sys>::applyClusterScale(Scalar scale, bool isFixed) {
|
||||
|
||||
#ifdef USE_LOGGING
|
||||
BOOST_LOG(log) << "Apply cluster scale: "<<scale;
|
||||
BOOST_LOG(log) << "initial transform scale: "<<m_transform.scaling().factor();
|
||||
#endif
|
||||
//ensure the usage of the right transform
|
||||
if(!fix)
|
||||
mapsToTransform(m_transform);
|
||||
|
||||
|
||||
|
||||
//when fixed, the geometries never get recalculated. therefore we have to do a calculate now
|
||||
//to alow the adoption of the scale. and no shift should been set.
|
||||
if(isFixed) {
|
||||
m_transform*=typename Kernel::Transform3D::Scaling(1./(scale*SKALEFAKTOR));
|
||||
m_ssrTransform*=typename Kernel::Transform3D::Scaling(1./(scale*SKALEFAKTOR));
|
||||
typename Kernel::DiffTransform3D diff(m_transform);
|
||||
//now calculate the scaled geometrys
|
||||
typedef typename std::vector<Geom>::iterator iter;
|
||||
for(iter it = m_geometry.begin(); it != m_geometry.end(); it++) {
|
||||
(*it)->recalculate(diff);
|
||||
#ifdef USE_LOGGING
|
||||
BOOST_LOG(log) << "Fixed cluster geometry value:" << (*it)->m_rotated.transpose();
|
||||
#endif
|
||||
};
|
||||
return;
|
||||
}
|
||||
|
||||
//if this is our scale then just applie the midpoint as shift
|
||||
if(Kernel::isSame(scale, m_scale)) {
|
||||
|
||||
}
|
||||
//if only one point exists we extend the origin-point-line to match the scale
|
||||
else if(mode==details::one) {
|
||||
if(Kernel::isSame(midpoint.norm(),0))
|
||||
midpoint << scale, 0, 0;
|
||||
else midpoint += scale*scale_dir;
|
||||
}
|
||||
//two and three points form a rectangular triangle, so same procedure
|
||||
else if(mode==details::two || mode==details::three) {
|
||||
|
||||
midpoint+= scale_dir*std::sqrt(std::pow(scale,2) - std::pow(m_scale,2));
|
||||
}
|
||||
//multiple points
|
||||
else if(mode==details::multiple_outrange) {
|
||||
|
||||
if(scale_dir(0)) {
|
||||
Scalar d = std::pow(maxm(1),2) + std::pow(maxm(2),2);
|
||||
Scalar h = std::sqrt(std::pow(MAXFAKTOR*scale,2)-d);
|
||||
midpoint(0) += maxm(0) + h;
|
||||
} else if(scale_dir(1)) {
|
||||
Scalar d = std::pow(maxm(0),2) + std::pow(maxm(2),2);
|
||||
Scalar h = std::sqrt(std::pow(MAXFAKTOR*scale,2)-d);
|
||||
midpoint(1) += maxm(1) + h;
|
||||
} else {
|
||||
Scalar d = std::pow(maxm(0),2) + std::pow(maxm(1),2);
|
||||
Scalar h = std::sqrt(std::pow(MAXFAKTOR*scale,2)-d);
|
||||
midpoint(2) += maxm(2) + h;
|
||||
}
|
||||
} else {
|
||||
|
||||
//TODO: it's possible that for this case we get too far away from the outer points.
|
||||
// The m_scale for "midpoint outside the bounding box" may be bigger than the
|
||||
// scale to applie, so it results in an error.
|
||||
//get the closest point
|
||||
typedef typename Vec::iterator iter;
|
||||
for(iter it = m_points.begin(); it != m_points.end(); it++) {
|
||||
|
||||
const Eigen::Vector3d point = (*it)-midpoint;
|
||||
if(point.norm()<MINFAKTOR*scale) {
|
||||
|
||||
if(scale_dir(0)) {
|
||||
Scalar d = std::pow(point(1),2) + std::pow(point(2),2);
|
||||
Scalar h = std::sqrt(std::pow(MINFAKTOR*scale,2)-d);
|
||||
midpoint(0) += point(0) + h;
|
||||
} else if(scale_dir(1)) {
|
||||
Scalar d = std::pow(point(0),2) + std::pow(point(2),2);
|
||||
Scalar h = std::sqrt(std::pow(MINFAKTOR*scale,2)-d);
|
||||
midpoint(1) += point(1) + h;
|
||||
} else {
|
||||
Scalar d = std::pow(point(0),2) + std::pow(point(1),2);
|
||||
Scalar h = std::sqrt(std::pow(MINFAKTOR*scale,2)-d);
|
||||
midpoint(2) += point(2) + h;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
typename Kernel::Transform3D ssTrans;
|
||||
ssTrans = typename Kernel::Transform3D::Translation(-midpoint);
|
||||
ssTrans *= typename Kernel::Transform3D::Scaling(1./(scale*SKALEFAKTOR));
|
||||
|
||||
//recalculate all geometries
|
||||
typedef typename std::vector<Geom>::iterator iter;
|
||||
for(iter it = m_geometry.begin(); it != m_geometry.end(); it++)
|
||||
(*it)->transform(ssTrans);
|
||||
|
||||
//set the new rotation and translation
|
||||
m_transform = ssTrans.inverse()*m_transform;
|
||||
m_ssrTransform *= ssTrans;
|
||||
|
||||
transformToMaps(m_transform);
|
||||
|
||||
#ifdef USE_LOGGING
|
||||
BOOST_LOG(log) << "sstrans scale: "<<ssTrans.scaling().factor();
|
||||
BOOST_LOG(log) << "finish transform scale: "<<m_transform.scaling().factor();
|
||||
#endif
|
||||
};
|
||||
|
||||
template<typename Sys>
|
||||
typename ClusterMath<Sys>::Scalar ClusterMath<Sys>::calcOnePoint(const typename ClusterMath<Sys>::Kernel::Vector3& p) {
|
||||
|
||||
//one point can have every scale when moving the midpoint on the origin - point vector
|
||||
midpoint = p;
|
||||
scale_dir = -midpoint;
|
||||
scale_dir.normalize();
|
||||
mode = details::one;
|
||||
m_scale = 0.;
|
||||
return 0.;
|
||||
};
|
||||
|
||||
template<typename Sys>
|
||||
typename ClusterMath<Sys>::Scalar ClusterMath<Sys>::calcTwoPoints(const typename ClusterMath<Sys>::Kernel::Vector3& p1,
|
||||
const typename ClusterMath<Sys>::Kernel::Vector3& p2) {
|
||||
|
||||
//two points have their minimal scale at the mid position. Scaling perpendicular to this
|
||||
//line allows arbitrary scale values. Best is to have the scale dir move towards the origin
|
||||
//as good as possible.
|
||||
midpoint = p1+(p2-p1)/2.;
|
||||
scale_dir = (p2-p1).cross(midpoint);
|
||||
scale_dir = scale_dir.cross(p2-p1);
|
||||
if(!Kernel::isSame(scale_dir.norm(),0)) scale_dir.normalize();
|
||||
else scale_dir(0) = 1;
|
||||
mode = details::two;
|
||||
m_scale = (p2-p1).norm()/2.;
|
||||
return m_scale;
|
||||
};
|
||||
|
||||
template<typename Sys>
|
||||
typename ClusterMath<Sys>::Scalar ClusterMath<Sys>::calcThreePoints(const typename ClusterMath<Sys>::Kernel::Vector3& p1,
|
||||
const typename ClusterMath<Sys>::Kernel::Vector3& p2, const typename ClusterMath<Sys>::Kernel::Vector3& p3) {
|
||||
|
||||
//Three points form a triangle with it's minimal scale at the center of it's outer circle.
|
||||
//Arbitrary scale values can be achieved by moving perpendicular to the triangle plane.
|
||||
typename Kernel::Vector3 d = p2-p1;
|
||||
typename Kernel::Vector3 e = p3-p1;
|
||||
|
||||
typename Kernel::Vector3 f = p1+0.5*d;
|
||||
typename Kernel::Vector3 g = p1+0.5*e;
|
||||
scale_dir = d.cross(e);
|
||||
|
||||
typename Kernel::Matrix3 m;
|
||||
m.row(0) = d.transpose();
|
||||
m.row(1) = e.transpose();
|
||||
m.row(2) = scale_dir.transpose();
|
||||
|
||||
typename Kernel::Vector3 res(d.transpose()*f, e.transpose()*g, scale_dir.transpose()*p1);
|
||||
|
||||
midpoint = m.colPivHouseholderQr().solve(res);
|
||||
scale_dir.normalize();
|
||||
mode = details::three;
|
||||
m_scale = (midpoint-p1).norm();
|
||||
|
||||
return m_scale;
|
||||
|
||||
};
|
||||
|
||||
}//details
|
||||
}//dcm
|
||||
|
||||
|
||||
#endif //GCM_CLUSTERMATH_H
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
33
src/Mod/Assembly/App/opendcm/module3d/defines.hpp
Normal file
33
src/Mod/Assembly/App/opendcm/module3d/defines.hpp
Normal file
@@ -0,0 +1,33 @@
|
||||
/*
|
||||
openDCM, dimensional constraint manager
|
||||
Copyright (C) 2012 Stefan Troeger <stefantroeger@gmx.net>
|
||||
|
||||
This library is free software; you can redistribute it and/or modify
|
||||
it under the terms of the GNU Lesser General Public License as published by
|
||||
the Free Software Foundation; either version 2.1 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 Lesser General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU Lesser General Public License along
|
||||
with this library; if not, write to the Free Software Foundation, Inc.,
|
||||
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
|
||||
*/
|
||||
|
||||
#ifndef GCM_DEFINES_3D_H
|
||||
#define GCM_DEFINES_3D_H
|
||||
|
||||
namespace dcm {
|
||||
namespace details {
|
||||
|
||||
enum { cluster3D = 100};
|
||||
|
||||
struct m3d {}; //base of module3d::type to allow other modules check for it
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
292
src/Mod/Assembly/App/opendcm/module3d/distance.hpp
Normal file
292
src/Mod/Assembly/App/opendcm/module3d/distance.hpp
Normal file
@@ -0,0 +1,292 @@
|
||||
/*
|
||||
openDCM, dimensional constraint manager
|
||||
Copyright (C) 2012 Stefan Troeger <stefantroeger@gmx.net>
|
||||
|
||||
This library is free software; you can redistribute it and/or modify
|
||||
it under the terms of the GNU Lesser General Public License as published by
|
||||
the Free Software Foundation; either version 2.1 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 Lesser General Public License for more detemplate tails.
|
||||
|
||||
You should have received a copy of the GNU Lesser General Public License along
|
||||
with this library; if not, write to the Free Software Foundation, Inc.,
|
||||
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
|
||||
*/
|
||||
|
||||
#ifndef GCM_DISTANCE3D_H
|
||||
#define GCM_DISTANCE3D_H
|
||||
|
||||
#include "geometry.hpp"
|
||||
#include <opendcm/core/constraint.hpp>
|
||||
|
||||
namespace dcm {
|
||||
|
||||
template<typename Kernel>
|
||||
struct Distance::type< Kernel, tag::point3D, tag::point3D > {
|
||||
|
||||
typedef typename Kernel::number_type Scalar;
|
||||
typedef typename Kernel::VectorMap Vector;
|
||||
typedef std::vector<typename Kernel::Vector3, Eigen::aligned_allocator<typename Kernel::Vector3> > Vec;
|
||||
|
||||
Scalar value, sc_value;
|
||||
|
||||
//template definition
|
||||
void calculatePseudo(typename Kernel::Vector& param1, Vec& v1, typename Kernel::Vector& param2, Vec& v2) {};
|
||||
void setScale(Scalar scale) {
|
||||
sc_value = value*scale;
|
||||
};
|
||||
Scalar calculate(Vector& param1, Vector& param2) {
|
||||
return (param1-param2).norm() - sc_value;
|
||||
};
|
||||
Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) {
|
||||
return (param1-param2).dot(dparam1) / (param1-param2).norm();
|
||||
};
|
||||
Scalar calculateGradientSecond(Vector& param1, Vector& param2, Vector& dparam2) {
|
||||
return (param1-param2).dot(-dparam2) / (param1-param2).norm();
|
||||
};
|
||||
void calculateGradientFirstComplete(Vector& param1, Vector& param2, Vector& gradient) {
|
||||
gradient = (param1-param2) / (param1-param2).norm();
|
||||
};
|
||||
void calculateGradientSecondComplete(Vector& param1, Vector& param2, Vector& gradient) {
|
||||
gradient = (param2-param1) / (param1-param2).norm();
|
||||
};
|
||||
};
|
||||
|
||||
template<typename Kernel>
|
||||
struct Distance::type< Kernel, tag::point3D, tag::plane3D > {
|
||||
typedef typename Kernel::number_type Scalar;
|
||||
typedef typename Kernel::VectorMap Vector;
|
||||
typedef std::vector<typename Kernel::Vector3, Eigen::aligned_allocator<typename Kernel::Vector3> > Vec;
|
||||
|
||||
Scalar value, sc_value;
|
||||
|
||||
#ifdef USE_LOGGING
|
||||
src::logger log;
|
||||
attrs::mutable_constant< std::string > tag;
|
||||
|
||||
type() : tag("Distance point3D plane3D") {
|
||||
log.add_attribute("Tag", tag);
|
||||
};
|
||||
#endif
|
||||
|
||||
//template definition
|
||||
void calculatePseudo(typename Kernel::Vector& param1, Vec& v1, typename Kernel::Vector& param2, Vec& v2) {
|
||||
//typename Kernel::Vector3 pp = param1.head(3)- ((param1.head(3)-param2.head(3)).dot(param2.tail(3)) / param2.tail(3).norm()*(param2.tail(3)));
|
||||
//v2.push_back(pp);
|
||||
typename Kernel::Vector3 dir = (param1.template head<3>()-param2.template head<3>()).cross(param2.template segment<3>(3));
|
||||
dir = param2.template segment<3>(3).cross(dir).normalized();
|
||||
typename Kernel::Vector3 pp = param2.head(3) + (param1.head(3)-param2.head(3)).norm()*dir;
|
||||
v2.push_back(pp);
|
||||
#ifdef USE_LOGGING
|
||||
if(!boost::math::isnormal(pp.norm()))
|
||||
BOOST_LOG(log) << "Unnormal pseudopoint detected";
|
||||
#endif
|
||||
};
|
||||
void setScale(Scalar scale) {
|
||||
sc_value = value*scale;
|
||||
};
|
||||
Scalar calculate(Vector& param1, Vector& param2) {
|
||||
//(p1-p2)°n / |n| - distance
|
||||
const Scalar res = (param1.head(3)-param2.head(3)).dot(param2.tail(3)) / param2.tail(3).norm() - sc_value;
|
||||
#ifdef USE_LOGGING
|
||||
if(!boost::math::isfinite(res))
|
||||
BOOST_LOG(log) << "Unnormal residual detected: "<<res;
|
||||
#endif
|
||||
return res;
|
||||
};
|
||||
|
||||
Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) {
|
||||
//dp1°n / |n|
|
||||
//if(dparam1.norm()!=1) return 0;
|
||||
const Scalar res = (dparam1.head(3)).dot(param2.tail(3)) / param2.tail(3).norm();
|
||||
#ifdef USE_LOGGING
|
||||
if(!boost::math::isfinite(res))
|
||||
BOOST_LOG(log) << "Unnormal first cluster gradient detected: "<<res;
|
||||
#endif
|
||||
return res;
|
||||
};
|
||||
|
||||
Scalar calculateGradientSecond(Vector& param1, Vector& param2, Vector& dparam2) {
|
||||
|
||||
const typename Kernel::Vector3 p1 = param1.head(3);
|
||||
const typename Kernel::Vector3 p2 = param2.head(3);
|
||||
const typename Kernel::Vector3 dp2 = dparam2.head(3);
|
||||
const typename Kernel::Vector3 n = param2.tail(3);
|
||||
const typename Kernel::Vector3 dn = dparam2.tail(3);
|
||||
//if(dparam2.norm()!=1) return 0;
|
||||
const Scalar res = (((-dp2).dot(n) + (p1-p2).dot(dn)) / n.norm() - (p1-p2).dot(n)* n.dot(dn)/std::pow(n.norm(),3));
|
||||
#ifdef USE_LOGGING
|
||||
if(!boost::math::isfinite(res))
|
||||
BOOST_LOG(log) << "Unnormal second cluster gradient detected: "<<res;
|
||||
#endif
|
||||
return res;
|
||||
};
|
||||
|
||||
void calculateGradientFirstComplete(Vector& param1, Vector& param2, Vector& gradient) {
|
||||
gradient = param2.tail(3) / param2.tail(3).norm();
|
||||
};
|
||||
|
||||
void calculateGradientSecondComplete(Vector& param1, Vector& param2, Vector& gradient) {
|
||||
const typename Kernel::Vector3 p1m2 = param1.head(3) - param2.head(3);
|
||||
const typename Kernel::Vector3 n = param2.tail(3);
|
||||
|
||||
gradient.head(3) = -n / n.norm();
|
||||
gradient.tail(3) = (p1m2)/n.norm() - (p1m2).dot(n)*n/std::pow(n.norm(),3);
|
||||
};
|
||||
};
|
||||
|
||||
|
||||
template<typename Kernel>
|
||||
struct Distance::type< Kernel, tag::plane3D, tag::plane3D > : public Distance::type< Kernel, tag::point3D, tag::plane3D > {
|
||||
|
||||
#ifdef USE_LOGGING
|
||||
type() : Distance::type< Kernel, tag::point3D, tag::plane3D >() {
|
||||
Distance::type< Kernel, tag::point3D, tag::plane3D >::tag.set("Distance plane3D plane3D");
|
||||
};
|
||||
#endif
|
||||
typedef typename Kernel::VectorMap Vector;
|
||||
void calculateGradientFirstComplete(Vector& p1, Vector& p2, Vector& g) {
|
||||
Distance::type< Kernel, tag::point3D, tag::plane3D >::calculateGradientFirstComplete(p1,p2,g);
|
||||
g.segment(3,3).setZero();
|
||||
};
|
||||
};
|
||||
|
||||
template<typename Kernel>
|
||||
struct Distance::type< Kernel, tag::point3D, tag::line3D > {
|
||||
|
||||
typedef typename Kernel::number_type Scalar;
|
||||
typedef typename Kernel::VectorMap Vector;
|
||||
typedef typename Kernel::Vector3 Vector3;
|
||||
typedef std::vector<typename Kernel::Vector3, Eigen::aligned_allocator<typename Kernel::Vector3> > Vec;
|
||||
|
||||
Scalar value, sc_value;
|
||||
Vector3 diff, n, dist;
|
||||
|
||||
#ifdef USE_LOGGING
|
||||
src::logger log;
|
||||
attrs::mutable_constant< std::string > tag;
|
||||
|
||||
type() : tag("Distance point3D line3D") {
|
||||
log.add_attribute("Tag", tag);
|
||||
};
|
||||
#endif
|
||||
|
||||
//template definition
|
||||
void calculatePseudo(typename Kernel::Vector& point, Vec& v1, typename Kernel::Vector& line, Vec& v2) {
|
||||
Vector3 pp = line.head(3) + (line.head(3)-point.head(3)).norm()*line.segment(3,3);
|
||||
#ifdef USE_LOGGING
|
||||
if(!boost::math::isnormal(pp.norm()))
|
||||
BOOST_LOG(log) << "Unnormal pseudopoint detected";
|
||||
#endif
|
||||
v2.push_back(pp);
|
||||
};
|
||||
void setScale(Scalar scale) {
|
||||
sc_value = value*scale;
|
||||
};
|
||||
Scalar calculate(Vector& point, Vector& line) {
|
||||
//diff = point1 - point2
|
||||
n = line.template segment<3>(3);
|
||||
diff = line.template head<3>() - point.template head<3>();
|
||||
dist = diff - diff.dot(n)*n;
|
||||
const Scalar res = dist.norm() - sc_value;
|
||||
#ifdef USE_LOGGING
|
||||
if(!boost::math::isfinite(res))
|
||||
BOOST_LOG(log) << "Unnormal residual detected: "<<res;
|
||||
#endif
|
||||
return res;
|
||||
};
|
||||
|
||||
Scalar calculateGradientFirst(Vector& point, Vector& line, Vector& dpoint) {
|
||||
if(dist.norm() == 0)
|
||||
return 1.;
|
||||
|
||||
const Vector3 d_diff = -dpoint.template head<3>();
|
||||
const Vector3 d_dist = d_diff - d_diff.dot(n)*n;
|
||||
const Scalar res = dist.dot(d_dist)/dist.norm();
|
||||
#ifdef USE_LOGGING
|
||||
if(!boost::math::isfinite(res))
|
||||
BOOST_LOG(log) << "Unnormal first cluster gradient detected: "<<res
|
||||
<<" with point: "<<point.transpose()<<", line: "<<line.transpose()
|
||||
<<" and dpoint: "<<dpoint.transpose();
|
||||
#endif
|
||||
return res;
|
||||
};
|
||||
|
||||
Scalar calculateGradientSecond(Vector& point, Vector& line, Vector& dline) {
|
||||
if(dist.norm() == 0)
|
||||
return 1.;
|
||||
|
||||
const Vector3 d_diff = dline.template head<3>();
|
||||
const Vector3 d_n = dline.template segment<3>(3);
|
||||
const Vector3 d_dist = d_diff - ((d_diff.dot(n)+diff.dot(d_n))*n + diff.dot(n)*d_n);
|
||||
const Scalar res = dist.dot(d_dist)/dist.norm();
|
||||
#ifdef USE_LOGGING
|
||||
if(!boost::math::isfinite(res))
|
||||
BOOST_LOG(log) << "Unnormal second cluster gradient detected: "<<res
|
||||
<<" with point: "<<point.transpose()<<", line: "<<line.transpose()
|
||||
<< "and dline: "<<dline.transpose();
|
||||
#endif
|
||||
return res;
|
||||
};
|
||||
|
||||
void calculateGradientFirstComplete(Vector& point, Vector& line, Vector& gradient) {
|
||||
if(dist.norm() == 0) {
|
||||
gradient.head(3).setOnes();
|
||||
return;
|
||||
}
|
||||
|
||||
const Vector3 res = (n*n.transpose())*dist - dist;
|
||||
gradient.head(3) = res/dist.norm();
|
||||
};
|
||||
|
||||
void calculateGradientSecondComplete(Vector& point, Vector& line, Vector& gradient) {
|
||||
if(dist.norm() == 0) {
|
||||
gradient.head(6).setOnes();
|
||||
return;
|
||||
}
|
||||
|
||||
const Vector3 res = (-n*n.transpose())*dist + dist;
|
||||
gradient.head(3) = res/dist.norm();
|
||||
|
||||
const Scalar mult = n.transpose()*dist;
|
||||
gradient.template segment<3>(3) = -(mult*diff + diff.dot(n)*dist)/dist.norm();
|
||||
};
|
||||
};
|
||||
|
||||
template<typename Kernel>
|
||||
struct Distance::type< Kernel, tag::line3D, tag::line3D > : public Distance::type< Kernel, tag::point3D, tag::line3D > {
|
||||
|
||||
#ifdef USE_LOGGING
|
||||
type() : Distance::type< Kernel, tag::point3D, tag::line3D >() {
|
||||
Distance::type< Kernel, tag::point3D, tag::line3D >::tag.set("Distance line3D line3D");
|
||||
};
|
||||
#endif
|
||||
typedef typename Kernel::VectorMap Vector;
|
||||
void calculateGradientFirstComplete(Vector& p1, Vector& p2, Vector& g) {
|
||||
Distance::type< Kernel, tag::point3D, tag::line3D >::calculateGradientFirstComplete(p1,p2,g);
|
||||
g.segment(3,3).setZero();
|
||||
};
|
||||
};
|
||||
|
||||
template<typename Kernel>
|
||||
struct Distance::type< Kernel, tag::cylinder3D, tag::cylinder3D > : public Distance::type< Kernel, tag::line3D, tag::line3D > {
|
||||
|
||||
#ifdef USE_LOGGING
|
||||
type() : Distance::type< Kernel, tag::line3D, tag::line3D >() {
|
||||
Distance::type< Kernel, tag::line3D, tag::line3D >::tag.set("Distance cylinder3D cylinder3D");
|
||||
};
|
||||
#endif
|
||||
typedef typename Kernel::VectorMap Vector;
|
||||
void calculateGradientFirstComplete(Vector& p1, Vector& p2, Vector& g) {
|
||||
Distance::type< Kernel, tag::line3D, tag::line3D >::calculateGradientFirstComplete(p1,p2,g);
|
||||
g(6) = 0;
|
||||
};
|
||||
};
|
||||
|
||||
}//namespace dcm
|
||||
|
||||
#endif //GCM_DISTANCE3D_H
|
||||
133
src/Mod/Assembly/App/opendcm/module3d/dof.hpp
Normal file
133
src/Mod/Assembly/App/opendcm/module3d/dof.hpp
Normal file
@@ -0,0 +1,133 @@
|
||||
/*
|
||||
openDCM, dimensional constraint manager
|
||||
Copyright (C) 2012 Stefan Troeger <stefantroeger@gmx.net>
|
||||
|
||||
This library is free software; you can redistribute it and/or modify
|
||||
it under the terms of the GNU Lesser General Public License as published by
|
||||
the Free Software Foundation; either version 2.1 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 Lesser General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU Lesser General Public License along
|
||||
with this library; if not, write to the Free Software Foundation, Inc.,
|
||||
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
|
||||
*/
|
||||
|
||||
#ifndef GCM_DOF_H
|
||||
#define GCM_DOF_H
|
||||
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
namespace dcm {
|
||||
|
||||
enum remaining {
|
||||
nothing = 0,
|
||||
line,
|
||||
plane,
|
||||
volume
|
||||
};
|
||||
|
||||
template<typename K, typename C>
|
||||
class Dof {
|
||||
|
||||
typedef typename K::Vector3 Vec;
|
||||
typedef std::pair<Vec, C> VecID;
|
||||
|
||||
public:
|
||||
typedef std::vector<C> ConstraintVector;
|
||||
typedef std::pair<bool, ConstraintVector> Result;
|
||||
|
||||
Dof() : m_translation(volume), m_rotation(volume) {};
|
||||
|
||||
int dofTranslational() {
|
||||
return m_translation;
|
||||
};
|
||||
int dofRotational() {
|
||||
return m_rotation;
|
||||
};
|
||||
int dof() {
|
||||
return dofTranslational() + dofRotational();
|
||||
};
|
||||
|
||||
|
||||
Result removeTranslationDirection(Vec& v, C constraint) {
|
||||
|
||||
if(m_translation == nothing) {
|
||||
ConstraintVector cv;
|
||||
cv.push_back(tp1.second);
|
||||
cv.push_back(tp2.second);
|
||||
cv.push_back(tp3.second);
|
||||
return std::make_pair(false,cv);
|
||||
} else if(m_translation == volume) {
|
||||
|
||||
m_translation = plane;
|
||||
tp1 = std::make_pair(v, constraint);
|
||||
} else if(m_translation == plane) {
|
||||
|
||||
if(K::isSame(tp1.first, v) || K::isOpposite(tp1.first, v)) {
|
||||
ConstraintVector cv;
|
||||
cv.push_back(tp1.second);
|
||||
return std::make_pair(false,cv);
|
||||
}
|
||||
m_translation = line;
|
||||
tp2 = std::make_pair(v, constraint);
|
||||
} else if(m_translation == line) {
|
||||
|
||||
if(tp1.first.cross(tp2.first).dot(v) < 0.001) {
|
||||
ConstraintVector cv;
|
||||
cv.push_back(tp1.second);
|
||||
cv.push_back(tp2.second);
|
||||
return std::make_pair(false,cv);
|
||||
}
|
||||
m_translation = nothing;
|
||||
tp3 = std::make_pair(v, constraint);
|
||||
}
|
||||
|
||||
return std::make_pair(true, ConstraintVector());
|
||||
};
|
||||
|
||||
Result allowOnlyRotationDirection(Vec& v, C constraint) {
|
||||
|
||||
if(m_rotation == nothing) {
|
||||
ConstraintVector cv;
|
||||
cv.push_back(rp1.second);
|
||||
cv.push_back(rp2.second);
|
||||
return std::make_pair(false, cv);
|
||||
} else if(m_rotation == volume) {
|
||||
|
||||
m_rotation = line;
|
||||
rp1 = std::make_pair(v, constraint);
|
||||
} else if(m_rotation == plane) {
|
||||
|
||||
return std::make_pair(false, ConstraintVector()); //error as every function call removes 2 dof's
|
||||
} else if(m_rotation == line) {
|
||||
|
||||
if(K::isSame(rp1.first, v) || K::isOpposite(rp1.first, v)) {
|
||||
ConstraintVector cv;
|
||||
cv.push_back(rp1.second);
|
||||
return std::make_pair(false, cv);
|
||||
}
|
||||
m_rotation = nothing;
|
||||
rp2 = std::make_pair(v, constraint);
|
||||
}
|
||||
|
||||
return std::make_pair(true, ConstraintVector());
|
||||
};
|
||||
|
||||
|
||||
private:
|
||||
int m_translation, m_rotation;
|
||||
VecID tp1,tp2,tp3; //translation pairs
|
||||
VecID rp1, rp2; //rotation pairs
|
||||
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif //GCM_DOF_H
|
||||
160
src/Mod/Assembly/App/opendcm/module3d/geometry.hpp
Normal file
160
src/Mod/Assembly/App/opendcm/module3d/geometry.hpp
Normal file
@@ -0,0 +1,160 @@
|
||||
/*
|
||||
openDCM, dimensional constraint manager
|
||||
Copyright (C) 2012 Stefan Troeger <stefantroeger@gmx.net>
|
||||
|
||||
This library is free software; you can redistribute it and/or modify
|
||||
it under the terms of the GNU Lesser General Public License as published by
|
||||
the Free Software Foundation; either version 2.1 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 Lesser General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU Lesser General Public License along
|
||||
with this library; if not, write to the Free Software Foundation, Inc.,
|
||||
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
|
||||
*/
|
||||
|
||||
#ifndef GCM_GEOMETRY_3D_H
|
||||
#define GCM_GEOMETRY_3D_H
|
||||
|
||||
#include <opendcm/core/geometry.hpp>
|
||||
|
||||
namespace dcm {
|
||||
namespace tag {
|
||||
|
||||
struct point3D {
|
||||
typedef mpl::int_<3> parameters;
|
||||
typedef mpl::int_<1> rotations;
|
||||
typedef mpl::int_<1> translations;
|
||||
typedef weight::point weight;
|
||||
};
|
||||
|
||||
struct direction3D {
|
||||
typedef mpl::int_<3> parameters;
|
||||
typedef mpl::int_<1> rotations;
|
||||
typedef mpl::int_<0> translations;
|
||||
typedef weight::direction weight;
|
||||
};
|
||||
|
||||
struct line3D {
|
||||
typedef mpl::int_<6> parameters;
|
||||
typedef mpl::int_<2> rotations;
|
||||
typedef mpl::int_<1> translations;
|
||||
typedef weight::line weight;
|
||||
};
|
||||
|
||||
struct plane3D {
|
||||
typedef mpl::int_<6> parameters;
|
||||
typedef mpl::int_<2> rotations;
|
||||
typedef mpl::int_<1> translations;
|
||||
typedef weight::plane weight;
|
||||
};
|
||||
|
||||
struct cylinder3D {
|
||||
typedef mpl::int_<7> parameters;
|
||||
typedef mpl::int_<2> rotations;
|
||||
typedef mpl::int_<1> translations;
|
||||
typedef weight::cylinder weight;
|
||||
};
|
||||
}
|
||||
|
||||
namespace modell {
|
||||
|
||||
struct XYZ {
|
||||
/*Modell XYZ:
|
||||
* 0 = X;
|
||||
* 1 = Y;
|
||||
* 2 = Z;
|
||||
*/
|
||||
template<typename Scalar, typename Accessor, typename Vector, typename Type>
|
||||
void extract(Type& t, Vector& v) {
|
||||
Accessor a;
|
||||
v(0) = a.template get<Scalar, 0>(t);
|
||||
v(1) = a.template get<Scalar, 1>(t);
|
||||
v(2) = a.template get<Scalar, 2>(t);
|
||||
}
|
||||
|
||||
template<typename Scalar, typename Accessor, typename Vector, typename Type>
|
||||
void inject(Type& t, Vector& v) {
|
||||
Accessor a;
|
||||
a.template set<Scalar, 0>(v(0), t);
|
||||
a.template set<Scalar, 1>(v(1), t);
|
||||
a.template set<Scalar, 2>(v(2), t);
|
||||
};
|
||||
};
|
||||
|
||||
struct XYZ2 {
|
||||
/*Modell XYZ2: two xyz parts after each other
|
||||
* 0 = X;
|
||||
* 1 = Y;
|
||||
* 2 = Z;
|
||||
* 3 = X dir;
|
||||
* 4 = Y dir;
|
||||
* 5 = Z dir;
|
||||
*/
|
||||
template<typename Scalar, typename Accessor, typename Vector, typename Type>
|
||||
void extract(Type& t, Vector& v) {
|
||||
Accessor a;
|
||||
v(0) = a.template get<Scalar, 0>(t);
|
||||
v(1) = a.template get<Scalar, 1>(t);
|
||||
v(2) = a.template get<Scalar, 2>(t);
|
||||
v(3) = a.template get<Scalar, 3>(t);
|
||||
v(4) = a.template get<Scalar, 4>(t);
|
||||
v(5) = a.template get<Scalar, 5>(t);
|
||||
}
|
||||
|
||||
template<typename Scalar, typename Accessor, typename Vector, typename Type>
|
||||
void inject(Type& t, Vector& v) {
|
||||
Accessor a;
|
||||
a.template set<Scalar, 0>(v(0), t);
|
||||
a.template set<Scalar, 1>(v(1), t);
|
||||
a.template set<Scalar, 2>(v(2), t);
|
||||
a.template set<Scalar, 3>(v(3), t);
|
||||
a.template set<Scalar, 4>(v(4), t);
|
||||
a.template set<Scalar, 5>(v(5), t);
|
||||
};
|
||||
};
|
||||
|
||||
struct XYZ2P {
|
||||
/*Modell XYZ2P: two xyz parts after each other and one parameter
|
||||
* 0 = X;
|
||||
* 1 = Y;
|
||||
* 2 = Z;
|
||||
* 3 = X dir;
|
||||
* 4 = Y dir;
|
||||
* 5 = Z dir;
|
||||
* 6 = Parameter
|
||||
*/
|
||||
template<typename Scalar, typename Accessor, typename Vector, typename Type>
|
||||
void extract(Type& t, Vector& v) {
|
||||
Accessor a;
|
||||
v(0) = a.template get<Scalar, 0>(t);
|
||||
v(1) = a.template get<Scalar, 1>(t);
|
||||
v(2) = a.template get<Scalar, 2>(t);
|
||||
v(3) = a.template get<Scalar, 3>(t);
|
||||
v(4) = a.template get<Scalar, 4>(t);
|
||||
v(5) = a.template get<Scalar, 5>(t);
|
||||
v(6) = a.template get<Scalar, 6>(t);
|
||||
}
|
||||
|
||||
template<typename Scalar, typename Accessor, typename Vector, typename Type>
|
||||
void inject(Type& t, Vector& v) {
|
||||
Accessor a;
|
||||
a.template set<Scalar, 0>(v(0), t);
|
||||
a.template set<Scalar, 1>(v(1), t);
|
||||
a.template set<Scalar, 2>(v(2), t);
|
||||
a.template set<Scalar, 3>(v(3), t);
|
||||
a.template set<Scalar, 4>(v(4), t);
|
||||
a.template set<Scalar, 5>(v(5), t);
|
||||
a.template set<Scalar, 6>(v(6), t);
|
||||
};
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif //GCM_GEOMETRY_3D_H
|
||||
546
src/Mod/Assembly/App/opendcm/module3d/module.hpp
Normal file
546
src/Mod/Assembly/App/opendcm/module3d/module.hpp
Normal file
@@ -0,0 +1,546 @@
|
||||
/*
|
||||
openDCM, dimensional constraint manager
|
||||
Copyright (C) 2012 Stefan Troeger <stefantroeger@gmx.net>
|
||||
|
||||
This library is free software; you can redistribute it and/or modify
|
||||
it under the terms of the GNU Lesser General Public License as published by
|
||||
the Free Software Foundation; either version 2.1 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 Lesser General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU Lesser General Public License along
|
||||
with this library; if not, write to the Free Software Foundation, Inc.,
|
||||
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
|
||||
*/
|
||||
|
||||
#ifndef GCM_MODULE_3D_H
|
||||
#define GCM_MODULE_3D_H
|
||||
|
||||
#include <boost/mpl/vector.hpp>
|
||||
#include <boost/mpl/map.hpp>
|
||||
#include <boost/mpl/less.hpp>
|
||||
#include <boost/mpl/if.hpp>
|
||||
#include <boost/mpl/size.hpp>
|
||||
|
||||
#include <boost/fusion/include/as_vector.hpp>
|
||||
#include <boost/fusion/include/mpl.hpp>
|
||||
#include <boost/fusion/include/at.hpp>
|
||||
|
||||
#include <boost/static_assert.hpp>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/preprocessor/iteration/local.hpp>
|
||||
#include <boost/variant.hpp>
|
||||
|
||||
#include <boost/function.hpp>
|
||||
#include <boost/bind.hpp>
|
||||
|
||||
#include "opendcm/core.hpp"
|
||||
#include "opendcm/core/object.hpp"
|
||||
#include "opendcm/core/clustergraph.hpp"
|
||||
#include "opendcm/core/sheduler.hpp"
|
||||
#include "opendcm/core/traits.hpp"
|
||||
#include "opendcm/core/geometry.hpp"
|
||||
#include "geometry.hpp"
|
||||
#include "distance.hpp"
|
||||
#include "parallel.hpp"
|
||||
#include "angle.hpp"
|
||||
#include "solver.hpp"
|
||||
#include "defines.hpp"
|
||||
#include "clustermath.hpp"
|
||||
|
||||
namespace mpl = boost::mpl;
|
||||
|
||||
namespace dcm {
|
||||
|
||||
namespace details {
|
||||
|
||||
template<typename seq, typename t>
|
||||
struct distance {
|
||||
typedef typename mpl::find<seq, t>::type iterator;
|
||||
typedef typename mpl::distance<typename mpl::begin<seq>::type, iterator>::type type;
|
||||
BOOST_MPL_ASSERT((mpl::not_< boost::is_same<iterator, typename mpl::end<seq>::type > >));
|
||||
};
|
||||
}
|
||||
}//dcm
|
||||
|
||||
namespace dcm {
|
||||
|
||||
template<typename Typelist, typename ID = No_Identifier>
|
||||
struct Module3D {
|
||||
|
||||
template<typename Sys>
|
||||
struct type : details::m3d {
|
||||
struct Constraint3D;
|
||||
struct Geometry3D;
|
||||
struct vertex_prop;
|
||||
struct inheriter_base;
|
||||
|
||||
typedef boost::shared_ptr<Geometry3D> Geom;
|
||||
typedef boost::shared_ptr<Constraint3D> Cons;
|
||||
|
||||
typedef mpl::map1< mpl::pair<remove, boost::function<void (Cons) > > > ConsSignal;
|
||||
|
||||
typedef ID Identifier;
|
||||
|
||||
typedef details::MES<Sys> MES;
|
||||
typedef details::SystemSolver<Sys> SystemSolver;
|
||||
|
||||
template<typename Derived>
|
||||
class Geometry3D_id : public detail::Geometry<Sys, Derived, Typelist, 3> {
|
||||
|
||||
typedef detail::Geometry<Sys, Derived, Typelist, 3> Base;
|
||||
|
||||
#ifdef USE_LOGGING
|
||||
attrs::mutable_constant< std::string > log_id;
|
||||
#endif
|
||||
public:
|
||||
template<typename T>
|
||||
Geometry3D_id(T geometry, Sys& system);
|
||||
|
||||
template<typename T>
|
||||
void set(T geometry, Identifier id);
|
||||
//somehow the base class set funtion is not found
|
||||
template<typename T>
|
||||
void set(T geometry);
|
||||
|
||||
Identifier& getIdentifier();
|
||||
void setIdentifier(Identifier id);
|
||||
};
|
||||
|
||||
struct Geometry3D : public mpl::if_<boost::is_same<Identifier, No_Identifier>,
|
||||
detail::Geometry<Sys, Geometry3D, Typelist, 3>, Geometry3D_id<Geometry3D> >::type {
|
||||
|
||||
typedef vertex_prop vertex_propertie;
|
||||
|
||||
template<typename T>
|
||||
Geometry3D(T geometry, Sys& system);
|
||||
|
||||
//allow accessing the internals by module3d classes but not by users
|
||||
friend struct details::ClusterMath<Sys>;
|
||||
friend struct details::ClusterMath<Sys>::map_downstream;
|
||||
friend struct details::SystemSolver<Sys>;
|
||||
friend struct details::SystemSolver<Sys>::Rescaler;
|
||||
friend class detail::Constraint<Sys, Constraint3D, ConsSignal, MES, Geometry3D>;
|
||||
};
|
||||
|
||||
template<typename Derived>
|
||||
class Constraint3D_id : public detail::Constraint<Sys, Derived, ConsSignal, MES, Geometry3D> {
|
||||
|
||||
typedef detail::Constraint<Sys, Derived, ConsSignal, MES, Geometry3D> base;
|
||||
public:
|
||||
Constraint3D_id(Sys& system, Geom f, Geom s);
|
||||
|
||||
Identifier& getIdentifier();
|
||||
void setIdentifier(Identifier id);
|
||||
};
|
||||
|
||||
struct Constraint3D : public mpl::if_<boost::is_same<Identifier, No_Identifier>,
|
||||
detail::Constraint<Sys, Constraint3D, ConsSignal, MES, Geometry3D>,
|
||||
Constraint3D_id<Constraint3D> >::type {
|
||||
|
||||
Constraint3D(Sys& system, Geom first, Geom second);
|
||||
|
||||
friend struct details::SystemSolver<Sys>;
|
||||
friend struct details::SystemSolver<Sys>::Rescaler;
|
||||
friend struct details::MES<Sys>;
|
||||
friend struct inheriter_base;
|
||||
};
|
||||
|
||||
struct inheriter_base {
|
||||
|
||||
inheriter_base();
|
||||
|
||||
template<typename T>
|
||||
Geom createGeometry3D(T geom);
|
||||
void removeGeometry3D(Geom g);
|
||||
|
||||
template<typename T1>
|
||||
Cons createConstraint3D(Geom first, Geom second, T1 constraint1);
|
||||
void removeConstraint3D(Cons c);
|
||||
|
||||
protected:
|
||||
Sys* m_this;
|
||||
void apply_edge_remove(GlobalEdge e);
|
||||
};
|
||||
|
||||
struct inheriter_id : public inheriter_base {
|
||||
|
||||
protected:
|
||||
using inheriter_base::m_this;
|
||||
|
||||
public:
|
||||
template<typename T>
|
||||
Geom createGeometry3D(T geom, Identifier id);
|
||||
template<typename T>
|
||||
Cons createConstraint3D(Identifier id, Geom first, Geom second, T constraint1);
|
||||
|
||||
void removeGeometry3D(Identifier id);
|
||||
void removeConstraint3D(Identifier id);
|
||||
|
||||
bool hasGeometry3D(Identifier id);
|
||||
Geom getGeometry3D(Identifier id);
|
||||
bool hasConstraint3D(Identifier id);
|
||||
Cons getConstraint3D(Identifier id);
|
||||
};
|
||||
|
||||
struct inheriter : public mpl::if_<boost::is_same<Identifier, No_Identifier>, inheriter_base, inheriter_id>::type {};
|
||||
|
||||
struct math_prop {
|
||||
typedef cluster_property kind;
|
||||
typedef details::ClusterMath<Sys> type;
|
||||
};
|
||||
struct fix_prop {
|
||||
typedef cluster_property kind;
|
||||
typedef bool type;
|
||||
};
|
||||
struct vertex_prop {
|
||||
typedef Geometry3D kind;
|
||||
typedef GlobalVertex type;
|
||||
};
|
||||
struct edge_prop {
|
||||
typedef Constraint3D kind;
|
||||
typedef GlobalEdge type;
|
||||
};
|
||||
|
||||
typedef mpl::vector4<vertex_prop, edge_prop, math_prop, fix_prop> properties;
|
||||
typedef mpl::vector<Geometry3D, Constraint3D> objects;
|
||||
|
||||
static void system_init(Sys& sys) {
|
||||
sys.m_sheduler.addProcessJob(new SystemSolver());
|
||||
};
|
||||
static void system_copy(const Sys& from, Sys& into) {
|
||||
//nothing to to as all objects and properties are copyed with the clustergraph
|
||||
};
|
||||
};
|
||||
};
|
||||
|
||||
namespace details {
|
||||
//allow direct access to the stored geometry in a Geometry3D, copyed from boost variant get
|
||||
template <typename T>
|
||||
struct get_visitor {
|
||||
private:
|
||||
|
||||
typedef typename boost::add_pointer<T>::type pointer;
|
||||
typedef typename boost::add_reference<T>::type reference;
|
||||
|
||||
public:
|
||||
|
||||
typedef pointer result_type;
|
||||
|
||||
public:
|
||||
pointer operator()(reference operand) const {
|
||||
return boost::addressof(operand);
|
||||
}
|
||||
|
||||
template <typename U>
|
||||
pointer operator()(const U&) const {
|
||||
return static_cast<pointer>(0);
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
template<typename T, typename G>
|
||||
typename boost::add_reference<T>::type get(G geom) {
|
||||
|
||||
typedef typename boost::add_pointer<T>::type T_ptr;
|
||||
details::get_visitor<T> v;
|
||||
T_ptr result = geom->apply(v);
|
||||
|
||||
//if (!result)
|
||||
//TODO:throw bad_get();
|
||||
return *result;
|
||||
};
|
||||
|
||||
|
||||
/*****************************************************************************************************************/
|
||||
/*****************************************************************************************************************/
|
||||
/*****************************************************************************************************************/
|
||||
/*****************************************************************************************************************/
|
||||
|
||||
|
||||
|
||||
template<typename Typelist, typename ID>
|
||||
template<typename Sys>
|
||||
template<typename Derived>
|
||||
template<typename T>
|
||||
Module3D<Typelist, ID>::type<Sys>::Geometry3D_id<Derived>::Geometry3D_id(T geometry, Sys& system)
|
||||
: detail::Geometry<Sys, Derived, Typelist, 3>(geometry, system)
|
||||
#ifdef USE_LOGGING
|
||||
, log_id("No ID")
|
||||
#endif
|
||||
{
|
||||
|
||||
#ifdef USE_LOGGING
|
||||
Base::log.add_attribute("ID", log_id);
|
||||
#endif
|
||||
};
|
||||
|
||||
template<typename Typelist, typename ID>
|
||||
template<typename Sys>
|
||||
template<typename Derived>
|
||||
template<typename T>
|
||||
void Module3D<Typelist, ID>::type<Sys>::Geometry3D_id<Derived>::set(T geometry, Identifier id) {
|
||||
this->template setProperty<id_prop<Identifier> >(id);
|
||||
Base::set(geometry);
|
||||
};
|
||||
|
||||
template<typename Typelist, typename ID>
|
||||
template<typename Sys>
|
||||
template<typename Derived>
|
||||
template<typename T>
|
||||
void Module3D<Typelist, ID>::type<Sys>::Geometry3D_id<Derived>::set(T geometry) {
|
||||
Base::set(geometry);
|
||||
};
|
||||
|
||||
template<typename Typelist, typename ID>
|
||||
template<typename Sys>
|
||||
template<typename Derived>
|
||||
typename Module3D<Typelist, ID>::template type<Sys>::Identifier&
|
||||
Module3D<Typelist, ID>::type<Sys>::Geometry3D_id<Derived>::getIdentifier() {
|
||||
return this->template getProperty<id_prop<Identifier> >();
|
||||
};
|
||||
|
||||
template<typename Typelist, typename ID>
|
||||
template<typename Sys>
|
||||
template<typename Derived>
|
||||
void Module3D<Typelist, ID>::type<Sys>::Geometry3D_id<Derived>::setIdentifier(Identifier id) {
|
||||
this->template setProperty<id_prop<Identifier> >(id);
|
||||
#ifdef USE_LOGGING
|
||||
std::stringstream str;
|
||||
str<<this->template getProperty<id_prop<Identifier> >();
|
||||
log_id.set(str.str());
|
||||
BOOST_LOG(Base::log)<<"Identifyer set: "<<id;
|
||||
#endif
|
||||
};
|
||||
|
||||
template<typename Typelist, typename ID>
|
||||
template<typename Sys>
|
||||
template<typename T>
|
||||
Module3D<Typelist, ID>::type<Sys>::Geometry3D::Geometry3D(T geometry, Sys& system)
|
||||
: mpl::if_<boost::is_same<Identifier, No_Identifier>,
|
||||
detail::Geometry<Sys, Geometry3D, Typelist, 3>,
|
||||
Geometry3D_id<Geometry3D> >::type(geometry, system) {
|
||||
|
||||
};
|
||||
|
||||
template<typename Typelist, typename ID>
|
||||
template<typename Sys>
|
||||
template<typename Derived>
|
||||
Module3D<Typelist, ID>::type<Sys>::Constraint3D_id<Derived>::Constraint3D_id(Sys& system, Geom f, Geom s)
|
||||
: detail::Constraint<Sys, Derived, ConsSignal, MES, Geometry3D>(system, f, s) {
|
||||
|
||||
};
|
||||
|
||||
template<typename Typelist, typename ID>
|
||||
template<typename Sys>
|
||||
template<typename Derived>
|
||||
typename Module3D<Typelist, ID>::template type<Sys>::Identifier&
|
||||
Module3D<Typelist, ID>::type<Sys>::Constraint3D_id<Derived>::getIdentifier() {
|
||||
return this->template getProperty<id_prop<Identifier> >();
|
||||
};
|
||||
|
||||
template<typename Typelist, typename ID>
|
||||
template<typename Sys>
|
||||
template<typename Derived>
|
||||
void Module3D<Typelist, ID>::type<Sys>::Constraint3D_id<Derived>::setIdentifier(Identifier id) {
|
||||
this->template setProperty<id_prop<Identifier> >(id);
|
||||
};
|
||||
|
||||
template<typename Typelist, typename ID>
|
||||
template<typename Sys>
|
||||
Module3D<Typelist, ID>::type<Sys>::Constraint3D::Constraint3D(Sys& system, Geom first, Geom second)
|
||||
: mpl::if_<boost::is_same<Identifier, No_Identifier>,
|
||||
detail::Constraint<Sys, Constraint3D, ConsSignal, MES, Geometry3D>,
|
||||
Constraint3D_id<Constraint3D> >::type(system, first, second) {
|
||||
|
||||
};
|
||||
|
||||
template<typename Typelist, typename ID>
|
||||
template<typename Sys>
|
||||
Module3D<Typelist, ID>::type<Sys>::inheriter_base::inheriter_base() {
|
||||
m_this = ((Sys*) this);
|
||||
};
|
||||
|
||||
template<typename Typelist, typename ID>
|
||||
template<typename Sys>
|
||||
template<typename T>
|
||||
typename Module3D<Typelist, ID>::template type<Sys>::Geom
|
||||
Module3D<Typelist, ID>::type<Sys>::inheriter_base::createGeometry3D(T geom) {
|
||||
|
||||
Geom g(new Geometry3D(geom, * ((Sys*) this)));
|
||||
fusion::vector<LocalVertex, GlobalVertex> res = m_this->m_cluster->addVertex();
|
||||
m_this->m_cluster->template setObject<Geometry3D> (fusion::at_c<0> (res), g);
|
||||
g->template setProperty<vertex_prop>(fusion::at_c<1>(res));
|
||||
m_this->push_back(g);
|
||||
return g;
|
||||
};
|
||||
|
||||
template<typename Typelist, typename ID>
|
||||
template<typename Sys>
|
||||
void Module3D<Typelist, ID>::type<Sys>::inheriter_base::removeGeometry3D(Geom g) {
|
||||
|
||||
GlobalVertex v = g->template getProperty<vertex_prop>();
|
||||
|
||||
//check if this vertex holds a constraint
|
||||
Cons c = m_this->m_cluster->template getObject<Constraint3D>(v);
|
||||
if(c)
|
||||
c->template emitSignal<remove>(c);
|
||||
|
||||
//emit remove geometry signal bevore actually deleting it, in case anyone want to access the
|
||||
//graph before
|
||||
g->template emitSignal<remove>(g);
|
||||
|
||||
//remove the vertex from graph and emit all edges that get removed with the functor
|
||||
boost::function<void(GlobalEdge)> functor = boost::bind(&inheriter_base::apply_edge_remove, this, _1);
|
||||
m_this->m_cluster->removeVertex(v, functor);
|
||||
m_this->erase(g);
|
||||
};
|
||||
|
||||
template<typename Typelist, typename ID>
|
||||
template<typename Sys>
|
||||
template<typename T1>
|
||||
typename Module3D<Typelist, ID>::template type<Sys>::Cons
|
||||
Module3D<Typelist, ID>::type<Sys>::inheriter_base::createConstraint3D(Geom first, Geom second, T1 constraint1) {
|
||||
|
||||
//build a constraint vector
|
||||
typedef mpl::vector<> cvec;
|
||||
typedef typename mpl::if_< mpl::is_sequence<T1>,
|
||||
typename mpl::fold< T1, cvec, mpl::push_back<mpl::_1,mpl::_2> >::type,
|
||||
mpl::vector1<T1> >::type cvec1;
|
||||
|
||||
//make a fusion sequence to hold the objects (as they hold the options)
|
||||
typedef typename fusion::result_of::as_vector<cvec1>::type covec;
|
||||
//set the objects
|
||||
covec cv;
|
||||
fusion::at_c<0>(cv) = constraint1;
|
||||
|
||||
//now create the constraint
|
||||
Cons c(new Constraint3D(*m_this, first, second));
|
||||
//set the type and values
|
||||
c->template initialize<cvec1>(cv);
|
||||
|
||||
//add it to the clustergraph
|
||||
fusion::vector<LocalEdge, GlobalEdge, bool, bool> res;
|
||||
res = m_this->m_cluster->addEdge(first->template getProperty<vertex_prop>(),
|
||||
second->template getProperty<vertex_prop>());
|
||||
if(!fusion::at_c<2>(res)) {
|
||||
Cons rc;
|
||||
return rc; //TODO: throw
|
||||
};
|
||||
m_this->m_cluster->template setObject<Constraint3D> (fusion::at_c<1> (res), c);
|
||||
//add the coresbondig edge to the constraint
|
||||
c->template setProperty<edge_prop>(fusion::at_c<1>(res));
|
||||
//store the constraint in general object vector of main system
|
||||
m_this->push_back(c);
|
||||
|
||||
return c;
|
||||
};
|
||||
|
||||
template<typename Typelist, typename ID>
|
||||
template<typename Sys>
|
||||
void Module3D<Typelist, ID>::type<Sys>::inheriter_base::removeConstraint3D(Cons c) {
|
||||
|
||||
GlobalEdge e = c->template getProperty<edge_prop>();
|
||||
c->template emitSignal<remove>(c);
|
||||
m_this->m_cluster->removeEdge(e);
|
||||
m_this->erase(c);
|
||||
};
|
||||
|
||||
template<typename Typelist, typename ID>
|
||||
template<typename Sys>
|
||||
void Module3D<Typelist, ID>::type<Sys>::inheriter_base::apply_edge_remove(GlobalEdge e) {
|
||||
Cons c = m_this->m_cluster->template getObject<Constraint3D>(e);
|
||||
c->template emitSignal<remove>(c);
|
||||
m_this->erase(c);
|
||||
};
|
||||
|
||||
template<typename Typelist, typename ID>
|
||||
template<typename Sys>
|
||||
template<typename T>
|
||||
typename Module3D<Typelist, ID>::template type<Sys>::Geom
|
||||
Module3D<Typelist, ID>::type<Sys>::inheriter_id::createGeometry3D(T geom, Identifier id) {
|
||||
Geom g = inheriter_base::createGeometry3D(geom);
|
||||
g->setIdentifier(id);
|
||||
return g;
|
||||
};
|
||||
|
||||
template<typename Typelist, typename ID>
|
||||
template<typename Sys>
|
||||
void Module3D<Typelist, ID>::type<Sys>::inheriter_id::removeGeometry3D(Identifier id) {
|
||||
|
||||
if(hasGeometry3D(id))
|
||||
inheriter_base::removeGeometry3D(getGeometry3D(id));
|
||||
};
|
||||
|
||||
template<typename Typelist, typename ID>
|
||||
template<typename Sys>
|
||||
template<typename T>
|
||||
typename Module3D<Typelist, ID>::template type<Sys>::Cons
|
||||
Module3D<Typelist, ID>::type<Sys>::inheriter_id::createConstraint3D(Identifier id, Geom first, Geom second, T constraint1) {
|
||||
|
||||
Cons c = inheriter_base::createConstraint3D(first, second, constraint1);
|
||||
c->setIdentifier(id);
|
||||
return c;
|
||||
};
|
||||
|
||||
template<typename Typelist, typename ID>
|
||||
template<typename Sys>
|
||||
void Module3D<Typelist, ID>::type<Sys>::inheriter_id::removeConstraint3D(Identifier id) {
|
||||
|
||||
if(hasConstraint3D(id))
|
||||
removeConstraint3D(getConstraint3D(id));
|
||||
};
|
||||
|
||||
|
||||
template<typename Typelist, typename ID>
|
||||
template<typename Sys>
|
||||
bool Module3D<Typelist, ID>::type<Sys>::inheriter_id::hasGeometry3D(Identifier id) {
|
||||
if(getGeometry3D(id)) return true;
|
||||
return false;
|
||||
};
|
||||
|
||||
template<typename Typelist, typename ID>
|
||||
template<typename Sys>
|
||||
typename Module3D<Typelist, ID>::template type<Sys>::Geom
|
||||
Module3D<Typelist, ID>::type<Sys>::inheriter_id::getGeometry3D(Identifier id) {
|
||||
std::vector< Geom >& vec = inheriter_base::m_this->template objectVector<Geometry3D>();
|
||||
typedef typename std::vector<Geom>::iterator iter;
|
||||
for(iter it=vec.begin(); it!=vec.end(); it++) {
|
||||
if(compare_traits<Identifier>::compare((*it)->getIdentifier(), id)) return *it;
|
||||
};
|
||||
return Geom();
|
||||
};
|
||||
|
||||
template<typename Typelist, typename ID>
|
||||
template<typename Sys>
|
||||
bool Module3D<Typelist, ID>::type<Sys>::inheriter_id::hasConstraint3D(Identifier id) {
|
||||
if(getConstraint3D(id)) return true;
|
||||
return false;
|
||||
};
|
||||
|
||||
template<typename Typelist, typename ID>
|
||||
template<typename Sys>
|
||||
typename Module3D<Typelist, ID>::template type<Sys>::Cons
|
||||
Module3D<Typelist, ID>::type<Sys>::inheriter_id::getConstraint3D(Identifier id) {
|
||||
std::vector< Cons >& vec = inheriter_base::m_this->template objectVector<Constraint3D>();
|
||||
typedef typename std::vector<Cons>::iterator iter;
|
||||
for(iter it=vec.begin(); it!=vec.end(); it++) {
|
||||
if(compare_traits<Identifier>::compare((*it)->getIdentifier(), id)) return *it;
|
||||
};
|
||||
return Cons();
|
||||
};
|
||||
|
||||
}//dcm
|
||||
|
||||
#endif //GCM_GEOMETRY3D_H
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
215
src/Mod/Assembly/App/opendcm/module3d/parallel.hpp
Normal file
215
src/Mod/Assembly/App/opendcm/module3d/parallel.hpp
Normal file
@@ -0,0 +1,215 @@
|
||||
/*
|
||||
openDCM, dimensional constraint manager
|
||||
Copyright (C) 2012 Stefan Troeger <stefantroeger@gmx.net>
|
||||
|
||||
This library is free software; you can redistribute it and/or modify
|
||||
it under the terms of the GNU Lesser General Public License as published by
|
||||
the Free Software Foundation; either version 2.1 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 Lesser General Public License for more detemplate tails.
|
||||
|
||||
You should have received a copy of the GNU Lesser General Public License along
|
||||
with this library; if not, write to the Free Software Foundation, Inc.,
|
||||
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
|
||||
*/
|
||||
|
||||
#ifndef GCM_PARALLEL_H
|
||||
#define GCM_PARALLEL_H
|
||||
|
||||
#include <opendcm/core/constraint.hpp>
|
||||
|
||||
#include "geometry.hpp"
|
||||
#include <boost/math/special_functions/fpclassify.hpp>
|
||||
|
||||
using boost::math::isnormal;
|
||||
|
||||
namespace dcm {
|
||||
|
||||
//the calculations( same as we always calculate directions we can outsource the work to this functions)
|
||||
namespace parallel_detail {
|
||||
|
||||
template<typename Kernel, typename T>
|
||||
inline typename Kernel::number_type calc(T d1,
|
||||
T d2,
|
||||
Direction dir) {
|
||||
|
||||
switch(dir) {
|
||||
case Same:
|
||||
return (d1-d2).norm();
|
||||
case Opposite:
|
||||
return (d1+d2).norm();
|
||||
case Both:
|
||||
if(d1.dot(d2) >= 0) {
|
||||
return (d1-d2).norm();
|
||||
}
|
||||
return (d1+d2).norm();
|
||||
default:
|
||||
assert(false);
|
||||
}
|
||||
return 0;
|
||||
};
|
||||
|
||||
|
||||
template<typename Kernel, typename T>
|
||||
inline typename Kernel::number_type calcGradFirst(T d1,
|
||||
T d2,
|
||||
T dd1,
|
||||
Direction dir) {
|
||||
|
||||
typename Kernel::number_type res;
|
||||
switch(dir) {
|
||||
case Same:
|
||||
res = ((d1-d2).dot(dd1) / (d1-d2).norm());
|
||||
break;
|
||||
case Opposite:
|
||||
res= ((d1+d2).dot(dd1) / (d1+d2).norm());
|
||||
break;
|
||||
case Both:
|
||||
if(d1.dot(d2) >= 0) {
|
||||
res = (((d1-d2).dot(dd1) / (d1-d2).norm()));
|
||||
break;
|
||||
}
|
||||
res = (((d1+d2).dot(dd1) / (d1+d2).norm()));
|
||||
break;
|
||||
}
|
||||
if((isnormal)(res)) return res;
|
||||
return 0;
|
||||
};
|
||||
|
||||
template<typename Kernel, typename T>
|
||||
inline typename Kernel::number_type calcGradSecond(T d1,
|
||||
T d2,
|
||||
T dd2,
|
||||
Direction dir) {
|
||||
|
||||
typename Kernel::number_type res;
|
||||
switch(dir) {
|
||||
case Same:
|
||||
res = ((d1-d2).dot(-dd2) / (d1-d2).norm());
|
||||
break;
|
||||
case Opposite:
|
||||
res = ((d1+d2).dot(dd2) / (d1+d2).norm());
|
||||
break;
|
||||
case Both:
|
||||
if(d1.dot(d2) >= 0) {
|
||||
res = (((d1-d2).dot(-dd2) / (d1-d2).norm()));
|
||||
break;
|
||||
}
|
||||
res = (((d1+d2).dot(dd2) / (d1+d2).norm()));
|
||||
break;
|
||||
}
|
||||
if((isnormal)(res)) return res;
|
||||
return 0;
|
||||
};
|
||||
|
||||
template<typename Kernel, typename T>
|
||||
inline void calcGradFirstComp(T d1,
|
||||
T d2,
|
||||
T grad,
|
||||
Direction dir) {
|
||||
|
||||
switch(dir) {
|
||||
case Same:
|
||||
grad = (d1-d2) / (d1-d2).norm();
|
||||
return;
|
||||
case Opposite:
|
||||
grad = (d1+d2) / (d1+d2).norm();
|
||||
return;
|
||||
case Both:
|
||||
assert(false);
|
||||
}
|
||||
};
|
||||
|
||||
template<typename Kernel, typename T>
|
||||
inline void calcGradSecondComp(T d1,
|
||||
T d2,
|
||||
T grad,
|
||||
Direction dir) {
|
||||
|
||||
switch(dir) {
|
||||
case Same:
|
||||
grad = (d2-d1) / (d1-d2).norm();
|
||||
return;
|
||||
case Opposite:
|
||||
grad = (d2+d1) / (d1+d2).norm();
|
||||
return;
|
||||
case Both:
|
||||
assert(false);
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
template< typename Kernel >
|
||||
struct Parallel::type< Kernel, tag::line3D, tag::line3D > : public dcm::PseudoScale<Kernel> {
|
||||
|
||||
typedef typename Kernel::number_type Scalar;
|
||||
typedef typename Kernel::VectorMap Vector;
|
||||
|
||||
Direction value;
|
||||
|
||||
//template definition
|
||||
Scalar calculate(Vector& param1, Vector& param2) {
|
||||
return parallel_detail::calc<Kernel>(param1.template tail<3>(), param2.template tail<3>(), value);
|
||||
};
|
||||
Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) {
|
||||
return parallel_detail::calcGradFirst<Kernel>(param1.template tail<3>(), param2.template tail<3>(), dparam1.template tail<3>(), value);
|
||||
};
|
||||
Scalar calculateGradientSecond(Vector& param1, Vector& param2, Vector& dparam2) {
|
||||
return parallel_detail::calcGradSecond<Kernel>(param1.template tail<3>(), param2.template tail<3>(), dparam2.template tail<3>(), value);
|
||||
};
|
||||
void calculateGradientFirstComplete(Vector& param1, Vector& param2, Vector& gradient) {
|
||||
gradient.template head<3>().setZero();
|
||||
parallel_detail::calcGradFirstComp<Kernel>(param1.template tail<3>(), param2.template tail<3>(), gradient.template tail<3>(), value);
|
||||
};
|
||||
void calculateGradientSecondComplete(Vector& param1, Vector& param2, Vector& gradient) {
|
||||
gradient.template head<3>().setZero();
|
||||
parallel_detail::calcGradSecondComp<Kernel>(param1.template tail<3>(), param2.template tail<3>(), gradient.template tail<3>(), value);
|
||||
};
|
||||
};
|
||||
|
||||
template< typename Kernel >
|
||||
struct Parallel::type< Kernel, tag::cylinder3D, tag::cylinder3D > : public dcm::PseudoScale<Kernel>{
|
||||
|
||||
typedef typename Kernel::number_type Scalar;
|
||||
typedef typename Kernel::VectorMap Vector;
|
||||
|
||||
Direction value;
|
||||
|
||||
Scalar calculate(Vector& param1, Vector& param2) {
|
||||
return parallel_detail::calc<Kernel>(param1.template segment<3>(3), param2.template segment<3>(3), value);
|
||||
};
|
||||
Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) {
|
||||
return parallel_detail::calcGradFirst<Kernel>(param1.template segment<3>(3), param2.template segment<3>(3),
|
||||
dparam1.template segment<3>(3), value);
|
||||
|
||||
};
|
||||
Scalar calculateGradientSecond(Vector& param1, Vector& param2, Vector& dparam2) {
|
||||
return parallel_detail::calcGradSecond<Kernel>(param1.template segment<3>(3), param2.template segment<3>(3),
|
||||
dparam2.template segment<3>(3), value);
|
||||
};
|
||||
void calculateGradientFirstComplete(Vector& param1, Vector& param2, Vector& gradient) {
|
||||
gradient.template head<3>().setZero();
|
||||
parallel_detail::calcGradFirstComp<Kernel>(param1.template segment<3>(3), param2.template segment<3>(3),
|
||||
gradient.template segment<3>(3), value);
|
||||
};
|
||||
void calculateGradientSecondComplete(Vector& param1, Vector& param2, Vector& gradient) {
|
||||
gradient.template head<3>().setZero();
|
||||
parallel_detail::calcGradSecondComp<Kernel>(param1.template segment<3>(3), param2.template segment<3>(3),
|
||||
gradient.template segment<3>(3), value);
|
||||
};
|
||||
};
|
||||
|
||||
template< typename Kernel >
|
||||
struct Parallel::type< Kernel, tag::line3D, tag::plane3D > : public Parallel::type<Kernel, tag::line3D, tag::line3D> {};
|
||||
|
||||
template< typename Kernel >
|
||||
struct Parallel::type< Kernel, tag::plane3D, tag::plane3D > : public Parallel::type<Kernel, tag::line3D, tag::line3D> {};
|
||||
|
||||
}
|
||||
|
||||
#endif //GCM_ANGLE
|
||||
367
src/Mod/Assembly/App/opendcm/module3d/solver.hpp
Normal file
367
src/Mod/Assembly/App/opendcm/module3d/solver.hpp
Normal file
@@ -0,0 +1,367 @@
|
||||
/*
|
||||
openDCM, dimensional constraint manager
|
||||
Copyright (C) 2012 Stefan Troeger <stefantroeger@gmx.net>
|
||||
|
||||
This library is free software; you can redistribute it and/or modify
|
||||
it under the terms of the GNU Lesser General Public License as published by
|
||||
the Free Software Foundation; either version 2.1 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 Lesser General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU Lesser General Public License along
|
||||
with this library; if not, write to the Free Software Foundation, Inc.,
|
||||
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
|
||||
*/
|
||||
|
||||
#ifndef GCM_SOLVER_3D_H
|
||||
#define GCM_SOLVER_3D_H
|
||||
|
||||
#include <boost/graph/adjacency_list.hpp>
|
||||
|
||||
#include "defines.hpp"
|
||||
#include "clustermath.hpp"
|
||||
#include "opendcm/core/sheduler.hpp"
|
||||
#include "opendcm/core/traits.hpp"
|
||||
|
||||
namespace dcm {
|
||||
namespace details {
|
||||
|
||||
template<typename Sys>
|
||||
struct MES : public system_traits<Sys>::Kernel::MappedEquationSystem {
|
||||
|
||||
typedef typename system_traits<Sys>::Kernel Kernel;
|
||||
typedef typename system_traits<Sys>::Cluster Cluster;
|
||||
typedef typename system_traits<Sys>::template getModule<m3d>::type module3d;
|
||||
typedef typename module3d::Geometry3D Geometry3D;
|
||||
typedef boost::shared_ptr<Geometry3D> Geom;
|
||||
typedef typename module3d::Constraint3D Constraint3D;
|
||||
typedef boost::shared_ptr<Constraint3D> Cons;
|
||||
typedef typename module3d::math_prop math_prop;
|
||||
typedef typename module3d::fix_prop fix_prop;
|
||||
typedef typename Kernel::number_type Scalar;
|
||||
typedef typename system_traits<Sys>::Kernel::MappedEquationSystem Base;
|
||||
|
||||
boost::shared_ptr<Cluster> m_cluster;
|
||||
|
||||
MES(boost::shared_ptr<Cluster> cl, int par, int eqn);
|
||||
virtual void recalculate();
|
||||
};
|
||||
|
||||
template<typename Sys>
|
||||
struct SystemSolver : public Job<Sys> {
|
||||
|
||||
typedef typename system_traits<Sys>::Cluster Cluster;
|
||||
typedef typename system_traits<Sys>::Kernel Kernel;
|
||||
typedef typename Kernel::number_type Scalar;
|
||||
typedef typename system_traits<Sys>::template getModule<m3d>::type module3d;
|
||||
typedef typename module3d::Geometry3D Geometry3D;
|
||||
typedef boost::shared_ptr<Geometry3D> Geom;
|
||||
typedef typename module3d::Constraint3D Constraint3D;
|
||||
typedef boost::shared_ptr<Constraint3D> Cons;
|
||||
typedef typename module3d::math_prop math_prop;
|
||||
typedef typename module3d::fix_prop fix_prop;
|
||||
typedef typename module3d::vertex_prop vertex_prop;
|
||||
|
||||
typedef MES<Sys> Mes;
|
||||
|
||||
#ifdef USE_LOGGING
|
||||
src::logger log;
|
||||
#endif
|
||||
struct Rescaler {
|
||||
|
||||
boost::shared_ptr<Cluster> cluster;
|
||||
Mes& mes;
|
||||
int rescales;
|
||||
|
||||
Rescaler(boost::shared_ptr<Cluster> c, Mes& m);
|
||||
|
||||
void operator()();
|
||||
|
||||
Scalar scaleClusters();
|
||||
void collectPseudoPoints(boost::shared_ptr<Cluster> parent,
|
||||
LocalVertex cluster,
|
||||
std::vector<typename Kernel::Vector3,
|
||||
Eigen::aligned_allocator<typename Kernel::Vector3> >& vec);
|
||||
};
|
||||
|
||||
SystemSolver();
|
||||
virtual void execute(Sys& sys);
|
||||
void solveCluster(boost::shared_ptr<Cluster> cluster, Sys& sys);
|
||||
};
|
||||
|
||||
|
||||
/*****************************************************************************************************************/
|
||||
/*****************************************************************************************************************/
|
||||
/*****************************************************************************************************************/
|
||||
/*****************************************************************************************************************/
|
||||
|
||||
|
||||
template<typename Sys>
|
||||
MES<Sys>::MES(boost::shared_ptr<Cluster> cl, int par, int eqn) : Base(par, eqn), m_cluster(cl) {
|
||||
|
||||
};
|
||||
|
||||
template<typename Sys>
|
||||
void MES<Sys>::recalculate() {
|
||||
|
||||
//first calculate all clusters
|
||||
typedef typename Cluster::cluster_iterator citer;
|
||||
std::pair<citer, citer> cit = m_cluster->clusters();
|
||||
for(; cit.first != cit.second; cit.first++) {
|
||||
|
||||
if(!(*cit.first).second->template getClusterProperty<fix_prop>())
|
||||
(*cit.first).second->template getClusterProperty<math_prop>().recalculate();
|
||||
|
||||
};
|
||||
|
||||
//with everything updated just nicely we can compute the constraints
|
||||
typedef typename Cluster::template object_iterator<Constraint3D> oiter;
|
||||
typedef typename boost::graph_traits<Cluster>::edge_iterator eiter;
|
||||
std::pair<eiter, eiter> eit = boost::edges(*m_cluster);
|
||||
for(; eit.first != eit.second; eit.first++) {
|
||||
//as always: every local edge can hold multiple global ones, so iterate over all constraints
|
||||
//hold by the individual edge
|
||||
std::pair< oiter, oiter > oit = m_cluster->template getObjects<Constraint3D>(*eit.first);
|
||||
for(; oit.first != oit.second; oit.first++) {
|
||||
if(*oit.first)
|
||||
(*oit.first)->calculate(Base::Scaling);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
template<typename Sys>
|
||||
SystemSolver<Sys>::Rescaler::Rescaler(boost::shared_ptr<Cluster> c, Mes& m) : cluster(c), mes(m), rescales(0) {
|
||||
|
||||
};
|
||||
|
||||
template<typename Sys>
|
||||
void SystemSolver<Sys>::Rescaler::operator()() {
|
||||
mes.Scaling = scaleClusters();
|
||||
rescales++;
|
||||
};
|
||||
|
||||
template<typename Sys>
|
||||
typename SystemSolver<Sys>::Scalar SystemSolver<Sys>::Rescaler::scaleClusters() {
|
||||
|
||||
typedef typename Cluster::cluster_iterator citer;
|
||||
std::pair<citer, citer> cit = cluster->clusters();
|
||||
//get the maximal scale
|
||||
Scalar sc = 0;
|
||||
for(cit = cluster->clusters(); cit.first != cit.second; cit.first++) {
|
||||
//fixed cluster are irrelevant for scaling
|
||||
if((*cit.first).second->template getClusterProperty<fix_prop>()) continue;
|
||||
|
||||
//get the biggest scale factor
|
||||
details::ClusterMath<Sys>& math = (*cit.first).second->template getClusterProperty<math_prop>();
|
||||
|
||||
//math.m_pseudo.clear();
|
||||
//collectPseudoPoints(cluster, (*cit.first).first, math.m_pseudo);
|
||||
|
||||
const Scalar s = math.calculateClusterScale();
|
||||
sc = (s>sc) ? s : sc;
|
||||
}
|
||||
//if no scaling-value returned we can use 1
|
||||
sc = (Kernel::isSame(sc,0)) ? 1. : sc;
|
||||
|
||||
typedef typename boost::graph_traits<Cluster>::vertex_iterator iter;
|
||||
std::pair<iter, iter> it = boost::vertices(*cluster);
|
||||
for(; it.first != it.second; it.first++) {
|
||||
|
||||
if(cluster->isCluster(*it.first)) {
|
||||
boost::shared_ptr<Cluster> c = cluster->getVertexCluster(*it.first);
|
||||
c->template getClusterProperty<math_prop>().applyClusterScale(sc,
|
||||
c->template getClusterProperty<fix_prop>());
|
||||
} else {
|
||||
Geom g = cluster->template getObject<Geometry3D>(*it.first);
|
||||
g->scale(sc*SKALEFAKTOR);
|
||||
}
|
||||
}
|
||||
return 1./(sc*SKALEFAKTOR);
|
||||
};
|
||||
|
||||
template<typename Sys>
|
||||
void SystemSolver<Sys>::Rescaler::collectPseudoPoints(
|
||||
boost::shared_ptr<typename SystemSolver<Sys>::Cluster> parent,
|
||||
LocalVertex cluster,
|
||||
std::vector<typename SystemSolver<Sys>::Kernel::Vector3,
|
||||
Eigen::aligned_allocator<typename SystemSolver<Sys>::Kernel::Vector3> >& vec) {
|
||||
|
||||
std::vector<typename Kernel::Vector3, Eigen::aligned_allocator<typename Kernel::Vector3> > vec2;
|
||||
typedef typename Cluster::template object_iterator<Constraint3D> c_iter;
|
||||
typedef typename boost::graph_traits<Cluster>::out_edge_iterator e_iter;
|
||||
std::pair<e_iter, e_iter> it = boost::out_edges(cluster, *parent);
|
||||
for(; it.first != it.second; it.first++) {
|
||||
|
||||
std::pair< c_iter, c_iter > cit = parent->template getObjects<Constraint3D>(*it.first);
|
||||
for(; cit.first != cit.second; cit.first++) {
|
||||
Cons c = *(cit.first);
|
||||
|
||||
if(!c)
|
||||
continue;
|
||||
|
||||
//get the first global vertex and see if we have it in the wanted cluster or not
|
||||
GlobalVertex v = c->first->template getProperty<vertex_prop>();
|
||||
std::pair<LocalVertex,bool> res = parent->getLocalVertex(v);
|
||||
if(!res.second)
|
||||
return; //means the geometry is in non of the clusters which is not allowed
|
||||
|
||||
if(res.first == cluster)
|
||||
c->collectPseudoPoints(vec, vec2);
|
||||
else
|
||||
c->collectPseudoPoints(vec2, vec);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
template<typename Sys>
|
||||
SystemSolver<Sys>::SystemSolver() {
|
||||
Job<Sys>::priority = 1000;
|
||||
#ifdef USE_LOGGING
|
||||
log.add_attribute("Tag", attrs::constant< std::string >("SystemSolver3D"));
|
||||
#endif
|
||||
};
|
||||
|
||||
template<typename Sys>
|
||||
void SystemSolver<Sys>::execute(Sys& sys) {
|
||||
solveCluster(sys.m_cluster, sys);
|
||||
};
|
||||
|
||||
template<typename Sys>
|
||||
void SystemSolver<Sys>::solveCluster(boost::shared_ptr<Cluster> cluster, Sys& sys) {
|
||||
|
||||
//set out and solve all relevant subclusters
|
||||
typedef typename Cluster::cluster_iterator citer;
|
||||
std::pair<citer, citer> cit = cluster->clusters();
|
||||
for(; cit.first != cit.second; cit.first++) {
|
||||
|
||||
boost::shared_ptr<Cluster> c = (*cit.first).second;
|
||||
if(c->template getClusterProperty<changed_prop>() &&
|
||||
c->template getClusterProperty<type_prop>() == details::cluster3D)
|
||||
solveCluster(c, sys);
|
||||
}
|
||||
|
||||
int params=0, constraints=0;
|
||||
typename Kernel::number_type scale = 1;
|
||||
|
||||
//get the ammount of parameters and constraint equations we need
|
||||
typedef typename boost::graph_traits<Cluster>::vertex_iterator iter;
|
||||
std::pair<iter, iter> it = boost::vertices(*cluster);
|
||||
for(; it.first != it.second; it.first++) {
|
||||
|
||||
//when cluster and not fixed it has trans and rot parameter
|
||||
if(cluster->isCluster(*it.first)) {
|
||||
if(!cluster->template getSubclusterProperty<fix_prop>(*it.first)) {
|
||||
params += 6;
|
||||
}
|
||||
} else {
|
||||
params += cluster->template getObject<Geometry3D>(*it.first)->m_parameterCount;
|
||||
};
|
||||
}
|
||||
|
||||
//count the equations in the constraints
|
||||
typedef typename Cluster::template object_iterator<Constraint3D> ocit;
|
||||
typedef typename boost::graph_traits<Cluster>::edge_iterator e_iter;
|
||||
std::pair<e_iter, e_iter> e_it = boost::edges(*cluster);
|
||||
for(; e_it.first != e_it.second; e_it.first++) {
|
||||
std::pair< ocit, ocit > it = cluster->template getObjects<Constraint3D>(*e_it.first);
|
||||
for(; it.first != it.second; it.first++)
|
||||
constraints += (*it.first)->equationCount();
|
||||
};
|
||||
|
||||
|
||||
//initialise the system with now known size
|
||||
//std::cout<<"constraints: "<<constraints<<", params: "<<params+rot_params+trans_params<<std::endl;
|
||||
Mes mes(cluster, params, constraints);
|
||||
|
||||
//iterate all geometrys again and set the needed maps
|
||||
it = boost::vertices(*cluster);
|
||||
for(; it.first != it.second; it.first++) {
|
||||
|
||||
if(cluster->isCluster(*it.first)) {
|
||||
boost::shared_ptr<Cluster> c = cluster->getVertexCluster(*it.first);
|
||||
details::ClusterMath<Sys>& cm = c->template getClusterProperty<math_prop>();
|
||||
//only get maps and propagate downstream if not fixed
|
||||
if(!c->template getClusterProperty<fix_prop>()) {
|
||||
//set norm Quaternion as map to the parameter vector
|
||||
int offset = mes.setParameterMap(cm.getNormQuaternionMap());
|
||||
//set translation as map to the parameter vector
|
||||
mes.setParameterMap(cm.getTranslationMap());
|
||||
//write initail values to the parameter maps
|
||||
//remember the parameter offset as all downstream geometry must use this offset
|
||||
cm.setParameterOffset(offset);
|
||||
//wirte initial values
|
||||
cm.initMaps();
|
||||
} else cm.initFixMaps();
|
||||
|
||||
//map all geometrie within that cluster to it's rotation matrix
|
||||
//for collecting all geometries which need updates
|
||||
cm.clearGeometry();
|
||||
cm.mapClusterDownstreamGeometry(c);
|
||||
|
||||
} else {
|
||||
Geom g = cluster->template getObject<Geometry3D>(*it.first);
|
||||
int offset = mes.setParameterMap(g->m_parameterCount, g->getParameterMap());
|
||||
g->m_offset = offset;
|
||||
//init the parametermap with initial values
|
||||
g->initMap();
|
||||
}
|
||||
}
|
||||
|
||||
//and now the constraints to set the residual and gradient maps
|
||||
typedef typename Cluster::template object_iterator<Constraint3D> oiter;
|
||||
e_it = boost::edges(*cluster);
|
||||
for(; e_it.first != e_it.second; e_it.first++) {
|
||||
|
||||
//as always: every local edge can hold multiple global ones, so iterate over all constraints
|
||||
//hold by the individual edge
|
||||
std::pair< oiter, oiter > oit = cluster->template getObjects<Constraint3D>(*e_it.first);
|
||||
for(; oit.first != oit.second; oit.first++) {
|
||||
|
||||
//set the maps
|
||||
Cons c = *oit.first;
|
||||
if(c) c->setMaps(mes);
|
||||
//TODO: else throw (as every global edge was counted as one equation)
|
||||
}
|
||||
}
|
||||
|
||||
int stop = 0;
|
||||
Rescaler re(cluster, mes);
|
||||
re();
|
||||
stop = Kernel::solve(mes, re);
|
||||
#ifdef USE_LOGGING
|
||||
BOOST_LOG(log)<< "Numbers of rescale: "<<re.rescales;
|
||||
#endif
|
||||
|
||||
//now go to all relevant geometries and clusters and write the values back
|
||||
it = boost::vertices(*cluster);
|
||||
for(; it.first != it.second; it.first++) {
|
||||
|
||||
if(cluster->isCluster(*it.first)) {
|
||||
boost::shared_ptr<Cluster> c = cluster->getVertexCluster(*it.first);
|
||||
if(!cluster->template getSubclusterProperty<fix_prop>(*it.first))
|
||||
c->template getClusterProperty<math_prop>().finishCalculation();
|
||||
else
|
||||
c->template getClusterProperty<math_prop>().finishFixCalculation();
|
||||
|
||||
std::vector<Geom>& vec = c->template getClusterProperty<math_prop>().getGeometry();
|
||||
for(typename std::vector<Geom>::iterator vit = vec.begin(); vit != vec.end(); vit++)
|
||||
(*vit)->finishCalculation();
|
||||
|
||||
} else {
|
||||
Geom g = cluster->template getObject<Geometry3D>(*it.first);
|
||||
g->scale(mes.Scaling);
|
||||
g->finishCalculation();
|
||||
}
|
||||
}
|
||||
//we have solved this cluster
|
||||
cluster->template setClusterProperty<changed_prop>(false);
|
||||
};
|
||||
|
||||
}//details
|
||||
}//dcm
|
||||
|
||||
#endif //DCM_SOLVER_3D_HPP
|
||||
95
src/Mod/Assembly/App/opendcm/module3d/state.hpp
Normal file
95
src/Mod/Assembly/App/opendcm/module3d/state.hpp
Normal file
@@ -0,0 +1,95 @@
|
||||
/*
|
||||
openDCM, dimensional constraint manager
|
||||
Copyright (C) 2013 Stefan Troeger <stefantroeger@gmx.net>
|
||||
|
||||
This library is free software; you can redistribute it and/or modify
|
||||
it under the terms of the GNU Lesser General Public License as published by
|
||||
the Free Software Foundation; either version 2.1 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 Lesser General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU Lesser General Public License along
|
||||
with this library; if not, write to the Free Software Foundation, Inc.,
|
||||
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
|
||||
*/
|
||||
|
||||
#ifndef DCM_MODULE3D_STATE_HPP
|
||||
#define DCM_MODULE3D_STATE_HPP
|
||||
|
||||
#include "module.hpp"
|
||||
#include "opendcm/moduleState/traits.hpp"
|
||||
#include <boost/spirit/include/karma.hpp>
|
||||
#include <boost/spirit/include/phoenix.hpp>
|
||||
|
||||
namespace karma = boost::spirit::karma;
|
||||
namespace ascii = boost::spirit::karma::ascii;
|
||||
namespace phx = boost::phoenix;
|
||||
|
||||
namespace dcm {
|
||||
|
||||
namespace details {
|
||||
|
||||
struct geom_visitor : public boost::static_visitor<int> {
|
||||
|
||||
template<typename T>
|
||||
int operator()(T& i) const {
|
||||
return geometry_traits<T>::tag::weight::value;
|
||||
};
|
||||
};
|
||||
|
||||
template<typename T>
|
||||
int getWeight(boost::shared_ptr<T> ptr) {
|
||||
return boost::apply_visitor(geom_visitor(), ptr->m_geometry);
|
||||
};
|
||||
|
||||
template<typename Kernel>
|
||||
void getStdVector(typename Kernel::Vector& eigen, std::vector<typename Kernel::number_type>& vec) {
|
||||
vec.resize(eigen.size());
|
||||
for(int i=0; i<eigen.size(); i++)
|
||||
vec[i] = eigen(i);
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
template<typename System>
|
||||
struct parser_generate< typename Module3D::type<System>::Geometry3D, System>
|
||||
: public mpl::true_{};
|
||||
|
||||
template<typename System, typename iterator>
|
||||
struct parser_generator< typename Module3D::type<System>::Geometry3D, System, iterator > {
|
||||
|
||||
typedef typename Sys::Kernel Kernel;
|
||||
typedef typename typename Module3D::type<System>::Geometry3D Geometry;
|
||||
typedef karma::rule<iterator, boost::shared_ptr<Geometry>() > generator;
|
||||
static void init(generator& r) {
|
||||
r = karma::lit("<type>Geometry3D</type>\n<class>")
|
||||
<< ascii::string[karma::_1 = phx::bind(&details::getWeight<Geometry>, karma::_val)]
|
||||
<< "</class>\n<value>"
|
||||
<< (karma::double_ % " ")[phx::bind(&details::getStdVector<Kernel>, )]
|
||||
};
|
||||
};
|
||||
|
||||
template<typename System>
|
||||
struct parser_parse< typename Module3D::type<System>::Geometry3D, System>
|
||||
: public mpl::true_{};
|
||||
|
||||
template<typename System, typename iterator>
|
||||
struct parser_parser< typename Module3D::type<System>::Geometry3D, System, iterator > {
|
||||
|
||||
typedef typename Module3D::type<System>::Geometry3D object_type;
|
||||
|
||||
typedef qi::rule<iterator, boost::shared_ptr<object_type>(System*), qi::space_type> parser;
|
||||
static void init(parser& r) {
|
||||
r = qi::lexeme[qi::lit("<type>object 1 prop</type>")[ qi::_val =
|
||||
phx::construct<boost::shared_ptr<object_type> >( phx::new_<object_type>(*qi::_r1))]] >> ("<value>HaHAHAHAHA</value>");
|
||||
};
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
|
||||
#endif //DCM_MODULE3D_STATE_HPP
|
||||
Reference in New Issue
Block a user