From b496dfded48fd542b18deb1faefffffec3450754 Mon Sep 17 00:00:00 2001 From: "U-EMEA\\jriegel" Date: Wed, 18 Jan 2012 19:00:01 +0100 Subject: [PATCH] Add ViewProvider for Assembly and Part and add FreeGCE3D --- src/Mod/Assembly/App/gcs3d/CMakeLists.txt | 24 + src/Mod/Assembly/App/gcs3d/Constraints.cpp | 416 ++++++ src/Mod/Assembly/App/gcs3d/Constraints.h | 133 ++ src/Mod/Assembly/App/gcs3d/Example/AS_ex1.gcx | 28 + src/Mod/Assembly/App/gcs3d/Example/AS_ex2.gcx | 61 + src/Mod/Assembly/App/gcs3d/Example/AS_ex3.gcx | 55 + src/Mod/Assembly/App/gcs3d/Example/AS_ex4.gcx | 64 + src/Mod/Assembly/App/gcs3d/Example/AS_ex5.gcx | 61 + src/Mod/Assembly/App/gcs3d/Example/AS_ex6.gcx | 64 + src/Mod/Assembly/App/gcs3d/Example/AS_ex7.gcx | 64 + src/Mod/Assembly/App/gcs3d/GCS.cpp | 1139 +++++++++++++++++ src/Mod/Assembly/App/gcs3d/GCS.h | 122 ++ src/Mod/Assembly/App/gcs3d/Geo.h | 67 + src/Mod/Assembly/App/gcs3d/InputParser.cpp | 359 ++++++ src/Mod/Assembly/App/gcs3d/InputParser.h | 60 + src/Mod/Assembly/App/gcs3d/SubSystem.cpp | 351 +++++ src/Mod/Assembly/App/gcs3d/SubSystem.h | 89 ++ src/Mod/Assembly/App/gcs3d/Util.h | 47 + src/Mod/Assembly/App/gcs3d/main.cpp | 122 ++ src/Mod/Assembly/App/gcs3d/qp_eq.cpp | 65 + src/Mod/Assembly/App/gcs3d/qp_eq.h | 25 + src/Mod/Assembly/Gui/ViewProviderAssembly.cpp | 50 + src/Mod/Assembly/Gui/ViewProviderAssembly.h | 51 + src/Mod/Assembly/Gui/ViewProviderPart.cpp | 50 + src/Mod/Assembly/Gui/ViewProviderPart.h | 51 + 25 files changed, 3618 insertions(+) create mode 100644 src/Mod/Assembly/App/gcs3d/CMakeLists.txt create mode 100644 src/Mod/Assembly/App/gcs3d/Constraints.cpp create mode 100644 src/Mod/Assembly/App/gcs3d/Constraints.h create mode 100644 src/Mod/Assembly/App/gcs3d/Example/AS_ex1.gcx create mode 100644 src/Mod/Assembly/App/gcs3d/Example/AS_ex2.gcx create mode 100644 src/Mod/Assembly/App/gcs3d/Example/AS_ex3.gcx create mode 100644 src/Mod/Assembly/App/gcs3d/Example/AS_ex4.gcx create mode 100644 src/Mod/Assembly/App/gcs3d/Example/AS_ex5.gcx create mode 100644 src/Mod/Assembly/App/gcs3d/Example/AS_ex6.gcx create mode 100644 src/Mod/Assembly/App/gcs3d/Example/AS_ex7.gcx create mode 100644 src/Mod/Assembly/App/gcs3d/GCS.cpp create mode 100644 src/Mod/Assembly/App/gcs3d/GCS.h create mode 100644 src/Mod/Assembly/App/gcs3d/Geo.h create mode 100644 src/Mod/Assembly/App/gcs3d/InputParser.cpp create mode 100644 src/Mod/Assembly/App/gcs3d/InputParser.h create mode 100644 src/Mod/Assembly/App/gcs3d/SubSystem.cpp create mode 100644 src/Mod/Assembly/App/gcs3d/SubSystem.h create mode 100644 src/Mod/Assembly/App/gcs3d/Util.h create mode 100644 src/Mod/Assembly/App/gcs3d/main.cpp create mode 100644 src/Mod/Assembly/App/gcs3d/qp_eq.cpp create mode 100644 src/Mod/Assembly/App/gcs3d/qp_eq.h create mode 100644 src/Mod/Assembly/Gui/ViewProviderAssembly.cpp create mode 100644 src/Mod/Assembly/Gui/ViewProviderAssembly.h create mode 100644 src/Mod/Assembly/Gui/ViewProviderPart.cpp create mode 100644 src/Mod/Assembly/Gui/ViewProviderPart.h diff --git a/src/Mod/Assembly/App/gcs3d/CMakeLists.txt b/src/Mod/Assembly/App/gcs3d/CMakeLists.txt new file mode 100644 index 0000000000..518b8553ef --- /dev/null +++ b/src/Mod/Assembly/App/gcs3d/CMakeLists.txt @@ -0,0 +1,24 @@ +project(solver) + +cmake_minimum_required(VERSION 2.6.0 FATAL_ERROR) +set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") + +find_package(XercesC REQUIRED) +find_package(Eigen3) + +include_directories(${EIGEN3_INCLUDE_DIR}) + +#add_subdirectory(NewSolver) + +set(solver_SRC main.cpp + Constraints.cpp + GCS.cpp + InputParser.cpp + qp_eq.cpp + SubSystem.cpp) + +add_executable(solver ${solver_SRC}) + +target_link_libraries(solver ${XERCESC_LIBRARIES}) + + diff --git a/src/Mod/Assembly/App/gcs3d/Constraints.cpp b/src/Mod/Assembly/App/gcs3d/Constraints.cpp new file mode 100644 index 0000000000..8dbe679358 --- /dev/null +++ b/src/Mod/Assembly/App/gcs3d/Constraints.cpp @@ -0,0 +1,416 @@ +/*************************************************************************** + * Copyright (c) Konstantinos Poulios (logari81@gmail.com) 2011 * + * * + * This file is part of the FreeCAD CAx development system. * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of the GNU Library General Public * + * License as published by the Free Software Foundation; either * + * version 2 of the License, or (at your option) any later version. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Library General Public License for more details. * + * * + * You should have received a copy of the GNU Library General Public * + * License along with this library; see the file COPYING.LIB. If not, * + * write to the Free Software Foundation, Inc., 59 Temple Place, * + * Suite 330, Boston, MA 02111-1307, USA * + * * + ***************************************************************************/ + +#include +#include "Constraints.h" + +using namespace Eigen; + +namespace GCS +{ + +/////////////////////////////////////// +// Constraints +/////////////////////////////////////// + +Constraint::Constraint() +: origpvec(0), pvec(0), scale(1.), tag(0) +{ +} + +void Constraint::redirectParams(MAP_pD_pD redirectionmap) +{ + int i=0; + for (VEC_pD::iterator param=origpvec.begin(); + param != origpvec.end(); ++param, i++) { + MAP_pD_pD::const_iterator it = redirectionmap.find(*param); + if (it != redirectionmap.end()) + pvec[i] = it->second; + } +} + +void Constraint::revertParams() +{ + pvec = origpvec; +} + +ConstraintType Constraint::getTypeId() +{ + return None; +} + +void Constraint::rescale(double coef) +{ + scale = coef * 1.; +} + +double Constraint::error() +{ + return 0.; +} + +double Constraint::grad(double *param) +{ + return 0.; +} + +double Constraint::maxStep(MAP_pD_D &dir, double lim) +{ + return lim; +} + + +//quattorot and derivatives + +Matrix3d rotation(double a, double b, double c, double d) { + + double norm = sqrt(pow(a,2)+pow(b,2)+pow(c,2)+pow(d,2)); + double x=a/norm, y=b/norm, z=c/norm, w=d/norm; + + Matrix3d rot; + rot(0,0) = 1-2*(pow(y,2)+pow(z,2)); + rot(0,1) = -2.0*w*z + 2.0*x*y; + rot(0,2) = 2.0*w*y + 2.0*x*z; + rot(1,0) = 2.0*w*z + 2.0*x*y; + rot(1,1) = 1-2*(pow(x,2)+pow(z,2)); + rot(1,2) = -2.0*w*x + 2.0*y*z; + rot(2,0) = -2.0*w*y + 2.0*x*z; + rot(2,1) = 2.0*w*x + 2.0*y*z; + rot(2,2) = 1-2*(pow(x,2)+pow(y,2)); + + return rot; +} + +Matrix3d rotation_da(double a, double b, double c, double d) { + + double no = sqrt(pow(a,2)+pow(b,2)+pow(c,2)+pow(d,2)); + double div = pow(pow(a,2)+pow(b,2)+pow(c,2)+pow(d,2), 3.0/2.0); + double x=a/no, y=b/no, z=c/no, w=d/no; + + double dxa = 1.0/no - pow(a,2)/pow(no,3); + double dya = (-1.0*b*a)/div; + double dza = (-1.0*c*a)/div; + double dwa = (-1.0*d*a)/div; + + Matrix3d rot; + rot << -4.0*(y*dya+z*dza), -2.0*(w*dza+dwa*z)+2.0*(x*dya+dxa*y), 2.0*(dwa*y+w*dya)+2.0*(dxa*z+x*dza), + 2.0*(w*dza+dwa*z)+2.0*(x*dya+dxa*y), -4.0*(x*dxa+z*dza), -2.0*(dwa*x+w*dxa)+2.0*(dya*z+y*dza), + -2.0*(dwa*y+w*dya)+2.0*(dxa*z+x*dza), 2.0*(dwa*x+w*dxa)+2.0*(dya*z+y*dza), -4.0*(x*dxa+y*dya); + + return rot; +} + +Matrix3d rotation_db(double a, double b, double c, double d) { + + double no = sqrt(pow(a,2)+pow(b,2)+pow(c,2)+pow(d,2)); + double div = pow(pow(a,2)+pow(b,2)+pow(c,2)+pow(d,2), 3.0/2.0); + double x=a/no, y=b/no, z=c/no, w=d/no; + + double dxb = (-1.0*a*b)/div; + double dyb = 1.0/no - pow(b,2)/pow(no,3); + double dzb = (-1.0*c*b)/div; + double dwb = (-1.0*d*b)/div; + + Matrix3d rot; + rot << -4.0*(y*dyb+z*dzb), -2.0*(w*dzb+dwb*z)+2.0*(x*dyb+dxb*y), 2.0*(dwb*y+w*dyb)+2.0*(dxb*z+x*dzb), + 2.0*(w*dzb+dwb*z)+2.0*(x*dyb+dxb*y), -4.0*(x*dxb+z*dzb), -2.0*(dwb*x+w*dxb)+2.0*(dyb*z+y*dzb), + -2.0*(dwb*y+w*dyb)+2.0*(dxb*z+x*dzb), 2.0*(dwb*x+w*dxb)+2.0*(dyb*z+y*dzb), -4.0*(x*dxb+y*dyb); + + return rot; +} + +Matrix3d rotation_dc(double a, double b, double c, double d) { + + double no = sqrt(pow(a,2)+pow(b,2)+pow(c,2)+pow(d,2)); + double div = pow(pow(a,2)+pow(b,2)+pow(c,2)+pow(d,2), 3.0/2.0); + double x=a/no, y=b/no, z=c/no, w=d/no; + + double dxc = (-1.0*a*c)/div; + double dyc = (-1.0*b*c)/div; + double dzc = 1.0/no - pow(c,2)/pow(no,3); + double dwc = (-1.0*d*c)/div; + + Matrix3d rot; + rot << -4.0*(y*dyc+z*dzc), -2.0*(w*dzc+dwc*z)+2.0*(x*dyc+dxc*y), 2.0*(dwc*y+w*dyc)+2.0*(dxc*z+x*dzc), + 2.0*(w*dzc+dwc*z)+2.0*(x*dyc+dxc*y), -4.0*(x*dxc+z*dzc), -2.0*(dwc*x+w*dxc)+2.0*(dyc*z+y*dzc), + -2.0*(dwc*y+w*dyc)+2.0*(dxc*z+x*dzc), 2.0*(dwc*x+w*dxc)+2.0*(dyc*z+y*dzc), -4.0*(x*dxc+y*dyc); + + return rot; +} + +Matrix3d rotation_dd(double a, double b, double c, double d) { + + double no = sqrt(pow(a,2)+pow(b,2)+pow(c,2)+pow(d,2)); + double div = pow(pow(a,2)+pow(b,2)+pow(c,2)+pow(d,2), 3.0/2.0); + double x=a/no, y=b/no, z=c/no, w=d/no; + + double dxd = (-1.0*a*d)/div; + double dyd = (-1.0*b*d)/div; + double dzd = (-1.0*c*d)/div; + double dwd = 1.0/no - pow(d,2)/pow(no,3); + + Matrix3d rot; + rot << -4.0*(y*dyd+z*dzd), -2.0*(w*dzd+dwd*z)+2.0*(x*dyd+dxd*y), 2.0*(dwd*y+w*dyd)+2.0*(dxd*z+x*dzd), + 2.0*(w*dzd+dwd*z)+2.0*(x*dyd+dxd*y), -4.0*(x*dxd+z*dzd), -2.0*(dwd*x+w*dxd)+2.0*(dyd*z+y*dzd), + -2.0*(dwd*y+w*dyd)+2.0*(dxd*z+x*dzd), 2.0*(dwd*x+w*dxd)+2.0*(dyd*z+y*dzd), -4.0*(x*dxd+y*dyd); + + return rot; +} + + +//Plane parallel (need to be treated special as angle=0° or angle=180° dos not work with angle constraint +ConstraintParralelFaceAS::ConstraintParralelFaceAS( GCS::Solid& s0, GCS::Solid& s1, ParallelType *t) +{ + type = t; + + //get the face placements in the objects coordinate system and calculate the normals + Vector3d a,b; + a << s0.n.x, s0.n.y, s0.n.z; + b << s1.n.x, s1.n.y, s1.n.z; + + //get the normal vector + n0 = (a).normalized(); + n1 = (b).normalized(); + + + pvec.push_back(s0.q.a); + pvec.push_back(s0.q.b); + pvec.push_back(s0.q.c); + pvec.push_back(s0.q.d); + pvec.push_back(s1.q.a); + pvec.push_back(s1.q.b); + pvec.push_back(s1.q.c); + pvec.push_back(s1.q.d); + origpvec = pvec; + rescale(); + +} + +ConstraintType ConstraintParralelFaceAS::getTypeId() +{ + return ASParallel; +} + +void ConstraintParralelFaceAS::rescale(double coef) +{ + scale = coef; +} + +double ConstraintParralelFaceAS::error() +{ + + Matrix3d rot0, rot1; + + rot0 = rotation(*q0a(), *q0b(), *q0c(), *q0d()); + rot1 = rotation(*q1a(), *q1b(), *q1c(), *q1d()); + + Vector3d n0_g = (rot0*n0); + Vector3d n1_g = (rot1*n1); + + double error = 0; + if (*type == GCS::NormalSame) + error = pow((n0_g-n1_g).norm(),2); + else + error = pow((n0_g+n1_g).norm(),2); + + return error; +} + +double ConstraintParralelFaceAS::grad(double* param) +{ + + Vector3d dn; + Matrix3d r0, r1; + r0 = rotation(*q0a(), *q0b(), *q0c(), *q0d()); + r1 = rotation(*q1a(), *q1b(), *q1c(), *q1d()); + + if (param == q0a()) { + Matrix3d rot = rotation_da(*q0a(), *q0b(), *q0c(), *q0d()); + dn = rot*n0; + } + else if (param == q0b()) { + Matrix3d rot = rotation_db(*q0a(), *q0b(), *q0c(), *q0d()); + dn = rot*n0; + } + else if (param == q0c()) { + Matrix3d rot = rotation_dc(*q0a(), *q0b(), *q0c(), *q0d()); + dn = rot*n0; + } + else if (param == q0d()) { + Matrix3d rot = rotation_dd(*q0a(), *q0b(), *q0c(), *q0d()); + dn = rot*n0; + } + else if (param == q1a()) { + Matrix3d rot = rotation_da(*q1a(), *q1b(), *q1c(), *q1d()); + dn = rot*n1*-1; + } + else if (param == q1b()) { + Matrix3d rot = rotation_db(*q1a(), *q1b(), *q1c(), *q1d()); + dn = rot*n1*-1; + } + else if (param == q1c()) { + Matrix3d rot = rotation_dc(*q1a(), *q1b(), *q1c(), *q1d()); + dn = rot*n1*-1; + } + else if (param == q1d()) { + Matrix3d rot = rotation_dd(*q1a(), *q1b(), *q1c(), *q1d()); + dn = rot*n1*-1; + } + else return 0; + + + Vector3d n0n = r0*n0; + Vector3d n1n = r1*n1; + + double div = 0; + if (*type == NormalSame) + div = (n0n-n1n).dot(dn)*2; + else + div = (n0n+n1n).dot(dn)*2; + + return div; +} + + +//dDistance constraint + +ConstraintFaceDistanceAS::ConstraintFaceDistanceAS(GCS::Solid& s0, GCS::Solid& s1, double *d) +{ + + n0 << s0.n.x, s0.n.y, s0.n.z; + n0.normalize(); + p0 << s0.p.x, s0.p.y, s0.p.z; + p1 << s1.p.x, s1.p.y, s1.p.z; + + //and positions + pvec.push_back(s0.d.x); + pvec.push_back(s0.d.y); + pvec.push_back(s0.d.z); + pvec.push_back(s1.d.x); + pvec.push_back(s1.d.y); + pvec.push_back(s1.d.z); + + //quaternions + pvec.push_back(s0.q.a); + pvec.push_back(s0.q.b); + pvec.push_back(s0.q.c); + pvec.push_back(s0.q.d); + pvec.push_back(s1.q.a); + pvec.push_back(s1.q.b); + pvec.push_back(s1.q.c); + pvec.push_back(s1.q.d); + + //distance + dist = d; + + origpvec = pvec; + rescale(); + +} + +ConstraintType ConstraintFaceDistanceAS::getTypeId() +{ + return ASDistance; +} + +void ConstraintFaceDistanceAS::rescale(double coef) +{ + scale = coef; +} + +double ConstraintFaceDistanceAS::error() +{ + + Matrix3d rot0, rot1; + Vector3d v0, v1; + + rot0 = rotation(*q0a(), *q0b(), *q0c(), *q0d()); + rot1 = rotation(*q1a(), *q1b(), *q1c(), *q1d()); + v0 << *p0x(), *p0y(), *p0z(); + v1 << *p1x(), *p1y(), *p1z(); + + double error = std::pow(((rot0*n0).dot(rot1*p1+v1) - (rot0*n0).dot(rot0*p0+v0))/(rot0*n0).norm() - *dist,2); + + return error; +} + +double ConstraintFaceDistanceAS::grad(double* param) +{ + + Matrix3d r0, r1; + Vector3d v0, v1; + + v0 << *p0x(), *p0y(), *p0z(); + v1 << *p1x(), *p1y(), *p1z(); + r0 = rotation(*q0a(), *q0b(), *q0c(), *q0d()); + r1 = rotation(*q1a(), *q1b(), *q1c(), *q1d()); + + Matrix3d dr0, dr1; + double div = 0; + double error=((r0*n0).dot(r1*p1+v1) - (r0*n0).dot(r0*p0+v0))/(r0*n0).norm() - *dist; + if (param == q0a() || param == q0b() || param == q0c() || param == q0d()) { + + if (param == q0a()) dr0 = rotation_da(*q0a(), *q0b(), *q0c(), *q0d()); + else if (param == q0b()) dr0 = rotation_db(*q0a(), *q0b(), *q0c(), *q0d()); + else if (param == q0c()) dr0 = rotation_dc(*q0a(), *q0b(), *q0c(), *q0d()); + else if (param == q0d()) dr0 = rotation_dd(*q0a(), *q0b(), *q0c(), *q0d()); + + VectorXd r0n = r0*n0; + div = ( (dr0*n0).dot(r1*p1+v1)*r0n.norm() - r0n.dot(r1*p1+v1)*r0n.dot(dr0*n0)/r0n.norm() ); + div -= ( ((dr0*n0).dot(r0*p0+v0)+r0n.dot(dr0*p0))*r0n.norm() ); + div -= ( r0n.dot(r0*p0+v0)*r0n.dot(dr0*n0)/r0n.norm() ); + div /= pow(r0n.norm(),2); + } + else if (param == q1a() || param == q1b() || param == q1c() || param == q1d()) { + + if (param == q1a()) dr1 = rotation_da(*q1a(), *q1b(), *q1c(), *q1d()); + else if (param == q1b()) dr1 = rotation_db(*q1a(), *q1b(), *q1c(), *q1d()); + else if (param == q1c()) dr1 = rotation_dc(*q1a(), *q1b(), *q1c(), *q1d()); + else if (param == q1d()) dr1 = rotation_dd(*q1a(), *q1b(), *q1c(), *q1d()); + + div = (r0*n0).dot(dr1*p1)/(r0*n0).norm(); + } + else if (param == p0x() || param == p0y() || param == p0z()) { + + Vector3d dp_g; + if (param == p0x()) dp_g << 1,0,0; + else if (param == p0y()) dp_g << 0,1,0; + else if (param == p0z()) dp_g << 0,0,1; + + div = -1*(r0*n0).dot(dp_g)/(r0*n0).norm(); + } + else if (param == p1x() || param == p1y() || param == p1z()) { + + Vector3d dp_g; + if (param == p1x()) dp_g << 1,0,0; + else if (param == p1y()) dp_g << 0,1,0; + else if (param == p1z()) dp_g << 0,0,1; + + div = (r0*n0).dot(dp_g)/(r0*n0).norm(); + } + else return 0; + + div *= 2*error; + return div; +} + +} //namespace GCS diff --git a/src/Mod/Assembly/App/gcs3d/Constraints.h b/src/Mod/Assembly/App/gcs3d/Constraints.h new file mode 100644 index 0000000000..f8b2b38c95 --- /dev/null +++ b/src/Mod/Assembly/App/gcs3d/Constraints.h @@ -0,0 +1,133 @@ +/*************************************************************************** + * Copyright (c) Konstantinos Poulios (logari81@gmail.com) 2011 * + * * + * This file is part of the FreeCAD CAx development system. * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of the GNU Library General Public * + * License as published by the Free Software Foundation; either * + * version 2 of the License, or (at your option) any later version. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Library General Public License for more details. * + * * + * You should have received a copy of the GNU Library General Public * + * License along with this library; see the file COPYING.LIB. If not, * + * write to the Free Software Foundation, Inc., 59 Temple Place, * + * Suite 330, Boston, MA 02111-1307, USA * + * * + ***************************************************************************/ + +#ifndef FREEGCS_CONSTRAINTS_H +#define FREEGCS_CONSTRAINTS_H + +#include "Geo.h" +#include "Util.h" +#include + +namespace GCS +{ + + /////////////////////////////////////// + // Constraints + /////////////////////////////////////// + + enum ConstraintType { + None = 0, + ASParallel, + ASDistance + }; + + enum ParallelType { + + NormalWhatever = 0, + NormalSame, + NormalOpposite + }; + + class Constraint + { + protected: + VEC_pD origpvec; // is used only as a reference for redirecting and reverting pvec + VEC_pD pvec; + double scale; + int tag; + public: + Constraint(); + + inline VEC_pD params() { return pvec; } + + void redirectParams(MAP_pD_pD redirectionmap); + void revertParams(); + void setTag(int tagId) { tag = tagId; } + int getTag() { return tag; } + + virtual ConstraintType getTypeId(); + virtual void rescale(double coef=1.); + virtual double error(); + virtual double grad(double *); + // virtual void grad(MAP_pD_D &deriv); --> TODO: vectorized grad version + virtual double maxStep(MAP_pD_D &dir, double lim=1.); + }; + + // AS_plane_parallel + class ConstraintParralelFaceAS : public Constraint + { + private: + inline double* q0a() { return pvec[0]; } + inline double* q0b() { return pvec[1]; } + inline double* q0c() { return pvec[2]; } + inline double* q0d() { return pvec[3]; } + inline double* q1a() { return pvec[4]; } + inline double* q1b() { return pvec[5]; } + inline double* q1c() { return pvec[6]; } + inline double* q1d() { return pvec[7]; } + + Eigen::Vector3d n0, n1; + ParallelType *type; + + public: + ConstraintParralelFaceAS(GCS::Solid& s0, GCS::Solid& s1, ParallelType *t); + + virtual ConstraintType getTypeId(); + virtual void rescale(double coef=1.); + virtual double error(); + virtual double grad(double *); + }; + + class ConstraintFaceDistanceAS : public Constraint + { + private: + inline double* p0x() { return pvec[0]; } + inline double* p0y() { return pvec[1]; } + inline double* p0z() { return pvec[2]; } + inline double* p1x() { return pvec[3]; } + inline double* p1y() { return pvec[4]; } + inline double* p1z() { return pvec[5]; } + inline double* q0a() { return pvec[6]; } + inline double* q0b() { return pvec[7]; } + inline double* q0c() { return pvec[8]; } + inline double* q0d() { return pvec[9]; } + inline double* q1a() { return pvec[10]; } + inline double* q1b() { return pvec[11]; } + inline double* q1c() { return pvec[12]; } + inline double* q1d() { return pvec[13]; } + + Eigen::Vector3d p0, p1; + Eigen::Vector3d n0; + + double *dist; + public: + ConstraintFaceDistanceAS(GCS::Solid& s0, GCS::Solid& s1, double *d); + virtual ConstraintType getTypeId(); + virtual void rescale(double coef=1.); + virtual double error(); + virtual double grad(double *); + }; + + +} //namespace GCS + +#endif // FREEGCS_CONSTRAINTS_H diff --git a/src/Mod/Assembly/App/gcs3d/Example/AS_ex1.gcx b/src/Mod/Assembly/App/gcs3d/Example/AS_ex1.gcx new file mode 100644 index 0000000000..8216a86e5e --- /dev/null +++ b/src/Mod/Assembly/App/gcs3d/Example/AS_ex1.gcx @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/Mod/Assembly/App/gcs3d/Example/AS_ex2.gcx b/src/Mod/Assembly/App/gcs3d/Example/AS_ex2.gcx new file mode 100644 index 0000000000..68e3d8c5db --- /dev/null +++ b/src/Mod/Assembly/App/gcs3d/Example/AS_ex2.gcx @@ -0,0 +1,61 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/Mod/Assembly/App/gcs3d/Example/AS_ex3.gcx b/src/Mod/Assembly/App/gcs3d/Example/AS_ex3.gcx new file mode 100644 index 0000000000..207e000674 --- /dev/null +++ b/src/Mod/Assembly/App/gcs3d/Example/AS_ex3.gcx @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/Mod/Assembly/App/gcs3d/Example/AS_ex4.gcx b/src/Mod/Assembly/App/gcs3d/Example/AS_ex4.gcx new file mode 100644 index 0000000000..5164d326eb --- /dev/null +++ b/src/Mod/Assembly/App/gcs3d/Example/AS_ex4.gcx @@ -0,0 +1,64 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/Mod/Assembly/App/gcs3d/Example/AS_ex5.gcx b/src/Mod/Assembly/App/gcs3d/Example/AS_ex5.gcx new file mode 100644 index 0000000000..d505e772ce --- /dev/null +++ b/src/Mod/Assembly/App/gcs3d/Example/AS_ex5.gcx @@ -0,0 +1,61 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/Mod/Assembly/App/gcs3d/Example/AS_ex6.gcx b/src/Mod/Assembly/App/gcs3d/Example/AS_ex6.gcx new file mode 100644 index 0000000000..c4ccdfa2c7 --- /dev/null +++ b/src/Mod/Assembly/App/gcs3d/Example/AS_ex6.gcx @@ -0,0 +1,64 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/Mod/Assembly/App/gcs3d/Example/AS_ex7.gcx b/src/Mod/Assembly/App/gcs3d/Example/AS_ex7.gcx new file mode 100644 index 0000000000..ab4cd68738 --- /dev/null +++ b/src/Mod/Assembly/App/gcs3d/Example/AS_ex7.gcx @@ -0,0 +1,64 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/Mod/Assembly/App/gcs3d/GCS.cpp b/src/Mod/Assembly/App/gcs3d/GCS.cpp new file mode 100644 index 0000000000..1a5fc76a03 --- /dev/null +++ b/src/Mod/Assembly/App/gcs3d/GCS.cpp @@ -0,0 +1,1139 @@ +/*************************************************************************** + * Copyright (c) Konstantinos Poulios (logari81@gmail.com) 2011 * + * * + * This file is part of the FreeCAD CAx development system. * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of the GNU Library General Public * + * License as published by the Free Software Foundation; either * + * version 2 of the License, or (at your option) any later version. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Library General Public License for more details. * + * * + * You should have received a copy of the GNU Library General Public * + * License along with this library; see the file COPYING.LIB. If not, * + * write to the Free Software Foundation, Inc., 59 Temple Place, * + * Suite 330, Boston, MA 02111-1307, USA * + * * + ***************************************************************************/ +#include +#include +#include + +#include "GCS.h" +#include "qp_eq.h" +#include + +#include +#include +#include + +namespace GCS +{ + + +/////////////////////////////////////// +// Solver +/////////////////////////////////////// + +// System +System::System() + : clist(0), + c2p(), p2c(), + subsys0(0), + subsys1(0), + subsys2(0), + reference(), + init(false) +{ +} + +System::System(std::vector clist_) + : c2p(), p2c(), + subsys0(0), + subsys1(0), + subsys2(0), + reference(), + init(false) +{ + // create own (shallow) copy of constraints + for (std::vector::iterator constr=clist_.begin(); + constr != clist_.end(); ++constr) { + Constraint *newconstr; + switch ((*constr)->getTypeId()) { + + case GCS::ASParallel: { + ConstraintParralelFaceAS *oldconstr = static_cast(*constr); + newconstr = new ConstraintParralelFaceAS(*oldconstr); + break; + } + case GCS::ASDistance: { + ConstraintFaceDistanceAS *oldconstr = static_cast(*constr); + newconstr = new ConstraintFaceDistanceAS(*oldconstr); + break; + } + case None: + break; + } + if (newconstr) + addConstraint(newconstr); + } +} + +System::~System() +{ + clear(); +} + +void System::clear() +{ + clearReference(); + clearSubSystems(); + free(clist); + c2p.clear(); + p2c.clear(); +} + +void System::clearByTag(int tagId) +{ + std::vector constrvec; + for (std::vector::const_iterator + constr=clist.begin(); constr != clist.end(); ++constr) { + if ((*constr)->getTag() == tagId) + constrvec.push_back(*constr); + } + for (std::vector::const_iterator + constr=constrvec.begin(); constr != constrvec.end(); ++constr) { + removeConstraint(*constr); + } +} + +int System::addConstraint(Constraint *constr) +{ + clearReference(); + + clist.push_back(constr); + VEC_pD constr_params = constr->params(); + for (VEC_pD::const_iterator param=constr_params.begin(); + param != constr_params.end(); ++param) { +// jacobi.set(constr, *param, 0.); + c2p[constr].push_back(*param); + p2c[*param].push_back(constr); + } + return clist.size()-1; +} + +void System::removeConstraint(Constraint *constr) +{ + clearReference(); + clearSubSystems(); + + std::vector::iterator it; + it = std::find(clist.begin(), clist.end(), constr); + clist.erase(it); + + VEC_pD constr_params = c2p[constr]; + for (VEC_pD::const_iterator param=constr_params.begin(); + param != constr_params.end(); ++param) { + std::vector &constraints = p2c[*param]; + it = std::find(constraints.begin(), constraints.end(), constr); + constraints.erase(it); + } + c2p.erase(constr); + + std::vector constrvec; + constrvec.push_back(constr); + free(constrvec); +} + + + +void System::initSolution(VEC_pD ¶ms) +{ + // - Stores the current parameters in the vector "reference" + // - Identifies the equality constraints tagged with ids >= 0 + // and prepares a corresponding system reduction + // - Organizes the rest of constraints into two subsystems for + // tag ids >=0 and < 0 respectively and applies the + // system reduction specified in the previous step + + clearReference(); + for (VEC_pD::const_iterator param=params.begin(); + param != params.end(); ++param) + reference[*param] = **param; + + // identification of equality constraints and parameter reduction + std::set eliminated; // constraints that will be eliminated through reduction + reductionmap.clear(); + { + VEC_pD reduced_params=params; + MAP_pD_I params_index; + for (int i=0; i < int(params.size()); ++i) + params_index[params[i]] = i; + + /* for (std::vector::const_iterator constr=clist.begin(); + constr != clist.end(); ++constr) { + if ((*constr)->getTag() >= 0 && (*constr)->getTypeId() == Equal) { + MAP_pD_I::const_iterator it1,it2; + it1 = params_index.find((*constr)->params()[0]); + it2 = params_index.find((*constr)->params()[1]); + if (it1 != params_index.end() && it2 != params_index.end()) { + eliminated.insert(*constr); + double *p_kept = reduced_params[it1->second]; + double *p_replaced = reduced_params[it2->second]; + for (int i=0; i < int(params.size()); ++i) + if (reduced_params[i] == p_replaced) + reduced_params[i] = p_kept; + } + } + }*/ + for (int i=0; i < int(params.size()); ++i) + if (params[i] != reduced_params[i]) + reductionmap[params[i]] = reduced_params[i]; + } + + int i=0; + std::vector clist0, clist1, clist2; + for (std::vector::const_iterator constr=clist.begin(); + constr != clist.end(); ++constr, i++) { + if (eliminated.count(*constr) == 0) { + if ((*constr)->getTag() >= 0) + clist0.push_back(*constr); + else if ((*constr)->getTag() == -1) // move constraints + clist1.push_back(*constr); + else // distance from reference constraints + clist2.push_back(*constr); + } + } + + clearSubSystems(); + if (clist0.size() > 0) + subsys0 = new SubSystem(clist0, params, reductionmap); + if (clist1.size() > 0) + subsys1 = new SubSystem(clist1, params, reductionmap); + if (clist2.size() > 0) + subsys2 = new SubSystem(clist2, params, reductionmap); + init = true; +} + +void System::clearReference() +{ + init = false; + reference.clear(); +} + +void System::resetToReference() +{ + for (MAP_pD_D::const_iterator it=reference.begin(); + it != reference.end(); ++it) + *(it->first) = it->second; +} + +int System::solve(VEC_pD ¶ms, bool isFine, Algorithm alg) +{ + initSolution(params); + return solve(isFine, alg); +} + +int System::solve(bool isFine, Algorithm alg) +{ + if (subsys0) { + resetToReference(); + if (subsys2) { + int ret = solve(subsys0, subsys2, isFine); + if (subsys1) // give subsys1 higher priority than subsys2 + // in this case subsys2 acts like a preconditioner + return solve(subsys0, subsys1, isFine); + else + return ret; + } + else if (subsys1) + return solve(subsys0, subsys1, isFine); + else + return solve(subsys0, isFine, alg); + } + else if (subsys1) { + resetToReference(); + if (subsys2) + return solve(subsys1, subsys2, isFine); + else + return solve(subsys1, isFine, alg); + } + else + // return success in order to permit coincidence constraints to be applied + return Success; +} + +int System::solve(SubSystem *subsys, bool isFine, Algorithm alg) +{ + + std::cout << std::endl << "Initial Error: " << subsys->error() << std::endl; + clock_t begin = clock(); + int ret; + if (alg == BFGS) + ret= solve_BFGS(subsys, isFine); + else if (alg == LevenbergMarquardt) + ret= solve_LM(subsys); + else if (alg == DogLeg) + ret= solve_DL(subsys); + else if (alg == HOPS) + ret= solve_EX(subsys); + + clock_t end = clock(); + std::cout << "Time elapsed: " << double( ((end-begin)*1000)/CLOCKS_PER_SEC ) << " ms"<< std::endl; + + return ret; +} + +int System::solve_EX(SubSystem* subsys) { + + + +} + + +int System::solve_BFGS(SubSystem *subsys, bool isFine) +{ + int xsize = subsys->pSize(); + if (xsize == 0) + return Success; + + subsys->redirectParams(); + + Eigen::MatrixXd D = Eigen::MatrixXd::Identity(xsize, xsize); + Eigen::VectorXd x(xsize); + Eigen::VectorXd xdir(xsize); + Eigen::VectorXd grad(xsize); + Eigen::VectorXd h(xsize); + Eigen::VectorXd y(xsize); + Eigen::VectorXd Dy(xsize); + + // Initial unknowns vector and initial gradient vector + subsys->getParams(x); + subsys->calcGrad(grad); + + // Initial search direction oposed to gradient (steepest-descent) + xdir = grad; + lineSearch(subsys, xdir); + double err = subsys->error(); + + h = x; + subsys->getParams(x); + h = x - h; // = x xold + + double convergence = isFine ? XconvergenceFine : XconvergenceRough; + int maxIterNumber = MaxIterations * xsize; + double diverging_lim = 1e6*err + 1e12; + + for (int iter=1; iter < maxIterNumber; iter++) { + + if (h.norm() <= convergence || err <= smallF) + break; + if (err > diverging_lim || err != err) // check for diverging and NaN + break; + + y = grad; + subsys->calcGrad(grad); + y = grad - y; // = grad gradold + + double hty = h.dot(y); + //make sure that hty is never 0 + if (hty == 0) + hty = .0000000001; + + Dy = D * y; + + double ytDy = y.dot(Dy); + + //Now calculate the BFGS update on D + D += (1.+ytDy/hty)/hty * h * h.transpose(); + D -= 1./hty * (h * Dy.transpose() + Dy * h.transpose()); + + xdir = D * grad; + lineSearch(subsys, xdir); + err = subsys->error(); + + h = x; + subsys->getParams(x); + h = x - h; // = x xold + } + + subsys->revertParams(); + + std::cout << "Error after BFGS solving: " << err << std::endl; + + if (err <= smallF) + return Success; + if (h.norm() <= convergence) + return Converged; + return Failed; +} + +int System::solve_LM(SubSystem* subsys) +{ + int xsize = subsys->pSize(); + int csize = subsys->cSize(); + + if (xsize == 0) + return Success; + + Eigen::VectorXd e(csize), e_new(csize); // vector of all function errors (every constraint is one function) + Eigen::MatrixXd J(csize, xsize); // Jacobi of the subsystem + Eigen::MatrixXd A(xsize, xsize); + Eigen::VectorXd x(xsize), h(xsize), x_new(xsize), g(xsize), diag_A(xsize); + + subsys->redirectParams(); + + subsys->getParams(x); + subsys->calcResidual(e); + e*=-1; + + int maxIterNumber = MaxIterations * xsize; + double diverging_lim = 1e6*e.squaredNorm() + 1e12; + + double eps=1e-10, eps1=1e-80; + double tau=1e-3; + double nu=2, mu=0; + int iter=0, stop=0; + for (iter=0; iter < maxIterNumber && !stop; ++iter) { + + // check error + double err=e.squaredNorm(); + if (err <= eps) { // error is small, Success + stop = 1; + break; + } + else if (err > diverging_lim || err != err) { // check for diverging and NaN + stop = 6; + break; + } + + // J^T J, J^T e + subsys->calcJacobi(J);; + + A = J.transpose()*J; + g = J.transpose()*e; + + // Compute ||J^T e||_inf + double g_inf = g.lpNorm(); + diag_A = A.diagonal(); // save diagonal entries so that augmentation can be later canceled + + // check for convergence + if (g_inf <= eps1) { + stop = 2; + break; + } + + // compute initial damping factor + if (iter == 0) + mu = tau * A.diagonal().lpNorm(); + + // determine increment using adaptive damping + while (1) { + // augment normal equations A = A+uI + for (int i=0; i < xsize; ++i) + A(i,i) += mu; + + //solve augmented functions A*h=-g + h = A.fullPivLu().solve(g); + double rel_error = (A*h - g).norm() / g.norm(); + + // check if solving works + if (rel_error < 1e-5) { +/* + double scale = subsys->maxStep() / h.lpNorm(); + if ( scale < 1.) + h *= scale; +*/ + // compute par's new estimate and ||d_par||^2 + x_new = x + h; + double h_norm = h.squaredNorm(); + + if (h_norm <= eps1*eps1*x.norm()) { // relative change in p is small, stop + stop = 3; + break; + } + else if (h_norm >= (x.norm()+eps1)/(DBL_EPSILON*DBL_EPSILON)) { // almost singular + stop = 4; + break; + } + + subsys->setParams(x_new); + subsys->calcResidual(e_new); + e_new *= -1; + + double dF = e.squaredNorm() - e_new.squaredNorm(); + double dL = h.dot(mu*h+g); + + if (dF>0. && dL>0.) { // reduction in error, increment is accepted + double tmp=2*dF/dL-1.; + mu *= std::max(1./3., 1.-tmp*tmp*tmp); + nu=2; + + // update par's estimate + x = x_new; + e = e_new; + break; + } + } + + // if this point is reached, either the linear system could not be solved or + // the error did not reduce; in any case, the increment must be rejected + + mu*=nu; + nu*=2.0; + for (int i=0; i < xsize; ++i) // restore diagonal J^T J entries + A(i,i) = diag_A(i); + } + } + + if (iter >= maxIterNumber) + stop = 5; + + std::cout << "Error after LM solving: " << subsys->error() << std::endl; + + subsys->revertParams(); + + return (stop == 1) ? Success : Failed; +} + + +int System::solve_DL(SubSystem* subsys) +{ + double tolg=1e-80, tolx=1e-80, tolf=1e-10; + + int xsize = subsys->pSize(); + int csize = subsys->cSize(); + + Eigen::VectorXd x(xsize), x_new(xsize); + Eigen::VectorXd fx(csize), fx_new(csize); + Eigen::MatrixXd Jx(csize, xsize), Jx_new(csize, xsize); + Eigen::VectorXd g(xsize), h_sd(xsize), h_gn(xsize), h_dl(xsize); + + subsys->redirectParams(); + + double err; + subsys->getParams(x); + subsys->calcResidual(fx, err); + subsys->calcJacobi(Jx); + + g = Jx.transpose()*(-fx); + + // get the infinity norm fx_inf and g_inf + double g_inf = g.lpNorm(); + double fx_inf = fx.lpNorm(); + + int maxIterNumber = MaxIterations * xsize; + double diverging_lim = 1e6*err + 1e12; + + double delta=0.1; + double alpha=0.; + double nu=2.; + int iter=0, stop=0, reduce=0; + while (!stop) { + + // check if finished + if (fx_inf <= tolf) // Success + stop = 1; + else if (g_inf <= tolg) + stop = 2; + else if (delta <= tolx*(tolx + x.norm())) + stop = 3; + else if (iter >= maxIterNumber) + stop = 4; + else if (err > diverging_lim || err != err) { // check for diverging and NaN + stop = 6; + } + else { + // get the steepest descent direction + alpha = g.squaredNorm()/(Jx*g).squaredNorm(); + h_sd = alpha*g; + + // get the gauss-newton step + h_gn = Jx.fullPivLu().solve(-fx); + double rel_error = (Jx*h_gn + fx).norm() / fx.norm(); + if (rel_error > 1e15) + break; + + // compute the dogleg step + if (h_gn.norm() < delta) { + h_dl = h_gn; + if (h_dl.norm() <= tolx*(tolx + x.norm())) { + stop = 5; + break; + } + } + else if (alpha*g.norm() >= delta) { + h_dl = (delta/(alpha*g.norm()))*h_sd; + } + else { + //compute beta + double beta = 0; + Eigen::VectorXd b = h_gn - h_sd; + double bb = (b.transpose()*b).norm(); + double gb = (h_sd.transpose()*b).norm(); + double c = (delta + h_sd.norm())*(delta - h_sd.norm()); + + if (gb > 0) + beta = c / (gb + sqrt(gb * gb + c * bb)); + else + beta = (sqrt(gb * gb + c * bb) - gb)/bb; + + // and update h_dl and dL with beta + h_dl = h_sd + beta*b; + } + } + + // see if we are already finished + if (stop) + break; +/* + double scale = subsys->maxStep() / h_dl.lpNorm(); + if ( scale < 1.) + h_dl *= scale; +*/ + // get the new values + double err_new; + x_new = x + h_dl; + subsys->setParams(x_new); + subsys->calcResidual(fx_new, err_new); + subsys->calcJacobi(Jx_new); + + // calculate the linear model and the update ratio + double dL = err - 0.5*(fx + Jx*h_dl).squaredNorm(); + double dF = err - err_new; + double rho = dL/dF; + + if (dF > 0 && dL > 0) { + x = x_new; + Jx = Jx_new; + fx = fx_new; + err = err_new; + + g = Jx.transpose()*(-fx); + + // get infinity norms + g_inf = g.lpNorm(); + fx_inf = fx.lpNorm(); + } + else + rho = -1; + + // update delta + if (fabs(rho-1.) < 0.2 && h_dl.norm() > delta/3. && reduce <= 0) { + delta = 3*delta; + nu = 2; + reduce = 0; + } + else if (rho < 0.25) { + delta = delta/nu; + nu = 2*nu; + reduce = 2; + } + else + reduce--; + + // count this iteration and start again + iter++; + } + + std::cout << "Error after DogLeg solving: " << subsys->error() << std::endl; + + + subsys->revertParams(); + + return (stop == 1) ? Success : Failed; +} + +// The following solver variant solves a system compound of two subsystems +// treating the first of them as of higher priority than the second +int System::solve(SubSystem *subsysA, SubSystem *subsysB, bool isFine) +{ + int xsizeA = subsysA->pSize(); + int xsizeB = subsysB->pSize(); + int csizeA = subsysA->cSize(); + + VEC_pD plist(xsizeA+xsizeB); + { + VEC_pD plistA, plistB; + subsysA->getParamList(plistA); + subsysB->getParamList(plistB); + + std::sort(plistA.begin(),plistA.end()); + std::sort(plistB.begin(),plistB.end()); + + VEC_pD::const_iterator it; + it = std::set_union(plistA.begin(),plistA.end(), + plistB.begin(),plistB.end(),plist.begin()); + plist.resize(it-plist.begin()); + } + int xsize = plist.size(); + + Eigen::MatrixXd B = Eigen::MatrixXd::Identity(xsize, xsize); + Eigen::MatrixXd JA(csizeA, xsize); + Eigen::MatrixXd Y,Z; + + Eigen::VectorXd resA(csizeA); + Eigen::VectorXd lambda(csizeA), lambda0(csizeA), lambdadir(csizeA); + Eigen::VectorXd x(xsize), x0(xsize), xdir(xsize), xdir1(xsize); + Eigen::VectorXd grad(xsize); + Eigen::VectorXd h(xsize); + Eigen::VectorXd y(xsize); + Eigen::VectorXd Bh(xsize); + + // We assume that there are no common constraints in subsysA and subsysB + subsysA->redirectParams(); + subsysB->redirectParams(); + + subsysB->getParams(plist,x); + subsysA->getParams(plist,x); + subsysB->setParams(plist,x); // just to ensure that A and B are synchronized + + subsysB->calcGrad(plist,grad); + subsysA->calcJacobi(plist,JA); + subsysA->calcResidual(resA); + + double convergence = isFine ? XconvergenceFine : XconvergenceRough; + int maxIterNumber = MaxIterations * xsize; + double diverging_lim = 1e6*subsysA->error() + 1e12; + + double mu = 0; + lambda.setZero(); + for (int iter=1; iter < maxIterNumber; iter++) { + int status = qp_eq(B, grad, JA, resA, xdir, Y, Z); + if (status) + break; + + x0 = x; + lambda0 = lambda; + lambda = Y.transpose() * (B * xdir + grad); + lambdadir = lambda - lambda0; + + // line search + { + double eta=0.25; + double tau=0.5; + double rho=0.5; + double alpha=1; + alpha = std::min(alpha, subsysA->maxStep(plist,xdir)); + + // Eq. 18.32 + // double mu = lambda.lpNorm() + 0.01; + // Eq. 18.33 + // double mu = grad.dot(xdir) / ( (1.-rho) * resA.lpNorm<1>()); + // Eq. 18.36 + mu = std::max(mu, + (grad.dot(xdir) + std::max(0., 0.5*xdir.dot(B*xdir))) / + ( (1. - rho) * resA.lpNorm<1>() ) ); + + // Eq. 18.27 + double f0 = subsysB->error() + mu * resA.lpNorm<1>(); + + // Eq. 18.29 + double deriv = grad.dot(xdir) - mu * resA.lpNorm<1>(); + + x = x0 + alpha * xdir; + subsysA->setParams(plist,x); + subsysB->setParams(plist,x); + subsysA->calcResidual(resA); + double f = subsysB->error() + mu * resA.lpNorm<1>(); + + // line search, Eq. 18.28 + bool first = true; + while (f > f0 + eta * alpha * deriv) { + if (first) { // try a second order step + // xdir1 = JA.jacobiSvd(Eigen::ComputeThinU | + // Eigen::ComputeThinV).solve(-resA); + xdir1 = -Y*resA; + x += xdir1; // = x0 + alpha * xdir + xdir1 + subsysA->setParams(plist,x); + subsysB->setParams(plist,x); + subsysA->calcResidual(resA); + f = subsysB->error() + mu * resA.lpNorm<1>(); + if (f < f0 + eta * alpha * deriv) + break; + } + alpha = tau * alpha; + x = x0 + alpha * xdir; + subsysA->setParams(plist,x); + subsysB->setParams(plist,x); + subsysA->calcResidual(resA); + f = subsysB->error() + mu * resA.lpNorm<1>(); + } + lambda = lambda0 + alpha * lambdadir; + + } + h = x - x0; + + y = grad - JA.transpose() * lambda; + { + subsysB->calcGrad(plist,grad); + subsysA->calcJacobi(plist,JA); + subsysA->calcResidual(resA); + } + y = grad - JA.transpose() * lambda - y; // Eq. 18.13 + + if (iter > 1) { + double yTh = y.dot(h); + if (yTh != 0) { + Bh = B * h; + //Now calculate the BFGS update on B + B += 1./yTh * y * y.transpose(); + B -= 1./h.dot(Bh) * (Bh * Bh.transpose()); + } + } + + double err = subsysA->error(); + if (h.norm() <= convergence && err <= smallF) + break; + if (err > diverging_lim || err != err) // check for diverging and NaN + break; + } + + int ret; + if (subsysA->error() <= smallF) + ret = Success; + else if (h.norm() <= convergence) + ret = Converged; + else + ret = Failed; + + subsysA->revertParams(); + subsysB->revertParams(); + return ret; + +} + +void System::getSubSystems(std::vector &subsysvec) +{ + subsysvec.clear(); + if (subsys0) + subsysvec.push_back(subsys0); + if (subsys1) + subsysvec.push_back(subsys1); + if (subsys2) + subsysvec.push_back(subsys2); +} + +void System::applySolution() +{ + if (subsys2) + subsys2->applySolution(); + if (subsys1) + subsys1->applySolution(); + if (subsys0) + subsys0->applySolution(); + + for (MAP_pD_pD::const_iterator it=reductionmap.begin(); + it != reductionmap.end(); ++it) + *(it->first) = *(it->second); +} + +int System::diagnose(VEC_pD ¶ms, VEC_I &conflicting) +{ + // Analyses the constrainess grad of the system and provides feedback + // The vector "conflicting" will hold a group of conflicting constraints + conflicting.clear(); + std::vector conflictingIndex; + VEC_I tags; + Eigen::MatrixXd J(clist.size(), params.size()); + int count=0; + for (std::vector::iterator constr=clist.begin(); + constr != clist.end(); ++constr) { + (*constr)->revertParams(); + if ((*constr)->getTag() >= 0) { + count++; + tags.push_back((*constr)->getTag()); + for (int j=0; j < int(params.size()); j++) + J(count-1,j) = (*constr)->grad(params[j]); + } + } + + if (J.rows() > 0) { + Eigen::FullPivHouseholderQR qrJT(J.topRows(count).transpose()); + Eigen::MatrixXd Q = qrJT.matrixQ (); + int params_num = qrJT.rows(); + int constr_num = qrJT.cols(); + int rank = qrJT.rank(); + + Eigen::MatrixXd R; + if (constr_num >= params_num) + R = qrJT.matrixQR().triangularView(); + else + R = qrJT.matrixQR().topRows(constr_num) + .triangularView(); + + if (constr_num > rank) { // conflicting constraints + for (int i=1; i < rank; i++) { + // eliminate non zeros above pivot + assert(R(i,i) != 0); + for (int row=0; row < i; row++) { + if (R(row,i) != 0) { + double coef=R(row,i)/R(i,i); + R.block(row,i+1,1,constr_num-i-1) -= coef * R.block(i,i+1,1,constr_num-i-1); + R(row,i) = 0; + } + } + } + conflictingIndex.resize(constr_num-rank); + for (int j=rank; j < constr_num; j++) { + for (int row=0; row < rank; row++) { + if (R(row,j) != 0) { + int orig_col = qrJT.colsPermutation().indices()[row]; + conflictingIndex[j-rank].push_back(orig_col); + } + } + int orig_col = qrJT.colsPermutation().indices()[j]; + conflictingIndex[j-rank].push_back(orig_col); + } + + SET_I tags_set; + for (int i=0; i < conflictingIndex.size(); i++) { + for (int j=0; j < conflictingIndex[i].size(); j++) { + tags_set.insert(tags[conflictingIndex[i][j]]); + } + } + tags_set.erase(0); // exclude constraints tagged with zero + conflicting.resize(tags_set.size()); + std::copy(tags_set.begin(), tags_set.end(), conflicting.begin()); + + if (params_num == rank) // over-constrained + return params_num - constr_num; + } + + return params_num - rank; + } + return params.size(); +} + +void System::clearSubSystems() +{ + init = false; + std::vector subsystems; + getSubSystems(subsystems); + free(subsystems); + subsys0 = NULL; + subsys1 = NULL; + subsys2 = NULL; +} + + +//calculates the wolfe condition test functions p and g +Eigen::Vector2d PG(SubSystem* sys, Eigen::VectorXd xdir, double sigma) { + + int xsize = sys->pSize(); + Eigen::Vector2d PG; + Eigen::VectorXd x(xsize), x_new(xsize); + Eigen::VectorXd gr(xsize), grn(xsize); + double fx, fxn; + + sys->getParams(x); + fx = sys->error(); + sys->calcGrad(gr); + x_new = x-sigma*xdir; + sys->setParams(x_new); + fxn = sys->error(); + sys->calcGrad(grn); + sys->setParams(x); + + if (sigma == 0) + PG(1) = 1; + else + PG(1) = (fx-fxn) / (sigma * gr.transpose()*xdir).norm(); + + PG(0) = (grn.transpose()*xdir).norm() / (gr.transpose()*xdir).norm(); + + return PG; +} + + +double lineSearch(SubSystem *subsys, Eigen::VectorXd &xdir) +{ + double alpha, beta; + int xsize = subsys->pSize(); + + + + //get inital alpha and beta interval + //********************************** + double c3 = 0.1, c4 = 5; + double delta = 0.01, kappa = 0.9, mu = 0.8, sigma, gamma; + double fx, fxmd; + bool run = true; + + Eigen::VectorXd x(xsize), x_new(xsize); + Eigen::VectorXd grad(xsize); + + + subsys->getParams(x); + subsys->calcGrad(grad); + fx = subsys->error(); + x_new = x-xdir; + subsys->setParams(x_new); + fxmd= subsys->error(); + subsys->setParams(x); + + //calculate an guess for sigma + double comp1 = (grad.transpose()*xdir).norm() / std::pow(xdir.norm(),2); + double comp2 = (grad.transpose()*xdir).norm() /(2*(fxmd-fx+(grad.transpose()*xdir).norm())); + sigma = std::min(c4*comp1 , std::max(c3*comp1, comp2)); + + //calculate test functions with guess + Eigen::Vector2d pg = PG(subsys, xdir, sigma); + + //see if we are alread fisished + if ( pg(1)>=delta && pg(0) <= kappa) + run = false; + else if ( pg(1)>=delta && pg(0)>kappa) { + alpha = sigma; + //beta is sigma/mu^j with j minimal so that g(beta)=delta and p(alpha)>=kappa (j element natural numbers) + for (int i=1; i<=1000; i++) { + alpha = sigma*(std::pow(mu, i)); + pg = PG(subsys, xdir, alpha); + if (pg(0)>=kappa && pg(1) >= delta) break; + } + } + + //start sigma search in interval alpha beta + //***************************************** + while (run) { + + gamma = (alpha + beta)/2.; + pg = PG(subsys, xdir, gamma); + + if (pg(0) > kappa && pg(1) >= delta) alpha = gamma; + else if (pg(0) <= kappa && pg(1) >= delta) { + sigma = gamma; + run = false; + } + else beta = gamma; + + if (alpha == beta) break; + } + + x_new = x - sigma* xdir; + subsys->setParams(x_new); + + std::cout << "sigma: "<maxStep(xdir); + + Eigen::VectorXd x0, x; + + //Save initial values + subsys->getParams(x0); + + //Start at the initial position alpha1 = 0 + alpha1 = 0.; + f1 = subsys->error(); + + //Take a step of alpha2 = 1 + alpha2 = 1.; + x = x0 + alpha2 * xdir; + subsys->setParams(x); + f2 = subsys->error(); + + //Take a step of alpha3 = 2*alpha2 + alpha3 = alpha2*2; + x = x0 + alpha3 * xdir; + subsys->setParams(x); + f3 = subsys->error(); + + //Now reduce or lengthen alpha2 and alpha3 until the minimum is + //Bracketed by the triplet f1>f2 f1 || f2 > f3) { + if (f2 > f1) { + //If f2 is greater than f1 then we shorten alpha2 and alpha3 closer to f1 + //Effectively both are shortened by a factor of two. + alpha3 = alpha2; + f3 = f2; + alpha2 = alpha2 / 2; + x = x0 + alpha2 * xdir; + subsys->setParams(x); + f2 = subsys->error(); + } + else if (f2 > f3) { + if (alpha3 >= alphaMax) + break; + //If f2 is greater than f3 then we increase alpha2 and alpha3 away from f1 + //Effectively both are lengthened by a factor of two. + alpha2 = alpha3; + f2 = f3; + alpha3 = alpha3 * 2; + x = x0 + alpha3 * xdir; + subsys->setParams(x); + f3 = subsys->error(); + } + } + //Get the alpha for the minimum f of the quadratic approximation + alphaStar = alpha2 + ((alpha2-alpha1)*(f1-f3))/(3*(f1-2*f2+f3)); + + //Guarantee that the new alphaStar is within the bracket + if (alphaStar >= alpha3 || alphaStar <= alpha1) + alphaStar = alpha2; + + if (alphaStar > alphaMax) + alphaStar = alphaMax; + + if (alphaStar != alphaStar) + alphaStar = 0.; + + //Take a final step to alphaStar + x = x0 + alphaStar * xdir; + subsys->setParams(x); + + return alphaStar; +}*/ + + +void free(VEC_pD &doublevec) +{ + for (VEC_pD::iterator it = doublevec.begin(); + it != doublevec.end(); ++it) + if (*it) delete *it; + doublevec.clear(); +} + +void free(std::vector &constrvec) +{ + for (std::vector::iterator constr=constrvec.begin(); + constr != constrvec.end(); ++constr) { + if (*constr) { + switch ((*constr)->getTypeId()) { + + case None: + default: + delete *constr; + } + } + } + constrvec.clear(); +} + +void free(std::vector &subsysvec) +{ + for (std::vector::iterator it=subsysvec.begin(); + it != subsysvec.end(); ++it) + if (*it) delete *it; +} + +} //namespace GCS diff --git a/src/Mod/Assembly/App/gcs3d/GCS.h b/src/Mod/Assembly/App/gcs3d/GCS.h new file mode 100644 index 0000000000..c4a2ac671d --- /dev/null +++ b/src/Mod/Assembly/App/gcs3d/GCS.h @@ -0,0 +1,122 @@ +/*************************************************************************** + * Copyright (c) Konstantinos Poulios (logari81@gmail.com) 2011 * + * * + * This file is part of the FreeCAD CAx development system. * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of the GNU Library General Public * + * License as published by the Free Software Foundation; either * + * version 2 of the License, or (at your option) any later version. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Library General Public License for more details. * + * * + * You should have received a copy of the GNU Library General Public * + * License along with this library; see the file COPYING.LIB. If not, * + * write to the Free Software Foundation, Inc., 59 Temple Place, * + * Suite 330, Boston, MA 02111-1307, USA * + * * + ***************************************************************************/ + +#ifndef FREEGCS_GCS_H +#define FREEGCS_GCS_H + +#include "SubSystem.h" + + +namespace GCS { + + + +/////////////////////////////////////// +// Solver +/////////////////////////////////////// + +enum SolveStatus { + Success = 0, // Found a solution zeroing the error function + Converged = 1, // Found a solution minimizing the error function + Failed = 2 // Failed to find any solution +}; + +enum Algorithm { + BFGS = 0, + LevenbergMarquardt = 1, + DogLeg = 2, + HOPS, +}; + +class System { + // This is the main class. It holds all constraints and information + // about partitioning into subsystems and solution strategies +private: + std::vector clist; + + std::map c2p; // constraint to parameter adjacency list + std::map > p2c; // parameter to constraint adjacency list + + SubSystem *subsys0; // has the highest priority, always used as the primary subsystem + SubSystem *subsys1; // normally used as secondary subsystem, it is considered primary only if subsys0 is missing + SubSystem *subsys2; // has the lowest priority, always used as secondary system + void clearSubSystems(); + + MAP_pD_D reference; + void clearReference(); + void resetToReference(); + + MAP_pD_pD reductionmap; // for simplification of equality constraints + + bool init; + + int solve_BFGS ( SubSystem *subsys, bool isFine ); + int solve_LM ( SubSystem *subsys ); + int solve_DL ( SubSystem *subsys ); + int solve_EX ( SubSystem *subsys ); +public: + System(); + System ( std::vector clist_ ); + ~System(); + + void clear(); + void clearByTag ( int tagId ); + + int addConstraint ( Constraint *constr ); + void removeConstraint ( Constraint *constr ); + + void initSolution ( VEC_pD ¶ms ); + + int solve ( bool isFine=true, Algorithm alg=DogLeg ); + int solve ( VEC_pD ¶ms, bool isFine=true, Algorithm alg=DogLeg ); + int solve ( SubSystem *subsys, bool isFine=true, Algorithm alg=DogLeg ); + int solve ( SubSystem *subsysA, SubSystem *subsysB, bool isFine=true ); + + void getSubSystems ( std::vector &subsysvec ); + void applySolution(); + + bool isInit() const { + return init; + } + + int diagnose ( VEC_pD ¶ms, VEC_I &conflicting ); +}; + +/////////////////////////////////////// +// BFGS Solver parameters +/////////////////////////////////////// +#define XconvergenceRough 1e-8 +#define XconvergenceFine 1e-10 +#define smallF 1e-20 +#define MaxIterations 100 //Note that the total number of iterations allowed is MaxIterations *xLength + +/////////////////////////////////////// +// Helper elements +/////////////////////////////////////// + +void free ( VEC_pD &doublevec ); +void free ( std::vector &constrvec ); +void free ( std::vector &subsysvec ); + +} //namespace GCS + +#endif // FREEGCS_GCS_H diff --git a/src/Mod/Assembly/App/gcs3d/Geo.h b/src/Mod/Assembly/App/gcs3d/Geo.h new file mode 100644 index 0000000000..c7629b51f0 --- /dev/null +++ b/src/Mod/Assembly/App/gcs3d/Geo.h @@ -0,0 +1,67 @@ +/*************************************************************************** + * Copyright (c) Konstantinos Poulios (logari81@gmail.com) 2011 * + * * + * This file is part of the FreeCAD CAx development system. * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of the GNU Library General Public * + * License as published by the Free Software Foundation; either * + * version 2 of the License, or (at your option) any later version. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Library General Public License for more details. * + * * + * You should have received a copy of the GNU Library General Public * + * License along with this library; see the file COPYING.LIB. If not, * + * write to the Free Software Foundation, Inc., 59 Temple Place, * + * Suite 330, Boston, MA 02111-1307, USA * + * * + ***************************************************************************/ + +#ifndef FREEGCS_GEO_H +#define FREEGCS_GEO_H + +namespace GCS +{ + + class Displacement { + + public: + Displacement(){x = 0; y = 0; z = 0;} + double *x, *y, *z; + }; + + class Point { + + public: + Point() { x=y=z=0;} + double x,y,z; + }; + + class Normal : public Point { + + public: + Normal() {}; + }; + + class Quaternion { + + public: + Quaternion() {a = b = c = d = 0;}; + double *a, *b, *c, *d; + }; + + class Solid { + + public: + Solid() {}; + Quaternion q; + Displacement d; + Normal n; + Point p; + }; +} //namespace GCS + +#endif // FREEGCS_GEO_H diff --git a/src/Mod/Assembly/App/gcs3d/InputParser.cpp b/src/Mod/Assembly/App/gcs3d/InputParser.cpp new file mode 100644 index 0000000000..43f47f59ec --- /dev/null +++ b/src/Mod/Assembly/App/gcs3d/InputParser.cpp @@ -0,0 +1,359 @@ +#include +#include + +#include "InputParser.h" + +#include + + + +InputParser::InputParser() +{ + try { + xercesc::XMLPlatformUtils::Initialize(); // Initialize Xerces infrastructure + } + catch (xercesc::XMLException& e) { + char* message = xercesc::XMLString::transcode(e.getMessage()); + std::cout << "XML toolkit initialization error: " << message << std::endl; + xercesc::XMLString::release(&message); + } + + // Tags and attributes used in XML file. + // Can't call transcode till after Xerces Initialize() + TAG_points = xercesc::XMLString::transcode("points"); + TAG_point = xercesc::XMLString::transcode("point"); + TAG_constraints= xercesc::XMLString::transcode("constraints"); + TAG_constraint = xercesc::XMLString::transcode("constraint"); + ATTR_id = xercesc::XMLString::transcode("id"); + ATTR_x = xercesc::XMLString::transcode("x"); + ATTR_y = xercesc::XMLString::transcode("y"); + ATTR_z = xercesc::XMLString::transcode("z"); + ATTR_point = xercesc::XMLString::transcode("point"); + ATTR_kind = xercesc::XMLString::transcode("kind"); + ATTR_distance = xercesc::XMLString::transcode("distance"); + + TAG_GCS_AS = xercesc::XMLString::transcode("GCS_AS"); + TAG_quaternion = xercesc::XMLString::transcode("quaternion"); + TAG_quaternions = xercesc::XMLString::transcode("quaternions"); + TAG_normal = xercesc::XMLString::transcode("normal"); + TAG_normals = xercesc::XMLString::transcode("normals"); + TAG_displacement = xercesc::XMLString::transcode("displacement"); + TAG_displacements = xercesc::XMLString::transcode("displacements"); + TAG_solid = xercesc::XMLString::transcode("solid"); + TAG_solids = xercesc::XMLString::transcode("solids"); + KIND_Parallel_AS = xercesc::XMLString::transcode("Parallel_AS"); + KIND_Distance_AS = xercesc::XMLString::transcode("Distance_AS"); + ATTR_a = xercesc::XMLString::transcode("a"); + ATTR_b= xercesc::XMLString::transcode("b"); + ATTR_c = xercesc::XMLString::transcode("c"); + ATTR_d = xercesc::XMLString::transcode("d"); + ATTR_type = xercesc::XMLString::transcode("type"); + ATTR_quaternion = xercesc::XMLString::transcode("quaternion"); + ATTR_normal = xercesc::XMLString::transcode("normal"); + ATTR_displacement = xercesc::XMLString::transcode("displacement"); + ATTR_solid1 = xercesc::XMLString::transcode("solid1"); + ATTR_solid2 = xercesc::XMLString::transcode("solid2"); + + + m_InputFileParser = new xercesc::XercesDOMParser; +} + +InputParser::~InputParser() +{ + // Free memory + delete m_InputFileParser; +} + +bool InputParser::readInputFileAS(std::string &fileName, + std::vector &variables, + std::vector ¶meters, + std::vector &points, + std::vector &norms, + std::vector &disps, + std::vector &quats, + std::vector &solids, + std::vector &constraints) +{ + int pointsOffset = points.size(); + int normsOffset = norms.size(); + int dispsOffset = disps.size(); + int quatsOffset = quats.size(); + int solidsOffset = solids.size(); + + // Test to see if the file is ok. + struct stat fileStatus; + if (stat(fileName.c_str(), &fileStatus) != 0) { + std::cout << "Error reading the file " << fileName << std::endl; + return false; + } + + // Configure DOM parser. + m_InputFileParser->setValidationScheme(xercesc::XercesDOMParser::Val_Never); + m_InputFileParser->setDoNamespaces(false); + m_InputFileParser->setDoSchema(false); + m_InputFileParser->setLoadExternalDTD(false); + + try { + m_InputFileParser->parse(fileName.c_str()); + + // no need to free this pointer - owned by the parent parser object + xercesc::DOMDocument* xmlDoc = m_InputFileParser->getDocument(); + + // Get the top-level element: Name is "GCS_3D". No attributes for "GCS_3D" + xercesc::DOMElement* elementRoot = xmlDoc->getDocumentElement(); + if (!elementRoot) { + std::cout << "empty XML document" << std::endl; + return false; + } + else if (!xercesc::XMLString::equals(elementRoot->getTagName(), TAG_GCS_AS)) { + std::cout << "wrong root element in the XML file" << std::endl; + return false; + } + + // Parse XML file for tags of interest: "ApplicationSettings" + // Look one level nested within "root". (child of root) + xercesc::DOMNodeList* children = elementRoot->getChildNodes(); + const XMLSize_t nodeCount = children->getLength(); + + // Points + xercesc::DOMNodeList* pointsNodes = elementRoot->getElementsByTagName(TAG_points); + const XMLSize_t pointsCount = pointsNodes->getLength(); + + for (XMLSize_t i = 0; i < pointsCount; ++i) { + xercesc::DOMNode* pointsNode = pointsNodes->item(i); + xercesc::DOMElement* pointsElement = dynamic_cast(pointsNode); + + xercesc::DOMNodeList* pointNodes = pointsElement->getElementsByTagName(TAG_point); + const XMLSize_t pointCount = pointNodes->getLength(); + + for (XMLSize_t j = 0; j < pointCount; ++j) { + xercesc::DOMNode* pointNode = pointNodes->item(j); + xercesc::DOMElement* pointElement = dynamic_cast(pointNode); + + int id = getIntAttr(pointElement, ATTR_id); + double x = getDoubleAttr(pointElement, ATTR_x); + double y = getDoubleAttr(pointElement, ATTR_y); + double z = getDoubleAttr(pointElement, ATTR_z); + + if (id + pointsOffset > points.size()) + points.resize(id + pointsOffset); + + GCS::Point p; + p.x = x; + p.y = y; + p.z = z; + points[id + pointsOffset -1] = p; + } + } + + // normals + xercesc::DOMNodeList* normsNodes = elementRoot->getElementsByTagName(TAG_normals); + const XMLSize_t normsCount = normsNodes->getLength(); + + for (XMLSize_t i = 0; i < normsCount; ++i) { + xercesc::DOMNode* normsNode = normsNodes->item(i); + xercesc::DOMElement* normsElement = dynamic_cast(normsNode); + + xercesc::DOMNodeList* lineNodes = normsElement->getElementsByTagName(TAG_normal); + const XMLSize_t lineCount = lineNodes->getLength(); + + for (XMLSize_t j = 0; j < lineCount; ++j) { + xercesc::DOMNode* normNode = lineNodes->item(j); + xercesc::DOMElement* normElement = dynamic_cast(normNode); + + int id = getIntAttr(normElement, ATTR_id); + double x = getDoubleAttr(normElement, ATTR_x); + double y = getDoubleAttr(normElement, ATTR_y); + double z = getDoubleAttr(normElement, ATTR_z); + + if (id + normsOffset > norms.size()) + norms.resize(id + normsOffset); + + GCS::Normal p; + p.x = x; + p.y = y; + p.z = z; + norms[id + normsOffset -1] = p; + } + } + + // Displacements + xercesc::DOMNodeList* dispsNodes = elementRoot->getElementsByTagName(TAG_displacements); + const XMLSize_t dispsCount = dispsNodes->getLength(); + + for (XMLSize_t i = 0; i < dispsCount; ++i) { + xercesc::DOMNode* dispsNode = dispsNodes->item(i); + xercesc::DOMElement* dispsElement = dynamic_cast(dispsNode); + + xercesc::DOMNodeList* lineNodes = dispsElement->getElementsByTagName(TAG_displacement); + const XMLSize_t lineCount = lineNodes->getLength(); + + for (XMLSize_t j = 0; j < lineCount; ++j) { + xercesc::DOMNode* dispNode = lineNodes->item(j); + xercesc::DOMElement* dispElement = dynamic_cast(dispNode); + + int id = getIntAttr(dispElement, ATTR_id); + double x = getDoubleAttr(dispElement, ATTR_x); + double y = getDoubleAttr(dispElement, ATTR_y); + double z = getDoubleAttr(dispElement, ATTR_z); + + if (id + dispsOffset > disps.size()) + disps.resize(id + dispsOffset); + + // Memory allocation!! + int varStartIndex = variables.size(); + variables.push_back(new double(x)); + variables.push_back(new double(y)); + variables.push_back(new double(z)); + + GCS::Displacement p; + p.x = variables[varStartIndex+0]; + p.y = variables[varStartIndex+1]; + p.z = variables[varStartIndex+2]; + disps[id + dispsOffset -1] = p; + } + } + + // Quaternions + xercesc::DOMNodeList* quatsNodes = elementRoot->getElementsByTagName(TAG_quaternions); + const XMLSize_t quatCount = quatsNodes->getLength(); + + for (XMLSize_t i = 0; i < quatCount; ++i) { + xercesc::DOMNode* quatsNode = quatsNodes->item(i); + xercesc::DOMElement* quatsElement = dynamic_cast(quatsNode); + + xercesc::DOMNodeList* quatNodes = quatsElement->getElementsByTagName(TAG_quaternion); + const XMLSize_t quatCount = quatNodes->getLength(); + + for (XMLSize_t j = 0; j < quatCount; ++j) { + xercesc::DOMNode* quatNode = quatNodes->item(j); + xercesc::DOMElement* quatElement = dynamic_cast(quatNode); + + int id = getIntAttr(quatElement, ATTR_id); + double a = getDoubleAttr(quatElement, ATTR_a); + double b = getDoubleAttr(quatElement, ATTR_b); + double c = getDoubleAttr(quatElement, ATTR_c); + double d = getDoubleAttr(quatElement, ATTR_d); + + if (id + quatsOffset > quats.size()) + quats.resize(id + quatsOffset); + + GCS::Quaternion q; + // Memory allocation!! + int varStartIndex = variables.size(); + variables.push_back(new double(a)); + variables.push_back(new double(b)); + variables.push_back(new double(c)); + variables.push_back(new double(d)); + + q.a = variables[varStartIndex+0]; + q.b = variables[varStartIndex+1]; + q.c = variables[varStartIndex+2]; + q.d = variables[varStartIndex+3]; + quats[id + quatsOffset -1] = q; + } + } + + // Solids + xercesc::DOMNodeList* solidsNodes = elementRoot->getElementsByTagName(TAG_solids); + const XMLSize_t solidCount = solidsNodes->getLength(); + + for (XMLSize_t i = 0; i < solidCount; ++i) { + xercesc::DOMNode* solidsNode = solidsNodes->item(i); + xercesc::DOMElement* solidsElement = dynamic_cast(solidsNode); + + xercesc::DOMNodeList* solidNodes = solidsElement->getElementsByTagName(TAG_solid); + const XMLSize_t solidCount = solidNodes->getLength(); + + for (XMLSize_t j = 0; j < solidCount; ++j) { + xercesc::DOMNode* solidNode = solidNodes->item(j); + xercesc::DOMElement* solidElement = dynamic_cast(solidNode); + + int id = getIntAttr(solidElement, ATTR_id); + int p = getIntAttr(solidElement, ATTR_point); + int n = getIntAttr(solidElement, ATTR_normal); + int d = getIntAttr(solidElement, ATTR_displacement); + int q = getIntAttr(solidElement, ATTR_quaternion); + + GCS::Solid s; + s.p = points[p-1]; + s.n = norms[n-1]; + s.d = disps[d-1]; + s.q = quats[q-1]; + + solids.push_back(s); + } + } + + + + // Constraints + xercesc::DOMNodeList* constraintsNodes = elementRoot->getElementsByTagName(TAG_constraints); + const XMLSize_t constraintsCount = constraintsNodes->getLength(); + + for (XMLSize_t i = 0; i < constraintsCount; ++i) { + xercesc::DOMNode* constraintsNode = constraintsNodes->item(i); + xercesc::DOMElement* constraintsElement = dynamic_cast(constraintsNode); + + xercesc::DOMNodeList* constraintNodes = constraintsElement->getElementsByTagName(TAG_constraint); + const XMLSize_t constraintCount = constraintNodes->getLength(); + + for (XMLSize_t j = 0; j < constraintCount; ++j) { + xercesc::DOMNode* constraintNode = constraintNodes->item(j); + xercesc::DOMElement* constraintElement = dynamic_cast(constraintNode); + + int id = getIntAttr(constraintElement, ATTR_id); + + const XMLCh* xmlch_kind = constraintElement->getAttribute(ATTR_kind); + + //PlaneParallel Assembly + if (xercesc::XMLString::equals(xmlch_kind, KIND_Parallel_AS)) { + int s1 = getIntAttr(constraintElement, ATTR_solid1); + int s2 = getIntAttr(constraintElement, ATTR_solid2); + int t = getIntAttr(constraintElement, ATTR_type); + + int paramStartIndex = parameters.size(); + parameters.push_back(new double(t)); + + GCS::Constraint *constr + = new GCS::ConstraintParralelFaceAS( solids[s1-1], solids[s2-1], + (GCS::ParallelType*) parameters[paramStartIndex] ); + constraints.push_back(constr); + } + //Distance Assembly + if (xercesc::XMLString::equals(xmlch_kind, KIND_Distance_AS)) { + int s1 = getIntAttr(constraintElement, ATTR_solid1); + int s2 = getIntAttr(constraintElement, ATTR_solid2); + double t = getDoubleAttr(constraintElement, ATTR_distance); + + int paramStartIndex = parameters.size(); + parameters.push_back(new double(t)); + + GCS::Constraint *constr + = new GCS::ConstraintFaceDistanceAS( solids[s1-1], solids[s2-1], parameters[paramStartIndex] ); + constraints.push_back(constr); + } + } + } + return true; + } + catch (xercesc::XMLException& e) { + char* message = xercesc::XMLString::transcode(e.getMessage()); + std::cout << "Error parsing file: " << message << std::endl; + xercesc::XMLString::release(&message); + } +} + +int InputParser::getIntAttr(xercesc::DOMElement* element, XMLCh* attr) +{ + const XMLCh* xmlch_val = element->getAttribute(attr); + return xercesc::XMLString::parseInt(xmlch_val); +} + +double InputParser::getDoubleAttr(xercesc::DOMElement* element, XMLCh* attr) +{ + const XMLCh* xmlch_val = element->getAttribute(attr); + xercesc::XMLDouble xmldouble_val(xmlch_val); + return xmldouble_val.getValue(); +} + diff --git a/src/Mod/Assembly/App/gcs3d/InputParser.h b/src/Mod/Assembly/App/gcs3d/InputParser.h new file mode 100644 index 0000000000..e6f71b1d57 --- /dev/null +++ b/src/Mod/Assembly/App/gcs3d/InputParser.h @@ -0,0 +1,60 @@ + +#include +#include + +#include "GCS.h" + +class InputParser +{ +public: + InputParser(); + ~InputParser(); + + bool readInputFileAS(std::string &fileName, + std::vector &variables, + std::vector ¶meters, + std::vector &points, + std::vector &normals, + std::vector &displacements, + std::vector &quaternions, + std::vector &solids, + std::vector &constraints); + + int getIntAttr(xercesc::DOMElement* element, XMLCh* attr); + double getDoubleAttr(xercesc::DOMElement* element, XMLCh* attr); +private: + xercesc::XercesDOMParser *m_InputFileParser; + + XMLCh* TAG_points; + XMLCh* TAG_constraints; + XMLCh* TAG_point; + XMLCh* TAG_constraint; + XMLCh* ATTR_id; + XMLCh* ATTR_x; + XMLCh* ATTR_y; + XMLCh* ATTR_z; + XMLCh* ATTR_point; + XMLCh* ATTR_kind; + XMLCh* ATTR_distance; + XMLCh* TAG_GCS_AS; + XMLCh* TAG_quaternions; + XMLCh* TAG_quaternion; + XMLCh* TAG_normals; + XMLCh* TAG_normal; + XMLCh* TAG_displacements; + XMLCh* TAG_displacement; + XMLCh* TAG_solids; + XMLCh* TAG_solid; + XMLCh* KIND_Parallel_AS; + XMLCh* KIND_Distance_AS; + XMLCh* ATTR_a; + XMLCh* ATTR_b; + XMLCh* ATTR_c; + XMLCh* ATTR_d; + XMLCh* ATTR_quaternion; + XMLCh* ATTR_type; + XMLCh* ATTR_normal; + XMLCh* ATTR_displacement; + XMLCh* ATTR_solid1; + XMLCh* ATTR_solid2; +}; diff --git a/src/Mod/Assembly/App/gcs3d/SubSystem.cpp b/src/Mod/Assembly/App/gcs3d/SubSystem.cpp new file mode 100644 index 0000000000..e6bb832123 --- /dev/null +++ b/src/Mod/Assembly/App/gcs3d/SubSystem.cpp @@ -0,0 +1,351 @@ +/*************************************************************************** + * Copyright (c) Konstantinos Poulios (logari81@gmail.com) 2011 * + * * + * This file is part of the FreeCAD CAx development system. * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of the GNU Library General Public * + * License as published by the Free Software Foundation; either * + * version 2 of the License, or (at your option) any later version. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Library General Public License for more details. * + * * + * You should have received a copy of the GNU Library General Public * + * License along with this library; see the file COPYING.LIB. If not, * + * write to the Free Software Foundation, Inc., 59 Temple Place, * + * Suite 330, Boston, MA 02111-1307, USA * + * * + ***************************************************************************/ + +#include +#include +#include "SubSystem.h" + +namespace GCS +{ + +// SubSystem +SubSystem::SubSystem(std::vector &clist_, VEC_pD ¶ms) +: clist(clist_) +{ + MAP_pD_pD dummymap; + initialize(params, dummymap); +} + +SubSystem::SubSystem(std::vector &clist_, VEC_pD ¶ms, + MAP_pD_pD &reductionmap) +: clist(clist_) +{ + initialize(params, reductionmap); +} + +SubSystem::~SubSystem() +{ +} + +void SubSystem::initialize(VEC_pD ¶ms, MAP_pD_pD &reductionmap) +{ + csize = clist.size(); + + // tmpplist will contain the subset of parameters from params that are + // relevant for the constraints listed in clist + VEC_pD tmpplist; + { + SET_pD s1(params.begin(), params.end()); + SET_pD s2; + for (std::vector::iterator constr=clist.begin(); + constr != clist.end(); ++constr) { + (*constr)->revertParams(); // ensure that the constraint points to the original parameters + VEC_pD constr_params = (*constr)->params(); + s2.insert(constr_params.begin(), constr_params.end()); + } + std::set_intersection(s1.begin(), s1.end(), s2.begin(), s2.end(), + std::back_inserter(tmpplist) ); + } + + plist.clear(); + MAP_pD_I rindex; + if (reductionmap.size() > 0) { + int i=0; + MAP_pD_I pindex; + for (VEC_pD::const_iterator itt=tmpplist.begin(); + itt != tmpplist.end(); itt++) { + MAP_pD_pD::const_iterator itr = reductionmap.find(*itt); + if (itr != reductionmap.end()) { + MAP_pD_I::const_iterator itp = pindex.find(itr->second); + if (itp == pindex.end()) { // the reduction target is not in plist yet, so add it now + plist.push_back(itr->second); + rindex[itr->first] = i; + pindex[itr->second] = i; + i++; + } + else // the reduction target is already in plist, just inform rindex + rindex[itr->first] = itp->second; + } + else if (pindex.find(*itt) == pindex.end()) { // not in plist yet, so add it now + plist.push_back(*itt); + pindex[*itt] = i; + i++; + } + } + } + else + plist = tmpplist; + + psize = plist.size(); + pvals.resize(psize); + pmap.clear(); + for (int j=0; j < psize; j++) { + pmap[plist[j]] = &pvals[j]; + pvals[j] = *plist[j]; + } + for (MAP_pD_I::const_iterator itr=rindex.begin(); itr != rindex.end(); ++itr) + pmap[itr->first] = &pvals[itr->second]; + + c2p.clear(); + p2c.clear(); + for (std::vector::iterator constr=clist.begin(); + constr != clist.end(); ++constr) { + (*constr)->revertParams(); // ensure that the constraint points to the original parameters + VEC_pD constr_params_orig = (*constr)->params(); + SET_pD constr_params; + for (VEC_pD::const_iterator p=constr_params_orig.begin(); + p != constr_params_orig.end(); ++p) { + MAP_pD_pD::const_iterator pmapfind = pmap.find(*p); + if (pmapfind != pmap.end()) + constr_params.insert(pmapfind->second); + } + for (SET_pD::const_iterator p=constr_params.begin(); + p != constr_params.end(); ++p) { +// jacobi.set(*constr, *p, 0.); + c2p[*constr].push_back(*p); + p2c[*p].push_back(*constr); + } +// (*constr)->redirectParams(pmap); // redirect parameters to pvec + } +} + +void SubSystem::redirectParams() +{ + // copying values to pvals + for (MAP_pD_pD::const_iterator p=pmap.begin(); + p != pmap.end(); ++p) + *(p->second) = *(p->first); + + // redirect constraints to point to pvals + for (std::vector::iterator constr=clist.begin(); + constr != clist.end(); ++constr) { + (*constr)->revertParams(); // this line will normally not be necessary + (*constr)->redirectParams(pmap); + } +} + +void SubSystem::revertParams() +{ + for (std::vector::iterator constr=clist.begin(); + constr != clist.end(); ++constr) + (*constr)->revertParams(); +} + +void SubSystem::getParamMap(MAP_pD_pD &pmapOut) +{ + pmapOut = pmap; +} + +void SubSystem::getParamList(VEC_pD &plistOut) +{ + plistOut = plist; +} + +void SubSystem::getParams(VEC_pD ¶ms, Eigen::VectorXd &xOut) +{ + if (xOut.size() != int(params.size())) + xOut.setZero(params.size()); + + for (int j=0; j < int(params.size()); j++) { + MAP_pD_pD::const_iterator + pmapfind = pmap.find(params[j]); + if (pmapfind != pmap.end()) + xOut[j] = *(pmapfind->second); + } +} + +void SubSystem::getParams(Eigen::VectorXd &xOut) +{ + if (xOut.size() != psize) + xOut.setZero(psize); + + for (int i=0; i < psize; i++) + xOut[i] = pvals[i]; +} + +void SubSystem::setParams(VEC_pD ¶ms, Eigen::VectorXd &xIn) +{ + assert(xIn.size() == int(params.size())); + for (int j=0; j < int(params.size()); j++) { + MAP_pD_pD::const_iterator + pmapfind = pmap.find(params[j]); + if (pmapfind != pmap.end()) + *(pmapfind->second) = xIn[j]; + } +} + +void SubSystem::setParams(Eigen::VectorXd &xIn) +{ + assert(xIn.size() == psize); + for (int i=0; i < psize; i++) + pvals[i] = xIn[i]; +} + +double SubSystem::error() +{ + double err = 0.; + for (std::vector::const_iterator constr=clist.begin(); + constr != clist.end(); ++constr) { + double tmp = (*constr)->error(); + err += tmp*tmp; + } + err *= 0.5; + return err; +} + +void SubSystem::calcResidual(Eigen::VectorXd &r) +{ + assert(r.size() == csize); + + int i=0; + for (std::vector::const_iterator constr=clist.begin(); + constr != clist.end(); ++constr, i++) { + r[i] = (*constr)->error(); + } +} + +void SubSystem::calcResidual(Eigen::VectorXd &r, double &err) +{ + assert(r.size() == csize); + + int i=0; + err = 0.; + for (std::vector::const_iterator constr=clist.begin(); + constr != clist.end(); ++constr, i++) { + r[i] = (*constr)->error(); + err += r[i]*r[i]; + } + err *= 0.5; +} + +/* +void SubSystem::calcJacobi() +{ + assert(grad.size() != xsize); + + for (MAP_pD_pD::const_iterator param=pmap.begin(); + param != pmap.end(); ++param) { + // assert(p2c.find(param->second) != p2c.end()); + std::vector constrs=p2c[param->second]; + for (std::vector::const_iterator constr = constrs.begin(); + constr != constrs.end(); ++constr) + jacobi.set(*constr,param->second,(*constr)->grad(param->second)); + } +} +*/ + +void SubSystem::calcJacobi(VEC_pD ¶ms, Eigen::MatrixXd &jacobi) +{ + jacobi.setZero(csize, params.size()); + for (int j=0; j < int(params.size()); j++) { + MAP_pD_pD::const_iterator + pmapfind = pmap.find(params[j]); + if (pmapfind != pmap.end()) + for (int i=0; i < csize; i++) + jacobi(i,j) = clist[i]->grad(pmapfind->second); + } +} + +void SubSystem::calcJacobi(Eigen::MatrixXd &jacobi) +{ + calcJacobi(plist, jacobi); +} + +void SubSystem::calcGrad(VEC_pD ¶ms, Eigen::VectorXd &grad) +{ + assert(grad.size() == int(params.size())); + + grad.setZero(); + for (int j=0; j < int(params.size()); j++) { + MAP_pD_pD::const_iterator + pmapfind = pmap.find(params[j]); + if (pmapfind != pmap.end()) { + // assert(p2c.find(pmapfind->second) != p2c.end()); + std::vector constrs=p2c[pmapfind->second]; + for (std::vector::const_iterator constr = constrs.begin(); + constr != constrs.end(); ++constr) + grad[j] += (*constr)->error() * (*constr)->grad(pmapfind->second); + } + } +} + +void SubSystem::calcGrad(Eigen::VectorXd &grad) +{ + calcGrad(plist, grad); +} + +double SubSystem::maxStep(VEC_pD ¶ms, Eigen::VectorXd &xdir) +{ + assert(xdir.size() == int(params.size())); + + MAP_pD_D dir; + for (int j=0; j < int(params.size()); j++) { + MAP_pD_pD::const_iterator pmapfind = pmap.find(params[j]); + if (pmapfind != pmap.end()) + dir[pmapfind->second] = xdir[j]; + } + + double alpha=1e10; + for (std::vector::iterator constr=clist.begin(); + constr != clist.end(); ++constr) + alpha = (*constr)->maxStep(dir, alpha); + + return alpha; +} + +double SubSystem::maxStep(Eigen::VectorXd &xdir) +{ + return maxStep(plist, xdir); +} + +void SubSystem::applySolution() +{ + for (MAP_pD_pD::const_iterator it=pmap.begin(); + it != pmap.end(); ++it) + *(it->first) = *(it->second); +} + +void SubSystem::analyse(Eigen::MatrixXd &J, Eigen::MatrixXd &ker, Eigen::MatrixXd &img) +{ +} + +void SubSystem::report() +{ +} + +void SubSystem::printResidual() +{ + Eigen::VectorXd r(csize); + int i=0; + double err = 0.; + for (std::vector::const_iterator constr=clist.begin(); + constr != clist.end(); ++constr, i++) { + r[i] = (*constr)->error(); + err += r[i]*r[i]; + } + err *= 0.5; + std::cout << "Residual r = " << r.transpose() << std::endl; + std::cout << "Residual err = " << err << std::endl; +} + + +} //namespace GCS diff --git a/src/Mod/Assembly/App/gcs3d/SubSystem.h b/src/Mod/Assembly/App/gcs3d/SubSystem.h new file mode 100644 index 0000000000..1e313e35ea --- /dev/null +++ b/src/Mod/Assembly/App/gcs3d/SubSystem.h @@ -0,0 +1,89 @@ +/*************************************************************************** + * Copyright (c) Konstantinos Poulios (logari81@gmail.com) 2011 * + * * + * This file is part of the FreeCAD CAx development system. * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of the GNU Library General Public * + * License as published by the Free Software Foundation; either * + * version 2 of the License, or (at your option) any later version. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Library General Public License for more details. * + * * + * You should have received a copy of the GNU Library General Public * + * License along with this library; see the file COPYING.LIB. If not, * + * write to the Free Software Foundation, Inc., 59 Temple Place, * + * Suite 330, Boston, MA 02111-1307, USA * + * * + ***************************************************************************/ + +#ifndef FREEGCS_SUBSYSTEM_H +#define FREEGCS_SUBSYSTEM_H + +#undef min +#undef max + +#include +#include "Constraints.h" + +namespace GCS +{ + + class SubSystem + { + private: + int psize, csize; + std::vector clist; + VEC_pD plist; // pointers to the original parameters + MAP_pD_pD pmap; // redirection map from the original parameters to pvals + VEC_D pvals; // current variables vector (psize) +// JacobianMatrix jacobi; // jacobi matrix of the residuals + std::map c2p; // constraint to parameter adjacency list + std::map > p2c; // parameter to constraint adjacency list + void initialize(VEC_pD ¶ms, MAP_pD_pD &reductionmap); // called by the constructors + public: + SubSystem(std::vector &clist_, VEC_pD ¶ms); + SubSystem(std::vector &clist_, VEC_pD ¶ms, + MAP_pD_pD &reductionmap); + ~SubSystem(); + + int pSize() { return psize; }; + int cSize() { return csize; }; + + void redirectParams(); + void revertParams(); + + void getParamMap(MAP_pD_pD &pmapOut); + void getParamList(VEC_pD &plistOut); + + void getParams(VEC_pD ¶ms, Eigen::VectorXd &xOut); + void getParams(Eigen::VectorXd &xOut); + void setParams(VEC_pD ¶ms, Eigen::VectorXd &xIn); + void setParams(Eigen::VectorXd &xIn); + + double error(); + void calcResidual(Eigen::VectorXd &r); + void calcResidual(Eigen::VectorXd &r, double &err); + void calcJacobi(VEC_pD ¶ms, Eigen::MatrixXd &jacobi); + void calcJacobi(Eigen::MatrixXd &jacobi); + void calcGrad(VEC_pD ¶ms, Eigen::VectorXd &grad); + void calcGrad(Eigen::VectorXd &grad); + + double maxStep(VEC_pD ¶ms, Eigen::VectorXd &xdir); + double maxStep(Eigen::VectorXd &xdir); + + void applySolution(); + void analyse(Eigen::MatrixXd &J, Eigen::MatrixXd &ker, Eigen::MatrixXd &img); + void report(); + +void printResidual(); + }; + + double lineSearch(SubSystem *subsys, Eigen::VectorXd &xdir); + +} //namespace GCS + +#endif // FREEGCS_SUBSYSTEM_H diff --git a/src/Mod/Assembly/App/gcs3d/Util.h b/src/Mod/Assembly/App/gcs3d/Util.h new file mode 100644 index 0000000000..18eacf9ad6 --- /dev/null +++ b/src/Mod/Assembly/App/gcs3d/Util.h @@ -0,0 +1,47 @@ +/*************************************************************************** + * Copyright (c) Konstantinos Poulios (logari81@gmail.com) 2011 * + * * + * This file is part of the FreeCAD CAx development system. * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of the GNU Library General Public * + * License as published by the Free Software Foundation; either * + * version 2 of the License, or (at your option) any later version. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Library General Public License for more details. * + * * + * You should have received a copy of the GNU Library General Public * + * License along with this library; see the file COPYING.LIB. If not, * + * write to the Free Software Foundation, Inc., 59 Temple Place, * + * Suite 330, Boston, MA 02111-1307, USA * + * * + ***************************************************************************/ + +#ifndef FREEGCS_UTIL_H +#define FREEGCS_UTIL_H + +#include +#include +#include + +namespace GCS +{ + typedef std::vector VEC_pD; + typedef std::vector VEC_D; + typedef std::vector VEC_I; + typedef std::map MAP_pD_pD; + typedef std::map MAP_pD_D; + typedef std::map MAP_pD_I; + typedef std::set SET_pD; + typedef std::set SET_I; + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +} //namespace GCS + +#endif // FREEGCS_UTIL_H diff --git a/src/Mod/Assembly/App/gcs3d/main.cpp b/src/Mod/Assembly/App/gcs3d/main.cpp new file mode 100644 index 0000000000..a68ac1f224 --- /dev/null +++ b/src/Mod/Assembly/App/gcs3d/main.cpp @@ -0,0 +1,122 @@ +#include +#include + +#include "Geo.h" +#include "Constraints.h" +#include "InputParser.h" +#include "OutputWriter.h" + +#include + +using namespace std; +using namespace Eigen; + +Vector3d Place(GCS::Point p, GCS::Quaternion q, GCS::Displacement di) { + + double a=*q.a, b=*q.b, c=*q.c, d=*q.d; + double norm = sqrt(pow(a,2)+pow(b,2)+pow(c,2)+pow(d,2)); + double x=a/norm, y=b/norm, z=c/norm, w=d/norm; + + Matrix3d rot; + rot(0,0) = 1-2*(pow(y,2)+pow(z,2)); + rot(0,1) = -2.0*w*z + 2.0*x*y; + rot(0,2) = 2.0*w*y + 2.0*x*z; + rot(1,0) = 2.0*w*z + 2.0*x*y; + rot(1,1) = 1-2*(pow(x,2)+pow(z,2)); + rot(1,2) = -2.0*w*x + 2.0*y*z; + rot(2,0) = -2.0*w*y + 2.0*x*z; + rot(2,1) = 2.0*w*x + 2.0*y*z; + rot(2,2) = 1-2*(pow(x,2)+pow(y,2)); + + Vector3d v(p.x, p.y, p.z); + Vector3d n = rot*v; + + if (di.x != NULL) { + n(0) += *di.x; + n(1) += *di.y; + n(2) += *di.z; + } + + return n; +} + +std::string Vec(Vector3d vec) { + + std::stringstream s; + s<<"Base.Vector(" << vec(0) << ", " << vec(1) << ", " << vec(2) <<")"; + return s.str(); +} + + +int main(int argc, char **argv) { + + if (argc<3) return 1; + + cout << endl << "loading file: " << argv[1]; + cout << endl << "generating output: " << argv[2] << endl << endl; + + vector variables; + vector parameters; + vector points; + vector norms; + vector disps; + vector quats; + vector solids; + vector constraints; + + string inputfile(argv[1]); + string outputfile(argv[2]); + + bool d2,d3; + + InputParser parser; + d3 = parser.readInputFileAS(inputfile, variables, parameters, points, norms, disps, quats, solids, constraints); + + cout << "Variables: " << variables.size() << endl << "parameters: " << parameters.size() << endl; + cout << "Points: " << points.size() << endl << "Normals: " << norms.size() << endl; + cout << "Displacements: " << disps.size() << endl; + cout << "Quaternions: " << quats.size() << endl; + cout << "Solids: " << solids.size() << endl; + cout << "Constraints: " << constraints.size() << endl; + + //init output writing + remove(outputfile.c_str()); + ofstream file; + file.open (outputfile.c_str()); + if (!file.is_open()) return 0; + //import all needed modules + file << "import Part" << endl << "from FreeCAD import Base" << endl << endl; + //open new document + file << "App.newDocument(\"solver\")" << endl << endl; + GCS::Displacement emd; + for (int i=0; i +#include + +using namespace Eigen; + +// minimizes ( 0.5 * x^T * H * x + g^T * x ) under the condition ( A*x + c = 0 ) +// it returns the solution in x, the row-space of A in Y, and the null space of A in Z +int qp_eq(MatrixXd &H, VectorXd &g, MatrixXd &A, VectorXd &c, + VectorXd &x, MatrixXd &Y, MatrixXd &Z) +{ + FullPivHouseholderQR qrAT(A.transpose()); + MatrixXd Q = qrAT.matrixQ (); + + size_t params_num = qrAT.rows(); + size_t constr_num = qrAT.cols(); + size_t rank = qrAT.rank(); + + if (rank != constr_num || constr_num > params_num) + return -1; + + // A^T = Q*R*P^T = Q1*R1*P^T + // Q = [Q1,Q2], R=[R1;0] + // Y = Q1 * inv(R^T) * P^T + // Z = Q2 + Y = qrAT.matrixQR().topRows(constr_num) + .triangularView() + .transpose() + .solve(Q.leftCols(rank)) + * qrAT.colsPermutation().transpose(); + if (params_num == rank) + x = - Y * c; + else { + Z = Q.rightCols(params_num-rank); + + MatrixXd ZTHZ = Z.transpose() * H * Z; + VectorXd rhs = Z.transpose() * (H * Y * c - g); + + VectorXd y = ZTHZ.colPivHouseholderQr().solve(rhs); + + x = - Y * c + Z * y; + } + + return 0; +} diff --git a/src/Mod/Assembly/App/gcs3d/qp_eq.h b/src/Mod/Assembly/App/gcs3d/qp_eq.h new file mode 100644 index 0000000000..f05ca6d522 --- /dev/null +++ b/src/Mod/Assembly/App/gcs3d/qp_eq.h @@ -0,0 +1,25 @@ +/*************************************************************************** + * Copyright (c) Konstantinos Poulios (logari81@gmail.com) 2011 * + * * + * This file is part of the FreeCAD CAx development system. * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of the GNU Library General Public * + * License as published by the Free Software Foundation; either * + * version 2 of the License, or (at your option) any later version. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Library General Public License for more details. * + * * + * You should have received a copy of the GNU Library General Public * + * License along with this library; see the file COPYING.LIB. If not, * + * write to the Free Software Foundation, Inc., 59 Temple Place, * + * Suite 330, Boston, MA 02111-1307, USA * + * * + ***************************************************************************/ +#include + +int qp_eq(Eigen::MatrixXd &H, Eigen::VectorXd &g, Eigen::MatrixXd &A, Eigen::VectorXd &c, + Eigen::VectorXd &x, Eigen::MatrixXd &Y, Eigen::MatrixXd &Z); diff --git a/src/Mod/Assembly/Gui/ViewProviderAssembly.cpp b/src/Mod/Assembly/Gui/ViewProviderAssembly.cpp new file mode 100644 index 0000000000..95e4992f8d --- /dev/null +++ b/src/Mod/Assembly/Gui/ViewProviderAssembly.cpp @@ -0,0 +1,50 @@ +/*************************************************************************** + * Copyright (c) 2011 Juergen Riegel * + * * + * This file is part of the FreeCAD CAx development system. * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of the GNU Library General Public * + * License as published by the Free Software Foundation; either * + * version 2 of the License, or (at your option) any later version. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Library General Public License for more details. * + * * + * You should have received a copy of the GNU Library General Public * + * License along with this library; see the file COPYING.LIB. If not, * + * write to the Free Software Foundation, Inc., 59 Temple Place, * + * Suite 330, Boston, MA 02111-1307, USA * + * * + ***************************************************************************/ + + +#include "PreCompiled.h" + +#ifndef _PreComp_ +#endif + +#include "ViewProvider.h" +#include +//#include + +using namespace AssemblyGui; + +PROPERTY_SOURCE(AssemblyGui::ViewProviderItem,PartGui::ViewProviderPart) + +ViewProviderItem::ViewProvider() +{ +} + +ViewProviderItem::~ViewProvider() +{ +} + +bool ViewProviderItem::doubleClicked(void) +{ + return true; +} + + diff --git a/src/Mod/Assembly/Gui/ViewProviderAssembly.h b/src/Mod/Assembly/Gui/ViewProviderAssembly.h new file mode 100644 index 0000000000..0ebf8ad117 --- /dev/null +++ b/src/Mod/Assembly/Gui/ViewProviderAssembly.h @@ -0,0 +1,51 @@ +/*************************************************************************** + * Copyright (c) 2011 Juergen Riegel * + * * + * This file is part of the FreeCAD CAx development system. * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of the GNU Library General Public * + * License as published by the Free Software Foundation; either * + * version 2 of the License, or (at your option) any later version. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Library General Public License for more details. * + * * + * You should have received a copy of the GNU Library General Public * + * License along with this library; see the file COPYING.LIB. If not, * + * write to the Free Software Foundation, Inc., 59 Temple Place, * + * Suite 330, Boston, MA 02111-1307, USA * + * * + ***************************************************************************/ + + +#ifndef PARTGUI_ViewProvider_H +#define PARTGUI_ViewProvider_H + +#include + + +namespace AssemblyGui { + +class AssemblyGuiExport ViewProviderItem : public PartGui::ViewProviderPart +{ + PROPERTY_HEADER(PartGui::ViewProviderItem); + +public: + /// constructor + ViewProviderItem(); + /// destructor + virtual ~ViewProviderItem(); + + virtual bool doubleClicked(void); + +}; + + + +} // namespace AssemblyGui + + +#endif // PARTGUI_ViewProviderHole_H diff --git a/src/Mod/Assembly/Gui/ViewProviderPart.cpp b/src/Mod/Assembly/Gui/ViewProviderPart.cpp new file mode 100644 index 0000000000..95e4992f8d --- /dev/null +++ b/src/Mod/Assembly/Gui/ViewProviderPart.cpp @@ -0,0 +1,50 @@ +/*************************************************************************** + * Copyright (c) 2011 Juergen Riegel * + * * + * This file is part of the FreeCAD CAx development system. * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of the GNU Library General Public * + * License as published by the Free Software Foundation; either * + * version 2 of the License, or (at your option) any later version. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Library General Public License for more details. * + * * + * You should have received a copy of the GNU Library General Public * + * License along with this library; see the file COPYING.LIB. If not, * + * write to the Free Software Foundation, Inc., 59 Temple Place, * + * Suite 330, Boston, MA 02111-1307, USA * + * * + ***************************************************************************/ + + +#include "PreCompiled.h" + +#ifndef _PreComp_ +#endif + +#include "ViewProvider.h" +#include +//#include + +using namespace AssemblyGui; + +PROPERTY_SOURCE(AssemblyGui::ViewProviderItem,PartGui::ViewProviderPart) + +ViewProviderItem::ViewProvider() +{ +} + +ViewProviderItem::~ViewProvider() +{ +} + +bool ViewProviderItem::doubleClicked(void) +{ + return true; +} + + diff --git a/src/Mod/Assembly/Gui/ViewProviderPart.h b/src/Mod/Assembly/Gui/ViewProviderPart.h new file mode 100644 index 0000000000..0ebf8ad117 --- /dev/null +++ b/src/Mod/Assembly/Gui/ViewProviderPart.h @@ -0,0 +1,51 @@ +/*************************************************************************** + * Copyright (c) 2011 Juergen Riegel * + * * + * This file is part of the FreeCAD CAx development system. * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of the GNU Library General Public * + * License as published by the Free Software Foundation; either * + * version 2 of the License, or (at your option) any later version. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Library General Public License for more details. * + * * + * You should have received a copy of the GNU Library General Public * + * License along with this library; see the file COPYING.LIB. If not, * + * write to the Free Software Foundation, Inc., 59 Temple Place, * + * Suite 330, Boston, MA 02111-1307, USA * + * * + ***************************************************************************/ + + +#ifndef PARTGUI_ViewProvider_H +#define PARTGUI_ViewProvider_H + +#include + + +namespace AssemblyGui { + +class AssemblyGuiExport ViewProviderItem : public PartGui::ViewProviderPart +{ + PROPERTY_HEADER(PartGui::ViewProviderItem); + +public: + /// constructor + ViewProviderItem(); + /// destructor + virtual ~ViewProviderItem(); + + virtual bool doubleClicked(void); + +}; + + + +} // namespace AssemblyGui + + +#endif // PARTGUI_ViewProviderHole_H