Commit 244394e9 authored by Václav Šmilauer's avatar Václav Šmilauer
Browse files

1. Change Wm3 interface so that it is eigen-compatible. Please let me know if...

1. Change Wm3 interface so that it is eigen-compatible. Please let me know if there are regressions.
2. Fix body selection with python when no onBodySelect callback is defined
parent aab18ddb
......@@ -331,8 +331,8 @@ if not env.GetOption('clean'):
def CheckLib_maybeMT(conf,lib,header,lang,func): return conf.CheckLibWithHeader(lib+'-mt',['limits.h',header],'c++',func,autoadd=1) or conf.CheckLibWithHeader(lib,['limits.h',header],lang,func,autoadd=1)
# in boost::filesystem>=1.35 depends on boost::system being explicitly linked to; if not present, we are probably in <=1.34, where boost::system didn't exist and wasn't needed either
CheckLib_maybeMT(conf,'boost_system','boost/system/error_code.hpp','c++','boost::system::error_code(); /* non-essential */')
ok&=CheckLib_maybeMT(conf,'boost_thread','boost/thread/thread.hpp','c++','boost::thread::thread();')
ok&=CheckLib_maybeMT(conf,'boost_date_time','boost/date_time/posix_time/posix_time.hpp','c++','boost::posix_time::time_duration::time_duration();')
ok&=CheckLib_maybeMT(conf,'boost_thread','boost/thread/thread.hpp','c++','boost::thread();')
ok&=CheckLib_maybeMT(conf,'boost_date_time','boost/date_time/posix_time/posix_time.hpp','c++','boost::posix_time::time_duration();')
ok&=CheckLib_maybeMT(conf,'boost_filesystem','boost/filesystem/path.hpp','c++','boost::filesystem::path();')
ok&=CheckLib_maybeMT(conf,'boost_iostreams','boost/iostreams/device/file.hpp','c++','boost::iostreams::file_sink("");')
ok&=CheckLib_maybeMT(conf,'boost_regex','boost/regex.hpp','c++','boost::regex("");')
......
......@@ -3,7 +3,8 @@
#include<yade/pkg-common/PeriodicEngines.hpp>
#include<yade/lib-pyutil/gil.hpp>
YADE_PLUGIN((Se3Interpolator))CREATE_LOGGER(Se3Interpolator);
YADE_PLUGIN((Se3Interpolator))
CREATE_LOGGER(Se3Interpolator);
void Se3Interpolator::action(Scene* mb){
assert(subscribedBodies.size()==1);
......
......@@ -7,11 +7,12 @@ void Cell::integrateAndUpdate(Real dt){
// total transformation; M = (Id+G).M = F.M
trsf+=_trsfInc*trsf;
// Hsize will contain colums with transformed base vectors
Matrix3r Hsize(refSize[0],refSize[1],refSize[2]); Hsize=trsf*Hsize;
Matrix3r Hsize(Matrix3r::Zero()); Hsize(0,0)=refSize[0]; Hsize(1,1)=refSize[1]; Hsize(2,2)=refSize[2]; // later with eigen: Hsize=Matrix::Zero(); Hsize.diagonal=refSize;
Hsize=trsf*Hsize;
// lengths of transformed cell vectors, skew cosines
Matrix3r Hnorm; // normalized transformed base vectors
for(int i=0; i<3; i++){
Vector3r base(Hsize.GetColumn(i));
Vector3r base(Hsize.col(i));
_size[i]=base.norm(); base/=_size[i]; //base is normalized now
// was: Hnorm.SetColumn(i,base);
// with eigen: Hnorm.col(i)=base;
......@@ -23,7 +24,7 @@ void Cell::integrateAndUpdate(Real dt){
for(int i=0; i<3; i++){
int i1=(i+1)%3, i2=(i+2)%3;
// sin between axes is cos of skew
_cos[i]=(Hnorm.GetColumn(i1).cross(Hnorm.GetColumn(i2))).squaredNorm();
_cos[i]=(Hnorm.col(i1).cross(Hnorm.col(i2))).squaredNorm();
}
// pure shear trsf: ones on diagonal
_shearTrsf=Hnorm;
......
......@@ -2,10 +2,14 @@
# demonstration of the yade.post2d module (see its documentation for details)
#
import pylab # the matlab-like interface of matplotlib
pylab.ioff()
import numpy
import os.path
# run uniax.py to get this file
O.load('/tmp/uniax-tension.xml.bz2')
loadFile='/tmp/uniax-tension.xml.bz2'
if not os.path.exists(loadFile): raise RuntimeError("Run uniax.py first so that %s is created"%loadFile)
O.load(loadFile)
# axis normal to the plane in which we do the histogram
axis=0 # x, i.e. plot the yz plane
......@@ -13,15 +17,17 @@ ax1,ax2=(axis+1)%3,(axis+2)%3 ## get the other two indices, i.e. 1 and 2 in this
angles,forces=[],[]
for i in O.interactions:
if not i.isReal: continue
norm=i.geom['normal']
norm=i.geom.normal
angle=atan(norm[ax2]/norm[ax1])
force=i.phys['normalForce'].Length()
force=i.phys.normalForce.Length()
angles.append(angle)
forces.append(force)
# easier: plain histogram
pylab.hist(angles,weights=forces,bins=20)
pylab.show()
# polar histogram
pylab.figure()
## prepare data
values,bins=numpy.histogram(angles,weights=forces,bins=20)
## prepare polar plot
......@@ -29,4 +35,10 @@ pylab.subplot(111,polar=True);
## plot bar chart, with the histogram data
### bins has one edge extra, remove it: [:-1]
pylab.bar(left=bins[:-1],height=values,width=.7*pi/20);
# predefined function
pylab.figure()
utils.plotDirections()
pylab.show()
......@@ -139,33 +139,33 @@ def initTest():
maxStrainRate[ax2]=1000*maxStrainRate[axis]
goal[axis]=1 if mode=='tension' else -1;
goal[ax1]=goal[axis]
strainer['maxStrainRate']=maxStrainRate
strainer['goal']=goal
strainer.maxStrainRate=maxStrainRate
strainer.goal=goal
try:
from yade import qt
renderer=qt.Renderer()
renderer['scaleDisplacements']=True
renderer['displacementScale']=(1000,1000,1000) if mode=='tension' else (100,100,100)
renderer.scaleDisplacements=True
renderer.displacementScale=(1000,1000,1000) if mode=='tension' else (100,100,100)
except ImportError: pass
print "init done, will now run."
O.step(); O.step(); # to create initial contacts
# now reset the interaction radius and go ahead
ss2d3dg['distFactor']=-1.
is2aabb['aabbEnlargeFactor']=-1.
ss2d3dg.distFactor=-1.
is2aabb.aabbEnlargeFactor=-1.
O.run()
def stopIfDamaged():
global mode
if O.iter<2 or not plot.data.has_key('sigma'): return # do nothing at the very beginning
sigma,eps=plot.data['sigma'],plot.data['eps']
extremum=max(sigma) if (strainer['maxStrainRate']>0) else min(sigma)
extremum=max(sigma) if (strainer.maxStrainRate>0) else min(sigma)
# FIXME: only temporary, should be .5
minMaxRatio=0.5 if mode=='tension' else 0.5
if extremum==0: return
print O.tags['id'],mode,strainer['strain'][axis],sigma[-1]
print O.tags['id'],mode,strainer.strain[axis],sigma[-1]
#print 'strain',strainer['strain'],'stress',strainer['stress']
import sys; sys.stdout.flush()
if abs(sigma[-1]/extremum)<minMaxRatio or abs(strainer['strain'][axis])>6e-3:
if abs(sigma[-1]/extremum)<minMaxRatio or abs(strainer.strain[axis])>6e-3:
if mode=='tension' and doModes & 2: # only if compression is enabled
mode='compression'
#O.save('/tmp/uniax-tension.xml.bz2')
......@@ -186,7 +186,7 @@ def stopIfDamaged():
sys.exit(0)
def addPlotData():
yade.plot.addData(t=O.time,i=O.iter,eps=strainer['strain'][axis],eps_=strainer['strain'][axis],sigma=strainer['stress'][axis]+isoPrestress,eps1=strainer['strain'][ax1],eps2=strainer['strain'][ax2],sig1=strainer['stress'][ax1],sig2=strainer['stress'][ax2],relResid=updater['avgRelResidual'])
yade.plot.addData(t=O.time,i=O.iter,eps=strainer.strain[axis],eps_=strainer.strain[axis],sigma=strainer.stress[axis]+isoPrestress,eps1=strainer.strain[ax1],eps2=strainer.strain[ax2],sig1=strainer.stress[ax1],sig2=strainer.stress[ax2],relResid=updater.avgRelResidual)
initTest()
# sleep forever if run by yade-multi, exit is called from stopIfDamaged
......
......@@ -4,8 +4,9 @@
from yade import post2d
import pylab # the matlab-like interface of matplotlib
# run uniax.py to get this file
O.load('/tmp/uniax-tension.xml.bz2')
loadFile='/tmp/uniax-tension.xml.bz2'
if not os.path.exists(loadFile): raise RuntimeError("Run uniax.py first so that %s is created"%loadFile)
O.load(loadFile)
# flattener that project to the xz plane
flattener=post2d.AxisFlatten(useRef=False,axis=1)
......
......@@ -72,8 +72,8 @@ if 'description' in O.tags.keys(): O.tags['id']=O.tags['id']+O.tags['description
# using spheres 7mm of diameter
concreteId=O.materials.append(CpmMat(young=young,frictionAngle=frictionAngle,poisson=poisson,density=4800,sigmaT=sigmaT,relDuctility=relDuctility,epsCrackOnset=epsCrackOnset,G_over_E=G_over_E,isoPrestress=isoPrestress))
#spheres=pack.randomDensePack(pack.inHyperboloid((0,0,-.5*specimenLength),(0,0,.5*specimenLength),.25*specimenLength,.17*specimenLength),spheresInCell=2000,radius=sphereRadius,memoizeDb='/tmp/triaxPackCache.sqlite',material=concreteId)
spheres=pack.randomDensePack(pack.inAlignedBox((-.25*specimenLength,-.25*specimenLength,-.5*specimenLength),(.25*specimenLength,.25*specimenLength,.5*specimenLength)),spheresInCell=2000,radius=sphereRadius,memoizeDb='/tmp/triaxPackCache.sqlite')
spheres=pack.randomDensePack(pack.inHyperboloid((0,0,-.5*specimenLength),(0,0,.5*specimenLength),.25*specimenLength,.17*specimenLength),spheresInCell=2000,radius=sphereRadius,memoizeDb='/tmp/triaxPackCache.sqlite',material=concreteId)
#spheres=pack.randomDensePack(pack.inAlignedBox((-.25*specimenLength,-.25*specimenLength,-.5*specimenLength),(.25*specimenLength,.25*specimenLength,.5*specimenLength)),spheresInCell=2000,radius=sphereRadius,memoizeDb='/tmp/triaxPackCache.sqlite')
O.bodies.append(spheres)
bb=utils.uniaxialTestFeatures()
negIds,posIds,axis,crossSectionArea=bb['negIds'],bb['posIds'],bb['axis'],bb['area']
......@@ -105,8 +105,7 @@ O.engines=[
plot.plots={'eps':('sigma',)} #'sigma.25','sigma.50','sigma.75')}
plot.maxDataLen=4000
#O.saveTmp('initial');
O.save('/tmp/init.xml')
O.saveTmp('initial');
O.timingEnabled=False
......@@ -118,8 +117,7 @@ def initTest():
print "init"
if O.iter>0:
O.wait();
#O.loadTmp('initial')
O.load('/tmp/init.xml')
O.loadTmp('initial')
print "Reversing plot data"; plot.reverseData()
strainer.strainRate=abs(strainRateTension) if mode=='tension' else -abs(strainRateCompression)
try:
......@@ -148,7 +146,8 @@ def stopIfDamaged():
if abs(sigma[-1]/extremum)<minMaxRatio or abs(strainer.strain)>(5e-3 if isoPrestress==0 else 5e-2):
if mode=='tension' and doModes & 2: # only if compression is enabled
mode='compression'
#O.save('/tmp/uniax-tension.xml.bz2')
O.save('/tmp/uniax-tension.xml.bz2')
print "Saved /tmp/uniax-tension.xml.bz2 (for use with interaction-histogram.py and uniax-post.py)"
print "Damaged, switching to compression... "; O.pause()
# important! initTest must be launched in a separate thread;
# otherwise O.load would wait for the iteration to finish,
......
......@@ -283,7 +283,7 @@ void GLViewer::keyPressEvent(QKeyEvent *e)
if(manipulatedClipPlane>=0 && manipulatedClipPlane<renderer->numClipPlanes){
/* here, we must update both manipulatedFrame orientation and renderer->clipPlaneSe3 orientation in the same way */
Quaternionr& ori=renderer->clipPlaneSe3[manipulatedClipPlane].orientation;
ori=Quaternionr(Vector3r(0,1,0),Mathr::PI)*ori;
ori=Quaternionr(AngleAxisr(Mathr::PI,Vector3r(0,1,0)))*ori;
manipulatedFrame()->setOrientation(qglviewer::Quaternion(qglviewer::Vec(0,1,0),Mathr::PI)*manipulatedFrame()->orientation());
displayMessage("Plane #"+lexical_cast<string>(manipulatedClipPlane+1)+" reversed.");
}
......@@ -503,23 +503,22 @@ void GLViewer::postSelection(const QPoint& point)
setSelectedName(selection);
LOG_DEBUG("New selection "<<selection);
displayMessage("Selected body #"+lexical_cast<string>(selection)+(Body::byId(selection)->isClump()?" (clump)":""));
//wasDynamic=Body::byId(selection)->isDynamic;
//Body::byId(selection)->isDynamic = false;
Quaternionr& q = Body::byId(selection)->state->ori;
Vector3r& v = Body::byId(selection)->state->pos;
manipulatedFrame()->setPositionAndOrientation(qglviewer::Vec(v[0],v[1],v[2]),qglviewer::Quaternion(q.x(),q.y(),q.z(),q.w()));
Omega::instance().getScene()->selectedBody = selection;
try{
PyGILState_STATE gstate;
gstate = PyGILState_Ensure();
gstate = PyGILState_Ensure();
python::object main=python::import("__main__");
python::object global=main.attr("__dict__");
python::eval(string("onBodySelect("+lexical_cast<string>(selection)+")").c_str(),global,global);
// the try/catch block must be properly nested inside PyGILState_Ensure and PyGILState_Release
try{
python::eval(string("onBodySelect("+lexical_cast<string>(selection)+")").c_str(),global,global);
} catch (python::error_already_set const &) {
LOG_DEBUG("unable to call onBodySelect. Not defined?");
}
PyGILState_Release(gstate);
// see https://svn.boost.org/trac/boost/ticket/2781 for exception handling
} catch (python::error_already_set const &) {
LOG_DEBUG("unable to call onBodySelect. Not defined?");
}
}
}
......@@ -553,7 +552,7 @@ void GLViewer::postDraw(){
Real scaleStep=pow(10,(floor(log10(displayedSceneRadius()*2)-.7))); // unconstrained
int nSegments=((int)(wholeDiameter/gridStep))+1;
Real realSize=nSegments*gridStep;
LOG_DEBUG("nSegments="<<nSegments<<",gridStep="<<gridStep<<",realSize="<<realSize);
//LOG_TRACE("nSegments="<<nSegments<<",gridStep="<<gridStep<<",realSize="<<realSize);
glPushMatrix();
nSegments *= 2; // there's an error in QGLViewer::drawGrid(), so we need to mitigate it by '* 2'
......
......@@ -53,7 +53,7 @@ if 'opengl' in env['features']:
yadeStaticOrSharedLib('yade-opengl',env.Combine('yade-opengl.cpp',['opengl/FpsTracker.cpp','opengl/GLTextLabel.cpp','opengl/GLWindow.cpp','opengl/GLWindowsManager.cpp','opengl/GLUtils.cpp']),LIBS=env['LIBS']+['glut','GL','$QGLVIEWER_LIB']),
#env.Install('$PREFIX/lib/yade$SUFFIX/lib',env.StaticLibrary
yadeStaticOrSharedLib('yade-support',[
env.Combine('yade-support.cpp',(['base/yadeEigenWrapper.cpp'] if 'nowm3' in env['features'] else ['base/yadeWm3Extra.cpp'])+
env.Combine('yade-support.cpp',
['factory/ClassFactory.cpp','serialization/SerializableSingleton.cpp','factory/DynLibManager.cpp','factory/FactoryExceptions.cpp',
'multimethods/Indexable.cpp','multimethods/MultiMethodsExceptions.cpp',
'serialization-xml/XMLFormatManager.cpp','serialization-xml/XMLSaxParser.cpp','serialization/Archive.cpp','serialization/IOFormatManager.cpp','serialization/IOManagerExceptions.cpp',
......
// © 2010 Václav Šmilauer <eudoxos@arcig.cz>
#pragma once
#ifdef QUAD_PRECISION
typedef long double quad;
typedef quad Real;
#else
typedef double Real;
#endif
#include<limits>
#include<cstdlib>
/*
* use Eigen http://eigen.tuxfamily.org
*/
#if defined(YADE_EIGEN) and defined(YADE_NOWM3)
/* eigen math wrapper that replaces wm3, use with caution
you MUST make symlinks from the wm3-eigen branch into this directory like this:
wm3-eigen/eigen/yadeMath.cpp → lib/base/yadeEigenWrapper.cpp
wm3-eigen/eigen/yadeMath.hpp → lib/base/yadeMath.hpp
wm3-eigen/eigen/yadeMath.inl → lib/base/yadeMath.inl
(names will be made more meaningful once this settles down)
good luck
*/
//#warning @@@@@@@@@@ Using wrapper around eigen to emulate wm3; this is EXPERIMENTAL for developers only; numerical results might very well be garbage. @@@@@@@@@@
#include<yade/lib-base/yadeMath.hpp>
// different macros for different versions of eigen:
// http://bitbucket.org/eigen/eigen/issue/96/eigen_dont_align-doesnt-exist-in-205-but-appears-in-web
#define EIGEN_DONT_VECTORIZE
#define EIGEN_DONT_ALIGN
#define EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT
#include<Eigen/Core>
USING_PART_OF_NAMESPACE_EIGEN
//using namespace eigen; // for eigen3
typedef Vector2<int> Vector2i;
typedef Vector2<Real> Vector2r;
typedef Vector3<int> Vector3i;
typedef Vector3<Real> Vector3r;
typedef Matrix3<Real> Matrix3r;
typedef Quaternion<Real> Quaternionr;
// io
template<class ScalarType> std::ostream & operator<<(std::ostream &os, const Vector2<ScalarType>& v){ os << v.x()<<" "<<v.y(); return os; };
template<class ScalarType> std::ostream & operator<<(std::ostream &os, const Vector3<ScalarType>& v){ os << v.x()<<" "<<v.y()<<" "<<v.z(); return os; };
template<class ScalarType> std::ostream & operator<<(std::ostream &os, const Quaternion<ScalarType>& q){ os<<q.w()<<" "<<q.x()<<" "<<q.y()<<" "<<q.z(); return os; };
// operators
//template<class ScalarType> Vector3<ScalarType> operator*(ScalarType s, const Vector3<ScalarType>& v) {return v*s;}
//template<class ScalarType> Matrix3<ScalarType> operator*(ScalarType s, const Matrix3<ScalarType>& m) { return m*s; }
//template<class ScalarType> Quaternion<ScalarType> operator*(ScalarType s, const Quaternion<ScalarType>& q) { return q*s; }
template<typename ScalarType> void matrixEigenDecomposition(const Matrix3r<ScalarType> m, Matrix3<ScalarType>& mRot, Matrix3<ScalarType>& mDiag){ Eigen::EigenSolver<Matrix3<ScalarType> > a(m); mDiag=a.eigenvalues().real().asDiagonal(); mRot=a.eigenvectors().real(); }
//__attribute__((warning("Replace this function with direct AngleAxis constructor from Quaternion")))
template<typename ScalarType> AngleAxis<ScalarType> angleAxisFromQuat(const Quaternion<ScalarType>& q){ return AngleAxis<ScalarType>(q); }
// http://eigen.tuxfamily.org/dox/TutorialGeometry.html
template<typename ScalarType> Matrix3<ScalarType> matrixFromEulerAnglesXYZ(ScalarType x, ScalarType y, ScalarType z){ Matrix3<ScalarType> ret=AngleAxis<ScalarType>(x,Vector3<ScalarType>::UnitX())*AngleAxis<ScalarType>(y,Vector3<ScalarType>::UnitY())*AngleAxis<ScalarType>(z,Vector3<ScalarType>::UnitZ()); return ret; }
#else
#include<yade/lib-base/yadeWm3Extra_dont_include_directly.hpp>
#include<Wm3Vector2.h>
#include<Wm3Vector3.h>
#include<Wm3Matrix3.h>
#include<Wm3Quaternion.h>
#include<Wm3Math.h>
namespace Wm3 {
template<class T> class Math; typedef Math<Real> Mathr;
template<class T> class Matrix3; typedef Matrix3<Real> Matrix3r;
template<class T> class Quaternion; typedef Quaternion<Real> Quaternionr;
template<class T> class Vector2; typedef Vector2<Real> Vector2r; typedef Vector2<int> Vector2i;
template<class T> class Vector3; typedef Vector3<Real> Vector3r; typedef Vector3<int> Vector3i;
template<class T> class AngleAxis; typedef AngleAxis<Real> AngleAxisr; // added to the Wm3Quaternion header
}
using namespace Wm3;
template<typename ScalarType> AngleAxis<ScalarType> angleAxisFromQuat(const Quaternion<ScalarType>& q){ AngleAxis<ScalarType> aa; q.ToAxisAngle_(aa.axis(),aa.angle()); return aa; }
template<typename ScalarType> void matrixEigenDecomposition(const Matrix3<ScalarType>& m, Matrix3<ScalarType>& mRot, Matrix3<ScalarType>& mDiag){ m.EigenDecomposition_(mRot,mDiag); }
template<typename ScalarType> Matrix3<ScalarType> matrixFromEulerAnglesXYZ(ScalarType x, ScalarType y, ScalarType z){ return Matrix3r().FromEulerAnglesXYZ(x,y,z); }
#endif
/*
* Compatibility bridge for Wm3 and eigen (will be removed once Wm3 is dropped and replaced by respective eigen constructs;
* see https://www.yade-dem.org/wiki/Wm3→Eigen
*/
// replace by outer product of 2 vectors: v1*v2.transpose();
template<typename RealType>
Matrix3<RealType> makeTensorProduct (const Vector3<RealType>& rkU, const Vector3<RealType>& rkV)
{
Matrix3<RealType> ret;
ret(0,0) = rkU[0]*rkV[0];
ret(0,1) = rkU[0]*rkV[1];
ret(0,2) = rkU[0]*rkV[2];
ret(1,0) = rkU[1]*rkV[0];
ret(1,1) = rkU[1]*rkV[1];
ret(1,2) = rkU[1]*rkV[2];
ret(2,0) = rkU[2]*rkV[0];
ret(2,1) = rkU[2]*rkV[1];
ret(2,2) = rkU[2]*rkV[2];
return ret;
}
// eigen2: v.cwise().min() / max()
// eigen3: v.array().min() / max()
template<typename ScalarType> Vector2<ScalarType> componentMaxVector(const Vector2<ScalarType>& a, const Vector2<ScalarType>& b){ return Vector2<ScalarType>(std::max(a.x(),b.x()),std::max(a.y(),b.y()));}
template<typename ScalarType> Vector2<ScalarType> componentMinVector(const Vector2<ScalarType>& a, const Vector2<ScalarType>& b){ return Vector2<ScalarType>(std::min(a.x(),b.x()),std::min(a.y(),b.y()));}
template<typename ScalarType> Vector3<ScalarType> componentMaxVector(const Vector3<ScalarType>& a, const Vector3<ScalarType>& b){ return Vector3<ScalarType>(std::max(a.x(),b.x()),std::max(a.y(),b.y()),std::max(a.z(),b.z()));}
template<typename ScalarType> Vector3<ScalarType> componentMinVector(const Vector3<ScalarType>& a, const Vector3<ScalarType>& b){ return Vector3<ScalarType>(std::min(a.x(),b.x()),std::min(a.y(),b.y()),std::min(a.z(),b.z()));}
// eigen2: v1.cwise()*v2;
// eigen3: v1.array()*v2.array()
template<typename ScalarType> Vector3<ScalarType> diagMult(const Vector3<ScalarType>& a, const Vector3<ScalarType>& b){return Vector3<ScalarType>(a.x()*b.x(),a.y()*b.y(),a.z()*b.z());}
template<typename ScalarType> Vector3<ScalarType> diagDiv(const Vector3<ScalarType>& a, const Vector3<ScalarType>& b){return Vector3<ScalarType>(a.x()/b.x(),a.y()/b.y(),a.z()/b.z());}
/*
* Extra yade math functions and classes
*/
__attribute__((unused))
const Real NaN(std::numeric_limits<Real>::signaling_NaN());
// void quaternionToEulerAngles (const Quaternionr& q, Vector3r& eulerAngles,Real threshold=1e-6f);
template<typename ScalarType> void quaterniontoGLMatrix(const Quaternion<ScalarType>& q, ScalarType m[16]){
ScalarType w2=2.*q.w(), x2=2.*q.x(), y2=2.*q.y(), z2=2.*q.z();
ScalarType x2w=w2*q.w(), y2w=y2*q.w(), z2w=z2*q.w();
ScalarType x2x=x2*q.x(), y2x=y2*q.x(), z2x=z2*q.x();
ScalarType x2y=y2*q.y(), y2y=y2*q.y(), z2y=z2*q.y();
ScalarType x2z=z2*q.z(), y2z=y2*q.z(), z2z=z2*q.z();
m[0]=1.-(y2y+z2z); m[4]=y2x-z2w; m[8]=z2x+y2w; m[12]=0;
m[1]=y2x+z2w; m[5]=1.-(x2x+z2z); m[9]=z2y-x2w; m[13]=0;
m[2]=z2x-y2w; m[6]=z2y+x2w; m[10]=1.-(x2x+y2y); m[14]=0;
m[3]=0.; m[7]=0.; m[11]=0.; m[15]=1.;
}
// se3
template <class RealType>
class Se3
{
public :
Vector3<RealType> position;
Quaternion<RealType> orientation;
Se3(){};
Se3(Vector3<RealType> rkP, Quaternion<RealType> qR){ position = rkP; orientation = qR; }
Se3(const Se3<RealType>& s){position = s.position;orientation = s.orientation;}
Se3(Se3<RealType>& a,Se3<RealType>& b){
position = b.orientation.inverse()*(a.position - b.position);
orientation = b.orientation.inverse()*a.orientation;
}
Se3<RealType> inverse(){ return Se3(-(orientation.inverse()*position), orientation.inverse());}
void toGLMatrix(float m[16]){ orientation.toGLMatrix(m); m[12] = position[0]; m[13] = position[1]; m[14] = position[2];}
Vector3<RealType> operator * (const Vector3<RealType>& b ){return orientation*b+position;}
Se3<RealType> operator * (const Quaternion<RealType>& b ){return Se3<RealType>(position , orientation*b);}
Se3<RealType> operator * (const Se3<RealType>& b ){return Se3<RealType>(orientation*b.position+position,orientation*b.orientation);}
};
// functions
template<typename ScalarType> ScalarType unitVectorsAngle(const Vector3<ScalarType>& a, const Vector3<ScalarType>& b){ return acos(a.dot(b)); }
// operators
template<class ScalarType> Vector3<ScalarType> operator*(const Quaternion<ScalarType>& q, const Vector3<ScalarType>& v){ return q.rotate(v); /* check whether it converts to matrix internally, which is supposedly faster*/ }
/*
* typedefs
*/
typedef Se3<Real> Se3r;
/*
* Serialization of math classes
*/
// gccxml chokes on the boost::serialization code; this part is not needed if wrapping miniWm3 anyway
#if !defined(__GCCXML__) and defined(YADE_BOOST_SERIALIZATION)
#include<boost/serialization/nvp.hpp>
#include<boost/serialization/is_bitwise_serializable.hpp>
// fast serialization (no version infor and no tracking) for basic math types
// http://www.boost.org/doc/libs/1_42_0/libs/serialization/doc/traits.html#bitwise
BOOST_IS_BITWISE_SERIALIZABLE(Vector2r);
BOOST_IS_BITWISE_SERIALIZABLE(Vector2<int>);
BOOST_IS_BITWISE_SERIALIZABLE(Vector3r);
BOOST_IS_BITWISE_SERIALIZABLE(Vector3<int>);
BOOST_IS_BITWISE_SERIALIZABLE(Quaternionr);
BOOST_IS_BITWISE_SERIALIZABLE(Se3r);
BOOST_IS_BITWISE_SERIALIZABLE(Matrix3r);
namespace boost {
namespace serialization {
template<class Archive>
void serialize(Archive & ar, Vector2r & g, const unsigned int version){
Real &x=g[0], &y=g[1];
ar & BOOST_SERIALIZATION_NVP(x) & BOOST_SERIALIZATION_NVP(y);
}
template<class Archive>
void serialize(Archive & ar, Vector2<int> & g, const unsigned int version){
int &x=g[0], &y=g[1];
ar & BOOST_SERIALIZATION_NVP(x) & BOOST_SERIALIZATION_NVP(y);
}
template<class Archive>
void serialize(Archive & ar, Vector3r & g, const unsigned int version)
{
Real &x=g[0], &y=g[1], &z=g[2];
ar & BOOST_SERIALIZATION_NVP(x) & BOOST_SERIALIZATION_NVP(y) & BOOST_SERIALIZATION_NVP(z);
}
template<class Archive>
void serialize(Archive & ar, Vector3<int> & g, const unsigned int version){
int &x=g[0], &y=g[1], &z=g[2];
ar & BOOST_SERIALIZATION_NVP(x) & BOOST_SERIALIZATION_NVP(y) & BOOST_SERIALIZATION_NVP(z);
}
template<class Archive>
void serialize(Archive & ar, Quaternionr & g, const unsigned int version)
{
Real &w=g.w(), &x=g.x(), &y=g.y(), &z=g.z();
ar & BOOST_SERIALIZATION_NVP(w) & BOOST_SERIALIZATION_NVP(x) & BOOST_SERIALIZATION_NVP(y) & BOOST_SERIALIZATION_NVP(z);
}
template<class Archive>
void serialize(Archive & ar, Se3r & g, const unsigned int version){
Vector3r& position=g.position; Quaternionr& orientation=g.orientation;
ar & BOOST_SERIALIZATION_NVP(position) & BOOST_SERIALIZATION_NVP(orientation);
}
template<class Archive>
void serialize(Archive & ar, Matrix3r & m, const unsigned int version){
Real &m00=m(0,0), &m01=m(0,1), &m02=m(0,2), &m10=m(1,0), &m11=m(1,1), &m12=m(1,2), &m20=m(2,0), &m21=m(2,1), &m22=m(2,2);
ar & BOOST_SERIALIZATION_NVP(m00) & BOOST_SERIALIZATION_NVP(m01) & BOOST_SERIALIZATION_NVP(m02) &
BOOST_SERIALIZATION_NVP(m10) & BOOST_SERIALIZATION_NVP(m11) & BOOST_SERIALIZATION_NVP(m12) &
BOOST_SERIALIZATION_NVP(m20) & BOOST_SERIALIZATION_NVP(m21) & BOOST_SERIALIZATION_NVP(m22);
}
} // namespace serialization
} // namespace boost
#endif /* __GCCXML */
#include "yadeWm3Extra_dont_include_directly.hpp"
// for std::min and std::max
#include<cstdlib>
/*consider moving all the functions here to the hpp and declare them as extern inline. */
/**************** explicit instantiations ****************************/
/* this template garbage will go away once we dump Real and go with double forver */
template Vector3<float> operator*<double, float>(double, Wm3::Vector3<float> const&);
template Vector3<long double> operator*<double, long double>(double, Wm3::Vector3<long double> const&);
/****** end of explicit instantiations *******/
Vector2r operator*(Real fScalar, const Vector2r& rkV){return Vector2r(fScalar*rkV[0],fScalar*rkV[1]);}
template<class RealType1, class RealType2>
Vector3<RealType2> operator* (RealType1 fScalar, const Vector3<RealType2>& rkV){ return Vector3<RealType2>(fScalar*rkV[0],fScalar*rkV[1],fScalar*rkV[2]);}
template<> Vector3r operator*(Real s, const Vector3r& v){ return v*s; }
/*__attribute__((deprecated)) Vector3f operator*(const double s, const Vector3f& v){return Vector3f(s*v[0],s*v[1],s*v[2]);}
__attribute__((deprecated)) Vector3d operator*(const float s, const Vector3d& v){return Vector3d(s*v[0],s*v[1],s*v[2]);}
__attribute__((deprecated)) Vector3f operator*(const Vector3f& v, const double s){return Vector3f(s*v[0],s*v[1],s*v[2]);}
__attribute__((deprecated)) Vector3d operator*(const Vector3d& v, const float s){return Vector3d(s*v[0],s*v[1],s*v[2]);}*/