Function dxQuickStepper

3rd_party/Src/ode/ode/src/quickstep.cpp:759–1082

*/

void dxQuickStepper (dxWorld *world, dxBody * const *body, int nb,
		     dxJoint **joint, int nj, dReal stepsize)
{
	
	int i,j;
	IFTIMING(dTimerStart("preprocessing");)

	dReal stepsize1 = dRecip(stepsize);

	// number all bodies in the body list - set their tag values
	for (i=0; i<nb; i++) body[i]->tag = i;
	
	// make a local copy of the joint array, because we might want to modify it.
	// (the "dxJoint *const*" declaration says we're allowed to modify the joints
	// but not the joint array, because the caller might need it unchanged).
	//@@@ do we really need to do this? we'll be sorting constraint rows individually, not joints
	//dxJoint **joint = (dxJoint**) alloca (nj * sizeof(dxJoint*));//slipch@ - no need  to copy with sipmle island processing
	//memcpy (joint,_joint,nj * sizeof(dxJoint*));
	
	// for all bodies, compute the inertia tensor and its inverse in the global
	// frame, and compute the rotational force and add it to the torque
	// accumulator. I and invI are a vertical stack of 3x4 matrices, one per body.
	dRealAllocaArray (I,3*4*nb);	// need to remember all I's for feedback purposes only
	dRealAllocaArray (invI,3*4*nb);
	for (i=0; i<nb; i++) {

		dMatrix3 tmp;
		dxBody	*b=body[i];
#ifdef DEBUG_VALID
		dIASSERT(dValid(b->tacc[0])&&dValid(b->tacc[1])&&dValid(b->tacc[2]));
#endif
		// compute inertia tensor in global frame
		dMULTIPLY2_333 (tmp,b->mass.I,body[i]->R);
		dMULTIPLY0_333 (I+i*12,b->R,tmp);
		// compute inverse inertia tensor in global frame
		dMULTIPLY2_333 (tmp,b->invI,b->R);
		dMULTIPLY0_333 (invI+i*12,b->R,tmp);
		// compute rotational force
		dMULTIPLY0_331 (tmp,I+i*12,b->avel);

		dCROSS (b->tacc,-=,b->avel,tmp);
#ifndef dNODEBUG
		if(!(dValid(b->tacc[0])&&dValid(b->tacc[1])&&dValid(b->tacc[2])))
		{
			//char s[64];
			//_snprintf (s,sizeof(s),"tmp %f,%f,%f \n avel %f,%f,%f",tmp[0],tmp[1],tmp[2],b->avel[0],b->avel[1],b->avel[2]);
			//dUASSERT(0,"tmp %f,%f,%f \n avel %f,%f,%f",tmp[0],tmp[1],tmp[2],b->avel[0],b->avel[1],b->avel[2]);

			dDebug (d_ERR_UASSERT," (%s:%d \n tmp %f,%f,%f \n avel %f,%f,%f)", __FILE__,__LINE__,tmp[0],tmp[1],tmp[2],b->avel[0],b->avel[1],b->avel[2]);
		}

#endif
	}

	// add the gravity force to all bodies
	for (i=0; i<nb; i++) {
		if ((body[i]->flags & dxBodyNoGravity)==0) {
			body[i]->facc[0] += body[i]->mass.mass * world->gravity[0];