Skip to content
Branch: master
Find file Copy path
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
216 lines (179 sloc) 9.45 KB
#if 0
void L6Geom::setInitialLocalCoords(const Vector3r& locX){
// initial local y-axis orientation, in the xz or xy plane, depending on which component is larger to avoid singularities
Vector3r locY=locX.cross(abs(locX[1])<abs(locX[2])?Vector3r::UnitY():Vector3r::UnitZ()); locY-=locX*; locY.normalize();
Vector3r locZ=locX.cross(locY);
// set our data
Matrix3r T; T.col(0)=locX; T.col(1)=locY; T.col(2)=locZ;
trsf=Quaternionr(T); // from transformation matrix
trsf.col(0)=locX; trsf.col(1)=locY; trsf.col(2)=locZ;
Generic function to compute L6Geom, used for {sphere,facet,wall}+sphere contacts
NB. the vel2 should be given WITHOUT periodic correction due to C->cellDist, it is handled inside
pos2 however is with periodic correction already!!
void Cg2_Any_Any_L6Geom__Base::handleSpheresLikeContact(const shared_ptr<Contact>& C, const Vector3r& pos1, const Vector3r& vel1, const Vector3r& angVel1, const Vector3r& pos2, const Vector3r& vel2, const Vector3r& angVel2, const Vector3r& normal, const Vector3r& contPt, Real uN, Real r1, Real r2){
// create geometry
L6Geom& g(C->geom->cast<L6Geom>());
// FIXME FIXME FIXME: this needs to be changed
// for now, assign lengths of half of the distance between spheres
// this works good for distance, but breaks radius, which is used for stiffness computation and is hacked around in Cp2_FrictMat_FrictPhys
// perhaps separating those 2 would help, or computing A/l1, A/l2 for stiffness in Cg2 functors already would be a good idea??
// XXX: this also fails with uN/2>ri, which is possible for Wall with one sense of contact only
g.lens=Vector2r(abs(r1)+uN/2,abs(r2)+uN/2); //
// this is a hack around that
if(g.lens[0]<0) g.lens[0]=g.lens[1];
if(g.lens[1]<0) g.lens[1]=g.lens[0];
//cerr<<"##"<<C->leakPA()->id<<"+"<<C->leakPB()->id<<": init trsf=\n"<<g.trsf<<endl<<"locX="<<locX<<", locY="<<locY<<", locZ="<<locZ<<"; normal="<<normal<<endl;
// update geometry
/* motion of the conctact consists in rigid motion (normRotVec, normTwistVec) and mutual motion (relShearDu);
they are used to update trsf, which is then used to convert relative motion to local coordinates
L6Geom& g(C->geom->cast<L6Geom>());
const Vector3r& currNormal(normal); const Vector3r& prevNormal(g.trsf.col(0));
const Vector3r& prevContPt(C->geom->node->pos);
const Real& dt(scene->dt);
Vector3r shiftVel2(scene->isPeriodic?scene->cell->intrShiftVel(C->cellDist):Vector3r::Zero());
//cerr<<"prevNormal="<<prevNormal<<", currNomal="<<currNormal<<endl;
// normal rotation vector, between last steps
Vector3r normRotVec=prevNormal.cross(currNormal);
Vector3r midNormal=(approxMask&APPROX_NO_MID_NORMAL) ? prevNormal : .5*(prevNormal+currNormal);
/* this can happen in exceptional cases (normal gets inverted by motion,
i.e. when sphere is pushed through a facet;
this situation is physically meaningless, so there is no physically correct result really;
it would cause NaN in midNormal.normalize(), which we want need to avoid
if(midNormal.norm()==0) midNormal=currNormal;
else if(!(approxMask&APPROX_NO_RENORM_MID_NORMAL) && !(approxMask&APPROX_NO_MID_NORMAL)) midNormal.normalize(); // normalize only if used and if requested via approxMask
Vector3r normTwistVec=midNormal*dt*.5*;
// compute current transformation, by updating previous axes
// the X axis can be prescribed directly (copy of normal)
// the mutual motion on the contact does not affect the transformation, that is handled below
const Matrix3r prevTrsf(g.trsf.toRotationMatrix());
Quaternionr prevTrsfQ(g.trsf);
const Matrix3r prevTrsf(g.trsf); // could be reference perhaps, but we need it to compute midTrsf (if applicable)
/* middle transformation first */
Matrix3r midTrsf;
if(approxMask&APPROX_NO_MID_TRSF){ midTrsf.col(0)=prevNormal; midTrsf.col(1)=prevTrsf.col(1); }
else{ midTrsf.col(0)=midNormal; midTrsf.col(1)=prevTrsf.col(1)-prevTrsf.col(1).cross(normRotVec+normTwistVec)/2.; }
/* current transformation now */
Matrix3r currTrsf;
Quaternionr currTrsfQ(currTrsf);
if(trsfRenorm>0 && (scene->iter % trsfRenorm)==0) currTrsfQ.normalize();
/* orthonormalize in a way to not alter local x-axis */
if(trsfRenorm>0 && (scene->step % trsfRenorm)==0){
currTrsf.col(1)-=currTrsf.col(0)*currTrsf.col(1).dot(currTrsf.col(0)); // take away y projected on x, to stabilize numerically
currTrsf.col(2)=currTrsf.col(0).cross(currTrsf.col(1)); // normalized automatically
#ifdef WOO_DEBUG
LOG_ERROR("##{}+{}, |trsf|={}",C->leakPA()->id,C->leakPB()->id,currTrsf.determinant());
throw runtime_error("Transformation matrix far from orthonormal.");
#if 0
// compute midTrsf
Quaternionr midTrsf=(approxMask&APPROX_NO_MID_TRSF) ? prevTrsfQ : prevTrsfQ.slerp(.5,currTrsfQ);
Quaternionr midTrsf=(approxMask&APPROX_NO_MID_TRSF) ? Quaternionr(prevTrsf) : Quaternionr(prevTrsf).slerp(.5,Quaternionr(currTrsf));
#ifdef WOO_DEBUG
// cerr<<"Error: prevNormal="<<prevNormal<<", currNomal="<<currNormal<<endl;
// if(normRotVec.squaredNorm()==0) throw std::runtime_error("Normal moving too fast (changed sense during one step), motion numerically unstable?");
// compute relative velocity
// noRatch: take radius or current distance as the branch vector; see discussion in ScGeom::precompute (avoidGranularRatcheting)
Vector3r midContPt, midPos1, midPos2;
if(approxMask&APPROX_NO_MID_BRANCH){ midContPt=contPt; midPos1=pos1; midPos2=pos2; }
else{ midContPt=.5*(prevContPt+contPt); midPos1=pos1-(dt/2.)*vel1; /* pos2 is wrapped, use corrected vel2 as well */ midPos2=pos2-(dt/2.)*(vel2+shiftVel2); }
Vector3r c1x=((noRatch && r1>0) ? ( r1*midNormal).eval() : (midContPt-midPos1).eval()); // used only for sphere-sphere
Vector3r c2x=((noRatch && r2>0) ? (-r2*midNormal).eval() : (midContPt-midPos2).eval());
//Vector3r state2velCorrected=state2.vel+(scene->isPeriodic?scene->cell->intrShiftVel(I->cellDist):Vector3r::Zero()); // velocity of the second particle, corrected with meanfield velocity if necessary
//cerr<<"correction "<<(scene->isPeriodic?scene->cell->intrShiftVel(I->cellDist):Vector3r::Zero())<<endl;
Vector3r relVel=(vel2+shiftVel2+angVel2.cross(c2x))-(vel1+angVel1.cross(c1x));
//Vector3r relShearDu=relShearVel*scene->dt;
// cerr<<"normRotVec="<<normRotVec<<", avgNormal="<<avgNormal<<", normTwistVec="<<normTwistVec<<"c1x="<<c1x<<", c2x="<<c2x<<", relVel="<<relVel<<endl;
/* Update of quantities in global coords consists in adding 3 increments we have computed; in global coords (a is any vector)
1. +relShearVel*scene->dt; // mutual motion of the contact
2. -a.cross(normRotVec); // rigid rotation perpendicular to the normal
3. -a.cross(normTwistVec); // rigid rotation parallel to the normal
/* Previous local trsf u'⁻ must be updated to current u'⁰. We have transformation T⁻ and T⁰,
δ(a) denotes increment of a as defined above. Two possibilities:
1. convert to global, update, convert back: T⁰(T⁻*(u'⁻)+δ(T⁻*(u'⁻))). Quite heavy.
2. update u'⁻ straight, using relShearVel in local coords; since relShearVel is computed
at (t-Δt/2), we would have to find intermediary transformation (same axis, half angle;
the same as slerp at t=.5 between the two).
This could be perhaps simplified by using T⁰ or T⁻ since they will not differ much,
but it would have to be verified somehow.
// if requested via approxMask, just use prevTrsf
// cerr<<"prevTrsf=\n"<<prevTrsf<<", currTrsf=\n"<<currTrsf<<", midTrsf=\n"<<Matrix3r(midTrsf)<<endl;
// updates of geom here
// midTrsf*relShearVel should have the 0-th component (approximately) zero -- to be checked
//cerr<<"midTrsf=\n"<<midTrsf<<",relShearDu="<<relShearDu<<", transformed "<<midTrsf*relShearDu<<endl;
// update data here
const auto& midTrsfInv=midTrsf.conjugate();
const auto& midTrsfInv=midTrsf.transpose();
g.uN=uN; // this does not have to be computed incrementally
// Node
You can’t perform that action at this time.