yade-dev team mailing list archive
-
yade-dev team
-
Mailing list archive
-
Message #12507
[Branch ~yade-pkg/yade/git-trunk] Rev 3775: Merge pull request #47 from booncw/master
Merge authors:
booncw (booncw)
------------------------------------------------------------
revno: 3775 [merge]
committer: Jan Stránský <honzik.stransky@xxxxxxxxx>
timestamp: Mon 2016-01-18 10:00:22 +0100
message:
Merge pull request #47 from booncw/master
Commit Potential Particles: non-spherical particles for DEM
added:
examples/PotentialParticles/
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
=== added directory 'examples/PotentialParticles'
=== added file 'examples/PotentialParticles/cubePPscaled.py'
--- examples/PotentialParticles/cubePPscaled.py 1970-01-01 00:00:00 +0000
+++ examples/PotentialParticles/cubePPscaled.py 2015-12-14 14:25:08 +0000
@@ -0,0 +1,296 @@
+#!/usr/local/bin/yade-trunk -x
+# -*- encoding=utf-8 -*-
+# CWBoon 2015
+
+from yade import pack
+import math
+
+
+O.engines=[
+ ForceResetter(),
+ InsertionSortCollider([PotentialParticle2AABB()],verletDist=0),
+ InteractionLoop(
+ [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(),
+ 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)
+
+]
+
+
+
+
+powderDensity = 10000
+distanceToCentre= 0.5
+meanSize = 1.0
+wallThickness = 0.5*meanSize
+O.materials.append(FrictMat(young=150e6,poisson=.4,frictionAngle=radians(0.0),density=powderDensity,label='frictionless'))
+lengthOfBase = 9.0*meanSize
+heightOfBase = 14.0*meanSize
+sp=pack.SpherePack()
+mn,mx=Vector3(-0.5*(lengthOfBase-wallThickness),0.5*meanSize,-0.5*(lengthOfBase-wallThickness)),Vector3(0.5*(lengthOfBase-wallThickness),7.0*heightOfBase,0.5*(lengthOfBase-wallThickness))
+sphereRad = sqrt(3.0)*0.5*meanSize
+sp.makeCloud(mn,mx,sphereRad,0,100,False)
+
+
+count= 0
+rPP=0.05*meanSize
+for s in sp:
+ b=Body()
+ radius=2.2
+ dynamic=True
+ wire=False
+ color=[0,0,255.0]
+ highlight=False
+ b.shape=PotentialParticle(k=0.2, r=0.05*meanSize, R=1.02*sphereRad, a=[1.0,-1.0,0.0,0.0,0.0,0.0], b=[0.0,0.0,1.0,-1.0,0.0,0.0], c=[0.0,0.0,0.0,0.0,1.0,-1.0], d=[distanceToCentre-rPP,distanceToCentre-rPP,distanceToCentre-rPP,distanceToCentre-rPP,distanceToCentre-rPP,distanceToCentre-rPP],isBoundary=False,color=color,wire=wire,highlight=highlight,minAabb=Vector3(sphereRad,sphereRad,sphereRad),maxAabb=Vector3(sphereRad,sphereRad,sphereRad),maxAabbRotated=Vector3(sphereRad,sphereRad,sphereRad),minAabbRotated=Vector3(sphereRad,sphereRad,sphereRad),AabbMinMax=False)
+ length=meanSize
+ V= 1.0
+ geomInert=(2./5.)*powderDensity*V*sphereRad**2
+ utils._commonBodySetup(b,V,Vector3(geomInert,geomInert,geomInert), material='frictionless',pos=s[0], noBound=False, resetState=True, fixed=False)
+ b.state.pos = s[0] #s[0] stores center
+ b.state.ori = Quaternion((random.random(),random.random(),random.random()),random.random()) #s[2]
+ O.bodies.append(b)
+ 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
+color=[0,255,0]
+highlight=False
+bbb.shape=PotentialParticle(k=0.1, r=0.1*wallThickness, R=0.2*lengthOfBase,a=[1,-1,0,0,0,0], b=[0,0,1,-1,0,0], c=[0,0,0,0,1,-1], d=[lengthOfBase/6.0-r,lengthOfBase/6.0-r,0.5*wallThickness-r,0.5*wallThickness-r,lengthOfBase/6.0-r,lengthOfBase/6.0-r], id=count,isBoundary=True,isBoundaryPlane=[True,True,True,True,True,True],color=color ,wire=wire,highlight=highlight,AabbMinMax=True, minAabb=1.02*Vector3(lengthOfBase/6.0,0.5*wallThickness,lengthOfBase/6.0),maxAabb=1.02*Vector3(lengthOfBase/6.0,0.5*wallThickness,lengthOfBase/6.0),maxAabbRotated=1.02*Vector3(lengthOfBase/6.0,0.5*wallThickness,lengthOfBase/6.0),minAabbRotated=1.02*Vector3(lengthOfBase/6.0,0.5*wallThickness,lengthOfBase/6.0))
+length=lengthOfBase
+V=lengthOfBase*lengthOfBase*wallThickness
+geomInert=(1./6.)*V*length*wallThickness
+utils._commonBodySetup(bbb,V,Vector3(geomInert,geomInert,geomInert), material='frictionless',pos=[0.0,0,0], noBound=False, resetState=True, fixed=True)
+bbb.dynamic=False
+bbb.state.pos = [0.0,0,0]
+lidID = O.bodies.append(bbb)
+
+
+
+b1=Body()
+wire=False
+color=[0,255,0]
+highlight=False
+b1.shape=PotentialParticle(k=0.1, r=0.1*wallThickness, R=0.2*lengthOfBase,a=[1,-1,0,0,0,0], b=[0,0,1,-1,0,0], c=[0,0,0,0,1,-1], d=[lengthOfBase/6.0-r,lengthOfBase/6.0-r,0.5*wallThickness-r,0.5*wallThickness-r,lengthOfBase/6.0-r,lengthOfBase/6.0-r], id=count,isBoundary=True,isBoundaryPlane=[True,True,True,True,True,True],color=color ,wire=wire,highlight=highlight,AabbMinMax=True, minAabb=1.02*Vector3(lengthOfBase/6.0,0.4*wallThickness,lengthOfBase/6.0),maxAabb=1.02*Vector3(lengthOfBase/6.0,0.4*wallThickness,lengthOfBase/6.0),maxAabbRotated=1.02*Vector3(lengthOfBase/6.0,0.5*wallThickness,lengthOfBase/6.0),minAabbRotated=1.02*Vector3(lengthOfBase/6.0,0.5*wallThickness,lengthOfBase/6.0))
+length=lengthOfBase
+V=lengthOfBase*lengthOfBase*wallThickness
+geomInert=(1./6.)*V*length*wallThickness
+utils._commonBodySetup(b1,V,Vector3(geomInert,geomInert,geomInert), material='frictionless',pos=[0.0,0,0], noBound=False, resetState=True,fixed=True)
+b1.dynamic=False
+b1.state.pos = [lengthOfBase/3.0,0,lengthOfBase/3.0]
+O.bodies.append(b1)
+
+b2=Body()
+wire=False
+color=[0,255,0]
+highlight=False
+b2.shape=PotentialParticle(k=0.1, r=0.1*wallThickness, R=0.2*lengthOfBase,a=[1,-1,0,0,0,0], b=[0,0,1,-1,0,0], c=[0,0,0,0,1,-1], d=[lengthOfBase/6.0-r,lengthOfBase/6.0-r,0.5*wallThickness-r,0.5*wallThickness-r,lengthOfBase/6.0-r,lengthOfBase/6.0-r], id=count,isBoundary=True,isBoundaryPlane=[True,True,True,True,True,True],color=color ,wire=wire,highlight=highlight,AabbMinMax=True, minAabb=1.02*Vector3(lengthOfBase/6.0,0.4*wallThickness,lengthOfBase/6.0),maxAabb=1.02*Vector3(lengthOfBase/6.0,0.4*wallThickness,lengthOfBase/6.0),maxAabbRotated=1.02*Vector3(lengthOfBase/6.0,0.5*wallThickness,lengthOfBase/6.0),minAabbRotated=1.02*Vector3(lengthOfBase/6.0,0.5*wallThickness,lengthOfBase/6.0))
+length=lengthOfBase
+V=lengthOfBase*lengthOfBase*wallThickness
+geomInert=(1./6.)*V*length*wallThickness
+utils._commonBodySetup(b2,V,Vector3(geomInert,geomInert,geomInert), material='frictionless',pos=[0.0,0,0], noBound=False, resetState=True,fixed=True)
+b2.dynamic=False
+b2.state.pos = [-lengthOfBase/3.0,0,lengthOfBase/3.0]
+O.bodies.append(b2)
+
+b3=Body()
+wire=False
+color=[0,255,0]
+highlight=False
+b3.shape=PotentialParticle(k=0.1, r=0.1*wallThickness, R=0.2*lengthOfBase,a=[1,-1,0,0,0,0], b=[0,0,1,-1,0,0], c=[0,0,0,0,1,-1], d=[lengthOfBase/6.0-r,lengthOfBase/6.0-r,0.5*wallThickness-r,0.5*wallThickness-r,lengthOfBase/6.0-r,lengthOfBase/6.0-r], id=count,isBoundary=True,isBoundaryPlane=[True,True,True,True,True,True],color=color ,wire=wire,highlight=highlight,AabbMinMax=True, minAabb=1.02*Vector3(lengthOfBase/6.0,0.4*wallThickness,lengthOfBase/6.0),maxAabb=1.02*Vector3(lengthOfBase/6.0,0.4*wallThickness,lengthOfBase/6.0),maxAabbRotated=1.02*Vector3(lengthOfBase/6.0,0.5*wallThickness,lengthOfBase/6.0),minAabbRotated=1.02*Vector3(lengthOfBase/6.0,0.5*wallThickness,lengthOfBase/6.0))
+length=lengthOfBase
+V=lengthOfBase*lengthOfBase*wallThickness
+geomInert=(1./6.)*V*length*wallThickness
+utils._commonBodySetup(b3,V,Vector3(geomInert,geomInert,geomInert), material='frictionless',pos=[0.0,0,0], noBound=False, resetState=True,fixed=True)
+b3.dynamic=False
+b3.state.pos = [0.0,0,lengthOfBase/3.0]
+O.bodies.append(b3)
+
+b4=Body()
+wire=False
+color=[0,255,0]
+highlight=False
+b4.shape=PotentialParticle(k=0.1, r=0.1*wallThickness, R=0.2*lengthOfBase,a=[1,-1,0,0,0,0], b=[0,0,1,-1,0,0], c=[0,0,0,0,1,-1], d=[lengthOfBase/6.0-r,lengthOfBase/6.0-r,0.5*wallThickness-r,0.5*wallThickness-r,lengthOfBase/6.0-r,lengthOfBase/6.0-r], id=count,isBoundary=True,isBoundaryPlane=[True,True,True,True,True,True],color=color ,wire=wire,highlight=highlight,AabbMinMax=True, minAabb=1.02*Vector3(lengthOfBase/6.0,0.4*wallThickness,lengthOfBase/6.0),maxAabb=1.02*Vector3(lengthOfBase/6.0,0.4*wallThickness,lengthOfBase/6.0),maxAabbRotated=1.02*Vector3(lengthOfBase/6.0,0.5*wallThickness,lengthOfBase/6.0),minAabbRotated=1.02*Vector3(lengthOfBase/6.0,0.5*wallThickness,lengthOfBase/6.0))
+length=lengthOfBase
+V=lengthOfBase*lengthOfBase*wallThickness
+geomInert=(1./6.)*V*length*wallThickness
+utils._commonBodySetup(b4,V,Vector3(geomInert,geomInert,geomInert), material='frictionless',pos=[0.0,0,0], noBound=False, resetState=True,fixed=True)
+b4.dynamic=False
+b4.state.pos = [lengthOfBase/3.0,0,-lengthOfBase/3.0]
+O.bodies.append(b4)
+
+b5=Body()
+wire=False
+color=[0,255,0]
+highlight=False
+b5.shape=PotentialParticle(k=0.1, r=0.1*wallThickness, R=0.2*lengthOfBase,a=[1,-1,0,0,0,0], b=[0,0,1,-1,0,0], c=[0,0,0,0,1,-1], d=[lengthOfBase/6.0-r,lengthOfBase/6.0-r,0.5*wallThickness-r,0.5*wallThickness-r,lengthOfBase/6.0-r,lengthOfBase/6.0-r], id=count,isBoundary=True,isBoundaryPlane=[True,True,True,True,True,True],color=color ,wire=wire,highlight=highlight,AabbMinMax=True, minAabb=1.02*Vector3(lengthOfBase/6.0,0.4*wallThickness,lengthOfBase/6.0),maxAabb=1.02*Vector3(lengthOfBase/6.0,0.4*wallThickness,lengthOfBase/6.0),maxAabbRotated=1.02*Vector3(lengthOfBase/6.0,0.5*wallThickness,lengthOfBase/6.0),minAabbRotated=1.02*Vector3(lengthOfBase/6.0,0.5*wallThickness,lengthOfBase/6.0))
+length=lengthOfBase
+V=lengthOfBase*lengthOfBase*wallThickness
+geomInert=(1./6.)*V*length*wallThickness
+utils._commonBodySetup(b5,V,Vector3(geomInert,geomInert,geomInert), material='frictionless',pos=[0.0,0,0], noBound=False, resetState=True,fixed=True)
+b5.dynamic=False
+b5.state.pos = [0.0,0,-lengthOfBase/3.0]
+O.bodies.append(b5)
+
+
+b6=Body()
+wire=False
+color=[0,255,0]
+highlight=False
+b6.shape=PotentialParticle(k=0.1, r=0.1*wallThickness, R=0.2*lengthOfBase,a=[1,-1,0,0,0,0], b=[0,0,1,-1,0,0], c=[0,0,0,0,1,-1], d=[lengthOfBase/6.0-r,lengthOfBase/6.0-r,0.5*wallThickness-r,0.5*wallThickness-r,lengthOfBase/6.0-r,lengthOfBase/6.0-r], id=count,isBoundary=True,isBoundaryPlane=[True,True,True,True,True,True],color=color ,wire=wire,highlight=highlight,AabbMinMax=True, minAabb=1.02*Vector3(lengthOfBase/6.0,0.4*wallThickness,lengthOfBase/6.0),maxAabb=1.02*Vector3(lengthOfBase/6.0,0.4*wallThickness,lengthOfBase/6.0),maxAabbRotated=1.02*Vector3(lengthOfBase/6.0,0.5*wallThickness,lengthOfBase/6.0),minAabbRotated=1.02*Vector3(lengthOfBase/6.0,0.5*wallThickness,lengthOfBase/6.0))
+length=lengthOfBase
+V=lengthOfBase*lengthOfBase*wallThickness
+geomInert=(1./6.)*V*length*wallThickness
+utils._commonBodySetup(b6,V,Vector3(geomInert,geomInert,geomInert), material='frictionless',pos=[0.0,0,0], noBound=False, resetState=True,fixed=True)
+b6.dynamic=False
+b6.state.pos = [-lengthOfBase/3.0,0.0,-lengthOfBase/3.0]
+O.bodies.append(b6)
+
+b7=Body()
+wire=False
+color=[0,255,0]
+highlight=False
+b7.shape=PotentialParticle(k=0.1, r=0.1*wallThickness, R=0.2*lengthOfBase,a=[1,-1,0,0,0,0], b=[0,0,1,-1,0,0], c=[0,0,0,0,1,-1], d=[lengthOfBase/6.0-r,lengthOfBase/6.0-r,0.5*wallThickness-r,0.5*wallThickness-r,lengthOfBase/6.0-r,lengthOfBase/6.0-r], id=count,isBoundary=True,isBoundaryPlane=[True,True,True,True,True,True],color=color ,wire=wire,highlight=highlight,AabbMinMax=True, minAabb=1.02*Vector3(lengthOfBase/6.0,0.4*wallThickness,lengthOfBase/6.0),maxAabb=1.02*Vector3(lengthOfBase/6.0,0.4*wallThickness,lengthOfBase/6.0),maxAabbRotated=1.02*Vector3(lengthOfBase/6.0,0.5*wallThickness,lengthOfBase/6.0),minAabbRotated=1.02*Vector3(lengthOfBase/6.0,0.5*wallThickness,lengthOfBase/6.0))
+length=lengthOfBase
+V=lengthOfBase*lengthOfBase*wallThickness
+geomInert=(1./6.)*V*length*wallThickness
+utils._commonBodySetup(b7,V,Vector3(geomInert,geomInert,geomInert), material='frictionless',pos=[0.0,0,0], noBound=False, resetState=True,fixed=True)
+b7.dynamic=False
+b7.state.pos = [-lengthOfBase/3.0,0.0,0.0]
+O.bodies.append(b7)
+
+
+b8=Body()
+wire=False
+color=[0,255,0]
+highlight=False
+b8.shape=PotentialParticle(k=0.1, r=0.1*wallThickness, R=0.2*lengthOfBase,a=[1,-1,0,0,0,0], b=[0,0,1,-1,0,0], c=[0,0,0,0,1,-1], d=[lengthOfBase/6.0-r,lengthOfBase/6.0-r,0.5*wallThickness-r,0.5*wallThickness-r,lengthOfBase/6.0-r,lengthOfBase/6.0-r], id=count,isBoundary=True,isBoundaryPlane=[True,True,True,True,True,True],color=color ,wire=wire,highlight=highlight,AabbMinMax=True, minAabb=1.02*Vector3(lengthOfBase/6.0,0.4*wallThickness,lengthOfBase/6.0),maxAabb=1.02*Vector3(lengthOfBase/6.0,0.4*wallThickness,lengthOfBase/6.0),maxAabbRotated=1.02*Vector3(lengthOfBase/6.0,0.5*wallThickness,lengthOfBase/6.0),minAabbRotated=1.02*Vector3(lengthOfBase/6.0,0.5*wallThickness,lengthOfBase/6.0))
+length=lengthOfBase
+V=lengthOfBase*lengthOfBase*wallThickness
+geomInert=(1./6.)*V*length*wallThickness
+utils._commonBodySetup(b8,V,Vector3(geomInert,geomInert,geomInert), material='frictionless',pos=[0.0,0,0], noBound=False, resetState=True,fixed=True)
+b8.dynamic=False
+b8.state.pos = [lengthOfBase/3.0,0.0,0.0]
+O.bodies.append(b8)
+
+bA=Body()
+wire=False
+color=[0,255,0]
+highlight=False
+bA.shape=PotentialParticle(k=0.1, r=0.1*wallThickness, R=0.5*heightOfBase,a=[1,-1,0,0,0,0], b=[0,0,1,-1,0,0], c=[0,0,0,0,1,-1], d=[0.5*wallThickness-r,0.5*wallThickness-r,0.5*heightOfBase-r,0.5*heightOfBase-r,0.5*lengthOfBase-r,0.5*lengthOfBase-r], id=count+1,isBoundary=True,isBoundaryPlane=[True,True,True,True,True,True],color=color ,wire=wire,highlight=highlight,AabbMinMax=True, minAabb=1.02*Vector3(0.3*wallThickness,0.5*heightOfBase,0.5*lengthOfBase),maxAabb=1.02*Vector3(0.3*wallThickness,0.5*heightOfBase,0.5*lengthOfBase),maxAabbRotated=1.02*Vector3(0.5*wallThickness,0.5*heightOfBase,0.5*lengthOfBase),minAabbRotated=1.02*Vector3(0.5*wallThickness,0.5*heightOfBase,0.5*lengthOfBase))
+length=lengthOfBase
+V=lengthOfBase*lengthOfBase*wallThickness
+geomInert=(1./6.)*V*length*wallThickness
+utils._commonBodySetup(bA,V,Vector3(geomInert,geomInert,geomInert), material='frictionless',pos=[0.0,0,0], noBound=False, resetState=True,fixed=True)
+bA.dynamic=False
+bA.state.pos = [0.5*lengthOfBase,0.5*heightOfBase,0]
+O.bodies.append(bA)
+
+bB=Body()
+wire=False
+color=[0,255,0]
+highlight=False
+bB.shape=PotentialParticle(k=0.1, r=0.1*wallThickness, R=0.5*heightOfBase,a=[1,-1,0,0,0,0], b=[0,0,1,-1,0,0], c=[0,0,0,0,1,-1], d=[0.5*wallThickness-r,0.5*wallThickness-r,0.5*heightOfBase-r,0.5*heightOfBase-r,0.5*lengthOfBase-r,0.5*lengthOfBase-r], id=count+2,isBoundary=True,isBoundaryPlane=[True,True,True,True,True,True],color=color ,wire=wire,highlight=highlight,AabbMinMax=True, minAabb=1.02*Vector3(0.3*wallThickness,0.5*heightOfBase,0.5*lengthOfBase),maxAabb=1.02*Vector3(0.3*wallThickness,0.5*heightOfBase,0.5*lengthOfBase),maxAabbRotated=1.02*Vector3(0.5*wallThickness,0.5*heightOfBase,0.5*lengthOfBase),minAabbRotated=1.02*Vector3(0.5*wallThickness,0.5*heightOfBase,0.5*lengthOfBase))
+length=lengthOfBase
+V=lengthOfBase*lengthOfBase*wallThickness
+geomInert=(1./6.)*V*length*wallThickness
+utils._commonBodySetup(bB,V,Vector3(geomInert,geomInert,geomInert), material='frictionless',pos=[0.0,0,0], noBound=False, resetState=True,fixed=True)
+bB.dynamic=False
+bB.state.pos = [-0.5*lengthOfBase,0.5*heightOfBase,0]
+O.bodies.append(bB)
+
+
+bC=Body()
+wire=False
+color=[0,255,0]
+highlight=False
+bC.shape=PotentialParticle(k=0.1, r=0.1*wallThickness, R=0.5*heightOfBase,a=[1,-1,0,0,0,0], b=[0,0,1,-1,0,0], c=[0,0,0,0,1,-1], d=[0.5*lengthOfBase-r,0.5*lengthOfBase-r,0.5*heightOfBase-r,0.5*heightOfBase-r,0.5*wallThickness-r,0.5*wallThickness-r], id=count+3,isBoundary=True,isBoundaryPlane=[True,True,True,True,True,True],color=color ,wire=wire,highlight=highlight,AabbMinMax=True, minAabb=1.02*Vector3(0.5*lengthOfBase,0.5*heightOfBase,0.3*wallThickness),maxAabb=1.02*Vector3(0.5*lengthOfBase,0.5*heightOfBase,0.3*wallThickness),maxAabbRotated=1.02*Vector3(0.5*lengthOfBase,0.5*heightOfBase,0.5*wallThickness),minAabbRotated=1.02*Vector3(0.5*lengthOfBase,0.5*heightOfBase,0.5*wallThickness))
+length=lengthOfBase
+V=lengthOfBase*lengthOfBase*wallThickness
+geomInert=(1./6.)*V*length*wallThickness
+utils._commonBodySetup(bC,V,Vector3(geomInert,geomInert,geomInert), material='frictionless',pos=[0.0,0,0], noBound=False, resetState=True,fixed=True)
+bC.dynamic=False
+bC.state.pos = [0,0.5*heightOfBase,0.5*lengthOfBase]
+O.bodies.append(bC)
+
+bD=Body()
+wire=False
+color=[0,255,0]
+highlight=False
+bD.shape=PotentialParticle(k=0.1, r=0.1*wallThickness, R=0.5*heightOfBase,a=[1,-1,0,0,0,0], b=[0,0,1,-1,0,0], c=[0,0,0,0,1,-1], d=[0.5*lengthOfBase-r,0.5*lengthOfBase-r,0.5*heightOfBase-r,0.5*heightOfBase-r,0.5*wallThickness-r,0.5*wallThickness-r], id=count+4,isBoundary=True,isBoundaryPlane=[True,True,True,True,True,True],color=color ,wire=wire,highlight=highlight,AabbMinMax=True, minAabb=1.02*Vector3(0.5*lengthOfBase,0.5*heightOfBase,0.3*wallThickness),maxAabb=1.02*Vector3(0.5*lengthOfBase,0.5*heightOfBase,0.3*wallThickness),maxAabbRotated=1.02*Vector3(0.5*lengthOfBase,0.5*heightOfBase,0.5*wallThickness),minAabbRotated=1.02*Vector3(0.5*lengthOfBase,0.5*heightOfBase,0.5*wallThickness))
+length=lengthOfBase
+V=lengthOfBase*lengthOfBase*wallThickness
+geomInert=(1./6.)*V*length*wallThickness
+utils._commonBodySetup(bD,V,Vector3(geomInert,geomInert,geomInert), material='frictionless',pos=[0.0,0,0], noBound=False, resetState=True,fixed=True)
+bD.dynamic=False
+bD.state.pos = [0.0,0.5*heightOfBase,-0.5*lengthOfBase]
+O.bodies.append(bD)
+
+
+escapeNo=0
+def myAddPlotData():
+ global escapeNo
+ global wallThickness
+ global meanSize
+ uf=utils.unbalancedForce()
+ if isnan(uf):
+ uf = 1.0
+ KE = utils.kineticEnergy()
+
+ for b in O.bodies:
+ if b.state.pos[1] < -5.0*meanSize and b.dynamic==True:
+ escapeNo = escapeNo+1
+ O.bodies.erase(b.id)
+ if O.iter>12000:
+ removeLid()
+ plot.addData(timeStep1=O.iter,timeStep2=O.iter,timeStep3=O.iter,timeStep4=O.iter,time=O.time,unbalancedForce=uf,kineticEn=KE,outsideNo=escapeNo)
+
+
+from yade import plot
+plot.plots={'timeStep1':('unbalancedForce'),'timeStep2':('kineticEn'),'time':('outsideNo')}
+O.engines=O.engines+[PyRunner(iterPeriod=10,command='myAddPlotData()')]
+
+def removeLid():
+ global lidID
+ 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()
=== added file 'pkg/common/Gl1_PotentialParticle.cpp'
--- pkg/common/Gl1_PotentialParticle.cpp 1970-01-01 00:00:00 +0000
+++ pkg/common/Gl1_PotentialParticle.cpp 2015-12-14 14:25:08 +0000
@@ -0,0 +1,610 @@
+/*CWBoon 2015 */
+#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 <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 <vtkContourFilter.h>
+#include <vtkPolyDataMapper.h>
+#include<vtkXMLPolyDataWriter.h>
+#include <vtkAppendPolyData.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 <vtkSphereSource.h>
+#include <vtkDiskSource.h>
+#include <vtkRegularPolygonSource.h>
+#include <vtkProperty.h>
+
+#include <vtkLabeledDataMapper.h>
+#include <vtkActor2D.h>
+#include <vtkVectorText.h>
+#include <vtkLinearExtrusionFilter.h>
+#include <vtkConeSource.h>
+#include <vtkCamera.h>
+//#include<pkg/dem/Clump.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());
+ int planeNo = pp->d.size();
+ Real maxD = pp->d[0];
+
+ 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);
+ 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));
+
+ isoStep=Vector3r(dx,dy,dz);
+}
+
+
+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]);//
+ }
+ }
+ }
+}
+
+
+
+vector<Gl1_PotentialParticle::scalarF> Gl1_PotentialParticle::SF;
+int Gl1_PotentialParticle::sizeX, Gl1_PotentialParticle::sizeY, Gl1_PotentialParticle::sizeZ;
+bool Gl1_PotentialParticle::store;
+bool Gl1_PotentialParticle::initialized;
+
+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());
+ 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();
+ }
+ initialized = true;
+}
+
+
+//if(std::isnan(shapeId)==true){return;}
+
+ calcMinMax(cm);
+ mc.init(sizeX,sizeY,sizeZ,min,max);
+ 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]));
+ glColor3v(cm->color);
+
+
+ vector<Vector3r>& triangles = SF[shapeId].triangles; //mc.getTriangles();
+ int nbTriangles = SF[shapeId].nbTriangles; // //mc.getNbTriangles();
+ 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)
+ {
+ glNormal3v(normals[i]);
+ glVertex3v(triangles[i]);
+ glNormal3v(normals[++i]);
+ glVertex3v(triangles[i]);
+ glNormal3v(normals[++i]);
+ 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);
+ }
+
+
+ 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;
+}
+
+
+// Create the function
+ImpFunc::ImpFunc()
+{
+ clump = false;
+ // 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){
+ 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];
+
+ 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){
+
+ Eigen::Vector3d xori(x[0],x[1],x[2]);
+ Eigen::Vector3d clumpMemberCentre(clumpMemberCentreX,clumpMemberCentreY,clumpMemberCentreZ);
+ Eigen::Vector3d xlocal = xori-clumpMemberCentre;
+ //xlocal[0] = rotationMatrix[0]*x[0] + rotationMatrix[3]*x[1] + rotationMatrix[6]*x[2];
+ //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;}
+ 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) );
+ return f;
+ // return 0;
+ }
+ // the value of the function
+}
+
+
+
+
+
+void PotentialParticleVTKRecorder::action(){
+ if(fileName.size()==0) return;
+ vtkSmartPointer<vtkPoints> pbPos = vtkSmartPointer<vtkPoints>::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();
+
+ // interactions ###############################################
+ vtkSmartPointer<vtkPoints> intrBodyPos = vtkSmartPointer<vtkPoints>::New();
+ vtkSmartPointer<vtkCellArray> intrCells = vtkSmartPointer<vtkCellArray>::New();
+ vtkSmartPointer<vtkFloatArray> intrForceN = vtkSmartPointer<vtkFloatArray>::New();
+ intrForceN->SetNumberOfComponents(3);
+ intrForceN->SetName("forceN");
+ vtkSmartPointer<vtkFloatArray> intrAbsForceT = vtkSmartPointer<vtkFloatArray>::New();
+ intrAbsForceT->SetNumberOfComponents(1);
+ intrAbsForceT->SetName("absForceT");
+ // interactions ###############################################
+
+ // interaction contact point ###############################################
+ vtkSmartPointer<vtkPoints> pbContactPoint = vtkSmartPointer<vtkPoints>::New();
+ vtkSmartPointer<vtkCellArray> pbCellsContact = vtkSmartPointer<vtkCellArray>::New();
+ vtkSmartPointer<vtkFloatArray> pbNormalForce = vtkSmartPointer<vtkFloatArray>::New();
+ pbNormalForce->SetNumberOfComponents(3);
+ pbNormalForce->SetName("normalForce"); //Linear velocity in Vector3 form
+ vtkSmartPointer<vtkFloatArray> pbShearForce = vtkSmartPointer<vtkFloatArray>::New();
+ pbShearForce->SetNumberOfComponents(3);
+ pbShearForce->SetName("shearForce"); //Angular velocity in Vector3 form
+ // interactions contact point###############################################
+
+
+ // velocity ###################################################
+ vtkSmartPointer<vtkCellArray> pbCells = vtkSmartPointer<vtkCellArray>::New();
+ 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()];
+ vtkSmartPointer<vtkLinearExtrusionFilter> extrude[scene->bodies->size()];
+ vtkSmartPointer<vtkActor> textActor[scene->bodies->size()];
+
+
+ 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());
+ if(!pb) continue;
+
+ if(REC_ID==true){
+ //#if 0
+ 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++;
+ }
+ //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->rotationMatrix[count] = directionCos(j,i);
+ function->rotationMatrix(i,j) = directionCos(j,i);
+ count++;
+ }
+ }
+
+
+ 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){
+ ymax = 0.0;
+ ymin = 0.0;
+ }
+ 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;}
+ 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);
+ 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); }
+ 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);
+ }
+ 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];
+ 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();
+ appendFilter->AddInputConnection(transformFilter-> GetOutputPort());
+
+
+ // ################## velocity ####################
+ if(REC_VELOCITY == true){
+ vtkIdType pid[1];
+ Vector3r pos(b->state->pos);
+ pid[0] = pbPos->InsertNextPoint(pos[0], pos[1], pos[2]);
+ pbCells->InsertNextCell(1,pid);
+ const Vector3r& vel = b->state->vel;
+ float v[3]; //v = { vel[0],vel[1],vel[2] };
+ v[0]=vel[0];
+ v[1]=vel[1];
+ v[2]=vel[2];
+ pbLinVelVec->InsertNextTupleValue(v);
+ pbLinVelLen->InsertNextValue(vel.norm());
+ const Vector3r& angVel = b->state->angVel;
+ float av[3]; //av = { angVel[0],angVel[1],angVel[2] };
+ av[0]=angVel[0];
+ av[1]=angVel[1];
+ av[2]=angVel[2];
+ pbAngVelVec->InsertNextTupleValue(av);
+ pbAngVelLen->InsertNextValue(angVel.norm());
+ }
+ // ################ velocity ###########################
+ polydata->DeleteCells();
+ sample->Delete();
+ contours->Delete();
+ //function->Delete();
+ sample = NULL;
+ 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();
+ }
+
+ //###################### 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 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();
+ }
+ }
+//#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();
+ //intrForceN->Delete();
+ //intrAbsForceT->Delete();
+ //pbContactPoint->Delete();
+ //pbCellsContact->Delete();
+ //pbNormalForce->Delete();
+ //pbShearForce->Delete();
+ //pbCells->Delete();
+ //pbLinVelVec->Delete();
+ //pbLinVelLen->Delete();
+ //pbAngVelVec->Delete();
+ //pbAngVelLen->Delete();
+
+}
+
+YADE_PLUGIN(/*(Gl1_PotentialParticle)*/(PotentialParticleVTKRecorder));
+
+
+//YADE_REQUIRE_FEATURE(OPENGL)
+
+
+
+
+
+
+
+
+
+
=== added file 'pkg/common/Gl1_PotentialParticle.hpp'
--- pkg/common/Gl1_PotentialParticle.hpp 1970-01-01 00:00:00 +0000
+++ pkg/common/Gl1_PotentialParticle.hpp 2015-12-14 14:25:08 +0000
@@ -0,0 +1,172 @@
+/*CWBoon 2015 */
+#pragma once
+#include<pkg/dem/PotentialParticle.hpp>
+#include<pkg/dem/PotentialParticle2AABB.hpp>
+#include<pkg/common/GLDrawFunctors.hpp>
+//#include<pkg/dem/MarchingCube.hpp>
+#include <vector>
+#include <pkg/common/PeriodicEngines.hpp>
+
+#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 <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 <vtkActor.h>
+#include <vtkAppendPolyData.h>
+
+#include <vtkTextActor.h>
+#include <vtkTextProperty.h>
+#include <vtkVectorText.h>
+#include <vtkLinearExtrusionFilter.h>
+#include <vtkTextActor3D.h>
+#include <vtkCylinderSource.h>
+
+
+
+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
+};
+
+#if 0
+class Gl1_PotentialParticle : public GlShapeFunctor
+{
+ private :
+ MarchingCube mc;
+ Vector3r min,max;
+ vector<vector<vector<float > > > scalarField,weights;
+ void generateScalarField(const shared_ptr<Shape>& cm);
+ void calcMinMax(const shared_ptr<Shape>& cm);
+ float oldIsoValue,oldIsoSec,oldIsoThick;
+ Vector3r isoStep;
+
+
+
+ public :
+ struct Leaf{
+ Vector3r centre;
+ 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;
+ };
+ 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"))
+
+ );
+
+
+
+};
+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();
+ ,
+
+ );
+};
+REGISTER_SERIALIZABLE(PotentialParticleVTKRecorder);
+
+
+
+
+
+
+
+
+
+
=== added file 'pkg/dem/Ig2_PP_PP_ScGeom.cpp'
--- pkg/dem/Ig2_PP_PP_ScGeom.cpp 1970-01-01 00:00:00 +0000
+++ pkg/dem/Ig2_PP_PP_ScGeom.cpp 2015-12-14 14:25:08 +0000
@@ -0,0 +1,1713 @@
+/*CWBoon 2015 */
+/* 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>
+
+#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/QR>
+
+#include <ctime>
+#include <cstdlib>
+
+
+
+
+
+YADE_PLUGIN((Ig2_PP_PP_ScGeom)
+//#ifdef YADE_OPENGL
+// (Gl1_Ig2_PP_PP_ScGeom)
+// #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)
+{
+
+
+ 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);
+ 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;
+ 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 ptOnP2(0.0,0.0,0.0);
+ bool converge = false;
+
+
+ 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;}
+ contactPt = scm->contactPoint;
+ }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;
+ }
+
+
+ converge = true;
+ fA= evaluatePP(cm1,state1, contactPt);
+ fB = evaluatePP(cm2,state2, contactPt);
+
+//Default does not have warmstart
+ //if(fA < 0.0 && fB <0.0){
+ // converge = customSolve(cm1,state1,cm2,state2,contactPt,true);
+ //}else{
+ 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
+ /* Mosek */
+ 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;
+}
+
+//std::cout<<"converge: "<<converge<<", fA: "<<fA<<", fB: "<<fB<<endl;
+
+ //timingDeltas->start();
+
+ if (fA < 0.0 && fB < 0.0 && converge == true){
+ contact = true;
+ }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 *//
+ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ 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;}
+
+ Vector3r step = avgNormal*stepBisection;
+ 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;
+
+ 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->normal = avgNormal;
+
+
+
+ }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);
+ }
+
+ return true;
+ }else{
+ scm->contactPoint = contactPt;scm->penetrationDepth=-1.0;scm->normal = Vector3r(0,0,0);
+ return false;
+ }
+
+ return false;
+}
+
+
+
+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;
+ Real q = 0.0;
+ Real r = 0.0;
+ Real s = 0.0;
+ Real c = 0.0;
+ Real d = 0.0;
+ Real e = 0.0;
+ Vector3r bracketRange = bracketB- bracketA;
+ Real fa = evaluatePP(cm1,state1, bracketA); //evaluateFNoSphere(cm1,state1, bracketA); //
+
+ Real fb = evaluatePP(cm1,state1, bracketB); //evaluateFNoSphere(cm1,state1, bracketB); //
+
+ 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;
+ }
+ }
+ 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{
+ 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;
+ 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){
+ q = -q;
+ }else{
+ p = -p;
+ }
+
+ 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{
+ d = m;
+ e = d;
+ }
+ }
+
+ a = b; fa = fb;
+ Real h = 0.0;
+ if(fabs(d) >tol){
+ h = d;
+ }else if(m > 0.0){
+ h = tol;
+ }else{
+ h = - tol;
+ }
+ b = b + h; // h*m_unitVec;
+
+ zero = bracketA + b*bracketRange;
+ fb = evaluatePP(cm1,state1, zero); //evaluateFNoSphere(cm1,state1, zero); //
+
+
+ }else{
+ zero = bracketA + b*bracketRange;
+ //std::cout<<"b: "<<b<<", m: "<<m<<", tol: "<<tol<<", c: "<<c<<", a: "<<a<<endl;
+ break;
+ }
+
+ counter++;
+ if(counter==10000){
+ std::cout<<"counter: "<<counter<<", m.norm: "<<m<<", fb: "<<fb<<endl;
+ }
+ }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){
+
+ PotentialParticle *s1=static_cast<PotentialParticle*>(cm1.get());
+ ///////////////////Transforming trial values to local frame of particles//////////////////
+ Vector3r tempP1 = newTrial - state1.pos;
+ /* Direction cosines */
+ //state1.ori.normalize();
+ 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;}
+ pSum2 += pow(plane,2);
+ }
+ 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 */
+ Real f = (1.0-k)*(pSum2/pow(r,2) - 1.0)+k*(sphere -1.0);
+ return f;
+}
+
+
+
+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;
+
+ /* Direction cosines */
+ 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;}
+ p.push_back(plane);
+ pSum2 += pow(p[i],2);
+ }
+ 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 */
+ 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++){
+ pdxSum += (s1->a[i]*p[i]);
+ pdySum += (s1->b[i]*p[i]);
+ pdzSum += (s1->c[i]*p[i]);
+ }
+
+ Real fdx = 2.0*(1.0-k) * pdxSum/pow(r,2) + 2.0*k*x/pow(R,2);
+ Real fdy = 2.0*(1.0-k) * pdySum/pow(r,2) + 2.0*k*y/pow(R,2);
+ Real fdz = 2.0*(1.0-k) * pdzSum/pow(r,2) + 2.0*k*z/pow(R,2);
+
+ ///////////////////// Assembling rotation matrix Qt for particle 1 //////////////////////////////
+ Matrix3r Q1=Eigen::Matrix3d::Zero();;
+ //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);
+
+ //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 /////////////////////////
+ Real Fdx = fdx * Q1(0,0) + fdy*Q1(1,0) + fdz*Q1(2,0);
+ 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){
+ 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;
+ }
+
+ return Vector3r(Fdx,Fdy,Fdz);
+}
+
+
+
+#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 );
+}
+#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());
+ 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 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;
+ }
+
+ }while(Mathr::Sign(fprevious)*Mathr::Sign(f)*1.0> 0.0 );
+ bracketA = ptOnParticle;
+ bracketB = ptOnParticle -step;
+ Vector3r zero(0,0,0);
+ BrentZeroSurf(cm1,state1,bracketA, bracketB, zero);
+ ptOnParticle = zero;
+ //if( fabs(f)>0.1){std::cout<<"getInitial point f:"<<f<<endl;}
+}
+
+
+
+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;
+//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*/
+#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];
+#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;
+// 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];
+
+ /*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];
+#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];
+
+ /*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*/
+#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;
+#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];
+//#if 0
+ #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);
+
+ //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));
+
+//#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;
+
+#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;
+ //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);
+
+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 */
+//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]);
+ }
+
+
+//timingDeltas->checkpoint("assemble H and g");
+
+//std::cout<<"before Chol, totalIter: "<<totalIter<<", s : "<<s<<", u1: "<<u1<<", u2: "<<u2<<", wLmincoeff: "<<minWL<<endl;
+
+/* 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)];
+ }
+ }
+//#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);
+ }
+ }
+#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);
+ }
+
+ 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;
+ }
+ }
+
+//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 );
+}
+
+
+
+
+
=== added file 'pkg/dem/Ig2_PP_PP_ScGeom.hpp'
--- pkg/dem/Ig2_PP_PP_ScGeom.hpp 1970-01-01 00:00:00 +0000
+++ pkg/dem/Ig2_PP_PP_ScGeom.hpp 2015-12-14 14:25:08 +0000
@@ -0,0 +1,128 @@
+/*CWBoon 2015 */
+/* 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 */
+
+#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>
+#include <stdio.h>
+
+# if 0
+#include <mosek.h>
+#include <math.h>
+#include <stdlib.h>
+#include <string.h>
+#endif
+
+
+class Ig2_PP_PP_ScGeom: public IGeomFunctor
+{
+
+ //protected:
+ //static std::ofstream output;
+#if 0
+ 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",
+ ((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;
+
+
+
+
+};
+
+REGISTER_SERIALIZABLE(Ig2_PP_PP_ScGeom);
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* 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);
+ // int ans;
+ // dgesv_(&varNo, &varNo2, H, &varNo3, pivot,g, &varNo4, &ans);
+ // return ans;
+ //}
+
+/* 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 */
+ 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 */
+ 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);
+
+ extern void daxpy_(const int *N, const double *da, double *dx, const int *incx, double *dy, const int *incy);
+
+ extern void dscal_(const int *N, const double *alpha, double *x, const int *incx);
+
+
+ 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
+
+
+
+
=== added file 'pkg/dem/KnKsLaw.cpp'
--- pkg/dem/KnKsLaw.cpp 1970-01-01 00:00:00 +0000
+++ pkg/dem/KnKsLaw.cpp 2015-12-14 14:25:08 +0000
@@ -0,0 +1,312 @@
+#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)
+
+);
+
+
+
+/********************** Law2_Dem3DofGeom_RockPMPhys_Rpm ****************************/
+CREATE_LOGGER(Law2_SCG_KnKsPhys_KnKsLaw);
+
+
+
+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();
+ State* de2 = Body::byId(id2,scene)->state.get();
+ Shape* shape1 = Body::byId(id1,scene)->shape.get();
+ Shape* shape2 = Body::byId(id2,scene)->shape.get();
+ PotentialParticle *s1=static_cast<PotentialParticle*>(shape1);
+ PotentialParticle *s2=static_cast<PotentialParticle*>(shape2);
+ 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 ){
+ if (neverErase) {
+ phys->shearForce = Vector3r::Zero();
+ phys->normalForce = Vector3r::Zero();
+ phys->normalViscous = Vector3r::Zero();
+ geom->normal = Vector3r::Zero();
+ phys->tensionBroken = true;
+ }else{
+ scene->interactions->requestErase(id1,id2);
+ return false;
+ }
+ 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;
+ //Linear elasticity giving "trial" shear force
+ Vector3r shift2(0,0,0);
+ 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)){
+ 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)){
+ phys->initialShearDir = phys->shearDir;
+ du = shearIncrement.norm();
+ if(fabs(phys->mobilizedShear)>0.99999){
+ phys->u_cumulative += du;
+ phys->cumulative_us += du;
+ }else{
+ phys->u_elastic +=du;
+ }
+ }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
+ phys->u_cumulative += du;
+ phys->cumulative_us += du;
+ }else{
+ phys->u_elastic +=du;
+ }
+ }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){
+ 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;
+ }
+ //}
+ 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 (neverErase) {
+ phys->shearForce = Vector3r::Zero();
+ phys->normalForce = Vector3r::Zero();
+ phys->normalViscous = Vector3r::Zero();
+ geom->normal = Vector3r::Zero();
+ phys->tensionBroken = true;
+ }else {
+ return false;
+ }
+ return true;
+ }
+
+
+
+
+ /*ORIGINAL */
+ Vector3r c1x = geom->contactPoint - de1->pos;
+ Vector3r c2x = geom->contactPoint - de2->pos;
+ incidentV = (de2->vel+de2->angVel.cross(c2x)) - (de1->vel+de1->angVel.cross(c1x));
+ incidentVn = geom->normal.dot(incidentV)*geom->normal; // contact normal velocity
+ incidentVs = incidentV-incidentVn; // contact shear velocity
+ shearIncrement=incidentVs*scene->dt;
+ if(!Talesnick){
+ double Ks=0.0;
+ if(phys->jointType == 3){
+ Ks = phys->ks_i*pow(phys->prevSigma,0.6);
+ }else{
+ Ks = phys->ks;
+ }
+ 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);
+ 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)
+ // Note: to compare with the analytical solution you provide cn and cs directly (since here we used a different method to define c_crit)
+ double cn = Cn_crit*phys->viscousDamping; // Damping normal coefficient
+ double cs = Cs_crit*phys->viscousDamping; // Damping tangential coefficient
+
+ // add normal viscous component if damping is included
+ 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();
+
+ /* 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;
+ 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){
+
+ 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{
+ 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;
+ }
+ }else{
+ maxFs = std::max( fN,0.0)*tan_effective_phi;
+ }
+ }
+ if( shearForce.norm() > maxFs ){
+ Real ratio = maxFs / shearForce.norm();
+ shearForce *= ratio;
+ dampedShearForce = shearForce;
+ if(allowBreakage == true){
+ phys->cohesionBroken = true;
+ }
+ phys->shearViscous = Vector3r(0,0,0);
+ }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;}
+
+
+
+ //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())){
+ 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;
+ scene->forces.addTorque(id1,c1x.cross(force));
+ scene->forces.addTorque(id2,-(c2x).cross(force));
+
+ phys->prevNormal = geom->normal;
+
+ return true;
+
+}
+
+
+
+CREATE_LOGGER(Ip2_FrictMat_FrictMat_KnKsPhys);
+
+
+
+
+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;
+
+ 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());
+ //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;
+
+ /* Pass values calculated from above to CSPhys */
+ contactPhysics->viscousDamping = viscousDamping;
+ contactPhysics->useOverlapVol = useOverlapVol;
+ contactPhysics->kn = Kn;
+ contactPhysics->ks = Ks;
+ contactPhysics->kn_i = Kn;
+ contactPhysics->ks_i = Ks;
+ contactPhysics->u_peak = u_peak;
+ contactPhysics->maxClosure = maxClosure;
+ contactPhysics->cohesionBroken = cohesionBroken;
+ contactPhysics->tensionBroken = tensionBroken;
+ contactPhysics->unitWidth2D = unitWidth2D;
+ 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->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!
+ contactPhysics->calJointLength = calJointLength;
+ contactPhysics->twoDimension = twoDimension;
+ contactPhysics->useFaceProperties = useFaceProperties;
+ contactPhysics->brittleLength = brittleLength;
+ interaction->phys = contactPhysics;
+
+}
+
+CREATE_LOGGER(KnKsPhys);
+/* KnKsPhys */
+KnKsPhys::~KnKsPhys(){}
+
+
=== added file 'pkg/dem/KnKsLaw.hpp'
--- pkg/dem/KnKsLaw.hpp 1970-01-01 00:00:00 +0000
+++ pkg/dem/KnKsLaw.hpp 2015-12-14 14:25:08 +0000
@@ -0,0 +1,194 @@
+#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>
+#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"))
+ ((Real,frictionAngle,0.0,,"fric angle"))
+ ((Real,tanFrictionAngle,0.0,,"tangent of fric angle"))
+ ((Vector3r,contactDetectionPt,Vector3r(0,0,0),,"contact detection result"))
+ ((Real, viscousDamping, 0.8, ,"viscousDamping"))
+ ((Real, unitWidth2D, 1.0, ,"viscousDamping"))
+ ((Real, maxClosure, 0.0002, ,"vmi"))
+ ((Real, u_peak, 0.05, ,"peak shear displacement"))
+ ((Real, u_elastic, 0.0, ,"elastic shear displacement"))
+ ((double, brittleLength, 5.0, ,"brittle rock"))
+ ((double, kn_i, 5.0, ,"initial normal stiffness"))
+ ((double, ks_i, 5.0, ,"initial shear stiffness"))
+ ((Vector3r, normalViscous, Vector3r(0,0,0), ,"viscousDamping"))
+ ((Vector3r, shearViscous, Vector3r(0,0,0), ,"viscousDamping"))
+ ((double, hwater, 0.0, ,"height of pore water"))
+ ((bool, rockJointContact, false, ,"brittle rock"))
+ ((bool, intactRock, false, ,"brittle rock"))
+ ((int, jointType, 0, ,"jointType"))
+ ((Real,Kshear_area,0.0,,"kshear area"))
+ ((Real,Knormal_area, 0.0,,"knormal area"))
+ ((Vector3r, shearDir, Vector3r(0,0,0), ,"shear direction"))
+ ((vector<Vector3r>, shearForces, , ,"shear direction"))
+ ((Vector3r, shear, Vector3r(0,0,0),, "shear displacement"))
+ ((Vector3r, prevNormal, Vector3r(0.0,0.0,0.0),, "previous Normal"))
+ ((Vector3r, normal, Vector3r(0.0,0.0,0.0),, " normalVector"))
+ ((vector<Vector3r>, pointsArea, ,, "intermediate contact points"))
+ ((vector<Vector3r>, pointsShear, ,, "points to calculate shear"))
+ ((vector<double>, areaShear, ,, "area to attribute shear"))
+ ((vector<double>, overlapDistances, ,, "area to attribute shear"))
+ ((Real, finalSize, 0.0,, "finalgridsize"))
+ ((int, finalGridNo, 0,, "final number of grids"))
+ ((vector<double>, dualityGap, , ,"duality gap for SOCP"))
+ ((bool, warmstart, false, ,"warmstart for SOCP"))
+ ((int, generation, 0,, "number of subdivisions"))
+ ((int, triNoMain, 24,, "number of subdivisions"))
+ ((int, triNoSub, 6,, "number of subdivisions"))
+ ((Vector3r, initial1, Vector3r(0.0,0.0,0.0),, "midpoint"))
+ ((Vector3r, ptOnP1, Vector3r(0.0,0.0,0.0),, "pt on particle"))
+ ((Vector3r, ptOnP2, Vector3r(0.0,0.0,0.0),, " pt on particle 2"))
+ ((vector<bool>, redundantA, , ,"activePlanes for interaction.id1"))
+ ((vector<bool>, redundantB, , ,"activePlanes for interaction.id1"))
+ ((vector<bool>, activePlanes1, , ,"activePlanes for interaction.id1"))
+ ((vector<bool>, activePlanes2, , ,"activePlanes for interaction.id2"))
+ ((vector<Real>, activeA1, , ,"activePlanes for interaction.id2"))
+ ((vector<Real>, activeB1, , ,"activePlanes for interaction.id2"))
+ ((vector<Real>, activeC1, , ,"activePlanes for interaction.id2"))
+ ((vector<Real>, activeD1, , ,"activePlanes for interaction.id2"))
+ ((vector<Real>, activeA2, , ,"activePlanes for interaction.id2"))
+ ((vector<Real>, activeB2, , ,"activePlanes for interaction.id2"))
+ ((vector<Real>, activeC2, , ,"activePlanes for interaction.id2"))
+ ((vector<Real>, activeD2, , ,"activePlanes for interaction.id2"))
+ ((int, noActive1, 0, ,"activePlanes for interaction.id1"))
+ ((int, noActive2,0 , ,"activePlanes for interaction.id2"))
+ ((int, smallerID,1 , ,"id of particle with smaller plane"))
+ ((Real, cumulative_us, 0.0,, "cumulative translation"))
+ ((Real, cumulativeRotation, 0.0,, "cumulative rotation"))
+ ((Real, mobilizedShear, , ,"mobilizedShear"))
+ ((Real, contactArea, 0.0, ,"contactArea"))
+ ((Real, radCurvFace, , ,"face"))
+ ((double, prevJointLength, 0.0, ,"previous joint length"))
+ ((Real, radCurvCorner, , ,"corners"))
+ ((Real, prevSigma,0.0 , ,"previous normal stress"))
+ ((vector<Real>, prevSigmaList,0.0 , ,"previous normal stress"))
+ ((bool, calJointLength, false,, "calculate joint length"))
+ ((bool, useOverlapVol, false,, "calculate overlap volume"))
+ ((bool, calContactArea, false,, "calculate contact area"))
+ ((double, jointLength, 0.0,, "approximatedJointLength"))
+ ((double, shearIncrementForCD, 0.0,, "toSeeWhether it is necessary to update contactArea"))
+ ((Real, overlappingVol,0.0 , ,"overlapping vol"))
+ ((Real, overlappingVolMulti,0.0 , ,"overlapping vol"))
+ ((double, gap_normalized, 0.0,, "distance between particles normalized by particle size. Estimated using Taubin Distance "))
+ ((double, gap, 0.0,, "distance between particles normalized by particle size. Estimated using Taubin Distance "))
+ ((bool, findCurv, false,, "to get radius of curvature"))
+ ((bool, useFaceProperties, false,,"boolean to get face properites"))
+ ((Real, cohesion, 0.0, ,"cohesion"))
+ ((Real, tension, 0.0, ,"tension"))
+ ((bool, cohesionBroken, true, ,"cohesion already broken"))
+ ((bool, tensionBroken, true, ,"tension already broken"))
+ ((bool, twoDimension, false, ,"tension already broken"))
+ ((Real, phi_b, 0.0, ,"basic friction angle (degrees)"))
+ ((Real, phi_r, 0.0, ,"residual friction angle (degrees)"))
+ ((Real, asperity, 3.0, ,"asperity height"))
+ ((Real, JRC, 0.0, ,"Joint Roughness Coefficient"))
+ ((Real, JRCmobilized, 0.0, ,"Joint Roughness Coefficient"))
+ ((Real, JCS, 0.0, ,"Joint Roughness Coefficient"))
+ ((Real, sigmaC,0.0 , ,"Joint Roughness Coefficient"))
+ ((Real, u_dilate,0.0 , ,"dilation distance"))
+ ((Real, dilation_angle,0.0 , ,"dilation distance"))
+ /* pore water pressure */
+ ((Real, lambda0,0.0 , ,"initial pore water pressure to stress ratio"))
+ ((Real, lambda_present,0.0 , ,"Voight&Faust (1992) Sitar et al. (2005)"))
+ ((Real, u_cumulative, 0.0,, "cumulative translation"))
+ ((Vector3r, prevShearDir, Vector3r(0.0,0.0,0.0),, "previous shear direction"))
+ ((Vector3r, initialShearDir, Vector3r(0.0,0.0,0.0),, "previous shear direction"))
+ ((double, delta_porePressure, 0.0,, "change in pore water pressure"))
+ ((double, porePressure, 0.0,, "pore water pressure"))
+ ((double, bandThickness, 0.1,, "clay layer thickness"))
+ ((double, heatCapacities, 0.0,, "clay layer thickness"))
+ ((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_SERIALIZABLE(KnKsPhys);
+
+
+
+
+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"))
+ );
+ FUNCTOR2D(FrictMat,FrictMat);
+ DECLARE_LOGGER;
+};
+REGISTER_SERIALIZABLE(Ip2_FrictMat_FrictMat_KnKsPhys);
+
+
+
+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"))
+ ,,
+
+ );
+
+ DECLARE_LOGGER;
+};
+REGISTER_SERIALIZABLE(Law2_SCG_KnKsPhys_KnKsLaw);
+
+
+
=== added file 'pkg/dem/PotentialParticle.cpp'
--- pkg/dem/PotentialParticle.cpp 1970-01-01 00:00:00 +0000
+++ pkg/dem/PotentialParticle.cpp 2015-12-14 14:25:08 +0000
@@ -0,0 +1,14 @@
+/*CWBoon 2015 */
+
+ //! To implement potential particles (Houlsby 2009) using sphere
+#include "PotentialParticle.hpp"
+
+YADE_PLUGIN((PotentialParticle));
+
+
+PotentialParticle::~PotentialParticle()
+{
+}
+
+
+
=== added file 'pkg/dem/PotentialParticle.hpp'
--- pkg/dem/PotentialParticle.hpp 1970-01-01 00:00:00 +0000
+++ pkg/dem/PotentialParticle.hpp 2015-12-14 14:25:08 +0000
@@ -0,0 +1,70 @@
+/*CWBoon 2015 */
+
+#pragma once
+
+#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
+{
+
+ 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];
+ Dmatrix(i,0) = d[i] + r;
+ }
+ #endif
+
+ );
+ //#endif
+
+ REGISTER_CLASS_INDEX(PotentialParticle,Shape);
+
+};
+}
+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);
+#ifdef __cplusplus
+};
+#endif
+
=== added file 'pkg/dem/PotentialParticle2AABB.cpp'
--- pkg/dem/PotentialParticle2AABB.cpp 1970-01-01 00:00:00 +0000
+++ pkg/dem/PotentialParticle2AABB.cpp 2015-12-14 14:25:08 +0000
@@ -0,0 +1,56 @@
+
+/*CWBoon 2015 */
+
+#include "PotentialParticle2AABB.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*){
+ PotentialParticle* pp = static_cast<PotentialParticle*>(cm.get());
+ if(!bv){ bv=shared_ptr<Bound>(new Aabb); }
+ Aabb* aabb = static_cast<Aabb*>(bv.get());
+ Matrix3r r = se3.orientation.toRotationMatrix();
+
+ if(pp->AabbMinMax == false){
+ Real distFromCentre = 1.05*pp->R; // std::max(maxD, pp->R);
+ halfSize = Vector3r(distFromCentre,distFromCentre,distFromCentre);
+ aabb->min = se3.position-halfSize;
+ aabb->max = se3.position+halfSize;
+ return;
+ }else{
+ Matrix3r r=se3.orientation.toRotationMatrix();
+ 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]));
+ pp->vertices.push_back(Vector3r(-pp->minAabbRotated[0],-pp->minAabbRotated[1],pp->maxAabbRotated[2]));
+ pp->vertices.push_back(Vector3r(-pp->minAabbRotated[0],-pp->minAabbRotated[1],-pp->minAabbRotated[2]));
+ pp->vertices.push_back(Vector3r(-pp->minAabbRotated[0],pp->maxAabbRotated[1],pp->maxAabbRotated[2]));
+ pp->vertices.push_back(Vector3r(-pp->minAabbRotated[0],pp->maxAabbRotated[1],-pp->minAabbRotated[2]));
+ pp->vertices.push_back(Vector3r(pp->maxAabbRotated[0],-pp->minAabbRotated[1],pp->maxAabbRotated[2]));
+ pp->vertices.push_back(Vector3r(pp->maxAabbRotated[0],-pp->minAabbRotated[1],-pp->minAabbRotated[2]));
+ }
+ Vector3r aabbMin(0,0,0);
+ Vector3r aabbMax(0,0,0);
+ 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(); }
+ }
+ 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));
=== added file 'pkg/dem/PotentialParticle2AABB.hpp'
--- pkg/dem/PotentialParticle2AABB.hpp 1970-01-01 00:00:00 +0000
+++ pkg/dem/PotentialParticle2AABB.hpp 2015-12-14 14:25:08 +0000
@@ -0,0 +1,24 @@
+/*CWBoon 2015 */
+
+#pragma once
+
+#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"))
+
+ );
+};
+
+REGISTER_SERIALIZABLE(PotentialParticle2AABB);
+
