[flatland-cvs] flatland/ode/src quickstep.cpp,1.1.1.1,1.2

SourceForge Headquarters 1320 Columbia Street Suite 310 San Diego, CA 92101 +1 (858) 422-6466

Update of /cvsroot/flatland/flatland/ode/src
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv21296/ode/src

Modified Files:
	quickstep.cpp 
Log Message:
 - converted project to VStudio 2005
 - added a #include to fix compilation problem (Joel Horne)
 - added to build instructions to refer to gl and wgl headers
 - added an item to the release procedure (add a news item)
 - fixed atan bug in flatland.cpp (gorchillo)
 - fixed line-ending problem in simple.cpp (gorchillo)
 - fixed virtual destructors (gorchillo)

Index: quickstep.cpp
===================================================================
RCS file: /cvsroot/flatland/flatland/ode/src/quickstep.cpp,v
retrieving revision 1.1.1.1
retrieving revision 1.2
diff -C2 -d -r1.1.1.1 -r1.2
*** quickstep.cpp	26 Feb 2005 19:26:48 -0000	1.1.1.1
--- quickstep.cpp	23 Apr 2006 17:23:50 -0000	1.2
***************
*** 92,100 ****
  		int b2 = jb[i*2+1];
  		dReal k = body[b1]->invMass;
! 		for (j=0; j<3; j++) iMJ_ptr[j] = k*J_ptr[j];
  		dMULTIPLY0_331 (iMJ_ptr + 3, invI + 12*b1, J_ptr + 3);
  		if (b2 >= 0) {
  			k = body[b2]->invMass;
! 			for (j=0; j<3; j++) iMJ_ptr[j+6] = k*J_ptr[j+6];
  			dMULTIPLY0_331 (iMJ_ptr + 9, invI + 12*b2, J_ptr + 9);
  		}
--- 92,100 ----
  		int b2 = jb[i*2+1];
  		dReal k = body[b1]->invMass;
! 		for (j=0; j<3; j++) iMJ_ptr[j] = k*J_ptr[j]; // Philip
  		dMULTIPLY0_331 (iMJ_ptr + 3, invI + 12*b1, J_ptr + 3);
  		if (b2 >= 0) {
  			k = body[b2]->invMass;
! 			for (j=0; j<3; j++) iMJ_ptr[j+6] = k*J_ptr[j+6]; // Philip
  			dMULTIPLY0_331 (iMJ_ptr + 9, invI + 12*b2, J_ptr + 9);
  		}
***************
*** 166,277 ****
  
  //***************************************************************************
- // conjugate gradient method with jacobi preconditioner
- // THIS IS EXPERIMENTAL CODE that doesn't work too well, so it is ifdefed out.
- //
- // adding CFM seems to be critically important to this method.
- 
- #if 0
- 
- static inline dReal dot (int n, dRealPtr x, dRealPtr y)
- {
- 	dReal sum=0;
- 	for (int i=0; i<n; i++) sum += x[i]*y[i];
- 	return sum;
- }
- 
- 
- // x = y + z*alpha
- 
- static inline void add (int n, dRealMutablePtr x, dRealPtr y, dRealPtr z, dReal alpha)
- {
- 	for (int i=0; i<n; i++) x[i] = y[i] + z[i]*alpha;
- }
- 
- 
- static void CG_LCP (int m, int nb, dRealMutablePtr J, int *jb, dxBody * const *body,
- 	dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr fc, dRealMutablePtr b,
- 	dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex,
- 	dxQuickStepParameters *qs)
- {
- 	int i,j;
- 	const int num_iterations = qs->num_iterations;
- 
- 	// precompute iMJ = inv(M)*J'
- 	dRealAllocaArray (iMJ,m*12);
- 	compute_invM_JT (m,J,iMJ,jb,body,invI);
- 
- 	dReal last_rho = 0;
- 	dRealAllocaArray (r,m);
- 	dRealAllocaArray (z,m);
- 	dRealAllocaArray (p,m);
- 	dRealAllocaArray (q,m);
- 
- 	// precompute 1 / diagonals of A
- 	dRealAllocaArray (Ad,m);
- 	dRealPtr iMJ_ptr = iMJ;
- 	dRealPtr J_ptr = J;
- 	for (i=0; i<m; i++) {
- 		dReal sum = 0;
- 		for (j=0; j<6; j++) sum += iMJ_ptr[j] * J_ptr[j];
- 		if (jb[i*2+1] >= 0) {
- 			for (j=6; j<12; j++) sum += iMJ_ptr[j] * J_ptr[j];
- 		}
- 		iMJ_ptr += 12;
- 		J_ptr += 12;
- 		Ad[i] = REAL(1.0) / (sum + cfm[i]);
- 	}
- 
- #ifdef WARM_STARTING
- 	// compute residual r = b - A*lambda
- 	multiply_J_invM_JT (m,nb,J,iMJ,jb,cfm,fc,lambda,r);
- 	for (i=0; i<m; i++) r[i] = b[i] - r[i];
- #else
- 	dSetZero (lambda,m);
- 	memcpy (r,b,m*sizeof(dReal));		// residual r = b - A*lambda
- #endif
- 	
- 	for (int iteration=0; iteration < num_iterations; iteration++) {
- 		for (i=0; i<m; i++) z[i] = r[i]*Ad[i];	// z = inv(M)*r
- 		dReal rho = dot (m,r,z);		// rho = r'*z
- 		
- 		// @@@
- 		// we must check for convergence, otherwise rho will go to 0 if
- 		// we get an exact solution, which will introduce NaNs into the equations.
- 		if (rho < 1e-10) {
- 			printf ("CG returned at iteration %d\n",iteration);
- 			break;
- 		}
- 		
- 		if (iteration==0) {
- 			memcpy (p,z,m*sizeof(dReal));	// p = z
- 		}
- 		else {
- 			add (m,p,z,p,rho/last_rho);	// p = z + (rho/last_rho)*p
- 		}
- 		
- 		// compute q = (J*inv(M)*J')*p
- 		multiply_J_invM_JT (m,nb,J,iMJ,jb,cfm,fc,p,q);
- 	
- 		dReal alpha = rho/dot (m,p,q);		// alpha = rho/(p'*q)
- 		add (m,lambda,lambda,p,alpha);		// lambda = lambda + alpha*p
- 		add (m,r,r,q,-alpha);			// r = r - alpha*q
- 		last_rho = rho;
- 	}
- 
- 	// compute fc = inv(M)*J'*lambda
- 	multiply_invM_JT (m,nb,iMJ,jb,lambda,fc);
- 
- #if 0
- 	// measure solution error
- 	multiply_J_invM_JT (m,nb,J,iMJ,jb,cfm,fc,lambda,r);
- 	dReal error = 0;
- 	for (i=0; i<m; i++) error += dFabs(r[i] - b[i]);
- 	printf ("lambda error = %10.6e\n",error);
- #endif
- }
- 
- #endif
- 
- //***************************************************************************
  // SOR-LCP method
  
--- 166,169 ----
***************
*** 562,566 ****
  			body[i]->facc[0] += body[i]->mass.mass * world->gravity[0];
  			body[i]->facc[1] += body[i]->mass.mass * world->gravity[1];
! 			body[i]->facc[2] += body[i]->mass.mass * world->gravity[2];
  		}
  	}
--- 454,459 ----
  			body[i]->facc[0] += body[i]->mass.mass * world->gravity[0];
  			body[i]->facc[1] += body[i]->mass.mass * world->gravity[1];
! 			//body[i]->facc[2] += body[i]->mass.mass * world->gravity[2]; // Philip
!             body[i]->facc[2] = 0;
  		}
  	}
***************
*** 663,669 ****
  		for (i=0; i<nb; i++) {
  			dReal body_invMass = body[i]->invMass;
! 			for (j=0; j<3; j++) tmp1[i*6+j] = body[i]->facc[j] * body_invMass + body[i]->lvel[j] * stepsize1;
  			dMULTIPLY0_331 (tmp1 + i*6 + 3,invI + i*12,body[i]->tacc);
! 			for (j=0; j<3; j++) tmp1[i*6+3+j] += body[i]->avel[j] * stepsize1;
  		}
  
--- 556,562 ----
  		for (i=0; i<nb; i++) {
  			dReal body_invMass = body[i]->invMass;
! 			for (j=0; j<3; j++) tmp1[i*6+j] = body[i]->facc[j] * body_invMass + body[i]->lvel[j] * stepsize1; // Philip
  			dMULTIPLY0_331 (tmp1 + i*6 + 3,invI + i*12,body[i]->tacc);
! 			for (j=0; j<3; j++) tmp1[i*6+3+j] += body[i]->avel[j] * stepsize1; // Philip
  		}
  
***************
*** 706,711 ****
  		// add stepsize * cforce to the body velocity
  		for (i=0; i<nb; i++) {
! 			for (j=0; j<3; j++) body[i]->lvel[j] += stepsize * cforce[i*6+j];
! 			for (j=0; j<3; j++) body[i]->avel[j] += stepsize * cforce[i*6+3+j];
  		}
  
--- 599,604 ----
  		// add stepsize * cforce to the body velocity
  		for (i=0; i<nb; i++) {
! 			for (j=0; j<3; j++) body[i]->lvel[j] += stepsize * cforce[i*6+j]; // Philip
! 			for (j=0; j<3; j++) body[i]->avel[j] += stepsize * cforce[i*6+3+j]; // Philip
  		}
  
***************
*** 722,731 ****
  					cforce [i*6+0] *= k;
  					cforce [i*6+1] *= k;
! 					cforce [i*6+2] *= k;
  					dVector3 tmp;
  					dMULTIPLY0_331 (tmp, I + 12*i, cforce + i*6 + 3);
  					cforce [i*6+3] = tmp[0];
  					cforce [i*6+4] = tmp[1];
! 					cforce [i*6+5] = tmp[2];
  				}
  				// compute feedback for this and all remaining joints
--- 615,626 ----
  					cforce [i*6+0] *= k;
  					cforce [i*6+1] *= k;
! 					//cforce [i*6+2] *= k; // Philip
!                     cforce [i*6+2] *= 0;
  					dVector3 tmp;
  					dMULTIPLY0_331 (tmp, I + 12*i, cforce + i*6 + 3);
  					cforce [i*6+3] = tmp[0];
  					cforce [i*6+4] = tmp[1];
! 					//cforce [i*6+5] = tmp[2]; // Philip
!                     cforce [i*6+5] = 0; // Philip
  				}
  				// compute feedback for this and all remaining joints
***************
*** 753,775 ****
  	for (i=0; i<nb; i++) {
  		dReal body_invMass = body[i]->invMass;
! 		for (j=0; j<3; j++) body[i]->lvel[j] += stepsize * body_invMass * body[i]->facc[j];
! 		for (j=0; j<3; j++) body[i]->tacc[j] *= stepsize;	
  		dMULTIPLYADD0_331 (body[i]->avel,invI + i*12,body[i]->tacc);
  	}
  
- #if 0
- 	// check that the updated velocity obeys the constraint (this check needs unmodified J)
- 	dRealAllocaArray (vel,nb*6);
- 	for (i=0; i<nb; i++) {
- 		for (j=0; j<3; j++) vel[i*6+j] = body[i]->lvel[j];
- 		for (j=0; j<3; j++) vel[i*6+3+j] = body[i]->avel[j];
- 	}
- 	dRealAllocaArray (tmp,m);
- 	multiply_J (m,J,jb,vel,tmp);
- 	dReal error = 0;
- 	for (i=0; i<m; i++) error += dFabs(tmp[i]);
- 	printf ("velocity error = %10.6e\n",error);
- #endif
- 
  	// update the position and orientation from the new linear/angular velocity
  	// (over the given timestep)
--- 648,656 ----
  	for (i=0; i<nb; i++) {
  		dReal body_invMass = body[i]->invMass;
! 		for (j=0; j<3; j++) body[i]->lvel[j] += stepsize * body_invMass * body[i]->facc[j]; // Philip
! 		for (j=0; j<3; j++) body[i]->tacc[j] *= stepsize;	 // Philip
  		dMULTIPLYADD0_331 (body[i]->avel,invI + i*12,body[i]->tacc);
  	}
  
  	// update the position and orientation from the new linear/angular velocity
  	// (over the given timestep)
***************
*** 781,786 ****
  	// zero all force accumulators
  	for (i=0; i<nb; i++) {
! 		dSetZero (body[i]->facc,3);
! 		dSetZero (body[i]->tacc,3);
  	}
  
--- 662,667 ----
  	// zero all force accumulators
  	for (i=0; i<nb; i++) {
! 		dSetZero (body[i]->facc,3); // Philip
! 		dSetZero (body[i]->tacc,3); // Philip
  	}