C++ Vector3r::cross方法代码示例

	notInNotch(const Vector3r& _c, const Vector3r& _edge, const Vector3r& _normal, Real _aperture){
		c=_c;
		edge=_edge; edge.normalize();
		normal=_normal; normal-=edge*edge.dot(normal); normal.normalize();
		inside=edge.cross(normal);
		aperture=_aperture;
		// LOG_DEBUG("edge="<<edge<<", normal="<<normal<<", inside="<<inside<<", aperture="<<aperture);
	}

void TorqueRecorder::action(){
	totalTorque=0;
	Vector3r tmpAxis = rotationAxis.normalized();
	
	FOREACH(Body::id_t id, ids){
		if (!(scene->bodies->exists(id))) continue;
		Body* b=Body::byId(id,scene).get();
		
		Vector3r tmpPos = b->state->pos;
		Vector3r radiusVector = tmpAxis.cross(tmpAxis.cross(tmpPos - zeroPoint));
		
		totalTorque+=tmpAxis.dot(scene->forces.getTorque(id)+radiusVector.cross(scene->forces.getForce(id)));
	};
	
	//Save data to a file
	out<<scene->iter<<" "<<totalTorque<<"\n";
	out.close();
}

//!			O/
bool Ig2_Sphere_GridConnection_ScGridCoGeom::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)
{	// Useful variables :
	const State*    sphereSt  = YADE_CAST<const State*>(&state1);
	//const State*    gridCoSt  = YADE_CAST<const State*>(&state2);
	Sphere*         sphere    = YADE_CAST<Sphere*>(cm1.get());
	GridConnection* gridCo    = YADE_CAST<GridConnection*>(cm2.get());
	//GridNode*       gridNo1   = YADE_CAST<GridNode*>(gridCo->node1->shape.get());
	//GridNode*       gridNo2   = YADE_CAST<GridNode*>(gridCo->node2->shape.get());
	State*          gridNo1St = YADE_CAST<State*>(gridCo->node1->state.get());
	State*          gridNo2St = YADE_CAST<State*>(gridCo->node2->state.get());
	bool isNew = !c->geom;
	shared_ptr<ScGridCoGeom> scm;
	if (!isNew) scm = YADE_PTR_CAST<ScGridCoGeom>(c->geom);
	Vector3r segt = gridCo->getSegment();
	float len = gridCo->getLength();
	Vector3r branch = sphereSt->pos - gridNo1St->pos;
	float relPos = branch.dot(segt)/(len*len);
	Vector3r fictiousPos=gridNo1St->pos+relPos*segt;
	Vector3r branchP = fictiousPos - sphereSt->pos;
 	float dist = branchP.norm();
	
	if(isNew){
		if(dist > (sphere->radius + gridCo->radius)) return false;
		else {scm=shared_ptr<ScGridCoGeom>(new ScGridCoGeom()); c->geom=scm;}
	}
	if(dist <= (sphere->radius + gridCo->radius)){
		scm->refR1=sphere->radius;	//FIXME don't know why I have to do that ...
		scm->refR2=gridCo->radius;
		scm->id3=gridCo->node1->getId();
 		scm->id4=gridCo->node2->getId();
		scm->relPos=relPos;
		Vector3r normal=branchP/dist;
		scm->penetrationDepth=sphere->radius+gridCo->radius-dist;
		scm->fictiousState.pos = gridNo1St->pos+segt*relPos;
		scm->fictiousState.vel = (1-relPos)*gridNo1St->vel + relPos*gridNo2St->vel;
		scm->fictiousState.angVel =
			((1-relPos)*gridNo1St->angVel + relPos*gridNo2St->angVel).dot(segt/len)*segt/len //twist part : interpolated
			+ segt.cross(gridNo2St->vel - gridNo1St->vel);// non-twist part : defined from nodes velocities
		scm->contactPoint = sphereSt->pos+normal*(sphere->radius-0.5*scm->penetrationDepth);
		scm->precompute(state1,scm->fictiousState,scene,c,normal,isNew,shift2,true);//use sphere-sphere precompute (with a virtual sphere)
	}
	return true;
}

/* Law2_ScGeom_ViscElPhys_Basic */
void Law2_ScGeom_ViscElPhys_Basic::go(shared_ptr<IGeom>& _geom, shared_ptr<IPhys>& _phys, Interaction* I){

	const ScGeom& geom=*static_cast<ScGeom*>(_geom.get());
	ViscElPhys& phys=*static_cast<ViscElPhys*>(_phys.get());

	const int id1 = I->getId1();
	const int id2 = I->getId2();
	
	if (geom.penetrationDepth<0) {
		if (phys.liqBridgeCreated and -geom.penetrationDepth<phys.sCrit and phys.Capillar) {
			phys.normalForce = -calculateCapillarForce(geom, phys)*geom.normal;
		  if (I->isActive) {
				addForce (id1,-phys.normalForce,scene);
				addForce (id2, phys.normalForce,scene);
			};
			return;
		} else {
			scene->interactions->requestErase(I);
			return;
		};
	};

	const BodyContainer& bodies = *scene->bodies;

	const State& de1 = *static_cast<State*>(bodies[id1]->state.get());
	const State& de2 = *static_cast<State*>(bodies[id2]->state.get());

  /*
   * This part for implementation of the capillar model.
   * All main equations are in calculateCapillarForce function. 
   * There is only the determination of critical distance between spheres, 
   * after that the liquid bridge will be broken.
   */ 
   
	if (not(phys.liqBridgeCreated) and phys.Capillar) {
		phys.liqBridgeCreated = true;
		Sphere* s1=dynamic_cast<Sphere*>(bodies[id1]->shape.get());
		Sphere* s2=dynamic_cast<Sphere*>(bodies[id2]->shape.get());
		if (s1 and s2) {
			phys.R = 2 * s1->radius * s2->radius / (s1->radius + s2->radius);
		} else if (s1 and not(s2)) {
			phys.R = s1->radius;
		} else {
			phys.R = s2->radius;
		}
		
		const Real Vstar = phys.Vb/(phys.R*phys.R*phys.R);
		const Real Sstar = (1+0.5*phys.theta)*(pow(Vstar,1/3.0) + 0.1*pow(Vstar,2.0/3.0));   // [Willett2000], equation (15), use the full-length e.g 2*Sc
		
		phys.sCrit = Sstar*phys.R;
	}

	Vector3r& shearForce = phys.shearForce;
	if (I->isFresh(scene)) shearForce=Vector3r(0,0,0);
	const Real& dt = scene->dt;
	shearForce = geom.rotate(shearForce);
	

	// Handle periodicity.
	const Vector3r shift2 = scene->isPeriodic ? scene->cell->intrShiftPos(I->cellDist): Vector3r::Zero(); 
	const Vector3r shiftVel = scene->isPeriodic ? scene->cell->intrShiftVel(I->cellDist): Vector3r::Zero(); 

	const Vector3r c1x = (geom.contactPoint - de1.pos);
	const Vector3r c2x = (geom.contactPoint - de2.pos - shift2);
	
	const Vector3r relativeVelocity = (de1.vel+de1.angVel.cross(c1x)) - (de2.vel+de2.angVel.cross(c2x)) + shiftVel;
	const Real normalVelocity	= geom.normal.dot(relativeVelocity);
	const Vector3r shearVelocity	= relativeVelocity-normalVelocity*geom.normal;
	
	// As Chiara Modenese suggest, we store the elastic part 
	// and then add the viscous part if we pass the Mohr-Coulomb criterion.
	// See http://www.mail-archive.com/[email protected]/msg01391.html
	shearForce += phys.ks*dt*shearVelocity; // the elastic shear force have a history, but
	Vector3r shearForceVisc = Vector3r::Zero(); // the viscous shear damping haven't a history because it is a function of the instant velocity 


	// Prevent appearing of attraction forces due to a viscous component
	// [Radjai2011], page 3, equation [1.7]
	// [Schwager2007]
	const Real normForceReal = phys.kn * geom.penetrationDepth + phys.cn * normalVelocity;
	if (normForceReal < 0) {
		phys.normalForce = Vector3r::Zero();
	} else {
		phys.normalForce = normForceReal * geom.normal;
	}
	
	Vector3r momentResistance = Vector3r::Zero();
	if (phys.mR>0.0) {
		const Vector3r relAngVel  = de1.angVel - de2.angVel;
		relAngVel.normalized();
		
		if (phys.mRtype == 1) { 
			momentResistance = -phys.mR*phys.normalForce.norm()*relAngVel;																														// [Zhou1999536], equation (3)
		} else if (phys.mRtype == 2) { 
			momentResistance = -phys.mR*(c1x.cross(de1.angVel) - c2x.cross(de2.angVel)).norm()*phys.normalForce.norm()*relAngVel;			// [Zhou1999536], equation (4)
		}
	}
	
	const Real maxFs = phys.normalForce.squaredNorm() * std::pow(phys.tangensOfFrictionAngle,2);
//.........这里部分代码省略.........

bool computeForceTorqueViscEl(shared_ptr<IGeom>& _geom, shared_ptr<IPhys>& _phys, Interaction* I, Vector3r & force, Vector3r & torque1, Vector3r & torque2) {
	ViscElPhys& phys=*static_cast<ViscElPhys*>(_phys.get());
	const ScGeom& geom=*static_cast<ScGeom*>(_geom.get());
	Scene* scene=Omega::instance().getScene().get();

#ifdef YADE_SPH
//=======================================================================================================
	if (phys.SPHmode) {
		if (computeForceSPH(_geom, _phys, I, force)) {
			return true;
		} else {
			return false;
		}
	}
//=======================================================================================================
#endif

	const int id1 = I->getId1();
	const int id2 = I->getId2();
	
	if (geom.penetrationDepth<0) {
		return false;
	} else {
		const BodyContainer& bodies = *scene->bodies;
	
		const State& de1 = *static_cast<State*>(bodies[id1]->state.get());
		const State& de2 = *static_cast<State*>(bodies[id2]->state.get());
	
		Vector3r& shearForce = phys.shearForce;
		if (I->isFresh(scene)) shearForce=Vector3r(0,0,0);
		const Real& dt = scene->dt;
		shearForce = geom.rotate(shearForce);
	
		// Handle periodicity.
		const Vector3r shift2 = scene->isPeriodic ? scene->cell->intrShiftPos(I->cellDist): Vector3r::Zero(); 
		const Vector3r shiftVel = scene->isPeriodic ? scene->cell->intrShiftVel(I->cellDist): Vector3r::Zero(); 
	
		const Vector3r c1x = (geom.contactPoint - de1.pos);
		const Vector3r c2x = (geom.contactPoint - de2.pos - shift2);
		
		const Vector3r relativeVelocity = (de1.vel+de1.angVel.cross(c1x)) - (de2.vel+de2.angVel.cross(c2x)) + shiftVel;
		const Real normalVelocity	= geom.normal.dot(relativeVelocity);
		const Vector3r shearVelocity	= relativeVelocity-normalVelocity*geom.normal;
		
		// As Chiara Modenese suggest, we store the elastic part 
		// and then add the viscous part if we pass the Mohr-Coulomb criterion.
		// See http://www.mail-archive.com/[email protected]/msg01391.html
		shearForce += phys.ks*dt*shearVelocity; // the elastic shear force have a history, but
		Vector3r shearForceVisc = Vector3r::Zero(); // the viscous shear damping haven't a history because it is a function of the instant velocity 
	
	
		// Prevent appearing of attraction forces due to a viscous component
		// [Radjai2011], page 3, equation [1.7]
		// [Schwager2007]
		phys.Fn = phys.kn * geom.penetrationDepth;
		phys.Fv = phys.cn * normalVelocity;
		const Real normForceReal = phys.Fn + phys.Fv;
		if (normForceReal < 0) {
			phys.normalForce = Vector3r::Zero();
		} else {
			phys.normalForce = normForceReal * geom.normal;
		}
		
		Vector3r momentResistance = Vector3r::Zero();
		if (phys.mR>0.0) {
			const Vector3r relAngVel  = de1.angVel - de2.angVel;
			relAngVel.normalized();
			
			if (phys.mRtype == 1) { 
				momentResistance = -phys.mR*phys.normalForce.norm()*relAngVel;																														// [Zhou1999536], equation (3)
			} else if (phys.mRtype == 2) { 
				momentResistance = -phys.mR*(c1x.cross(de1.angVel) - c2x.cross(de2.angVel)).norm()*phys.normalForce.norm()*relAngVel;			// [Zhou1999536], equation (4)
			}
		}
		
		const Real maxFs = phys.normalForce.squaredNorm() * std::pow(phys.tangensOfFrictionAngle,2);
		if( shearForce.squaredNorm() > maxFs )
		{
			// Then Mohr-Coulomb is violated (so, we slip), 
			// we have the max value of the shear force, so 
			// we consider only friction damping.
			const Real ratio = sqrt(maxFs) / shearForce.norm();
			shearForce *= ratio;
		} 
		else 
		{
			// Then no slip occurs we consider friction damping + viscous damping.
			shearForceVisc = phys.cs*shearVelocity; 
		}
		force = phys.normalForce + shearForce + shearForceVisc;
		torque1 = -c1x.cross(force)+momentResistance;
		torque2 =  c2x.cross(force)-momentResistance;
		return true;
	}
}

/* Law2_ScGeom_ViscElPhys_Basic */
void Law2_ScGeom_ViscElPhys_Basic::go(shared_ptr<IGeom>& _geom, shared_ptr<IPhys>& _phys, Interaction* I){

	const ScGeom& geom=*static_cast<ScGeom*>(_geom.get());
	ViscElPhys& phys=*static_cast<ViscElPhys*>(_phys.get());

	const int id1 = I->getId1();
	const int id2 = I->getId2();
	
	if (geom.penetrationDepth<0) {
 		scene->interactions->requestErase(I);
		return;
	}

	const BodyContainer& bodies = *scene->bodies;

	const State& de1 = *static_cast<State*>(bodies[id1]->state.get());
	const State& de2 = *static_cast<State*>(bodies[id2]->state.get());

	Vector3r& shearForce = phys.shearForce;
	if (I->isFresh(scene)) shearForce=Vector3r(0,0,0);
	const Real& dt = scene->dt;
	//Vector3r axis = phys.prevNormal.cross(geom.normal);
	//shearForce -= shearForce.cross(axis);
	//const Real angle = dt*0.5*geom.normal.dot(de1.angVel + de2.angVel);
	//axis = angle*geom.normal;
	//shearForce -= shearForce.cross(axis);
	shearForce = geom.rotate(shearForce);
	

	// Handle periodicity.
	const Vector3r shift2 = scene->isPeriodic ? scene->cell->intrShiftPos(I->cellDist): Vector3r::Zero(); 
	const Vector3r shiftVel = scene->isPeriodic ? scene->cell->intrShiftVel(I->cellDist): Vector3r::Zero(); 

	const Vector3r c1x = (geom.contactPoint - de1.pos);
	const Vector3r c2x = (geom.contactPoint - de2.pos - shift2);
	
	const Vector3r relativeVelocity = (de1.vel+de1.angVel.cross(c1x)) - (de2.vel+de2.angVel.cross(c2x)) + shiftVel;
	const Real normalVelocity	= geom.normal.dot(relativeVelocity);
	const Vector3r shearVelocity	= relativeVelocity-normalVelocity*geom.normal;

	// As Chiara Modenese suggest, we store the elastic part 
	// and then add the viscous part if we pass the Mohr-Coulomb criterion.
	// See http://www.mail-archive.com/[email protected]/msg01391.html
	shearForce += phys.ks*dt*shearVelocity; // the elastic shear force have a history, but
	Vector3r shearForceVisc = Vector3r::Zero(); // the viscous shear damping haven't a history because it is a function of the instant velocity 

	phys.normalForce = ( phys.kn * geom.penetrationDepth + phys.cn * normalVelocity ) * geom.normal;
	//phys.prevNormal = geom.normal;

	const Real maxFs = phys.normalForce.squaredNorm() * std::pow(phys.tangensOfFrictionAngle,2);
	if( shearForce.squaredNorm() > maxFs )
	{
		// Then Mohr-Coulomb is violated (so, we slip), 
		// we have the max value of the shear force, so 
		// we consider only friction damping.
		const Real ratio = sqrt(maxFs) / shearForce.norm();
		shearForce *= ratio;
	} 
	else 
	{
		// Then no slip occurs we consider friction damping + viscous damping.
		shearForceVisc = phys.cs*shearVelocity; 
	}

	const Vector3r f = phys.normalForce + shearForce + shearForceVisc;
	addForce (id1,-f,scene);
	addForce (id2, f,scene);
	addTorque(id1,-c1x.cross(f),scene);
	addTorque(id2, c2x.cross(f),scene);
}

//.........这里部分代码省略.........
				Real relPosNext = (branchN.dot(segtNext))/(segtNext.norm()*segtNext.norm());
				const shared_ptr<Interaction> intr = scene->interactions->find(c->id1,gridNo2->ConnList[i]->getId());
				if(relPosNext<=0){ //if the sphere projection is outside both the current Connection AND this neighbouring connection, then create the interaction if the neighbour did not already do it before.
					if(intr && intr->isReal() && isNew) return false;
					if(intr && intr->isReal() && !isNew) {scm->isDuplicate=1;/*cout<<"Declare "<<c->id1<<"-"<<c->id2<<" as duplicated."<<endl;*/}
				}
				else{//the sphere projection is outside the current Connection but inside the previous neighbour. The contact has to be handled by the Prev GridConnection, not here.
					if (isNew)return false;
					else {//cout<<"The contact "<<c->id1<<"-"<<c->id2<<" HAVE to be copied and deleted NOW."<<endl ;
						scm->isDuplicate=1 ;
						scm->trueInt=-1 ;
						return true;}
				}
			}
		}
	}
	
	else if (isNew && relPos<=0.5){
		if(gridNo1->ConnList.size()>1){//	if the node is not an extremity of the Grid (only one connection)
			for(int unsigned i=0;i<gridNo1->ConnList.size();i++){	// ... loop on all the Connections of the same Node ...
				GridConnection* GC = (GridConnection*)gridNo1->ConnList[i]->shape.get();
				if(GC==gridCo)continue;//	self comparison.
				Vector3r segtCandidate1 = GC->node1->state->pos - gridNo1St->pos; // (be sure of the direction of segtPrev to compare relPosPrev.)
				Vector3r segtCandidate2 = GC->node2->state->pos - gridNo1St->pos;
				Vector3r segtPrev = segtCandidate1.norm()>segtCandidate2.norm() ? segtCandidate1:segtCandidate2;
				for(int j=0;j<3;j++){
					if(abs(segtPrev[j])<1e-14) segtPrev[j]=0.0;
				}
				Real relPosPrev = (branch.dot(segtPrev))/(segtPrev.norm()*segtPrev.norm());
				if(relPosPrev<=0){ //the sphere projection is inside the current Connection and outide this neighbour connection.
					const shared_ptr<Interaction> intr = scene->interactions->find(c->id1,gridNo1->ConnList[i]->getId());
					if( intr && intr->isReal() ){// if an ineraction exist between the sphere and the previous connection, import parameters.
						//cout<<"Copying contact geom and phys from "<<intr->id1<<"-"<<intr->id2<<" to here ("<<c->id1<<"-"<<c->id2<<")"<<endl;
						scm=YADE_PTR_CAST<ScGridCoGeom>(intr->geom);
						c->geom=scm;
						c->phys=intr->phys;
						scm->trueInt=c->id2;
						scm->isDuplicate=2;	//command the old contact deletion.
						isNew=0;
						break;
					}
				}
			}
		}
	}
	
	else if (isNew && relPos>0.5){
		if(gridNo2->ConnList.size()>1){
			for(int unsigned i=0;i<gridNo2->ConnList.size();i++){
				GridConnection* GC = (GridConnection*)gridNo2->ConnList[i]->shape.get();
				if(GC==gridCo)continue;//	self comparison.
				Vector3r segtCandidate1 = GC->node1->state->pos - gridNo2St->pos;
				Vector3r segtCandidate2 = GC->node2->state->pos - gridNo2St->pos;
				Vector3r segtNext = segtCandidate1.norm()>segtCandidate2.norm() ? segtCandidate1:segtCandidate2;
				for(int j=0;j<3;j++){
					if(abs(segtNext[j])<1e-14) segtNext[j]=0.0;
				}
				Real relPosNext = (branchN.dot(segtNext))/(segtNext.norm()*segtNext.norm());
				if(relPosNext<=0){ //the sphere projection is inside the current Connection and outide this neighbour connection.
					const shared_ptr<Interaction> intr = scene->interactions->find(c->id1,gridNo2->ConnList[i]->getId());
					if( intr && intr->isReal() ){// if an ineraction exist between the sphere and the previous connection, import parameters.
						//cout<<"Copying contact geom and phys from "<<intr->id1<<"-"<<intr->id2<<" to here ("<<c->id1<<"-"<<c->id2<<")"<<endl;
						scm=YADE_PTR_CAST<ScGridCoGeom>(intr->geom);
						c->geom=scm;
						c->phys=intr->phys;
						scm->trueInt=c->id2;
						scm->isDuplicate=2;	//command the old contact deletion.
						isNew=0;
						break;
					}
				}
			}
		}
	}
	
	relPos=relPos<0?0:relPos;	//min value of relPos : 0
	relPos=relPos>1?1:relPos;	//max value of relPos : 1
	Vector3r fictiousPos=gridNo1St->pos+relPos*segt;
	Vector3r branchF = fictiousPos - spherePos;
 	Real dist = branchF.norm();
	if(isNew && (dist > (sphere->radius + gridCo->radius))) return false;
	
	//	Create the geometry :
	if(isNew) c->geom=scm;
	scm->radius1=sphere->radius;
	scm->radius2=gridCo->radius;
	scm->id3=gridCo->node1->getId();
	scm->id4=gridCo->node2->getId();
	scm->relPos=relPos;
	Vector3r normal=branchF/dist;
	scm->penetrationDepth = sphere->radius+gridCo->radius-dist;
	scm->fictiousState.pos = fictiousPos;
	scm->contactPoint = spherePos + normal*(scm->radius1 - 0.5*scm->penetrationDepth);
	scm->fictiousState.vel = (1-relPos)*gridNo1St->vel + relPos*gridNo2St->vel;
	scm->fictiousState.angVel =
		((1-relPos)*gridNo1St->angVel + relPos*gridNo2St->angVel).dot(segt/len)*segt/len //twist part : interpolated
		+ segt.cross(gridNo2St->vel - gridNo1St->vel);// non-twist part : defined from nodes velocities
	scm->precompute(state1,scm->fictiousState,scene,c,normal,isNew,shift2,true);//use sphere-sphere precompute (with a virtual sphere)
	return true;
}

/*! Constitutive law */
void FCPM::go(shared_ptr<IGeom>& _geom, shared_ptr<IPhys>& _phys, Interaction* I){

	
	const ScGeom& geom=*static_cast<ScGeom*>(_geom.get());
	FreshConcretePhys& phys=*static_cast<FreshConcretePhys*>(_phys.get());

	const int id1 = I->getId1();
	const int id2 = I->getId2();
	
	const BodyContainer& bodies = *scene->bodies;

	const State& de1 = *static_cast<State*>(bodies[id1]->state.get());
	const State& de2 = *static_cast<State*>(bodies[id2]->state.get());

//Calculation of the max penetretion and the radius of the overlap area 
	Sphere* s1=dynamic_cast<Sphere*>(bodies[id1]->shape.get());
	Sphere* s2=dynamic_cast<Sphere*>(bodies[id2]->shape.get());

	Real dist;
	Real contactRadius;
	Real OverlapRadius;

	if (s1 and s2) {
		phys.maxPenetration=s1->radius * phys.Penetration1 + s2->radius * phys.Penetration2;
		dist = s1->radius + s2->radius - geom.penetrationDepth;
		OverlapRadius = pow(((4 * pow(dist,2) * pow(s1->radius,2) - pow((pow(dist,2) - pow(s2->radius,2) + pow(s1->radius,2)),2)) / (4 * pow(dist,2))),(1.0/2.0));
		//contactRadius = (pow(s1->radius,2) + pow(s2->radius,2))/(s1->radius + s2->radius);
		contactRadius = s1->radius;
	} else if (s1 and not(s2)) {
		phys.maxPenetration=s1->radius * phys.Penetration1;
		dist = s1->radius - geom.penetrationDepth;
		OverlapRadius =pow((pow(s1->radius,2) - pow(dist,2)),(1.0/2.0));
		contactRadius = s1->radius;
	} else {
		phys.maxPenetration=s2->radius * phys.Penetration2;
		dist = s2->radius - geom.penetrationDepth;
		OverlapRadius = pow((pow(s2->radius,2) - pow(dist,2)),(1.0/2.0));
		contactRadius = s2->radius;
	}


	Vector3r& shearForce = phys.shearForce;
	if (I->isFresh(scene)) shearForce=Vector3r(0,0,0);
	const Real& dt = scene->dt;
	shearForce = geom.rotate(shearForce);


	// Handle periodicity.
	const Vector3r shift2 = scene->isPeriodic ? scene->cell->intrShiftPos(I->cellDist): Vector3r::Zero(); 
	const Vector3r shiftVel = scene->isPeriodic ? scene->cell->intrShiftVel(I->cellDist): Vector3r::Zero(); 

	const Vector3r c1x = (geom.contactPoint - de1.pos);
	const Vector3r c2x = (geom.contactPoint - de2.pos - shift2);
	
	const Vector3r relativeVelocity = (de1.vel+de1.angVel.cross(c1x)) - (de2.vel+de2.angVel.cross(c2x)) + shiftVel;
	const Real normalVelocity	= geom.normal.dot(relativeVelocity);
	const Vector3r shearVelocity	= relativeVelocity-normalVelocity*geom.normal;

//Normal Force
	
	Real OverlapArea;
	Real MaxArea;

	OverlapArea = 3.1415 * pow(OverlapRadius,2);
	MaxArea = 3.1415 * pow(contactRadius,2);

	Real Mult;
	if(OverlapRadius < contactRadius){
		Mult = OverlapArea / MaxArea;
	}
	else{
		Mult = 1;
	}
	
	Real Fn;
	if(geom.penetrationDepth>=0){
//Compression
		if(normalVelocity>=0){
			if (geom.penetrationDepth >= phys.maxPenetration){
				Fn = phys.knI * (geom.penetrationDepth - phys.previousun) + phys.previousElasticN + phys.cnI * normalVelocity;
				phys.previousElasticN += phys.knI * (geom.penetrationDepth - phys.previousun);
				phys.normalForce = Fn * geom.normal;
				phys.t = 0; phys.t2 = 0;
				//phys.previousElasticTensionN = 0;
			} 
			else{
				Fn = Mult * phys.kn * (geom.penetrationDepth - phys.previousun) + Mult * phys.previousElasticN + phys.cn * normalVelocity;
				phys.previousElasticN += Mult * phys.kn * (geom.penetrationDepth - phys.previousun);
				phys.finalElasticN += Mult * phys.kn * (geom.penetrationDepth - phys.previousun);
				phys.normalForce = Fn * geom.normal;
				phys.t = 0; phys.t2 = 0;
				//phys.previousElasticTensionN = 0;
			}
		}
//Tension
		else if(normalVelocity<0){
			if(phys.t == 0){
				phys.maxNormalComp = - phys.finalElasticN;
				//printf("------> %E \n", phys.maxNormalComp);
//.........这里部分代码省略.........

/*
Generic function to compute L3Geom (with colinear points), used for {sphere,facet,wall}+sphere contacts now
*/
void Ig2_Sphere_Sphere_L3Geom::handleSpheresLikeContact(const shared_ptr<Interaction>& I, const State& state1, const State& state2, const Vector3r& shift2, bool is6Dof, const Vector3r& normal, const Vector3r& contPt, Real uN, Real r1, Real r2){
	// create geometry
	if(!I->geom){
		if(is6Dof) I->geom=shared_ptr<L6Geom>(new L6Geom);
		else       I->geom=shared_ptr<L3Geom>(new L3Geom);
		L3Geom& g(I->geom->cast<L3Geom>());
		g.contactPoint=contPt;
		g.refR1=r1; g.refR2=r2;
		g.normal=normal;
		// g.trsf.setFromTwoVectors(Vector3r::UnitX(),g.normal); // quaternion just from the X-axis; does not seem to work for some reason?!
		const Vector3r& locX(g.normal);
		// initial local y-axis orientation, in the xz or xy plane, depending on which component is larger to avoid singularities
		Vector3r locY=normal.cross(abs(normal[1])<abs(normal[2])?Vector3r::UnitY():Vector3r::UnitZ()); locY-=locX*locY.dot(locX); locY.normalize();
		Vector3r locZ=normal.cross(locY);
		#ifdef L3_TRSF_QUATERNION
			Matrix3r trsf; trsf.row(0)=locX; trsf.row(1)=locY; trsf.row(2)=locZ;
			g.trsf=Quaternionr(trsf); // from transformation matrix
		#else
			g.trsf.row(0)=locX; g.trsf.row(1)=locY; g.trsf.row(2)=locZ;
		#endif
		g.u=Vector3r(uN,0,0); // zero shear displacement
		if(distFactor<0) g.u0[0]=uN;
		// L6Geom::phi is initialized to Vector3r::Zero() automatically
		//cerr<<"Init trsf=\n"<<g.trsf<<endl<<"locX="<<locX<<", locY="<<locY<<", locZ="<<locZ<<endl;
		return;
	}
	
	// update geometry

	/* motion of the conctact consists in rigid motion (normRotVec, normTwistVec) and mutual motion (relShearDu);
	   they are used to update trsf and u
	*/

	L3Geom& g(I->geom->cast<L3Geom>());
	const Vector3r& currNormal(normal); const Vector3r& prevNormal(g.normal);
	// normal rotation vector, between last steps
	Vector3r normRotVec=prevNormal.cross(currNormal);
	// contrary to what ScGeom::precompute does now (r2486), we take average normal, i.e. .5*(prevNormal+currNormal),
	// so that all terms in the equation are in the previous mid-step
	// the re-normalization might not be necessary for very small increments, but better do it
	Vector3r avgNormal=(approxMask&APPROX_NO_MID_NORMAL) ? prevNormal : .5*(prevNormal+currNormal);
	if(!(approxMask&APPROX_NO_RENORM_MID_NORMAL) && !(approxMask&APPROX_NO_MID_NORMAL)) avgNormal.normalize(); // normalize only if used and if requested via approxMask
	// twist vector of the normal from the last step
	Vector3r normTwistVec=avgNormal*scene->dt*.5*avgNormal.dot(state1.angVel+state2.angVel);
	// compute relative velocity
	// noRatch: take radius or current distance as the branch vector; see discussion in ScGeom::precompute (avoidGranularRatcheting)
	Vector3r c1x=((noRatch && !r1>0) ? ( r1*normal).eval() : (contPt-state1.pos).eval()); // used only for sphere-sphere
	Vector3r c2x=(noRatch ? (-r2*normal).eval() : (contPt-state2.pos+shift2).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 relShearVel=(state2.vel+state2.angVel.cross(c2x))-(state1.vel+state1.angVel.cross(c1x));
	// account for relative velocity of particles in different cell periods
	if(scene->isPeriodic) relShearVel+=scene->cell->intrShiftVel(I->cellDist);
	relShearVel-=avgNormal.dot(relShearVel)*avgNormal;
	Vector3r relShearDu=relShearVel*scene->dt;

	/* 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

	*/

	// 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 change transformation
	#ifdef L3_TRSF_QUATERNION
		const Matrix3r prevTrsf(g.trsf.toRotationMatrix());
		Quaternionr prevTrsfQ(g.trsf);
	#else
		const Matrix3r prevTrsf(g.trsf); // could be reference perhaps, but we need it to compute midTrsf (if applicable)
	#endif
	Matrix3r currTrsf; currTrsf.row(0)=currNormal;
	for(int i=1; i<3; i++){
		currTrsf.row(i)=prevTrsf.row(i)-prevTrsf.row(i).cross(normRotVec)-prevTrsf.row(i).cross(normTwistVec);
	}
	#ifdef L3_TRSF_QUATERNION
		Quaternionr currTrsfQ(currTrsf);
		if((scene->iter % trsfRenorm)==0 && trsfRenorm>0) currTrsfQ.normalize();
	#else
		if((scene->iter % trsfRenorm)==0 && trsfRenorm>0){
			#if 1
				currTrsf.row(0).normalize();
				currTrsf.row(1)-=currTrsf.row(0)*currTrsf.row(1).dot(currTrsf.row(0)); // take away y projected on x, to stabilize numerically
				currTrsf.row(1).normalize();
				currTrsf.row(2)=currTrsf.row(0).cross(currTrsf.row(1));
				currTrsf.row(2).normalize();
			#else
				currTrsf=Matrix3r(Quaternionr(currTrsf).normalized());
			#endif
			#ifdef YADE_DEBUG
				if(abs(currTrsf.determinant()-1)>.05){
					LOG_ERROR("##"<<I->getId1()<<"+"<<I->getId2()<<", |trsf|="<<currTrsf.determinant());
					g.trsf=currTrsf;
					throw runtime_error("Transformation matrix far from orthonormal.");
				}
//.........这里部分代码省略.........