yade-dev team mailing list archive

[noreply@xxxxxxxxxxxxx]

Merge authors:
  Sergei D. <sega@think>
------------------------------------------------------------
revno: 1827 [merge]
committer: Sergei D. <sega@think>
branch nick: trunk
timestamp: Mon 2009-11-30 15:50:41 +0300
message:
  Integrate new rotate integrator into the NewtonDampedLaw framework
modified:
  core/State.hpp
  pkg/dem/Engine/StandAloneEngine/NewtonsDampedLaw.cpp
  pkg/dem/Engine/StandAloneEngine/NewtonsDampedLaw.hpp


--
lp:yade
https://code.launchpad.net/~yade-dev/yade/trunk

Your team Yade developers is subscribed to branch lp:yade.
To unsubscribe from this branch go to https://code.launchpad.net/~yade-dev/yade/trunk/+edit-subscription.

=== modified file 'core/State.hpp'
--- core/State.hpp	2009-11-27 11:33:19 +0000
+++ core/State.hpp	2009-11-30 12:50:41 +0000
@@ -28,10 +28,9 @@
 		Quaternionr& ori;
 		Vector3r angVel;
 		Vector3r angAccel;
+		Vector3r angMom;
 		Vector3r inertia;
-
-		Vector3r prevAngMom;
-
+		//
 		/// reference values
 		Vector3r refPos;
 		Quaternionr refOri;
@@ -60,9 +59,9 @@
 		//! Setter of blockedDOFs from list of strings (['x','rx','rz'] → DOF_X | DOR_RX | DOF_RZ)
 		void blockedDOFs_vec_set(const std::vector<std::string>& dofs);
 
-	State(): se3(Vector3r::ZERO,Quaternionr::IDENTITY),pos(se3.position),vel(Vector3r::ZERO),accel(Vector3r::ZERO),mass(0.),ori(se3.orientation),angVel(Vector3r::ZERO),angAccel(Vector3r::ZERO),inertia(Vector3r::ZERO),prevAngMom(Vector3r::ZERO),refPos(Vector3r::ZERO),refOri(Quaternionr::IDENTITY),blockedDOFs(DOF_NONE){}
+	State(): se3(Vector3r::ZERO,Quaternionr::IDENTITY),pos(se3.position),vel(Vector3r::ZERO),accel(Vector3r::ZERO),mass(0.),ori(se3.orientation),angVel(Vector3r::ZERO),angAccel(Vector3r::ZERO),angMom(Vector3r::ZERO),inertia(Vector3r::ZERO),refPos(Vector3r::ZERO),refOri(Quaternionr::IDENTITY),blockedDOFs(DOF_NONE){}
 
 	REGISTER_CLASS_AND_BASE(State,Serializable);
-	REGISTER_ATTRIBUTES(Serializable,(pos)(vel)(accel)(mass)(ori)(angVel)(angAccel)(inertia)(refPos)(refOri)(blockedDOFs)(prevAngMom));
+	REGISTER_ATTRIBUTES(Serializable,(pos)(vel)(accel)(mass)(ori)(angVel)(angAccel)(angMom)(inertia)(refPos)(refOri)(blockedDOFs));
 };
 REGISTER_SERIALIZABLE(State);

=== modified file 'pkg/dem/Engine/StandAloneEngine/NewtonsDampedLaw.cpp'
--- pkg/dem/Engine/StandAloneEngine/NewtonsDampedLaw.cpp	2009-11-27 09:27:08 +0000
+++ pkg/dem/Engine/StandAloneEngine/NewtonsDampedLaw.cpp	2009-11-30 11:31:25 +0000
@@ -14,11 +14,8 @@
 
 YADE_PLUGIN((NewtonsDampedLaw));
 CREATE_LOGGER(NewtonsDampedLaw);
-void NewtonsDampedLaw::cundallDamp(const Real& dt, const Vector3r& f, const Vector3r& velocity, Vector3r& acceleration, const Vector3r& m, const Vector3r& angularVelocity, Vector3r& angularAcceleration){
-	for(int i=0; i<3; i++){
-		angularAcceleration[i]*= 1 - damping*Mathr::Sign ( m[i]*(angularVelocity[i] + (Real) 0.5 *dt*angularAcceleration[i]) );
-		acceleration       [i]*= 1 - damping*Mathr::Sign ( f[i]*(velocity       [i] + (Real) 0.5 *dt*acceleration       [i]) );
-	}
+void NewtonsDampedLaw::cundallDamp(const Real& dt, const Vector3r& N, const Vector3r& V, Vector3r& A){
+	for(int i=0; i<3; i++) A[i]*= 1 - damping*Mathr::Sign ( N[i]*(V[i] + (Real) 0.5 *dt*A[i]) );
 }
 void NewtonsDampedLaw::blockTranslateDOFs(unsigned blockedDOFs, Vector3r& v) {
 	if(blockedDOFs==State::DOF_NONE) return;
@@ -43,7 +40,9 @@
 	// angular acceleration from: normal torque + torque generated by the force WRT particle centroid on the clump centroid
 	Vector3r diffClumpAngularAccel=diagDiv(m,clumpState->inertia)+diagDiv((state->pos-clumpState->pos).Cross(f),clumpState->inertia); 
 	// damp increment of accels on the clump, using velocities of the clump MEMBER
-	cundallDamp(ncb->dt,f,state->vel,diffClumpAccel,m,state->angVel,diffClumpAngularAccel);
+	//cundallDamp(ncb->dt,f,state->vel,diffClumpAccel,m,state->angVel,diffClumpAngularAccel);
+	cundallDamp(ncb->dt,f,state->vel,diffClumpAccel);
+	cundallDamp(ncb->dt,m,state->angVel,diffClumpAngularAccel);
 	// clumpState->{acceleration,angularAcceleration} are reset byt Clump::moveMembers, it is ok to just increment here
 	clumpState->accel+=diffClumpAccel;
 	clumpState->angAccel+=diffClumpAngularAccel;
@@ -87,72 +86,82 @@
 				#endif
 				continue;
 			}
-			const Vector3r& m=ncb->bex.getTorque(id); const Vector3r& f=ncb->bex.getForce(id);
 
 			if (b->isStandalone()){
-				state->accel=f/state->mass;
-				state->angAccel=diagDiv(m,state->inertia);
-				cundallDamp(dt,f,state->vel,state->accel,m,state->angVel,state->angAccel);
-			}
-			else if (b->isClump()){
-				if (accRigidBodyRot) {
-					accurateRigidBodyRotationIntegrator(ncb,b);
-					continue;
+				// translate equation
+				const Vector3r& f=ncb->bex.getForce(id); 
+				state->accel=f/state->mass; 
+				cundallDamp(dt,f,state->vel,state->accel); 
+				lfTranslate(ncb,state,id,dt);
+				// rotate equation
+				if (/*b->isSpheral || */ !accRigidBodyRot){ // spheral body or accRigidBodyRot disabled
+					const Vector3r& m=ncb->bex.getTorque(id); 
+					state->angAccel=diagDiv(m,state->inertia);
+					cundallDamp(dt,m,state->angVel,state->angAccel);
+					lfSpheralRotate(ncb,state,id,dt);
+				} else { // non spheral body and accRigidBodyRot enabled
+					const Vector3r& m=ncb->bex.getTorque(id); 
+					lfRigidBodyRotate(ncb,state,id,dt,m);
 				}
+			} else if (b->isClump()){
 				state->accel=state->angAccel=Vector3r::ZERO; // to make sure; should be reset in Clump::moveMembers
-				// sum force on clump memebrs, add them to the clump itself
-				FOREACH(Clump::memberMap::value_type mm, static_cast<Clump*>(b.get())->members){
-					handleClumpMember(ncb,mm.first,state);
+				if (accRigidBodyRot){
+					// forces applied to clump proper, if there are such
+					const Vector3r& f=ncb->bex.getForce(id);
+					Vector3r dLinAccel=f/state->mass;
+					cundallDamp(dt,f,state->vel,dLinAccel);
+					state->accel+=dLinAccel;
+					const Vector3r& m=ncb->bex.getTorque(id);
+					Vector3r M(m);
+					// sum forces on clump members
+					FOREACH(Clump::memberMap::value_type mm, static_cast<Clump*>(b.get())->members){
+						const body_id_t memberId=mm.first;
+						const shared_ptr<Body>& member=Body::byId(memberId,ncb);
+						assert(member->isClumpMember());
+						State* memberState=member->state.get();
+						// Linear acceleration
+						const Vector3r& f=ncb->bex.getForce(memberId); 
+						Vector3r diffClumpAccel=f/state->mass;
+						// damp increment of accel on the clump, using velocities of the clump MEMBER
+						cundallDamp(dt,f,memberState->vel,diffClumpAccel);
+						state->accel+=diffClumpAccel;
+						// Momentum
+						const Vector3r& m=ncb->bex.getTorque(memberId);
+						M+=(memberState->pos-state->pos).Cross(f)+m;
+						if(haveBins) velocityBins->binVelSqUse(memberId,VelocityBins::getBodyVelSq(memberState));
+						#ifdef YADE_OPENMP
+							Real& thrMaxVSq=threadMaxVelocitySq[omp_get_thread_num()]; thrMaxVSq=max(thrMaxVSq,memberState->vel.SquaredLength());
+						#else
+							maxVelocitySq=max(maxVelocitySq,memberState->vel.SquaredLength());
+						#endif
+					}
+					// motion
+					lfTranslate(ncb,state,id,dt);
+					lfRigidBodyRotate(ncb,state,id,dt,M);
+				} else { // accRigidBodyRot disabled
+					// sum force on clump memebrs, add them to the clump itself
+					FOREACH(Clump::memberMap::value_type mm, static_cast<Clump*>(b.get())->members){
+						handleClumpMember(ncb,mm.first,state);
+					}
+					// forces applied to clump proper, if there are such
+					const Vector3r& m=ncb->bex.getTorque(id); const Vector3r& f=ncb->bex.getForce(id);
+					Vector3r dLinAccel=f/state->mass, dAngAccel=diagDiv(m,state->inertia);
+					cundallDamp(dt,f,state->vel,dLinAccel); cundallDamp(dt,m,state->angVel,dAngAccel);
+					state->accel+=dLinAccel; state->angAccel+=dAngAccel;
+					// motion
+					lfTranslate(ncb,state,id,dt);
+					lfSpheralRotate(ncb,state,id,dt);
 				}
-				// at this point, forces from clump members are already summed up, this is just for forces applied to clump proper, if there are such
-				Vector3r dLinAccel=f/state->mass, dAngAccel=diagDiv(m,state->inertia);
-				cundallDamp(dt,f,state->vel,dLinAccel,m,state->angVel,dAngAccel);
-				state->accel+=dLinAccel;
-				state->angAccel+=dAngAccel;
+				static_cast<Clump*>(b.get())->moveMembers();
 			}
 
-			// blocking DOFs
-			blockTranslateDOFs( state->blockedDOFs, state->accel );
-			state->vel+=+dt*state->accel;
-
-			blockRotateDOFs( state->blockedDOFs, state->angAccel );
-			state->angVel+=dt*state->angAccel;
-
-			//if(state->blockedDOFs==State::DOF_NONE){
-				//state->angVel+=dt*state->angAccel;
-				//state->vel+=+dt*state->accel;
-			//} else if(state->blockedDOFs==State::DOF_ALL){
-				//[> do nothing <]
-			//} else {
-				//// handle more complicated cases here
-				//if((state->blockedDOFs & State::DOF_X)==0) state->vel[0]+=dt*state->accel[0];
-				//if((state->blockedDOFs & State::DOF_Y)==0) state->vel[1]+=dt*state->accel[1];
-				//if((state->blockedDOFs & State::DOF_Z)==0) state->vel[2]+=dt*state->accel[2];
-				//if((state->blockedDOFs & State::DOF_RX)==0) state->angVel[0]+=dt*state->angAccel[0];
-				//if((state->blockedDOFs & State::DOF_RY)==0) state->angVel[1]+=dt*state->angAccel[1];
-				//if((state->blockedDOFs & State::DOF_RZ)==0) state->angVel[2]+=dt*state->angAccel[2];
-			//}
-
-			// velocities are ready now, save maxima
+			// save maxima velocity
 				if(haveBins) {velocityBins->binVelSqUse(id,VelocityBins::getBodyVelSq(state));}
 				#ifdef YADE_OPENMP
 					Real& thrMaxVSq=threadMaxVelocitySq[omp_get_thread_num()]; thrMaxVSq=max(thrMaxVSq,state->vel.SquaredLength());
 				#else
 					maxVelocitySq=max(maxVelocitySq,state->vel.SquaredLength());
 				#endif
-
-			Vector3r axis = state->angVel;
-			Real angle = axis.Normalize();
-			Quaternionr q;
-			q.FromAxisAngle ( axis,angle*dt );
-			state->ori = q*state->ori;
-			if(ncb->bex.getMoveRotUsed() && ncb->bex.getRot(id)!=Vector3r::ZERO){ Vector3r r(ncb->bex.getRot(id)); Real norm=r.Normalize(); q.FromAxisAngle(r,norm); state->ori=q*state->ori; }
-			state->ori.Normalize();
-
-			Vector3r prevPos=state->pos;
-			state->pos += state->vel*dt + ncb->bex.getMove(id);
-
-			if(b->isClump()) static_cast<Clump*>(b.get())->moveMembers();
 	}
 	#ifdef YADE_OPENMP
 		FOREACH(const Real& thrMaxVSq, threadMaxVelocitySq) { maxVelocitySq=max(maxVelocitySq,thrMaxVSq); }
@@ -160,57 +169,38 @@
 	if(haveBins) velocityBins->binVelSqFinalize();
 }
 
-void NewtonsDampedLaw::accurateRigidBodyRotationIntegrator(MetaBody* ncb, const shared_ptr<Body>& rb){
-	const Real dt=Omega::instance().getTimeStep();
-	State* state=rb->state.get();
-	const body_id_t id=rb->getId();
-	state->accel=state->angAccel=Vector3r::ZERO; // to make sure; should be reset in Clump::moveMembers
-	// sum of forces and torques
-	const Vector3r& f=ncb->bex.getForce(id); const Vector3r& m=ncb->bex.getTorque(id);
-	Vector3r F(f), M(m);
-	FOREACH(Clump::memberMap::value_type mm, static_cast<Clump*>(rb.get())->members){
-		const body_id_t memberId=mm.first;
-		const shared_ptr<Body>& b=Body::byId(memberId,ncb);
-		assert(b->isClumpMember());
-		State* bs=b->state.get();
-		const Vector3r& f=ncb->bex.getForce(memberId); const Vector3r& m=ncb->bex.getTorque(memberId);
-		F+=f; M+=(bs->pos-state->pos).Cross(f)+m;
-	}
-	// translation equation
-	state->accel=F/state->mass; blockTranslateDOFs( state->blockedDOFs, state->accel );
-	state->vel+=dt*state->accel; state->pos+=state->vel*dt+ncb->bex.getMove(id);
-	// rotation equation
+inline void NewtonsDampedLaw::lfTranslate(MetaBody* ncb, State* state, const body_id_t& id, const Real& dt )
+{
+	blockTranslateDOFs(state->blockedDOFs, state->accel);
+	state->vel+=dt*state->accel;
+	state->pos += state->vel*dt + ncb->bex.getMove(id);
+}
+inline void NewtonsDampedLaw::lfSpheralRotate(MetaBody* ncb, State* state, const body_id_t& id, const Real& dt )
+{
+	blockRotateDOFs(state->blockedDOFs, state->angAccel);
+	state->angVel+=dt*state->angAccel;
+	Vector3r axis = state->angVel; Real angle = axis.Normalize();
+	Quaternionr q; q.FromAxisAngle ( axis,angle*dt );
+	state->ori = q*state->ori;
+	if(ncb->bex.getMoveRotUsed() && ncb->bex.getRot(id)!=Vector3r::ZERO){ Vector3r r(ncb->bex.getRot(id)); Real norm=r.Normalize(); Quaternionr q; q.FromAxisAngle(r,norm); state->ori=q*state->ori; }
+	state->ori.Normalize();
+}
+void NewtonsDampedLaw::lfRigidBodyRotate(MetaBody* ncb, State* state, const body_id_t& id, const Real& dt, const Vector3r& M){
 	Matrix3r A; state->ori.Conjugate().ToRotationMatrix(A); // rotation matrix from global to local r.f.
-	const Vector3r l_n = state->prevAngMom + dt/2 * M; // global angular momentum at time n
-	//Vector3r l_b_n = state->ori.Conjugate().Rotate(l_n); // local angular momentum at time n
+	const Vector3r l_n = state->angMom + dt/2 * M; // global angular momentum at time n
 	const Vector3r l_b_n = A*l_n; // local angular momentum at time n
 	const Vector3r angVel_b_n = diagDiv(l_b_n,state->inertia); // local angular velocity at time n
 	const Quaternionr dotQ_n=DotQ(angVel_b_n,state->ori); // dQ/dt at time n
 	const Quaternionr Q_half = state->ori + dt/2 * dotQ_n; // Q at time n+1/2
-	state->prevAngMom+=dt*M; // global angular momentum at time n+1/2
-	//Vector3r l_b_half = state->ori.Conjugate().Rotate(state->prevAngMom); // local angular momentum at time n+1/2
-	const Vector3r l_b_half = A*state->prevAngMom; // local angular momentum at time n+1/2
+	state->angMom+=dt*M; // global angular momentum at time n+1/2
+	const Vector3r l_b_half = A*state->angMom; // local angular momentum at time n+1/2
 	Vector3r angVel_b_half = diagDiv(l_b_half,state->inertia); // local angular velocity at time n+1/2
 	blockRotateDOFs( state->blockedDOFs, angVel_b_half );
 	const Quaternionr dotQ_half=DotQ(angVel_b_half,Q_half); // dQ/dt at time n+1/2
-	state->ori+=dt*dotQ_half; state->ori.Normalize(); // Q at time n+1
+	state->ori+=dt*dotQ_half; // Q at time n+1
 	state->angVel=state->ori.Rotate(angVel_b_half); // global angular velocity at time n+1/2
-	//if(rb->isClump()) static_cast<Clump*>(rb.get())->moveMembers();
-	static_cast<Clump*>(rb.get())->moveMembers();
-
-	LOG_TRACE("\nforce: " << F << " torque: " << M
-		<< "\nglobal angular momentum at time n: " << l_n
-		<< "\nlocal angular momentum at time n:  " << l_b_n
-		<< "\nlocal angular velocity at time n:  " << angVel_b_n
-		<< "\ndQ/dt at time n:                   " << dotQ_n
-		<< "\nQ at time n+1/2:                   " << Q_half
-		<< "\nglobal angular momentum at time n+1/2: " << state->prevAngMom
-		<< "\nlocal angular momentum at time n+1/2:  " << l_b_half
-		<< "\nlocal angular velocity at time n+1/2:  " << angVel_b_half
-		<< "\ndQ/dt at time n+1/2:                   " << dotQ_half
-		<< "\nQ at time n+1:                         " << state->ori
-		<< "\nglobal angular velocity at time n+1/2: " << state->angVel
-	);
+	if(ncb->bex.getMoveRotUsed() && ncb->bex.getRot(id)!=Vector3r::ZERO){ Vector3r r(ncb->bex.getRot(id)); Real norm=r.Normalize(); Quaternionr q; q.FromAxisAngle(r,norm); state->ori=q*state->ori; }
+	state->ori.Normalize(); 
 }
 	
 Quaternionr NewtonsDampedLaw::DotQ(const Vector3r& angVel, const Quaternionr& Q){

=== modified file 'pkg/dem/Engine/StandAloneEngine/NewtonsDampedLaw.hpp'
--- pkg/dem/Engine/StandAloneEngine/NewtonsDampedLaw.hpp	2009-11-27 09:27:08 +0000
+++ pkg/dem/Engine/StandAloneEngine/NewtonsDampedLaw.hpp	2009-11-30 11:14:47 +0000
@@ -38,10 +38,12 @@
 class VelocityBins;
 
 class NewtonsDampedLaw : public StandAloneEngine{
-	inline void cundallDamp(const Real& dt, const Vector3r& f, const Vector3r& velocity, Vector3r& acceleration, const Vector3r& m, const Vector3r& angularVelocity, Vector3r& angularAcceleration);
+	inline void cundallDamp(const Real& dt, const Vector3r& N, const Vector3r& V, Vector3r& A);
 	void handleClumpMember(MetaBody* ncb, const body_id_t memberId, State* clumpState);
 	bool haveBins;
-	void accurateRigidBodyRotationIntegrator(MetaBody* ncb, const shared_ptr<Body>& rb);
+	inline void lfTranslate(MetaBody* ncb, State* state, const body_id_t& id, const Real& dt); // leap-frog translate
+	inline void lfSpheralRotate(MetaBody* ncb, State* state, const body_id_t& id, const Real& dt); // leap-frog rotate of spheral body
+	inline void lfRigidBodyRotate(MetaBody* ncb, State* state, const body_id_t& id, const Real& dt, const Vector3r& M); // leap-frog rotate of rigid (non symmetric) body
 	Quaternionr DotQ(const Vector3r& angVel, const Quaternionr& Q);
 	inline void blockTranslateDOFs(unsigned blockedDOFs, Vector3r& v);
 	inline void blockRotateDOFs(unsigned blockedDOFs, Vector3r& v);