[ODE] Damage calculation

[Tomas Sakalauskas]

Hi,

I saw quite old thread about damage calculation with some hints on
implementation, but was digging analyzing ODE to actually implement it.
Maybe this simple solution will save some time for others.

I save constraint force in step function for each joint as dReal[8] vector.
Indices 0-3 store force, 4-6 - torque. Contacts generated during collision
are filled with this information after dWorldStep. 

Please comment if you know a better way of handling it.

1. Add constaint force information to joint structure (add this line to end
of dxJoint declaration in joint.h):
	dReal cforce[8];

2. Define additional matrix multiplication function in step.cpp

static void Multiply1_8q1 (dReal *A, dReal *B, dReal *C, int q)
{
  int k;
  dReal sum;
  dIASSERT (q>0 && A && B && C);
  sum = 0;
  for (k=0; k<q; k++) sum += B[k*8] * C[k];
  A[0] = sum;
  sum = 0;
  for (k=0; k<q; k++) sum += B[1+k*8] * C[k];
  A[1] = sum;
  sum = 0;
  for (k=0; k<q; k++) sum += B[2+k*8] * C[k];
  A[2] = sum;
  sum = 0;
  for (k=0; k<q; k++) sum += B[4+k*8] * C[k];
  A[4] = sum;
  sum = 0;
  for (k=0; k<q; k++) sum += B[5+k*8] * C[k];
  A[5] = sum;
  sum = 0;
  for (k=0; k<q; k++) sum += B[6+k*8] * C[k];
  A[6] = sum;
}

3. Save constaint force by adding the following line to step.cpp,
dInternalStepIsland_x2 in block starting with comment // compute cforce =
J'*lambda:

    // compute cforce = J'*lambda
    for (i=0; i<nj; i++) {
      dReal *JJ = J + 2*8*ofs[i];

      //TS: begin
      Multiply1_8q1(_joint[i]->cforce,JJ, lambda+ofs[i], info[i].m);
      //TS: end

      MultiplyAdd1_8q1 (cforce + 8*joint[i]->node[0].body->tag,JJ,
			lambda+ofs[i], info[i].m);
      if (joint[i]->node[1].body) {
	MultiplyAdd1_8q1 (cforce + 8*joint[i]->node[1].body->tag,
			  JJ + 8*info[i].m, lambda+ofs[i], info[i].m);
      }
    }
  }



Tomas