yade-dev team mailing list archive
-
yade-dev team
-
Mailing list archive
-
Message #12511
[Branch ~yade-pkg/yade/git-trunk] Rev 3776: Potential particles modification
------------------------------------------------------------
revno: 3776
committer: Jan Stransky <jan.stransky@xxxxxxxxxxx>
timestamp: Tue 2016-01-19 01:59:38 +0100
message:
Potential particles modification
add option to CMakeLists (because of external dependency off by default)
code reindentation, deleted unused parts
fixed compilation warnings
added MarchingCube.*pp from r2177 + update to compile
added:
lib/computational-geometry/MarchingCube.cpp
lib/computational-geometry/MarchingCube.hpp
modified:
CMakeLists.txt
examples/PotentialParticles/cubePPscaled.py
pkg/common/Gl1_PotentialParticle.cpp
pkg/common/Gl1_PotentialParticle.hpp
pkg/dem/Ig2_PP_PP_ScGeom.cpp
pkg/dem/Ig2_PP_PP_ScGeom.hpp
pkg/dem/KnKsLaw.cpp
pkg/dem/KnKsLaw.hpp
pkg/dem/PotentialParticle.cpp
pkg/dem/PotentialParticle.hpp
pkg/dem/PotentialParticle2AABB.cpp
pkg/dem/PotentialParticle2AABB.hpp
--
lp:yade
https://code.launchpad.net/~yade-pkg/yade/git-trunk
Your team Yade developers is subscribed to branch lp:yade.
To unsubscribe from this branch go to https://code.launchpad.net/~yade-pkg/yade/git-trunk/+edit-subscription
=== modified file 'CMakeLists.txt'
--- CMakeLists.txt 2015-11-11 06:51:22 +0000
+++ CMakeLists.txt 2016-01-19 00:59:38 +0000
@@ -19,6 +19,7 @@
# ENABLE_LIQMIGRATION: enable LIQMIGRATION-option, see [Mani2013] for details (OFF by default)
# ENABLE_MASK_ARBITRARY: enable MASK_ARBITRARY option (OFF by default)
# ENABLE_PROFILING: enable profiling, e.g. shows some more metrics, which can define bottlenecks of the code (OFF by default)
+# ENABLE_POTENTIAL_PARTICLES: enable potential particles option (OFF by default)
# runtimePREFIX: used for packaging, when install directory is not the same is runtime directory (/usr/local by default)
# CHUNKSIZE: set >1, if you want several sources to be compiled at once. Increases compilation speed and RAM-consumption during it (1 by default)
# VECTORIZE: enables vectorization and alignment in Eigen3 library, experimental (OFF by default)
@@ -129,6 +130,7 @@
OPTION(ENABLE_LIQMIGRATION "Enable liquid control (very experimental), see [Mani2013] for details" OFF)
OPTION(ENABLE_MASK_ARBITRARY "Enable arbitrary precision of bitmask variables (only Body::groupMask yet implemented) (experimental). Use -DMASK_ARBITRARY_SIZE=int to set number of used bits (256 by default)" OFF)
OPTION(ENABLE_PROFILING "Enable profiling, e.g. shows some more metrics, which can define bottlenecks of the code (OFF by default)" OFF)
+OPTION(ENABLE_POTENTIAL_PARTICLES "Enable potential particles" OFF)
OPTION(USE_QT5 "USE Qt5 for GUI" OFF)
#===========================================================
@@ -390,6 +392,25 @@
ENDIF(ENABLE_PROFILING)
#===========================================================
+IF(ENABLE_POTENTIAL_PARTICLES)
+ FIND_PACKAGE(OpenBlas)
+ INCLUDE(FindLAPACK)
+ FIND_PACKAGE(Lapack)
+ IF(OPENBLAS_FOUND AND LAPACK_FOUND)
+ ADD_DEFINITIONS("-DYADE_POTENTIAL_PARTICLES")
+ SET(LINKLIBS "${LINKLIBS};${OPENBLAS_LIBRARY};${LAPACK_LIBRARY}")
+ MESSAGE(STATUS "Found OpenBlas")
+ MESSAGE(STATUS "Found Lapack")
+ SET(CONFIGURED_FEATS "${CONFIGURED_FEATS} PotentialParticles")
+ ELSE(OPENBLAS_FOUND AND LAPACK_FOUND)
+ MESSAGE(STATUS "Missing dependency for PotentialParticles, disabled")
+ SET(DISABLED_FEATS "${DISABLED_FEATS} PotentialParticles")
+ SET(ENABLE_POTENTIAL_PARTICLES OFF)
+ ENDIF(OPENBLAS_FOUND AND LAPACK_FOUND)
+ELSE(ENABLE_POTENTIAL_PARTICLES)
+ SET(DISABLED_FEATS "${DISABLED_FEATS} PotentialParticles")
+ENDIF(ENABLE_POTENTIAL_PARTICLES)
+#===========================================================
IF (INSTALL_PREFIX)
SET(CMAKE_INSTALL_PREFIX ${INSTALL_PREFIX})
@@ -448,6 +469,9 @@
SET(SRC_LIB "${SRC_LIB};lib/base/Math.cpp;lib/factory/ClassFactory.cpp;lib/factory/DynLibManager.cpp")
SET(SRC_LIB "${SRC_LIB};lib/serialization/Serializable.cpp;lib/pyutil/gil.cpp;core/main/pyboot.cpp;${GUI_SRC_LIB};${CGAL_SRC_LIB}")
+IF(ENABLE_POTENTIAL_PARTICLES)
+ SET(SRC_LIB "${SRC_LIB};lib/computational-geometry/MarchingCube.cpp")
+ENDIF(ENABLE_POTENTIAL_PARTICLES)
#===========================================================
=== modified file 'examples/PotentialParticles/cubePPscaled.py'
--- examples/PotentialParticles/cubePPscaled.py 2015-12-14 14:25:08 +0000
+++ examples/PotentialParticles/cubePPscaled.py 2016-01-19 00:59:38 +0000
@@ -5,7 +5,6 @@
from yade import pack
import math
-
O.engines=[
ForceResetter(),
InsertionSortCollider([PotentialParticle2AABB()],verletDist=0),
@@ -13,19 +12,12 @@
[Ig2_PP_PP_ScGeom()],
[Ip2_FrictMat_FrictMat_KnKsPhys(Knormal = 1e8, Kshear = 1e8,useFaceProperties=False,calJointLength=False,twoDimension=True,unitWidth2D=1.0,viscousDamping=0.7)],
[Law2_SCG_KnKsPhys_KnKsLaw(label='law',neverErase=False)]
- #[Ip2_FrictMat_FrictMat_FrictPhys()],
- #[Law2_ScGeom_FrictPhys_CundallStrack()]
),
- #GravityEngine(gravity=[0,-10,0]),
- #GlobalStiffnessTimeStepper(),
+ GravityEngine(gravity=[0,-10,0]),
NewtonIntegrator(damping=0.0,exactAsphericalRot=False,gravity=[0,-10,0]),
- #PotentialParticleVTKRecorder(fileName='/home/chiab/yadeNew/mosek/8Nov/BranchA/scripts2/boon/ComputersGeotechnics/vtk/1000PP',iterPeriod=100,sampleX=50,sampleY=50,sampleZ=50)
-
+ PotentialParticleVTKRecorder(fileName='/tmp/potentialParticleTest',iterPeriod=100,sampleX=50,sampleY=50,sampleZ=50)
]
-
-
-
powderDensity = 10000
distanceToCentre= 0.5
meanSize = 1.0
@@ -59,12 +51,6 @@
b.dynamic = True
count =count+1
-#v1 = (0, 0, 0.2) (0, 0, 1) (0,0,2.498)
-#v2 = (-0.0943, 0.1633, -0.0667) (-0.4714, 0.8165, -0.3334)
-#v3 = (0.1886 0 -0.0667) (0.9428, 0, -0.3333)
-#edge = 0.3266 1.6333 4.08
-#volume = 0.0041 0.5132 8
-
r=0.1*wallThickness
bbb=Body()
wire=False
@@ -269,28 +255,4 @@
if (O.bodies[lidID]):
O.bodies.erase(lidID)
-
-
-O.engines=O.engines+[PotentialParticleVTKRecorder(fileName='/home/boon/yadeRev/trunk/examples/PotentialParticles/vtk/cubeScaled',iterPeriod=1000,sampleX=50,sampleY=50,sampleZ=50)]
-
-#for b in O.bodies:
-# b.state.blockedDOFs=['rx','ry','rz','x','z']
-
-#O.bodies[0].state.pos = [0,meanSize*10.0,0]
-#O.bodies[0].state.vel =[0,0.0,0]
-O.dt = 0.2*sqrt(O.bodies[0].state.mass*0.33333333/1.0e8)
-#from yade import qt
-#qt.Controller()
-#qt.View()
-
-#Gl1_PotentialParticle.sizeX = 30
-#Gl1_PotentialParticle.sizeY = 30
-#Gl1_PotentialParticle.sizeZ = 30
-#from yade import qt
-#qt.View()
-
-import yade.timing
-O.timingEnabled = True
-yade.timing.reset()
-#O.engines[2].geomDispatcher.functors[0].timingDeltas.data
-#yade.timing.stats()
+O.dt = 0.2*sqrt(O.bodies[0].state.mass*0.33333333/1.0e8)
=== added file 'lib/computational-geometry/MarchingCube.cpp'
--- lib/computational-geometry/MarchingCube.cpp 1970-01-01 00:00:00 +0000
+++ lib/computational-geometry/MarchingCube.cpp 2016-01-19 00:59:38 +0000
@@ -0,0 +1,558 @@
+/*************************************************************************
+* Copyright (C) 2004 by Olivier Galizzi *
+* olivier.galizzi@xxxxxxx *
+* *
+* This program is free software; it is licensed under the terms of the *
+* GNU General Public License v2 or later. See file LICENSE for details. *
+*************************************************************************/
+
+#include "MarchingCube.hpp"
+
+
+MarchingCube::MarchingCube( )
+{
+ nbTriangles = 0;
+}
+
+
+MarchingCube::~MarchingCube()
+{
+}
+
+
+void MarchingCube::init(int sx, int sy, int sz, const Vector3r& min, const Vector3r& max)
+{
+ sizeX = sx;
+ sizeY = sy;
+ sizeZ = sz;
+
+ alpha = (max-min).cwiseProduct(Vector3r(1.0/(float)sx, 1.0/(float)sy, 1.0/(float)sz));
+ beta = ((min-max).cwiseProduct(Vector3r(1.0/(Real)sx, 1.0/(Real)sy, 1.0/(Real)sz)))*.5+min;
+
+ triangles.resize(16*sx*sy*sz);
+ normals.resize(16*sx*sy*sz);
+
+ positions.resize(sizeX);
+ for(int i=0;i<sizeX;i++)
+ positions[i].resize(sizeY);
+ for(int i=0;i<sizeX;i++)
+ for(int j=0;j<sizeY;j++)
+ positions[i][j].resize(sizeZ);
+
+ for(int i=0;i<sizeX;i++)
+ for(int j=0;j<sizeY;j++)
+ for(int k=0;k<sizeZ;k++)
+ positions[i][j][k] = alpha.cwiseProduct(Vector3r(i,j,k))+beta;
+}
+
+
+void MarchingCube::resizeScalarField(vector<vector<vector<float> > >& scalarField, int sx, int sy, int sz)
+{
+ sizeX = sx;
+ sizeY = sy;
+ sizeZ = sz;
+
+ scalarField.resize(sx);
+ for(int i=0;i<sx;i++)
+ scalarField[i].resize(sy);
+ for(int i=0;i<sx;i++)
+ for(int j=0;j<sy;j++)
+ scalarField[i][j].resize(sz,0);
+}
+
+
+void MarchingCube::computeTriangulation(const vector<vector<vector<float> > >& scalarField, float iso)
+{
+ isoValue = iso;
+ nbTriangles = 0;
+ for(int i=1;i<sizeX-2;i++)
+ for(int j=1;j<sizeY-2;j++)
+ for(int k=1;k<sizeZ-2;k++)
+ polygonize(scalarField,i,j,k);
+}
+
+
+void MarchingCube::polygonize(const vector<vector<vector<float> > >& scalarField, int x,int y,int z)
+{
+ static vector<float> cellValues(8);
+ static vector<Vector3r> cellPositions(8);
+ static vector<Vector3r> vertexList(12);
+
+ cellValues[0] = scalarField[x][y][z];
+ cellValues[1] = scalarField[x+1][y][z];
+ cellValues[2] = scalarField[x+1][y][z+1];
+ cellValues[3] = scalarField[x][y][z+1];
+ cellValues[4] = scalarField[x][y+1][z];
+ cellValues[5] = scalarField[x+1][y+1][z];
+ cellValues[6] = scalarField[x+1][y+1][z+1];
+ cellValues[7] = scalarField[x][y+1][z+1];
+
+ cellPositions[0] = positions[x][y][z];
+ cellPositions[1] = positions[x+1][y][z];
+ cellPositions[2] = positions[x+1][y][z+1];
+ cellPositions[3] = positions[x][y][z+1];
+ cellPositions[4] = positions[x][y+1][z];
+ cellPositions[5] = positions[x+1][y+1][z];
+ cellPositions[6] = positions[x+1][y+1][z+1];
+ cellPositions[7] = positions[x][y+1][z+1];
+
+ /* compute index in edgeArray that tells how the surface intersect the cell */
+ int index = 0;
+ if (cellValues[0]>isoValue)
+ index |= 1;
+ if (cellValues[1]>isoValue)
+ index |= 2;
+ if (cellValues[2]>isoValue)
+ index |= 4;
+ if (cellValues[3]>isoValue)
+ index |= 8;
+ if (cellValues[4]>isoValue)
+ index |= 16;
+ if (cellValues[5]>isoValue)
+ index |= 32;
+ if (cellValues[6]>isoValue)
+ index |= 64;
+ if (cellValues[7]>isoValue)
+ index |= 128;
+
+ /* compute position of vertices where the surface interesct the cell*/
+ int config = edgeArray[index];
+ if (config == 0) /* the cell is not intersected by surface */
+ return;
+ if (config & 1)
+ interpolate(cellPositions[0], cellPositions[1], cellValues[0], cellValues[1], vertexList[0]);
+ if (config & 2)
+ interpolate(cellPositions[1], cellPositions[2], cellValues[1], cellValues[2], vertexList[1]);
+ if (config & 4)
+ interpolate(cellPositions[2], cellPositions[3], cellValues[2], cellValues[3], vertexList[2]);
+ if (config & 8)
+ interpolate(cellPositions[3], cellPositions[0], cellValues[3], cellValues[0], vertexList[3]);
+ if (config & 16)
+ interpolate(cellPositions[4], cellPositions[5], cellValues[4], cellValues[5], vertexList[4]);
+ if (config & 32)
+ interpolate(cellPositions[5], cellPositions[6], cellValues[5], cellValues[6], vertexList[5]);
+ if (config & 64)
+ interpolate(cellPositions[6], cellPositions[7], cellValues[6], cellValues[7], vertexList[6]);
+ if (config & 128)
+ interpolate(cellPositions[7], cellPositions[4], cellValues[7], cellValues[4], vertexList[7]);
+ if (config & 256)
+ interpolate(cellPositions[0], cellPositions[4], cellValues[0], cellValues[4], vertexList[8]);
+ if (config & 512)
+ interpolate(cellPositions[1], cellPositions[5], cellValues[1], cellValues[5], vertexList[9]);
+ if (config & 1024)
+ interpolate(cellPositions[2], cellPositions[6], cellValues[2], cellValues[6], vertexList[10]);
+ if (config & 2048)
+ interpolate(cellPositions[3], cellPositions[7], cellValues[3], cellValues[7], vertexList[11]);
+
+ /* compute triangles and normals*/
+ int offset,i;
+ const int * tri = triTable[index];
+
+ for (i=0; tri[i]!=-1; ++i)
+ {
+ offset = nbTriangles*3;
+
+ index = tri[i];
+ triangles[offset] = vertexList[index];
+ computeNormal(scalarField,x,y,z,offset,index);
+
+ offset++;
+ index = tri[++i];
+ triangles[offset] = vertexList[index];
+ computeNormal(scalarField,x,y,z,offset,index);
+
+ offset++;
+ index = tri[++i];
+ triangles[offset] = vertexList[index];
+ computeNormal(scalarField,x,y,z,offset,index);
+
+ nbTriangles++;
+ }
+}
+
+
+void MarchingCube::computeNormal(const vector<vector<vector<float> > >& scalarField, int x, int y, int z,int offset, int triangleNum)
+{
+ switch (triangleNum)
+ {
+ case 0 : normals[offset] = computeNormalX(scalarField,x, y, z); break;
+ case 1 : normals[offset] = computeNormalZ(scalarField,x+1, y, z); break;
+ case 2 : normals[offset] = computeNormalX(scalarField,x, y, z+1); break;
+ case 3 : normals[offset] = computeNormalZ(scalarField,x, y, z); break;
+ case 4 : normals[offset] = computeNormalX(scalarField,x, y+1, z); break;
+ case 5 : normals[offset] = computeNormalZ(scalarField,x+1, y+1, z); break;
+ case 6 : normals[offset] = computeNormalX(scalarField,x, y+1, z+1); break;
+ case 7 : normals[offset] = computeNormalZ(scalarField,x, y+1, z); break;
+ case 8 : normals[offset] = computeNormalY(scalarField,x, y, z); break;
+ case 9 : normals[offset] = computeNormalY(scalarField,x+1, y, z); break;
+ case 10 : normals[offset] = computeNormalY(scalarField,x+1, y, z+1); break;
+ case 11 : normals[offset] = computeNormalY(scalarField,x, y, z+1); break;
+ }
+}
+
+
+void MarchingCube::interpolate(const Vector3r& vect1, const Vector3r& vect2, float val1, float val2, Vector3r& vect)
+{
+ vect[0] = interpolateValue(val1, val2, vect1[0], vect2[0]);
+ vect[1] = interpolateValue(val1, val2, vect1[1], vect2[1]);
+ vect[2] = interpolateValue(val1, val2, vect1[2], vect2[2]);
+}
+
+
+float MarchingCube::interpolateValue( float val1, float val2, float val_cible1, float val_cible2)
+{
+ float a = (val_cible2-val_cible1)/(val2-val1);
+ float b = val_cible1-a*val1;
+ return a*isoValue+b;
+}
+
+
+const Vector3r& MarchingCube::computeNormalX(const vector<vector<vector<float> > >& scalarField, int x, int y, int z)
+{
+ static Vector3r normal;
+
+ float xyz = scalarField[x][y][z];
+ float xp1yz = scalarField[x+1][y][z];
+
+ normal[0] = interpolateValue( xp1yz, xyz, scalarField[x+2][y][z]-xyz, xp1yz-scalarField[x-1][y][z] );
+ normal[1] = interpolateValue( xyz, xp1yz, scalarField[x][y+1][z], scalarField[x+1][y+1][z] ) -
+ interpolateValue( xyz, xp1yz, scalarField[x][y-1][z], scalarField[x+1][y-1][z] );
+ normal[2] = interpolateValue( xyz, xp1yz, scalarField[x][y][z+1], scalarField[x+1][y][z+1] ) -
+ interpolateValue( xyz, xp1yz, scalarField[x][y][z-1], scalarField[x+1][y][z-1] );
+
+ normal.normalize();
+ return normal;
+}
+
+
+const Vector3r& MarchingCube::computeNormalY(const vector<vector<vector<float> > >& scalarField, int x, int y, int z )
+{
+ static Vector3r normal;
+
+ float xyz = scalarField[x][y][z];
+ float xyp1z = scalarField[x][y+1][z];
+
+ normal[0] = interpolateValue( xyz, xyp1z, scalarField[x+1][y][z], scalarField[x+1][y+1][z] ) -
+ interpolateValue( xyz, xyp1z, scalarField[x-1][y][z], scalarField[x-1][y+1][z] );
+ normal[1] = interpolateValue( xyp1z, xyz, scalarField[x][y+2][z]-xyz, xyp1z-scalarField[x][y-1][z] );
+ normal[2] = interpolateValue( xyz, xyp1z, scalarField[x][y][z+1], scalarField[x][y+1][z+1] ) -
+ interpolateValue( xyz, xyp1z, scalarField[x][y][z-1], scalarField[x][y+1][z-1] );
+
+ normal.normalize();
+ return normal;
+}
+
+
+const Vector3r& MarchingCube::computeNormalZ(const vector<vector<vector<float> > >& scalarField, int x, int y, int z)
+{
+ static Vector3r normal;
+
+ float xyz = scalarField[x][y][z];
+ float xyzp1 = scalarField[x][y][z+1];
+
+ normal[0] = interpolateValue( xyz, xyzp1, scalarField[x+1][y][z], scalarField[x+1][y][z+1] ) -
+ interpolateValue( xyz, xyzp1, scalarField[x-1][y][z], scalarField[x-1][y][z+1] );
+
+ normal[1] = interpolateValue( xyz, xyzp1, scalarField[x][y+1][z], scalarField[x][y+1][z+1] ) -
+ interpolateValue( xyz, xyzp1, scalarField[x][y-1][z], scalarField[x][y-1][z+1] );
+ normal[2] = interpolateValue( xyzp1, xyz, scalarField[x][y][z+2]-xyz, xyzp1-scalarField[x][y][z-1] );
+
+ normal.normalize();
+ return normal;
+}
+
+
+const int MarchingCube::edgeArray[256] = {
+ 0x0 , 0x109, 0x203, 0x30a, 0x406, 0x50f, 0x605, 0x70c,
+ 0x80c, 0x905, 0xa0f, 0xb06, 0xc0a, 0xd03, 0xe09, 0xf00,
+ 0x190, 0x99 , 0x393, 0x29a, 0x596, 0x49f, 0x795, 0x69c,
+ 0x99c, 0x895, 0xb9f, 0xa96, 0xd9a, 0xc93, 0xf99, 0xe90,
+ 0x230, 0x339, 0x33 , 0x13a, 0x636, 0x73f, 0x435, 0x53c,
+ 0xa3c, 0xb35, 0x83f, 0x936, 0xe3a, 0xf33, 0xc39, 0xd30,
+ 0x3a0, 0x2a9, 0x1a3, 0xaa , 0x7a6, 0x6af, 0x5a5, 0x4ac,
+ 0xbac, 0xaa5, 0x9af, 0x8a6, 0xfaa, 0xea3, 0xda9, 0xca0,
+ 0x460, 0x569, 0x663, 0x76a, 0x66 , 0x16f, 0x265, 0x36c,
+ 0xc6c, 0xd65, 0xe6f, 0xf66, 0x86a, 0x963, 0xa69, 0xb60,
+ 0x5f0, 0x4f9, 0x7f3, 0x6fa, 0x1f6, 0xff , 0x3f5, 0x2fc,
+ 0xdfc, 0xcf5, 0xfff, 0xef6, 0x9fa, 0x8f3, 0xbf9, 0xaf0,
+ 0x650, 0x759, 0x453, 0x55a, 0x256, 0x35f, 0x55 , 0x15c,
+ 0xe5c, 0xf55, 0xc5f, 0xd56, 0xa5a, 0xb53, 0x859, 0x950,
+ 0x7c0, 0x6c9, 0x5c3, 0x4ca, 0x3c6, 0x2cf, 0x1c5, 0xcc ,
+ 0xfcc, 0xec5, 0xdcf, 0xcc6, 0xbca, 0xac3, 0x9c9, 0x8c0,
+ 0x8c0, 0x9c9, 0xac3, 0xbca, 0xcc6, 0xdcf, 0xec5, 0xfcc,
+ 0xcc , 0x1c5, 0x2cf, 0x3c6, 0x4ca, 0x5c3, 0x6c9, 0x7c0,
+ 0x950, 0x859, 0xb53, 0xa5a, 0xd56, 0xc5f, 0xf55, 0xe5c,
+ 0x15c, 0x55 , 0x35f, 0x256, 0x55a, 0x453, 0x759, 0x650,
+ 0xaf0, 0xbf9, 0x8f3, 0x9fa, 0xef6, 0xfff, 0xcf5, 0xdfc,
+ 0x2fc, 0x3f5, 0xff , 0x1f6, 0x6fa, 0x7f3, 0x4f9, 0x5f0,
+ 0xb60, 0xa69, 0x963, 0x86a, 0xf66, 0xe6f, 0xd65, 0xc6c,
+ 0x36c, 0x265, 0x16f, 0x66 , 0x76a, 0x663, 0x569, 0x460,
+ 0xca0, 0xda9, 0xea3, 0xfaa, 0x8a6, 0x9af, 0xaa5, 0xbac,
+ 0x4ac, 0x5a5, 0x6af, 0x7a6, 0xaa , 0x1a3, 0x2a9, 0x3a0,
+ 0xd30, 0xc39, 0xf33, 0xe3a, 0x936, 0x83f, 0xb35, 0xa3c,
+ 0x53c, 0x435, 0x73f, 0x636, 0x13a, 0x33 , 0x339, 0x230,
+ 0xe90, 0xf99, 0xc93, 0xd9a, 0xa96, 0xb9f, 0x895, 0x99c,
+ 0x69c, 0x795, 0x49f, 0x596, 0x29a, 0x393, 0x99 , 0x190,
+ 0xf00, 0xe09, 0xd03, 0xc0a, 0xb06, 0xa0f, 0x905, 0x80c,
+ 0x70c, 0x605, 0x50f, 0x406, 0x30a, 0x203, 0x109, 0x0
+};
+
+
+const int MarchingCube::triTable[256][16] = {
+ {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 0, 8, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 0, 1, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 1, 8, 3, 9, 8, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 1, 2, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 0, 8, 3, 1, 2, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 9, 2, 10, 0, 2, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 2, 8, 3, 2, 10, 8, 10, 9, 8, -1, -1, -1, -1, -1, -1, -1},
+ { 3, 11, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 0, 11, 2, 8, 11, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 1, 9, 0, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 1, 11, 2, 1, 9, 11, 9, 8, 11, -1, -1, -1, -1, -1, -1, -1},
+ { 3, 10, 1, 11, 10, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 0, 10, 1, 0, 8, 10, 8, 11, 10, -1, -1, -1, -1, -1, -1, -1},
+ { 3, 9, 0, 3, 11, 9, 11, 10, 9, -1, -1, -1, -1, -1, -1, -1},
+ { 9, 8, 10, 10, 8, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 4, 7, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 4, 3, 0, 7, 3, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 0, 1, 9, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 4, 1, 9, 4, 7, 1, 7, 3, 1, -1, -1, -1, -1, -1, -1, -1},
+ { 1, 2, 10, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 3, 4, 7, 3, 0, 4, 1, 2, 10, -1, -1, -1, -1, -1, -1, -1},
+ { 9, 2, 10, 9, 0, 2, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1},
+ { 2, 10, 9, 2, 9, 7, 2, 7, 3, 7, 9, 4, -1, -1, -1, -1},
+ { 8, 4, 7, 3, 11, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {11, 4, 7, 11, 2, 4, 2, 0, 4, -1, -1, -1, -1, -1, -1, -1},
+ { 9, 0, 1, 8, 4, 7, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1},
+ { 4, 7, 11, 9, 4, 11, 9, 11, 2, 9, 2, 1, -1, -1, -1, -1},
+ { 3, 10, 1, 3, 11, 10, 7, 8, 4, -1, -1, -1, -1, -1, -1, -1},
+ { 1, 11, 10, 1, 4, 11, 1, 0, 4, 7, 11, 4, -1, -1, -1, -1},
+ { 4, 7, 8, 9, 0, 11, 9, 11, 10, 11, 0, 3, -1, -1, -1, -1},
+ { 4, 7, 11, 4, 11, 9, 9, 11, 10, -1, -1, -1, -1, -1, -1, -1},
+ { 9, 5, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 9, 5, 4, 0, 8, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 0, 5, 4, 1, 5, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 8, 5, 4, 8, 3, 5, 3, 1, 5, -1, -1, -1, -1, -1, -1, -1},
+ { 1, 2, 10, 9, 5, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 3, 0, 8, 1, 2, 10, 4, 9, 5, -1, -1, -1, -1, -1, -1, -1},
+ { 5, 2, 10, 5, 4, 2, 4, 0, 2, -1, -1, -1, -1, -1, -1, -1},
+ { 2, 10, 5, 3, 2, 5, 3, 5, 4, 3, 4, 8, -1, -1, -1, -1},
+ { 9, 5, 4, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 0, 11, 2, 0, 8, 11, 4, 9, 5, -1, -1, -1, -1, -1, -1, -1},
+ { 0, 5, 4, 0, 1, 5, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1},
+ { 2, 1, 5, 2, 5, 8, 2, 8, 11, 4, 8, 5, -1, -1, -1, -1},
+ {10, 3, 11, 10, 1, 3, 9, 5, 4, -1, -1, -1, -1, -1, -1, -1},
+ { 4, 9, 5, 0, 8, 1, 8, 10, 1, 8, 11, 10, -1, -1, -1, -1},
+ { 5, 4, 0, 5, 0, 11, 5, 11, 10, 11, 0, 3, -1, -1, -1, -1},
+ { 5, 4, 8, 5, 8, 10, 10, 8, 11, -1, -1, -1, -1, -1, -1, -1},
+ { 9, 7, 8, 5, 7, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 9, 3, 0, 9, 5, 3, 5, 7, 3, -1, -1, -1, -1, -1, -1, -1},
+ { 0, 7, 8, 0, 1, 7, 1, 5, 7, -1, -1, -1, -1, -1, -1, -1},
+ { 1, 5, 3, 3, 5, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 9, 7, 8, 9, 5, 7, 10, 1, 2, -1, -1, -1, -1, -1, -1, -1},
+ {10, 1, 2, 9, 5, 0, 5, 3, 0, 5, 7, 3, -1, -1, -1, -1},
+ { 8, 0, 2, 8, 2, 5, 8, 5, 7, 10, 5, 2, -1, -1, -1, -1},
+ { 2, 10, 5, 2, 5, 3, 3, 5, 7, -1, -1, -1, -1, -1, -1, -1},
+ { 7, 9, 5, 7, 8, 9, 3, 11, 2, -1, -1, -1, -1, -1, -1, -1},
+ { 9, 5, 7, 9, 7, 2, 9, 2, 0, 2, 7, 11, -1, -1, -1, -1},
+ { 2, 3, 11, 0, 1, 8, 1, 7, 8, 1, 5, 7, -1, -1, -1, -1},
+ {11, 2, 1, 11, 1, 7, 7, 1, 5, -1, -1, -1, -1, -1, -1, -1},
+ { 9, 5, 8, 8, 5, 7, 10, 1, 3, 10, 3, 11, -1, -1, -1, -1},
+ { 5, 7, 0, 5, 0, 9, 7, 11, 0, 1, 0, 10, 11, 10, 0, -1},
+ {11, 10, 0, 11, 0, 3, 10, 5, 0, 8, 0, 7, 5, 7, 0, -1},
+ {11, 10, 5, 7, 11, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {10, 6, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 0, 8, 3, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 9, 0, 1, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 1, 8, 3, 1, 9, 8, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1},
+ { 1, 6, 5, 2, 6, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 1, 6, 5, 1, 2, 6, 3, 0, 8, -1, -1, -1, -1, -1, -1, -1},
+ { 9, 6, 5, 9, 0, 6, 0, 2, 6, -1, -1, -1, -1, -1, -1, -1},
+ { 5, 9, 8, 5, 8, 2, 5, 2, 6, 3, 2, 8, -1, -1, -1, -1},
+ { 2, 3, 11, 10, 6, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {11, 0, 8, 11, 2, 0, 10, 6, 5, -1, -1, -1, -1, -1, -1, -1},
+ { 0, 1, 9, 2, 3, 11, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1},
+ { 5, 10, 6, 1, 9, 2, 9, 11, 2, 9, 8, 11, -1, -1, -1, -1},
+ { 6, 3, 11, 6, 5, 3, 5, 1, 3, -1, -1, -1, -1, -1, -1, -1},
+ { 0, 8, 11, 0, 11, 5, 0, 5, 1, 5, 11, 6, -1, -1, -1, -1},
+ { 3, 11, 6, 0, 3, 6, 0, 6, 5, 0, 5, 9, -1, -1, -1, -1},
+ { 6, 5, 9, 6, 9, 11, 11, 9, 8, -1, -1, -1, -1, -1, -1, -1},
+ { 5, 10, 6, 4, 7, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 4, 3, 0, 4, 7, 3, 6, 5, 10, -1, -1, -1, -1, -1, -1, -1},
+ { 1, 9, 0, 5, 10, 6, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1},
+ {10, 6, 5, 1, 9, 7, 1, 7, 3, 7, 9, 4, -1, -1, -1, -1},
+ { 6, 1, 2, 6, 5, 1, 4, 7, 8, -1, -1, -1, -1, -1, -1, -1},
+ { 1, 2, 5, 5, 2, 6, 3, 0, 4, 3, 4, 7, -1, -1, -1, -1},
+ { 8, 4, 7, 9, 0, 5, 0, 6, 5, 0, 2, 6, -1, -1, -1, -1},
+ { 7, 3, 9, 7, 9, 4, 3, 2, 9, 5, 9, 6, 2, 6, 9, -1},
+ { 3, 11, 2, 7, 8, 4, 10, 6, 5, -1, -1, -1, -1, -1, -1, -1},
+ { 5, 10, 6, 4, 7, 2, 4, 2, 0, 2, 7, 11, -1, -1, -1, -1},
+ { 0, 1, 9, 4, 7, 8, 2, 3, 11, 5, 10, 6, -1, -1, -1, -1},
+ { 9, 2, 1, 9, 11, 2, 9, 4, 11, 7, 11, 4, 5, 10, 6, -1},
+ { 8, 4, 7, 3, 11, 5, 3, 5, 1, 5, 11, 6, -1, -1, -1, -1},
+ { 5, 1, 11, 5, 11, 6, 1, 0, 11, 7, 11, 4, 0, 4, 11, -1},
+ { 0, 5, 9, 0, 6, 5, 0, 3, 6, 11, 6, 3, 8, 4, 7, -1},
+ { 6, 5, 9, 6, 9, 11, 4, 7, 9, 7, 11, 9, -1, -1, -1, -1},
+ { 10, 4, 9, 6, 4, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 4, 10, 6, 4, 9, 10, 0, 8, 3, -1, -1, -1, -1, -1, -1, -1},
+ {10, 0, 1, 10, 6, 0, 6, 4, 0, -1, -1, -1, -1, -1, -1, -1},
+ { 8, 3, 1, 8, 1, 6, 8, 6, 4, 6, 1, 10, -1, -1, -1, -1},
+ { 1, 4, 9, 1, 2, 4, 2, 6, 4, -1, -1, -1, -1, -1, -1, -1},
+ { 3, 0, 8, 1, 2, 9, 2, 4, 9, 2, 6, 4, -1, -1, -1, -1},
+ { 0, 2, 4, 4, 2, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 8, 3, 2, 8, 2, 4, 4, 2, 6, -1, -1, -1, -1, -1, -1, -1},
+ {10, 4, 9, 10, 6, 4, 11, 2, 3, -1, -1, -1, -1, -1, -1, -1},
+ { 0, 8, 2, 2, 8, 11, 4, 9, 10, 4, 10, 6, -1, -1, -1, -1},
+ { 3, 11, 2, 0, 1, 6, 0, 6, 4, 6, 1, 10, -1, -1, -1, -1},
+ { 6, 4, 1, 6, 1, 10, 4, 8, 1, 2, 1, 11, 8, 11, 1, -1},
+ { 9, 6, 4, 9, 3, 6, 9, 1, 3, 11, 6, 3, -1, -1, -1, -1},
+ { 8, 11, 1, 8, 1, 0, 11, 6, 1, 9, 1, 4, 6, 4, 1, -1},
+ { 3, 11, 6, 3, 6, 0, 0, 6, 4, -1, -1, -1, -1, -1, -1, -1},
+ { 6, 4, 8, 11, 6, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 7, 10, 6, 7, 8, 10, 8, 9, 10, -1, -1, -1, -1, -1, -1, -1},
+ { 0, 7, 3, 0, 10, 7, 0, 9, 10, 6, 7, 10, -1, -1, -1, -1},
+ {10, 6, 7, 1, 10, 7, 1, 7, 8, 1, 8, 0, -1, -1, -1, -1},
+ {10, 6, 7, 10, 7, 1, 1, 7, 3, -1, -1, -1, -1, -1, -1, -1},
+ { 1, 2, 6, 1, 6, 8, 1, 8, 9, 8, 6, 7, -1, -1, -1, -1},
+ { 2, 6, 9, 2, 9, 1, 6, 7, 9, 0, 9, 3, 7, 3, 9, -1},
+ { 7, 8, 0, 7, 0, 6, 6, 0, 2, -1, -1, -1, -1, -1, -1, -1},
+ { 7, 3, 2, 6, 7, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 2, 3, 11, 10, 6, 8, 10, 8, 9, 8, 6, 7, -1, -1, -1, -1},
+ { 2, 0, 7, 2, 7, 11, 0, 9, 7, 6, 7, 10, 9, 10, 7, -1},
+ { 1, 8, 0, 1, 7, 8, 1, 10, 7, 6, 7, 10, 2, 3, 11, -1},
+ {11, 2, 1, 11, 1, 7, 10, 6, 1, 6, 7, 1, -1, -1, -1, -1},
+ { 8, 9, 6, 8, 6, 7, 9, 1, 6, 11, 6, 3, 1, 3, 6, -1},
+ { 0, 9, 1, 11, 6, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 7, 8, 0, 7, 0, 6, 3, 11, 0, 11, 6, 0, -1, -1, -1, -1},
+ { 7, 11, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 7, 6, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 3, 0, 8, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 0, 1, 9, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 8, 1, 9, 8, 3, 1, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1},
+ {10, 1, 2, 6, 11, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 1, 2, 10, 3, 0, 8, 6, 11, 7, -1, -1, -1, -1, -1, -1, -1},
+ { 2, 9, 0, 2, 10, 9, 6, 11, 7, -1, -1, -1, -1, -1, -1, -1},
+ { 6, 11, 7, 2, 10, 3, 10, 8, 3, 10, 9, 8, -1, -1, -1, -1},
+ { 7, 2, 3, 6, 2, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 7, 0, 8, 7, 6, 0, 6, 2, 0, -1, -1, -1, -1, -1, -1, -1},
+ { 2, 7, 6, 2, 3, 7, 0, 1, 9, -1, -1, -1, -1, -1, -1, -1},
+ { 1, 6, 2, 1, 8, 6, 1, 9, 8, 8, 7, 6, -1, -1, -1, -1},
+ {10, 7, 6, 10, 1, 7, 1, 3, 7, -1, -1, -1, -1, -1, -1, -1},
+ {10, 7, 6, 1, 7, 10, 1, 8, 7, 1, 0, 8, -1, -1, -1, -1},
+ { 0, 3, 7, 0, 7, 10, 0, 10, 9, 6, 10, 7, -1, -1, -1, -1},
+ { 7, 6, 10, 7, 10, 8, 8, 10, 9, -1, -1, -1, -1, -1, -1, -1},
+ { 6, 8, 4, 11, 8, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 3, 6, 11, 3, 0, 6, 0, 4, 6, -1, -1, -1, -1, -1, -1, -1},
+ { 8, 6, 11, 8, 4, 6, 9, 0, 1, -1, -1, -1, -1, -1, -1, -1},
+ { 9, 4, 6, 9, 6, 3, 9, 3, 1, 11, 3, 6, -1, -1, -1, -1},
+ { 6, 8, 4, 6, 11, 8, 2, 10, 1, -1, -1, -1, -1, -1, -1, -1},
+ { 1, 2, 10, 3, 0, 11, 0, 6, 11, 0, 4, 6, -1, -1, -1, -1},
+ { 4, 11, 8, 4, 6, 11, 0, 2, 9, 2, 10, 9, -1, -1, -1, -1},
+ {10, 9, 3, 10, 3, 2, 9, 4, 3, 11, 3, 6, 4, 6, 3, -1},
+ { 8, 2, 3, 8, 4, 2, 4, 6, 2, -1, -1, -1, -1, -1, -1, -1},
+ { 0, 4, 2, 4, 6, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 1, 9, 0, 2, 3, 4, 2, 4, 6, 4, 3, 8, -1, -1, -1, -1},
+ { 1, 9, 4, 1, 4, 2, 2, 4, 6, -1, -1, -1, -1, -1, -1, -1},
+ { 8, 1, 3, 8, 6, 1, 8, 4, 6, 6, 10, 1, -1, -1, -1, -1},
+ {10, 1, 0, 10, 0, 6, 6, 0, 4, -1, -1, -1, -1, -1, -1, -1},
+ { 4, 6, 3, 4, 3, 8, 6, 10, 3, 0, 3, 9, 10, 9, 3, -1},
+ {10, 9, 4, 6, 10, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 4, 9, 5, 7, 6, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 0, 8, 3, 4, 9, 5, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1},
+ { 5, 0, 1, 5, 4, 0, 7, 6, 11, -1, -1, -1, -1, -1, -1, -1},
+ {11, 7, 6, 8, 3, 4, 3, 5, 4, 3, 1, 5, -1, -1, -1, -1},
+ { 9, 5, 4, 10, 1, 2, 7, 6, 11, -1, -1, -1, -1, -1, -1, -1},
+ { 6, 11, 7, 1, 2, 10, 0, 8, 3, 4, 9, 5, -1, -1, -1, -1},
+ { 7, 6, 11, 5, 4, 10, 4, 2, 10, 4, 0, 2, -1, -1, -1, -1},
+ { 3, 4, 8, 3, 5, 4, 3, 2, 5, 10, 5, 2, 11, 7, 6, -1},
+ { 7, 2, 3, 7, 6, 2, 5, 4, 9, -1, -1, -1, -1, -1, -1, -1},
+ { 9, 5, 4, 0, 8, 6, 0, 6, 2, 6, 8, 7, -1, -1, -1, -1},
+ { 3, 6, 2, 3, 7, 6, 1, 5, 0, 5, 4, 0, -1, -1, -1, -1},
+ { 6, 2, 8, 6, 8, 7, 2, 1, 8, 4, 8, 5, 1, 5, 8, -1},
+ { 9, 5, 4, 10, 1, 6, 1, 7, 6, 1, 3, 7, -1, -1, -1, -1},
+ { 1, 6, 10, 1, 7, 6, 1, 0, 7, 8, 7, 0, 9, 5, 4, -1},
+ { 4, 0, 10, 4, 10, 5, 0, 3, 10, 6, 10, 7, 3, 7, 10, -1},
+ { 7, 6, 10, 7, 10, 8, 5, 4, 10, 4, 8, 10, -1, -1, -1, -1},
+ { 6, 9, 5, 6, 11, 9, 11, 8, 9, -1, -1, -1, -1, -1, -1, -1},
+ { 3, 6, 11, 0, 6, 3, 0, 5, 6, 0, 9, 5, -1, -1, -1, -1},
+ { 0, 11, 8, 0, 5, 11, 0, 1, 5, 5, 6, 11, -1, -1, -1, -1},
+ { 6, 11, 3, 6, 3, 5, 5, 3, 1, -1, -1, -1, -1, -1, -1, -1},
+ { 1, 2, 10, 9, 5, 11, 9, 11, 8, 11, 5, 6, -1, -1, -1, -1},
+ { 0, 11, 3, 0, 6, 11, 0, 9, 6, 5, 6, 9, 1, 2, 10, -1},
+ {11, 8, 5, 11, 5, 6, 8, 0, 5, 10, 5, 2, 0, 2, 5, -1},
+ { 6, 11, 3, 6, 3, 5, 2, 10, 3, 10, 5, 3, -1, -1, -1, -1},
+ { 5, 8, 9, 5, 2, 8, 5, 6, 2, 3, 8, 2, -1, -1, -1, -1},
+ { 9, 5, 6, 9, 6, 0, 0, 6, 2, -1, -1, -1, -1, -1, -1, -1},
+ { 1, 5, 8, 1, 8, 0, 5, 6, 8, 3, 8, 2, 6, 2, 8, -1},
+ { 1, 5, 6, 2, 1, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 1, 3, 6, 1, 6, 10, 3, 8, 6, 5, 6, 9, 8, 9, 6, -1},
+ {10, 1, 0, 10, 0, 6, 9, 5, 0, 5, 6, 0, -1, -1, -1, -1},
+ { 0, 3, 8, 5, 6, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {10, 5, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {11, 5, 10, 7, 5, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {11, 5, 10, 11, 7, 5, 8, 3, 0, -1, -1, -1, -1, -1, -1, -1},
+ { 5, 11, 7, 5, 10, 11, 1, 9, 0, -1, -1, -1, -1, -1, -1, -1},
+ {10, 7, 5, 10, 11, 7, 9, 8, 1, 8, 3, 1, -1, -1, -1, -1},
+ {11, 1, 2, 11, 7, 1, 7, 5, 1, -1, -1, -1, -1, -1, -1, -1},
+ { 0, 8, 3, 1, 2, 7, 1, 7, 5, 7, 2, 11, -1, -1, -1, -1},
+ { 9, 7, 5, 9, 2, 7, 9, 0, 2, 2, 11, 7, -1, -1, -1, -1},
+ { 7, 5, 2, 7, 2, 11, 5, 9, 2, 3, 2, 8, 9, 8, 2, -1},
+ { 2, 5, 10, 2, 3, 5, 3, 7, 5, -1, -1, -1, -1, -1, -1, -1},
+ { 8, 2, 0, 8, 5, 2, 8, 7, 5, 10, 2, 5, -1, -1, -1, -1},
+ { 9, 0, 1, 5, 10, 3, 5, 3, 7, 3, 10, 2, -1, -1, -1, -1},
+ { 9, 8, 2, 9, 2, 1, 8, 7, 2, 10, 2, 5, 7, 5, 2, -1},
+ { 1, 3, 5, 3, 7, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 0, 8, 7, 0, 7, 1, 1, 7, 5, -1, -1, -1, -1, -1, -1, -1},
+ { 9, 0, 3, 9, 3, 5, 5, 3, 7, -1, -1, -1, -1, -1, -1, -1},
+ { 9, 8, 7, 5, 9, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 5, 8, 4, 5, 10, 8, 10, 11, 8, -1, -1, -1, -1, -1, -1, -1},
+ { 5, 0, 4, 5, 11, 0, 5, 10, 11, 11, 3, 0, -1, -1, -1, -1},
+ { 0, 1, 9, 8, 4, 10, 8, 10, 11, 10, 4, 5, -1, -1, -1, -1},
+ {10, 11, 4, 10, 4, 5, 11, 3, 4, 9, 4, 1, 3, 1, 4, -1},
+ { 2, 5, 1, 2, 8, 5, 2, 11, 8, 4, 5, 8, -1, -1, -1, -1},
+ { 0, 4, 11, 0, 11, 3, 4, 5, 11, 2, 11, 1, 5, 1, 11, -1},
+ { 0, 2, 5, 0, 5, 9, 2, 11, 5, 4, 5, 8, 11, 8, 5, -1},
+ { 9, 4, 5, 2, 11, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 2, 5, 10, 3, 5, 2, 3, 4, 5, 3, 8, 4, -1, -1, -1, -1},
+ { 5, 10, 2, 5, 2, 4, 4, 2, 0, -1, -1, -1, -1, -1, -1, -1},
+ { 3, 10, 2, 3, 5, 10, 3, 8, 5, 4, 5, 8, 0, 1, 9, -1},
+ { 5, 10, 2, 5, 2, 4, 1, 9, 2, 9, 4, 2, -1, -1, -1, -1},
+ { 8, 4, 5, 8, 5, 3, 3, 5, 1, -1, -1, -1, -1, -1, -1, -1},
+ { 0, 4, 5, 1, 0, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 8, 4, 5, 8, 5, 3, 9, 0, 5, 0, 3, 5, -1, -1, -1, -1},
+ { 9, 4, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 4, 11, 7, 4, 9, 11, 9, 10, 11, -1, -1, -1, -1, -1, -1, -1},
+ { 0, 8, 3, 4, 9, 7, 9, 11, 7, 9, 10, 11, -1, -1, -1, -1},
+ { 1, 10, 11, 1, 11, 4, 1, 4, 0, 7, 4, 11, -1, -1, -1, -1},
+ { 3, 1, 4, 3, 4, 8, 1, 10, 4, 7, 4, 11, 10, 11, 4, -1},
+ { 4, 11, 7, 9, 11, 4, 9, 2, 11, 9, 1, 2, -1, -1, -1, -1},
+ { 9, 7, 4, 9, 11, 7, 9, 1, 11, 2, 11, 1, 0, 8, 3, -1},
+ {11, 7, 4, 11, 4, 2, 2, 4, 0, -1, -1, -1, -1, -1, -1, -1},
+ {11, 7, 4, 11, 4, 2, 8, 3, 4, 3, 2, 4, -1, -1, -1, -1},
+ { 2, 9, 10, 2, 7, 9, 2, 3, 7, 7, 4, 9, -1, -1, -1, -1},
+ { 9, 10, 7, 9, 7, 4, 10, 2, 7, 8, 7, 0, 2, 0, 7, -1},
+ { 3, 7, 10, 3, 10, 2, 7, 4, 10, 1, 10, 0, 4, 0, 10, -1},
+ { 1, 10, 2, 8, 7, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 4, 9, 1, 4, 1, 7, 7, 1, 3, -1, -1, -1, -1, -1, -1, -1},
+ { 4, 9, 1, 4, 1, 7, 0, 8, 1, 8, 7, 1, -1, -1, -1, -1},
+ { 4, 0, 3, 7, 4, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 4, 8, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 9, 10, 8, 10, 11, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 3, 0, 9, 3, 9, 11, 11, 9, 10, -1, -1, -1, -1, -1, -1, -1},
+ { 0, 1, 10, 0, 10, 8, 8, 10, 11, -1, -1, -1, -1, -1, -1, -1},
+ { 3, 1, 10, 11, 3, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 1, 2, 11, 1, 11, 9, 9, 11, 8, -1, -1, -1, -1, -1, -1, -1},
+ { 3, 0, 9, 3, 9, 11, 1, 2, 9, 2, 11, 9, -1, -1, -1, -1},
+ { 0, 2, 11, 8, 0, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 3, 2, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 2, 3, 8, 2, 8, 10, 10, 8, 9, -1, -1, -1, -1, -1, -1, -1},
+ { 9, 10, 2, 0, 9, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 2, 3, 8, 2, 8, 10, 0, 1, 8, 1, 10, 8, -1, -1, -1, -1},
+ { 1, 10, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 1, 3, 8, 9, 1, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 0, 9, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ { 0, 3, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}
+};
=== added file 'lib/computational-geometry/MarchingCube.hpp'
--- lib/computational-geometry/MarchingCube.hpp 1970-01-01 00:00:00 +0000
+++ lib/computational-geometry/MarchingCube.hpp 2016-01-19 00:59:38 +0000
@@ -0,0 +1,73 @@
+/*************************************************************************
+* Copyright (C) 2004 by Olivier Galizzi *
+* olivier.galizzi@xxxxxxx *
+* *
+* This program is free software; it is licensed under the terms of the *
+* GNU General Public License v2 or later. See file LICENSE for details. *
+*************************************************************************/
+
+#pragma once
+
+#include <cmath>
+#include <string>
+#include <vector>
+#include <lib/base/Math.hpp>
+
+class MarchingCube
+{
+
+/// ATTRIBUTES
+
+ private : vector<Vector3r> triangles;
+ public : const vector<Vector3r>& getTriangles() { return triangles; }
+
+ private : vector<Vector3r> normals;
+ public : const vector<Vector3r>& getNormals() { return normals; }
+
+ private : int nbTriangles;
+ public : int getNbTriangles() { return nbTriangles; }
+
+ private : Vector3r beta;
+ private : Vector3r alpha;
+ private : int sizeX,sizeY,sizeZ;
+ private : float isoValue;
+
+ private : vector<vector<vector<Vector3r> > > positions;
+ private : static const int edgeArray[256];
+ private : static const int triTable[256][16];
+ Vector3r aNormal;
+
+/// PRIVATE METHOD
+
+ /** triangulate cell (x,y,z) **/
+ private : void polygonize (const vector<vector<vector<float> > >& scalarField, int x, int y, int z);
+
+ /** compute normals of the triangles previously found with polygonizecalcule les normales des triangles trouver dans la case (x,y,z)
+ @param n : indice of the first triangle to process
+ **/
+ private : void computeNormal(const vector<vector<vector<float> > >& scalarField, int x, int y, int z,int offset, int triangleNum);
+
+ /** interpolate coordinates of point vect (that is on isosurface) from coordinates of points vect1 et vect2 **/
+ private : void interpolate (const Vector3r& vect1, const Vector3r& vect2, float val1, float val2,Vector3r& vect);
+
+ /** Same as interpolate but in 1D **/
+ private : float interpolateValue(float val1, float val2, float val_cible1, float val_cible2);
+
+ /** Compute normal to vertice or triangle inside cell (x,y,z) **/
+ private : const Vector3r& computeNormalX(const vector<vector<vector<float> > >& scalarField, int x, int y, int z);
+ private : const Vector3r& computeNormalY(const vector<vector<vector<float> > >& scalarField, int x, int y, int z );
+ private : const Vector3r& computeNormalZ(const vector<vector<vector<float> > >& scalarField, int x, int y, int z);
+
+/// CONSTRUCTOR/DESTRUCTOR
+
+ public : MarchingCube ();
+ public : ~MarchingCube ();
+
+/// PULIC METHODS
+
+ public : void computeTriangulation(const vector<vector<vector<float> > >& scalarField, float iso);
+
+ public : void init(int sx, int sy, int sz, const Vector3r& min, const Vector3r& max);
+
+ public : void resizeScalarField(vector<vector<vector<float> > >& scalarField, int sx, int sy, int sz);
+};
=== modified file 'pkg/common/Gl1_PotentialParticle.cpp'
--- pkg/common/Gl1_PotentialParticle.cpp 2015-12-14 14:25:08 +0000
+++ pkg/common/Gl1_PotentialParticle.cpp 2016-01-19 00:59:38 +0000
@@ -1,42 +1,43 @@
/*CWBoon 2015 */
+#ifdef YADE_POTENTIAL_PARTICLES
#include "Gl1_PotentialParticle.hpp"
//#include<lib-opengl/OpenGLWrapper.hpp>
#include <vtkFloatArray.h>
-#include<vtkUnstructuredGrid.h>
-#include<vtkXMLUnstructuredGridWriter.h>
-#include<vtkTriangle.h>
-#include<vtkSmartPointer.h>
-#include<vtkFloatArray.h>
-#include<vtkCellArray.h>
-#include<vtkCellData.h>
+#include <vtkUnstructuredGrid.h>
+#include <vtkXMLUnstructuredGridWriter.h>
+#include <vtkTriangle.h>
+#include <vtkSmartPointer.h>
+#include <vtkFloatArray.h>
+#include <vtkCellArray.h>
+#include <vtkCellData.h>
#include <vtkSampleFunction.h>
#include <vtkStructuredPoints.h>
-#include<vtkStructuredPointsWriter.h>
-#include<vtkWriter.h>
-#include<vtkExtractVOI.h>
-#include<vtkXMLImageDataWriter.h>
-#include<vtkXMLStructuredGridWriter.h>
-#include<vtkTransformPolyDataFilter.h>
-#include<vtkTransform.h>
+#include <vtkStructuredPointsWriter.h>
+#include <vtkWriter.h>
+#include <vtkExtractVOI.h>
+#include <vtkXMLImageDataWriter.h>
+#include <vtkXMLStructuredGridWriter.h>
+#include <vtkTransformPolyDataFilter.h>
+#include <vtkTransform.h>
#include <vtkContourFilter.h>
#include <vtkPolyDataMapper.h>
-#include<vtkXMLPolyDataWriter.h>
+#include <vtkXMLPolyDataWriter.h>
#include <vtkAppendPolyData.h>
-#include <vtkRenderWindowInteractor.h>
+#include <vtkRenderWindowInteractor.h>
#include <vtkUnsignedCharArray.h>
#include <vtkPointData.h>
#include <vtkLookupTable.h>
#include <vtkXMLDataSetWriter.h>
-//#include<lib-opengl/OpenGLWrapper.hpp>
-#include<pkg/dem/KnKsLaw.hpp>
-#include<pkg/dem/ScGeom.hpp>
-#include<vtkLine.h>
+#include <lib/opengl/OpenGLWrapper.hpp>
+#include <pkg/dem/KnKsLaw.hpp>
+#include <pkg/dem/ScGeom.hpp>
+#include <vtkLine.h>
#include <vtkSphereSource.h>
#include <vtkDiskSource.h>
#include <vtkRegularPolygonSource.h>
@@ -49,29 +50,26 @@
#include <vtkConeSource.h>
#include <vtkCamera.h>
//#include<pkg/dem/Clump.hpp>
-#include<pkg/common/Aabb.hpp>
+#include <pkg/common/Aabb.hpp>
#include <vtkImplicitBoolean.h>
#include <vtkIntArray.h>
-#if 0
-void Gl1_PotentialParticle::calcMinMax(const shared_ptr<Shape>& cm)
-{
- PotentialParticle* pp = static_cast<PotentialParticle*>(cm.get());
+#ifdef YADE_OPENGL
+void Gl1_PotentialParticle::calcMinMax(const shared_ptr<Shape>& cm) {
+ PotentialParticle* pp = static_cast<PotentialParticle*>(cm.get());
int planeNo = pp->d.size();
Real maxD = pp->d[0];
- for (int i=0; i<planeNo; ++i){
+ for (int i=0; i<planeNo; ++i) {
if (pp->d[i] > maxD) {
maxD = pp->d[i];
}
}
- Real R = pp->R;
- Real r = pp->r;
- Real maxTip = R; //std::max(maxD + r, R);
+ //Real R = pp->R;
min =-1.1*pp->minAabb; // 1.5*Vector3r(-maxTip,-maxTip,-maxTip);
max = 1.1*pp->maxAabb; //-1.0*min;
-
+
float dx = (max[0]-min[0])/((float)(sizeX));
float dy = (max[1]-min[1])/((float)(sizeY));
float dz = (max[2]-min[2])/((float)(sizeZ));
@@ -80,21 +78,20 @@
}
-void Gl1_PotentialParticle::generateScalarField(const shared_ptr<Shape>& cm)
-{
- for(int i=0;i<sizeX;i++){
- for(int j=0;j<sizeY;j++){
- for(int k=0;k<sizeZ;k++)
+void Gl1_PotentialParticle::generateScalarField(const shared_ptr<Shape>& cm) {
+ for(int i=0; i<sizeX; i++) {
+ for(int j=0; j<sizeY; j++) {
+ for(int k=0; k<sizeZ; k++)
{
scalarField[i][j][k] = 0.0;
}
}
}
- for(int i=0;i<sizeX;i++){
- for(int j=0;j<sizeY;j++){
- for(int k=0;k<sizeZ;k++){
- scalarField[i][j][k] = evaluateF(cm, min[0]+ double(i)*isoStep[0], min[1]+ double(j)*isoStep[1], min[2]+double(k)*isoStep[2]);//
+ for(int i=0; i<sizeX; i++) {
+ for(int j=0; j<sizeY; j++) {
+ for(int k=0; k<sizeZ; k++) {
+ scalarField[i][j][k] = evaluateF(cm, min[0]+ double(i)*isoStep[0], min[1]+ double(j)*isoStep[1], min[2]+double(k)*isoStep[2]);//
}
}
}
@@ -107,69 +104,70 @@
bool Gl1_PotentialParticle::store;
bool Gl1_PotentialParticle::initialized;
-void Gl1_PotentialParticle::clearMemory(){
-SF.clear();
+void Gl1_PotentialParticle::clearMemory() {
+ SF.clear();
}
-void Gl1_PotentialParticle::go( const shared_ptr<Shape>& cm, const shared_ptr<State>& state ,bool wire2, const GLViewInfo&){
-
-
- PotentialParticle* pp = static_cast<PotentialParticle*>(cm.get());
+void Gl1_PotentialParticle::go( const shared_ptr<Shape>& cm, const shared_ptr<State>& state ,bool wire2, const GLViewInfo&) {
+
+ PotentialParticle* pp = static_cast<PotentialParticle*>(cm.get());
int shapeId = pp->id;
-
-if(store == false){
- if(SF.size()>0){SF.clear(); initialized = false; }
- return;
-}
-
-if(initialized == false ){
- FOREACH(const shared_ptr<Body>& b, *scene->bodies){
- if (!b) continue;
- const shared_ptr<Shape>& cmbody=b->shape;
- calcMinMax(cmbody);
- mc.init(sizeX,sizeY,sizeZ,min,max);
- mc.resizeScalarField(scalarField,sizeX,sizeY,sizeZ);
- SF.push_back(scalarF());
- generateScalarField(cmbody);
- mc.computeTriangulation(scalarField,0.0);
- SF[b->id].triangles = mc.getTriangles();
- SF[b->id].normals = mc.getNormals();
- SF[b->id].nbTriangles = mc.getNbTriangles();
- for(int i=0; i<scalarField.size(); i++){
- for(int j=0; j<scalarField[i].size(); j++) scalarField[i][j].clear();
- scalarField[i].clear();
- }
- scalarField.clear();
+
+ if(store == false) {
+ if(SF.size()>0) {
+ SF.clear();
+ initialized = false;
+ }
+ }
+
+
+ if(initialized == false ) {
+ FOREACH(const shared_ptr<Body>& b, *scene->bodies) {
+ if (!b) continue;
+ const shared_ptr<Shape>& cmbody=b->shape;
+ calcMinMax(cmbody);
+ mc.init(sizeX,sizeY,sizeZ,min,max);
+ mc.resizeScalarField(scalarField,sizeX,sizeY,sizeZ);
+ SF.push_back(scalarF());
+ generateScalarField(cmbody);
+ mc.computeTriangulation(scalarField,0.0);
+ SF[b->id].triangles = mc.getTriangles();
+ SF[b->id].normals = mc.getNormals();
+ SF[b->id].nbTriangles = mc.getNbTriangles();
+ for(unsigned int i=0; i<scalarField.size(); i++) {
+ for(unsigned int j=0; j<scalarField[i].size(); j++) scalarField[i][j].clear();
+ scalarField[i].clear();
}
- initialized = true;
-}
-
-
-//if(std::isnan(shapeId)==true){return;}
-
- calcMinMax(cm);
+ scalarField.clear();
+ }
+ initialized = true;
+ }
+
+
+ //if(std::isnan(shapeId)==true){return;}
+
+ calcMinMax(cm);
mc.init(sizeX,sizeY,sizeZ,min,max);
- mc.resizeScalarField(scalarField,sizeX,sizeY,sizeZ);
-
-
+ mc.resizeScalarField(scalarField,sizeX,sizeY,sizeZ);
+
+
// FIXME : check that : one of those 2 lines are useless
- glMaterialv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, Vector3r(cm->color[0],cm->color[1],cm->color[2]));
+ glMaterialv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, Vector3r(cm->color[0],cm->color[1],cm->color[2]));
glColor3v(cm->color);
-
-
- vector<Vector3r>& triangles = SF[shapeId].triangles; //mc.getTriangles();
+
+
+ vector<Vector3r>& triangles = SF[shapeId].triangles; //mc.getTriangles();
int nbTriangles = SF[shapeId].nbTriangles; // //mc.getNbTriangles();
- vector<Vector3r>& normals = SF[shapeId].normals; //mc.getNormals();
+ vector<Vector3r>& normals = SF[shapeId].normals; //mc.getNormals();
glDisable(GL_CULL_FACE);
glEnable(GL_LIGHTING); // 2D
glEnable(GL_NORMALIZE);
glBegin(GL_TRIANGLES);
- for(int i=0;i<3*nbTriangles;++i)
- {
+ for(int i=0; i<3*nbTriangles; ++i) {
glNormal3v(normals[i]);
glVertex3v(triangles[i]);
glNormal3v(normals[++i]);
@@ -178,88 +176,92 @@
glVertex3v(triangles[i]);
}
glEnd();
-
return;
-
}
-double Gl1_PotentialParticle::evaluateF(const shared_ptr<Shape>& cm, double x, double y, double z){
-
- PotentialParticle* pp = static_cast<PotentialParticle*>(cm.get());
- Real k = pp->k;
- Real r = pp->r;
- Real R = pp->R;
-
-
- int planeNo = pp->a.size();
-
- vector<double>a; vector<double>b; vector<double>c; vector<double>d; vector<double>p; Real pSum3 = 0.0;
- for (int i=0; i<planeNo; i++){
- a.push_back(pp->a[i]);
- b.push_back(pp->b[i]);
- c.push_back(pp->c[i]);
- d.push_back(pp->d[i]);
- Real plane = a[i]*x + b[i]*y + c[i]*z - d[i]; if (plane<pow(10,-15)){plane = 0.0;}
- p.push_back(plane);
- pSum3 += pow(p[i],2);
+double Gl1_PotentialParticle::evaluateF(const shared_ptr<Shape>& cm, double x, double y, double z) {
+
+ PotentialParticle* pp = static_cast<PotentialParticle*>(cm.get());
+ Real k = pp->k;
+ Real r = pp->r;
+ Real R = pp->R;
+
+
+ int planeNo = pp->a.size();
+
+ vector<double>a;
+ vector<double>b;
+ vector<double>c;
+ vector<double>d;
+ vector<double>p;
+ Real pSum3 = 0.0;
+ for (int i=0; i<planeNo; i++) {
+ a.push_back(pp->a[i]);
+ b.push_back(pp->b[i]);
+ c.push_back(pp->c[i]);
+ d.push_back(pp->d[i]);
+ Real plane = a[i]*x + b[i]*y + c[i]*z - d[i];
+ if (plane<pow(10,-15)) {
+ plane = 0.0;
}
-
-
- Real sphere = ( pow(x,2) + pow(y,2) + pow(z,2) ) ;
- Real f = (1.0-k)*(pSum3/pow(r,2) - 1.0)+k*(sphere/pow(R,2)-1.0);
-
- return f;
-
+ p.push_back(plane);
+ pSum3 += pow(p[i],2);
+ }
+
+ Real sphere = ( pow(x,2) + pow(y,2) + pow(z,2) ) ;
+ Real f = (1.0-k)*(pSum3/pow(r,2) - 1.0)+k*(sphere/pow(R,2)-1.0);
+
+ return f;
}
-#endif
-
-ImpFunc * ImpFunc::New()
-{
- // Skip factory stuff - create class
- return new ImpFunc;
+#endif // YADE_OPENGL
+
+
+ImpFunc * ImpFunc::New() {
+ // Skip factory stuff - create class
+ return new ImpFunc;
}
// Create the function
-ImpFunc::ImpFunc()
-{
+ImpFunc::ImpFunc() {
clump = false;
- // Initialize members here if you need
+ // Initialize members here if you need
}
-ImpFunc::~ImpFunc()
-{
- // Initialize members here if you need
+ImpFunc::~ImpFunc() {
+ // Initialize members here if you need
}
// Evaluate function
-double ImpFunc::FunctionValue(double x[3])
-{
- int planeNo = a.size();
- vector<double>p; double pSum2 = 0.0;
- if(clump==false){
+double ImpFunc::FunctionValue(double x[3]) {
+ int planeNo = a.size();
+ vector<double>p;
+ double pSum2 = 0.0;
+ if (!clump) {
Eigen::Vector3d xori(x[0],x[1],x[2]);
Eigen::Vector3d xlocal = rotationMatrix*xori;
xlocal[0] = rotationMatrix(0,0)*x[0] + rotationMatrix(0,1)*x[1] + rotationMatrix(0,2)*x[2];
xlocal[1] = rotationMatrix(1,0)*x[0] + rotationMatrix(1,1)*x[1] + rotationMatrix(1,2)*x[2];
xlocal[2] = rotationMatrix(2,0)*x[0] + rotationMatrix(2,1)*x[1] + rotationMatrix(2,2)*x[2];
//std::cout<<"rotationMatrix: "<<endl<<rotationMatrix<<endl;
- //x[0]=xlocal[0]; x[1]=xlocal[1]; x[2]=xlocal[2];
+ //x[0]=xlocal[0]; x[1]=xlocal[1]; x[2]=xlocal[2];
- for (int i=0; i<planeNo; i++){
- double plane = a[i]*xlocal[0] + b[i]*xlocal[1] + c[i]*xlocal[2] - d[i]; if (plane<pow(10,-15)){plane = 0.0;}
+ for (int i=0; i<planeNo; i++) {
+ double plane = a[i]*xlocal[0] + b[i]*xlocal[1] + c[i]*xlocal[2] - d[i];
+ if (plane<pow(10,-15)) {
+ plane = 0.0;
+ }
p.push_back(plane);
pSum2 += pow(p[i],2);
- }
- double sphere = ( pow(xlocal[0],2) + pow(xlocal[1],2) + pow(xlocal[2],2) ) ;
- // Real f = (1.0-k)*(pSum2/pow(r,2) - 1.0)+k*(sphere/pow(R,2)-1.0);
- Real f = (pSum2 - pow(r,2) );
- return f;
- }else if(clump==true){
-
+ }
+ double sphere = ( pow(xlocal[0],2) + pow(xlocal[1],2) + pow(xlocal[2],2) ) ;
+ Real f = (1.0-k)*(pSum2/pow(r,2) - 1.0)+k*(sphere/pow(R,2)-1.0);
+ return f;
+ } else {
+
Eigen::Vector3d xori(x[0],x[1],x[2]);
Eigen::Vector3d clumpMemberCentre(clumpMemberCentreX,clumpMemberCentreY,clumpMemberCentreZ);
Eigen::Vector3d xlocal = xori-clumpMemberCentre;
@@ -267,30 +269,32 @@
//xlocal[1] = rotationMatrix[1]*x[0] + rotationMatrix[4]*x[1] + rotationMatrix[7]*x[2];
//xlocal[2] = rotationMatrix[2]*x[0] + rotationMatrix[5]*x[1] + rotationMatrix[8]*x[2];
//std::cout<<"rotationMatrix: "<<endl<<rotationMatrix<<endl;
- //x[0]=xlocal[0]; x[1]=xlocal[1]; x[2]=xlocal[2];
-
- for (int i=0; i<planeNo; i++){
- double plane = a[i]*xlocal[0] + b[i]*xlocal[1] + c[i]*xlocal[2] - d[i]; if (plane<pow(10,-15)){plane = 0.0;}
+ //x[0]=xlocal[0]; x[1]=xlocal[1]; x[2]=xlocal[2];
+
+ for (int i=0; i<planeNo; i++) {
+ double plane = a[i]*xlocal[0] + b[i]*xlocal[1] + c[i]*xlocal[2] - d[i];
+ if (plane<pow(10,-15)) {
+ plane = 0.0;
+ }
p.push_back(plane);
pSum2 += pow(p[i],2);
- }
- //double sphere = ( pow(xlocal[0],2) + pow(xlocal[1],2) + pow(xlocal[2],2) ) ;
- // Real f = (1.0-k)*(pSum2/pow(r,2) - 1.0)+k*(sphere/pow(R,2)-1.0);
- Real f = (pSum2 - 1.2*pow(r,2) );
+ }
+ double sphere = ( pow(xlocal[0],2) + pow(xlocal[1],2) + pow(xlocal[2],2) ) ;
+ Real f = (1.0-k)*(pSum2/pow(r,2) - 1.0)+k*(sphere/pow(R,2)-1.0);
return f;
- // return 0;
+ // return 0;
}
- // the value of the function
+ // the value of the function
}
-void PotentialParticleVTKRecorder::action(){
+void PotentialParticleVTKRecorder::action() {
if(fileName.size()==0) return;
vtkSmartPointer<vtkPoints> pbPos = vtkSmartPointer<vtkPoints>::New();
- vtkSmartPointer<vtkAppendPolyData> appendFilter = vtkSmartPointer<vtkAppendPolyData>::New();
+ vtkSmartPointer<vtkAppendPolyData> appendFilter = vtkSmartPointer<vtkAppendPolyData>::New();
vtkSmartPointer<vtkAppendPolyData> appendFilterID = vtkSmartPointer<vtkAppendPolyData>::New();
//vtkSmartPointer<vtkTransformPolyDataFilter> transformFilter = vtkSmartPointer<vtkTransformPolyDataFilter>::New();
//vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New();
@@ -323,29 +327,27 @@
vtkSmartPointer<vtkFloatArray> pbLinVelVec = vtkSmartPointer<vtkFloatArray>::New();
pbLinVelVec->SetNumberOfComponents(3);
pbLinVelVec->SetName("linVelVec"); //Linear velocity in Vector3 form
-
+
vtkSmartPointer<vtkFloatArray> pbLinVelLen = vtkSmartPointer<vtkFloatArray>::New();
pbLinVelLen->SetNumberOfComponents(1);
pbLinVelLen->SetName("linVelLen"); //Length (magnitude) of linear velocity
-
+
vtkSmartPointer<vtkFloatArray> pbAngVelVec = vtkSmartPointer<vtkFloatArray>::New();
pbAngVelVec->SetNumberOfComponents(3);
pbAngVelVec->SetName("angVelVec"); //Angular velocity in Vector3 form
-
+
vtkSmartPointer<vtkFloatArray> pbAngVelLen = vtkSmartPointer<vtkFloatArray>::New();
pbAngVelLen->SetNumberOfComponents(1);
pbAngVelLen->SetName("angVelLen"); //Length (magnitude) of angular velocity
// velocity ####################################################
// bodyId ##############################################################
- //#if 0
vtkSmartPointer<vtkPoints> pbPosID = vtkSmartPointer<vtkPoints>::New();
vtkSmartPointer<vtkCellArray> pbIdCells = vtkSmartPointer<vtkCellArray>::New();
vtkSmartPointer<vtkIntArray> blockId = vtkSmartPointer<vtkIntArray>::New();
blockId->SetNumberOfComponents(1);
blockId->SetName("id");
// bodyId ##############################################################
- //#endif
int countID = 0;
vtkSmartPointer<vtkVectorText> textArray2[scene->bodies->size()];
vtkSmartPointer<vtkPolyDataMapper> txtMapper[scene->bodies->size()];
@@ -353,27 +355,33 @@
vtkSmartPointer<vtkActor> textActor[scene->bodies->size()];
- FOREACH(const shared_ptr<Body>& b, *scene->bodies){
+ FOREACH(const shared_ptr<Body>& b, *scene->bodies) {
if (!b) continue;
if (b->isClump() == true) continue;
- const PotentialParticle* pb=dynamic_cast<PotentialParticle*>(b->shape.get());
+ const PotentialParticle* pb=dynamic_cast<PotentialParticle*>(b->shape.get());
if(!pb) continue;
-
- if(REC_ID==true){
- //#if 0
- blockId->InsertNextValue(b->getId());
+
+ if(REC_ID==true) {
+ blockId->InsertNextValue(b->getId());
vtkIdType pid[1];
Vector3r pos(b->state->pos);
pid[0] = pbPosID->InsertNextPoint(pos[0], pos[1], pos[2]);
pbIdCells->InsertNextCell(1,pid);
- //#endif
-
- countID++;
+
+ countID++;
}
//vtkSmartPointer<ImpFunc> function = ImpFunc::New();
- function->a = pb->a; function->b = pb->b; function->c = pb->c; function->d = pb->d; function->R = pb->R; function->r = pb->r; function->k = pb->k; Eigen::Matrix3d directionCos = b->state->ori.conjugate().toRotationMatrix(); int count = 0;
- for (int i=0; i<3; i++){
- for (int j=0; j<3; j++){
+ function->a = pb->a;
+ function->b = pb->b;
+ function->c = pb->c;
+ function->d = pb->d;
+ function->R = pb->R;
+ function->r = pb->r;
+ function->k = pb->k;
+ Eigen::Matrix3d directionCos = b->state->ori.conjugate().toRotationMatrix();
+ int count = 0;
+ for (int i=0; i<3; i++) {
+ for (int j=0; j<3; j++) {
//function->rotationMatrix[count] = directionCos(j,i);
function->rotationMatrix(i,j) = directionCos(j,i);
count++;
@@ -383,64 +391,81 @@
vtkSmartPointer<vtkSampleFunction> sample = vtkSampleFunction::New();
sample->SetImplicitFunction(function);
- double value = 1.05*pb->R;
-
//double xmin = -pb->halfSize.x(); double xmax = pb->halfSize.x(); double ymin = -pb->halfSize.y(); double ymax=pb->halfSize.y(); double zmin=-pb->halfSize.z(); double zmax=pb->halfSize.z();
//double xmin = -value; double xmax = value; double ymin = -value; double ymax=value; double zmin=-value; double zmax=value;
- double xmin = -std::max(pb->minAabb.x(),pb->maxAabb.x()); double xmax = -xmin; double ymin = -std::max(pb->minAabb.y(),pb->maxAabb.y()); double ymax=-ymin; double zmin=-std::max(pb->minAabb.z(),pb->maxAabb.z()); double zmax=-zmin;
- if(twoDimension==true){
+ double xmin = -std::max(pb->minAabb.x(),pb->maxAabb.x());
+ double xmax = -xmin;
+ double ymin = -std::max(pb->minAabb.y(),pb->maxAabb.y());
+ double ymax=-ymin;
+ double zmin=-std::max(pb->minAabb.z(),pb->maxAabb.z());
+ double zmax=-zmin;
+ if(twoDimension==true) {
ymax = 0.0;
ymin = 0.0;
}
- sample->SetModelBounds(xmin, xmax, ymin, ymax, zmin, zmax);
+ sample->SetModelBounds(xmin, xmax, ymin, ymax, zmin, zmax);
//sample->SetModelBounds(pb->minAabb.x(), pb->maxAabb.x(), pb->minAabb.y(), pb->maxAabb.y(), pb->minAabb.z(), pb->maxAabb.z());
- int sampleXno = sampleX; int sampleYno = sampleY; int sampleZno = sampleZ;
- if(fabs(xmax-xmin)/static_cast<double>(sampleX) > maxDimension) { sampleXno = static_cast<int>(fabs(xmax-xmin)/maxDimension); }
- if(fabs(ymax-ymin)/static_cast<double>(sampleY) > maxDimension) { sampleYno = static_cast<int>(fabs(ymax-ymin)/maxDimension); }
- if(fabs(zmax-zmin)/static_cast<double>(sampleZ) > maxDimension) { sampleZno = static_cast<int>(fabs(zmax-zmin)/maxDimension); }
- if(twoDimension==true){sampleYno=1;}
+ int sampleXno = sampleX;
+ int sampleYno = sampleY;
+ int sampleZno = sampleZ;
+ if(fabs(xmax-xmin)/static_cast<double>(sampleX) > maxDimension) {
+ sampleXno = static_cast<int>(fabs(xmax-xmin)/maxDimension);
+ }
+ if(fabs(ymax-ymin)/static_cast<double>(sampleY) > maxDimension) {
+ sampleYno = static_cast<int>(fabs(ymax-ymin)/maxDimension);
+ }
+ if(fabs(zmax-zmin)/static_cast<double>(sampleZ) > maxDimension) {
+ sampleZno = static_cast<int>(fabs(zmax-zmin)/maxDimension);
+ }
+ if(twoDimension==true) {
+ sampleYno=1;
+ }
sample->SetSampleDimensions(sampleXno,sampleYno,sampleZno);
sample->ComputeNormalsOff();
//sample->Update();
vtkSmartPointer<vtkContourFilter> contours = vtkContourFilter::New();
contours->SetInput(sample->GetOutput());
contours->SetNumberOfContours(1);
- contours->SetValue(0,0.0);
+ contours->SetValue(0,0.0);
vtkSmartPointer<vtkPolyData> polydata = vtkSmartPointer<vtkPolyData>::New();
contours->Update();
polydata->DeepCopy(contours->GetOutput());
//polydata->Update();
-
+
vtkSmartPointer<vtkUnsignedCharArray> pbColors = vtkSmartPointer<vtkUnsignedCharArray>::New();
pbColors->SetName("pbColors");
pbColors->SetNumberOfComponents(3);
Vector3r color = pb->color; //Vector3r(0,100,0);
- if (b->isDynamic() == false){ color = Vector3r(157,157,157); }
+ if (b->isDynamic() == false) {
+ color = Vector3r(157,157,157);
+ }
unsigned char c[3]; //c = {color[0],color[1],color[2]};
c[0]=color[0];
c[1]=color[1];
c[2]=color[2];
- int nbCells=polydata->GetNumberOfPoints();
- for (int i=0;i<nbCells;i++){
- pbColors->InsertNextTupleValue(c);
- }
+ int nbCells=polydata->GetNumberOfPoints();
+ for (int i=0; i<nbCells; i++) {
+ pbColors->InsertNextTupleValue(c);
+ }
polydata->GetPointData()->SetScalars(pbColors);
//polydata->Update();
-
-
+
+
Vector3r centre (b->state->pos[0], b->state->pos[1], b->state->pos[2]);
- Quaternionr orientation= b->state->ori; orientation.normalize();
- AngleAxisr aa(orientation); Vector3r axis = aa.axis(); /* axis.normalize(); */ double angle = aa.angle()/3.14159*180.0; double xAxis = axis[0]; double yAxis = axis[1]; double zAxis = axis[2];
+ Quaternionr orientation= b->state->ori;
+ orientation.normalize();
+ AngleAxisr aa(orientation);
+ //Vector3r axis = aa.axis();
vtkSmartPointer<vtkTransformPolyDataFilter> transformFilter = vtkSmartPointer<vtkTransformPolyDataFilter>::New();
transformFilter->SetInput( polydata );
vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New();
transformFilter->SetTransform( transform );
transform->PostMultiply();
-
+
transform->Translate (centre[0], centre[1],centre[2]);
//transform->RotateWXYZ(angle,xAxis, yAxis, zAxis);
//transformFilter->Update();
@@ -448,7 +473,7 @@
// ################## velocity ####################
- if(REC_VELOCITY == true){
+ if(REC_VELOCITY == true) {
vtkIdType pid[1];
Vector3r pos(b->state->pos);
pid[0] = pbPos->InsertNextPoint(pos[0], pos[1], pos[2]);
@@ -464,7 +489,7 @@
float av[3]; //av = { angVel[0],angVel[1],angVel[2] };
av[0]=angVel[0];
av[1]=angVel[1];
- av[2]=angVel[2];
+ av[2]=angVel[2];
pbAngVelVec->InsertNextTupleValue(av);
pbAngVelLen->InsertNextValue(angVel.norm());
}
@@ -477,114 +502,99 @@
contours = NULL;
}
- if(REC_VELOCITY == true){
- vtkSmartPointer<vtkUnstructuredGrid> pbUg = vtkSmartPointer<vtkUnstructuredGrid>::New();
- pbUg->SetPoints(pbPos);
- pbUg->SetCells(VTK_VERTEX, pbCells);
- pbUg->GetPointData()->AddArray(pbLinVelVec);
- pbUg->GetPointData()->AddArray(pbAngVelVec);
- pbUg->GetPointData()->AddArray(pbLinVelLen);
- pbUg->GetPointData()->AddArray(pbAngVelLen);
- vtkSmartPointer<vtkXMLUnstructuredGridWriter> writerA = vtkSmartPointer<vtkXMLUnstructuredGridWriter>::New();
- writerA->SetDataModeToAscii();
- string fv=fileName+"vel."+std::to_string(scene->iter)+".vtu";
- writerA->SetFileName(fv.c_str());
- writerA->SetInput(pbUg);
- writerA->Write();
- //writerA->Delete();
- //pbUg->Delete();
+ if(REC_VELOCITY == true) {
+ vtkSmartPointer<vtkUnstructuredGrid> pbUg = vtkSmartPointer<vtkUnstructuredGrid>::New();
+ pbUg->SetPoints(pbPos);
+ pbUg->SetCells(VTK_VERTEX, pbCells);
+ pbUg->GetPointData()->AddArray(pbLinVelVec);
+ pbUg->GetPointData()->AddArray(pbAngVelVec);
+ pbUg->GetPointData()->AddArray(pbLinVelLen);
+ pbUg->GetPointData()->AddArray(pbAngVelLen);
+ vtkSmartPointer<vtkXMLUnstructuredGridWriter> writerA = vtkSmartPointer<vtkXMLUnstructuredGridWriter>::New();
+ writerA->SetDataModeToAscii();
+ string fv=fileName+"vel."+std::to_string(scene->iter)+".vtu";
+ writerA->SetFileName(fv.c_str());
+ writerA->SetInput(pbUg);
+ writerA->Write();
+ //writerA->Delete();
+ //pbUg->Delete();
}
//###################### bodyId ###############################
- if(REC_ID == true){
- #if 0
- vtkSmartPointer<vtkXMLPolyDataWriter> writerA = vtkXMLPolyDataWriter::New();
- writerA->SetDataModeToAscii();
- string fn=fileName+"-Id."+std::to_string(scene->iter)+".vtp";
- writerA->SetFileName(fn.c_str());
- writerA->SetInputConnection(appendFilterID->GetOutputPort());//(extrude->GetOutputPort());
- writerA->Write();
-
- writerA->Delete();
- #endif
- //#if 0
- vtkSmartPointer<vtkUnstructuredGrid> pbUg = vtkSmartPointer<vtkUnstructuredGrid>::New();
- pbUg->SetPoints(pbPosID);
- pbUg->SetCells(VTK_VERTEX, pbIdCells);
- pbUg->GetPointData()->AddArray(blockId);
- vtkSmartPointer<vtkXMLUnstructuredGridWriter> writerA = vtkSmartPointer<vtkXMLUnstructuredGridWriter>::New();
- writerA->SetDataModeToAscii();
- string fv=fileName+"Id."+std::to_string(scene->iter)+".vtu";
- writerA->SetFileName(fv.c_str());
- writerA->SetInput(pbUg);
- writerA->Write();
- //writerA->Delete();
- //pbUg->Delete();
- //#endif
+ if(REC_ID == true) {
+ vtkSmartPointer<vtkUnstructuredGrid> pbUg = vtkSmartPointer<vtkUnstructuredGrid>::New();
+ pbUg->SetPoints(pbPosID);
+ pbUg->SetCells(VTK_VERTEX, pbIdCells);
+ pbUg->GetPointData()->AddArray(blockId);
+ vtkSmartPointer<vtkXMLUnstructuredGridWriter> writerA = vtkSmartPointer<vtkXMLUnstructuredGridWriter>::New();
+ writerA->SetDataModeToAscii();
+ string fv=fileName+"Id."+std::to_string(scene->iter)+".vtu";
+ writerA->SetFileName(fv.c_str());
+ writerA->SetInput(pbUg);
+ writerA->Write();
+ //writerA->Delete();
+ //pbUg->Delete();
}
-//#if 0
// ################## contact point ####################
- if(REC_INTERACTION == true){
- int count = 0;
- FOREACH(const shared_ptr<Interaction>& I, *scene->interactions){
- if(!I->isReal()) {
- continue;
- }
- const KnKsPhys* phys = YADE_CAST<KnKsPhys*>(I->phys.get());
- const ScGeom* geom = YADE_CAST<ScGeom*>(I->geom.get());
- vtkIdType pid[1];
- Vector3r pos(geom->contactPoint);
- pid[0] = pbContactPoint->InsertNextPoint(pos[0], pos[1], pos[2]);
- pbCellsContact->InsertNextCell(1,pid);
- //intrBodyPos->InsertNextPoint(geom->contactPoint[0],geom->contactPoint[1],geom->contactPoint[2]);
- // gives _signed_ scalar of normal force, following the convention used in the respective constitutive law
- float fn[3]={phys->normalForce[0],phys->normalForce[1], phys->normalForce[2]};
- float fs[3]={phys->shearForce[0], phys->shearForce[1], phys->shearForce[2]};
- pbNormalForce->InsertNextTupleValue(fn);
- pbShearForce->InsertNextTupleValue(fs);
- count++;
- }
- if(count>0){
- vtkSmartPointer<vtkUnstructuredGrid> pbUgCP = vtkSmartPointer<vtkUnstructuredGrid>::New();
- pbUgCP->SetPoints(pbContactPoint);
- pbUgCP->SetCells(VTK_VERTEX, pbCellsContact);
- pbUgCP->GetPointData()->AddArray(pbNormalForce);
- pbUgCP->GetPointData()->AddArray(pbShearForce);
- vtkSmartPointer<vtkXMLUnstructuredGridWriter> writerB = vtkSmartPointer<vtkXMLUnstructuredGridWriter>::New();
- writerB->SetDataModeToAscii();
- string fcontact=fileName+"contactPoint."+std::to_string(scene->iter)+".vtu";
- writerB->SetFileName(fcontact.c_str());
- writerB->SetInput(pbUgCP);
- writerB->Write();
- //writerB->Delete();
- //pbUgCP->Delete();
- }
+ if(REC_INTERACTION == true) {
+ int count = 0;
+ FOREACH(const shared_ptr<Interaction>& I, *scene->interactions) {
+ if(!I->isReal()) {
+ continue;
+ }
+ const KnKsPhys* phys = YADE_CAST<KnKsPhys*>(I->phys.get());
+ const ScGeom* geom = YADE_CAST<ScGeom*>(I->geom.get());
+ vtkIdType pid[1];
+ Vector3r pos(geom->contactPoint);
+ pid[0] = pbContactPoint->InsertNextPoint(pos[0], pos[1], pos[2]);
+ pbCellsContact->InsertNextCell(1,pid);
+ //intrBodyPos->InsertNextPoint(geom->contactPoint[0],geom->contactPoint[1],geom->contactPoint[2]);
+ // gives _signed_ scalar of normal force, following the convention used in the respective constitutive law
+ float fn[3]= {(float)phys->normalForce[0], (float)phys->normalForce[1], (float)phys->normalForce[2]};
+ float fs[3]= {(float)phys->shearForce[0], (float)phys->shearForce[1], (float)phys->shearForce[2]};
+ pbNormalForce->InsertNextTupleValue(fn);
+ pbShearForce->InsertNextTupleValue(fs);
+ count++;
+ }
+ if(count>0) {
+ vtkSmartPointer<vtkUnstructuredGrid> pbUgCP = vtkSmartPointer<vtkUnstructuredGrid>::New();
+ pbUgCP->SetPoints(pbContactPoint);
+ pbUgCP->SetCells(VTK_VERTEX, pbCellsContact);
+ pbUgCP->GetPointData()->AddArray(pbNormalForce);
+ pbUgCP->GetPointData()->AddArray(pbShearForce);
+ vtkSmartPointer<vtkXMLUnstructuredGridWriter> writerB = vtkSmartPointer<vtkXMLUnstructuredGridWriter>::New();
+ writerB->SetDataModeToAscii();
+ string fcontact=fileName+"contactPoint."+std::to_string(scene->iter)+".vtu";
+ writerB->SetFileName(fcontact.c_str());
+ writerB->SetInput(pbUgCP);
+ writerB->Write();
+ //writerB->Delete();
+ //pbUgCP->Delete();
+ }
}
-//#endif
-
// ################ contact point ###########################
-
-
-
-
+
+
+
+
vtkSmartPointer<vtkXMLPolyDataWriter> writer = vtkXMLPolyDataWriter::New();
writer->SetDataModeToAscii();
string fn=fileName+"-pb."+std::to_string(scene->iter)+".vtp";
writer->SetFileName(fn.c_str());
writer->SetInputConnection(appendFilter->GetOutputPort());
writer->Write();
-
- writer->Delete();
-
- //intrBodyPos->Delete();
+
+ writer->Delete();
+
+ //intrBodyPos->Delete();
//intrForceN->Delete();
//intrAbsForceT->Delete();
- //pbContactPoint->Delete();
- //pbCellsContact->Delete();
- //pbNormalForce->Delete();
+ //pbContactPoint->Delete();
+ //pbCellsContact->Delete();
+ //pbNormalForce->Delete();
//pbShearForce->Delete();
//pbCells->Delete();
//pbLinVelVec->Delete();
@@ -594,17 +604,9 @@
}
-YADE_PLUGIN(/*(Gl1_PotentialParticle)*/(PotentialParticleVTKRecorder));
+YADE_PLUGIN((Gl1_PotentialParticle)(PotentialParticleVTKRecorder));
//YADE_REQUIRE_FEATURE(OPENGL)
-
-
-
-
-
-
-
-
-
+#endif // YADE_POTENTIAL_PARTICLES
=== modified file 'pkg/common/Gl1_PotentialParticle.hpp'
--- pkg/common/Gl1_PotentialParticle.hpp 2015-12-14 14:25:08 +0000
+++ pkg/common/Gl1_PotentialParticle.hpp 2016-01-19 00:59:38 +0000
@@ -1,34 +1,35 @@
/*CWBoon 2015 */
#pragma once
-#include<pkg/dem/PotentialParticle.hpp>
-#include<pkg/dem/PotentialParticle2AABB.hpp>
-#include<pkg/common/GLDrawFunctors.hpp>
-//#include<pkg/dem/MarchingCube.hpp>
+#ifdef YADE_POTENTIAL_PARTICLES
+#include <pkg/dem/PotentialParticle.hpp>
+#include <pkg/dem/PotentialParticle2AABB.hpp>
+#include <pkg/common/GLDrawFunctors.hpp>
+#include <lib/computational-geometry/MarchingCube.hpp>
#include <vector>
#include <pkg/common/PeriodicEngines.hpp>
-#include<vtkImplicitFunction.h>
+#include <vtkImplicitFunction.h>
#include <vtkRenderWindow.h>
#include <vtkRenderer.h>
#include <vtkPolyData.h>
-#include<vtkXMLUnstructuredGridWriter.h>
-#include<vtkTriangle.h>
-#include<vtkSmartPointer.h>
-#include<vtkFloatArray.h>
-#include<vtkCellArray.h>
-#include<vtkCellData.h>
+#include <vtkXMLUnstructuredGridWriter.h>
+#include <vtkTriangle.h>
+#include <vtkSmartPointer.h>
+#include <vtkFloatArray.h>
+#include <vtkCellArray.h>
+#include <vtkCellData.h>
#include <vtkSampleFunction.h>
#include <vtkStructuredPoints.h>
-#include<vtkStructuredPointsWriter.h>
-#include<vtkWriter.h>
-#include<vtkExtractVOI.h>
-#include<vtkXMLImageDataWriter.h>
-#include<vtkXMLStructuredGridWriter.h>
-#include<vtkTransformPolyDataFilter.h>
-#include<vtkTransform.h>
-#include <vtkRenderWindowInteractor.h>
-#include<vtkXMLUnstructuredGridWriter.h>
+#include <vtkStructuredPointsWriter.h>
+#include <vtkWriter.h>
+#include <vtkExtractVOI.h>
+#include <vtkXMLImageDataWriter.h>
+#include <vtkXMLStructuredGridWriter.h>
+#include <vtkTransformPolyDataFilter.h>
+#include <vtkTransform.h>
+#include <vtkRenderWindowInteractor.h>
+#include <vtkXMLUnstructuredGridWriter.h>
#include <vtkActor.h>
#include <vtkAppendPolyData.h>
@@ -42,52 +43,57 @@
class ImpFunc : public vtkImplicitFunction {
-public:
- vtkTypeMacro(ImpFunc,vtkImplicitFunction);
- //void PrintSelf(ostream& os, vtkIndent indent);
-
- // Description
- // Create a new function
- static ImpFunc * New(void);
- vector<double>a; vector<double>b; vector<double>c; vector<double>d;
- double k; double r; double R; Eigen::Matrix3d rotationMatrix;
- bool clump;
- double clumpMemberCentreX;
- double clumpMemberCentreY;
- double clumpMemberCentreZ;
- // Description
- // Evaluate function
- double FunctionValue(double x[3]);
- double EvaluateFunction(double x[3]){
- //return this->vtkImplicitFunction::EvaluateFunction(x);
- return FunctionValue(x);
- };
-
- double EvaluateFunction(double x, double y, double z) {
- return this->vtkImplicitFunction::EvaluateFunction(x, y, z);
- };
-
-
-
-
- // Description
- // Evaluate gradient for function
- void EvaluateGradient(double x[3], double n[3]){ };
-
- // If you need to set parameters, add methods here
-
-protected:
- ImpFunc();
- ~ImpFunc();
- ImpFunc(const ImpFunc&) {}
- void operator=(const ImpFunc&) {}
-
- // Add parameters/members here if you need
+ public:
+ vtkTypeMacro(ImpFunc,vtkImplicitFunction);
+ //void PrintSelf(ostream& os, vtkIndent indent);
+
+ // Description
+ // Create a new function
+ static ImpFunc * New(void);
+ vector<double>a;
+ vector<double>b;
+ vector<double>c;
+ vector<double>d;
+ double k;
+ double r;
+ double R;
+ Eigen::Matrix3d rotationMatrix;
+ bool clump;
+ double clumpMemberCentreX;
+ double clumpMemberCentreY;
+ double clumpMemberCentreZ;
+ // Description
+ // Evaluate function
+ double FunctionValue(double x[3]);
+ double EvaluateFunction(double x[3]) {
+ //return this->vtkImplicitFunction::EvaluateFunction(x);
+ return FunctionValue(x);
+ };
+
+ double EvaluateFunction(double x, double y, double z) {
+ return this->vtkImplicitFunction::EvaluateFunction(x, y, z);
+ };
+
+
+
+
+ // Description
+ // Evaluate gradient for function
+ void EvaluateGradient(double x[3], double n[3]) { };
+
+ // If you need to set parameters, add methods here
+
+ protected:
+ ImpFunc();
+ ~ImpFunc();
+ ImpFunc(const ImpFunc&) {}
+ void operator=(const ImpFunc&) {}
+
+ // Add parameters/members here if you need
};
-#if 0
-class Gl1_PotentialParticle : public GlShapeFunctor
-{
+#ifdef YADE_OPENGL
+class Gl1_PotentialParticle : public GlShapeFunctor {
private :
MarchingCube mc;
Vector3r min,max;
@@ -96,77 +102,62 @@
void calcMinMax(const shared_ptr<Shape>& cm);
float oldIsoValue,oldIsoSec,oldIsoThick;
Vector3r isoStep;
-
-
public :
- struct Leaf{
+ struct Leaf {
Vector3r centre;
- Leaf(Vector3r pos){centre = pos;}
- Leaf(){centre = Vector3r(0,0,0);}
+ Leaf(Vector3r pos) {
+ centre = pos;
+ }
+ Leaf() {
+ centre = Vector3r(0,0,0);
+ }
};
- struct scalarF{
- vector<vector<vector<float > > > scalarField2;
- vector<Vector3r> triangles;
- vector<Vector3r> normals;
- int nbTriangles;
+ struct scalarF {
+ vector<vector<vector<float > > > scalarField2;
+ vector<Vector3r> triangles;
+ vector<Vector3r> normals;
+ int nbTriangles;
};
virtual void go(const shared_ptr<Shape>&, const shared_ptr<State>&,bool,const GLViewInfo&);
double evaluateF(const shared_ptr<Shape>& cm, double x, double y, double z);
static vector<scalarF> SF ;
void clearMemory();
- RENDERS(PotentialParticle);
- //YADE_CLASS_BASE_DOC_STATICATTRS(Gl1_PotentialParticle,GlShapeFunctor,"Renders :yref:`Sphere` object",
- //(( vector<scalarF>, SF ," "))
- //);
- YADE_CLASS_BASE_DOC_STATICATTRS(Gl1_PotentialParticle,GlShapeFunctor,"Renders :yref:`Sphere` object",
- ((int,sizeX,80,,"Number of divisions in the x direction for triangulation"))
- ((int,sizeY,80,,"Number of divisions in the x direction for triangulation"))
- ((int,sizeZ,80,,"Number of divisions in the y direction for triangulation"))
- ((bool,store,false,,"Number of divisions in the z direction for triangulation"))
- ((bool,initialized,false,,"Number of divisions in the z direction for triangulation"))
-
- );
-
-
-
+ YADE_CLASS_BASE_DOC_STATICATTRS(Gl1_PotentialParticle,GlShapeFunctor,"Renders :yref:`PotentialParticle` object",
+ ((int,sizeX,10,,"Number of divisions in the x direction for triangulation"))
+ ((int,sizeY,10,,"Number of divisions in the y direction for triangulation"))
+ ((int,sizeZ,10,,"Number of divisions in the z direction for triangulation"))
+ ((bool,store,false,,"store computed triangulation or not"))
+ ((bool,initialized,false,,"if triangulation is initialized"))
+ );
+ RENDERS(PotentialParticle);
};
REGISTER_SERIALIZABLE(Gl1_PotentialParticle);
-
-
-#endif
-
-
-class PotentialParticleVTKRecorder: public PeriodicEngine{
- public:
- vtkSmartPointer<ImpFunc> function;
-
- virtual void action(void);
- YADE_CLASS_BASE_DOC_ATTRS_CTOR_PY(PotentialParticleVTKRecorder,PeriodicEngine,"Engine recording potential blocks as surfaces into files with given periodicity.",
- ((string,fileName,,,"File prefix to save to"))
- ((int,sampleX,30,,"size of contact point"))
- ((int,sampleY,30,,"size of contact point"))
- ((int,sampleZ,30,,"size of contact point"))
- ((double,maxDimension,30,,"size of contact point"))
- ((bool,twoDimension,false,,"size of contact point"))
- ((bool,REC_INTERACTION,false,,"contact point and forces"))
- ((bool,REC_COLORS,false,,"colors"))
- ((bool,REC_VELOCITY,false,,"velocity"))
- ((bool,REC_ID,true,,"id"))
- ,
- function = ImpFunc::New();
- ,
-
- );
+#endif // YADE_OPENGL
+
+
+class PotentialParticleVTKRecorder: public PeriodicEngine {
+ public:
+ vtkSmartPointer<ImpFunc> function;
+
+ virtual void action(void);
+ YADE_CLASS_BASE_DOC_ATTRS_CTOR_PY(PotentialParticleVTKRecorder,PeriodicEngine,"Engine recording potential blocks as surfaces into files with given periodicity.",
+ ((string,fileName,,,"File prefix to save to"))
+ ((int,sampleX,30,,"Number of divisions in the x direction for triangulation"))
+ ((int,sampleY,30,,"Number of divisions in the y direction for triangulation"))
+ ((int,sampleZ,30,,"Number of divisions in the z direction for triangulation"))
+ ((double,maxDimension,30,,"max dimension"))
+ ((bool,twoDimension,false,,"2D or not"))
+ ((bool,REC_INTERACTION,false,,"contact point and forces"))
+ ((bool,REC_COLORS,false,,"colors"))
+ ((bool,REC_VELOCITY,false,,"velocity"))
+ ((bool,REC_ID,true,,"id"))
+ ,
+ function = ImpFunc::New();
+ ,
+
+ );
};
REGISTER_SERIALIZABLE(PotentialParticleVTKRecorder);
-
-
-
-
-
-
-
-
-
+#endif // YADE_POTENTIAL_PARTICLES
=== modified file 'pkg/dem/Ig2_PP_PP_ScGeom.cpp'
--- pkg/dem/Ig2_PP_PP_ScGeom.cpp 2015-12-14 14:25:08 +0000
+++ pkg/dem/Ig2_PP_PP_ScGeom.cpp 2016-01-19 00:59:38 +0000
@@ -2,23 +2,24 @@
/* C.W. Boon, G.T. Houlsby, S. Utili (2013). A new contact detection algorithm for three-dimensional non-spherical particles. Powder Technology, 248, pp 94-102. */
/* code for calling MOSEK was for ver 6. Please uncomment if you have the licence */
-#include"Ig2_PP_PP_ScGeom.hpp"
-#include<pkg/dem/ScGeom.hpp>
-#include<pkg/dem/PotentialParticle.hpp>
-//#include<yade/lib-base/yadeWm3Extra.hpp>
-#include<pkg/dem/KnKsLaw.hpp>
+#ifdef YADE_POTENTIAL_PARTICLES
+#include "Ig2_PP_PP_ScGeom.hpp"
+#include <pkg/dem/ScGeom.hpp>
+#include <pkg/dem/PotentialParticle.hpp>
+//#include <yade/lib-base/yadeWm3Extra.hpp>
+#include <pkg/dem/KnKsLaw.hpp>
-#include<core/Scene.hpp>
-#include<lib/base/Math.hpp>
-#include<pkg/common/InteractionLoop.hpp>
-#include<core/Omega.hpp>
+#include <core/Scene.hpp>
+#include <lib/base/Math.hpp>
+#include <pkg/common/InteractionLoop.hpp>
+#include <core/Omega.hpp>
#include <iostream>
#include <fstream>
#include <iomanip>
-#include<Eigen/Core>
-#include <Eigen/LU>
+#include <Eigen/Core>
+#include <Eigen/LU>
#include <Eigen/QR>
#include <ctime>
@@ -28,63 +29,71 @@
-YADE_PLUGIN((Ig2_PP_PP_ScGeom)
+YADE_PLUGIN((Ig2_PP_PP_ScGeom)
//#ifdef YADE_OPENGL
// (Gl1_Ig2_PP_PP_ScGeom)
-// #endif
-);
+// #endif
+ );
CREATE_LOGGER(Ig2_PP_PP_ScGeom);
-bool Ig2_PP_PP_ScGeom::go(const shared_ptr<Shape>& cm1,const shared_ptr<Shape>& cm2,const State& state1, const State& state2, const Vector3r& shift2, const bool& force,const shared_ptr<Interaction>& c)
-{
-
+bool Ig2_PP_PP_ScGeom::go(const shared_ptr<Shape>& cm1,const shared_ptr<Shape>& cm2,const State& state1, const State& state2, const Vector3r& shift2, const bool& force,const shared_ptr<Interaction>& c) {
PotentialParticle *s1=static_cast<PotentialParticle*>(cm1.get());
PotentialParticle *s2=static_cast<PotentialParticle*>(cm2.get());
-
+
/* Short circuit if both particles are boundary particles */
- if((s1->isBoundary==true)&&(s2->isBoundary==true)) {return false;}
-
-
- bool hasGeom = false;
- Vector3r contactPt(0,0,0);
+ if((s1->isBoundary==true)&&(s2->isBoundary==true)) {
+ return false;
+ }
+
+
+ bool hasGeom = false;
+ Vector3r contactPt(0,0,0);
shared_ptr<ScGeom> scm;
shared_ptr<KnKsPhys> phys;
- double stepBisection = 0.001*std::min(s1->R,s2->R); if(stepBisection<pow(10,-6)){std::cout<<"R1: "<<s1->R<<", R2: "<<s2->R<<", stepBisection: "<<stepBisection<<", id1: "<<c->getId1()<<", id2: "<<c->getId2()<<endl;}
-
- bool contact = false;
-
- Real fA = 0.0;
+ double stepBisection = 0.001*std::min(s1->R,s2->R);
+ if(stepBisection<pow(10,-6)) {
+ std::cout<<"R1: "<<s1->R<<", R2: "<<s2->R<<", stepBisection: "<<stepBisection<<", id1: "<<c->getId1()<<", id2: "<<c->getId2()<<endl;
+ }
+
+ bool contact = false;
+
+ Real fA = 0.0;
Real fB = 0.0;
- Vector3r normalP1(0.0,0.0,0.0);
- Vector3r normalP2(0.0,0.0,0.0);
- Vector3r avgNormal(0.0,0.0,0.0);
- Vector3r ptOnP1(0.0,0.0,0.0);
+ Vector3r normalP1(0.0,0.0,0.0);
+ Vector3r normalP2(0.0,0.0,0.0);
+ Vector3r avgNormal(0.0,0.0,0.0);
+ Vector3r ptOnP1(0.0,0.0,0.0);
Vector3r ptOnP2(0.0,0.0,0.0);
bool converge = false;
- if(c->geom){
+ if(c->geom) {
hasGeom = true;
scm=YADE_PTR_CAST<ScGeom>(c->geom);
- if (scm->penetrationDepth>stepBisection ){ stepBisection = 0.5*scm->penetrationDepth;}
- if(stepBisection<pow(10,-6)){std::cout<<"stepBisection: "<<stepBisection<<", penetrationDepth: "<<scm->penetrationDepth<<endl;}
+ if (scm->penetrationDepth>stepBisection ) {
+ stepBisection = 0.5*scm->penetrationDepth;
+ }
+ if(stepBisection<pow(10,-6)) {
+ std::cout<<"stepBisection: "<<stepBisection<<", penetrationDepth: "<<scm->penetrationDepth<<endl;
+ }
contactPt = scm->contactPoint;
- }else{
- scm=shared_ptr<ScGeom>(new ScGeom());
+ } else {
+ scm=shared_ptr<ScGeom>(new ScGeom());
c->geom=scm;
contactPt = 0.5*(state1.pos+state2.pos);
-
+
}
bool hasPhys=false;
- if(c->phys){
- phys=YADE_PTR_CAST<KnKsPhys>(c->phys); hasPhys=true;
+ if(c->phys) {
+ phys=YADE_PTR_CAST<KnKsPhys>(c->phys);
+ hasPhys=true;
}
@@ -96,91 +105,101 @@
//if(fA < 0.0 && fB <0.0){
// converge = customSolve(cm1,state1,cm2,state2,contactPt,true);
//}else{
- converge = customSolve(cm1,state1,cm2,state2,contactPt,false);
+ converge = customSolve(cm1,state1,cm2,state2,contactPt,false);
//}
-
-
-// if you have mosek uncomment this. Mosek is more robust but slightly slower as an external library
-#if 0
+
+
+// if you have mosek uncomment this. Mosek is more robust but slightly slower as an external library
+#ifdef YADE_MOSEK
/* Mosek */
- if( converge==false ){
+ if( converge==false ) {
//std::cout<<"mosek used"<<endl;
contactPt = 0.5*(state1.pos+state2.pos);
contactPtMosekF2(cm1, state1, cm2, state2, contactPt);
}
#endif
-
+
fA= evaluatePP(cm1,state1, contactPt);
fB = evaluatePP(cm2,state2, contactPt);
-if (fA*fB<0.0){
-std::cout<<"fA: "<<fA<<", fB: "<<fB<<endl;
-}
+ if (fA*fB<0.0) {
+ std::cout<<"fA: "<<fA<<", fB: "<<fB<<endl;
+ }
//std::cout<<"converge: "<<converge<<", fA: "<<fA<<", fB: "<<fB<<endl;
- //timingDeltas->start();
-
- if (fA < 0.0 && fB < 0.0 && converge == true){
+ //timingDeltas->start();
+
+ if (fA < 0.0 && fB < 0.0 && converge == true) {
contact = true;
- }else{
- contact = false; contactPt = 0.5*(state1.pos+state2.pos);
+ } else {
+ contact = false;
+ contactPt = 0.5*(state1.pos+state2.pos);
}
//////////////////////////////////////////////////////////* PASS VARIABLES TO ScGeom and Phys Functor */////////////////////////////////////////////////////////////
//* 1. Get overlap *//
- //* 2. Get information on active planes. If active planes are not the same as the previous, get new physical parameters. Otherwise keep the same parameters *//
+ //* 2. Get information on active planes. If active planes are not the same as the previous, get new physical parameters. Otherwise keep the same parameters *//
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- if (contact == true || c->isReal() || force){
- if (contact == true){
- normalP1 = getNormal(cm1,state1,contactPt); normalP1.normalize();
- normalP2 = getNormal(cm2,state2,contactPt); normalP2.normalize();
+ if (contact == true || c->isReal() || force) {
+ if (contact == true) {
+ normalP1 = getNormal(cm1,state1,contactPt);
+ normalP1.normalize();
+ normalP2 = getNormal(cm2,state2,contactPt);
+ normalP2.normalize();
//if(s1->isBoundary==true){avgNormal=normalP1;}else if(s2->isBoundary==true){avgNormal=-normalP2;}else{avgNormal = (normalP1 - normalP2);}
-#if 0
- if(s1->isBoundary==true){avgNormal=normalP1;}else if(s2->isBoundary==true){avgNormal=-normalP2;}else{
-
- avgNormal = (normalP1 - normalP2);
-
-
- }
-#endif
avgNormal = (normalP1 - normalP2);
avgNormal.normalize();
-
- if(hasPhys == true){
- bool calJointLength = phys->calJointLength; int smallerID = 1; bool twoD = phys->twoDimension;
- if (twoD == true){phys->jointLength = 1.0;}
- }
- if(s1->fixedNormal==true){ avgNormal = s1->boundaryNormal;}
- if(s2->fixedNormal==true){ avgNormal = -s2->boundaryNormal;}
+
+ if(hasPhys == true) {
+ //bool calJointLength = phys->calJointLength;
+ //int smallerID = 1;
+ bool twoD = phys->twoDimension;
+ if (twoD == true) {
+ phys->jointLength = 1.0;
+ }
+ }
+ if(s1->fixedNormal==true) {
+ avgNormal = s1->boundaryNormal;
+ }
+ if(s2->fixedNormal==true) {
+ avgNormal = -s2->boundaryNormal;
+ }
Vector3r step = avgNormal*stepBisection;
- int locationStuck = 2;
- getPtOnParticle2(cm1,state1,contactPt, step, ptOnP1);
- getPtOnParticle2(cm2,state2,contactPt, -1.0*step, ptOnP2);
+ //int locationStuck = 2;
+ getPtOnParticle2(cm1,state1,contactPt, step, ptOnP1);
+ getPtOnParticle2(cm2,state2,contactPt, -1.0*step, ptOnP2);
vector<Vector3r> points;
Real penetrationDepth = (ptOnP2-ptOnP1).norm();// overlap;
- //std::cout<<"contactpoint: "<<contactPt<<", penetrationDepth: "<<penetrationDepth<<", avgNormal: "<<avgNormal<<endl;
+ //std::cout<<"contactpoint: "<<contactPt<<", penetrationDepth: "<<penetrationDepth<<", avgNormal: "<<avgNormal<<endl;
scm->precompute(state1,state2,scene,c,avgNormal,!(hasGeom),Vector3r(0,0,0)/*shift 2 */, false /* avoidGranularRatcheting */); //Assign contact point and normal after precompute!!!!
- scm->contactPoint = contactPt; scm->penetrationDepth=penetrationDepth; if(isnan(avgNormal.norm())){std::cout<<"avgNormal: "<<avgNormal<<endl;}
+ scm->contactPoint = contactPt;
+ scm->penetrationDepth=penetrationDepth;
+ if(isnan(avgNormal.norm())) {
+ std::cout<<"avgNormal: "<<avgNormal<<endl;
+ }
scm->normal = avgNormal;
-
-
-
- }else{
-
+
+
+
+ } else {
+
//scm->normal = Vector3r(0,0,0);
scm->precompute(state1,state2,scene,c,avgNormal,!(hasGeom),Vector3r(0,0,0)/*shift 2 */, false /* avoidGranularRatcheting */); //Assign contact point and normal after precompute!!!!
- scm->contactPoint = contactPt;scm->penetrationDepth=-1.0; //scm->normal = Vector3r(0,0,0);
+ scm->contactPoint = contactPt;
+ scm->penetrationDepth=-1.0; //scm->normal = Vector3r(0,0,0);
}
-
+
return true;
- }else{
- scm->contactPoint = contactPt;scm->penetrationDepth=-1.0;scm->normal = Vector3r(0,0,0);
+ } else {
+ scm->contactPoint = contactPt;
+ scm->penetrationDepth=-1.0;
+ scm->normal = Vector3r(0,0,0);
return false;
}
@@ -189,9 +208,10 @@
-void Ig2_PP_PP_ScGeom::BrentZeroSurf(const shared_ptr<Shape>& cm1, const State& state1, const Vector3r bracketA, const Vector3r bracketB, Vector3r& zero){
-
- Real a = 0.0; Real b = 1.0;
+void Ig2_PP_PP_ScGeom::BrentZeroSurf(const shared_ptr<Shape>& cm1, const State& state1, const Vector3r bracketA, const Vector3r bracketB, Vector3r& zero) {
+
+ Real a = 0.0;
+ Real b = 1.0;
Real t = pow(10,-16);
Real m = 0.0;
Real p = 0.0;
@@ -206,111 +226,135 @@
Real fb = evaluatePP(cm1,state1, bracketB); //evaluateFNoSphere(cm1,state1, bracketB); //
- if(fa*fb > 0.00001){
+ if(fa*fb > 0.00001) {
std::cout<<"fa: "<<fa<<", fb: "<<fb<<endl;
}
//if(fabs(fa)<fabs(fb)){bracketRange *= -1.0; Vector3r temp = bracketA; bracketA= bracketB; bracketB = temp;}
Real fc = 0.0;
-
+
Real tol = 2.0*DBL_EPSILON*fabs(b) + t;
int counter = 0;
do {
- if(counter == 0){
- c = a; fc = fa; d = b-a; e = d;
- if (fabs(fc) < fabs(fb) ){
- a = b; b = c; c = a;
- fa = fb; fb = fc; fc = fa;
- }
- }else{
- if (fb*fc > 0.0){
- c = a; fc = fa; d = b-a; e = d;
- }
- if(fabs(fc) < fabs(fb) ){
- a = b; b = c; c = a;
- fa = fb; fb = fc; fc = fa;
+ if(counter == 0) {
+ c = a;
+ fc = fa;
+ d = b-a;
+ e = d;
+ if (fabs(fc) < fabs(fb) ) {
+ a = b;
+ b = c;
+ c = a;
+ fa = fb;
+ fb = fc;
+ fc = fa;
+ }
+ } else {
+ if (fb*fc > 0.0) {
+ c = a;
+ fc = fa;
+ d = b-a;
+ e = d;
+ }
+ if(fabs(fc) < fabs(fb) ) {
+ a = b;
+ b = c;
+ c = a;
+ fa = fb;
+ fb = fc;
+ fc = fa;
}
}
- tol = 2.0*DBL_EPSILON*fabs(b) + t; m = 0.5*(c-b);
- if (fabs(m) > tol && fabs(fb) > pow(10,-13) ){
- if(fabs(e) < tol || fabs(fa) <= fabs(fb) ){
+ tol = 2.0*DBL_EPSILON*fabs(b) + t;
+ m = 0.5*(c-b);
+ if (fabs(m) > tol && fabs(fb) > pow(10,-13) ) {
+ if(fabs(e) < tol || fabs(fa) <= fabs(fb) ) {
d = m;
e = d;
- }else{
+ } else {
s = fb/fa;
- if(fabs(a - c)<pow(10,-15)){
- p = 2.0*m*s; q = 1.0 - s;
- }else{
- q = fa/fc; r = fb/fc;
+ if(fabs(a - c)<pow(10,-15)) {
+ p = 2.0*m*s;
+ q = 1.0 - s;
+ } else {
+ q = fa/fc;
+ r = fb/fc;
p = s*(2.0*m*q*(q-r) - (b-a)*(r-1.0));
q = (q-1.0)*(r-1.0)*(s-1.0);
}
-
- if (p > 0.0){
+
+ if (p > 0.0) {
q = -q;
- }else{
+ } else {
p = -p;
}
-
- s = e; e = d;
-
- if(( 2.0*p < (3.0*m*q - fabs(tol*q))) && (p < fabs(0.5*s*q)) ){
+
+ s = e;
+ e = d;
+
+ if(( 2.0*p < (3.0*m*q - fabs(tol*q))) && (p < fabs(0.5*s*q)) ) {
d = p/q;
- }else{
+ } else {
d = m;
e = d;
}
}
-
- a = b; fa = fb;
+
+ a = b;
+ fa = fb;
Real h = 0.0;
- if(fabs(d) >tol){
+ if(fabs(d) >tol) {
h = d;
- }else if(m > 0.0){
+ } else if(m > 0.0) {
h = tol;
- }else{
+ } else {
h = - tol;
}
b = b + h; // h*m_unitVec;
zero = bracketA + b*bracketRange;
fb = evaluatePP(cm1,state1, zero); //evaluateFNoSphere(cm1,state1, zero); //
-
-
- }else{
+
+
+ } else {
zero = bracketA + b*bracketRange;
//std::cout<<"b: "<<b<<", m: "<<m<<", tol: "<<tol<<", c: "<<c<<", a: "<<a<<endl;
break;
}
counter++;
- if(counter==10000){
+ if(counter==10000) {
std::cout<<"counter: "<<counter<<", m.norm: "<<m<<", fb: "<<fb<<endl;
}
- }while(1);
+ } while(1);
//}while( m.norm() > tol && fabs(fb) > pow(10,-13) );
}
/* Routine to get value of function (constraint) at a particular position */
-double Ig2_PP_PP_ScGeom::evaluatePP(const shared_ptr<Shape>& cm1, const State& state1, const Vector3r newTrial){
+double Ig2_PP_PP_ScGeom::evaluatePP(const shared_ptr<Shape>& cm1, const State& state1, const Vector3r newTrial) {
PotentialParticle *s1=static_cast<PotentialParticle*>(cm1.get());
///////////////////Transforming trial values to local frame of particles//////////////////
- Vector3r tempP1 = newTrial - state1.pos;
+ Vector3r tempP1 = newTrial - state1.pos;
/* Direction cosines */
//state1.ori.normalize();
- Vector3r localP1 = state1.ori.conjugate()*tempP1;
+ Vector3r localP1 = state1.ori.conjugate()*tempP1;
Real x = localP1.x();
Real y = localP1.y();
Real z = localP1.z();
int planeNo = s1->a.size();
- Real pSum2 = 0.0;
- for (int i=0; i<planeNo; i++){
- Real plane = s1->a[i]*x + s1->b[i]*y + s1->c[i]*z - s1->d[i]; if (plane<pow(10,-15)){plane = 0.0;}
+ Real pSum2 = 0.0;
+ for (int i=0; i<planeNo; i++) {
+ Real plane = s1->a[i]*x + s1->b[i]*y + s1->c[i]*z - s1->d[i];
+ if (plane<pow(10,-15)) {
+ plane = 0.0;
+ }
pSum2 += pow(plane,2);
}
- Real r = s1->r; Real R = s1->R; Real k = s1->k;
+ Real r = s1->r;
+ Real R = s1->R;
+ Real k = s1->k;
/* Additional sphere */
Real sphere = (pow(x,2) + pow(y,2) + pow(z,2))/pow(R,2);
/* Complete potential particle */
@@ -320,36 +364,44 @@
-Vector3r Ig2_PP_PP_ScGeom::getNormal(const shared_ptr<Shape>& cm1, const State& state1, const Vector3r newTrial){
+Vector3r Ig2_PP_PP_ScGeom::getNormal(const shared_ptr<Shape>& cm1, const State& state1, const Vector3r newTrial) {
PotentialParticle *s1=static_cast<PotentialParticle*>(cm1.get());
///////////////////Transforming trial values to local frame of particles//////////////////
- Vector3r tempP1 = newTrial - state1.pos;
+ Vector3r tempP1 = newTrial - state1.pos;
/* Direction cosines */
- Vector3r localP1(0,0,0);
+ Vector3r localP1(0,0,0);
localP1 = state1.ori.conjugate()*tempP1; //the body has been rotated by state.ori. So I will need to rotate the frame by state.ori too
Real x = localP1[0];
Real y = localP1[1];
Real z = localP1[2];
-
+
////////////////////////////// assembling potential planes particle 1//////////////////////////////
- int planeNo = s1->a.size();
- vector<double>p; Real pSum2 = 0.0;
- for (int i=0; i<planeNo; i++){
- Real plane = s1->a[i]*x + s1->b[i]*y + s1->c[i]*z -s1-> d[i]; if (plane<pow(10,-15)){plane = 0.0;}
+ int planeNo = s1->a.size();
+ vector<double>p;
+ Real pSum2 = 0.0;
+ for (int i=0; i<planeNo; i++) {
+ Real plane = s1->a[i]*x + s1->b[i]*y + s1->c[i]*z -s1-> d[i];
+ if (plane<pow(10,-15)) {
+ plane = 0.0;
+ }
p.push_back(plane);
pSum2 += pow(p[i],2);
}
- Real r = s1->r; Real R = s1->R; Real k = s1->k;
+ Real r = s1->r;
+ Real R = s1->R;
+ Real k = s1->k;
/* Additional sphere */
- Real sphere = (pow(x,2) + pow(y,2) + pow(z,2))/pow(R,2);
+ //Real sphere = (pow(x,2) + pow(y,2) + pow(z,2))/pow(R,2);
/* Complete potential particle */
- Real f = (1.0-k)*(pSum2/pow(r,2)-1.0)+k*(sphere -1.0);
+ //Real f = (1.0-k)*(pSum2/pow(r,2)-1.0)+k*(sphere -1.0);
//////////////////////////////// local first derivitive particle 1 /////////////////////////////////////
- Real pdxSum = 0.0; Real pdySum = 0.0; Real pdzSum = 0.0;
- for (int i=0; i<planeNo; i++){
+ Real pdxSum = 0.0;
+ Real pdySum = 0.0;
+ Real pdzSum = 0.0;
+ for (int i=0; i<planeNo; i++) {
pdxSum += (s1->a[i]*p[i]);
pdySum += (s1->b[i]*p[i]);
pdzSum += (s1->c[i]*p[i]);
@@ -364,23 +416,23 @@
//if(state1.ori==Quaternionr(1,0,0,0)){
// Q1.setIdentity();
//}else{
- Real q0 = state1.ori.w();
- Real q1 = state1.ori.x();
- Real q2 = state1.ori.y();
- Real q3 = state1.ori.z();
- Q1(0,0) = 2.0*pow(q0,2.0) -1.0 + 2.0*pow(q1,2.0);
- Q1(0,1) = 2.0*q1*q2 + 2.0*q0*q3;
- Q1(0,2) = 2.0*q1*q3 - 2.0*q0*q2;
- Q1(1,0) = 2.0*q1*q2 - 2.0*q0*q3;
- Q1(1,1) = 2.0*pow(q0,2.0) -1.0 + 2.0*pow(q2,2.0);
- Q1(1,2) = 2.0*q2*q3 + 2.0*q0*q1;
- Q1(2,0) = 2.0*q1*q3 + 2.0*q0*q2;
- Q1(2,1) = 2.0*q2*q3 - 2.0*q0*q1;
- Q1(2,2) = 2.0*pow(q0,2) -1.0 + 2.0*pow(q3,2.0);
+ Real q0 = state1.ori.w();
+ Real q1 = state1.ori.x();
+ Real q2 = state1.ori.y();
+ Real q3 = state1.ori.z();
+ Q1(0,0) = 2.0*pow(q0,2.0) -1.0 + 2.0*pow(q1,2.0);
+ Q1(0,1) = 2.0*q1*q2 + 2.0*q0*q3;
+ Q1(0,2) = 2.0*q1*q3 - 2.0*q0*q2;
+ Q1(1,0) = 2.0*q1*q2 - 2.0*q0*q3;
+ Q1(1,1) = 2.0*pow(q0,2.0) -1.0 + 2.0*pow(q2,2.0);
+ Q1(1,2) = 2.0*q2*q3 + 2.0*q0*q1;
+ Q1(2,0) = 2.0*q1*q3 + 2.0*q0*q2;
+ Q1(2,1) = 2.0*q2*q3 - 2.0*q0*q1;
+ Q1(2,2) = 2.0*pow(q0,2) -1.0 + 2.0*pow(q3,2.0);
- //Matrix3r Q1n = state1.ori.toRotationMatrix();
- //Matrix3r Q1c = state1.ori.conjugate().toRotationMatrix();
- //std::cout<<"Q1: "<<endl<<Q1<<endl<<"Q1n: "<<endl<<Q1n<<endl<<"Q1c: "<<endl<<Q1c<<endl<<endl;
+ //Matrix3r Q1n = state1.ori.toRotationMatrix();
+ //Matrix3r Q1c = state1.ori.conjugate().toRotationMatrix();
+ //std::cout<<"Q1: "<<endl<<Q1<<endl<<"Q1n: "<<endl<<Q1n<<endl<<"Q1c: "<<endl<<Q1c<<endl<<endl;
//}
///////////////////// global first and second derivitive for particle 1 /////////////////////////
@@ -388,7 +440,7 @@
Real Fdy = fdx * Q1(0,1) + fdy*Q1(1,1) + fdz*Q1(2,1);
Real Fdz = fdx * Q1(0,2) + fdy*Q1(1,2) + fdz*Q1(2,2);
- if (isnan(Fdx) == true || isnan(Fdy) == true || isnan(Fdz)==true){
+ if (isnan(Fdx) == true || isnan(Fdy) == true || isnan(Fdz)==true) {
std::cout<<"Q1(0,0): "<<Q1(0,0)<<","<<Q1(0,1)<<","<<Q1(0,2)<<","<<Q1(1,0)<<","<<Q1(1,1)<<","<<Q1(1,2)<<","<<Q1(2,0)<<","<<Q1(2,1)<<","<<Q1(2,2)<<", q:"<<q0<<","<<q1<<","<<q2<<","<<q3<<", fd: "<<fdx<<","<<fdy<<","<<fdz<<endl;
}
@@ -397,560 +449,603 @@
-#if 0
-bool Ig2_PP_PP_ScGeom::contactPtMosekF2(const shared_ptr<Shape>& cm1, const State& state1, const shared_ptr<Shape>& cm2, const State& state2, Vector3r &contactPt){
-
- PotentialParticle *s1=static_cast<PotentialParticle*>(cm1.get());
- PotentialParticle *s2=static_cast<PotentialParticle*>(cm2.get());
- Vector3r xGlobal(0.0,0.0,0.0);
-
-
-/* Parameters for particles A and B */
- double rA = s1->r;
- double kA = s1->k;
- double RA = s1->R;
- double rB = s2->r;
- double kB = s2->k;
- double RB = s2->R;
- int planeNoA = s1->a.size();
- int planeNoB = s2->a.size();
-
-/* Variables to keep things neat */
- double ksA = RA/sqrt(kA);
- double kpA = rA/sqrt(1.0 - kA);
- double ksB = RB/sqrt(kB);
- double kpB = rB/sqrt(1.0 - kB);
- int NUMCON = 3 + 1+ planeNoA + planeNoB;
- int NUMVAR = 6 + 2 + planeNoA + planeNoB;
-
-/* LINEAR CONSTRAINTS */
- MSKboundkeye bkc[NUMCON];
- bkc[0] = MSK_BK_FX;
- bkc[1] = MSK_BK_FX;
- bkc[2] = MSK_BK_FX;
- bkc[3] = MSK_BK_FX;
- for(int i=0; i<planeNoA + planeNoB; i++ ){
- bkc[4+i] = MSK_BK_UP;
- };
-
- double blc[NUMCON];
- blc[0] = state2.pos.x() - state1.pos.x();
- blc[1] = state2.pos.y() - state1.pos.y();
- blc[2] = state2.pos.z() - state1.pos.z();
- blc[3] = 0.0;
- for(int i=0; i<planeNoA; i++ ){
- blc[4+i] = -MSK_INFINITY;
- };
- for(int i=0; i<planeNoB; i++ ){
- blc[4+planeNoA+i] = -MSK_INFINITY;
- };
-
- double buc[NUMCON];
- buc[0] = state2.pos.x() - state1.pos.x();
- buc[1] = state2.pos.y() - state1.pos.y();
- buc[2] = state2.pos.z() - state1.pos.z();
- buc[3] = 0.0;
- for(int i=0; i<planeNoA; i++ ){
- buc[4+i] = s1->d[i];
- };
- for(int i=0; i<planeNoB; i++ ){
- buc[4+planeNoA+i] = s2->d[i];
- };
-
-/* BOUNDS */
- MSKboundkeye bkx[NUMVAR ];
- bkx[0] = MSK_BK_FR;
- bkx[1] = MSK_BK_FR;
- bkx[2] = MSK_BK_FR;
- bkx[3] = MSK_BK_FR;
- bkx[4] = MSK_BK_FR;
- bkx[5] = MSK_BK_FR;
- bkx[6] = MSK_BK_FR;
- bkx[7] = MSK_BK_FR;
- for(int i=0; i<planeNoA + planeNoB; i++ ){
- bkx[8+i] = MSK_BK_FR;
- };
-
-
- double blx[NUMVAR];
- blx[0] = -MSK_INFINITY;
- blx[1] = -MSK_INFINITY;
- blx[2] = -MSK_INFINITY;
- blx[3] = -MSK_INFINITY;
- blx[4] = -MSK_INFINITY;
- blx[5] = -MSK_INFINITY;
- blx[6] = -MSK_INFINITY;
- blx[7] = -MSK_INFINITY;
- for(int i=0; i<planeNoA + planeNoB; i++ ){
- blx[8+i] = -MSK_INFINITY;
- };
-
- double bux[NUMVAR];
- bux[0] = +MSK_INFINITY;
- bux[1] = +MSK_INFINITY;
- bux[2] = +MSK_INFINITY;
- bux[3] = +MSK_INFINITY;
- bux[4] = +MSK_INFINITY;
- bux[5] = +MSK_INFINITY;
- bux[6] = +MSK_INFINITY;
- bux[7] = +MSK_INFINITY;
- for(int i=0; i<planeNoA + planeNoB; i++ ){
- bux[8+i] = +MSK_INFINITY;
- };
-
- double c[NUMVAR];
- c[0] = 0.0;
- c[1] = 0.0;
- c[2] = 0.0;
- c[3] = 0.0;
- c[4] = 0.0;
- c[5] = 0.0;
- c[6] = 1.0;
- c[7] = 1.0;
- for(int i=0; i<planeNoA + planeNoB; i++ ){
- c[8+i] = 0.0;
- };
-
- vector<double> aval;
- vector<MSKintt> aptrb;
- vector<MSKintt> aptre;
- vector<MSKidxt> asub;
-
- Matrix3r Q1 = state1.ori.toRotationMatrix();
- Matrix3r Q2 = state2.ori.toRotationMatrix();
-
- int totalCount = 0;
- /* column 0 xA*/
- aptrb.push_back(0);
- int count = 0;
- if(fabs(Q1(0,0))>pow(10,-15) ){
- aval.push_back(ksA*Q1(0,0)); asub.push_back(0); count++;
- }
- if(fabs(Q1(1,0) )>pow(10,-15) ){
- aval.push_back(ksA*Q1(1,0)); asub.push_back(1); count++;
- }
- if(fabs(Q1(2,0) )>pow(10,-15) ){
- aval.push_back(ksA*Q1(2,0)); asub.push_back(2); count++;
- }
- for(int i=0; i < planeNoA; i++){
- if(fabs(s1->a[i]) > pow(10,-15) ){
- aval.push_back(ksA*s1->a[i]); asub.push_back(4+i);
- count++;
- }
- }
- aptre.push_back(count);
- totalCount += count;
-
- /* column 1 yA*/
- aptrb.push_back(aptre[0]);
- count = 0;
- if(fabs(Q1(0,1) )>pow(10,-15) ){
- aval.push_back(ksA*Q1(0,1)); asub.push_back(0); count++;
- }
- if(fabs(Q1(1,1))>pow(10,-15) ){
- aval.push_back(ksA*Q1(1,1)); asub.push_back(1); count++;
- }
- if(fabs(Q1(2,1) )>pow(10,-15) ){
- aval.push_back(ksA*Q1(2,1)); asub.push_back(2); count++;
- }
- for(int i=0; i < planeNoA; i++){
- if(fabs(s1->b[i]) > pow(10,-15) ){
- aval.push_back(ksA*s1->b[i]); asub.push_back(4+i);
- count++;
- }
- }
- aptre.push_back(aptrb[1] + count);
- totalCount += count;
-
- /* column 2 zA*/
- aptrb.push_back(aptre[1]);
- count = 0;
- if(fabs(Q1(0,2) )>pow(10,-15) ){
- aval.push_back(ksA*Q1(0,2)); asub.push_back(0); count++;
- }
- if(fabs(Q1(1,2))>pow(10,-15) ){
- aval.push_back(ksA*Q1(1,2)); asub.push_back(1); count++;
- }
- if(fabs(Q1(2,2) )>pow(10,-15) ){
- aval.push_back(ksA*Q1(2,2)); asub.push_back(2); count++;
- }
- for(int i=0; i < planeNoA; i++){
- if(fabs(s1->c[i]) > pow(10,-15) ){
- aval.push_back(ksA*s1->c[i]); asub.push_back(4+i);
- count++;
- }
- }
- aptre.push_back(aptrb[2] + count);
- totalCount += count;
-
-
- /* column 3 xB*/
- aptrb.push_back(aptre[2]);
- count = 0;
- if(fabs(Q2(0,0))>pow(10,-15) ){
- aval.push_back(-ksB*Q2(0,0)); asub.push_back(0); count++;
- }
- if(fabs(Q2(1,0) )>pow(10,-15) ){
- aval.push_back(-ksB*Q2(1,0)); asub.push_back(1); count++;
- }
- if(fabs(Q2(2,0) )>pow(10,-15) ){
- aval.push_back(-ksB*Q2(2,0)); asub.push_back(2); count++;
- }
- for(int i=0; i < planeNoB; i++){
- if(fabs(s2->a[i]) > pow(10,-15) ){
- aval.push_back(ksB*s2->a[i]); asub.push_back(4+planeNoA +i);
- count++;
- }
- }
- aptre.push_back(aptrb[3] + count);
- totalCount += count;
-
-
- /* column 4 yB*/
- aptrb.push_back(aptre[3]);
- count = 0;
- if(fabs(Q2(0,1))>pow(10,-15) ){
- aval.push_back(-ksB*Q2(0,1)); asub.push_back(0); count++;
- }
- if(fabs(Q2(1,1) )>pow(10,-15) ){
- aval.push_back(-ksB*Q2(1,1)); asub.push_back(1); count++;
- }
- if(fabs(Q2(2,1) )>pow(10,-15) ){
- aval.push_back(-ksB*Q2(2,1)); asub.push_back(2); count++;
- }
- for(int i=0; i < planeNoB; i++){
- if(fabs(s2->b[i]) > pow(10,-15) ){
- aval.push_back(ksB*s2->b[i]); asub.push_back(4+planeNoA+i);
- count++;
- }
- }
- aptre.push_back(aptrb[4] + count);
- totalCount += count;
-
-
- /* column 5 zB*/
- aptrb.push_back(aptre[4]);
- count = 0;
- if(fabs(Q2(0,2))>pow(10,-15) ){
- aval.push_back(-ksB*Q2(0,2)); asub.push_back(0); count++;
- }
- if(fabs(Q2(1,2) )>pow(10,-15) ){
- aval.push_back(-ksB*Q2(1,2)); asub.push_back(1); count++;
- }
- if(fabs(Q2(2,2) )>pow(10,-15) ){
- aval.push_back(-ksB*Q2(2,2)); asub.push_back(2); count++;
- }
- for(int i=0; i < planeNoB; i++){
- if(fabs(s2->c[i]) > pow(10,-15) ){
- aval.push_back(ksB*s2->c[i]); asub.push_back(4+planeNoA+i);
- count++;
- }
- }
- aptre.push_back(aptrb[5] + count);
- totalCount += count;
-
-
- /*column 6 */
- aptrb.push_back(aptre[5]);
- aval.push_back(1.0); asub.push_back(3);
- aptre.push_back(aptrb[6]+1);
- totalCount += 1;
-
-
- /*column 7 */
- aptrb.push_back(aptre[6]);
- aval.push_back(-1.0); asub.push_back(3);
- aptre.push_back(aptrb[7]+1);
- totalCount += 1;
-
- /*column 8 + i*/
- for(int i=0; i < planeNoA; i++){
- aptrb.push_back(aptre[8-1+i]);
- aval.push_back(-1.0*kpA); asub.push_back(4 + i);
- aptre.push_back(aptrb[8+i]+1);
- }
- totalCount += planeNoA;
-
- /*column 8 + planeNoA + i */
- for(int i=0; i < planeNoB; i++){
- aptrb.push_back(aptre[ 8 + planeNoA-1+i]);
- aval.push_back(-1.0*kpB); asub.push_back(4 + planeNoA + i);
- aptre.push_back(aptrb[8 + planeNoA+i]+1);
- }
- totalCount += planeNoB;
-
- MSKidxt i,j,csubA[1+ 3 + planeNoA], csubB[1+ 3 + planeNoB] ;
- double xx[NUMVAR];
- MSKenv_t & env=mosekEnv;
- MSKtask_t task;
- MSKrescodee & r=mosekTaskEnv;
-
-
- if ( r==MSK_RES_OK )
- {
- /* Directs the log stream to the
- 'printstr' function. */
- // MSK_linkfunctoenvstream(env,MSK_STREAM_LOG,NULL,NULL); //printstr
- // MSK_linkfunctoenvstream(env,MSK_STREAM_LOG,NULL,NULL);
- }
-
-
-
- if ( r==MSK_RES_OK )
- {
- /* Create the optimization task. */
- r = MSK_maketask(env,NUMCON,NUMVAR,&task);
-
- if ( r==MSK_RES_OK )
- {
- // MSK_linkfunctotaskstream(task,MSK_STREAM_LOG,NULL,printstr);
-
- /* Give MOSEK an estimate of the size of the input data.
- This is done to increase the speed of inputting data.
- However, it is optional. */
- if (r == MSK_RES_OK)
- r = MSK_putmaxnumvar(task,NUMVAR);
-
- if (r == MSK_RES_OK)
- r = MSK_putmaxnumcon(task,NUMCON);
-
- int NUMANZ = totalCount;
- if (r == MSK_RES_OK)
- r = MSK_putmaxnumanz(task,NUMANZ);
-
- /* Append 'NUMCON' empty constraints.
- The constraints will initially have no bounds. */
- if ( r == MSK_RES_OK )
- r = MSK_append(task,MSK_ACC_CON,NUMCON);
-
- /* Append 'NUMVAR' variables.
- The variables will initially be fixed at zero (x=0). */
- if ( r == MSK_RES_OK )
- r = MSK_append(task,MSK_ACC_VAR,NUMVAR);
-
- /* Optionally add a constant term to the objective. */
- if ( r ==MSK_RES_OK )
- r = MSK_putcfix(task,0.0);
-
-
- for(j=0; j<NUMVAR && r == MSK_RES_OK; ++j)
- {
- /* Set the linear term c_j in the objective.*/
- if(r == MSK_RES_OK)
- r = MSK_putcj(task,j,c[j]);
-
-
- /* Set the bounds on variable j.
- blx[j] <= x_j <= bux[j] */
- if(r == MSK_RES_OK)
- r = MSK_putbound(task,
- MSK_ACC_VAR, /* Put bounds on variables.*/
- j, /* Index of variable.*/
- bkx[j], /* Bound key.*/
- blx[j], /* Numerical value of lower bound.*/
- bux[j]); /* Numerical value of upper bound.*/
-
-
- /* Input column j of A */
- if(r == MSK_RES_OK)
- r = MSK_putavec(task,
- MSK_ACC_VAR, /* Input columns of A.*/
- j, /* Variable (column) index.*/
- aptre[j]-aptrb[j], /* Number of non-zeros in column j.*/
- &asub[0]+aptrb[j], /* Pointer to row indexes of column j.*/
- &aval[0]+aptrb[j]); /* Pointer to Values of column j.*/
-
-
-
- }
-
-
- /* Set the bounds on constraints.
- for i=1, ...,NUMCON : blc[i] <= constraint i <= buc[i] */
- for(i=0; i<NUMCON && r==MSK_RES_OK; ++i)
-{
-
- r = MSK_putbound(task,
- MSK_ACC_CON, /* Put bounds on constraints.*/
- i, /* Index of constraint.*/
- bkc[i], /* Bound key.*/
- blc[i], /* Numerical value of lower bound.*/
- buc[i]); /* Numerical value of upper bound.*/
- }
-
- if ( r==MSK_RES_OK )
- {
- /* Append the first cone. */
- csubA[0] = 6;
- csubA[1] = 0;
- csubA[2] = 1;
- csubA[3] = 2;
- for(int i=0; i<planeNoA; i++){
- csubA[4+i] = 8+i;
- };
- r = MSK_appendcone(task,
- MSK_CT_QUAD,
- 0.0, /* For future use only, can be set to 0.0 */
-
- 1 + 3 + planeNoA,
- csubA);
- }
-
- if ( r==MSK_RES_OK )
- {
- /* Append the second cone. */
- csubB[0] = 7;
- csubB[1] = 3;
- csubB[2] = 4;
- csubB[3] = 5;
- for(int i=0; i<planeNoB; i++){
- csubB[4+i] = 8 + planeNoA + i ;
- };
- r = MSK_appendcone(task, MSK_CT_QUAD, 0.0, 1 + 3 + planeNoB, csubB);
- }
-
-
- if ( r==MSK_RES_OK )
- {
- MSKrescodee trmcode;
-
- r = MSK_putintparam ( task , MSK_IPAR_PRESOLVE_USE , MSK_PRESOLVE_MODE_OFF );
- r = MSK_putintparam ( task , MSK_IPAR_CHECK_CONVEXITY, MSK_CHECK_CONVEXITY_NONE );
- r = MSK_putintparam ( task , MSK_IPAR_INTPNT_SCALING, MSK_SCALING_NONE);
- r = MSK_putintparam ( task , MSK_IPAR_INTPNT_SOLVE_FORM, MSK_SOLVE_DUAL );
- r = MSK_putdouparam ( task , MSK_DPAR_INTPNT_CO_TOL_PFEAS , pow(10,-5) );
- r = MSK_putdouparam ( task , MSK_DPAR_INTPNT_CO_TOL_DFEAS, pow(10,-5) );
- r = MSK_putdouparam ( task , MSK_DPAR_INTPNT_CO_TOL_REL_GAP, pow(10,-8) ); //optimality
- r = MSK_putdouparam ( task , MSK_DPAR_INTPNT_TOL_INFEAS, pow(10,-5) );
- r = MSK_putdouparam ( task , MSK_DPAR_INTPNT_CO_TOL_MU_RED, pow(10,-8) ); //complementary
- /* Run optimizer */
- //MSK_writedata(task,"ig.opf.gz");
- r = MSK_optimizetrm(task,&trmcode);
- /* Print a summary containing information
- about the solution for debugging purposes*/
- //MSK_solutionsummary (task,MSK_STREAM_LOG);
-
- if ( r==MSK_RES_OK )
- {
- MSKsolstae solsta;
- MSKidxt j;
- MSK_getsolutionstatus (task,
- MSK_SOL_ITR,
- NULL,
- &solsta);
-
- switch(solsta)
- {
- case MSK_SOL_STA_OPTIMAL:
- MSK_getsolutionslice(task,
- MSK_SOL_ITR, /* Request the interior solution. */
- MSK_SOL_ITEM_XX,/* Which part of solution. */
- 0, /* Index of first variable. */
- NUMVAR, /* Index of last variable+1. */
- xx);
- break;
- case MSK_SOL_STA_NEAR_OPTIMAL:
- MSK_getsolutionslice(task,
- MSK_SOL_ITR, /* Request the interior solution. */
- MSK_SOL_ITEM_XX,/* Which part of solution. */
- 0, /* Index of first variable. */
- NUMVAR, /* Index of last variable+1. */
- xx);
-
- printf("Near Optimal primal solution\n");
- // for(j=0; j<NUMVAR; ++j)
- // printf("x[%d]: %e\n",j,xx[j]);
-
- break;
- case MSK_SOL_STA_DUAL_INFEAS_CER:
- case MSK_SOL_STA_PRIM_INFEAS_CER:
- case MSK_SOL_STA_NEAR_DUAL_INFEAS_CER:
- case MSK_SOL_STA_NEAR_PRIM_INFEAS_CER:
- printf("Primal or dual infeasibility certificate found.\n");
- break;
-
- case MSK_SOL_STA_UNKNOWN:
- printf("The status of the solution could not be determined.\n");
- //MSK_writedata(task,"mytask.opf.gz");
- break;
- default:
- printf("Other solution status.");
- break;
- }
-
-
- double xlocalA = xx[0]*ksA;
- double ylocalA = xx[1]*ksA;
- double zlocalA = xx[2]*ksA;
- double xlocalB = xx[3]*ksB;
- double ylocalB = xx[4]*ksB;
- double zlocalB = xx[5]*ksB;
- Vector3r localA = Vector3r(xlocalA,ylocalA,zlocalA);
- Vector3r localB = Vector3r(xlocalB,ylocalB,zlocalB);
- xGlobal = state1.ori*localA + state1.pos;
-
-
- //printf("xA: %f \t yA: %f \nxB: %f \t yB: %f \nxgA: %f \t ygA: %f \nxgB: %f \t ygB: %f \n",xlocalA,ylocalA, xlocalB, ylocalB, xGlobalA, yGlobalA, xGlobalB, yGlobalB);
- }
- else
- {
- printf("Error while optimizing.\n");
- }
- }
-
- if (r != MSK_RES_OK)
- {
- /* In case of an error print error code and description. */
- char symname[MSK_MAX_STR_LEN];
- char desc[MSK_MAX_STR_LEN];
-
- printf("An error occurred while optimizing.\n");
- MSK_getcodedesc (r,
- symname,
- desc);
- printf("Error %s - '%s'\n",symname,desc);
- }
- }
- /* Delete the task and the associated data. */
- MSK_deletetask(&task);
- }
-
- /* Delete the environment and the associated data. */
- // MSK_deleteenv(&env);
-
- contactPt = xGlobal;
- aval.clear();
- asub.clear();
-
- return ( true );
+#ifdef YADE_MOSEK
+bool Ig2_PP_PP_ScGeom::contactPtMosekF2(const shared_ptr<Shape>& cm1, const State& state1, const shared_ptr<Shape>& cm2, const State& state2, Vector3r &contactPt) {
+
+ PotentialParticle *s1=static_cast<PotentialParticle*>(cm1.get());
+ PotentialParticle *s2=static_cast<PotentialParticle*>(cm2.get());
+ Vector3r xGlobal(0.0,0.0,0.0);
+
+
+ /* Parameters for particles A and B */
+ double rA = s1->r;
+ double kA = s1->k;
+ double RA = s1->R;
+ double rB = s2->r;
+ double kB = s2->k;
+ double RB = s2->R;
+ int planeNoA = s1->a.size();
+ int planeNoB = s2->a.size();
+
+ /* Variables to keep things neat */
+ double ksA = RA/sqrt(kA);
+ double kpA = rA/sqrt(1.0 - kA);
+ double ksB = RB/sqrt(kB);
+ double kpB = rB/sqrt(1.0 - kB);
+ int NUMCON = 3 + 1+ planeNoA + planeNoB;
+ int NUMVAR = 6 + 2 + planeNoA + planeNoB;
+
+ /* LINEAR CONSTRAINTS */
+ MSKboundkeye bkc[NUMCON];
+ bkc[0] = MSK_BK_FX;
+ bkc[1] = MSK_BK_FX;
+ bkc[2] = MSK_BK_FX;
+ bkc[3] = MSK_BK_FX;
+ for(int i=0; i<planeNoA + planeNoB; i++ ) {
+ bkc[4+i] = MSK_BK_UP;
+ };
+
+ double blc[NUMCON];
+ blc[0] = state2.pos.x() - state1.pos.x();
+ blc[1] = state2.pos.y() - state1.pos.y();
+ blc[2] = state2.pos.z() - state1.pos.z();
+ blc[3] = 0.0;
+ for(int i=0; i<planeNoA; i++ ) {
+ blc[4+i] = -MSK_INFINITY;
+ };
+ for(int i=0; i<planeNoB; i++ ) {
+ blc[4+planeNoA+i] = -MSK_INFINITY;
+ };
+
+ double buc[NUMCON];
+ buc[0] = state2.pos.x() - state1.pos.x();
+ buc[1] = state2.pos.y() - state1.pos.y();
+ buc[2] = state2.pos.z() - state1.pos.z();
+ buc[3] = 0.0;
+ for(int i=0; i<planeNoA; i++ ) {
+ buc[4+i] = s1->d[i];
+ };
+ for(int i=0; i<planeNoB; i++ ) {
+ buc[4+planeNoA+i] = s2->d[i];
+ };
+
+ /* BOUNDS */
+ MSKboundkeye bkx[NUMVAR ];
+ bkx[0] = MSK_BK_FR;
+ bkx[1] = MSK_BK_FR;
+ bkx[2] = MSK_BK_FR;
+ bkx[3] = MSK_BK_FR;
+ bkx[4] = MSK_BK_FR;
+ bkx[5] = MSK_BK_FR;
+ bkx[6] = MSK_BK_FR;
+ bkx[7] = MSK_BK_FR;
+ for(int i=0; i<planeNoA + planeNoB; i++ ) {
+ bkx[8+i] = MSK_BK_FR;
+ };
+
+
+ double blx[NUMVAR];
+ blx[0] = -MSK_INFINITY;
+ blx[1] = -MSK_INFINITY;
+ blx[2] = -MSK_INFINITY;
+ blx[3] = -MSK_INFINITY;
+ blx[4] = -MSK_INFINITY;
+ blx[5] = -MSK_INFINITY;
+ blx[6] = -MSK_INFINITY;
+ blx[7] = -MSK_INFINITY;
+ for(int i=0; i<planeNoA + planeNoB; i++ ) {
+ blx[8+i] = -MSK_INFINITY;
+ };
+
+ double bux[NUMVAR];
+ bux[0] = +MSK_INFINITY;
+ bux[1] = +MSK_INFINITY;
+ bux[2] = +MSK_INFINITY;
+ bux[3] = +MSK_INFINITY;
+ bux[4] = +MSK_INFINITY;
+ bux[5] = +MSK_INFINITY;
+ bux[6] = +MSK_INFINITY;
+ bux[7] = +MSK_INFINITY;
+ for(int i=0; i<planeNoA + planeNoB; i++ ) {
+ bux[8+i] = +MSK_INFINITY;
+ };
+
+ double c[NUMVAR];
+ c[0] = 0.0;
+ c[1] = 0.0;
+ c[2] = 0.0;
+ c[3] = 0.0;
+ c[4] = 0.0;
+ c[5] = 0.0;
+ c[6] = 1.0;
+ c[7] = 1.0;
+ for(int i=0; i<planeNoA + planeNoB; i++ ) {
+ c[8+i] = 0.0;
+ };
+
+ vector<double> aval;
+ vector<MSKintt> aptrb;
+ vector<MSKintt> aptre;
+ vector<MSKidxt> asub;
+
+ Matrix3r Q1 = state1.ori.toRotationMatrix();
+ Matrix3r Q2 = state2.ori.toRotationMatrix();
+
+ int totalCount = 0;
+ /* column 0 xA*/
+ aptrb.push_back(0);
+ int count = 0;
+ if(fabs(Q1(0,0))>pow(10,-15) ) {
+ aval.push_back(ksA*Q1(0,0));
+ asub.push_back(0);
+ count++;
+ }
+ if(fabs(Q1(1,0) )>pow(10,-15) ) {
+ aval.push_back(ksA*Q1(1,0));
+ asub.push_back(1);
+ count++;
+ }
+ if(fabs(Q1(2,0) )>pow(10,-15) ) {
+ aval.push_back(ksA*Q1(2,0));
+ asub.push_back(2);
+ count++;
+ }
+ for(int i=0; i < planeNoA; i++) {
+ if(fabs(s1->a[i]) > pow(10,-15) ) {
+ aval.push_back(ksA*s1->a[i]);
+ asub.push_back(4+i);
+ count++;
+ }
+ }
+ aptre.push_back(count);
+ totalCount += count;
+
+ /* column 1 yA*/
+ aptrb.push_back(aptre[0]);
+ count = 0;
+ if(fabs(Q1(0,1) )>pow(10,-15) ) {
+ aval.push_back(ksA*Q1(0,1));
+ asub.push_back(0);
+ count++;
+ }
+ if(fabs(Q1(1,1))>pow(10,-15) ) {
+ aval.push_back(ksA*Q1(1,1));
+ asub.push_back(1);
+ count++;
+ }
+ if(fabs(Q1(2,1) )>pow(10,-15) ) {
+ aval.push_back(ksA*Q1(2,1));
+ asub.push_back(2);
+ count++;
+ }
+ for(int i=0; i < planeNoA; i++) {
+ if(fabs(s1->b[i]) > pow(10,-15) ) {
+ aval.push_back(ksA*s1->b[i]);
+ asub.push_back(4+i);
+ count++;
+ }
+ }
+ aptre.push_back(aptrb[1] + count);
+ totalCount += count;
+
+ /* column 2 zA*/
+ aptrb.push_back(aptre[1]);
+ count = 0;
+ if(fabs(Q1(0,2) )>pow(10,-15) ) {
+ aval.push_back(ksA*Q1(0,2));
+ asub.push_back(0);
+ count++;
+ }
+ if(fabs(Q1(1,2))>pow(10,-15) ) {
+ aval.push_back(ksA*Q1(1,2));
+ asub.push_back(1);
+ count++;
+ }
+ if(fabs(Q1(2,2) )>pow(10,-15) ) {
+ aval.push_back(ksA*Q1(2,2));
+ asub.push_back(2);
+ count++;
+ }
+ for(int i=0; i < planeNoA; i++) {
+ if(fabs(s1->c[i]) > pow(10,-15) ) {
+ aval.push_back(ksA*s1->c[i]);
+ asub.push_back(4+i);
+ count++;
+ }
+ }
+ aptre.push_back(aptrb[2] + count);
+ totalCount += count;
+
+
+ /* column 3 xB*/
+ aptrb.push_back(aptre[2]);
+ count = 0;
+ if(fabs(Q2(0,0))>pow(10,-15) ) {
+ aval.push_back(-ksB*Q2(0,0));
+ asub.push_back(0);
+ count++;
+ }
+ if(fabs(Q2(1,0) )>pow(10,-15) ) {
+ aval.push_back(-ksB*Q2(1,0));
+ asub.push_back(1);
+ count++;
+ }
+ if(fabs(Q2(2,0) )>pow(10,-15) ) {
+ aval.push_back(-ksB*Q2(2,0));
+ asub.push_back(2);
+ count++;
+ }
+ for(int i=0; i < planeNoB; i++) {
+ if(fabs(s2->a[i]) > pow(10,-15) ) {
+ aval.push_back(ksB*s2->a[i]);
+ asub.push_back(4+planeNoA +i);
+ count++;
+ }
+ }
+ aptre.push_back(aptrb[3] + count);
+ totalCount += count;
+
+
+ /* column 4 yB*/
+ aptrb.push_back(aptre[3]);
+ count = 0;
+ if(fabs(Q2(0,1))>pow(10,-15) ) {
+ aval.push_back(-ksB*Q2(0,1));
+ asub.push_back(0);
+ count++;
+ }
+ if(fabs(Q2(1,1) )>pow(10,-15) ) {
+ aval.push_back(-ksB*Q2(1,1));
+ asub.push_back(1);
+ count++;
+ }
+ if(fabs(Q2(2,1) )>pow(10,-15) ) {
+ aval.push_back(-ksB*Q2(2,1));
+ asub.push_back(2);
+ count++;
+ }
+ for(int i=0; i < planeNoB; i++) {
+ if(fabs(s2->b[i]) > pow(10,-15) ) {
+ aval.push_back(ksB*s2->b[i]);
+ asub.push_back(4+planeNoA+i);
+ count++;
+ }
+ }
+ aptre.push_back(aptrb[4] + count);
+ totalCount += count;
+
+
+ /* column 5 zB*/
+ aptrb.push_back(aptre[4]);
+ count = 0;
+ if(fabs(Q2(0,2))>pow(10,-15) ) {
+ aval.push_back(-ksB*Q2(0,2));
+ asub.push_back(0);
+ count++;
+ }
+ if(fabs(Q2(1,2) )>pow(10,-15) ) {
+ aval.push_back(-ksB*Q2(1,2));
+ asub.push_back(1);
+ count++;
+ }
+ if(fabs(Q2(2,2) )>pow(10,-15) ) {
+ aval.push_back(-ksB*Q2(2,2));
+ asub.push_back(2);
+ count++;
+ }
+ for(int i=0; i < planeNoB; i++) {
+ if(fabs(s2->c[i]) > pow(10,-15) ) {
+ aval.push_back(ksB*s2->c[i]);
+ asub.push_back(4+planeNoA+i);
+ count++;
+ }
+ }
+ aptre.push_back(aptrb[5] + count);
+ totalCount += count;
+
+
+ /*column 6 */
+ aptrb.push_back(aptre[5]);
+ aval.push_back(1.0);
+ asub.push_back(3);
+ aptre.push_back(aptrb[6]+1);
+ totalCount += 1;
+
+
+ /*column 7 */
+ aptrb.push_back(aptre[6]);
+ aval.push_back(-1.0);
+ asub.push_back(3);
+ aptre.push_back(aptrb[7]+1);
+ totalCount += 1;
+
+ /*column 8 + i*/
+ for(int i=0; i < planeNoA; i++) {
+ aptrb.push_back(aptre[8-1+i]);
+ aval.push_back(-1.0*kpA);
+ asub.push_back(4 + i);
+ aptre.push_back(aptrb[8+i]+1);
+ }
+ totalCount += planeNoA;
+
+ /*column 8 + planeNoA + i */
+ for(int i=0; i < planeNoB; i++) {
+ aptrb.push_back(aptre[ 8 + planeNoA-1+i]);
+ aval.push_back(-1.0*kpB);
+ asub.push_back(4 + planeNoA + i);
+ aptre.push_back(aptrb[8 + planeNoA+i]+1);
+ }
+ totalCount += planeNoB;
+
+ MSKidxt i,j,csubA[1+ 3 + planeNoA], csubB[1+ 3 + planeNoB] ;
+ double xx[NUMVAR];
+ MSKenv_t & env=mosekEnv;
+ MSKtask_t task;
+ MSKrescodee & r=mosekTaskEnv;
+
+
+ if ( r==MSK_RES_OK )
+
+ /* Directs the log stream to the
+ 'printstr' function. */
+ // MSK_linkfunctoenvstream(env,MSK_STREAM_LOG,NULL,NULL); //printstr
+ // MSK_linkfunctoenvstream(env,MSK_STREAM_LOG,NULL,NULL);
+
+ f ( r==MSK_RES_OK )
+
+ /* Create the optimization task. */
+ r = MSK_maketask(env,NUMCON,NUMVAR,&task);
+
+ if ( r==MSK_RES_OK )
+ {
+ // MSK_linkfunctotaskstream(task,MSK_STREAM_LOG,NULL,printstr);
+
+ /* Give MOSEK an estimate of the size of the input data.
+ This is done to increase the speed of inputting data.
+ However, it is optional. */
+ if (r == MSK_RES_OK)
+ r = MSK_putmaxnumvar(task,NUMVAR);
+
+ if (r == MSK_RES_OK)
+ r = MSK_putmaxnumcon(task,NUMCON);
+
+ int NUMANZ = totalCount;
+ if (r == MSK_RES_OK)
+ r = MSK_putmaxnumanz(task,NUMANZ);
+
+ /* Append 'NUMCON' empty constraints.
+ The constraints will initially have no bounds. */
+ if ( r == MSK_RES_OK )
+ r = MSK_append(task,MSK_ACC_CON,NUMCON);
+
+ /* Append 'NUMVAR' variables.
+ The variables will initially be fixed at zero (x=0). */
+ if ( r == MSK_RES_OK )
+ r = MSK_append(task,MSK_ACC_VAR,NUMVAR);
+
+ /* Optionally add a constant term to the objective. */
+ if ( r ==MSK_RES_OK )
+ r = MSK_putcfix(task,0.0);
+
+
+ for(j=0; j<NUMVAR && r == MSK_RES_OK; ++j)
+ {
+ /* Set the linear term c_j in the objective.*/
+ if(r == MSK_RES_OK)
+ r = MSK_putcj(task,j,c[j]);
+
+
+ /* Set the bounds on variable j.
+ blx[j] <= x_j <= bux[j] */
+ if(r == MSK_RES_OK)
+ r = MSK_putbound(task,
+ MSK_ACC_VAR, /* Put bounds on variables.*/
+ j, /* Index of variable.*/
+ bkx[j], /* Bound key.*/
+ blx[j], /* Numerical value of lower bound.*/
+ bux[j]); /* Numerical value of upper bound.*/
+
+
+ /* Input column j of A */
+ if(r == MSK_RES_OK)
+ r = MSK_putavec(task,
+ MSK_ACC_VAR, /* Input columns of A.*/
+ j, /* Variable (column) index.*/
+ aptre[j]-aptrb[j], /* Number of non-zeros in column j.*/
+ &asub[0]+aptrb[j], /* Pointer to row indexes of column j.*/
+ &aval[0]+aptrb[j]); /* Pointer to Values of column j.*/
+
+
+
+ }
+
+
+ /* Set the bounds on constraints.
+ for i=1, ...,NUMCON : blc[i] <= constraint i <= buc[i] */
+ for(i=0; i<NUMCON && r==MSK_RES_OK; ++i)
+ {
+
+ r = MSK_putbound(task,
+ MSK_ACC_CON, /* Put bounds on constraints.*/
+ i, /* Index of constraint.*/
+ bkc[i], /* Bound key.*/
+ blc[i], /* Numerical value of lower bound.*/
+ buc[i]); /* Numerical value of upper bound.*/
+ }
+
+ if ( r==MSK_RES_OK )
+ {
+ /* Append the first cone. */
+ csubA[0] = 6;
+ csubA[1] = 0;
+ csubA[2] = 1;
+ csubA[3] = 2;
+ for(int i=0; i<planeNoA; i++) {
+ csubA[4+i] = 8+i;
+ };
+ r = MSK_appendcone(task,
+ MSK_CT_QUAD,
+ 0.0, /* For future use only, can be set to 0.0 */
+
+ 1 + 3 + planeNoA,
+ csubA);
+ }
+
+ if ( r==MSK_RES_OK )
+ {
+ /* Append the second cone. */
+ csubB[0] = 7;
+ csubB[1] = 3;
+ csubB[2] = 4;
+ csubB[3] = 5;
+ for(int i=0; i<planeNoB; i++) {
+ csubB[4+i] = 8 + planeNoA + i ;
+ };
+ r = MSK_appendcone(task, MSK_CT_QUAD, 0.0, 1 + 3 + planeNoB, csubB);
+ }
+
+
+ if ( r==MSK_RES_OK )
+ {
+ MSKrescodee trmcode;
+
+ r = MSK_putintparam ( task , MSK_IPAR_PRESOLVE_USE , MSK_PRESOLVE_MODE_OFF );
+ r = MSK_putintparam ( task , MSK_IPAR_CHECK_CONVEXITY, MSK_CHECK_CONVEXITY_NONE );
+ r = MSK_putintparam ( task , MSK_IPAR_INTPNT_SCALING, MSK_SCALING_NONE);
+ r = MSK_putintparam ( task , MSK_IPAR_INTPNT_SOLVE_FORM, MSK_SOLVE_DUAL );
+ r = MSK_putdouparam ( task , MSK_DPAR_INTPNT_CO_TOL_PFEAS , pow(10,-5) );
+ r = MSK_putdouparam ( task , MSK_DPAR_INTPNT_CO_TOL_DFEAS, pow(10,-5) );
+ r = MSK_putdouparam ( task , MSK_DPAR_INTPNT_CO_TOL_REL_GAP, pow(10,-8) ); //optimality
+ r = MSK_putdouparam ( task , MSK_DPAR_INTPNT_TOL_INFEAS, pow(10,-5) );
+ r = MSK_putdouparam ( task , MSK_DPAR_INTPNT_CO_TOL_MU_RED, pow(10,-8) ); //complementary
+ /* Run optimizer */
+ //MSK_writedata(task,"ig.opf.gz");
+ r = MSK_optimizetrm(task,&trmcode);
+ /* Print a summary containing information
+ about the solution for debugging purposes*/
+ //MSK_solutionsummary (task,MSK_STREAM_LOG);
+
+ if ( r==MSK_RES_OK )
+ {
+ MSKsolstae solsta;
+ MSKidxt j;
+ MSK_getsolutionstatus (task,
+ MSK_SOL_ITR,
+ NULL,
+ &solsta);
+
+ switch(solsta)
+ {
+ case MSK_SOL_STA_OPTIMAL:
+ MSK_getsolutionslice(task,
+ MSK_SOL_ITR, /* Request the interior solution. */
+ MSK_SOL_ITEM_XX,/* Which part of solution. */
+ 0, /* Index of first variable. */
+ NUMVAR, /* Index of last variable+1. */
+ xx);
+ break;
+ case MSK_SOL_STA_NEAR_OPTIMAL:
+ MSK_getsolutionslice(task,
+ MSK_SOL_ITR, /* Request the interior solution. */
+ MSK_SOL_ITEM_XX,/* Which part of solution. */
+ 0, /* Index of first variable. */
+ NUMVAR, /* Index of last variable+1. */
+ xx);
+
+ printf("Near Optimal primal solution\n");
+ // for(j=0; j<NUMVAR; ++j)
+ // printf("x[%d]: %e\n",j,xx[j]);
+
+ break;
+ case MSK_SOL_STA_DUAL_INFEAS_CER:
+ case MSK_SOL_STA_PRIM_INFEAS_CER:
+ case MSK_SOL_STA_NEAR_DUAL_INFEAS_CER:
+ case MSK_SOL_STA_NEAR_PRIM_INFEAS_CER:
+ printf("Primal or dual infeasibility certificate found.\n");
+ break;
+
+ case MSK_SOL_STA_UNKNOWN:
+ printf("The status of the solution could not be determined.\n");
+ //MSK_writedata(task,"mytask.opf.gz");
+ break;
+ default:
+ printf("Other solution status.");
+ break;
+ }
+
+
+ double xlocalA = xx[0]*ksA;
+ double ylocalA = xx[1]*ksA;
+ double zlocalA = xx[2]*ksA;
+ double xlocalB = xx[3]*ksB;
+ double ylocalB = xx[4]*ksB;
+ double zlocalB = xx[5]*ksB;
+ Vector3r localA = Vector3r(xlocalA,ylocalA,zlocalA);
+ Vector3r localB = Vector3r(xlocalB,ylocalB,zlocalB);
+ xGlobal = state1.ori*localA + state1.pos;
+
+
+ //printf("xA: %f \t yA: %f \nxB: %f \t yB: %f \nxgA: %f \t ygA: %f \nxgB: %f \t ygB: %f \n",xlocalA,ylocalA, xlocalB, ylocalB, xGlobalA, yGlobalA, xGlobalB, yGlobalB);
+ }
+ else
+ {
+ printf("Error while optimizing.\n");
+ }
+ }
+
+ if (r != MSK_RES_OK)
+ {
+ /* In case of an error print error code and description. */
+ char symname[MSK_MAX_STR_LEN];
+ char desc[MSK_MAX_STR_LEN];
+
+ printf("An error occurred while optimizing.\n");
+ MSK_getcodedesc (r,
+ symname,
+ desc);
+ printf("Error %s - '%s'\n",symname,desc);
+ }
+ }
+ /* Delete the task and the associated data. */
+ MSK_deletetask(&task);
+}
+
+/* Delete the environment and the associated data. */
+// MSK_deleteenv(&env);
+
+contactPt = xGlobal;
+aval.clear();
+asub.clear();
+
+return ( true );
}
#endif
-void Ig2_PP_PP_ScGeom::getPtOnParticle2(const shared_ptr<Shape>& cm1, const State& state1, Vector3r midPoint, Vector3r normal, Vector3r& ptOnParticle){
- PotentialParticle *s1=static_cast<PotentialParticle*>(cm1.get());
+void Ig2_PP_PP_ScGeom::getPtOnParticle2(const shared_ptr<Shape>& cm1, const State& state1, Vector3r midPoint, Vector3r normal, Vector3r& ptOnParticle) {
+ //PotentialParticle *s1=static_cast<PotentialParticle*>(cm1.get());
ptOnParticle = midPoint;
Real f = evaluatePP(cm1,state1, ptOnParticle);//evaluateFNoSphere(cm1,state1, ptOnParticle); //
Real fprevious = f;
int counter = 0;
//normal.normalize();
- Vector3r step = normal*Mathr::Sign(f) *-1.0;
+ Vector3r step = normal*Mathr::Sign(f) *-1.0;
Vector3r bracketA(0,0,0);
Vector3r bracketB(0,0,0);
- do{
- ptOnParticle += step;
- fprevious = f;
- f = evaluatePP(cm1,state1, ptOnParticle); //evaluateFNoSphere(cm1,state1, ptOnParticle); //
- counter++;
- if (counter == 50000 ){
- //LOG_WARN("Initial point searching exceeded 500 iterations!");
- std::cout<<"ptonparticle2 search exceeded 50000 iterations! step:"<<step<<endl;
- }
+ do {
+ ptOnParticle += step;
+ fprevious = f;
+ f = evaluatePP(cm1,state1, ptOnParticle); //evaluateFNoSphere(cm1,state1, ptOnParticle); //
+ counter++;
+ if (counter == 50000 ) {
+ //LOG_WARN("Initial point searching exceeded 500 iterations!");
+ std::cout<<"ptonparticle2 search exceeded 50000 iterations! step:"<<step<<endl;
+ }
- }while(Mathr::Sign(fprevious)*Mathr::Sign(f)*1.0> 0.0 );
+ } while(Mathr::Sign(fprevious)*Mathr::Sign(f)*1.0> 0.0 );
bracketA = ptOnParticle;
bracketB = ptOnParticle -step;
Vector3r zero(0,0,0);
@@ -961,753 +1056,857 @@
-bool Ig2_PP_PP_ScGeom::customSolve(const shared_ptr<Shape>& cm1, const State& state1, const shared_ptr<Shape>& cm2, const State& state2, Vector3r &contactPt, bool warmstart){
- //timingDeltas->start();
- PotentialParticle *s1=static_cast<PotentialParticle*>(cm1.get());
- PotentialParticle *s2=static_cast<PotentialParticle*>(cm2.get());
- Vector3r xGlobal(0.0,0.0,0.0);
- int iter = 0;
- int totalIter = 0;
-
- /* Parameters for particles A and B */
- double rA = s1->r;double kA = s1->k;double RA = s1->R;
- double rB = s2->r;double kB = s2->k;double RB = s2->R;
- int planeNoA = s1->a.size();int planeNoB = s2->a.size();
- int varNo = 3+1+planeNoA+planeNoB; int planeNoAB = planeNoA + planeNoB; int varNo2 = varNo*varNo;
- int planeNoA3 = 3+planeNoA; int planeNoB3=3+planeNoB; int planeNoA2 =planeNoA*planeNoA; int planeNoB2=planeNoB*planeNoB;
- Matrix3r QA = state1.ori.conjugate().toRotationMatrix(); /*direction cosine */
-
- Matrix3r QB = state2.ori.conjugate().toRotationMatrix(); /*direction cosine */
-
- int blas3 = 3; char blasNT = 'N'; char blasT= 'T'; int blas1planeNoA = std::max(1,planeNoA); int blas1planeNoB = std::max(1,planeNoB); int blas1planeNoAB = std::max(1,planeNoAB); double blas0 = 0.0; double blas1 = 1.0; double blasNeg1 = -1.0;
-
- double blasQA[9]; double blasQB[9];
- double blasPosA[3]; double blasPosB[3];
- double blasContactPt[3];
- double blasC[varNo];
- for (int i=0; i<varNo; i++){
- blasC[i] = 0.0;
- }
- blasC[3]= 1.0;
- int blasCount = 0;
- for(int i=0; i<3; i++){
- for(int j=0; j<3; j++){
- blasQA[blasCount]=QA(j,i);
- blasQB[blasCount]=QB(j,i);
- blasCount++;
- }
- blasPosA[i]=state1.pos[i];
- blasPosB[i]=state2.pos[i];
- blasContactPt[i]=contactPt[i];
- }
+bool Ig2_PP_PP_ScGeom::customSolve(const shared_ptr<Shape>& cm1, const State& state1, const shared_ptr<Shape>& cm2, const State& state2, Vector3r &contactPt, bool warmstart) {
+ //timingDeltas->start();
+ PotentialParticle *s1=static_cast<PotentialParticle*>(cm1.get());
+ PotentialParticle *s2=static_cast<PotentialParticle*>(cm2.get());
+ Vector3r xGlobal(0.0,0.0,0.0);
+ int iter = 0;
+ int totalIter = 0;
+
+ /* Parameters for particles A and B */
+ double rA = s1->r;
+ double kA = s1->k;
+ double RA = s1->R;
+ double rB = s2->r;
+ double kB = s2->k;
+ double RB = s2->R;
+ int planeNoA = s1->a.size();
+ int planeNoB = s2->a.size();
+ int varNo = 3+1+planeNoA+planeNoB;
+ int planeNoAB = planeNoA + planeNoB;
+ int varNo2 = varNo*varNo;
+ int planeNoA3 = 3+planeNoA;
+ int planeNoB3=3+planeNoB;
+ //int planeNoA2 =planeNoA*planeNoA;
+ //int planeNoB2=planeNoB*planeNoB;
+ Matrix3r QA = state1.ori.conjugate().toRotationMatrix(); /*direction cosine */
+
+ Matrix3r QB = state2.ori.conjugate().toRotationMatrix(); /*direction cosine */
+
+ int blas3 = 3;
+ char blasNT = 'N';
+ char blasT= 'T';
+ int blas1planeNoA = std::max(1,planeNoA);
+ int blas1planeNoB = std::max(1,planeNoB);
+ int blas1planeNoAB = std::max(1,planeNoAB);
+ double blas0 = 0.0;
+ double blas1 = 1.0;
+ double blasNeg1 = -1.0;
+
+ double blasQA[9];
+ double blasQB[9];
+ double blasPosA[3];
+ double blasPosB[3];
+ double blasContactPt[3];
+ double blasC[varNo];
+ for (int i=0; i<varNo; i++) {
+ blasC[i] = 0.0;
+ }
+ blasC[3]= 1.0;
+ int blasCount = 0;
+ for(int i=0; i<3; i++) {
+ for(int j=0; j<3; j++) {
+ blasQA[blasCount]=QA(j,i);
+ blasQB[blasCount]=QB(j,i);
+ blasCount++;
+ }
+ blasPosA[i]=state1.pos[i];
+ blasPosB[i]=state2.pos[i];
+ blasContactPt[i]=contactPt[i];
+ }
//std::cout<<"QA: "<<QA<<endl;std::cout<<"blasQA: "<<endl;
//for (int i=0; i<9; i++){
// std::cout<<blasQA[i]<<endl;
//}
- /* Variables to keep things neat */
- double kAs = sqrt(kA)/RA;
- double kAp = sqrt(1.0 - kA)/rA;
- double kBs = sqrt(kB)/RB;
- double kBp = sqrt(1.0 - kB)/rB;
-
- /* penalty */
- double t = 1.0; double mu=10.0; double planePert = 0.1*rA; double sPert = 1.0; //+ 10.0*planePert*(planeNoA+planeNoB)/(rA*rA);
- if (warmstart == true){
- t = 0.01; mu= 50.0; planePert = 0.01; sPert = 0.01; ///* pow(10,-1)+ */ sqrt(planePert*planePert*(planeNoA+planeNoB)/(rA*rA));
- }
- /* s */
- double s = 0.0; double m=2.0; double NTTOL = pow(10,-8); double tol = accuracyTol/*pow(10,-4)* RA*pow(10,-6)*/; double gap =0.0;
- /* x */
- double blasX[varNo]; double blasNewX[varNo];
- blasX[0] = contactPt[0]; blasX[1] = contactPt[1]; blasX[2] = contactPt[2];
-
- //////////////////// evaluate fA and fB to initialize ////////////////////////////////
- /* fA */
- double blasTempP1[3]; double blasLocalP1[3];
- for(int i=0; i<3; i++){
- blasTempP1[i] = blasContactPt[i] - blasPosA[i];
- }
- char transA = 'N'; char transB = 'N'; int blasM = 3; int blasN = 3; int blasK = 3;
- int blasLDA = 3; int blasLDB = 3; double blasAlpha = 1.0; double blasBeta = 0.0;
- int blasLDC = 3; int incx=1; int incy=1;
+ /* Variables to keep things neat */
+ double kAs = sqrt(kA)/RA;
+ double kAp = sqrt(1.0 - kA)/rA;
+ double kBs = sqrt(kB)/RB;
+ double kBp = sqrt(1.0 - kB)/rB;
+
+ /* penalty */
+ double t = 1.0;
+ double mu=10.0;
+ double planePert = 0.1*rA;
+ double sPert = 1.0; //+ 10.0*planePert*(planeNoA+planeNoB)/(rA*rA);
+ if (warmstart == true) {
+ t = 0.01;
+ mu= 50.0;
+ planePert = 0.01;
+ sPert = 0.01; ///* pow(10,-1)+ */ sqrt(planePert*planePert*(planeNoA+planeNoB)/(rA*rA));
+ }
+ /* s */
+ double s = 0.0;
+ double m=2.0;
+ double NTTOL = pow(10,-8);
+ double tol = accuracyTol/*pow(10,-4)* RA*pow(10,-6)*/;
+ double gap =0.0;
+ /* x */
+ double blasX[varNo];
+ double blasNewX[varNo];
+ blasX[0] = contactPt[0];
+ blasX[1] = contactPt[1];
+ blasX[2] = contactPt[2];
+
+ //////////////////// evaluate fA and fB to initialize ////////////////////////////////
+ /* fA */
+ double blasTempP1[3];
+ double blasLocalP1[3];
+ for(int i=0; i<3; i++) {
+ blasTempP1[i] = blasContactPt[i] - blasPosA[i];
+ }
+ char transA = 'N';
+ //char transB = 'N';
+ //int blasM = 3;
+ //int blasN = 3;
+ int blasK = 3;
+ //int blasLDA = 3;
+ int blasLDB = 3;
+ double blasAlpha = 1.0;
+ double blasBeta = 0.0;
+ //int blasLDC = 3;
+ int incx=1;
+ int incy=1;
//dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasQA[0], &blasLDA, &blasTempP1[0], &incx, &blasBeta, &blasLocalP1[0], &incy);
- dgemv_(&blasNT, &blas3, &blas3, &blas1, &blasQA[0], &blas3, &blasTempP1[0], &incx, &blas0, &blasLocalP1[0], &incy);
-
- //Vector3r tempP1 = contactPt - posA;
- //Vector3r localP1 = QA*tempP1;
- /* P1Q */ //Eigen::MatrixXd P1 = Eigen::MatrixXd::Zero(planeNoA,3);
- /*d1*/ //Eigen::MatrixXd d1 = Eigen::MatrixXd::Zero(planeNoA,1);
-
- double blasP1[planeNoA*3];
- double blasD1[planeNoA];
- double blasP1Q[planeNoA*3];
- double pertSumA2 = 0.0;
-
- for (int i=0; i<planeNoA; i++){
- double plane = s1->a[i]*blasLocalP1[0] + s1->b[i]*blasLocalP1[1] + s1->c[i]*blasLocalP1[2] - s1->d[i];
- if (plane<pow(10,-15)){
- plane = 0.0;
- blasX[4+i] = plane + planePert;
- }else{
- blasX[4+i] = plane + planePert;
- }
- pertSumA2 += pow((plane+planePert),2);
- blasP1[i] = s1->a[i]; blasP1[i+planeNoA] = s1->b[i]; blasP1[i+2*planeNoA] = s1->c[i];
- blasD1[i] = s1->d[i];
- }
- //Eigen::MatrixXd P1Q = P1*QA;
- //transA = 'N'; transB = 'N'; blasM = planeNoA; blasN = 3; blasK = 3;
- // blasLDA = std::max(1,blasM); blasLDC = std::max(1,blasM); blasLDB = 3; blasAlpha=1.0; blasBeta = 0.0;
- //dgemm_(&transA, &transB, &blasM, &blasN, &blasK, &blasAlpha, &blasP1[0], &blasLDA, &blasQA[0], &blasLDB, &blasBeta, &blasP1Q[0], &blasLDC);
- dgemm_(&blasNT, &blasNT, &planeNoA, &blas3, &blas3, &blas1, &blasP1[0], &blas1planeNoA, &blasQA[0], &blasLDB, &blas0, &blasP1Q[0], &blas1planeNoA);
-
- Real sphereA = (pow(blasLocalP1[0],2) + pow(blasLocalP1[1],2) + pow(blasLocalP1[2],2))/pow(RA,2);
- Real sA = (1.0-kA)*(pertSumA2/pow(rA,2) - 1.0)+kA*(sphereA -1.0);
-
- /* fB */
- double blasTempP2[3]; double blasLocalP2[3];
- for(int i=0; i<3; i++){
- blasTempP2[i] = blasContactPt[i] - blasPosB[i];
- }
- //transA = 'N'; blasM = 3; blasN = 3; blasLDA = 3; blasAlpha=1.0; blasBeta = 0.0;
- //dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasQB[0], &blasLDA, &blasTempP2[0], &incx, &blasBeta, &blasLocalP2[0], &incy);
- dgemv_(&blasNT, &blas3, &blas3, &blas1, &blasQB[0], &blas3, &blasTempP2[0], &incx, &blas0, &blasLocalP2[0], &incy);
-
- // Vector3r tempP2 = contactPt - posB;
- // Vector3r localP2 = QB*tempP2;
- /*P2Q*/ //Eigen::MatrixXd P2 = Eigen::MatrixXd::Zero(planeNoB,3);
- /*d2*/ //Eigen::MatrixXd d2 = Eigen::MatrixXd::Zero(planeNoB,1);
- double blasP2[planeNoB*3];
- double blasD2[planeNoB];
- double blasP2Q[planeNoB*3]; double pertSumB2 = 0.0;
- for (int i=0; i<planeNoB; i++){
- double plane = s2->a[i]*blasLocalP2[0] + s2->b[i]*blasLocalP2[1] + s2->c[i]*blasLocalP2[2] - s2->d[i];
- if (plane<pow(10,-15)){
- plane = 0.0;
- blasX[4+planeNoA+i] = plane+ planePert;
- }else{
- blasX[4+planeNoA+i] = plane + planePert;
- }
- pertSumB2 += pow((plane+planePert),2);
- blasP2[i] = s2->a[i]; blasP2[i+planeNoB] = s2->b[i]; blasP2[i+2*planeNoB] = s2->c[i];
- blasD2[i] = s2->d[i];
- }
- //Eigen::MatrixXd P2Q = P2*QB;
- //transA = 'N'; transB = 'N'; blasM = planeNoB; blasN = 3; blasK = 3;
- //blasLDA = std::max(1,blasM); blasLDC = std::max(1,blasM); blasLDB = 3; blasAlpha=1.0; blasBeta = 0.0;
- // dgemm_(&transA, &transB, &blasM, &blasN, &blasK, &blasAlpha, &blasP2[0], &blasLDA, &blasQB[0], &blasLDB, &blasBeta, &blasP2Q[0], &blasLDC);
- dgemm_(&blasNT, &blasNT, &planeNoB, &blas3, &blas3, &blasAlpha, &blasP2[0], &blas1planeNoB, &blasQB[0], &blas3, &blasBeta, &blasP2Q[0], &blas1planeNoB);
- Real sphereB = (pow(blasLocalP2[0],2) + pow(blasLocalP2[1],2) + pow(blasLocalP2[2],2))/pow(RB,2);
- Real sB = (1.0-kB)*(pertSumB2/pow(rB,2) - 1.0)+kB*(sphereB -1.0);
- //sPert = fabs(fA-fB);
-
- s = std::max(sqrt(fabs(sA+1.0)), sqrt(fabs(sB+1.0))) + sPert; //sqrt(fA+fB+2.0)+sPert; //sqrt(std::max(fabs(fA+1.0), fabs(fB+1.0))) + sPert; //
- //x[3] = s;
- blasX[3]= s;
-
- /////////////////// algebra formulation of the SOCP ///////////////////
- /* c */
- // Eigen::MatrixXd c=Eigen::MatrixXd::Zero(varNo,1);
- // c[3] = 1.0;
-
- double blasA1[(3+planeNoA)*varNo]; double blasA2[(3+planeNoB)*varNo];
- /* Second order cone constraints */
- /* A1 */
- //Eigen::MatrixXd A1(3+planeNoA,varNo);
- //Matrix3r QAs=kAs*QA; //cwise()
- double blasQAs[9]; int noElements=9; double scaleFactor = kAs;
- dcopy_(&noElements, &blasQA[0], &incx, &blasQAs[0], &incx);
- dscal_(&noElements, &scaleFactor, &blasQAs[0], &incx);
-
- // A1 << QAs,Eigen::MatrixXd::Zero(3,1+planeNoAB),
- // Eigen::MatrixXd::Zero(planeNoA,4),kAp*Eigen::MatrixXd::Identity(planeNoA, planeNoA),Eigen::MatrixXd::Zero(planeNoA,planeNoB);
- memset(blasA1,0.0,sizeof(blasA1));
- for (int i=0; i<3; i++){
- blasA1[i] = blasQAs[i]; blasA1[i+planeNoA3] = blasQAs[i+3]; blasA1[i+2*planeNoA3] = blasQAs[i+6];
- }
- for (int i=0; i<planeNoA; i++){
- blasA1[3+i+(planeNoA3)*(4+i)] = kAp;
- }
-
-
- /* A2 */
- //Eigen::MatrixXd A2(3+planeNoB,varNo);
- //Matrix3r QBs=kBs*QB; //cwise();
- double blasQBs[9]; noElements=9; scaleFactor = kBs;
- dcopy_(&noElements, &blasQB[0], &incx, &blasQBs[0], &incx);
- dscal_(&noElements, &scaleFactor, &blasQBs[0], &incx);
-
- // A2 << QBs,Eigen::MatrixXd::Zero(3,1+planeNoAB),
- // Eigen::MatrixXd::Zero(planeNoB,4),Eigen::MatrixXd::Zero(planeNoB,planeNoA),kBp*Eigen::MatrixXd::Identity(planeNoB, planeNoB);
- memset(blasA2,0.0,sizeof(blasA2));
- for (int i=0; i<3; i++){
- blasA2[i] = blasQBs[i]; blasA2[i+planeNoB3] = blasQBs[i+3]; blasA2[i+2*(planeNoB3)] = blasQBs[i+6];
- }
-
- for (int i=0; i<planeNoB; i++){
- blasA2[3+i+(planeNoB3)*(4+planeNoA+i)] = kBp;
- }
-
-
-
- /*b1*/
+ dgemv_(&blasNT, &blas3, &blas3, &blas1, &blasQA[0], &blas3, &blasTempP1[0], &incx, &blas0, &blasLocalP1[0], &incy);
+
+ //Vector3r tempP1 = contactPt - posA;
+ //Vector3r localP1 = QA*tempP1;
+ /* P1Q */ //Eigen::MatrixXd P1 = Eigen::MatrixXd::Zero(planeNoA,3);
+ /*d1*/ //Eigen::MatrixXd d1 = Eigen::MatrixXd::Zero(planeNoA,1);
+
+ double blasP1[planeNoA*3];
+ double blasD1[planeNoA];
+ double blasP1Q[planeNoA*3];
+ double pertSumA2 = 0.0;
+
+ for (int i=0; i<planeNoA; i++) {
+ double plane = s1->a[i]*blasLocalP1[0] + s1->b[i]*blasLocalP1[1] + s1->c[i]*blasLocalP1[2] - s1->d[i];
+ if (plane<pow(10,-15)) {
+ plane = 0.0;
+ blasX[4+i] = plane + planePert;
+ } else {
+ blasX[4+i] = plane + planePert;
+ }
+ pertSumA2 += pow((plane+planePert),2);
+ blasP1[i] = s1->a[i];
+ blasP1[i+planeNoA] = s1->b[i];
+ blasP1[i+2*planeNoA] = s1->c[i];
+ blasD1[i] = s1->d[i];
+ }
+ //Eigen::MatrixXd P1Q = P1*QA;
+ //transA = 'N'; transB = 'N'; blasM = planeNoA; blasN = 3; blasK = 3;
+// blasLDA = std::max(1,blasM); blasLDC = std::max(1,blasM); blasLDB = 3; blasAlpha=1.0; blasBeta = 0.0;
+ //dgemm_(&transA, &transB, &blasM, &blasN, &blasK, &blasAlpha, &blasP1[0], &blasLDA, &blasQA[0], &blasLDB, &blasBeta, &blasP1Q[0], &blasLDC);
+ dgemm_(&blasNT, &blasNT, &planeNoA, &blas3, &blas3, &blas1, &blasP1[0], &blas1planeNoA, &blasQA[0], &blasLDB, &blas0, &blasP1Q[0], &blas1planeNoA);
+
+ Real sphereA = (pow(blasLocalP1[0],2) + pow(blasLocalP1[1],2) + pow(blasLocalP1[2],2))/pow(RA,2);
+ Real sA = (1.0-kA)*(pertSumA2/pow(rA,2) - 1.0)+kA*(sphereA -1.0);
+
+ /* fB */
+ double blasTempP2[3];
+ double blasLocalP2[3];
+ for(int i=0; i<3; i++) {
+ blasTempP2[i] = blasContactPt[i] - blasPosB[i];
+ }
+ //transA = 'N'; blasM = 3; blasN = 3; blasLDA = 3; blasAlpha=1.0; blasBeta = 0.0;
+ //dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasQB[0], &blasLDA, &blasTempP2[0], &incx, &blasBeta, &blasLocalP2[0], &incy);
+ dgemv_(&blasNT, &blas3, &blas3, &blas1, &blasQB[0], &blas3, &blasTempP2[0], &incx, &blas0, &blasLocalP2[0], &incy);
+
+ // Vector3r tempP2 = contactPt - posB;
+ // Vector3r localP2 = QB*tempP2;
+ /*P2Q*/ //Eigen::MatrixXd P2 = Eigen::MatrixXd::Zero(planeNoB,3);
+ /*d2*/ //Eigen::MatrixXd d2 = Eigen::MatrixXd::Zero(planeNoB,1);
+ double blasP2[planeNoB*3];
+ double blasD2[planeNoB];
+ double blasP2Q[planeNoB*3];
+ double pertSumB2 = 0.0;
+ for (int i=0; i<planeNoB; i++) {
+ double plane = s2->a[i]*blasLocalP2[0] + s2->b[i]*blasLocalP2[1] + s2->c[i]*blasLocalP2[2] - s2->d[i];
+ if (plane<pow(10,-15)) {
+ plane = 0.0;
+ blasX[4+planeNoA+i] = plane+ planePert;
+ } else {
+ blasX[4+planeNoA+i] = plane + planePert;
+ }
+ pertSumB2 += pow((plane+planePert),2);
+ blasP2[i] = s2->a[i];
+ blasP2[i+planeNoB] = s2->b[i];
+ blasP2[i+2*planeNoB] = s2->c[i];
+ blasD2[i] = s2->d[i];
+ }
+//Eigen::MatrixXd P2Q = P2*QB;
+ //transA = 'N'; transB = 'N'; blasM = planeNoB; blasN = 3; blasK = 3;
+ //blasLDA = std::max(1,blasM); blasLDC = std::max(1,blasM); blasLDB = 3; blasAlpha=1.0; blasBeta = 0.0;
+// dgemm_(&transA, &transB, &blasM, &blasN, &blasK, &blasAlpha, &blasP2[0], &blasLDA, &blasQB[0], &blasLDB, &blasBeta, &blasP2Q[0], &blasLDC);
+ dgemm_(&blasNT, &blasNT, &planeNoB, &blas3, &blas3, &blasAlpha, &blasP2[0], &blas1planeNoB, &blasQB[0], &blas3, &blasBeta, &blasP2Q[0], &blas1planeNoB);
+ Real sphereB = (pow(blasLocalP2[0],2) + pow(blasLocalP2[1],2) + pow(blasLocalP2[2],2))/pow(RB,2);
+ Real sB = (1.0-kB)*(pertSumB2/pow(rB,2) - 1.0)+kB*(sphereB -1.0);
+ //sPert = fabs(fA-fB);
+
+ s = std::max(sqrt(fabs(sA+1.0)), sqrt(fabs(sB+1.0))) + sPert; //sqrt(fA+fB+2.0)+sPert; //sqrt(std::max(fabs(fA+1.0), fabs(fB+1.0))) + sPert; //
+ //x[3] = s;
+ blasX[3]= s;
+
+ /////////////////// algebra formulation of the SOCP ///////////////////
+ /* c */
+// Eigen::MatrixXd c=Eigen::MatrixXd::Zero(varNo,1);
+// c[3] = 1.0;
+
+ double blasA1[(3+planeNoA)*varNo];
+ double blasA2[(3+planeNoB)*varNo];
+ /* Second order cone constraints */
+ /* A1 */
+ //Eigen::MatrixXd A1(3+planeNoA,varNo);
+ //Matrix3r QAs=kAs*QA; //cwise()
+ double blasQAs[9];
+ int noElements=9;
+ double scaleFactor = kAs;
+ dcopy_(&noElements, &blasQA[0], &incx, &blasQAs[0], &incx);
+ dscal_(&noElements, &scaleFactor, &blasQAs[0], &incx);
+
+ // A1 << QAs,Eigen::MatrixXd::Zero(3,1+planeNoAB),
+ // Eigen::MatrixXd::Zero(planeNoA,4),kAp*Eigen::MatrixXd::Identity(planeNoA, planeNoA),Eigen::MatrixXd::Zero(planeNoA,planeNoB);
+ memset(blasA1,0.0,sizeof(blasA1));
+ for (int i=0; i<3; i++) {
+ blasA1[i] = blasQAs[i];
+ blasA1[i+planeNoA3] = blasQAs[i+3];
+ blasA1[i+2*planeNoA3] = blasQAs[i+6];
+ }
+ for (int i=0; i<planeNoA; i++) {
+ blasA1[3+i+(planeNoA3)*(4+i)] = kAp;
+ }
+
+
+ /* A2 */
+ //Eigen::MatrixXd A2(3+planeNoB,varNo);
+ //Matrix3r QBs=kBs*QB; //cwise();
+ double blasQBs[9];
+ noElements=9;
+ scaleFactor = kBs;
+ dcopy_(&noElements, &blasQB[0], &incx, &blasQBs[0], &incx);
+ dscal_(&noElements, &scaleFactor, &blasQBs[0], &incx);
+
+ // A2 << QBs,Eigen::MatrixXd::Zero(3,1+planeNoAB),
+ // Eigen::MatrixXd::Zero(planeNoB,4),Eigen::MatrixXd::Zero(planeNoB,planeNoA),kBp*Eigen::MatrixXd::Identity(planeNoB, planeNoB);
+ memset(blasA2,0.0,sizeof(blasA2));
+ for (int i=0; i<3; i++) {
+ blasA2[i] = blasQBs[i];
+ blasA2[i+planeNoB3] = blasQBs[i+3];
+ blasA2[i+2*(planeNoB3)] = blasQBs[i+6];
+ }
+
+ for (int i=0; i<planeNoB; i++) {
+ blasA2[3+i+(planeNoB3)*(4+planeNoA+i)] = kBp;
+ }
+
+
+
+ /*b1*/
#if 0
- Eigen::MatrixXd b1=Eigen::MatrixXd::Zero(3+planeNoA,1);
- Eigen::Vector3d b1temp = QAs*posA;
- b1[0] = -b1temp[0]; b1[1]= -b1temp[1]; b1[2] = -b1temp[2];
+ Eigen::MatrixXd b1=Eigen::MatrixXd::Zero(3+planeNoA,1);
+ Eigen::Vector3d b1temp = QAs*posA;
+ b1[0] = -b1temp[0];
+ b1[1]= -b1temp[1];
+ b1[2] = -b1temp[2];
#endif
- double blasB1[planeNoA3]; double blasB1temp[3];
- memset(blasB1,0.0,sizeof(blasB1));
- // blasM = 3; blasN=3;
-// blasLDA = 3; blasAlpha = -1.0; blasBeta=0.0; blasLDC = 3;
+ double blasB1[planeNoA3];
+ double blasB1temp[3];
+ memset(blasB1,0.0,sizeof(blasB1));
+// blasM = 3; blasN=3;
+// blasLDA = 3; blasAlpha = -1.0; blasBeta=0.0; blasLDC = 3;
// dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasQAs[0], &blasLDA, &blasPosA[0], &incx, &blasBeta, &blasB1temp[0], &incy);
- dgemv_(&blasNT, &blas3, &blas3, &blasNeg1, &blasQAs[0], &blas3, &blasPosA[0], &incx, &blas0, &blasB1temp[0], &incy);
- blasB1[0]=blasB1temp[0]; blasB1[1]=blasB1temp[1]; blasB1[2]=blasB1temp[2];
+ dgemv_(&blasNT, &blas3, &blas3, &blasNeg1, &blasQAs[0], &blas3, &blasPosA[0], &incx, &blas0, &blasB1temp[0], &incy);
+ blasB1[0]=blasB1temp[0];
+ blasB1[1]=blasB1temp[1];
+ blasB1[2]=blasB1temp[2];
- /*b2*/
+ /*b2*/
#if 0
- Eigen::MatrixXd b2=Eigen::MatrixXd::Zero(3+planeNoB,1);
- Eigen::Vector3d b2temp = QBs*posB;
- b2[0] = -b2temp[0]; b2[1]= -b2temp[1]; b2[2] = -b2temp[2];
+ Eigen::MatrixXd b2=Eigen::MatrixXd::Zero(3+planeNoB,1);
+ Eigen::Vector3d b2temp = QBs*posB;
+ b2[0] = -b2temp[0];
+ b2[1]= -b2temp[1];
+ b2[2] = -b2temp[2];
#endif
- double blasB2[planeNoB3]; double blasB2temp[3];
- memset(blasB2,0.0,sizeof(blasB2));
- // blasM = 3; blasN=3; blasLDA = 3; blasAlpha=-1.0; blasBeta=0.0;
- // dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasQBs[0], &blasLDA, &blasPosB[0], &incx, &blasBeta, &blasB2temp[0], &incy);
- dgemv_(&blasNT, &blas3, &blas3, &blasNeg1, &blasQBs[0], &blas3, &blasPosB[0], &incx, &blas0, &blasB2temp[0], &incy);
- blasB2[0]=blasB2temp[0]; blasB2[1]=blasB2temp[1]; blasB2[2]=blasB2temp[2];
+ double blasB2[planeNoB3];
+ double blasB2temp[3];
+ memset(blasB2,0.0,sizeof(blasB2));
+// blasM = 3; blasN=3; blasLDA = 3; blasAlpha=-1.0; blasBeta=0.0;
+// dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasQBs[0], &blasLDA, &blasPosB[0], &incx, &blasBeta, &blasB2temp[0], &incy);
+ dgemv_(&blasNT, &blas3, &blas3, &blasNeg1, &blasQBs[0], &blas3, &blasPosB[0], &incx, &blas0, &blasB2temp[0], &incy);
+ blasB2[0]=blasB2temp[0];
+ blasB2[1]=blasB2temp[1];
+ blasB2[2]=blasB2temp[2];
- /*AL*/
- // Eigen::MatrixXd AL(planeNoAB,varNo);
- // AL<<P1Q, Eigen::MatrixXd::Zero(planeNoA,1), -1.0*Eigen::MatrixXd::Identity(planeNoA,planeNoA), Eigen::MatrixXd::Zero(planeNoA,planeNoB), //cwise()
+ /*AL*/
+// Eigen::MatrixXd AL(planeNoAB,varNo);
+// AL<<P1Q, Eigen::MatrixXd::Zero(planeNoA,1), -1.0*Eigen::MatrixXd::Identity(planeNoA,planeNoA), Eigen::MatrixXd::Zero(planeNoA,planeNoB), //cwise()
// P2Q, Eigen::MatrixXd::Zero(planeNoB,1), Eigen::MatrixXd::Zero(planeNoB,planeNoA), -1.0*Eigen::MatrixXd::Identity(planeNoB,planeNoB);
- double blasAL[planeNoAB*varNo];
- memset(blasAL,0.0,sizeof(blasAL));
- for (int i=0; i<planeNoA; i++){
- blasAL[i] = blasP1Q[i]; blasAL[i+planeNoAB] = blasP1Q[i+planeNoA]; blasAL[i+2*planeNoAB] = blasP1Q[i+2*planeNoA];
- blasAL[i+(4+i)*planeNoAB] = -1.0;
- }
- for (int i=0; i<planeNoB; i++){
- blasAL[planeNoA+i] = blasP2Q[i]; blasAL[planeNoA+i+planeNoAB] = blasP2Q[i+planeNoB]; blasAL[planeNoA+i+2*planeNoAB] = blasP2Q[i+2*planeNoB];
- blasAL[planeNoA+i+(4+planeNoA+i)*planeNoAB] = -1.0;
- }
-
-
-
- /*bL*/
+ double blasAL[planeNoAB*varNo];
+ memset(blasAL,0.0,sizeof(blasAL));
+ for (int i=0; i<planeNoA; i++) {
+ blasAL[i] = blasP1Q[i];
+ blasAL[i+planeNoAB] = blasP1Q[i+planeNoA];
+ blasAL[i+2*planeNoAB] = blasP1Q[i+2*planeNoA];
+ blasAL[i+(4+i)*planeNoAB] = -1.0;
+ }
+ for (int i=0; i<planeNoB; i++) {
+ blasAL[planeNoA+i] = blasP2Q[i];
+ blasAL[planeNoA+i+planeNoAB] = blasP2Q[i+planeNoB];
+ blasAL[planeNoA+i+2*planeNoAB] = blasP2Q[i+2*planeNoB];
+ blasAL[planeNoA+i+(4+planeNoA+i)*planeNoAB] = -1.0;
+ }
+
+
+
+ /*bL*/
#if 0
- Eigen::MatrixXd bL(planeNoAB,1);
- Eigen::MatrixXd pos1(3,1); pos1(0,0) = posA[0]; pos1(1,0) = posA[1]; pos1(2,0)=posA[2];
- Eigen::MatrixXd pos2(3,1); pos2(0,0) = posB[0]; pos2(1,0) = posB[1]; pos2(2,0)=posB[2];
- Eigen::MatrixXd btempU = P1Q*pos1 + d1;
- Eigen::MatrixXd btempL = P2Q*pos2 + d2;
- bL<<btempU,btempL;
+ Eigen::MatrixXd bL(planeNoAB,1);
+ Eigen::MatrixXd pos1(3,1);
+ pos1(0,0) = posA[0];
+ pos1(1,0) = posA[1];
+ pos1(2,0)=posA[2];
+ Eigen::MatrixXd pos2(3,1);
+ pos2(0,0) = posB[0];
+ pos2(1,0) = posB[1];
+ pos2(2,0)=posB[2];
+ Eigen::MatrixXd btempU = P1Q*pos1 + d1;
+ Eigen::MatrixXd btempL = P2Q*pos2 + d2;
+ bL<<btempU,btempL;
#endif
- double blasBL[planeNoAB]; double blasBTempU[planeNoA]; double blasBTempL[planeNoB];
- noElements = planeNoA;
- dcopy_(&noElements, &blasD1[0], &incx, &blasBTempU[0], &incx);
- //transA = 'N'; blasM = planeNoA; blasN = 3;
- //blasLDA = std::max(1,planeNoA); blasAlpha = 1.0; blasBeta = 1.0;
- //dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasP1Q[0], &blasLDA,&blasPosA[0], &incx, &blasBeta, &blasBTempU[0], &incy);
- dgemv_(&blasNT, &planeNoA, &blas3, &blas1, &blasP1Q[0], &blas1planeNoA,&blasPosA[0], &incx, &blas1, &blasBTempU[0], &incy);
- noElements = planeNoB;
- dcopy_(&noElements, &blasD2[0], &incx, &blasBTempL[0], &incx);
- //transA = 'N'; blasM = planeNoB; blasN = 3;
- //blasLDA = std::max(1,planeNoB); blasAlpha = 1.0; blasBeta = 1.0;
- //dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasP2Q[0], &blasLDA,&blasPosB[0], &incx, &blasBeta, &blasBTempL[0], &incy);
- dgemv_(&blasNT, &planeNoB, &blas3, &blas1, &blasP2Q[0], &blas1planeNoB,&blasPosB[0], &incx, &blas1, &blasBTempL[0], &incy);
- for (int i=0; i<planeNoA; i++){
- blasBL[i]=blasBTempU[i];
- }
- for (int i=0; i<planeNoB; i++){
- blasBL[planeNoA+i]=blasBTempL[i];
- }
-
- double u1; double u2;
- double blasCCtranspose[varNo2];
- memset(blasCCtranspose,0.0,sizeof(blasCCtranspose));
- blasCCtranspose[3+varNo*3]=1.0;
-
- double blasCa1[varNo2];
+ double blasBL[planeNoAB];
+ double blasBTempU[planeNoA];
+ double blasBTempL[planeNoB];
+ noElements = planeNoA;
+ dcopy_(&noElements, &blasD1[0], &incx, &blasBTempU[0], &incx);
+ //transA = 'N'; blasM = planeNoA; blasN = 3;
+ //blasLDA = std::max(1,planeNoA); blasAlpha = 1.0; blasBeta = 1.0;
+ //dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasP1Q[0], &blasLDA,&blasPosA[0], &incx, &blasBeta, &blasBTempU[0], &incy);
+ dgemv_(&blasNT, &planeNoA, &blas3, &blas1, &blasP1Q[0], &blas1planeNoA,&blasPosA[0], &incx, &blas1, &blasBTempU[0], &incy);
+ noElements = planeNoB;
+ dcopy_(&noElements, &blasD2[0], &incx, &blasBTempL[0], &incx);
+ //transA = 'N'; blasM = planeNoB; blasN = 3;
+ //blasLDA = std::max(1,planeNoB); blasAlpha = 1.0; blasBeta = 1.0;
+ //dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasP2Q[0], &blasLDA,&blasPosB[0], &incx, &blasBeta, &blasBTempL[0], &incy);
+ dgemv_(&blasNT, &planeNoB, &blas3, &blas1, &blasP2Q[0], &blas1planeNoB,&blasPosB[0], &incx, &blas1, &blasBTempL[0], &incy);
+ for (int i=0; i<planeNoA; i++) {
+ blasBL[i]=blasBTempU[i];
+ }
+ for (int i=0; i<planeNoB; i++) {
+ blasBL[planeNoA+i]=blasBTempL[i];
+ }
+
+ double u1;
+ double u2;
+ double blasCCtranspose[varNo2];
+ memset(blasCCtranspose,0.0,sizeof(blasCCtranspose));
+ blasCCtranspose[3+varNo*3]=1.0;
+
+ double blasCa1[varNo2];
//#if 0
- #if 0
- Eigen::MatrixXd ca1Transpose = ccTranspose - A1.transpose()*A1;
- Eigen::MatrixXd ca2Transpose = ccTranspose - A2.transpose()*A2;
- #endif
+#if 0
+ Eigen::MatrixXd ca1Transpose = ccTranspose - A1.transpose()*A1;
+ Eigen::MatrixXd ca2Transpose = ccTranspose - A2.transpose()*A2;
+#endif
/* ca1Transpose */
- noElements = varNo2; incx =1; incy=1;
- dcopy_(&noElements, &blasCCtranspose[0], &incx, &blasCa1[0], &incy);
+ noElements = varNo2;
+ incx =1;
+ incy=1;
+ dcopy_(&noElements, &blasCCtranspose[0], &incx, &blasCa1[0], &incy);
- //transA = 'T'; transB = 'N'; blasM = varNo; blasN = varNo; blasK = 3+planeNoA;
- //blasLDA = blasK; blasLDB = blasK; blasAlpha = -1.0; blasBeta = 1.0;
- //blasLDC = varNo;
- //dgemm_(&transA, &transB, &blasM, &blasN, &blasK, &blasAlpha, &blasA1[0], &blasLDA, &blasA1[0], &blasLDB, &blasBeta, &blasCa1[0], &blasLDC);
- dgemm_(&blasT, &blasNT, &varNo, &varNo, &planeNoA3, &blasNeg1, &blasA1[0], &planeNoA3, &blasA1[0], &planeNoA3, &blas1, &blasCa1[0], &varNo);
+ //transA = 'T'; transB = 'N'; blasM = varNo; blasN = varNo; blasK = 3+planeNoA;
+ //blasLDA = blasK; blasLDB = blasK; blasAlpha = -1.0; blasBeta = 1.0;
+ //blasLDC = varNo;
+ //dgemm_(&transA, &transB, &blasM, &blasN, &blasK, &blasAlpha, &blasA1[0], &blasLDA, &blasA1[0], &blasLDB, &blasBeta, &blasCa1[0], &blasLDC);
+ dgemm_(&blasT, &blasNT, &varNo, &varNo, &planeNoA3, &blasNeg1, &blasA1[0], &planeNoA3, &blasA1[0], &planeNoA3, &blas1, &blasCa1[0], &varNo);
/* ca2Transpose */
- double blasCa2[varNo2]; blasCount = 0;
- dcopy_(&noElements, &blasCCtranspose[0], &incx, &blasCa2[0], &incy);
-
- //transA = 'T'; transB = 'N'; blasM = varNo; blasN = varNo; blasK = 3+planeNoB;
- // blasLDA = blasK; blasLDB = blasK; blasAlpha = -1.0; blasBeta = 1.0;
- // blasLDC = varNo;
- // dgemm_(&transA, &transB, &blasM, &blasN, &blasK, &blasAlpha, &blasA2[0], &blasLDA, &blasA2[0], &blasLDB, &blasBeta, &blasCa2[0], &blasLDC);
- dgemm_(&blasT, &blasNT, &varNo, &varNo, &planeNoB3, &blasNeg1, &blasA2[0], &planeNoB3, &blasA2[0], &planeNoB3, &blas1, &blasCa2[0], &varNo);
-
-/* DL */
-double blasDL[planeNoAB*planeNoAB]; blasCount = 0;
-memset(blasDL,0.0,sizeof(blasDL));
+ double blasCa2[varNo2];
+ blasCount = 0;
+ dcopy_(&noElements, &blasCCtranspose[0], &incx, &blasCa2[0], &incy);
+
+ //transA = 'T'; transB = 'N'; blasM = varNo; blasN = varNo; blasK = 3+planeNoB;
+ // blasLDA = blasK; blasLDB = blasK; blasAlpha = -1.0; blasBeta = 1.0;
+ // blasLDC = varNo;
+ // dgemm_(&transA, &transB, &blasM, &blasN, &blasK, &blasAlpha, &blasA2[0], &blasLDA, &blasA2[0], &blasLDB, &blasBeta, &blasCa2[0], &blasLDC);
+ dgemm_(&blasT, &blasNT, &varNo, &varNo, &planeNoB3, &blasNeg1, &blasA2[0], &planeNoB3, &blasA2[0], &planeNoB3, &blas1, &blasCa2[0], &varNo);
+
+ /* DL */
+ double blasDL[planeNoAB*planeNoAB];
+ blasCount = 0;
+ memset(blasDL,0.0,sizeof(blasDL));
//#endif
- double wLlogsum = 0.0;
- double val = 0.0;
- double newval = 0.0;
- /*Linesearch */
- double backtrack = 1.0;
- double penalty = 1.0/t;
-
-/* LAPACK */
- int info; char UPLO ='L'; int KD = varNo-1; int nrhs=1;
- double gradient[varNo];
- double HessianChol[varNo][varNo];
-
-
- double blasGA[varNo]; double blasGB[varNo]; double blasGL[varNo];
-
- double blasW1[3+planeNoA]; double blasW2[3+planeNoB]; double blasWL[planeNoAB]; double blasVL[planeNoAB]; double minWL=0.0;
- double blasHA[varNo*varNo]; double blasHB[varNo*varNo]; double blasHL[varNo*varNo]; double blasADAtemp[varNo*planeNoAB];
- noElements = varNo;
- double blasW1dot=0.0; double blasW2dot=0.0;
- double blasGrad[varNo]; double blasHess[varNo*varNo]; double blasStep[varNo]; double blasFprime = 0.0;
-
+ double wLlogsum = 0.0;
+ double val = 0.0;
+ double newval = 0.0;
+ /*Linesearch */
+ double backtrack = 1.0;
+ double penalty = 1.0/t;
+
+ /* LAPACK */
+ int info;
+ char UPLO ='L';
+ int KD = varNo-1;
+ int nrhs=1;
+ double HessianChol[varNo][varNo];
+
+
+ double blasGA[varNo];
+ double blasGB[varNo];
+ double blasGL[varNo];
+
+ double blasW1[3+planeNoA];
+ double blasW2[3+planeNoB];
+ double blasWL[planeNoAB];
+ double blasVL[planeNoAB];
+ double minWL=0.0;
+ double blasHA[varNo*varNo];
+ double blasHB[varNo*varNo];
+ double blasHL[varNo*varNo];
+ double blasADAtemp[varNo*planeNoAB];
+ noElements = varNo;
+ double blasW1dot=0.0;
+ double blasW2dot=0.0;
+ double blasGrad[varNo];
+ double blasHess[varNo*varNo];
+ double blasStep[varNo];
+ double blasFprime = 0.0;
+
#if 0
// MAKE SURE POINTS ARE FEASIBLE //
- /* temp variables */
- /* w1 = A1*x + b1; */
- noElements = 3+planeNoA;
- dcopy_(&noElements, &blasB1[0], &incx, &blasW1[0], &incy);
- //transA = 'N'; blasM = 3+planeNoA; blasN = varNo;
+ /* temp variables */
+ /* w1 = A1*x + b1; */
+ noElements = 3+planeNoA;
+ dcopy_(&noElements, &blasB1[0], &incx, &blasW1[0], &incy);
+ //transA = 'N'; blasM = 3+planeNoA; blasN = varNo;
//blasLDA = 3+planeNoA; blasAlpha = 1.0; blasBeta = 1.0; incx =1; incy=1;
//dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasA1[0], &blasLDA, &blasX[0], &incx, &blasBeta, &blasW1[0], &incy);
dgemv_(&blasNT, &planeNoA3, &varNo, &blas1, &blasA1[0], &planeNoA3, &blasX[0], &incx, &blas1, &blasW1[0], &incy);
- /* w2 = A2*x + b2; */
- noElements = 3+planeNoB;
- dcopy_(&noElements, &blasB2[0], &incx, &blasW2[0], &incy);
- //transA = 'N'; blasM = 3+planeNoB; blasN = varNo;
- //blasLDA = 3+planeNoB; blasAlpha = 1.0; blasBeta = 1.0;
+ /* w2 = A2*x + b2; */
+ noElements = 3+planeNoB;
+ dcopy_(&noElements, &blasB2[0], &incx, &blasW2[0], &incy);
+ //transA = 'N'; blasM = 3+planeNoB; blasN = varNo;
+ //blasLDA = 3+planeNoB; blasAlpha = 1.0; blasBeta = 1.0;
//dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasA2[0], &blasLDA, &blasX[0], &incx, &blasBeta, &blasW2[0], &incy);
dgemv_(&blasNT, &planeNoB3, &varNo, &blas1, &blasA2[0], &planeNoB3, &blasX[0], &incx, &blas1, &blasW2[0], &incy);
- /* u1 = s*s - w1.dot(w1); */
- /* u2 = s*s - w2.dot(w2); */
- //noElements = 3+planeNoA;
- blasW1dot = ddot_(&planeNoA3, &blasW1[0], &incx, &blasW1[0], &incy);
- //noElements = 3+planeNoB;
- blasW2dot = ddot_(&planeNoB3, &blasW2[0], &incx, &blasW2[0], &incy);
+ /* u1 = s*s - w1.dot(w1); */
+ /* u2 = s*s - w2.dot(w2); */
+ //noElements = 3+planeNoA;
+ blasW1dot = ddot_(&planeNoA3, &blasW1[0], &incx, &blasW1[0], &incy);
+ //noElements = 3+planeNoB;
+ blasW2dot = ddot_(&planeNoB3, &blasW2[0], &incx, &blasW2[0], &incy);
-s = std::max(sqrt(fabs(blasW1dot)),sqrt(fabs(blasW2dot)))+0.1;
-blasX[3]=s;
+ s = std::max(sqrt(fabs(blasW1dot)),sqrt(fabs(blasW2dot)))+0.1;
+ blasX[3]=s;
#endif
//timingDeltas->checkpoint("setup");
-
-
-while(totalIter<500){
- penalty = 1.0/t;
- /* Newton's method */
- /* s=x[3]; */
- s=blasX[3];
-
- /* temp variables */
- /* w1 = A1*x + b1; */
- noElements = 3+planeNoA;
- dcopy_(&noElements, &blasB1[0], &incx, &blasW1[0], &incy);
- //transA = 'N'; blasM = 3+planeNoA; blasN = varNo;
- //blasLDA = 3+planeNoA; blasAlpha = 1.0; blasBeta = 1.0; incx =1; incy=1;
- //dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasA1[0], &blasLDA, &blasX[0], &incx, &blasBeta, &blasW1[0], &incy);
- dgemv_(&blasNT, &planeNoA3, &varNo, &blas1, &blasA1[0], &planeNoA3, &blasX[0], &incx, &blas1, &blasW1[0], &incy);
-
- /* w2 = A2*x + b2; */
- noElements = 3+planeNoB;
- dcopy_(&noElements, &blasB2[0], &incx, &blasW2[0], &incy);
- //transA = 'N'; blasM = 3+planeNoB; blasN = varNo;
- //blasLDA = 3+planeNoB; blasAlpha = 1.0; blasBeta = 1.0;
- //dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasA2[0], &blasLDA, &blasX[0], &incx, &blasBeta, &blasW2[0], &incy);
- dgemv_(&blasNT, &planeNoB3, &varNo, &blas1, &blasA2[0], &planeNoB3, &blasX[0], &incx, &blas1, &blasW2[0], &incy);
-
- /* u1 = s*s - w1.dot(w1); */
- /* u2 = s*s - w2.dot(w2); */
- //noElements = 3+planeNoA;
- blasW1dot = ddot_(&planeNoA3, &blasW1[0], &incx, &blasW1[0], &incy);
- //noElements = 3+planeNoB;
- blasW2dot = ddot_(&planeNoB3, &blasW2[0], &incx, &blasW2[0], &incy);
- u1 =s*s -blasW1dot;
- u2 =s*s -blasW2dot;
-
- /* wL = bL - AL*x; */
- noElements = planeNoAB;
- dcopy_(&noElements, &blasBL[0], &incx, &blasWL[0], &incy);
- //transA = 'N'; blasN = varNo;
- //blasM = planeNoAB; blasLDA = std::max(1,planeNoAB); blasAlpha = -1.0; blasBeta = 1.0;
- //dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasAL[0], &blasLDA, &blasX[0], &incx, &blasBeta, &blasWL[0], &incy);
- dgemv_(&blasNT, &planeNoAB, &varNo, &blasNeg1, &blasAL[0], &blas1planeNoAB, &blasX[0], &incx, &blas1, &blasWL[0], &incy);
-
- for (int i=0; i<planeNoAB; i++ ){
- blasVL[i] = 1.0/blasWL[i];
- blasDL[i*planeNoAB+i] = blasVL[i]*blasVL[i];
- }
-
- /* Gradient */
+
+
+ while(totalIter<500) {
+ penalty = 1.0/t;
+ /* Newton's method */
+ /* s=x[3]; */
+ s=blasX[3];
+
+ /* temp variables */
+ /* w1 = A1*x + b1; */
+ noElements = 3+planeNoA;
+ dcopy_(&noElements, &blasB1[0], &incx, &blasW1[0], &incy);
+ //transA = 'N'; blasM = 3+planeNoA; blasN = varNo;
+ //blasLDA = 3+planeNoA; blasAlpha = 1.0; blasBeta = 1.0; incx =1; incy=1;
+ //dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasA1[0], &blasLDA, &blasX[0], &incx, &blasBeta, &blasW1[0], &incy);
+ dgemv_(&blasNT, &planeNoA3, &varNo, &blas1, &blasA1[0], &planeNoA3, &blasX[0], &incx, &blas1, &blasW1[0], &incy);
+
+ /* w2 = A2*x + b2; */
+ noElements = 3+planeNoB;
+ dcopy_(&noElements, &blasB2[0], &incx, &blasW2[0], &incy);
+ //transA = 'N'; blasM = 3+planeNoB; blasN = varNo;
+ //blasLDA = 3+planeNoB; blasAlpha = 1.0; blasBeta = 1.0;
+ //dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasA2[0], &blasLDA, &blasX[0], &incx, &blasBeta, &blasW2[0], &incy);
+ dgemv_(&blasNT, &planeNoB3, &varNo, &blas1, &blasA2[0], &planeNoB3, &blasX[0], &incx, &blas1, &blasW2[0], &incy);
+
+ /* u1 = s*s - w1.dot(w1); */
+ /* u2 = s*s - w2.dot(w2); */
+ //noElements = 3+planeNoA;
+ blasW1dot = ddot_(&planeNoA3, &blasW1[0], &incx, &blasW1[0], &incy);
+ //noElements = 3+planeNoB;
+ blasW2dot = ddot_(&planeNoB3, &blasW2[0], &incx, &blasW2[0], &incy);
+ u1 =s*s -blasW1dot;
+ u2 =s*s -blasW2dot;
+
+ /* wL = bL - AL*x; */
+ noElements = planeNoAB;
+ dcopy_(&noElements, &blasBL[0], &incx, &blasWL[0], &incy);
+ //transA = 'N'; blasN = varNo;
+ //blasM = planeNoAB; blasLDA = std::max(1,planeNoAB); blasAlpha = -1.0; blasBeta = 1.0;
+ //dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasAL[0], &blasLDA, &blasX[0], &incx, &blasBeta, &blasWL[0], &incy);
+ dgemv_(&blasNT, &planeNoAB, &varNo, &blasNeg1, &blasAL[0], &blas1planeNoAB, &blasX[0], &incx, &blas1, &blasWL[0], &incy);
+
+ for (int i=0; i<planeNoAB; i++ ) {
+ blasVL[i] = 1.0/blasWL[i];
+ blasDL[i*planeNoAB+i] = blasVL[i]*blasVL[i];
+ }
+
+ /* Gradient */
//gA = -2.0/u1*(s*c - A1.transpose()*w1); //-2.0/u1*(s*c - tempG); //
//gB = -2.0/u2*(s*c - A2.transpose()*w2); //-2.0/u2*(s*c - tempG); //
// gL = AL.transpose()*vL; //tempG; //
// g = (c + penalty*(gA + gB + gL) );
- /* gA */
- //noElements = varNo;
- dcopy_(&varNo, &blasC[0], &incx, &blasGA[0], &incy);
- //transA = 'T'; blasM = 3+planeNoA; blasLDA = 3+planeNoA;
- blasAlpha = -1.0*-2.0/u1; blasBeta = 1.0*-2.0/u1*s;
- //dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasA1[0], &blasLDA, &blasW1[0], &incx, &blasBeta, &blasGA[0], &incy);
- dgemv_(&blasT, &planeNoA3, &varNo, &blasAlpha, &blasA1[0], &planeNoA3, &blasW1[0], &incx, &blasBeta, &blasGA[0], &incy);
-
- /* gB */
- dcopy_(&varNo, &blasC[0], &incx, &blasGB[0], &incy);
- //transA = 'T'; blasM = 3+planeNoB; blasLDA = 3+planeNoB;
- blasAlpha = -1.0*-2.0/u2; blasBeta = 1.0*-2.0/u2*s;
- //dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasA2[0], &blasLDA, &blasW2[0], &incx, &blasBeta, &blasGB[0], &incy);
- dgemv_(&blasT, &planeNoB3, &varNo, &blasAlpha, &blasA2[0], &planeNoB3, &blasW2[0], &incx, &blasBeta, &blasGB[0], &incy);
-
-/* gL */
- //transA = 'T'; blasM = planeNoAB; blasLDA = std::max(1,planeNoAB); blasAlpha = 1.0; blasBeta = 0.0;
- //dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasAL[0], &blasLDA, &blasVL[0], &incx, &blasBeta, &blasGL[0], &incy);
- dgemv_(&blasT, &planeNoAB, &varNo, &blas1, &blasAL[0], &blas1planeNoAB, &blasVL[0], &incx, &blas0, &blasGL[0], &incy);
-
-
-/* g */
- for (int i = 0; i<varNo; i++){
- blasGrad[i] = blasC[i] + penalty*(blasGA[i] + blasGB[i] + blasGL[i]);
- }
-
- /* Hessian */
- /* HA */ //Eigen::MatrixXd HA = (-2.0/u1)*(ca1Transpose)+ gA*gA.transpose();
- //noElements = varNo2;
- dcopy_(&varNo2, &blasCa1[0], &incx, &blasHA[0], &incy);
- //transA = 'N'; transB = 'T'; blasM = varNo; blasN = varNo; blasK = 1;blasLDA = blasM; blasLDB = blasN; blasAlpha = 1.0; blasLDC = blasM;
- blasBeta = -2.0/(u1); blasK = 1;
- //dgemm_(&transA, &transB, &blasM, &blasN, &blasK, &blasAlpha, &blasGA[0], &blasLDA, &blasGA[0], &blasLDB, &blasBeta, &blasHA[0], &blasLDC);
- dgemm_(&blasNT, &blasT, &varNo, &varNo, &blasK, &blas1, &blasGA[0], &varNo, &blasGA[0], &varNo, &blasBeta, &blasHA[0], &varNo);
-
- /* HB */ // HB = (-2.0/u2)*(ca2Transpose)+ gB*gB.transpose();
- dcopy_(&varNo2, &blasCa2[0], &incx, &blasHB[0], &incy);
- //transA = 'N'; transB = 'T'; blasM = varNo; blasN = varNo; blasK = 1; blasLDA = blasM; blasLDB = blasN; blasAlpha = 1.0; blasLDC = blasM;
- blasBeta = -2.0/(u2);
- //dgemm_(&transA, &transB, &blasM, &blasN, &blasK, &blasAlpha, &blasGB[0], &blasLDA, &blasGB[0], &blasLDB, &blasBeta, &blasHB[0], &blasLDC);
- dgemm_(&blasNT, &blasT, &varNo, &varNo, &blasK, &blas1, &blasGB[0], &varNo, &blasGB[0], &varNo, &blasBeta, &blasHB[0], &varNo);
-
-
- /* HL */ //Eigen::MatrixXd HL1 = AL.transpose()*DL*AL;
- //transA = 'T'; transB = 'N'; blasM = varNo; blasN = planeNoAB; blasK = planeNoAB;
- //blasLDA = std::max(1,blasK); blasLDB = std::max(1,blasK); blasAlpha = 1.0; blasBeta = 0.0; blasLDC = blasM;
- //dgemm_(&transA, &transB, &blasM, &blasN, &blasK, &blasAlpha, &blasAL[0], &blasLDA, &blasDL[0], &blasLDB, &blasBeta, &blasADAtemp[0], &blasLDC);
- dgemm_(&blasT, &blasNT, &varNo, &planeNoAB, &planeNoAB, &blas1, &blasAL[0], &blas1planeNoAB, &blasDL[0], &blas1planeNoAB, &blas0, &blasADAtemp[0], &varNo);
- //transA = 'N'; transB = 'N'; blasM = varNo; blasN = varNo; blasK = planeNoAB;
- //blasLDA = blasM; blasLDB = std::max(1,blasK); blasAlpha = 1.0; blasBeta = 0.0; blasLDC = blasM;
- //dgemm_(&transA, &transB, &blasM, &blasN, &blasK, &blasAlpha, &blasADAtemp[0], &blasLDA, &blasAL[0], &blasLDB, &blasBeta, &blasHL[0], &blasLDC);
- dgemm_(&blasNT, &blasNT, &varNo, &varNo, &planeNoAB, &blas1, &blasADAtemp[0], &varNo, &blasAL[0], &blas1planeNoAB, &blas0, &blasHL[0], &varNo);
-
- /* H */ // H = penalty*(HA + HB + HL);
- for (int i = 0; i<varNo2; i++){
- blasHess[i] = penalty*(blasHA[i] + blasHB[i] + blasHL[i]);
- }
+ /* gA */
+ //noElements = varNo;
+ dcopy_(&varNo, &blasC[0], &incx, &blasGA[0], &incy);
+ //transA = 'T'; blasM = 3+planeNoA; blasLDA = 3+planeNoA;
+ blasAlpha = -1.0*-2.0/u1;
+ blasBeta = 1.0*-2.0/u1*s;
+ //dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasA1[0], &blasLDA, &blasW1[0], &incx, &blasBeta, &blasGA[0], &incy);
+ dgemv_(&blasT, &planeNoA3, &varNo, &blasAlpha, &blasA1[0], &planeNoA3, &blasW1[0], &incx, &blasBeta, &blasGA[0], &incy);
+
+ /* gB */
+ dcopy_(&varNo, &blasC[0], &incx, &blasGB[0], &incy);
+ //transA = 'T'; blasM = 3+planeNoB; blasLDA = 3+planeNoB;
+ blasAlpha = -1.0*-2.0/u2;
+ blasBeta = 1.0*-2.0/u2*s;
+ //dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasA2[0], &blasLDA, &blasW2[0], &incx, &blasBeta, &blasGB[0], &incy);
+ dgemv_(&blasT, &planeNoB3, &varNo, &blasAlpha, &blasA2[0], &planeNoB3, &blasW2[0], &incx, &blasBeta, &blasGB[0], &incy);
+
+ /* gL */
+ //transA = 'T'; blasM = planeNoAB; blasLDA = std::max(1,planeNoAB); blasAlpha = 1.0; blasBeta = 0.0;
+ //dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasAL[0], &blasLDA, &blasVL[0], &incx, &blasBeta, &blasGL[0], &incy);
+ dgemv_(&blasT, &planeNoAB, &varNo, &blas1, &blasAL[0], &blas1planeNoAB, &blasVL[0], &incx, &blas0, &blasGL[0], &incy);
+
+
+ /* g */
+ for (int i = 0; i<varNo; i++) {
+ blasGrad[i] = blasC[i] + penalty*(blasGA[i] + blasGB[i] + blasGL[i]);
+ }
+
+ /* Hessian */
+ /* HA */ //Eigen::MatrixXd HA = (-2.0/u1)*(ca1Transpose)+ gA*gA.transpose();
+ //noElements = varNo2;
+ dcopy_(&varNo2, &blasCa1[0], &incx, &blasHA[0], &incy);
+ //transA = 'N'; transB = 'T'; blasM = varNo; blasN = varNo; blasK = 1;blasLDA = blasM; blasLDB = blasN; blasAlpha = 1.0; blasLDC = blasM;
+ blasBeta = -2.0/(u1);
+ blasK = 1;
+ //dgemm_(&transA, &transB, &blasM, &blasN, &blasK, &blasAlpha, &blasGA[0], &blasLDA, &blasGA[0], &blasLDB, &blasBeta, &blasHA[0], &blasLDC);
+ dgemm_(&blasNT, &blasT, &varNo, &varNo, &blasK, &blas1, &blasGA[0], &varNo, &blasGA[0], &varNo, &blasBeta, &blasHA[0], &varNo);
+
+ /* HB */ // HB = (-2.0/u2)*(ca2Transpose)+ gB*gB.transpose();
+ dcopy_(&varNo2, &blasCa2[0], &incx, &blasHB[0], &incy);
+ //transA = 'N'; transB = 'T'; blasM = varNo; blasN = varNo; blasK = 1; blasLDA = blasM; blasLDB = blasN; blasAlpha = 1.0; blasLDC = blasM;
+ blasBeta = -2.0/(u2);
+ //dgemm_(&transA, &transB, &blasM, &blasN, &blasK, &blasAlpha, &blasGB[0], &blasLDA, &blasGB[0], &blasLDB, &blasBeta, &blasHB[0], &blasLDC);
+ dgemm_(&blasNT, &blasT, &varNo, &varNo, &blasK, &blas1, &blasGB[0], &varNo, &blasGB[0], &varNo, &blasBeta, &blasHB[0], &varNo);
+
+
+ /* HL */ //Eigen::MatrixXd HL1 = AL.transpose()*DL*AL;
+ //transA = 'T'; transB = 'N'; blasM = varNo; blasN = planeNoAB; blasK = planeNoAB;
+ //blasLDA = std::max(1,blasK); blasLDB = std::max(1,blasK); blasAlpha = 1.0; blasBeta = 0.0; blasLDC = blasM;
+ //dgemm_(&transA, &transB, &blasM, &blasN, &blasK, &blasAlpha, &blasAL[0], &blasLDA, &blasDL[0], &blasLDB, &blasBeta, &blasADAtemp[0], &blasLDC);
+ dgemm_(&blasT, &blasNT, &varNo, &planeNoAB, &planeNoAB, &blas1, &blasAL[0], &blas1planeNoAB, &blasDL[0], &blas1planeNoAB, &blas0, &blasADAtemp[0], &varNo);
+ //transA = 'N'; transB = 'N'; blasM = varNo; blasN = varNo; blasK = planeNoAB;
+ //blasLDA = blasM; blasLDB = std::max(1,blasK); blasAlpha = 1.0; blasBeta = 0.0; blasLDC = blasM;
+ //dgemm_(&transA, &transB, &blasM, &blasN, &blasK, &blasAlpha, &blasADAtemp[0], &blasLDA, &blasAL[0], &blasLDB, &blasBeta, &blasHL[0], &blasLDC);
+ dgemm_(&blasNT, &blasNT, &varNo, &varNo, &planeNoAB, &blas1, &blasADAtemp[0], &varNo, &blasAL[0], &blas1planeNoAB, &blas0, &blasHL[0], &varNo);
+
+ /* H */ // H = penalty*(HA + HB + HL);
+ for (int i = 0; i<varNo2; i++) {
+ blasHess[i] = penalty*(blasHA[i] + blasHB[i] + blasHL[i]);
+ }
//timingDeltas->checkpoint("assemble H and g");
-
+
//std::cout<<"before Chol, totalIter: "<<totalIter<<", s : "<<s<<", u1: "<<u1<<", u2: "<<u2<<", wLmincoeff: "<<minWL<<endl;
-/* Cholesky factorization */
+ /* Cholesky factorization */
//#if 0
-/* L */
- for( int j=1; j<=varNo ; j++){
- blasStep[j-1]=-blasGrad[j-1];
- for (int i=j; i<=j+KD && i<=varNo; i++){
- HessianChol[j-1][1+i-j-1] = blasHess[(i-1)+varNo*(j-1)];
- }
- }
+ /* L */
+ for( int j=1; j<=varNo ; j++) {
+ blasStep[j-1]=-blasGrad[j-1];
+ for (int i=j; i<=j+KD && i<=varNo; i++) {
+ HessianChol[j-1][1+i-j-1] = blasHess[(i-1)+varNo*(j-1)];
+ }
+ }
//#endif
#if 0
- /* U */
- for( int j=1; j<=varNo ; j++){
- blasStep[j-1]=-blasGrad[j-1];
- for (int i=std::max(1,j-KD); i<=j ; i++){
- HessianChol[j-1][KD+1+i-j-1] = blasHess[(i-1)+varNo*(j-1)]; //H(i-1,j-1);
- }
- }
+ /* U */
+ for( int j=1; j<=varNo ; j++) {
+ blasStep[j-1]=-blasGrad[j-1];
+ for (int i=std::max(1,j-KD); i<=j ; i++) {
+ HessianChol[j-1][KD+1+i-j-1] = blasHess[(i-1)+varNo*(j-1)]; //H(i-1,j-1);
+ }
+ }
#endif
- dpbsv_( &UPLO, &varNo, &KD, &nrhs, &HessianChol[0][0], &varNo, blasStep, &varNo, &info );
- if(info != 0){
- std::cout<<"chol error"<<", planeA: "<<planeNoA<<", planeB: "<<planeNoB<<endl;
- //return false;
- /* LU factorization */
- for (int i=0; i<varNo; i++ ){
- blasStep[i]=-1.0*blasGrad[i]; //g[i];
- }
- int ipiv[varNo]; int bColNo=1;
- dgesv_( &varNo, &bColNo, blasHess, &varNo, ipiv, blasStep, &varNo, &info);
- }
+ dpbsv_( &UPLO, &varNo, &KD, &nrhs, &HessianChol[0][0], &varNo, blasStep, &varNo, &info );
+ if(info != 0) {
+ std::cout<<"chol error"<<", planeA: "<<planeNoA<<", planeB: "<<planeNoB<<endl;
+ //return false;
+ /* LU factorization */
+ for (int i=0; i<varNo; i++ ) {
+ blasStep[i]=-1.0*blasGrad[i]; //g[i];
+ }
+ int ipiv[varNo];
+ int bColNo=1;
+ dgesv_( &varNo, &bColNo, blasHess, &varNo, ipiv, blasStep, &varNo, &info);
+ }
- if (info!=0){
- std::cout<<"linear algebra error"<<endl;
- }
+ if (info!=0) {
+ std::cout<<"linear algebra error"<<endl;
+ }
//timingDeltas->checkpoint("Cholesky");
- //fprime = step.transpose()*g;
- //noElements = varNo;
- blasFprime = ddot_(&varNo, &blasStep[0], &incx, &blasGrad[0], &incy);
-
- wLlogsum = 0.0;
- for (int i=0; i<planeNoAB; i++){
- //wLlogsum += log(wL[i]);
- wLlogsum += log(blasWL[i]);
- }
-
- val = /* c.transpose()*/ blasX[3] -penalty*( log(u1) + log(u2) + wLlogsum );
-
- /*Linesearch */
- backtrack = 1.0;
- //noElements = varNo;
- dcopy_(&varNo, &blasX[0], &incx, &blasNewX[0], &incy);
- daxpy_(&varNo, &backtrack, &blasStep[0], &incx, &blasNewX[0], &incy);
-
-
-
- s = blasNewX[3];
- // w1 = A1*x + b1;
-
- dcopy_(&planeNoA3, &blasB1[0], &incx, &blasW1[0], &incy);
- //transA = 'N'; blasAlpha = 1.0; blasBeta = 1.0;
- //dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasA1[0], &blasLDA, &blasNewX[0], &incx, &blasBeta, &blasW1[0], &incy);
- dgemv_(&blasNT, &planeNoA3, &varNo, &blas1, &blasA1[0], &planeNoA3, &blasNewX[0], &incx, &blas1, &blasW1[0], &incy);
-
- // w2 = A2*x + b2;
- dcopy_(&planeNoB3, &blasB2[0], &incx, &blasW2[0], &incy);
- //transA = 'N'; blasAlpha = 1.0; blasBeta = 1.0;
- //dgemv_(&transA, &planeNoB3, &varNo, &blasAlpha, &blasA2[0], &planeNoB3, &blasNewX[0], &incx, &blasBeta, &blasW2[0], &incy);
- dgemv_(&blasNT, &planeNoB3, &varNo, &blas1, &blasA2[0], &planeNoB3, &blasNewX[0], &incx, &blas1, &blasW2[0], &incy);
-
-
- blasW1dot = ddot_(&planeNoA3, &blasW1[0], &incx, &blasW1[0], &incy);
- blasW2dot = ddot_(&planeNoB3, &blasW2[0], &incx, &blasW2[0], &incy);
-
- //u1 = s*s - w1.dot(w1);
- //u2 = s*s - w2.dot(w2);
- u1 =s*s -blasW1dot;
- u2 =s*s -blasW2dot;
-
- //wL = bL - AL*x;
- dcopy_(&planeNoAB, &blasBL[0], &incx, &blasWL[0], &incy);
- //blasAlpha = -1.0;
- //blasM=planeNoAB; blasN=varNo; blasLDA=std::max(1,planeNoAB);
- //dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasAL[0], &blasLDA, &blasNewX[0], &incx, &blasBeta, &blasWL[0], &incy);
- dgemv_(&transA, &planeNoAB, &varNo, &blasNeg1, &blasAL[0], &blas1planeNoAB, &blasNewX[0], &incx, &blas1, &blasWL[0], &incy);
-
- minWL =0.00001;
- if(planeNoAB>0){
- minWL = blasWL[0];
- }
- for(int i=0; i<planeNoAB; i++){
- if(blasWL[i] < minWL){
- minWL = blasWL[i];
- }
- }
-
- int count = 0;
- while(s < 0.0 || u1<0.0 || u2<0.0 || minWL < 0.0){
- backtrack = 0.5*backtrack;
- dcopy_(&varNo, &blasX[0], &incx, &blasNewX[0], &incy);
- daxpy_(&varNo, &backtrack, &blasStep[0], &incx, &blasNewX[0], &incy);
-
- s = blasNewX[3];
- // w1 = A1*x + b1;
-
- dcopy_(&planeNoA3, &blasB1[0], &incx, &blasW1[0], &incy);
- //transA = 'N'; blasAlpha = 1.0; blasBeta = 1.0;
- //dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasA1[0], &blasLDA, &blasNewX[0], &incx, &blasBeta, &blasW1[0], &incy);
- dgemv_(&blasNT, &planeNoA3, &varNo, &blas1, &blasA1[0], &planeNoA3, &blasNewX[0], &incx, &blas1, &blasW1[0], &incy);
-
- // w2 = A2*x + b2;
- dcopy_(&planeNoB3, &blasB2[0], &incx, &blasW2[0], &incy);
- //transA = 'N'; blasAlpha = 1.0; blasBeta = 1.0;
- //dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasA2[0], &blasLDA, &blasNewX[0], &incx, &blasBeta, &blasW2[0], &incy);
- dgemv_(&blasNT, &planeNoB3, &varNo, &blas1, &blasA2[0], &planeNoB3, &blasNewX[0], &incx, &blas1, &blasW2[0], &incy);
- blasW1dot = ddot_(&planeNoA3, &blasW1[0], &incx, &blasW1[0], &incy);
- blasW2dot = ddot_(&planeNoB3, &blasW2[0], &incx, &blasW2[0], &incy);
-
- //u1 = s*s - w1.dot(w1);
- //u2 = s*s - w2.dot(w2);
- u1 =s*s -blasW1dot;
- u2 =s*s -blasW2dot;
-
- //wL = bL - AL*x;
-
- dcopy_(&planeNoAB, &blasBL[0], &incx, &blasWL[0], &incy);
- // blasAlpha = -1.0;
- //blasM = planeNoAB; blasLDA = std::max(1,planeNoAB); blasN = varNo; blasAlpha = -1.0; blasBeta = 1.0;
- //dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasAL[0], &blasLDA, &blasNewX[0], &incx, &blasBeta, &blasWL[0], &incy);
- dgemv_(&blasNT, &planeNoAB, &varNo, &blasNeg1, &blasAL[0], &blas1planeNoAB, &blasNewX[0], &incx, &blas1, &blasWL[0], &incy);
- if(planeNoAB>0){
- minWL = blasWL[0];
- }
- for(int i=0; i<planeNoAB; i++){
- if(blasWL[i] < minWL){
- minWL = blasWL[i];
- }
- }
- count++;
- //std::cout<<"count: "<<count<<", s : "<<s<<", u1: "<<u1<<", u2: "<<u2<<", wLmincoeff: "<<minWL<<endl;
- if(count==200){
- std::cout<<"count: "<<count<<", totalIter: "<<totalIter<<", backtrack: "<<backtrack<<"s : "<<s<<", u1: "<<u1<<", u2: "<<u2<<", wLmincoeff: "<<minWL<<endl;
- //std::cout<<"wL: "<<endl<<wL<<endl<<endl;
- }
- }
+ //fprime = step.transpose()*g;
+ //noElements = varNo;
+ blasFprime = ddot_(&varNo, &blasStep[0], &incx, &blasGrad[0], &incy);
+
+ wLlogsum = 0.0;
+ for (int i=0; i<planeNoAB; i++) {
+ //wLlogsum += log(wL[i]);
+ wLlogsum += log(blasWL[i]);
+ }
+
+ val = /* c.transpose()*/ blasX[3] -penalty*( log(u1) + log(u2) + wLlogsum );
+
+ /*Linesearch */
+ backtrack = 1.0;
+ //noElements = varNo;
+ dcopy_(&varNo, &blasX[0], &incx, &blasNewX[0], &incy);
+ daxpy_(&varNo, &backtrack, &blasStep[0], &incx, &blasNewX[0], &incy);
+
+
+
+ s = blasNewX[3];
+// w1 = A1*x + b1;
+
+ dcopy_(&planeNoA3, &blasB1[0], &incx, &blasW1[0], &incy);
+ //transA = 'N'; blasAlpha = 1.0; blasBeta = 1.0;
+ //dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasA1[0], &blasLDA, &blasNewX[0], &incx, &blasBeta, &blasW1[0], &incy);
+ dgemv_(&blasNT, &planeNoA3, &varNo, &blas1, &blasA1[0], &planeNoA3, &blasNewX[0], &incx, &blas1, &blasW1[0], &incy);
+
+// w2 = A2*x + b2;
+ dcopy_(&planeNoB3, &blasB2[0], &incx, &blasW2[0], &incy);
+ //transA = 'N'; blasAlpha = 1.0; blasBeta = 1.0;
+ //dgemv_(&transA, &planeNoB3, &varNo, &blasAlpha, &blasA2[0], &planeNoB3, &blasNewX[0], &incx, &blasBeta, &blasW2[0], &incy);
+ dgemv_(&blasNT, &planeNoB3, &varNo, &blas1, &blasA2[0], &planeNoB3, &blasNewX[0], &incx, &blas1, &blasW2[0], &incy);
+
+
+ blasW1dot = ddot_(&planeNoA3, &blasW1[0], &incx, &blasW1[0], &incy);
+ blasW2dot = ddot_(&planeNoB3, &blasW2[0], &incx, &blasW2[0], &incy);
+
+ //u1 = s*s - w1.dot(w1);
+ //u2 = s*s - w2.dot(w2);
+ u1 =s*s -blasW1dot;
+ u2 =s*s -blasW2dot;
+
+ //wL = bL - AL*x;
+ dcopy_(&planeNoAB, &blasBL[0], &incx, &blasWL[0], &incy);
+ //blasAlpha = -1.0;
+ //blasM=planeNoAB; blasN=varNo; blasLDA=std::max(1,planeNoAB);
+ //dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasAL[0], &blasLDA, &blasNewX[0], &incx, &blasBeta, &blasWL[0], &incy);
+ dgemv_(&transA, &planeNoAB, &varNo, &blasNeg1, &blasAL[0], &blas1planeNoAB, &blasNewX[0], &incx, &blas1, &blasWL[0], &incy);
+
+ minWL =0.00001;
+ if(planeNoAB>0) {
+ minWL = blasWL[0];
+ }
+ for(int i=0; i<planeNoAB; i++) {
+ if(blasWL[i] < minWL) {
+ minWL = blasWL[i];
+ }
+ }
+
+ int count = 0;
+ while(s < 0.0 || u1<0.0 || u2<0.0 || minWL < 0.0) {
+ backtrack = 0.5*backtrack;
+ dcopy_(&varNo, &blasX[0], &incx, &blasNewX[0], &incy);
+ daxpy_(&varNo, &backtrack, &blasStep[0], &incx, &blasNewX[0], &incy);
+
+ s = blasNewX[3];
+ // w1 = A1*x + b1;
+
+ dcopy_(&planeNoA3, &blasB1[0], &incx, &blasW1[0], &incy);
+ //transA = 'N'; blasAlpha = 1.0; blasBeta = 1.0;
+ //dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasA1[0], &blasLDA, &blasNewX[0], &incx, &blasBeta, &blasW1[0], &incy);
+ dgemv_(&blasNT, &planeNoA3, &varNo, &blas1, &blasA1[0], &planeNoA3, &blasNewX[0], &incx, &blas1, &blasW1[0], &incy);
+
+ // w2 = A2*x + b2;
+ dcopy_(&planeNoB3, &blasB2[0], &incx, &blasW2[0], &incy);
+ //transA = 'N'; blasAlpha = 1.0; blasBeta = 1.0;
+ //dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasA2[0], &blasLDA, &blasNewX[0], &incx, &blasBeta, &blasW2[0], &incy);
+ dgemv_(&blasNT, &planeNoB3, &varNo, &blas1, &blasA2[0], &planeNoB3, &blasNewX[0], &incx, &blas1, &blasW2[0], &incy);
+ blasW1dot = ddot_(&planeNoA3, &blasW1[0], &incx, &blasW1[0], &incy);
+ blasW2dot = ddot_(&planeNoB3, &blasW2[0], &incx, &blasW2[0], &incy);
+
+ //u1 = s*s - w1.dot(w1);
+ //u2 = s*s - w2.dot(w2);
+ u1 =s*s -blasW1dot;
+ u2 =s*s -blasW2dot;
+
+ //wL = bL - AL*x;
+
+ dcopy_(&planeNoAB, &blasBL[0], &incx, &blasWL[0], &incy);
+ // blasAlpha = -1.0;
+ //blasM = planeNoAB; blasLDA = std::max(1,planeNoAB); blasN = varNo; blasAlpha = -1.0; blasBeta = 1.0;
+ //dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasAL[0], &blasLDA, &blasNewX[0], &incx, &blasBeta, &blasWL[0], &incy);
+ dgemv_(&blasNT, &planeNoAB, &varNo, &blasNeg1, &blasAL[0], &blas1planeNoAB, &blasNewX[0], &incx, &blas1, &blasWL[0], &incy);
+ if(planeNoAB>0) {
+ minWL = blasWL[0];
+ }
+ for(int i=0; i<planeNoAB; i++) {
+ if(blasWL[i] < minWL) {
+ minWL = blasWL[i];
+ }
+ }
+ count++;
+ //std::cout<<"count: "<<count<<", s : "<<s<<", u1: "<<u1<<", u2: "<<u2<<", wLmincoeff: "<<minWL<<endl;
+ if(count==200) {
+ std::cout<<"count: "<<count<<", totalIter: "<<totalIter<<", backtrack: "<<backtrack<<"s : "<<s<<", u1: "<<u1<<", u2: "<<u2<<", wLmincoeff: "<<minWL<<endl;
+ //std::cout<<"wL: "<<endl<<wL<<endl<<endl;
+ }
+ }
//timingDeltas->checkpoint("barrier search");
- wLlogsum = 0.0;
- for (int i=0; i<planeNoAB; i++){
- wLlogsum += log(blasWL[i]);
- }
-
-
- newval = /*c.tranpose()*/blasNewX[3] -penalty*( log(u1) + log(u2) + wLlogsum );
- count = 0;
- while (newval > val + backtrack*0.01*blasFprime){
- backtrack = 0.5*backtrack;
- //for (int i = 0; i<varNo; i++){
- // blasNewX[i] = blasX[i] + backtrack*blasStep[i];
- //}
- //noElements = varNo;
- dcopy_(&varNo, &blasX[0], &incx, &blasNewX[0], &incy);
- daxpy_(&varNo, &backtrack, &blasStep[0], &incx, &blasNewX[0], &incy);
-
- s = blasNewX[3];
- // w1 = A1*x + b1;
- //noElements = 3+planeNoA;
- dcopy_(&planeNoA3, &blasB1[0], &incx, &blasW1[0], &incy);
- //transA = 'N'; blasM = 3+planeNoA; blasN = varNo;
- //blasLDA = 3+planeNoA; blasAlpha = 1.0; blasBeta = 1.0; incx =1; incy=1;
- //dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasA1[0], &blasLDA, &blasNewX[0], &incx, &blasBeta, &blasW1[0], &incy);
- dgemv_(&blasNT, &planeNoA3, &varNo, &blas1, &blasA1[0], &planeNoA3, &blasNewX[0], &incx, &blas1, &blasW1[0], &incy);
-
- // w2 = A2*x + b2;
- //noElements = 3+planeNoB;
- dcopy_(&planeNoB3, &blasB2[0], &incx, &blasW2[0], &incy);
- //blasM = 3+planeNoB; blasLDA = 3+planeNoB; blasN = varNo;
- //dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasA2[0], &blasLDA, &blasNewX[0], &incx, &blasBeta, &blasW2[0], &incy);
- dgemv_(&blasNT, &planeNoB3, &varNo, &blas1, &blasA2[0], &planeNoB3, &blasNewX[0], &incx, &blas1, &blasW2[0], &incy);
-
- //noElements = 3+planeNoA;
- blasW1dot = ddot_(&planeNoA3, &blasW1[0], &incx, &blasW1[0], &incy);
- //noElements = 3+planeNoB;
- blasW2dot = ddot_(&planeNoB3, &blasW2[0], &incx, &blasW2[0], &incy);
-
- //u1 = s*s - w1.dot(w1);
- //u2 = s*s - w2.dot(w2);
- u1 =s*s -blasW1dot;
- u2 =s*s -blasW2dot;
-
- //wL = bL - AL*x;
- //noElements = planeNoAB;
- dcopy_(&planeNoAB, &blasBL[0], &incx, &blasWL[0], &incy);
- //blasM = planeNoAB; blasLDA = std::max(1,planeNoAB); blasN = varNo; blasAlpha = -1.0; blasBeta = 1.0;
- //dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasAL[0], &blasLDA, &blasNewX[0], &incx, &blasBeta, &blasWL[0], &incy);
- dgemv_(&blasNT, &planeNoAB, &varNo, &blasNeg1, &blasAL[0], &blas1planeNoAB, &blasNewX[0], &incx, &blas1, &blasWL[0], &incy);
-
- count++;
- if(count==200){
- std::cout<<"count: "<<count<<", totalIter: "<<totalIter<<", backtrack: "<<backtrack<<"s : "<<s<<", u1: "<<u1<<", u2: "<<u2<<", wLmincoeff: "<<minWL<<endl;
- //std::cout<<"wL: "<<endl<<wL<<endl<<endl;
- }
-
- wLlogsum = 0.0;
- for (int i=0; i<planeNoAB; i++){
- wLlogsum += log(blasWL[i]);
- }
-
- newval = /* c.transpose()*/blasNewX[3] - penalty*( log(u1) + log(u2) + wLlogsum );
-
- count++;
- if(backtrack<pow(10,-11) ){
- std::cout<<"iter: "<<iter<<", totalIter: "<<totalIter<<", backtrack: "<<backtrack<<", val: "<<val<<", newval: "<<newval<<", t: "<<t<<", fprime: "<<blasFprime<<endl;
-
- break;
- }
- }
-//timingDeltas->checkpoint("line search");
-
- noElements = varNo;
- dcopy_(&noElements, &blasNewX[0], &incx, &blasX[0], &incy);
-
-
- if(blasFprime >0.0){
- std::cout<<"count: "<<count<<", totalIter: "<<totalIter<<", blasFprime: "<<blasFprime<<endl;
- }
-
-
- if (-blasFprime*0.5 < NTTOL){
- gap = 2.0*m/t;
- //if (warmstart == true){break;}
- if (gap < tol){
- contactPt[0] = blasX[0]; contactPt[1]=blasX[1]; contactPt[2]=blasX[2];
- //if(warmstart==true){std::cout<<" totalIter : "<<totalIter<<endl;}
- Real fA = evaluatePP(cm1,state1,contactPt);
- Real fB = evaluatePP(cm2,state2,contactPt);
- if(fabs(fA-fB)>0.001 ){std::cout<<"inside fA-fB: "<<fA-fB<<endl;}
-
-//timingDeltas->checkpoint("newton");
- return true;
- }
- t = std::min(t*mu, (2.0*m+1.0)/tol);
- iter = 0;
- totalIter = totalIter+1;
- }
- if(totalIter>100){
-
- std::cout<<"totalIter: "<<totalIter<<", t: "<<t<<", gap: "<<2.0*m/t<<", blasFprime: "<<blasFprime<<endl;
- for (int i=0; i<varNo; i++){
- std::cout<<"blasStep, i"<<i<<", value: "<<blasStep[i]<<endl;
- }
- for (int i=0; i<varNo; i++){
- std::cout<<"blasGrad, i"<<i<<", value: "<<blasGrad[i]<<endl;
- }
- return false;
- //break;
- }
- iter++;
- totalIter++;
-
-//timingDeltas->checkpoint("complete");
-}
-
-
- return ( true );
-}
-
-
-
-
-
+ wLlogsum = 0.0;
+ for (int i=0; i<planeNoAB; i++) {
+ wLlogsum += log(blasWL[i]);
+ }
+
+
+ newval = /*c.tranpose()*/blasNewX[3] -penalty*( log(u1) + log(u2) + wLlogsum );
+ count = 0;
+ while (newval > val + backtrack*0.01*blasFprime) {
+ backtrack = 0.5*backtrack;
+ //for (int i = 0; i<varNo; i++){
+ // blasNewX[i] = blasX[i] + backtrack*blasStep[i];
+ //}
+ //noElements = varNo;
+ dcopy_(&varNo, &blasX[0], &incx, &blasNewX[0], &incy);
+ daxpy_(&varNo, &backtrack, &blasStep[0], &incx, &blasNewX[0], &incy);
+
+ s = blasNewX[3];
+ // w1 = A1*x + b1;
+ //noElements = 3+planeNoA;
+ dcopy_(&planeNoA3, &blasB1[0], &incx, &blasW1[0], &incy);
+ //transA = 'N'; blasM = 3+planeNoA; blasN = varNo;
+ //blasLDA = 3+planeNoA; blasAlpha = 1.0; blasBeta = 1.0; incx =1; incy=1;
+ //dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasA1[0], &blasLDA, &blasNewX[0], &incx, &blasBeta, &blasW1[0], &incy);
+ dgemv_(&blasNT, &planeNoA3, &varNo, &blas1, &blasA1[0], &planeNoA3, &blasNewX[0], &incx, &blas1, &blasW1[0], &incy);
+
+ // w2 = A2*x + b2;
+ //noElements = 3+planeNoB;
+ dcopy_(&planeNoB3, &blasB2[0], &incx, &blasW2[0], &incy);
+ //blasM = 3+planeNoB; blasLDA = 3+planeNoB; blasN = varNo;
+ //dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasA2[0], &blasLDA, &blasNewX[0], &incx, &blasBeta, &blasW2[0], &incy);
+ dgemv_(&blasNT, &planeNoB3, &varNo, &blas1, &blasA2[0], &planeNoB3, &blasNewX[0], &incx, &blas1, &blasW2[0], &incy);
+
+ //noElements = 3+planeNoA;
+ blasW1dot = ddot_(&planeNoA3, &blasW1[0], &incx, &blasW1[0], &incy);
+ //noElements = 3+planeNoB;
+ blasW2dot = ddot_(&planeNoB3, &blasW2[0], &incx, &blasW2[0], &incy);
+
+ //u1 = s*s - w1.dot(w1);
+ //u2 = s*s - w2.dot(w2);
+ u1 =s*s -blasW1dot;
+ u2 =s*s -blasW2dot;
+
+ //wL = bL - AL*x;
+ //noElements = planeNoAB;
+ dcopy_(&planeNoAB, &blasBL[0], &incx, &blasWL[0], &incy);
+ //blasM = planeNoAB; blasLDA = std::max(1,planeNoAB); blasN = varNo; blasAlpha = -1.0; blasBeta = 1.0;
+ //dgemv_(&transA, &blasM, &blasN, &blasAlpha, &blasAL[0], &blasLDA, &blasNewX[0], &incx, &blasBeta, &blasWL[0], &incy);
+ dgemv_(&blasNT, &planeNoAB, &varNo, &blasNeg1, &blasAL[0], &blas1planeNoAB, &blasNewX[0], &incx, &blas1, &blasWL[0], &incy);
+
+ count++;
+ if(count==200) {
+ std::cout<<"count: "<<count<<", totalIter: "<<totalIter<<", backtrack: "<<backtrack<<"s : "<<s<<", u1: "<<u1<<", u2: "<<u2<<", wLmincoeff: "<<minWL<<endl;
+ //std::cout<<"wL: "<<endl<<wL<<endl<<endl;
+ }
+
+ wLlogsum = 0.0;
+ for (int i=0; i<planeNoAB; i++) {
+ wLlogsum += log(blasWL[i]);
+ }
+
+ newval = /* c.transpose()*/blasNewX[3] - penalty*( log(u1) + log(u2) + wLlogsum );
+
+ count++;
+ if(backtrack<pow(10,-11) ) {
+ std::cout<<"iter: "<<iter<<", totalIter: "<<totalIter<<", backtrack: "<<backtrack<<", val: "<<val<<", newval: "<<newval<<", t: "<<t<<", fprime: "<<blasFprime<<endl;
+
+ break;
+ }
+ }
+ //timingDeltas->checkpoint("line search");
+
+ noElements = varNo;
+ dcopy_(&noElements, &blasNewX[0], &incx, &blasX[0], &incy);
+
+
+ if(blasFprime >0.0) {
+ std::cout<<"count: "<<count<<", totalIter: "<<totalIter<<", blasFprime: "<<blasFprime<<endl;
+ }
+
+
+ if (-blasFprime*0.5 < NTTOL) {
+ gap = 2.0*m/t;
+ //if (warmstart == true){break;}
+ if (gap < tol) {
+ contactPt[0] = blasX[0];
+ contactPt[1]=blasX[1];
+ contactPt[2]=blasX[2];
+ //if(warmstart==true){std::cout<<" totalIter : "<<totalIter<<endl;}
+ Real fA = evaluatePP(cm1,state1,contactPt);
+ Real fB = evaluatePP(cm2,state2,contactPt);
+ if(fabs(fA-fB)>0.001 ) {
+ std::cout<<"inside fA-fB: "<<fA-fB<<endl;
+ }
+ //timingDeltas->checkpoint("newton");
+ return true;
+ }
+ t = std::min(t*mu, (2.0*m+1.0)/tol);
+ iter = 0;
+ totalIter = totalIter+1;
+ }
+ if(totalIter>100) {
+
+ std::cout<<"totalIter: "<<totalIter<<", t: "<<t<<", gap: "<<2.0*m/t<<", blasFprime: "<<blasFprime<<endl;
+ for (int i=0; i<varNo; i++) {
+ std::cout<<"blasStep, i"<<i<<", value: "<<blasStep[i]<<endl;
+ }
+ for (int i=0; i<varNo; i++) {
+ std::cout<<"blasGrad, i"<<i<<", value: "<<blasGrad[i]<<endl;
+ }
+ return false;
+ //break;
+ }
+ iter++;
+ totalIter++;
+
+ //timingDeltas->checkpoint("complete");
+ }
+
+
+ return ( true );
+}
+
+#endif // YADE_POTENTIAL_PARTICLES
=== modified file 'pkg/dem/Ig2_PP_PP_ScGeom.hpp'
--- pkg/dem/Ig2_PP_PP_ScGeom.hpp 2015-12-14 14:25:08 +0000
+++ pkg/dem/Ig2_PP_PP_ScGeom.hpp 2016-01-19 00:59:38 +0000
@@ -3,80 +3,75 @@
/* code for calling MOSEK was for ver 6. Please uncomment if you have the licence */
#pragma once
-#include<lib/serialization/Serializable.hpp>
-#include<pkg/dem/PotentialParticle.hpp>
-#include<pkg/common/Dispatching.hpp>
-#include<pkg/common/Sphere.hpp>
-#include<Python.h>
-#include<Eigen/Core>
+#ifdef YADE_POTENTIAL_PARTICLES
+#include <lib/serialization/Serializable.hpp>
+#include <pkg/dem/PotentialParticle.hpp>
+#include <pkg/common/Dispatching.hpp>
+#include <pkg/common/Sphere.hpp>
+#include <Python.h>
+#include <Eigen/Core>
#include <stdio.h>
-# if 0
-#include <mosek.h>
+# ifdef YADE_MOSEK
+#include <mosek.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#endif
-class Ig2_PP_PP_ScGeom: public IGeomFunctor
-{
+class Ig2_PP_PP_ScGeom: public IGeomFunctor {
- //protected:
- //static std::ofstream output;
-#if 0
+#ifdef YADE_MOSEK
+ protected:
std::string myfile;
std::string Key;
MSKrescodee mosekTaskEnv;
MSKenv_t mosekEnv;
-
#endif
public :
virtual bool go(const shared_ptr<Shape>& cm1, const shared_ptr<Shape>& cm2, const State& state1, const State& se32, const Vector3r& shift2, const bool& force, const shared_ptr<Interaction>& c);
-
-
-
+
+
+
double evaluatePP(const shared_ptr<Shape>& cm1, const State& state1, const Vector3r newTrial);
void getPtOnParticle2(const shared_ptr<Shape>& cm1, const State& state1, Vector3r previousPt, Vector3r normal, Vector3r& newlocalPoint);
-
+
bool contactPtMosekF2(const shared_ptr<Shape>& cm1, const State& state1, const shared_ptr<Shape>& cm2, const State& state2, Vector3r &contactPt);
-
+
bool customSolve(const shared_ptr<Shape>& cm1, const State& state1, const shared_ptr<Shape>& cm2, const State& state2, Vector3r &contactPt, bool warmstart);
-
-
+
+
Vector3r getNormal(const shared_ptr<Shape>& cm1, const State& state1, const Vector3r newTrial);
-
+
void BrentZeroSurf(const shared_ptr<Shape>& cm1, const State& state1, const Vector3r bracketA, const Vector3r bracketB, Vector3r& zero);
-
-
-
-
-
- /////////////////////////////////////////////////////////////////////////////////////////////////////
-
-
-
- YADE_CLASS_BASE_DOC_ATTRS_CTOR(Ig2_PP_PP_ScGeom,IGeomFunctor,"pp",
+
+
+
+
+
+ /////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+
+ YADE_CLASS_BASE_DOC_ATTRS_CTOR(Ig2_PP_PP_ScGeom,IGeomFunctor,"IGeom functor for PotentialParticle - PotentialParticle pair",
((double, accuracyTol, pow(10,-7),, "accuracy desired, tolerance criteria for SOCP"))
((double,interactionDetectionFactor,1.0,,"bool to avoid granular ratcheting")),
//((std::string,myfile,"./PotentialParticles"+"","string")),
//timingDeltas=shared_ptr<TimingDeltas>(new TimingDeltas);
//mosekTaskEnv = MSK_makeenv(&mosekEnv,NULL,NULL,NULL,NULL);
//mosekTaskEnv = MSK_initenv(mosekEnv);
- );
-
-
-
-
- FUNCTOR2D(PotentialParticle,PotentialParticle);
- // needed for the dispatcher, even if it is symmetric
- DEFINE_FUNCTOR_ORDER_2D(PotentialParticle,PotentialParticle);
- DECLARE_LOGGER;
-
-
-
+ );
+
+
+
+
+ FUNCTOR2D(PotentialParticle,PotentialParticle);
+ // needed for the dispatcher, even if it is symmetric
+ DEFINE_FUNCTOR_ORDER_2D(PotentialParticle,PotentialParticle);
+ DECLARE_LOGGER;
};
@@ -87,26 +82,26 @@
extern "C" {
#endif
-/* LAPACK LU */
+ /* LAPACK LU */
//int dgesv(int varNo, int varNo2, double *H, int varNo3, int *pivot, double* g, int varNo4, int info){
- extern void dgesv_(const int *N, const int *nrhs, double *Hessian, const int *lda, int *ipiv, double *gradient, const int *ldb, int *info);
+ extern void dgesv_(const int *N, const int *nrhs, double *Hessian, const int *lda, int *ipiv, double *gradient, const int *ldb, int *info);
// int ans;
// dgesv_(&varNo, &varNo2, H, &varNo3, pivot,g, &varNo4, &ans);
// return ans;
- //}
+ //}
-/* LAPACK Cholesky */
+ /* LAPACK Cholesky */
extern void dpbsv_(const char *uplo, const int *n, const int *kd, const int *nrhs, double *AB, const int *ldab, double *B, const int *ldb, int *info);
-/* LAPACK QR */
+ /* LAPACK QR */
extern void dgels_(const char *Trans, const int *m, const int *n, const int *nrhs, double *A, const int *lda, double *B, const int *ldb, const double *work, const int *lwork, int *info);
-
-/*BLAS */
+
+ /*BLAS */
extern void dgemm_(const char *transA, const char *transB, const int *m, const int *n, const int *k, const double *alpha, double *A, const int *lda, double *B, const int *ldb, const double *beta, double *C, const int *ldc);
extern void dgemv_(const char *trans, const int *m, const int *n, const double *alpha, double *A, const int *lda, double *x, const int *incx, const double *beta, double *y, const int *incy);
-
+
extern void dcopy_(const int *N, double *x, const int *incx, double *y, const int *incy);
extern double ddot_(const int *N, double *x, const int *incx, double *y, const int *incy);
@@ -123,6 +118,4 @@
};
#endif
-
-
-
+#endif // YADE_POTENTIAL_PARTICLES
=== modified file 'pkg/dem/KnKsLaw.cpp'
--- pkg/dem/KnKsLaw.cpp 2015-12-14 14:25:08 +0000
+++ pkg/dem/KnKsLaw.cpp 2016-01-19 00:59:38 +0000
@@ -1,12 +1,13 @@
-#include"KnKsLaw.hpp"
-#include<core/Scene.hpp>
-#include<pkg/dem/ScGeom.hpp>
-#include<core/Omega.hpp>
-#include<pkg/dem/PotentialParticle.hpp>
+#ifdef YADE_POTENTIAL_PARTICLES
+#include "KnKsLaw.hpp"
+#include <core/Scene.hpp>
+#include <pkg/dem/ScGeom.hpp>
+#include <core/Omega.hpp>
+#include <pkg/dem/PotentialParticle.hpp>
YADE_PLUGIN((Law2_SCG_KnKsPhys_KnKsLaw)(Ip2_FrictMat_FrictMat_KnKsPhys)(KnKsPhys)
-);
+ );
@@ -15,9 +16,10 @@
-bool Law2_SCG_KnKsPhys_KnKsLaw::go(shared_ptr<IGeom>& ig, shared_ptr<IPhys>& ip, Interaction* contact){
- const Real& dt = scene->dt;
- int id1 = contact->getId1(); int id2 = contact->getId2();
+bool Law2_SCG_KnKsPhys_KnKsLaw::go(shared_ptr<IGeom>& ig, shared_ptr<IPhys>& ip, Interaction* contact) {
+ //const Real& dt = scene->dt;
+ int id1 = contact->getId1();
+ int id2 = contact->getId2();
ScGeom* geom= static_cast<ScGeom*>(ig.get());
KnKsPhys* phys = static_cast<KnKsPhys*>(ip.get());
State* de1 = Body::byId(id1,scene)->state.get();
@@ -29,113 +31,105 @@
Vector3r& shearForce = phys->shearForce;
Real un=geom->penetrationDepth;
TRVAR3(geom->penetrationDepth,de1->se3.position,de2->se3.position);
-
-/* Need to initialise in python. In the 1st time step. All the particles in contact (controlled by initialOverlap) are identified. The interactions are set to tensile and cohesive (tensionBroken = false and cohesionBroken = false). If there is no initial tension or cohesion, the contact law is run in a tensionless or cohesionless mode */
-
- if(geom->penetrationDepth <0.0 ){
+
+ /* Need to initialise in python. In the 1st time step. All the particles in contact (controlled by initialOverlap) are identified. The interactions are set to tensile and cohesive (tensionBroken = false and cohesionBroken = false). If there is no initial tension or cohesion, the contact law is run in a tensionless or cohesionless mode */
+
+ if(geom->penetrationDepth <0.0 ) {
if (neverErase) {
phys->shearForce = Vector3r::Zero();
phys->normalForce = Vector3r::Zero();
phys->normalViscous = Vector3r::Zero();
geom->normal = Vector3r::Zero();
phys->tensionBroken = true;
- }else{
+ } else {
scene->interactions->requestErase(id1,id2);
return false;
}
- return true;
+ return true;
}
Vector3r shearForceBeforeRotate = shearForce;
Vector3r shiftVel = Vector3r(0,0,0); //scene->isPeriodic ? (Vector3r)((scene->cell->velGrad*scene->cell->Hsize)*Vector3r((Real) contact->cellDist[0],(Real) contact->cellDist[1],(Real) contact->cellDist[2])) : Vector3r::Zero();
geom->rotate(shearForce); //AndGetShear(shearForce,phys->prevNormal,de1,de2,dt,shiftVel,/*avoid ratcheting*/false);
-Vector3r shearForceAfterRotate = shearForce;
+ Vector3r shearForceAfterRotate = shearForce;
//Linear elasticity giving "trial" shear force
Vector3r shift2(0,0,0);
- Vector3r incidentV = geom->getIncidentVel(de1, de2, scene->dt, shift2, shiftVel, /*preventGranularRatcheting*/false );
+ Vector3r incidentV = geom->getIncidentVel(de1, de2, scene->dt, shift2, shiftVel, /*preventGranularRatcheting*/false );
Vector3r incidentVn = geom->normal.dot(incidentV)*geom->normal; // contact normal velocity
Vector3r incidentVs = incidentV-incidentVn; // contact shear velocity
Vector3r shearIncrement=incidentVs*scene->dt;
phys->shearDir = shearIncrement;
phys->shearIncrementForCD += shearIncrement.norm();
- double du = 0.0; double debugFn = 0.0;
- double u_prev = fabs(phys->u_cumulative);
- if(phys->shearDir.norm() > pow(10,-15)){
+ double du = 0.0;
+ double debugFn = 0.0;
+ //double u_prev = fabs(phys->u_cumulative);
+ if(phys->shearDir.norm() > pow(10,-15)) {
phys->shearDir.normalize();
}
double degradeLength = phys->brittleLength; /*jointLength = 100u_peak */
/* Elastic and plastic displacement can have negative signs but must be consistent throughout the simulation */
- if(phys->initialShearDir.norm() < pow(10,-11)){
+ if(phys->initialShearDir.norm() < pow(10,-11)) {
phys->initialShearDir = phys->shearDir;
du = shearIncrement.norm();
- if(fabs(phys->mobilizedShear)>0.99999){
+ if(fabs(phys->mobilizedShear)>0.99999) {
phys->u_cumulative += du;
phys->cumulative_us += du;
- }else{
+ } else {
phys->u_elastic +=du;
}
- }else{
+ } else {
du = Mathr::Sign(phys->initialShearDir.dot(phys->shearDir))*shearIncrement.norm(); //check cumulative shear displacement
- if(fabs(phys->mobilizedShear) > 0.99999){
- if(du>0.0){ //if negative it means it is unloading
+ if(fabs(phys->mobilizedShear) > 0.99999) {
+ if(du>0.0) { //if negative it means it is unloading
phys->u_cumulative += du;
- phys->cumulative_us += du;
- }else{
+ phys->cumulative_us += du;
+ } else {
phys->u_elastic +=du;
- }
- }else{
+ }
+ } else {
phys->u_elastic +=du;
}
}
-
+
/* Original */
- if(phys->twoDimension) { phys->contactArea = phys->unitWidth2D*phys->jointLength;}
- if(s1->isBoundary == true || s2->isBoundary==true){phys->tensionBroken = true; phys->cohesionBroken = true;}
- if(!Talesnick){
+ if(phys->twoDimension) {
+ phys->contactArea = phys->unitWidth2D*phys->jointLength;
+ }
+ if(s1->isBoundary == true || s2->isBoundary==true) {
+ phys->tensionBroken = true;
+ phys->cohesionBroken = true;
+ }
+ if(!Talesnick) {
un = un-initialOverlapDistance;
-
- #if 0
- if(un > 0.0 && phys->rockJointContact == true ){
- if(phys->cohesionBroken == true && phys->intactRock == true && phys->shearForce.norm()>phys->normalForce.norm()*tan(phys->phi_r/180.0*3.14159) ){
- phys->dilation_angle = atan(shearForce.norm()/std::max(0.00001,phys->normalForce.norm()));
- double dilate_inc = tan( phys->dilation_angle - phys->phi_r/180.0*3.141592653589)*du; /* du could be negative */
- phys->u_dilate += dilate_inc;
- phys->u_dilate = std::min(phys->u_dilate,0.1);
- }
- //un = un+phys->u_dilate;
- phys->kn = ((1.0 - un/phys->maxClosure)*(phys->kn_i) - (un*phys->kn_i)*(-1.0/phys->maxClosure) )/pow( (1.0-un/phys->maxClosure),2);
- phys->prevSigma = un*phys->kn_i/(1.0 - std::min(un/phys->maxClosure,0.999) );
- }//else{
- #endif
-
- if (phys->jointType==3){
- phys->prevSigma = un*phys->kn_i/(1.0-un/phys->maxClosure);
- }else{
- phys->prevSigma = phys->kn*un;
- }
+
+ if (phys->jointType==3) {
+ phys->prevSigma = un*phys->kn_i/(1.0-un/phys->maxClosure);
+ } else {
+ phys->prevSigma = phys->kn*un;
+ }
//}
phys->normalForce = phys->prevSigma*std::max(pow(10,-15),phys->contactArea)*geom->normal;
}
phys->Knormal_area = phys->kn*std::max(pow(10,-8),phys->contactArea);
- if((un <0.0 && fabs(phys->prevSigma)>phys->tension && phys->tensionBroken == false /* first time tension is broken */) || (un<0.0 && phys->tensionBroken==true)){
+ if((un <0.0 && fabs(phys->prevSigma)>phys->tension && phys->tensionBroken == false /* first time tension is broken */) || (un<0.0 && phys->tensionBroken==true)) {
if (neverErase) {
phys->shearForce = Vector3r::Zero();
phys->normalForce = Vector3r::Zero();
phys->normalViscous = Vector3r::Zero();
geom->normal = Vector3r::Zero();
phys->tensionBroken = true;
- }else {
+ } else {
return false;
}
- return true;
+ return true;
}
-
-
-
+
+
+
/*ORIGINAL */
Vector3r c1x = geom->contactPoint - de1->pos;
Vector3r c2x = geom->contactPoint - de2->pos;
@@ -143,20 +137,20 @@
incidentVn = geom->normal.dot(incidentV)*geom->normal; // contact normal velocity
incidentVs = incidentV-incidentVn; // contact shear velocity
shearIncrement=incidentVs*scene->dt;
- if(!Talesnick){
+ if(!Talesnick) {
double Ks=0.0;
- if(phys->jointType == 3){
+ if(phys->jointType == 3) {
Ks = phys->ks_i*pow(phys->prevSigma,0.6);
- }else{
+ } else {
Ks = phys->ks;
}
- shearForce -= Ks*shearIncrement*std::max(pow(10,-11),phys->contactArea);
+ shearForce -= Ks*shearIncrement*std::max(pow(10,-11),phys->contactArea);
}
phys->Kshear_area = phys->ks*std::max(pow(10,-11),phys->contactArea);
- const shared_ptr<Body>& b1=Body::byId(id1,scene);
- const shared_ptr<Body>& b2=Body::byId(id2,scene);
+ const shared_ptr<Body>& b1=Body::byId(id1,scene);
+ const shared_ptr<Body>& b2=Body::byId(id2,scene);
Real mbar = (!b1->isDynamic() && b2->isDynamic()) ? de2->mass : ((!b2->isDynamic() && b1->isDynamic()) ? de1->mass : (de1->mass*de2->mass / (de1->mass + de2->mass))); // get equivalent mass if both bodies are dynamic, if not set it equal to the one of the dynamic body
Real Cn_crit = 2.*sqrt(mbar*phys->Knormal_area); // Knormal_area Critical damping coefficient (normal direction)
Real Cs_crit = 2.*sqrt(mbar*phys->Kshear_area); // Kshear_area Critical damping coefficient (shear direction)
@@ -165,79 +159,84 @@
double cs = Cs_crit*phys->viscousDamping; // Damping tangential coefficient
// add normal viscous component if damping is included
- double maxFnViscous = phys->normalForce.norm();
+ //double maxFnViscous = phys->normalForce.norm();
phys->normalViscous = cn*incidentVn;
//if(phys->normalViscous.norm() > maxFnViscous){
// phys->normalViscous = phys->normalViscous * maxFnViscous/phys->normalViscous.norm();
//}
phys->normalForce -= phys->normalViscous;
- double baseElevation = geom->contactPoint.z();
+ //double baseElevation = geom->contactPoint.z();
/* Water pressure, heat effect */
-
+
/* strength degradation */
const double PI = std::atan(1.0)*4;
double tan_effective_phi = 0.0;
-
-
-
- if(s1->isBoundary==true || s2->isBoundary == true || phys->jointType==2 ){ // clay layer at boundary;
+
+
+
+ if(s1->isBoundary==true || s2->isBoundary == true || phys->jointType==2 ) { // clay layer at boundary;
phys->effective_phi = phys->phi_b; // - 3.25*(1.0-exp(-fabs(phys->cumulative_us)/0.4));
tan_effective_phi = tan(phys->effective_phi/180.0*PI);
- }else if(phys->intactRock == true){
-
+ } else if(phys->intactRock == true) {
+
phys->effective_phi = phys->phi_r + (phys->phi_b-phys->phi_r)*(exp(-fabs(phys->u_cumulative)/degradeLength));
tan_effective_phi = tan(phys->effective_phi/180.0*PI);
- }else{
+ } else {
phys->effective_phi = phys->phi_b;
tan_effective_phi = tan(phys->effective_phi/180.0*PI);
}
-
-
-
+
+
+
/* shear loss */
Vector3r dampedShearForce = shearForce;
double cohesiveForce = phys->cohesion*std::max(pow(10,-11),phys->contactArea);
- Real maxFs = cohesiveForce;
- if (un>0.0 /*compression*/){
- double fN = phys->normalForce.norm();
- if(phys->intactRock == true){
- if (phys->cohesionBroken == true && allowBreakage == true){
- maxFs = std::max( fN,0.0)*tan_effective_phi;
- }else{
- maxFs = cohesiveForce+std::max( fN,0.0)*tan_effective_phi;
+ Real maxFs = cohesiveForce;
+ if (un>0.0 /*compression*/) {
+ double fN = phys->normalForce.norm();
+ if(phys->intactRock == true) {
+ if (phys->cohesionBroken == true && allowBreakage == true) {
+ maxFs = std::max( fN,0.0)*tan_effective_phi;
+ } else {
+ maxFs = cohesiveForce+std::max( fN,0.0)*tan_effective_phi;
}
- }else{
- maxFs = std::max( fN,0.0)*tan_effective_phi;
+ } else {
+ maxFs = std::max( fN,0.0)*tan_effective_phi;
}
}
- if( shearForce.norm() > maxFs ){
+ if( shearForce.norm() > maxFs ) {
Real ratio = maxFs / shearForce.norm();
shearForce *= ratio;
dampedShearForce = shearForce;
- if(allowBreakage == true){
+ if(allowBreakage == true) {
phys->cohesionBroken = true;
}
phys->shearViscous = Vector3r(0,0,0);
- }else{ /* no damping when it slides */
+ } else { /* no damping when it slides */
phys->shearViscous = cs*incidentVs;
dampedShearForce = shearForce - phys->shearViscous;
}
- if(shearForce.norm() < pow(10,-11) ){phys->mobilizedShear = 1.0;}else{phys->mobilizedShear = shearForce.norm()/maxFs;}
-
-
+ if(shearForce.norm() < pow(10,-11) ) {
+ phys->mobilizedShear = 1.0;
+ }
+ else {
+ phys->mobilizedShear = shearForce.norm()/maxFs;
+ }
+
+
//we need to use correct branches in the periodic case, the following apply for spheres only
Vector3r force = -phys->normalForce-dampedShearForce;
- if(isnan(force.norm())){
+ if(isnan(force.norm())) {
std::cout<<"shearForce: "<<shearForce<<", normalForce: "<<phys->normalForce<<", debugFn: "<<debugFn<<", viscous: "<<phys->normalViscous<<", normal: "<<phys->normal<<", geom normal: "<<geom->normal<<", effective_phi: "<<phys->effective_phi<<", shearIncrement: "<<shearIncrement<<", id1: "<<id1<<", id2: "<<id2<<", shearForceBeforeRotate: "<<shearForceBeforeRotate<<", shearForceAfterRotate: " <<shearForceAfterRotate<<endl;
}
scene->forces.addForce(id1,force);
scene->forces.addForce(id2,-force);
- Vector3r normal = geom->normal;
+ //Vector3r normal = geom->normal;
scene->forces.addTorque(id1,c1x.cross(force));
scene->forces.addTorque(id2,-(c2x).cross(force));
-
+
phys->prevNormal = geom->normal;
return true;
@@ -251,27 +250,27 @@
-void Ip2_FrictMat_FrictMat_KnKsPhys::go(const shared_ptr<Material>& b1, const shared_ptr<Material>& b2, const shared_ptr<Interaction>& interaction){
-
+void Ip2_FrictMat_FrictMat_KnKsPhys::go(const shared_ptr<Material>& b1, const shared_ptr<Material>& b2, const shared_ptr<Interaction>& interaction) {
+
const double PI = 3.14159265358979323846;
- if(interaction->phys) return;
+ if(interaction->phys) return;
ScGeom* scg=YADE_CAST<ScGeom*>(interaction->geom.get());
-
+
assert(scg);
const shared_ptr<FrictMat>& sdec1 = YADE_PTR_CAST<FrictMat>(b1);
const shared_ptr<FrictMat>& sdec2 = YADE_PTR_CAST<FrictMat>(b2);
-
- shared_ptr<KnKsPhys> contactPhysics(new KnKsPhys());
- //interaction->interactionPhysics = shared_ptr<MomentPhys>(new MomentPhys());
+
+ shared_ptr<KnKsPhys> contactPhysics(new KnKsPhys());
+ //interaction->interactionPhysics = shared_ptr<MomentPhys>(new MomentPhys());
//const shared_ptr<MomentPhys>& contactPhysics = YADE_PTR_CAST<MomentPhys>(interaction->interactionPhysics);
/* From interaction physics */
Real fa = sdec1->frictionAngle;
Real fb = sdec2->frictionAngle;
-
-
+
+
/* calculate stiffness */
Real Kn= Knormal;
Real Ks= Kshear;
@@ -288,12 +287,12 @@
contactPhysics->cohesionBroken = cohesionBroken;
contactPhysics->tensionBroken = tensionBroken;
contactPhysics->unitWidth2D = unitWidth2D;
- contactPhysics->frictionAngle = std::min(fa,fb);
- if(!useFaceProperties){
+ contactPhysics->frictionAngle = std::min(fa,fb);
+ if(!useFaceProperties) {
contactPhysics->phi_r = std::min(fa,fb)/PI*180.0;
contactPhysics->phi_b = contactPhysics->phi_r;
}
- contactPhysics->tanFrictionAngle = std::tan(contactPhysics->frictionAngle);
+ contactPhysics->tanFrictionAngle = std::tan(contactPhysics->frictionAngle);
//contactPhysics->initialOrientation1 = Body::byId(interaction->getId1())->state->ori;
//contactPhysics->initialOrientation2 = Body::byId(interaction->getId2())->state->ori;
contactPhysics->prevNormal = scg->normal; //This is also done in the Contact Law. It is not redundant because this class is only called ONCE!
@@ -302,11 +301,11 @@
contactPhysics->useFaceProperties = useFaceProperties;
contactPhysics->brittleLength = brittleLength;
interaction->phys = contactPhysics;
-
+
}
CREATE_LOGGER(KnKsPhys);
-/* KnKsPhys */
-KnKsPhys::~KnKsPhys(){}
-
-
+/* KnKsPhys */
+KnKsPhys::~KnKsPhys() {}
+
+#endif // YADE_POTENTIAL_PARTICLES
=== modified file 'pkg/dem/KnKsLaw.hpp'
--- pkg/dem/KnKsLaw.hpp 2015-12-14 14:25:08 +0000
+++ pkg/dem/KnKsLaw.hpp 2016-01-19 00:59:38 +0000
@@ -1,26 +1,22 @@
#pragma once
-#include<pkg/common/ElastMat.hpp>
-#include<pkg/common/Dispatching.hpp>
-#include<pkg/common/NormShearPhys.hpp>
-#include<pkg/dem/FrictPhys.hpp>
-#include<pkg/dem/ScGeom.hpp>
+#ifdef YADE_POTENTIAL_PARTICLES
+#include <pkg/common/ElastMat.hpp>
+#include <pkg/common/Dispatching.hpp>
+#include <pkg/common/NormShearPhys.hpp>
+#include <pkg/dem/FrictPhys.hpp>
+#include <pkg/dem/ScGeom.hpp>
#include <set>
#include <boost/tuple/tuple.hpp>
class KnKsPhys: public FrictPhys {
-
-
-
-
public:
-
- virtual ~KnKsPhys();
- YADE_CLASS_BASE_DOC_ATTRS_CTOR(KnKsPhys,FrictPhys,"Simple phys",
- ((vector<double>,lambdaIPOPT,0.0,,"lagrane multiplier for equality constraints"))
- ((vector<int>,cstatCPLEX,,,"lagrane multiplier for equality constraints"))
- ((vector<int>,rstatCPLEX,,,"lagrane multiplier for equality constraints"))
+ virtual ~KnKsPhys();
+ YADE_CLASS_BASE_DOC_ATTRS_CTOR(KnKsPhys,FrictPhys,"IPhys originally for potential particles",
+ ((vector<double>,lambdaIPOPT,0.0,,"Lagrange multiplier for equality constraints"))
+ ((vector<int>,cstatCPLEX,,,"Lagrange multiplier for equality constraints"))
+ ((vector<int>,rstatCPLEX,,,"Lagrange multiplier for equality constraints"))
((Real,frictionAngle,0.0,,"fric angle"))
((Real,tanFrictionAngle,0.0,,"tangent of fric angle"))
((Vector3r,contactDetectionPt,Vector3r(0,0,0),,"contact detection result"))
@@ -121,44 +117,44 @@
((double, effective_phi, 0.0,, "friction angle in clay after displacement"))
((double, prevOverlap, 0.0,, "friction angle in clay after displacement"))
((Real, h, 0.0,,"cd")),
-
+
//((Real, cumulativeRotation, 0.0,, "cumulative rotation"))
//((Quaternionr, initialOrientation1, Quaternionr(1.0,0.0,0.0,0.0),, "orientation1"))
//((Quaternionr, initialOrientation2, Quaternionr(1.0,0.0,0.0,0.0),, "orientation2")),
createIndex();
-
- );
-
- REGISTER_CLASS_INDEX(KnKsPhys,FrictPhys);
- DECLARE_LOGGER;
+
+ );
+
+ REGISTER_CLASS_INDEX(KnKsPhys,FrictPhys);
+ DECLARE_LOGGER;
};
REGISTER_SERIALIZABLE(KnKsPhys);
-class Ip2_FrictMat_FrictMat_KnKsPhys: public IPhysFunctor{
+class Ip2_FrictMat_FrictMat_KnKsPhys: public IPhysFunctor {
public:
virtual void go(const shared_ptr<Material>& pp1, const shared_ptr<Material>& pp2, const shared_ptr<Interaction>& interaction);
- YADE_CLASS_BASE_DOC_ATTRS(Ip2_FrictMat_FrictMat_KnKsPhys,IPhysFunctor,"Calculation",
- ((Real,Knormal,0.0,,"allows user to input values directly from python scripts"))
- ((Real,Kshear,0.0,,"allows user to input values directly from python scripts"))
- ((Real, unitWidth2D, 1.0, ,"viscousDamping"))
- ((double, brittleLength, 3.0, ,"brittle rock"))
- ((double, kn_i, 0.002, ,"brittle rock"))
- ((double, ks_i, 0.002, ,"brittle rock"))
- ((double, u_peak, -1.0, ,"brittle rock"))
- ((double, maxClosure, 0.002, ,"brittle rock"))
- ((Real, viscousDamping, 0.8, ,"viscousDamping"))
- ((Real, cohesion, 0.0, ,"viscousDamping"))
- ((Real, tension, 0.0, ,"viscousDamping"))
- ((bool, cohesionBroken, true, ,"cohesion"))
- ((bool, tensionBroken, true, ,"tension"))
- ((Real, phi_b, 0.0, ,"viscousDamping"))
- ((bool, useOverlapVol, false,, "calculate overlap volume"))
- ((bool, useFaceProperties, false,,"boolean to get face properites"))
- ((bool, calJointLength, false,, "calculate joint length"))
- ((bool, twoDimension, false, ,"tension already broken"))
+ YADE_CLASS_BASE_DOC_ATTRS(Ip2_FrictMat_FrictMat_KnKsPhys,IPhysFunctor,"Ip2 functor for :yref:`KnKsPhys`",
+ ((Real,Knormal,0.0,,"allows user to input values directly from python scripts"))
+ ((Real,Kshear,0.0,,"allows user to input values directly from python scripts"))
+ ((Real, unitWidth2D, 1.0, ,"viscousDamping"))
+ ((double, brittleLength, 3.0, ,"brittle rock"))
+ ((double, kn_i, 0.002, ,"brittle rock"))
+ ((double, ks_i, 0.002, ,"brittle rock"))
+ ((double, u_peak, -1.0, ,"brittle rock"))
+ ((double, maxClosure, 0.002, ,"brittle rock"))
+ ((Real, viscousDamping, 0.8, ,"viscousDamping"))
+ ((Real, cohesion, 0.0, ,"viscousDamping"))
+ ((Real, tension, 0.0, ,"viscousDamping"))
+ ((bool, cohesionBroken, true, ,"cohesion"))
+ ((bool, tensionBroken, true, ,"tension"))
+ ((Real, phi_b, 0.0, ,"viscousDamping"))
+ ((bool, useOverlapVol, false,, "calculate overlap volume"))
+ ((bool, useFaceProperties, false,,"boolean to get face properites"))
+ ((bool, calJointLength, false,, "calculate joint length"))
+ ((bool, twoDimension, false, ,"tension already broken"))
);
FUNCTOR2D(FrictMat,FrictMat);
DECLARE_LOGGER;
@@ -167,28 +163,27 @@
-class Law2_SCG_KnKsPhys_KnKsLaw: public LawFunctor{
+class Law2_SCG_KnKsPhys_KnKsLaw: public LawFunctor {
public:
-
+
static Real Real0;
//OpenMPAccumulator<Real,&Law2_SCG_KnKsPhys_KnKsLaw::Real0> plasticDissipation;
virtual bool go(shared_ptr<IGeom>& _geom, shared_ptr<IPhys>& _phys, Interaction* I);
FUNCTOR2D(ScGeom,KnKsPhys);
YADE_CLASS_BASE_DOC_ATTRS_CTOR_PY(Law2_SCG_KnKsPhys_KnKsLaw,LawFunctor,"Law for linear compression, without cohesion and Mohr-Coulomb plasticity surface.\n\n.. note::\n This law uses :yref:`ScGeom`; there is also functionally equivalent :yref:`Law2_Dem3DofGeom_FrictPhys_Basic`, which uses :yref:`Dem3DofGeom` (sphere-box interactions are not implemented for the latest).",
- ((bool,neverErase,false,,"Keep interactions even if particles go away from each other (only in case another constitutive law is in the scene, e.g. :yref:`Law2_ScGeom_CapillaryPhys_Capillarity`)"))
- ((bool,preventGranularRatcheting,false,,"bool to avoid granular ratcheting"))
- ((bool,traceEnergy,false,,"Define the total energy dissipated in plastic slips at all contacts."))
- ((bool,Talesnick,false,,"Define the total energy dissipated in plastic slips at all contacts."))
- ((double,waterLevel,0.0,,"Define the total energy dissipated in plastic slips at all contacts."))
- ((bool, allowBreakage, false, ,"cohesion = 0, once broken"))
- ((double,initialOverlapDistance,0.0,,"initial overlap distance"))
- ,,
-
+ ((bool,neverErase,false,,"Keep interactions even if particles go away from each other (only in case another constitutive law is in the scene, e.g. :yref:`Law2_ScGeom_CapillaryPhys_Capillarity`)"))
+ ((bool,preventGranularRatcheting,false,,"bool to avoid granular ratcheting"))
+ ((bool,traceEnergy,false,,"Define the total energy dissipated in plastic slips at all contacts."))
+ ((bool,Talesnick,false,,"Define the total energy dissipated in plastic slips at all contacts."))
+ ((double,waterLevel,0.0,,"Define the total energy dissipated in plastic slips at all contacts."))
+ ((bool, allowBreakage, false, ,"cohesion = 0, once broken"))
+ ((double,initialOverlapDistance,0.0,,"initial overlap distance"))
+ ,,
+
);
- DECLARE_LOGGER;
+ DECLARE_LOGGER;
};
REGISTER_SERIALIZABLE(Law2_SCG_KnKsPhys_KnKsLaw);
-
-
+#endif // YADE_POTENTIAL_PARTICLES
=== modified file 'pkg/dem/PotentialParticle.cpp'
--- pkg/dem/PotentialParticle.cpp 2015-12-14 14:25:08 +0000
+++ pkg/dem/PotentialParticle.cpp 2016-01-19 00:59:38 +0000
@@ -1,14 +1,13 @@
/*CWBoon 2015 */
- //! To implement potential particles (Houlsby 2009) using sphere
+#ifdef YADE_POTENTIAL_PARTICLES
+//! To implement potential particles (Houlsby 2009) using sphere
#include "PotentialParticle.hpp"
YADE_PLUGIN((PotentialParticle));
-PotentialParticle::~PotentialParticle()
-{
+PotentialParticle::~PotentialParticle() {
}
-
-
+#endif // YADE_POTENTIAL_PARTICLES
=== modified file 'pkg/dem/PotentialParticle.hpp'
--- pkg/dem/PotentialParticle.hpp 2015-12-14 14:25:08 +0000
+++ pkg/dem/PotentialParticle.hpp 2016-01-19 00:59:38 +0000
@@ -1,70 +1,70 @@
/*CWBoon 2015 */
#pragma once
+#ifdef YADE_POTENTIAL_PARTICLES
-#include<vector>
-#include<core/Shape.hpp>
-#include<Eigen/Core>
-#include <Eigen/LU>
+#include <vector>
+#include <core/Shape.hpp>
+#include <Eigen/Core>
+#include <Eigen/LU>
#include <Eigen/QR>
-#include<lib/base/openmp-accu.hpp>
-namespace yade{
-class PotentialParticle : public Shape
-{
+#include <lib/base/openmp-accu.hpp>
+namespace yade {
+class PotentialParticle : public Shape {
public:
-
-
virtual ~PotentialParticle ();
-
- YADE_CLASS_BASE_DOC_ATTRS_CTOR(PotentialParticle,Shape,"Geometry of PotentialParticle.",
- ((int, id, 1,, "idNo"))
- ((bool, isBoundary, false,, "boundary"))
- ((bool, fixedNormal, false,, "use fixed normal"))
- ((Vector3r, boundaryNormal, Vector3r::Zero(),,"normal direction of boundary"))
- ((bool, AabbMinMax, false,, "aabb"))
- ((Vector3r, minAabb, Vector3r::Zero(),,"min from box centre"))
- ((Vector3r, maxAabb, Vector3r::Zero(),,"max frin box centre"))
- ((Vector3r, minAabbRotated, Vector3r::Zero(),,"min from box centre"))
- ((Vector3r, maxAabbRotated, Vector3r::Zero(),,"max frin box centre"))
- ((Vector3r, halfSize, Vector3r::Zero(),,"max frin box centre"))
- ((Quaternionr , oriAabb, Quaternionr::Identity(),, "r "))
- ((Real , r, 0.1,, "r "))
- ((Real , R, 1.0,, "R "))
- ((Real , k, 0.1,, "k "))
- ((vector<Vector3r>, vertices,,,"vertices"))
- ((vector<bool> , isBoundaryPlane, ,, "whether it is a boundaryPlane "))
- ((vector<double> , a, ,, "a "))
- ((vector<double> , b, ,, "b "))
- ((vector<double> , c, ,, "c "))
- ((vector<double> , d, ,, "d "))
- ,
- createIndex(); /*ctor*/
- #if 0
- for (int i=0; i<a.size(); i++){
- Amatrix(i,0) = a[i]; Amatrix(i,1)=b[i]; Amatrix(i,2)=c[i];
+ YADE_CLASS_BASE_DOC_ATTRS_CTOR(PotentialParticle,Shape,"Geometry of PotentialParticle.",
+ ((int, id, 1,, "idNo"))
+ ((bool, isBoundary, false,, "boundary"))
+ ((bool, fixedNormal, false,, "use fixed normal"))
+ ((Vector3r, boundaryNormal, Vector3r::Zero(),,"normal direction of boundary"))
+ ((bool, AabbMinMax, false,, "aabb"))
+ ((Vector3r, minAabb, Vector3r::Zero(),,"min from box centre"))
+ ((Vector3r, maxAabb, Vector3r::Zero(),,"max from box centre"))
+ ((Vector3r, minAabbRotated, Vector3r::Zero(),,"min from box centre"))
+ ((Vector3r, maxAabbRotated, Vector3r::Zero(),,"max from box centre"))
+ ((Vector3r, halfSize, Vector3r::Zero(),,"max from box centre"))
+ ((Quaternionr , oriAabb, Quaternionr::Identity(),, "r "))
+ ((Real , r, 0.1,, "r "))
+ ((Real , R, 1.0,, "R "))
+ ((Real , k, 0.1,, "k "))
+ ((vector<Vector3r>, vertices,,,"vertices"))
+ ((vector<bool> , isBoundaryPlane, ,, "whether it is a boundaryPlane "))
+ ((vector<double> , a, ,, "a "))
+ ((vector<double> , b, ,, "b "))
+ ((vector<double> , c, ,, "c "))
+ ((vector<double> , d, ,, "d "))
+ ,
+ createIndex(); /*ctor*/
+#if 0
+ for (int i=0; i<a.size(); i++) {
+ Amatrix(i,0) = a[i];
+ Amatrix(i,1)=b[i];
+ Amatrix(i,2)=c[i];
Dmatrix(i,0) = d[i] + r;
- }
- #endif
-
- );
- //#endif
-
- REGISTER_CLASS_INDEX(PotentialParticle,Shape);
-
+ }
+#endif
+
+ );
+ //#endif
+
+ REGISTER_CLASS_INDEX(PotentialParticle,Shape);
+
};
}
-using namespace yade;
+using namespace yade;
REGISTER_SERIALIZABLE(PotentialParticle);
#ifdef __cplusplus
extern "C" {
#endif
- void dgesv_(const int *N, const int *nrhs, double *Hessian, const int *lda, int *ipiv, double *gradient, const int *ldb, int *info);
- void dsyev_(const char *jobz, const char *uplo, const int *N, double *A, const int *lda, double *W, double *work, int *lwork, int *info);
+void dgesv_(const int *N, const int *nrhs, double *Hessian, const int *lda, int *ipiv, double *gradient, const int *ldb, int *info);
+void dsyev_(const char *jobz, const char *uplo, const int *N, double *A, const int *lda, double *W, double *work, int *lwork, int *info);
#ifdef __cplusplus
};
#endif
+#endif // YADE_POTENTIAL_PARTICLES
=== modified file 'pkg/dem/PotentialParticle2AABB.cpp'
--- pkg/dem/PotentialParticle2AABB.cpp 2015-12-14 14:25:08 +0000
+++ pkg/dem/PotentialParticle2AABB.cpp 2016-01-19 00:59:38 +0000
@@ -1,26 +1,29 @@
/*CWBoon 2015 */
+#ifdef YADE_POTENTIAL_PARTICLES
#include "PotentialParticle2AABB.hpp"
-#include<pkg/dem/PotentialParticle.hpp>
-#include<pkg/common/Aabb.hpp>
+#include <pkg/dem/PotentialParticle.hpp>
+#include <pkg/common/Aabb.hpp>
-void PotentialParticle2AABB::go(const shared_ptr<Shape>& cm, shared_ptr<Bound>& bv, const Se3r& se3, const Body*){
+void PotentialParticle2AABB::go(const shared_ptr<Shape>& cm, shared_ptr<Bound>& bv, const Se3r& se3, const Body*) {
PotentialParticle* pp = static_cast<PotentialParticle*>(cm.get());
- if(!bv){ bv=shared_ptr<Bound>(new Aabb); }
+ if(!bv) {
+ bv=shared_ptr<Bound>(new Aabb);
+ }
Aabb* aabb = static_cast<Aabb*>(bv.get());
- Matrix3r r = se3.orientation.toRotationMatrix();
- if(pp->AabbMinMax == false){
+ if(pp->AabbMinMax == false) {
Real distFromCentre = 1.05*pp->R; // std::max(maxD, pp->R);
- halfSize = Vector3r(distFromCentre,distFromCentre,distFromCentre);
+ halfSize = Vector3r(distFromCentre,distFromCentre,distFromCentre);
aabb->min = se3.position-halfSize;
aabb->max = se3.position+halfSize;
return;
- }else{
+ } else {
Matrix3r r=se3.orientation.toRotationMatrix();
- Vector3r halfSizeMin(Vector3r::Zero());Vector3r halfSizeMax(Vector3r::Zero());
- if(pp->vertices.size() ==0){
+ Vector3r halfSizeMin(Vector3r::Zero());
+ Vector3r halfSizeMax(Vector3r::Zero());
+ if(pp->vertices.size() ==0) {
//pp->vertices.clear();
pp->vertices.push_back(Vector3r(pp->maxAabbRotated[0],pp->maxAabbRotated[1],pp->maxAabbRotated[2]));
pp->vertices.push_back(Vector3r(pp->maxAabbRotated[0],pp->maxAabbRotated[1],-pp->minAabbRotated[2]));
@@ -33,24 +36,38 @@
}
Vector3r aabbMin(0,0,0);
Vector3r aabbMax(0,0,0);
- for (int i=0; i<8; i++){
+ for (int i=0; i<8; i++) {
Vector3r vertex = r*(pp->oriAabb.conjugate()*pp->vertices[i]);
- if(vertex.x() < aabbMin.x()){ aabbMin.x() = vertex.x(); }
- if(vertex.y() < aabbMin.y()){ aabbMin.y() = vertex.y(); }
- if(vertex.z() < aabbMin.z()){ aabbMin.z() = vertex.z(); }
- if(vertex.x() > aabbMax.x()){ aabbMax.x() = vertex.x(); }
- if(vertex.y() > aabbMax.y()){ aabbMax.y() = vertex.y(); }
- if(vertex.z() > aabbMax.z()){ aabbMax.z() = vertex.z(); }
+ if(vertex.x() < aabbMin.x()) {
+ aabbMin.x() = vertex.x();
+ }
+ if(vertex.y() < aabbMin.y()) {
+ aabbMin.y() = vertex.y();
+ }
+ if(vertex.z() < aabbMin.z()) {
+ aabbMin.z() = vertex.z();
+ }
+ if(vertex.x() > aabbMax.x()) {
+ aabbMax.x() = vertex.x();
+ }
+ if(vertex.y() > aabbMax.y()) {
+ aabbMax.y() = vertex.y();
+ }
+ if(vertex.z() > aabbMax.z()) {
+ aabbMax.z() = vertex.z();
+ }
}
aabb->min= se3.position + 1.05*aabbMin;
aabb->max = se3.position + 1.05*aabbMax;
halfSizeMin = -1.0*aabbMin;
halfSizeMax = 1.0*aabbMax;
-
+
return;
}
-
+
}
-
+
YADE_PLUGIN((PotentialParticle2AABB));
+
+#endif // YADE_POTENTIAL_PARTICLES
=== modified file 'pkg/dem/PotentialParticle2AABB.hpp'
--- pkg/dem/PotentialParticle2AABB.hpp 2015-12-14 14:25:08 +0000
+++ pkg/dem/PotentialParticle2AABB.hpp 2016-01-19 00:59:38 +0000
@@ -1,24 +1,25 @@
/*CWBoon 2015 */
#pragma once
-
-#include<pkg/common/Dispatching.hpp>
-#include<pkg/dem/PotentialParticle.hpp>
-
-class PotentialParticle2AABB : public BoundFunctor
-{
+#ifdef YADE_POTENTIAL_PARTICLES
+
+#include <pkg/common/Dispatching.hpp>
+#include <pkg/dem/PotentialParticle.hpp>
+
+class PotentialParticle2AABB : public BoundFunctor {
public :
-
+
void go(const shared_ptr<Shape>& cm, shared_ptr<Bound>& bv, const Se3r&, const Body*);
-
- FUNCTOR1D(PotentialParticle);
- //REGISTER_ATTRIBUTES(BoundFunctor,(aabbEnlargeFactor));
- YADE_CLASS_BASE_DOC_ATTRS(PotentialParticle2AABB,BoundFunctor,"Functor creating :yref:`Aabb` from :yref:`Sphere`.",
- ((Real,aabbEnlargeFactor,((void)"deactivated",-1),,"Relative enlargement of the bounding box; deactivated if negative.\n\n.. note::\n\tThis attribute is used to create distant interaction, but is only meaningful with an :yref:`InteractionGeometryFunctor` which will not simply discard such interactions: :yref:`Ig2_Sphere_Sphere_Dem3DofGeom::distFactor` / :yref:`Ig2_Sphere_Sphere_ScGeom::interactionDetectionFactor` should have the same value as :yref:`aabbEnlargeFactor<Bo1_Sphere_Aabb::aabbEnlargeFactor>`."))
- ((Vector3r, halfSize, Vector3r::Zero(),,"halfSize"))
-
- );
+
+ FUNCTOR1D(PotentialParticle);
+ //REGISTER_ATTRIBUTES(BoundFunctor,(aabbEnlargeFactor));
+ YADE_CLASS_BASE_DOC_ATTRS(PotentialParticle2AABB,BoundFunctor,"Functor creating :yref:`Aabb` from :yref:`PotentialParticle`.",
+ ((Real,aabbEnlargeFactor,((void)"deactivated",-1),,"see :yref:`Sphere2AABB`."))
+ ((Vector3r, halfSize, Vector3r::Zero(),,"halfSize"))
+
+ );
};
REGISTER_SERIALIZABLE(PotentialParticle2AABB);
+#endif // YADE_POTENTIAL_PARTICLES
