static void space_geom_collider (void *data, dxGeom *o1, dxGeom *o2)
{
SpaceGeomColliderData *d = (SpaceGeomColliderData*) data;
if (d->flags & NUMC_MASK) {
int n = dCollide (o1,o2,d->flags,d->contact,d->skip);
d->contact = CONTACT (d->contact,d->skip*n);
d->flags -= n;
#ifndef dNODEBUG
if(d->flags<=0)
{
//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]);
dMessage(0, "Collider exceed contact buffer");
}
#endif
}
}
void resetSimulation()
{
int i;
i = 0;
// destroy world if it exists
if (bodies)
{
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
}
for (i = 0; i < 1000; i++)
wb_stepsdis[i] = 0;
// recreate world
world = dWorldCreate();
// space = dHashSpaceCreate( 0 );
// space = dSimpleSpaceCreate( 0 );
space = dSweepAndPruneSpaceCreate( 0, dSAP_AXES_XYZ );
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-1.5);
dWorldSetCFM (world, 1e-5);
dWorldSetERP (world, 0.8);
dWorldSetQuickStepNumIterations (world,ITERS);
ground = dCreatePlane (space,0,0,1,0);
bodies = 0;
joints = 0;
boxes = 0;
spheres = 0;
wb = 0;
#ifdef CARS
for (dReal x = 0.0; x < COLS*(LENGTH+RADIUS); x += LENGTH+RADIUS)
for (dReal y = -((ROWS-1)*(WIDTH/2+RADIUS)); y <= ((ROWS-1)*(WIDTH/2+RADIUS)); y += WIDTH+RADIUS*2)
makeCar(x, y, bodies, joints, boxes, spheres);
#endif
#ifdef WALL
bool offset = false;
for (dReal z = WBOXSIZE/2.0; z <= WALLHEIGHT; z+=WBOXSIZE)
{
offset = !offset;
for (dReal y = (-WALLWIDTH+z)/2; y <= (WALLWIDTH-z)/2; y+=WBOXSIZE)
{
wall_bodies[wb] = dBodyCreate (world);
dBodySetPosition (wall_bodies[wb],-20,y,z);
dMassSetBox (&m,1,WBOXSIZE,WBOXSIZE,WBOXSIZE);
dMassAdjust (&m, WALLMASS);
dBodySetMass (wall_bodies[wb],&m);
wall_boxes[wb] = dCreateBox (space,WBOXSIZE,WBOXSIZE,WBOXSIZE);
dGeomSetBody (wall_boxes[wb],wall_bodies[wb]);
//dBodyDisable(wall_bodies[wb++]);
wb++;
}
}
dMessage(0,"wall boxes: %i", wb);
#endif
#ifdef BALLS
for (dReal x = -7; x <= -4; x+=1)
for (dReal y = -1.5; y <= 1.5; y+=1)
for (dReal z = 1; z <= 4; z+=1)
{
b = dBodyCreate (world);
dBodySetPosition (b,x*RADIUS*2,y*RADIUS*2,z*RADIUS*2);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m, BALLMASS);
dBodySetMass (b,&m);
sphere[spheres] = dCreateSphere (space,RADIUS);
dGeomSetBody (sphere[spheres++],b);
}
#endif
#ifdef ONEBALL
b = dBodyCreate (world);
dBodySetPosition (b,0,0,2);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m, 1);
dBodySetMass (b,&m);
sphere[spheres] = dCreateSphere (space,RADIUS);
dGeomSetBody (sphere[spheres++],b);
#endif
#ifdef BALLSTACK
for (dReal z = 1; z <= 6; z+=1)
{
b = dBodyCreate (world);
dBodySetPosition (b,0,0,z*RADIUS*2);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m, 0.1);
dBodySetMass (b,&m);
sphere[spheres] = dCreateSphere (space,RADIUS);
dGeomSetBody (sphere[spheres++],b);
}
#endif
#ifdef CENTIPEDE
dBodyID lastb = 0;
for (dReal y = 0; y < 10*LENGTH; y+=LENGTH+0.1)
{
// chassis body
b = body[bodies] = dBodyCreate (world);
dBodySetPosition (body[bodies],-15,y,STARTZ);
dMassSetBox (&m,1,WIDTH,LENGTH,HEIGHT);
dMassAdjust (&m,CMASS);
dBodySetMass (body[bodies],&m);
box[boxes] = dCreateBox (space,WIDTH,LENGTH,HEIGHT);
dGeomSetBody (box[boxes++],body[bodies++]);
for (dReal x = -17; x > -20; x-=RADIUS*2)
{
body[bodies] = dBodyCreate (world);
dBodySetPosition(body[bodies], x, y, STARTZ);
dMassSetSphere(&m, 1, RADIUS);
dMassAdjust(&m, WMASS);
dBodySetMass(body[bodies], &m);
sphere[spheres] = dCreateSphere (space, RADIUS);
dGeomSetBody (sphere[spheres++], body[bodies]);
joint[joints] = dJointCreateHinge2 (world,0);
if (x == -17)
dJointAttach (joint[joints],b,body[bodies]);
else
dJointAttach (joint[joints],body[bodies-2],body[bodies]);
const dReal *a = dBodyGetPosition (body[bodies++]);
dJointSetHinge2Anchor (joint[joints],a[0],a[1],a[2]);
dJointSetHinge2Axis1 (joint[joints],0,0,1);
dJointSetHinge2Axis2 (joint[joints],1,0,0);
dJointSetHinge2Param (joint[joints],dParamSuspensionERP,1.0);
dJointSetHinge2Param (joint[joints],dParamSuspensionCFM,1e-5);
dJointSetHinge2Param (joint[joints],dParamLoStop,0);
dJointSetHinge2Param (joint[joints],dParamHiStop,0);
dJointSetHinge2Param (joint[joints],dParamVel2,-10.0);
dJointSetHinge2Param (joint[joints++],dParamFMax2,FMAX);
body[bodies] = dBodyCreate (world);
dBodySetPosition(body[bodies], -30 - x, y, STARTZ);
dMassSetSphere(&m, 1, RADIUS);
dMassAdjust(&m, WMASS);
dBodySetMass(body[bodies], &m);
sphere[spheres] = dCreateSphere (space, RADIUS);
dGeomSetBody (sphere[spheres++], body[bodies]);
joint[joints] = dJointCreateHinge2 (world,0);
if (x == -17)
dJointAttach (joint[joints],b,body[bodies]);
else
dJointAttach (joint[joints],body[bodies-2],body[bodies]);
const dReal *b = dBodyGetPosition (body[bodies++]);
dJointSetHinge2Anchor (joint[joints],b[0],b[1],b[2]);
dJointSetHinge2Axis1 (joint[joints],0,0,1);
dJointSetHinge2Axis2 (joint[joints],1,0,0);
dJointSetHinge2Param (joint[joints],dParamSuspensionERP,1.0);
dJointSetHinge2Param (joint[joints],dParamSuspensionCFM,1e-5);
dJointSetHinge2Param (joint[joints],dParamLoStop,0);
dJointSetHinge2Param (joint[joints],dParamHiStop,0);
dJointSetHinge2Param (joint[joints],dParamVel2,10.0);
dJointSetHinge2Param (joint[joints++],dParamFMax2,FMAX);
}
if (lastb)
{
dJointID j = dJointCreateFixed(world,0);
dJointAttach (j, b, lastb);
dJointSetFixed(j);
}
lastb = b;
}
#endif
#ifdef BOX
body[bodies] = dBodyCreate (world);
dBodySetPosition (body[bodies],0,0,HEIGHT/2);
dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT);
dMassAdjust (&m, 1);
dBodySetMass (body[bodies],&m);
box[boxes] = dCreateBox (space,LENGTH,WIDTH,HEIGHT);
dGeomSetBody (box[boxes++],body[bodies++]);
#endif
#ifdef CANNON
cannon_ball_body = dBodyCreate (world);
cannon_ball_geom = dCreateSphere (space,CANNON_BALL_RADIUS);
dMassSetSphereTotal (&m,CANNON_BALL_MASS,CANNON_BALL_RADIUS);
dBodySetMass (cannon_ball_body,&m);
dGeomSetBody (cannon_ball_geom,cannon_ball_body);
dBodySetPosition (cannon_ball_body,CANNON_X,CANNON_Y,CANNON_BALL_RADIUS);
#endif
}
bool dSolveLCP (int n, dReal *A, dReal *x, dReal *b,
dReal *outer_w/*=nullptr*/, int nub, dReal *lo, dReal *hi, int *findex, bool earlyTermination)
{
dAASSERT (n>0 && A && x && b && lo && hi && nub >= 0 && nub <= n);
# ifndef dNODEBUG
{
// check restrictions on lo and hi
for (int k=0; k<n; ++k) dIASSERT (lo[k] <= 0 && hi[k] >= 0);
}
# endif
// if all the variables are unbounded then we can just factor, solve,
// and return
if (nub >= n) {
dReal *d = new dReal[n];
dSetZero (d, n);
int nskip = dPAD(n);
dFactorLDLT (A, d, n, nskip);
dSolveLDLT (A, d, b, n, nskip);
memcpy (x, b, n*sizeof(dReal));
delete[] d;
return true;
}
const int nskip = dPAD(n);
dReal *L = new dReal[ (n*nskip)];
dReal *d = new dReal[ (n)];
dReal *w = outer_w ? outer_w : (new dReal[n]);
dReal *delta_w = new dReal[ (n)];
dReal *delta_x = new dReal[ (n)];
dReal *Dell = new dReal[ (n)];
dReal *ell = new dReal[ (n)];
#ifdef ROWPTRS
dReal **Arows = new dReal* [n];
#else
dReal **Arows = nullptr;
#endif
int *p = new int[n];
int *C = new int[n];
// for i in N, state[i] is 0 if x(i)==lo(i) or 1 if x(i)==hi(i)
bool *state = new bool[n];
// create LCP object. note that tmp is set to delta_w to save space, this
// optimization relies on knowledge of how tmp is used, so be careful!
dLCP lcp(n,nskip,nub,A,x,b,w,lo,hi,L,d,Dell,ell,delta_w,state,findex,p,C,Arows);
int adj_nub = lcp.getNub();
// loop over all indexes adj_nub..n-1. for index i, if x(i),w(i) satisfy the
// LCP conditions then i is added to the appropriate index set. otherwise
// x(i),w(i) is driven either +ve or -ve to force it to the valid region.
// as we drive x(i), x(C) is also adjusted to keep w(C) at zero.
// while driving x(i) we maintain the LCP conditions on the other variables
// 0..i-1. we do this by watching out for other x(i),w(i) values going
// outside the valid region, and then switching them between index sets
// when that happens.
bool hit_first_friction_index = false;
for (int i=adj_nub; i<n; ++i) {
bool s_error = false;
// the index i is the driving index and indexes i+1..n-1 are "dont care",
// i.e. when we make changes to the system those x's will be zero and we
// don't care what happens to those w's. in other words, we only consider
// an (i+1)*(i+1) sub-problem of A*x=b+w.
// if we've hit the first friction index, we have to compute the lo and
// hi values based on the values of x already computed. we have been
// permuting the indexes, so the values stored in the findex vector are
// no longer valid. thus we have to temporarily unpermute the x vector.
// for the purposes of this computation, 0*infinity = 0 ... so if the
// contact constraint's normal force is 0, there should be no tangential
// force applied.
if (!hit_first_friction_index && findex && findex[i] >= 0) {
// un-permute x into delta_w, which is not being used at the moment
for (int j=0; j<n; ++j) delta_w[p[j]] = x[j];
// set lo and hi values
for (int k=i; k<n; ++k) {
dReal wfk = delta_w[findex[k]];
if (wfk == 0) {
hi[k] = 0;
lo[k] = 0;
}
else {
hi[k] = dFabs (hi[k] * wfk);
lo[k] = -hi[k];
}
}
hit_first_friction_index = true;
}
// thus far we have not even been computing the w values for indexes
// greater than i, so compute w[i] now.
w[i] = lcp.AiC_times_qC (i,x) + lcp.AiN_times_qN (i,x) - b[i];
// if lo=hi=0 (which can happen for tangential friction when normals are
// 0) then the index will be assigned to set N with some state. however,
// set C's line has zero size, so the index will always remain in set N.
// with the "normal" switching logic, if w changed sign then the index
// would have to switch to set C and then back to set N with an inverted
// state. this is pointless, and also computationally expensive. to
// prevent this from happening, we use the rule that indexes with lo=hi=0
// will never be checked for set changes. this means that the state for
// these indexes may be incorrect, but that doesn't matter.
// see if x(i),w(i) is in a valid region
if (lo[i]==0 && w[i] >= 0) {
lcp.transfer_i_to_N (i);
state[i] = false;
}
else if (hi[i]==0 && w[i] <= 0) {
lcp.transfer_i_to_N (i);
state[i] = true;
}
else if (w[i]==0) {
// this is a degenerate case. by the time we get to this test we know
// that lo != 0, which means that lo < 0 as lo is not allowed to be +ve,
// and similarly that hi > 0. this means that the line segment
// corresponding to set C is at least finite in extent, and we are on it.
// NOTE: we must call lcp.solve1() before lcp.transfer_i_to_C()
lcp.solve1 (delta_x,i,0,1);
lcp.transfer_i_to_C (i);
}
else {
// we must push x(i) and w(i)
for (;;) {
int dir;
dReal dirf;
// find direction to push on x(i)
if (w[i] <= 0) {
dir = 1;
dirf = REAL(1.0);
}
else {
dir = -1;
dirf = REAL(-1.0);
}
// compute: delta_x(C) = -dir*A(C,C)\A(C,i)
lcp.solve1 (delta_x,i,dir);
// note that delta_x[i] = dirf, but we wont bother to set it
// compute: delta_w = A*delta_x ... note we only care about
// delta_w(N) and delta_w(i), the rest is ignored
lcp.pN_equals_ANC_times_qC (delta_w,delta_x);
lcp.pN_plusequals_ANi (delta_w,i,dir);
delta_w[i] = lcp.AiC_times_qC (i,delta_x) + lcp.Aii(i)*dirf;
// find largest step we can take (size=s), either to drive x(i),w(i)
// to the valid LCP region or to drive an already-valid variable
// outside the valid region.
int cmd = 1; // index switching command
int si = 0; // si = index to switch if cmd>3
dReal s = -w[i]/delta_w[i];
if (dir > 0) {
if (hi[i] < dInfinity) {
dReal s2 = (hi[i]-x[i])*dirf; // was (hi[i]-x[i])/dirf // step to x(i)=hi(i)
if (s2 < s) {
s = s2;
cmd = 3;
}
}
}
else {
if (lo[i] > -dInfinity) {
dReal s2 = (lo[i]-x[i])*dirf; // was (lo[i]-x[i])/dirf // step to x(i)=lo(i)
if (s2 < s) {
s = s2;
cmd = 2;
}
}
}
{
const int numN = lcp.numN();
for (int k=0; k < numN; ++k) {
const int indexN_k = lcp.indexN(k);
if (!state[indexN_k] ? delta_w[indexN_k] < 0 : delta_w[indexN_k] > 0) {
// don't bother checking if lo=hi=0
if (lo[indexN_k] == 0 && hi[indexN_k] == 0) continue;
dReal s2 = -w[indexN_k] / delta_w[indexN_k];
if (s2 < s) {
s = s2;
cmd = 4;
si = indexN_k;
}
}
}
}
{
const int numC = lcp.numC();
for (int k=adj_nub; k < numC; ++k) {
const int indexC_k = lcp.indexC(k);
if (delta_x[indexC_k] < 0 && lo[indexC_k] > -dInfinity) {
dReal s2 = (lo[indexC_k]-x[indexC_k]) / delta_x[indexC_k];
if (s2 < s) {
s = s2;
cmd = 5;
si = indexC_k;
}
}
if (delta_x[indexC_k] > 0 && hi[indexC_k] < dInfinity) {
dReal s2 = (hi[indexC_k]-x[indexC_k]) / delta_x[indexC_k];
if (s2 < s) {
s = s2;
cmd = 6;
si = indexC_k;
}
}
}
}
//static char* cmdstring[8] = {0,"->C","->NL","->NH","N->C",
// "C->NL","C->NH"};
//printf ("cmd=%d (%s), si=%d\n",cmd,cmdstring[cmd],(cmd>3) ? si : i);
// if s <= 0 then we've got a problem. if we just keep going then
// we're going to get stuck in an infinite loop. instead, just cross
// our fingers and exit with the current solution.
if (s <= REAL(0.0)) {
if (earlyTermination) {
if (!outer_w)
delete[] w;
delete[] L;
delete[] d;
delete[] delta_w;
delete[] delta_x;
delete[] Dell;
delete[] ell;
#ifdef ROWPTRS
delete[] Arows;
#endif
delete[] p;
delete[] C;
delete[] state;
return false;
}
// We shouldn't be overly aggressive about printing this warning,
// because sometimes it gets spammed if s is just a tiny bit beneath
// 0.0.
if (s < REAL(-1e-6)) {
dMessage (d_ERR_LCP, "LCP internal error, s <= 0 (s=%.4e)",
(double)s);
}
if (i < n) {
dSetZero (x+i,n-i);
dSetZero (w+i,n-i);
}
s_error = true;
break;
}
// apply x = x + s * delta_x
lcp.pC_plusequals_s_times_qC (x, s, delta_x);
x[i] += s * dirf;
// apply w = w + s * delta_w
lcp.pN_plusequals_s_times_qN (w, s, delta_w);
w[i] += s * delta_w[i];
// void *tmpbuf;
// switch indexes between sets if necessary
switch (cmd) {
case 1: // done
w[i] = 0;
lcp.transfer_i_to_C (i);
break;
case 2: // done
x[i] = lo[i];
state[i] = false;
lcp.transfer_i_to_N (i);
break;
case 3: // done
x[i] = hi[i];
state[i] = true;
lcp.transfer_i_to_N (i);
break;
case 4: // keep going
w[si] = 0;
lcp.transfer_i_from_N_to_C (si);
break;
case 5: // keep going
x[si] = lo[si];
state[si] = false;
lcp.transfer_i_from_C_to_N (si, nullptr);
break;
case 6: // keep going
x[si] = hi[si];
state[si] = true;
lcp.transfer_i_from_C_to_N (si, nullptr);
break;
}
if (cmd <= 3) break;
} // for (;;)
} // else
if (s_error) {
break;
}
} // for (int i=adj_nub; i<n; ++i)
lcp.unpermute();
if (!outer_w)
delete[] w;
delete[] L;
delete[] d;
delete[] delta_w;
delete[] delta_x;
delete[] Dell;
delete[] ell;
#ifdef ROWPTRS
delete[] Arows;
#endif
delete[] p;
delete[] C;
delete[] state;
return true;
}
void dSolveLCP (int n, dReal *A, dReal *x, dReal *b,
dReal *w, int nub, dReal *lo, dReal *hi, int *findex)
{
dAASSERT (n>0 && A && x && b && w && lo && hi && nub >= 0 && nub <= n);
int i,k,hit_first_friction_index = 0;
int nskip = dPAD(n);
// if all the variables are unbounded then we can just factor, solve,
// and return
if (nub >= n) {
dFactorLDLT (A,w,n,nskip); // use w for d
dSolveLDLT (A,w,b,n,nskip);
memcpy (x,b,n*sizeof(dReal));
dSetZero (w,n);
return;
}
# ifndef dNODEBUG
// check restrictions on lo and hi
for (k=0; k<n; k++) dIASSERT (lo[k] <= 0 && hi[k] >= 0);
# endif
ALLOCA (dReal,L,n*nskip*sizeof(dReal));
#ifdef dUSE_MALLOC_FOR_ALLOCA
if (L == NULL) {
dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
return;
}
#endif
ALLOCA (dReal,d,n*sizeof(dReal));
#ifdef dUSE_MALLOC_FOR_ALLOCA
if (d == NULL) {
UNALLOCA(L);
dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
return;
}
#endif
ALLOCA (dReal,delta_x,n*sizeof(dReal));
#ifdef dUSE_MALLOC_FOR_ALLOCA
if (delta_x == NULL) {
UNALLOCA(d);
UNALLOCA(L);
dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
return;
}
#endif
ALLOCA (dReal,delta_w,n*sizeof(dReal));
#ifdef dUSE_MALLOC_FOR_ALLOCA
if (delta_w == NULL) {
UNALLOCA(delta_x);
UNALLOCA(d);
UNALLOCA(L);
dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
return;
}
#endif
ALLOCA (dReal,Dell,n*sizeof(dReal));
#ifdef dUSE_MALLOC_FOR_ALLOCA
if (Dell == NULL) {
UNALLOCA(delta_w);
UNALLOCA(delta_x);
UNALLOCA(d);
UNALLOCA(L);
dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
return;
}
#endif
ALLOCA (dReal,ell,n*sizeof(dReal));
#ifdef dUSE_MALLOC_FOR_ALLOCA
if (ell == NULL) {
UNALLOCA(Dell);
UNALLOCA(delta_w);
UNALLOCA(delta_x);
UNALLOCA(d);
UNALLOCA(L);
dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
return;
}
#endif
ALLOCA (dReal*,Arows,n*sizeof(dReal*));
#ifdef dUSE_MALLOC_FOR_ALLOCA
if (Arows == NULL) {
UNALLOCA(ell);
UNALLOCA(Dell);
UNALLOCA(delta_w);
UNALLOCA(delta_x);
UNALLOCA(d);
UNALLOCA(L);
dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
return;
}
#endif
ALLOCA (int,p,n*sizeof(int));
#ifdef dUSE_MALLOC_FOR_ALLOCA
if (p == NULL) {
UNALLOCA(Arows);
UNALLOCA(ell);
UNALLOCA(Dell);
UNALLOCA(delta_w);
UNALLOCA(delta_x);
UNALLOCA(d);
UNALLOCA(L);
dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
return;
}
#endif
ALLOCA (int,C,n*sizeof(int));
#ifdef dUSE_MALLOC_FOR_ALLOCA
if (C == NULL) {
UNALLOCA(p);
UNALLOCA(Arows);
UNALLOCA(ell);
UNALLOCA(Dell);
UNALLOCA(delta_w);
UNALLOCA(delta_x);
UNALLOCA(d);
UNALLOCA(L);
dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
return;
}
#endif
int dir;
dReal dirf;
// for i in N, state[i] is 0 if x(i)==lo(i) or 1 if x(i)==hi(i)
ALLOCA (int,state,n*sizeof(int));
#ifdef dUSE_MALLOC_FOR_ALLOCA
if (state == NULL) {
UNALLOCA(C);
UNALLOCA(p);
UNALLOCA(Arows);
UNALLOCA(ell);
UNALLOCA(Dell);
UNALLOCA(delta_w);
UNALLOCA(delta_x);
UNALLOCA(d);
UNALLOCA(L);
dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
return;
}
#endif
// create LCP object. note that tmp is set to delta_w to save space, this
// optimization relies on knowledge of how tmp is used, so be careful!
dLCP *lcp=new dLCP(n,nub,A,x,b,w,lo,hi,L,d,Dell,ell,delta_w,state,findex,p,C,Arows);
nub = lcp->getNub();
// loop over all indexes nub..n-1. for index i, if x(i),w(i) satisfy the
// LCP conditions then i is added to the appropriate index set. otherwise
// x(i),w(i) is driven either +ve or -ve to force it to the valid region.
// as we drive x(i), x(C) is also adjusted to keep w(C) at zero.
// while driving x(i) we maintain the LCP conditions on the other variables
// 0..i-1. we do this by watching out for other x(i),w(i) values going
// outside the valid region, and then switching them between index sets
// when that happens.
for (i=nub; i<n; i++) {
// the index i is the driving index and indexes i+1..n-1 are "dont care",
// i.e. when we make changes to the system those x's will be zero and we
// don't care what happens to those w's. in other words, we only consider
// an (i+1)*(i+1) sub-problem of A*x=b+w.
// if we've hit the first friction index, we have to compute the lo and
// hi values based on the values of x already computed. we have been
// permuting the indexes, so the values stored in the findex vector are
// no longer valid. thus we have to temporarily unpermute the x vector.
// for the purposes of this computation, 0*infinity = 0 ... so if the
// contact constraint's normal force is 0, there should be no tangential
// force applied.
if (hit_first_friction_index == 0 && findex && findex[i] >= 0) {
// un-permute x into delta_w, which is not being used at the moment
for (k=0; k<n; k++) delta_w[p[k]] = x[k];
// set lo and hi values
for (k=i; k<n; k++) {
dReal wfk = delta_w[findex[k]];
if (wfk == 0) {
hi[k] = 0;
lo[k] = 0;
}
else {
hi[k] = dFabs (hi[k] * wfk);
lo[k] = -hi[k];
}
}
hit_first_friction_index = 1;
}
// thus far we have not even been computing the w values for indexes
// greater than i, so compute w[i] now.
w[i] = lcp->AiC_times_qC (i,x) + lcp->AiN_times_qN (i,x) - b[i];
// if lo=hi=0 (which can happen for tangential friction when normals are
// 0) then the index will be assigned to set N with some state. however,
// set C's line has zero size, so the index will always remain in set N.
// with the "normal" switching logic, if w changed sign then the index
// would have to switch to set C and then back to set N with an inverted
// state. this is pointless, and also computationally expensive. to
// prevent this from happening, we use the rule that indexes with lo=hi=0
// will never be checked for set changes. this means that the state for
// these indexes may be incorrect, but that doesn't matter.
// see if x(i),w(i) is in a valid region
if (lo[i]==0 && w[i] >= 0) {
lcp->transfer_i_to_N (i);
state[i] = 0;
}
else if (hi[i]==0 && w[i] <= 0) {
lcp->transfer_i_to_N (i);
state[i] = 1;
}
else if (w[i]==0) {
// this is a degenerate case. by the time we get to this test we know
// that lo != 0, which means that lo < 0 as lo is not allowed to be +ve,
// and similarly that hi > 0. this means that the line segment
// corresponding to set C is at least finite in extent, and we are on it.
// NOTE: we must call lcp->solve1() before lcp->transfer_i_to_C()
lcp->solve1 (delta_x,i,0,1);
#ifdef dUSE_MALLOC_FOR_ALLOCA
if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY) {
UNALLOCA(state);
UNALLOCA(C);
UNALLOCA(p);
UNALLOCA(Arows);
UNALLOCA(ell);
UNALLOCA(Dell);
UNALLOCA(delta_w);
UNALLOCA(delta_x);
UNALLOCA(d);
UNALLOCA(L);
return;
}
#endif
lcp->transfer_i_to_C (i);
}
else {
// we must push x(i) and w(i)
for (;;) {
// find direction to push on x(i)
if (w[i] <= 0) {
dir = 1;
dirf = REAL(1.0);
}
else {
dir = -1;
dirf = REAL(-1.0);
}
// compute: delta_x(C) = -dir*A(C,C)\A(C,i)
lcp->solve1 (delta_x,i,dir);
#ifdef dUSE_MALLOC_FOR_ALLOCA
if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY) {
UNALLOCA(state);
UNALLOCA(C);
UNALLOCA(p);
UNALLOCA(Arows);
UNALLOCA(ell);
UNALLOCA(Dell);
UNALLOCA(delta_w);
UNALLOCA(delta_x);
UNALLOCA(d);
UNALLOCA(L);
return;
}
#endif
// note that delta_x[i] = dirf, but we wont bother to set it
// compute: delta_w = A*delta_x ... note we only care about
// delta_w(N) and delta_w(i), the rest is ignored
lcp->pN_equals_ANC_times_qC (delta_w,delta_x);
lcp->pN_plusequals_ANi (delta_w,i,dir);
delta_w[i] = lcp->AiC_times_qC (i,delta_x) + lcp->Aii(i)*dirf;
// find largest step we can take (size=s), either to drive x(i),w(i)
// to the valid LCP region or to drive an already-valid variable
// outside the valid region.
int cmd = 1; // index switching command
int si = 0; // si = index to switch if cmd>3
dReal s = -w[i]/delta_w[i];
if (dir > 0) {
if (hi[i] < dInfinity) {
dReal s2 = (hi[i]-x[i])/dirf; // step to x(i)=hi(i)
if (s2 < s) {
s = s2;
cmd = 3;
}
}
}
else {
if (lo[i] > -dInfinity) {
dReal s2 = (lo[i]-x[i])/dirf; // step to x(i)=lo(i)
if (s2 < s) {
s = s2;
cmd = 2;
}
}
}
for (k=0; k < lcp->numN(); k++) {
if ((state[lcp->indexN(k)]==0 && delta_w[lcp->indexN(k)] < 0) ||
(state[lcp->indexN(k)]!=0 && delta_w[lcp->indexN(k)] > 0)) {
// don't bother checking if lo=hi=0
if (lo[lcp->indexN(k)] == 0 && hi[lcp->indexN(k)] == 0) continue;
dReal s2 = -w[lcp->indexN(k)] / delta_w[lcp->indexN(k)];
if (s2 < s) {
s = s2;
cmd = 4;
si = lcp->indexN(k);
}
}
}
for (k=nub; k < lcp->numC(); k++) {
if (delta_x[lcp->indexC(k)] < 0 && lo[lcp->indexC(k)] > -dInfinity) {
dReal s2 = (lo[lcp->indexC(k)]-x[lcp->indexC(k)]) /
delta_x[lcp->indexC(k)];
if (s2 < s) {
s = s2;
cmd = 5;
si = lcp->indexC(k);
}
}
if (delta_x[lcp->indexC(k)] > 0 && hi[lcp->indexC(k)] < dInfinity) {
dReal s2 = (hi[lcp->indexC(k)]-x[lcp->indexC(k)]) /
delta_x[lcp->indexC(k)];
if (s2 < s) {
s = s2;
cmd = 6;
si = lcp->indexC(k);
}
}
}
//static char* cmdstring[8] = {0,"->C","->NL","->NH","N->C",
// "C->NL","C->NH"};
//printf ("cmd=%d (%s), si=%d\n",cmd,cmdstring[cmd],(cmd>3) ? si : i);
// if s <= 0 then we've got a problem. if we just keep going then
// we're going to get stuck in an infinite loop. instead, just cross
// our fingers and exit with the current solution.
if (s <= 0) {
dMessage (d_ERR_LCP, "LCP internal error, s <= 0 (s=%.4e)",s);
if (i < (n-1)) {
dSetZero (x+i,n-i);
dSetZero (w+i,n-i);
}
goto done;
}
// apply x = x + s * delta_x
lcp->pC_plusequals_s_times_qC (x,s,delta_x);
x[i] += s * dirf;
// apply w = w + s * delta_w
lcp->pN_plusequals_s_times_qN (w,s,delta_w);
w[i] += s * delta_w[i];
// switch indexes between sets if necessary
switch (cmd) {
case 1: // done
w[i] = 0;
lcp->transfer_i_to_C (i);
break;
case 2: // done
x[i] = lo[i];
state[i] = 0;
lcp->transfer_i_to_N (i);
break;
case 3: // done
x[i] = hi[i];
state[i] = 1;
lcp->transfer_i_to_N (i);
break;
case 4: // keep going
w[si] = 0;
lcp->transfer_i_from_N_to_C (si);
break;
case 5: // keep going
x[si] = lo[si];
state[si] = 0;
lcp->transfer_i_from_C_to_N (si);
break;
case 6: // keep going
x[si] = hi[si];
state[si] = 1;
lcp->transfer_i_from_C_to_N (si);
break;
}
if (cmd <= 3) break;
}
}
}
done:
lcp->unpermute();
delete lcp;
UNALLOCA (L);
UNALLOCA (d);
UNALLOCA (delta_x);
UNALLOCA (delta_w);
UNALLOCA (Dell);
UNALLOCA (ell);
UNALLOCA (Arows);
UNALLOCA (p);
UNALLOCA (C);
UNALLOCA (state);
}
void dSolveLCPBasic (int n, dReal *A, dReal *x, dReal *b,
dReal *w, int nub, dReal *lo, dReal *hi)
{
dAASSERT (n>0 && A && x && b && w && nub == 0);
int i,k;
int nskip = dPAD(n);
ALLOCA (dReal,L,n*nskip*sizeof(dReal));
#ifdef dUSE_MALLOC_FOR_ALLOCA
if (L == NULL) {
dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
return;
}
#endif
ALLOCA (dReal,d,n*sizeof(dReal));
#ifdef dUSE_MALLOC_FOR_ALLOCA
if (d == NULL) {
UNALLOCA(L);
dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
return;
}
#endif
ALLOCA (dReal,delta_x,n*sizeof(dReal));
#ifdef dUSE_MALLOC_FOR_ALLOCA
if (delta_x == NULL) {
UNALLOCA(d);
UNALLOCA(L);
dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
return;
}
#endif
ALLOCA (dReal,delta_w,n*sizeof(dReal));
#ifdef dUSE_MALLOC_FOR_ALLOCA
if (delta_w == NULL) {
UNALLOCA(delta_x);
UNALLOCA(d);
UNALLOCA(L);
dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
return;
}
#endif
ALLOCA (dReal,Dell,n*sizeof(dReal));
#ifdef dUSE_MALLOC_FOR_ALLOCA
if (Dell == NULL) {
UNALLOCA(delta_w);
UNALLOCA(delta_x);
UNALLOCA(d);
UNALLOCA(L);
dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
return;
}
#endif
ALLOCA (dReal,ell,n*sizeof(dReal));
#ifdef dUSE_MALLOC_FOR_ALLOCA
if (ell == NULL) {
UNALLOCA(Dell);
UNALLOCA(delta_w);
UNALLOCA(delta_x);
UNALLOCA(d);
UNALLOCA(L);
dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
return;
}
#endif
ALLOCA (dReal,tmp,n*sizeof(dReal));
#ifdef dUSE_MALLOC_FOR_ALLOCA
if (tmp == NULL) {
UNALLOCA(ell);
UNALLOCA(Dell);
UNALLOCA(delta_w);
UNALLOCA(delta_x);
UNALLOCA(d);
UNALLOCA(L);
dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
return;
}
#endif
ALLOCA (dReal*,Arows,n*sizeof(dReal*));
#ifdef dUSE_MALLOC_FOR_ALLOCA
if (Arows == NULL) {
UNALLOCA(tmp);
UNALLOCA(ell);
UNALLOCA(Dell);
UNALLOCA(delta_w);
UNALLOCA(delta_x);
UNALLOCA(d);
UNALLOCA(L);
dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
return;
}
#endif
ALLOCA (int,p,n*sizeof(int));
#ifdef dUSE_MALLOC_FOR_ALLOCA
if (p == NULL) {
UNALLOCA(Arows);
UNALLOCA(tmp);
UNALLOCA(ell);
UNALLOCA(Dell);
UNALLOCA(delta_w);
UNALLOCA(delta_x);
UNALLOCA(d);
UNALLOCA(L);
dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
return;
}
#endif
ALLOCA (int,C,n*sizeof(int));
#ifdef dUSE_MALLOC_FOR_ALLOCA
if (C == NULL) {
UNALLOCA(p);
UNALLOCA(Arows);
UNALLOCA(tmp);
UNALLOCA(ell);
UNALLOCA(Dell);
UNALLOCA(delta_w);
UNALLOCA(delta_x);
UNALLOCA(d);
UNALLOCA(L);
dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
return;
}
#endif
ALLOCA (int,dummy,n*sizeof(int));
#ifdef dUSE_MALLOC_FOR_ALLOCA
if (dummy == NULL) {
UNALLOCA(C);
UNALLOCA(p);
UNALLOCA(Arows);
UNALLOCA(tmp);
UNALLOCA(ell);
UNALLOCA(Dell);
UNALLOCA(delta_w);
UNALLOCA(delta_x);
UNALLOCA(d);
UNALLOCA(L);
dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
return;
}
#endif
dLCP lcp (n,0,A,x,b,w,tmp,tmp,L,d,Dell,ell,tmp,dummy,dummy,p,C,Arows);
nub = lcp.getNub();
for (i=0; i<n; i++) {
w[i] = lcp.AiC_times_qC (i,x) - b[i];
if (w[i] >= 0) {
lcp.transfer_i_to_N (i);
}
else {
for (;;) {
// compute: delta_x(C) = -A(C,C)\A(C,i)
dSetZero (delta_x,n);
lcp.solve1 (delta_x,i);
#ifdef dUSE_MALLOC_FOR_ALLOCA
if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY) {
UNALLOCA(dummy);
UNALLOCA(C);
UNALLOCA(p);
UNALLOCA(Arows);
UNALLOCA(tmp);
UNALLOCA(ell);
UNALLOCA(Dell);
UNALLOCA(delta_w);
UNALLOCA(delta_x);
UNALLOCA(d);
UNALLOCA(L);
return;
}
#endif
delta_x[i] = 1;
// compute: delta_w = A*delta_x
dSetZero (delta_w,n);
lcp.pN_equals_ANC_times_qC (delta_w,delta_x);
lcp.pN_plusequals_ANi (delta_w,i);
delta_w[i] = lcp.AiC_times_qC (i,delta_x) + lcp.Aii(i);
// find index to switch
int si = i; // si = switch index
int si_in_N = 0; // set to 1 if si in N
dReal s = -w[i]/delta_w[i];
if (s <= 0) {
dMessage (d_ERR_LCP, "LCP internal error, s <= 0 (s=%.4e)",s);
if (i < (n-1)) {
dSetZero (x+i,n-i);
dSetZero (w+i,n-i);
}
goto done;
}
for (k=0; k < lcp.numN(); k++) {
if (delta_w[lcp.indexN(k)] < 0) {
dReal s2 = -w[lcp.indexN(k)] / delta_w[lcp.indexN(k)];
if (s2 < s) {
s = s2;
si = lcp.indexN(k);
si_in_N = 1;
}
}
}
for (k=0; k < lcp.numC(); k++) {
if (delta_x[lcp.indexC(k)] < 0) {
dReal s2 = -x[lcp.indexC(k)] / delta_x[lcp.indexC(k)];
if (s2 < s) {
s = s2;
si = lcp.indexC(k);
si_in_N = 0;
}
}
}
// apply x = x + s * delta_x
lcp.pC_plusequals_s_times_qC (x,s,delta_x);
x[i] += s;
lcp.pN_plusequals_s_times_qN (w,s,delta_w);
w[i] += s * delta_w[i];
// switch indexes between sets if necessary
if (si==i) {
w[i] = 0;
lcp.transfer_i_to_C (i);
break;
}
if (si_in_N) {
w[si] = 0;
lcp.transfer_i_from_N_to_C (si);
}
else {
x[si] = 0;
lcp.transfer_i_from_C_to_N (si);
}
}
}
}
done:
lcp.unpermute();
UNALLOCA (L);
UNALLOCA (d);
UNALLOCA (delta_x);
UNALLOCA (delta_w);
UNALLOCA (Dell);
UNALLOCA (ell);
UNALLOCA (tmp);
UNALLOCA (Arows);
UNALLOCA (p);
UNALLOCA (C);
UNALLOCA (dummy);
}
void dSolveLCPBasic (int n, dReal *A, dReal *x, dReal *b,
dReal *w, int nub, dReal *lo, dReal *hi)
{
dAASSERT (n>0 && A && x && b && w && nub == 0);
int i,k;
int nskip = dPAD(n);
dReal *L = (dReal*) ALLOCA (n*nskip*sizeof(dReal));
dReal *d = (dReal*) ALLOCA (n*sizeof(dReal));
dReal *delta_x = (dReal*) ALLOCA (n*sizeof(dReal));
dReal *delta_w = (dReal*) ALLOCA (n*sizeof(dReal));
dReal *Dell = (dReal*) ALLOCA (n*sizeof(dReal));
dReal *ell = (dReal*) ALLOCA (n*sizeof(dReal));
dReal *tmp = (dReal*) ALLOCA (n*sizeof(dReal));
dReal **Arows = (dReal**) ALLOCA (n*sizeof(dReal*));
int *p = (int*) ALLOCA (n*sizeof(int));
int *C = (int*) ALLOCA (n*sizeof(int));
int *dummy = (int*) ALLOCA (n*sizeof(int));
dLCP lcp (n,0,A,x,b,w,tmp,tmp,L,d,Dell,ell,tmp,dummy,dummy,p,C,Arows);
nub = lcp.getNub();
for (i=0; i<n; i++) {
w[i] = lcp.AiC_times_qC (i,x) - b[i];
if (w[i] >= 0) {
lcp.transfer_i_to_N (i);
}
else {
for (;;) {
// compute: delta_x(C) = -A(C,C)\A(C,i)
dSetZero (delta_x,n);
lcp.solve1 (delta_x,i);
delta_x[i] = 1;
// compute: delta_w = A*delta_x
dSetZero (delta_w,n);
lcp.pN_equals_ANC_times_qC (delta_w,delta_x);
lcp.pN_plusequals_ANi (delta_w,i);
delta_w[i] = lcp.AiC_times_qC (i,delta_x) + lcp.Aii(i);
// find index to switch
int si = i; // si = switch index
int si_in_N = 0; // set to 1 if si in N
dReal s = -w[i]/delta_w[i];
if (s <= 0) {
dMessage (d_ERR_LCP, "LCP internal error, s <= 0 (s=%.4e)",s);
if (i < (n-1)) {
dSetZero (x+i,n-i);
dSetZero (w+i,n-i);
}
goto done;
}
for (k=0; k < lcp.numN(); k++) {
if (delta_w[lcp.indexN(k)] < 0) {
dReal s2 = -w[lcp.indexN(k)] / delta_w[lcp.indexN(k)];
if (s2 < s) {
s = s2;
si = lcp.indexN(k);
si_in_N = 1;
}
}
}
for (k=0; k < lcp.numC(); k++) {
if (delta_x[lcp.indexC(k)] < 0) {
dReal s2 = -x[lcp.indexC(k)] / delta_x[lcp.indexC(k)];
if (s2 < s) {
s = s2;
si = lcp.indexC(k);
si_in_N = 0;
}
}
}
// apply x = x + s * delta_x
lcp.pC_plusequals_s_times_qC (x,s,delta_x);
x[i] += s;
lcp.pN_plusequals_s_times_qN (w,s,delta_w);
w[i] += s * delta_w[i];
// switch indexes between sets if necessary
if (si==i) {
w[i] = 0;
lcp.transfer_i_to_C (i);
break;
}
if (si_in_N) {
w[si] = 0;
lcp.transfer_i_from_N_to_C (si);
}
else {
x[si] = 0;
lcp.transfer_i_from_C_to_N (si);
}
}
}
}
done:
lcp.unpermute();
}
