suzanne.soy/FreeCAD
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add ViewProvider for Assembly and Part and add FreeGCE3D

Browse Source
This commit is contained in:
U-EMEA\jriegel 2012-01-18 19:00:01 +01:00 committed by Stefan Tröger
parent 1feafdd2c5
commit 1bb3fbc119
25 changed files with 3618 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
src/Mod/Assembly/App/gcs3d/CMakeLists.txt Normal file
View File

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

416
src/Mod/Assembly/App/gcs3d/Constraints.cpp Normal file
View File

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

133
src/Mod/Assembly/App/gcs3d/Constraints.h Normal file
View File

@ -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 <Eigen/Dense>
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

28
src/Mod/Assembly/App/gcs3d/Example/AS_ex1.gcx Normal file
View File

@ -0,0 +1,28 @@
<GCS_AS>
<!-- This example constraint two faces to each other -->
<points>
<point id="1" x="-58" y="28" z="0"></point>
<point id="2" x="-58" y="28" z="50"></point>
</points>
<normals>
<normal id="1" x="0" y="1" z="0"></normal>
<normal id="2" x="0" y="0" z="1"></normal>
</normals>
<quaternions>
<quaternion id="1" a="0" b="0" c="0" d="1"></quaternion>
<quaternion id="2" a="0" b="0" c="0" d="1"></quaternion>
</quaternions>
<displacements>
<displacement id="1" x="0" y="100" z="0"></displacement>
<displacement id="2" x="0" y="200" z="0"></displacement>
</displacements>
<solids>
<solid id="1" point="1" normal="1" displacement="1" quaternion="1"></solid>
<solid id="2" point="2" normal="2" displacement="2" quaternion="2"></solid>
</solids>
<constraints>
<constraint id="1" kind="Parallel_AS" solid1="1" solid2="2" type="2"></constraint>
<constraint id="2" kind="Distance_AS" solid1="2" solid2="2" distance="0"></constraint>
</constraints>
</GCS_AS>

61
src/Mod/Assembly/App/gcs3d/Example/AS_ex2.gcx Normal file
View File

@ -0,0 +1,61 @@
<GCS_AS>
<!-- This example constraints 3 bodys to each other so that no rotational degree of freedom is left.
Body one has three constraints to body two and body two three constraints to body 3.
Note that this system is overconstrained, but this is the case that would appear when added 3 mate
constraints to 2 bodys as needed to fix all translational dofs.-->
<points>
<point id="1" x="1" y="0" z="0"></point>
<point id="2" x="2" y="1" z="1"></point>
<point id="3" x="3" y="2" z="2"></point>
<point id="4" x="7" y="6" z="7"></point>
<point id="5" x="8" y="7" z="8"></point>
<point id="6" x="9" y="8" z="9"></point>
<point id="7" x="3" y="-1" z="7"></point>
<point id="8" x="4" y="-2" z="8"></point>
<point id="9" x="5" y="-3" z="9"></point>
</points>
<normals>
<!-- norm 1 is perpendicular to norm 2 and 3 perpendicular to both-->
<normal id="1" x="1" y="5" z="2"></normal>
<normal id="2" x="3" y="1" z="-4"></normal>
<normal id="3" x="-22" y="10" z="-14"></normal>
<!-- norm 4 is perpendicular to norm 5 and 6 perpendicular to both -->
<normal id="4" x="4" y="-2" z="3"></normal>
<normal id="5" x="-5" y="-1" z="6"></normal>
<normal id="6" x="-9" y="-39" z="-14"></normal>
<!-- norm 7 is perpendicular to norm 8 and 9 perpendicular to both -->
<normal id="7" x="-3" y="3" z="0"></normal>
<normal id="8" x="3" y="3" z="-6"></normal>
<normal id="9" x="-18" y="-18" z="-18"></normal>
</normals>
<quaternions>
<quaternion id="1" a="2" b="-3" c="4" d="1"></quaternion>
<quaternion id="2" a="0" b="2" c="0" d="1"></quaternion>
<quaternion id="3" a="1" b="2" c="3" d="2"></quaternion>
</quaternions>
<displacements>
<displacement id="1" x="12" y="1" z="3"></displacement>
<displacement id="2" x="4" y="1" z="-2"></displacement>
<displacement id="3" x="-2" y="7" z="5"></displacement>
</displacements>
<solids>
<solid id="1" point="1" normal="1" displacement="1" quaternion="1"></solid>
<solid id="2" point="2" normal="2" displacement="1" quaternion="1"></solid>
<solid id="3" point="3" normal="3" displacement="1" quaternion="1"></solid>
<solid id="4" point="4" normal="4" displacement="2" quaternion="2"></solid>
<solid id="5" point="5" normal="5" displacement="2" quaternion="2"></solid>
<solid id="6" point="6" normal="6" displacement="2" quaternion="2"></solid>
<solid id="7" point="7" normal="7" displacement="3" quaternion="3"></solid>
<solid id="8" point="8" normal="8" displacement="3" quaternion="3"></solid>
<solid id="9" point="9" normal="9" displacement="3" quaternion="3"></solid>
</solids>
<constraints>
<constraint id="1" kind="Parallel_AS" solid1="1" solid2="4" type="0"></constraint>
<constraint id="2" kind="Parallel_AS" solid1="2" solid2="5" type="0"></constraint>
<constraint id="3" kind="Parallel_AS" solid1="3" solid2="6" type="0"></constraint>
<constraint id="4" kind="Parallel_AS" solid1="7" solid2="4" type="0"></constraint>
<constraint id="5" kind="Parallel_AS" solid1="8" solid2="5" type="0"></constraint>
<constraint id="6" kind="Parallel_AS" solid1="9" solid2="6" type="0"></constraint>
</constraints>
</GCS_AS>

55
src/Mod/Assembly/App/gcs3d/Example/AS_ex3.gcx Normal file
View File

@ -0,0 +1,55 @@
<GCS_AS>
<!-- This example constraints 2 bodys to each other by one mate constraint (1 rot and 1 trans constraint)
Note that all values left the same as in the first examples but many are not used-->
<points>
<point id="1" x="1" y="0" z="0"></point>
<point id="2" x="2" y="1" z="1"></point>
<point id="3" x="3" y="2" z="2"></point>
<point id="4" x="7" y="6" z="7"></point>
<point id="5" x="8" y="7" z="8"></point>
<point id="6" x="9" y="8" z="9"></point>
<point id="7" x="3" y="-1" z="7"></point>
<point id="8" x="4" y="-2" z="8"></point>
<point id="9" x="5" y="-3" z="9"></point>
</points>
<normals>
<!-- norm 1 is perpendicular to norm 2 and 3 perpendicular to both-->
<normal id="1" x="1" y="5" z="2"></normal>
<normal id="2" x="3" y="1" z="-4"></normal>
<normal id="3" x="-22" y="10" z="-14"></normal>
<!-- norm 4 is perpendicular to norm 5 and 6 perpendicular to both -->
<normal id="4" x="4" y="-2" z="3"></normal>
<normal id="5" x="-5" y="-1" z="6"></normal>
<normal id="6" x="-9" y="-39" z="-14"></normal>
<!-- norm 7 is perpendicular to norm 8 and 9 perpendicular to both -->
<normal id="7" x="-3" y="3" z="0"></normal>
<normal id="8" x="3" y="3" z="-6"></normal>
<normal id="9" x="-18" y="-18" z="-18"></normal>
</normals>
<quaternions>
<quaternion id="1" a="2" b="-3" c="4" d="1"></quaternion>
<quaternion id="2" a="0" b="2" c="0" d="1"></quaternion>
<quaternion id="3" a="1" b="2" c="3" d="2"></quaternion>
</quaternions>
<displacements>
<displacement id="1" x="12" y="1" z="3"></displacement>
<displacement id="2" x="4" y="1" z="-2"></displacement>
<displacement id="3" x="-2" y="7" z="5"></displacement>
</displacements>
<solids>
<solid id="1" point="1" normal="1" displacement="1" quaternion="1"></solid>
<solid id="2" point="2" normal="2" displacement="1" quaternion="1"></solid>
<solid id="3" point="3" normal="3" displacement="1" quaternion="1"></solid>
<solid id="4" point="4" normal="4" displacement="2" quaternion="2"></solid>
<solid id="5" point="5" normal="5" displacement="2" quaternion="2"></solid>
<solid id="6" point="6" normal="6" displacement="2" quaternion="2"></solid>
<solid id="7" point="7" normal="7" displacement="3" quaternion="3"></solid>
<solid id="8" point="8" normal="8" displacement="3" quaternion="3"></solid>
<solid id="9" point="9" normal="9" displacement="3" quaternion="3"></solid>
</solids>
<constraints>
<constraint id="1" kind="Parallel_AS" solid1="1" solid2="4" type="0"></constraint>
<constraint id="2" kind="Distance_AS" solid1="1" solid2="4" distance="0"></constraint>
</constraints>
</GCS_AS>

64
src/Mod/Assembly/App/gcs3d/Example/AS_ex4.gcx Normal file
View File

@ -0,0 +1,64 @@
<GCS_AS>
<!-- This example constraints 3 bodys to each other by 6 mate constraint (6 rot and 6 trans constraint)-->
<points>
<point id="1" x="1" y="0" z="0"></point>
<point id="2" x="2" y="1" z="1"></point>
<point id="3" x="3" y="2" z="2"></point>
<point id="4" x="7" y="6" z="7"></point>
<point id="5" x="8" y="7" z="8"></point>
<point id="6" x="9" y="8" z="9"></point>
<point id="7" x="3" y="-1" z="7"></point>
<point id="8" x="4" y="-2" z="8"></point>
<point id="9" x="5" y="-3" z="9"></point>
</points>
<normals>
<!-- norm 1 is perpendicular to norm 2 and 3 perpendicular to both-->
<normal id="1" x="1" y="5" z="2"></normal>
<normal id="2" x="3" y="1" z="-4"></normal>
<normal id="3" x="-22" y="10" z="-14"></normal>
<!-- norm 4 is perpendicular to norm 5 and 6 perpendicular to both -->
<normal id="4" x="4" y="-2" z="3"></normal>
<normal id="5" x="-5" y="-1" z="6"></normal>
<normal id="6" x="-9" y="-39" z="-14"></normal>
<!-- norm 7 is perpendicular to norm 8 and 9 perpendicular to both -->
<normal id="7" x="-3" y="3" z="0"></normal>
<normal id="8" x="3" y="3" z="-6"></normal>
<normal id="9" x="-18" y="-18" z="-18"></normal>
</normals>
<quaternions>
<quaternion id="1" a="2" b="-3" c="4" d="1"></quaternion>
<quaternion id="2" a="0" b="2" c="0" d="1"></quaternion>
<quaternion id="3" a="1" b="2" c="3" d="2"></quaternion>
</quaternions>
<displacements>
<displacement id="1" x="0" y="0" z="0"></displacement>
<displacement id="2" x="0" y="0" z="0"></displacement>
<displacement id="3" x="0" y="0" z="0"></displacement>
</displacements>
<solids>
<solid id="1" point="1" normal="1" displacement="1" quaternion="1"></solid>
<solid id="2" point="2" normal="2" displacement="1" quaternion="1"></solid>
<solid id="3" point="3" normal="3" displacement="1" quaternion="1"></solid>
<solid id="4" point="4" normal="4" displacement="2" quaternion="2"></solid>
<solid id="5" point="5" normal="5" displacement="2" quaternion="2"></solid>
<solid id="6" point="6" normal="6" displacement="2" quaternion="2"></solid>
<solid id="7" point="7" normal="7" displacement="3" quaternion="3"></solid>
<solid id="8" point="8" normal="8" displacement="3" quaternion="3"></solid>
<solid id="9" point="9" normal="9" displacement="3" quaternion="3"></solid>
</solids>
<constraints>
<constraint id="1" kind="Parallel_AS" solid1="1" solid2="4" type="0"></constraint>
<constraint id="2" kind="Distance_AS" solid1="1" solid2="4" distance="0"></constraint>
<constraint id="3" kind="Parallel_AS" solid1="2" solid2="5" type="0"></constraint>
<constraint id="4" kind="Distance_AS" solid1="2" solid2="5" distance="0"></constraint>
<constraint id="5" kind="Parallel_AS" solid1="3" solid2="6" type="0"></constraint>
<constraint id="6" kind="Distance_AS" solid1="3" solid2="6" distance="0"></constraint>
<constraint id="7" kind="Parallel_AS" solid1="7" solid2="4" type="0"></constraint>
<constraint id="8" kind="Distance_AS" solid1="7" solid2="4" distance="0"></constraint>
<constraint id="9" kind="Parallel_AS" solid1="8" solid2="5" type="0"></constraint>
<constraint id="10" kind="Distance_AS" solid1="8" solid2="5" distance="0"></constraint>
<constraint id="11" kind="Parallel_AS" solid1="9" solid2="6" type="0"></constraint>
<constraint id="12" kind="Distance_AS" solid1="9" solid2="6" distance="0"></constraint>
</constraints>
</GCS_AS>

61
src/Mod/Assembly/App/gcs3d/Example/AS_ex5.gcx Normal file
View File

@ -0,0 +1,61 @@
<GCS_AS>
<!-- This example constraints 3 bodys to each other by 6 mate constraint (6 rot and 6 trans constraint)-->
<points>
<point id="1" x="1" y="0" z="0"></point>
<point id="2" x="2" y="1" z="1"></point>
<point id="3" x="3" y="2" z="2"></point>
</points>
<normals>
<!-- norm 1 is perpendicular to norm 2 and 3 perpendicular to both-->
<normal id="1" x="1" y="0" z="0"></normal>
<normal id="2" x="0" y="1" z="0"></normal>
<normal id="3" x="0" y="0" z="1"></normal>
</normals>
<quaternions>
<quaternion id="1" a="0" b="0" c="0" d="1"></quaternion>
<quaternion id="2" a="0" b="0" c="0" d="1"></quaternion>
<quaternion id="3" a="0" b="0" c="0" d="1"></quaternion>
<quaternion id="4" a="0" b="0" c="0" d="1"></quaternion>
</quaternions>
<displacements>
<displacement id="1" x="0" y="10" z="0"></displacement>
<displacement id="2" x="0" y="20" z="0"></displacement>
<displacement id="3" x="0" y="30" z="0"></displacement>
<displacement id="4" x="0" y="40" z="0"></displacement>
</displacements>
<solids>
<solid id="1" point="1" normal="1" displacement="1" quaternion="1"></solid>
<solid id="2" point="2" normal="2" displacement="1" quaternion="1"></solid>
<solid id="3" point="3" normal="3" displacement="1" quaternion="1"></solid>
<solid id="4" point="1" normal="1" displacement="2" quaternion="2"></solid>
<solid id="5" point="2" normal="2" displacement="2" quaternion="2"></solid>
<solid id="6" point="3" normal="3" displacement="2" quaternion="2"></solid>
<solid id="7" point="1" normal="1" displacement="3" quaternion="3"></solid>
<solid id="8" point="2" normal="2" displacement="3" quaternion="3"></solid>
<solid id="9" point="3" normal="3" displacement="3" quaternion="3"></solid>
<solid id="10" point="1" normal="1" displacement="4" quaternion="4"></solid>
<solid id="11" point="2" normal="2" displacement="4" quaternion="4"></solid>
<solid id="12" point="3" normal="3" displacement="4" quaternion="4"></solid>
</solids>
<constraints>
<constraint id="1" kind="Parallel_AS" solid1="5" solid2="1" type="0"></constraint>
<constraint id="2" kind="Distance_AS" solid1="5" solid2="1" distance="0"></constraint>
<constraint id="3" kind="Parallel_AS" solid1="6" solid2="2" type="0"></constraint>
<constraint id="4" kind="Distance_AS" solid1="6" solid2="2" distance="0"></constraint>
<constraint id="5" kind="Parallel_AS" solid1="4" solid2="3" type="0"></constraint>
<constraint id="6" kind="Distance_AS" solid1="4" solid2="3" distance="0"></constraint>
<constraint id="7" kind="Parallel_AS" solid1="8" solid2="4" type="0"></constraint>
<constraint id="8" kind="Distance_AS" solid1="8" solid2="4" distance="0"></constraint>
<constraint id="9" kind="Parallel_AS" solid1="9" solid2="5" type="0"></constraint>
<constraint id="10" kind="Distance_AS" solid1="9" solid2="5" distance="0"></constraint>
<constraint id="11" kind="Parallel_AS" solid1="7" solid2="6" type="0"></constraint>
<constraint id="12" kind="Distance_AS" solid1="7" solid2="6" distance="0"></constraint>
<constraint id="13" kind="Parallel_AS" solid1="11" solid2="7" type="0"></constraint>
<constraint id="14" kind="Distance_AS" solid1="11" solid2="7" distance="0"></constraint>
<constraint id="15" kind="Parallel_AS" solid1="12" solid2="8" type="0"></constraint>
<constraint id="16" kind="Distance_AS" solid1="12" solid2="8" distance="0"></constraint>
<constraint id="17" kind="Parallel_AS" solid1="10" solid2="9" type="0"></constraint>
<constraint id="18" kind="Distance_AS" solid1="10" solid2="9" distance="0"></constraint>
</constraints>
</GCS_AS>

64
src/Mod/Assembly/App/gcs3d/Example/AS_ex6.gcx Normal file
View File

@ -0,0 +1,64 @@
<GCS_AS>
<!-- This example constraints 3 bodys to each other by 6 mate constraint (6 rot and 6 trans constraint)-->
<points>
<point id="1" x="1" y="0" z="0"></point>
<point id="2" x="2" y="1" z="1"></point>
<point id="3" x="3" y="2" z="2"></point>
<point id="4" x="7" y="6" z="7"></point>
<point id="5" x="8" y="7" z="8"></point>
<point id="6" x="9" y="8" z="9"></point>
<point id="7" x="3" y="-1" z="7"></point>
<point id="8" x="4" y="-2" z="8"></point>
<point id="9" x="5" y="-3" z="9"></point>
</points>
<normals>
<!-- norm 1 is perpendicular to norm 2 and 3 perpendicular to both-->
<normal id="1" x="1" y="5" z="2"></normal>
<normal id="2" x="3" y="1" z="-4"></normal>
<normal id="3" x="-22" y="10" z="-14"></normal>
<!-- norm 4 is perpendicular to norm 5 and 6 perpendicular to both -->
<normal id="4" x="4" y="-2" z="3"></normal>
<normal id="5" x="-5" y="-1" z="6"></normal>
<normal id="6" x="-9" y="-39" z="-14"></normal>
<!-- norm 7 is perpendicular to norm 8 and 9 perpendicular to both -->
<normal id="7" x="-3" y="3" z="0"></normal>
<normal id="8" x="3" y="3" z="-6"></normal>
<normal id="9" x="-18" y="-18" z="-18"></normal>
</normals>
<quaternions>
<quaternion id="1" a="0" b="0" c="0" d="1"></quaternion>
<quaternion id="2" a="0" b="0" c="0" d="1"></quaternion>
<quaternion id="3" a="0" b="0" c="0" d="1"></quaternion>
</quaternions>
<displacements>
<displacement id="1" x="0" y="0" z="0"></displacement>
<displacement id="2" x="0" y="0" z="0"></displacement>
<displacement id="3" x="0" y="0" z="0"></displacement>
</displacements>
<solids>
<solid id="1" point="1" normal="1" displacement="1" quaternion="1"></solid>
<solid id="2" point="2" normal="2" displacement="1" quaternion="1"></solid>
<solid id="3" point="3" normal="3" displacement="1" quaternion="1"></solid>
<solid id="4" point="4" normal="4" displacement="2" quaternion="2"></solid>
<solid id="5" point="5" normal="5" displacement="2" quaternion="2"></solid>
<solid id="6" point="6" normal="6" displacement="2" quaternion="2"></solid>
<solid id="7" point="7" normal="7" displacement="3" quaternion="3"></solid>
<solid id="8" point="8" normal="8" displacement="3" quaternion="3"></solid>
<solid id="9" point="9" normal="9" displacement="3" quaternion="3"></solid>
</solids>
<constraints>
<constraint id="1" kind="Parallel_AS" solid1="1" solid2="4" type="0"></constraint>
<constraint id="2" kind="Distance_AS" solid1="1" solid2="4" distance="0"></constraint>
<constraint id="3" kind="Parallel_AS" solid1="2" solid2="5" type="0"></constraint>
<constraint id="4" kind="Distance_AS" solid1="2" solid2="5" distance="0"></constraint>
<constraint id="5" kind="Parallel_AS" solid1="3" solid2="6" type="0"></constraint>
<constraint id="6" kind="Distance_AS" solid1="3" solid2="6" distance="0"></constraint>
<constraint id="7" kind="Parallel_AS" solid1="7" solid2="4" type="0"></constraint>
<constraint id="8" kind="Distance_AS" solid1="7" solid2="4" distance="0"></constraint>
<constraint id="9" kind="Parallel_AS" solid1="8" solid2="5" type="0"></constraint>
<constraint id="10" kind="Distance_AS" solid1="8" solid2="5" distance="0"></constraint>
<constraint id="11" kind="Parallel_AS" solid1="9" solid2="6" type="0"></constraint>
<constraint id="12" kind="Distance_AS" solid1="9" solid2="6" distance="0"></constraint>
</constraints>
</GCS_AS>

64
src/Mod/Assembly/App/gcs3d/Example/AS_ex7.gcx Normal file
View File

@ -0,0 +1,64 @@
<GCS_AS>
<!-- This example constraints 3 bodys to each other by 6 mate constraint (6 rot and 6 trans constraint)-->
<points>
<point id="1" x="1" y="0" z="0"></point>
<point id="2" x="2" y="1" z="1"></point>
<point id="3" x="3" y="2" z="2"></point>
<point id="4" x="7" y="6" z="7"></point>
<point id="5" x="8" y="7" z="8"></point>
<point id="6" x="9" y="8" z="9"></point>
<point id="7" x="3" y="-1" z="7"></point>
<point id="8" x="4" y="-2" z="8"></point>
<point id="9" x="5" y="-3" z="9"></point>
</points>
<normals>
<!-- norm 1 is perpendicular to norm 2 and 3 perpendicular to both-->
<normal id="1" x="1" y="0" z="0"></normal>
<normal id="2" x="0" y="1" z="0"></normal>
<normal id="3" x="0" y="0" z="1"></normal>
<!-- norm 4 is perpendicular to norm 5 and 6 perpendicular to both -->
<normal id="4" x="0" y="1" z="0"></normal>
<normal id="5" x="0" y="0" z="1"></normal>
<normal id="6" x="1" y="0" z="0"></normal>
<!-- norm 7 is perpendicular to norm 8 and 9 perpendicular to both -->
<normal id="7" x="0" y="0" z="1"></normal>
<normal id="8" x="1" y="0" z="0"></normal>
<normal id="9" x="0" y="1" z="0"></normal>
</normals>
<quaternions>
<quaternion id="1" a="0" b="0" c="0" d="1"></quaternion>
<quaternion id="2" a="0" b="0" c="0" d="1"></quaternion>
<quaternion id="3" a="0" b="0" c="0" d="1"></quaternion>
</quaternions>
<displacements>
<displacement id="1" x="0" y="0" z="0"></displacement>
<displacement id="2" x="0" y="0" z="0"></displacement>
<displacement id="3" x="0" y="0" z="0"></displacement>
</displacements>
<solids>
<solid id="1" point="1" normal="1" displacement="1" quaternion="1"></solid>
<solid id="2" point="2" normal="2" displacement="1" quaternion="1"></solid>
<solid id="3" point="3" normal="3" displacement="1" quaternion="1"></solid>
<solid id="4" point="4" normal="4" displacement="2" quaternion="2"></solid>
<solid id="5" point="5" normal="5" displacement="2" quaternion="2"></solid>
<solid id="6" point="6" normal="6" displacement="2" quaternion="2"></solid>
<solid id="7" point="7" normal="7" displacement="3" quaternion="3"></solid>
<solid id="8" point="8" normal="8" displacement="3" quaternion="3"></solid>
<solid id="9" point="9" normal="9" displacement="3" quaternion="3"></solid>
</solids>
<constraints>
<constraint id="1" kind="Parallel_AS" solid1="1" solid2="4" type="0"></constraint>
<constraint id="2" kind="Distance_AS" solid1="1" solid2="4" distance="0"></constraint>
<constraint id="3" kind="Parallel_AS" solid1="2" solid2="5" type="0"></constraint>
<constraint id="4" kind="Distance_AS" solid1="2" solid2="5" distance="0"></constraint>
<constraint id="5" kind="Parallel_AS" solid1="3" solid2="6" type="0"></constraint>
<constraint id="6" kind="Distance_AS" solid1="3" solid2="6" distance="0"></constraint>
<constraint id="7" kind="Parallel_AS" solid1="7" solid2="4" type="0"></constraint>
<constraint id="8" kind="Distance_AS" solid1="7" solid2="4" distance="0"></constraint>
<constraint id="9" kind="Parallel_AS" solid1="8" solid2="5" type="0"></constraint>
<constraint id="10" kind="Distance_AS" solid1="8" solid2="5" distance="0"></constraint>
<constraint id="11" kind="Parallel_AS" solid1="9" solid2="6" type="0"></constraint>
<constraint id="12" kind="Distance_AS" solid1="9" solid2="6" distance="0"></constraint>
</constraints>
</GCS_AS>

1139
src/Mod/Assembly/App/gcs3d/GCS.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

122
src/Mod/Assembly/App/gcs3d/GCS.h Normal file
View File

@ -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<Constraint *> clist;
std::map<Constraint *,VEC_pD > c2p; // constraint to parameter adjacency list
std::map<double *,std::vector<Constraint *> > 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<Constraint *> clist_ );
~System();
void clear();
void clearByTag ( int tagId );
int addConstraint ( Constraint *constr );
void removeConstraint ( Constraint *constr );
void initSolution ( VEC_pD &params );
int solve ( bool isFine=true, Algorithm alg=DogLeg );
int solve ( VEC_pD &params, 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<SubSystem *> &subsysvec );
void applySolution();
bool isInit() const {
return init;
}
int diagnose ( VEC_pD &params, 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<Constraint *> &constrvec );
void free ( std::vector<SubSystem *> &subsysvec );
} //namespace GCS
#endif // FREEGCS_GCS_H

67
src/Mod/Assembly/App/gcs3d/Geo.h Normal file
View File

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

359
src/Mod/Assembly/App/gcs3d/InputParser.cpp Normal file
View File

@ -0,0 +1,359 @@
#include <sys/stat.h>
#include <iostream>
#include "InputParser.h"
#include <xercesc/util/XMLDouble.hpp>
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<double *> &variables,
std::vector<double *> &parameters,
std::vector<GCS::Point> &points,
std::vector<GCS::Normal> &norms,
std::vector<GCS::Displacement> &disps,
std::vector<GCS::Quaternion> &quats,
std::vector<GCS::Solid> &solids,
std::vector<GCS::Constraint *> &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<xercesc::DOMElement*>(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<xercesc::DOMElement*>(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<xercesc::DOMElement*>(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<xercesc::DOMElement*>(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<xercesc::DOMElement*>(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<xercesc::DOMElement*>(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<xercesc::DOMElement*>(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<xercesc::DOMElement*>(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<xercesc::DOMElement*>(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<xercesc::DOMElement*>(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<xercesc::DOMElement*>(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<xercesc::DOMElement*>(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();
}

60
src/Mod/Assembly/App/gcs3d/InputParser.h Normal file
View File

@ -0,0 +1,60 @@
#include <xercesc/dom/DOM.hpp>
#include <xercesc/parsers/XercesDOMParser.hpp>
#include "GCS.h"
class InputParser
{
public:
InputParser();
~InputParser();
bool readInputFileAS(std::string &fileName,
std::vector<double *> &variables,
std::vector<double *> &parameters,
std::vector<GCS::Point> &points,
std::vector<GCS::Normal> &normals,
std::vector<GCS::Displacement> &displacements,
std::vector<GCS::Quaternion> &quaternions,
std::vector<GCS::Solid> &solids,
std::vector<GCS::Constraint *> &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;
};

351
src/Mod/Assembly/App/gcs3d/SubSystem.cpp Normal file
View File

@ -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 <iostream>
#include <iterator>
#include "SubSystem.h"
namespace GCS
{
// SubSystem
SubSystem::SubSystem(std::vector<Constraint *> &clist_, VEC_pD &params)
: clist(clist_)
{
MAP_pD_pD dummymap;
initialize(params, dummymap);
}
SubSystem::SubSystem(std::vector<Constraint *> &clist_, VEC_pD &params,
MAP_pD_pD &reductionmap)
: clist(clist_)
{
initialize(params, reductionmap);
}
SubSystem::~SubSystem()
{
}
void SubSystem::initialize(VEC_pD &params, 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<Constraint *>::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<Constraint *>::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<Constraint *>::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<Constraint *>::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 &params, 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 &params, 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<Constraint *>::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<Constraint *>::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<Constraint *>::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<Constraint *> constrs=p2c[param->second];
for (std::vector<Constraint *>::const_iterator constr = constrs.begin();
constr != constrs.end(); ++constr)
jacobi.set(*constr,param->second,(*constr)->grad(param->second));
}
}
*/
void SubSystem::calcJacobi(VEC_pD &params, 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 &params, 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<Constraint *> constrs=p2c[pmapfind->second];
for (std::vector<Constraint *>::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 &params, 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<Constraint *>::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<Constraint *>::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

89
src/Mod/Assembly/App/gcs3d/SubSystem.h Normal file
View File

@ -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 <Eigen/Dense>
#include "Constraints.h"
namespace GCS
{
class SubSystem
{
private:
int psize, csize;
std::vector<Constraint *> 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<Constraint *,VEC_pD > c2p; // constraint to parameter adjacency list
std::map<double *,std::vector<Constraint *> > p2c; // parameter to constraint adjacency list
void initialize(VEC_pD &params, MAP_pD_pD &reductionmap); // called by the constructors
public:
SubSystem(std::vector<Constraint *> &clist_, VEC_pD &params);
SubSystem(std::vector<Constraint *> &clist_, VEC_pD &params,
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 &params, Eigen::VectorXd &xOut);
void getParams(Eigen::VectorXd &xOut);
void setParams(VEC_pD &params, 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 &params, Eigen::MatrixXd &jacobi);
void calcJacobi(Eigen::MatrixXd &jacobi);
void calcGrad(VEC_pD &params, Eigen::VectorXd &grad);
void calcGrad(Eigen::VectorXd &grad);
double maxStep(VEC_pD &params, 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

47
src/Mod/Assembly/App/gcs3d/Util.h Normal file
View File

@ -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 <vector>
#include <map>
#include <set>
namespace GCS
{
typedef std::vector<double *> VEC_pD;
typedef std::vector<double> VEC_D;
typedef std::vector<int> VEC_I;
typedef std::map<double *, double *> MAP_pD_pD;
typedef std::map<double *, double> MAP_pD_D;
typedef std::map<double *, int> MAP_pD_I;
typedef std::set<double *> SET_pD;
typedef std::set<int> SET_I;
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
} //namespace GCS
#endif // FREEGCS_UTIL_H

122
src/Mod/Assembly/App/gcs3d/main.cpp Normal file
View File

@ -0,0 +1,122 @@
#include <iostream>
#include <vector>
#include "Geo.h"
#include "Constraints.h"
#include "InputParser.h"
#include "OutputWriter.h"
#include <fstream>
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<double *> variables;
vector<double *> parameters;
vector<GCS::Point> points;
vector<GCS::Normal> norms;
vector<GCS::Displacement> disps;
vector<GCS::Quaternion> quats;
vector<GCS::Solid> solids;
vector<GCS::Constraint *> 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<solids.size(); i++) {
file << "plane = Part.makePlane(2,2,"<<Vec(Place(solids[i].p, solids[i].q, solids[i].d))<<",";
file << Vec(Place(solids[i].n, solids[i].q, emd)) << ")" << endl;
file << "Part.show(plane)" << endl;
file << "Gui.ActiveDocument.ActiveObject.ShapeColor = ("<< 0.9-0.05*(double)i <<",0.0,0.0)" << endl;
}
cout<<endl<<"solving...";
GCS::System GCSsys(constraints);
GCSsys.solve(variables);
GCSsys.applySolution();
for (int i=0; i<solids.size(); i++) {
file << "plane = Part.makePlane(2,2,"<<Vec(Place(solids[i].p, solids[i].q, solids[i].d))<<",";
file << Vec(Place(solids[i].n, solids[i].q, emd)) << ")" << endl;
file << "Part.show(plane)" << endl;
file << "Gui.ActiveDocument.ActiveObject.ShapeColor = (0.0,0.0,"<< 0.9-0.05*(double)i <<")" << endl;
}
// Clean up
GCS::free(variables);
GCS::free(parameters);
GCS::free(constraints);
return 0;
}

65
src/Mod/Assembly/App/gcs3d/qp_eq.cpp Normal file
View File

@ -0,0 +1,65 @@
/***************************************************************************
* 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 <iostream>
#include <Eigen/QR>
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<MatrixXd> 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<Upper>()
.transpose()
.solve<OnTheRight>(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;
}

25
src/Mod/Assembly/App/gcs3d/qp_eq.h Normal file
View File

@ -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 <Eigen/Dense>
int qp_eq(Eigen::MatrixXd &H, Eigen::VectorXd &g, Eigen::MatrixXd &A, Eigen::VectorXd &c,
Eigen::VectorXd &x, Eigen::MatrixXd &Y, Eigen::MatrixXd &Z);

50
src/Mod/Assembly/Gui/ViewProviderAssembly.cpp Normal file
View File

@ -0,0 +1,50 @@
/***************************************************************************
* Copyright (c) 2011 Juergen Riegel <FreeCAD@juergen-riegel.net> *
* *
* This file is part of the FreeCAD CAx development system. *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of the GNU Library General Public *
* License as published by the Free Software Foundation; either *
* version 2 of the License, or (at your option) any later version. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Library General Public License for more details. *
* *
* You should have received a copy of the GNU Library General Public *
* License along with this library; see the file COPYING.LIB. If not, *
* write to the Free Software Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307, USA *
* *
***************************************************************************/
#include "PreCompiled.h"
#ifndef _PreComp_
#endif
#include "ViewProvider.h"
#include <Gui/Command.h>
//#include <Gui/Document.h>
using namespace AssemblyGui;
PROPERTY_SOURCE(AssemblyGui::ViewProviderItem,PartGui::ViewProviderPart)
ViewProviderItem::ViewProvider()
{
}
ViewProviderItem::~ViewProvider()
{
}
bool ViewProviderItem::doubleClicked(void)
{
return true;
}

51
src/Mod/Assembly/Gui/ViewProviderAssembly.h Normal file
View File

@ -0,0 +1,51 @@
/***************************************************************************
* Copyright (c) 2011 Juergen Riegel <FreeCAD@juergen-riegel.net> *
* *
* This file is part of the FreeCAD CAx development system. *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of the GNU Library General Public *
* License as published by the Free Software Foundation; either *
* version 2 of the License, or (at your option) any later version. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Library General Public License for more details. *
* *
* You should have received a copy of the GNU Library General Public *
* License along with this library; see the file COPYING.LIB. If not, *
* write to the Free Software Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307, USA *
* *
***************************************************************************/
#ifndef PARTGUI_ViewProvider_H
#define PARTGUI_ViewProvider_H
#include <Mod/Part/Gui/ViewProvider.h>
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

50
src/Mod/Assembly/Gui/ViewProviderPart.cpp Normal file
View File

@ -0,0 +1,50 @@
/***************************************************************************
* Copyright (c) 2011 Juergen Riegel <FreeCAD@juergen-riegel.net> *
* *
* This file is part of the FreeCAD CAx development system. *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of the GNU Library General Public *
* License as published by the Free Software Foundation; either *
* version 2 of the License, or (at your option) any later version. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Library General Public License for more details. *
* *
* You should have received a copy of the GNU Library General Public *
* License along with this library; see the file COPYING.LIB. If not, *
* write to the Free Software Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307, USA *
* *
***************************************************************************/
#include "PreCompiled.h"
#ifndef _PreComp_
#endif
#include "ViewProvider.h"
#include <Gui/Command.h>
//#include <Gui/Document.h>
using namespace AssemblyGui;
PROPERTY_SOURCE(AssemblyGui::ViewProviderItem,PartGui::ViewProviderPart)
ViewProviderItem::ViewProvider()
{
}
ViewProviderItem::~ViewProvider()
{
}
bool ViewProviderItem::doubleClicked(void)
{
return true;
}

51
src/Mod/Assembly/Gui/ViewProviderPart.h Normal file
View File

@ -0,0 +1,51 @@
/***************************************************************************
* Copyright (c) 2011 Juergen Riegel <FreeCAD@juergen-riegel.net> *
* *
* This file is part of the FreeCAD CAx development system. *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of the GNU Library General Public *
* License as published by the Free Software Foundation; either *
* version 2 of the License, or (at your option) any later version. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Library General Public License for more details. *
* *
* You should have received a copy of the GNU Library General Public *
* License along with this library; see the file COPYING.LIB. If not, *
* write to the Free Software Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307, USA *
* *
***************************************************************************/
#ifndef PARTGUI_ViewProvider_H
#define PARTGUI_ViewProvider_H
#include <Mod/Part/Gui/ViewProvider.h>
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