dxGeom::dxGeom (dSpaceID _space, int is_placeable)
{
  initColliders();

  // setup body vars. invalid type of -1 must be changed by the constructor.
  type = -1;
  gflags = GEOM_DIRTY | GEOM_AABB_BAD | GEOM_ENABLED;
  if (is_placeable) gflags |= GEOM_PLACEABLE;
  data = 0;
  body = 0;
  body_next = 0;
  if (is_placeable) {
    dxPosR *pr = (dxPosR*) dAlloc (sizeof(dxPosR));
    pos = pr->pos;
    R = pr->R;
    dSetZero (pos,4);
    dRSetIdentity (R);
  }
  else {
    pos = 0;
    R = 0;
  }

  // setup space vars
  next = 0;
  tome = 0;
  parent_space = 0;
  dSetZero (aabb,6);
  category_bits = ~0;
  collide_bits = ~0;

  // put this geom in a space if required
  if (_space) dSpaceAdd (_space,this);
}

dxJoint::dxJoint( dxWorld *w ) :
        dObject( w )
{
    //printf("constructing %p\n", this);
    dIASSERT( w );
    flags = 0;
    node[0].joint = this;
    node[0].body = 0;
    node[0].next = 0;
    node[1].joint = this;
    node[1].body = 0;
    node[1].next = 0;
    dSetZero( lambda, 6 );
    dSetZero( lambda_erp, 6 );

    addObjectToList( this, ( dObject ** ) &w->firstjoint );

    w->nj++;
    feedback = 0;

    // Moved here by OSRF
    // Default to negative value, which means the current global value
    // will be used. If set non-negative, then this joint-specific 
    // value will be used.
    erp = -1;  // world->global_erp;
    cfm = -1;  // world->global_cfm;
}

int _dInvertPDMatrix (const dReal *A, dReal *Ainv, int n, void *tmpbuf/*[nskip*(n+2)]*/)
{
  dAASSERT (n > 0 && A && Ainv);
  bool success = false;
  size_t FactorCholesky_size = _dEstimateFactorCholeskyTmpbufSize(n);
  size_t SolveCholesky_size = _dEstimateSolveCholeskyTmpbufSize(n);
  size_t MaxCholesky_size = FactorCholesky_size > SolveCholesky_size ? FactorCholesky_size : SolveCholesky_size;
  dIASSERT(MaxCholesky_size % sizeof(dReal) == 0);
  const int nskip = dPAD (n);
  const int nskip_mul_n = nskip*n;
  dReal *tmp = tmpbuf ? (dReal *)tmpbuf : (dReal*) ALLOCA (MaxCholesky_size + (nskip + nskip_mul_n)*sizeof(dReal));
  dReal *X = (dReal *)((char *)tmp + MaxCholesky_size);
  dReal *L = X + nskip;
  memcpy (L, A, nskip_mul_n*sizeof(dReal));
  if (dFactorCholesky (L,n,tmp)) {
    dSetZero (Ainv,nskip_mul_n);	// make sure all padding elements set to 0
    dReal *aa = Ainv, *xi = X, *xiend = X + n;
    for (; xi != xiend; ++aa, ++xi) {
      dSetZero(X, n);
      *xi = REAL(1.0);
      dSolveCholesky (L,X,n,tmp);
      dReal *a = aa;
      const dReal *x = X, *xend = X + n;
      for (; x!=xend; a+=nskip, ++x) {
        *a = *x;
      }
    }
    success = true;
  }
  return success ? 1 : 0;  
}

void testMatrixMultiply()
{
  // A is 2x3, B is 3x4, B2 is B except stored columnwise, C is 2x4
  dReal A[8],B[12],A2[12],B2[16],C[8];
  int i;

  HEADER;
  dSetZero (A,8);
  for (i=0; i<3; i++) A[i] = i+2;
  for (i=0; i<3; i++) A[i+4] = i+3+2;
  for (i=0; i<12; i++) B[i] = i+8;
  dSetZero (A2,12);
  for (i=0; i<6; i++) A2[i+2*(i/2)] = A[i+i/3];
  dSetZero (B2,16);
  for (i=0; i<12; i++) B2[i+i/3] = B[i];

  dMultiply0 (C,A,B,2,3,4);
  if (C[0] != 116 || C[1] != 125 || C[2] != 134 || C[3] != 143 ||
      C[4] != 224 || C[5] != 242 || C[6] != 260 || C[7] != 278)
    printf ("\tFAILED (1)\n"); else printf ("\tpassed (1)\n");

  dMultiply1 (C,A2,B,2,3,4);
  if (C[0] != 160 || C[1] != 172 || C[2] != 184 || C[3] != 196 ||
      C[4] != 196 || C[5] != 211 || C[6] != 226 || C[7] != 241)
    printf ("\tFAILED (2)\n"); else printf ("\tpassed (2)\n");

  dMultiply2 (C,A,B2,2,3,4);
  if (C[0] != 83 || C[1] != 110 || C[2] != 137 || C[3] != 164 ||
      C[4] != 164 || C[5] != 218 || C[6] != 272 || C[7] != 326)
    printf ("\tFAILED (3)\n"); else printf ("\tpassed (3)\n");
}

dxGeom::dxGeom (dSpaceID _space, int is_placeable)
{
    // setup body vars. invalid type of -1 must be changed by the constructor.
    type = -1;
    gflags = GEOM_DIRTY | GEOM_AABB_BAD | GEOM_ENABLED;
    if (is_placeable) gflags |= GEOM_PLACEABLE;
    data = 0;
    body = 0;
    body_next = 0;
    if (is_placeable) {
        final_posr = dAllocPosr();
        dSetZero (final_posr->pos,4);
        dRSetIdentity (final_posr->R);
    }
    else {
        final_posr = 0;
    }
    offset_posr = 0;

    // setup space vars
    next = 0;
    tome = 0;
    next_ex = 0;
    tome_ex = 0;
    parent_space = 0;
    dSetZero (aabb,6);
    category_bits = ~0;
    collide_bits = ~0;

    // put this geom in a space if required
    if (_space) dSpaceAdd (_space,this);
}

void dMassSetZero (dMass *m)
{
  dAASSERT (m);
  m->mass = REAL(0.0);
  dSetZero (m->c,sizeof(m->c) / sizeof(dReal));
  dSetZero (m->I,sizeof(m->I) / sizeof(dReal));
}

dxJointBall::dxJointBall( dxWorld *w ) :
        dxJoint( w )
{
    dSetZero( anchor1, 4 );
    dSetZero( anchor2, 4 );
    erp = world->global_erp;
    cfm = world->global_cfm;
}

dxJointFixed::dxJointFixed ( dxWorld *w ) :
        dxJoint ( w )
{
    dSetZero ( offset, 4 );
    dSetZero ( qrel, 4 );
    erp = world->global_erp;
    cfm = world->global_cfm;
}

dxJointFixed::dxJointFixed ( dxWorld *w ) :
        dxJoint ( w )
{
    dSetZero ( offset, 4 );
    dSetZero ( qrel, 4 );
    // These are now set in dxJoint constructor
    // erp = world->global_erp;
    // cfm = world->global_cfm;
}

dxJointDBall::dxJointDBall(dxWorld *w) :
    dxJoint(w)
{
    dSetZero(anchor1, 3);
    dSetZero(anchor2, 3);
    targetDistance = 0;
    erp = world->global_erp;
    cfm = world->global_cfm;
}

dxJointSlider::dxJointSlider ( dxWorld *w ) :
    dxJoint ( w )
{
    dSetZero ( axis1, 4 );
    axis1[0] = 1;
    dSetZero ( qrel, 4 );
    dSetZero ( offset, 4 );
    limot.init ( world );
}

dxJointGearbox::dxJointGearbox(dxWorld* w) :
    dxJoint(w)
{
    ratio = 1.0;
    flags |= dJOINT_TWOBODIES;
    dSetZero( qrel1, 4 );
    dSetZero( qrel2, 4 );
    cumulative_angle1 = 0.0;
    cumulative_angle2 = 0.0;
}

dxPlanarJoint::dxPlanarJoint(dxWorld *world) :
        dxJoint(world)
{
    dSetZero(anchor, 3);
    dCopyVector3(anchor1, anchor);
    dCopyVector3(anchor2, anchor);

    // default to plane z=0
    dSetZero(planeNormal, 3);
    planeNormal[2] = 1;
    dCopyVector3(axis1, planeNormal);
    dCopyVector3(axis2, planeNormal);
}

dxJointHinge::dxJointHinge( dxWorld *w ) :
        dxJoint( w )
{
    dSetZero( anchor1, 4 );
    dSetZero( anchor2, 4 );
    dSetZero( axis1, 4 );
    axis1[0] = 1;
    dSetZero( axis2, 4 );
    axis2[0] = 1;
    dSetZero( qrel, 4 );
    limot.init( world );
    cumulative_angle = 0;
}

dLCP::dLCP (int _n, int _nub, dReal *_Adata, dReal *_x, dReal *_b, dReal *_w,
	    dReal *_lo, dReal *_hi, dReal *_L, dReal *_d,
	    dReal *_Dell, dReal *_ell, dReal *_tmp,
	    int *_state, int *_findex, int *_p, int *_C, dReal **Arows)
{
  dUASSERT (_findex==0,"slow dLCP object does not support findex array");

  n = _n;
  nub = _nub;
  Adata = _Adata;
  A = 0;
  x = _x;
  b = _b;
  w = _w;
  lo = _lo;
  hi = _hi;
  nskip = dPAD(n);
  dSetZero (x,n);
  last_i_for_solve1 = -1;

  int i,j;
  C.setSize (n);
  N.setSize (n);
  for (int i=0; i<n; i++) {
    C[i] = 0;
    N[i] = 0;
  }

# ifdef ROWPTRS
  // make matrix row pointers
  A = Arows;
  for (i=0; i<n; i++) A[i] = Adata + i*nskip;
# else
  A = Adata;
# endif

  // lets make A symmetric
  for (i=0; i<n; i++) {
    for (j=i+1; j<n; j++) AROW(i)[j] = AROW(j)[i];
  }

  // if nub>0, put all indexes 0..nub-1 into C and solve for x
  if (nub > 0) {
    for (i=0; i<nub; i++) memcpy (_L+i*nskip,AROW(i),(i+1)*sizeof(dReal));
    dFactorLDLT (_L,_d,nub,nskip);
    memcpy (x,b,nub*sizeof(dReal));
    dSolveLDLT (_L,_d,x,nub,nskip);
    dSetZero (_w,nub);
    for (i=0; i<nub; i++) C[i] = 1;
  }
}

dxJointScrew::dxJointScrew( dxWorld *w ) :
        dxJoint( w )
{
    dSetZero( anchor1, 4 );
    dSetZero( anchor2, 4 );
    dSetZero( axis1, 4 );
    axis1[0] = 1;
    dSetZero( axis2, 4 );
    axis2[0] = 1;
    dSetZero( qrel, 4 );
    limot.init( world );
    cumulative_angle = 0;
    thread_pitch = 1.0;  // rad/m (3141.6 rad/m is about 0.002 m/rev)
}

void testLDLTAddTL()
{
  int i,j;
  dReal A[MSIZE4*MSIZE], L[MSIZE4*MSIZE], d[MSIZE], a[MSIZE],
    DL[MSIZE4*MSIZE], ATEST[MSIZE4*MSIZE], diff;
  HEADER;

  dMakeRandomMatrix (A,MSIZE,MSIZE,1.0);
  dMultiply2 (L,A,A,MSIZE,MSIZE,MSIZE);
  memcpy (A,L,MSIZE4*MSIZE*sizeof(dReal));
  dFactorLDLT (L,d,MSIZE,MSIZE4);

  // delete first row and column of factorization
  for (i=0; i<MSIZE; i++) a[i] = -A[i*MSIZE4];
  a[0] += 1;
  dLDLTAddTL (L,d,a,MSIZE,MSIZE4);
  for (i=1; i<MSIZE; i++) L[i*MSIZE4] = 0;
  d[0] = 1;

  // get modified L*D*L'
  dClearUpperTriangle (L,MSIZE);
  for (i=0; i<MSIZE; i++) L[i*MSIZE4+i] = 1.0;
  dSetZero (DL,MSIZE4*MSIZE);
  for (i=0; i<MSIZE; i++) {
    for (j=0; j<MSIZE; j++) DL[i*MSIZE4+j] = L[i*MSIZE4+j] / d[j];
  }
  dMultiply2 (ATEST,L,DL,MSIZE,MSIZE,MSIZE);

  // compare it to A with its first row/column removed
  for (i=1; i<MSIZE; i++) A[i*MSIZE4] = A[i] = 0;
  A[0] = 1;
  diff = dMaxDifference(A,ATEST,MSIZE,MSIZE);
  printf ("\tmaximum difference = %.6e - %s\n",diff,
	  diff > tol ? "FAILED" : "passed");
}

void testInvertPDMatrix()
{
  int i,j,ok;
  dReal A[MSIZE4*MSIZE], Ainv[MSIZE4*MSIZE], I[MSIZE4*MSIZE];
  HEADER;

  dMakeRandomMatrix (A,MSIZE,MSIZE,1.0);
  dMultiply2 (Ainv,A,A,MSIZE,MSIZE,MSIZE);
  memcpy (A,Ainv,MSIZE4*MSIZE*sizeof(dReal));
  dSetZero (Ainv,MSIZE4*MSIZE);

  if (dInvertPDMatrix (A,Ainv,MSIZE))
    printf ("\tpassed (1)\n"); else printf ("\tFAILED (1)\n");
  dMultiply0 (I,A,Ainv,MSIZE,MSIZE,MSIZE);

  // compare with identity
  ok = 1;
  for (i=0; i<MSIZE; i++) {
    for (j=0; j<MSIZE; j++) {
      if (i != j) if (cmp (I[i*MSIZE4+j],0.0)==0) ok = 0;
    }
  }
  for (i=0; i<MSIZE; i++) {
    if (cmp (I[i*MSIZE4+i],1.0)==0) ok = 0;
  }
  if (ok) printf ("\tpassed (2)\n"); else printf ("\tFAILED (2)\n");
}

dxJointAMotor::dxJointAMotor( dxWorld *w ) :
        dxJoint( w )
{
    int i;
    num = 0;
    mode = dAMotorUser;
    for ( i = 0; i < 3; i++ )
    {
        rel[i] = 0;
        dSetZero( axis[i], 4 );
        limot[i].init( world );
        angle[i] = 0;
    }
    dSetZero( reference1, 4 );
    dSetZero( reference2, 4 );
}

dMatrix::dMatrix (int rows, int cols)
{
  if (rows < 1 || cols < 1) dDebug (0,"bad matrix size");
  n = rows;
  m = cols;
  data = (dReal*) dAlloc (n*m*sizeof(dReal));
  dSetZero (data,n*m);
}