dxQuadTreeSpace::dxQuadTreeSpace(dSpaceID _space, dVector3 Center, dVector3 Extents, int Depth) : dxSpace(_space){
type = dQuadTreeSpaceClass;
int BlockCount = 0;
for (int i = 0; i <= Depth; i++){
BlockCount += (int)pow((dReal)SPLITS, i);
}
Blocks = (Block*)dAlloc(BlockCount * sizeof(Block));
Block* Blocks = this->Blocks + 1; // This pointer gets modified!
this->Blocks[0].Create(Center, Extents, 0, Depth, Blocks);
CurrentBlock = 0;
CurrentChild = (int*)dAlloc((Depth + 1) * sizeof(int));
CurrentLevel = 0;
CurrentObject = 0;
CurrentIndex = -1;
// Init AABB. We initialize to infinity because it is not illegal for an object to be outside of the tree. Its simply inserted in the root block
aabb[0] = -dInfinity;
aabb[1] = dInfinity;
aabb[2] = -dInfinity;
aabb[3] = dInfinity;
aabb[4] = -dInfinity;
aabb[5] = dInfinity;
}
void *dObStack::alloc (int num_bytes)
{
if ((size_t)num_bytes > MAX_ALLOC_SIZE) dDebug (0,"num_bytes too large");
// allocate or move to a new arena if necessary
if (!first) {
// allocate the first arena if necessary
first = last = (Arena *) dAlloc (dOBSTACK_ARENA_SIZE);
first->next = 0;
first->used = sizeof (Arena);
ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (first,first->used);
}
else {
// we already have one or more arenas, see if a new arena must be used
if ((last->used + num_bytes) > dOBSTACK_ARENA_SIZE) {
if (!last->next) {
last->next = (Arena *) dAlloc (dOBSTACK_ARENA_SIZE);
last->next->next = 0;
}
last = last->next;
last->used = sizeof (Arena);
ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (last,last->used);
}
}
// allocate an area in the arena
char *c = ((char*) last) + last->used;
last->used += num_bytes;
ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (last,last->used);
return c;
}
dxQuadTreeSpace::dxQuadTreeSpace(dSpaceID _space, const dVector3 Center, const dVector3 Extents, int Depth) : dxSpace(_space){
type = dQuadTreeSpaceClass;
int BlockCount = 0;
// TODO: should be just BlockCount = (4^(n+1) - 1)/3
for (int i = 0; i <= Depth; i++){
BlockCount += (int)pow((dReal)SPLITS, i);
}
Blocks = (Block*)dAlloc(BlockCount * sizeof(Block));
Block* Blocks = this->Blocks + 1; // This pointer gets modified!
dReal MinX = Center[AXIS0] - Extents[AXIS0];
dReal MaxX = dNextAfter((Center[AXIS0] + Extents[AXIS0]), (dReal)dInfinity);
dReal MinZ = Center[AXIS1] - Extents[AXIS1];
dReal MaxZ = dNextAfter((Center[AXIS1] + Extents[AXIS1]), (dReal)dInfinity);
this->Blocks[0].Create(MinX, MaxX, MinZ, MaxZ, 0, Depth, Blocks);
CurrentBlock = 0;
CurrentChild = (int*)dAlloc((Depth + 1) * sizeof(int));
CurrentLevel = 0;
CurrentObject = 0;
CurrentIndex = -1;
// Init AABB. We initialize to infinity because it is not illegal for an object to be outside of the tree. Its simply inserted in the root block
aabb[0] = -dInfinity;
aabb[1] = dInfinity;
aabb[2] = -dInfinity;
aabb[3] = dInfinity;
aabb[4] = -dInfinity;
aabb[5] = dInfinity;
}
dReal dMatrixComparison::nextMatrix (dReal *A, int n, int m, int lower_tri,
char *name, ...)
{
if (A==0 || n < 1 || m < 1 || name==0) dDebug (0,"bad args to nextMatrix");
int num = n*dPAD(m);
if (afterfirst==0) {
dMatInfo *mi = (dMatInfo*) dAlloc (sizeof(dMatInfo));
mi->n = n;
mi->m = m;
mi->size = num * sizeof(dReal);
mi->data = (dReal*) dAlloc (mi->size);
memcpy (mi->data,A,mi->size);
va_list ap;
va_start (ap,name);
vsprintf (mi->name,name,ap);
if (strlen(mi->name) >= sizeof (mi->name)) dDebug (0,"name too long");
mat.push (mi);
return 0;
}
else {
if (lower_tri && n != m)
dDebug (0,"dMatrixComparison, lower triangular matrix must be square");
if (index >= mat.size()) dDebug (0,"dMatrixComparison, too many matrices");
dMatInfo *mp = mat[index];
index++;
dMatInfo mi;
va_list ap;
va_start (ap,name);
vsprintf (mi.name,name,ap);
if (strlen(mi.name) >= sizeof (mi.name)) dDebug (0,"name too long");
if (strcmp(mp->name,mi.name) != 0)
dDebug (0,"dMatrixComparison, name mismatch (\"%s\" and \"%s\")",
mp->name,mi.name);
if (mp->n != n || mp->m != m)
dDebug (0,"dMatrixComparison, size mismatch (%dx%d and %dx%d)",
mp->n,mp->m,n,m);
dReal maxdiff;
if (lower_tri) {
maxdiff = dMaxDifferenceLowerTriangle (A,mp->data,n);
}
else {
maxdiff = dMaxDifference (A,mp->data,n,m);
}
if (maxdiff > tol)
dDebug (0,"dMatrixComparison, matrix error (size=%dx%d, name=\"%s\", "
"error=%.4e)",n,m,mi.name,maxdiff);
return maxdiff;
}
}
chfac *CfcAlloc(int maxrow,
char *info)
{
chfac *r=NULL;
if (maxrow) {
r=(chfac*)calloc(1,sizeof(chfac));
if (!r) ExitProc(OutOfSpc,info);
r->mrow =maxrow;
r->nrow =maxrow;
r->snnz =0;
r->shead=iAlloc(maxrow,info);
r->ssize=iAlloc(maxrow,info);
r->ssub =NULL;
r->diag =dAlloc(maxrow,info);
r->unnz =0;
r->ujnz =0;
r->ujbeg=iAlloc(maxrow,info);
r->uhead=iAlloc(maxrow,info);
r->ujsze=iAlloc(maxrow,info);
r->usub =NULL;
r->uval =NULL;
r->perm =iAlloc(maxrow,info);
r->invp =iAlloc(maxrow,info);
r->nsnds=0;
r->subg =iAlloc(maxrow+1,info);
}
return r;
} /* SchlAlloc */
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);
}
void dGeomSetBody (dxGeom *g, dxBody *b)
{
dAASSERT (g);
dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
CHECK_NOT_LOCKED (g->parent_space);
if (b) {
if (!g->body) dFree (g->pos,sizeof(dxPosR));
g->pos = b->pos;
g->R = b->R;
dGeomMoved (g);
if (g->body != b) {
g->bodyRemove();
g->bodyAdd (b);
}
}
else {
if (g->body) {
dxPosR *pr = (dxPosR*) dAlloc (sizeof(dxPosR));
g->pos = pr->pos;
g->R = pr->R;
memcpy (g->pos,g->body->pos,sizeof(dVector3));
memcpy (g->R,g->body->R,sizeof(dMatrix3));
g->bodyRemove();
}
// dGeomMoved() should not be called if the body is being set to 0, as the
// new position of the geom is set to the old position of the body, so the
// effective position of the geom remains unchanged.
}
}
dxUserGeom::dxUserGeom (int class_num) : dxGeom (0,1)
{
type = class_num;
int size = user_classes[type-dFirstUserClass].bytes;
user_data = dAlloc (size);
memset (user_data,0,size);
}
dMatrix::dMatrix (const dMatrix &a)
{
n = a.n;
m = a.m;
data = (dReal*) dAlloc (n*m*sizeof(dReal));
memcpy (data,a.data,n*m*sizeof(dReal));
}
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);
}
int CfcAlloc(int maxrow,
char *info,chfac**rr)
{
chfac *r=NULL;
int ierr=0;
if (maxrow) {
r=(chfac*)calloc(1,sizeof(chfac));
if (!r) ExitProc(OutOfSpc,info);
r->mrow =maxrow;
r->nrow =maxrow;
r->snnz =0;
ierr=iAlloc(maxrow,info,&r->shead); if(ierr) return 1;
ierr=iAlloc(maxrow,info,&r->ssize); if(ierr) return 1;
r->ssub =NULL;
ierr=dAlloc(maxrow,info,&r->diag); if(ierr) return 1;
ierr=dAlloc(maxrow,info,&r->sqrtdiag); if(ierr) return 1;
r->unnz =0;
r->ujnz =0;
ierr=iAlloc(maxrow,info,&r->ujbeg); if(ierr) return 1;
ierr=iAlloc(maxrow,info,&r->uhead); if(ierr) return 1;
ierr=iAlloc(maxrow,info,&r->ujsze); if(ierr) return 1;
r->usub =NULL;
r->uval =NULL;
ierr=iAlloc(maxrow,info,&r->perm); if(ierr) return 1;
ierr=iAlloc(maxrow,info,&r->invp); if(ierr) return 1;
r->nsnds=0;
ierr=iAlloc(maxrow+1,info,&r->subg); if(ierr) return 1;
r->n=maxrow;
r->alldense=0;
r->tolpiv=1.0e-13; /* Standard */
r->tolpiv=1.0e-35;
r->cachesize =256;
#ifdef DSDPCACHESIZE
if (DSDPCACHESIZE>0){
r->cachesize = (int)DSDPCACHESIZE;
}
#endif
r->cacheunit =1000;
}
*rr=r;
return 0;
} /* SchlAlloc */
dMatrix::dMatrix (int rows, int cols,
dReal *_data, int rowskip, int colskip)
{
if (rows < 1 || cols < 1) dDebug (0,"bad matrix size");
n = rows;
m = cols;
data = (dReal*) dAlloc (n*m*sizeof(dReal));
for (int i=0; i<n; i++) {
for (int j=0; j<m; j++) data[i*m+j] = _data[i*rowskip + j*colskip];
}
}
bool dxThreadingThreadPool::InitializeThreads(size_t thread_count, size_t stack_size, unsigned int ode_data_allocate_flags)
{
dIASSERT(m_thread_infos == NULL);
bool result = false;
bool wait_event_allocated = false;
dxThreadPoolThreadInfo *thread_infos = NULL;
bool thread_infos_allocated = false;
do
{
if (!m_ready_wait_event.InitializeObject(false, false))
{
break;
}
wait_event_allocated = true;
thread_infos = (dxThreadPoolThreadInfo *)dAlloc(thread_count * sizeof(dxThreadPoolThreadInfo));
if (thread_infos == NULL)
{
break;
}
thread_infos_allocated = true;
if (!InitializeIndividualThreadInfos(thread_infos, thread_count, stack_size, ode_data_allocate_flags))
{
break;
}
m_thread_infos = thread_infos;
m_thread_count = thread_count;
result = true;
}
while (false);
if (!result)
{
if (wait_event_allocated)
{
if (thread_infos_allocated)
{
dFree(thread_infos, thread_count * sizeof(dxThreadPoolThreadInfo));
}
m_ready_wait_event.FinalizeObject();
}
}
return result;
}
void dMatrix::operator= (const dMatrix &a)
{
if (data) dFree (data,n*m*sizeof(dReal));
n = a.n;
m = a.m;
if (n > 0 && m > 0) {
data = (dReal*) dAlloc (n*m*sizeof(dReal));
memcpy (data,a.data,n*m*sizeof(dReal));
}
else data = 0;
}
static inline dxPosR* dAllocPosr()
{
dxPosR *retPosR;
#if dATOMICS_ENABLED
retPosR = (dxPosR *)AtomicExchangePointer(&s_cachedPosR, NULL);
if (!retPosR)
#endif
{
retPosR = (dxPosR*) dAlloc (sizeof(dxPosR));
}
return retPosR;
}
void dArrayBase::_setSize (int newsize, int sizeofT)
{
if (newsize < 0) return;
if (newsize > _anum) {
if (_data == this+1) {
// this is a no-no, because constructLocalArray() was called
dDebug (0,"setSize() out of space in LOCAL array");
}
int newanum = roundUpToPowerOfTwo (newsize);
if (_data) _data = dRealloc (_data, _anum*sizeofT, newanum*sizeofT);
else _data = dAlloc (newanum*sizeofT);
_anum = newanum;
}
_size = newsize;
}
symat *SymAlloc(int nrow,
int nnzo,
char *info)
{
symat *r;
r=(symat*)calloc(1,sizeof(symat));
if (!r) ExitProc(OutOfSpc,info);
if (nrow) {
r->diag=dAlloc(nrow,info);
r->roff=(array*)calloc(nrow,sizeof(array));
if (!r) ExitProc(OutOfSpc,info);
if (nnzo) {
r->roff->ja=iAlloc(nnzo,info);
r->roff->an=dAlloc(nnzo,info);
}
}
r->nrow=nrow;
r->nnzo=nnzo;
return r;
} /* SymAlloc */
int LvalAlloc(chfac *sf,
char *info)
{
int nnz;
nnz=iSum(sf->nrow,sf->ujsze);
if ( nnz<=sf->unnz )
return true;
sf->unnz=0;
if (sf->uval) dFree(&sf->uval);
sf->uval=dAlloc(nnz,info);
sf->unnz=nnz;
return true;
} /* LvalAlloc */
int LvalAlloc(chfac *sf,
char *info)
{
int ierr=0,nnz;
nnz=iSum(sf->nrow,sf->ujsze);
if ( nnz<=sf->unnz )
return 1;
sf->unnz=0;
if (sf->uval) dFree(&sf->uval);
ierr=dAlloc(nnz,info,&sf->uval);
sf->unnz=nnz;
if (ierr) return 1;
return 0;
} /* LvalAlloc */
void *dObStack::alloc (size_t num_bytes)
{
if (num_bytes > MAX_ALLOC_SIZE) dDebug (0,"num_bytes too large");
bool last_alloc_needed = false, last_init_needed = false;
Arena **last_ptr = NULL;
if (m_last != NULL) {
if ((m_last->m_used + num_bytes) > dOBSTACK_ARENA_SIZE) {
if (m_last->m_next != NULL) {
m_last = m_last->m_next;
last_init_needed = true;
} else {
last_ptr = &m_last->m_next;
last_alloc_needed = true;
}
}
} else {
last_ptr = &m_last;
last_alloc_needed = true;
}
if (last_alloc_needed) {
Arena *new_last = (Arena *) dAlloc (dOBSTACK_ARENA_SIZE);
new_last->m_next = 0;
*last_ptr = new_last;
if (m_first == NULL) {
m_first = new_last;
}
m_last = new_last;
last_init_needed = true;
}
if (last_init_needed) {
m_last->m_used = sizeof (Arena);
ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (m_last,m_last->m_used);
}
// allocate an area in the arena
char *c = ((char*) m_last) + m_last->m_used;
m_last->m_used += num_bytes;
ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (m_last,m_last->m_used);
return c;
}
static void copyChl(chfac *af,
chfac *bf)
{
iFree(&af->shead);
iFree(&af->ssize);
iFree(&af->ssub);
iFree(&bf->shead);
iFree(&bf->ssize);
iFree(&bf->ssub);
bf->unnz =af->unnz;
bf->ujnz =af->ujnz;
bf->nsnds=af->nsnds;
bf->ndens=af->ndens;
bf->nsndn=af->nsndn;
bf->sdens=af->sdens;
bf->upst =af->upst;
bf->usub =iAlloc(bf->ujnz,"usub, copyChl");
bf->uval =dAlloc(bf->unnz,"uval, copyChl");
iCopy(af->ujnz,af->usub,bf->usub);
iCopy(af->nrow,af->ujbeg,bf->ujbeg);
iCopy(af->nrow,af->uhead,bf->uhead);
iCopy(af->nrow,af->ujsze,bf->ujsze);
iCopy(af->nrow,af->perm,bf->perm);
iCopy(af->nrow,af->invp,bf->invp);
iCopy(af->nrow+1,af->subg,bf->subg);
bf->dhead=iAlloc(af->ndens+1,"dhead, copyChl");
iCopy(af->ndens+1,af->dhead,bf->dhead);
if (af->nsnds) {
bf->dbeg=iAlloc(af->nsnds,"dbeg, copyChl");
bf->dsub=iAlloc(af->nsnds,"dsub, copyChl");
iCopy(af->nsnds,af->dbeg,bf->dbeg);
iCopy(af->nsnds,af->dsub,bf->dsub);
}
} /* copyChl */
int dPtAlloc(int n,
char *info,double ***rr)
{
int ierr,i;
double **r;
r=NULL;
*rr=NULL;
if (!n) return 0;
r=(double **)calloc(n,sizeof(double*));
if (!r){
ExitProc(OutOfSpc,info);
return 1;
}
ierr=dAlloc(n*(n-1)/2,info,&r[0]);
if(ierr) return 1;
for (i=1; i<n; i++)
r[i]=r[i-1]+n-i;
*rr=r;
return 0;
} /* dPtAlloc */
smatx *SmtAlloc(int nrow,
int nnzo,
char *info)
{
smatx *r;
r=(smatx*)calloc(1,sizeof(smatx));
if (!r) ExitProc(OutOfSpc,info);
if (nrow) {
r->rows=(array*)calloc(nrow,sizeof(array));
if (!r) ExitProc(OutOfSpc,info);
if (nnzo) {
r->rows->ja=iAlloc(nnzo,info);
r->rows->an=dAlloc(nnzo,info);
}
}
r->maxnrow=nrow;
r->nrow=nrow;
r->non0=nnzo;
return r;
} /* SmatAlloc */
void * dArrayBase::operator new (size_t size)
{
return dAlloc (size);
}
void RankReduce(sdpdat *pd)
/* This routine generates a specific cut or solution to the original,
unrelaxed problem. */
{
int i,j,dim=pd->nrow, type = pd->ptyp;
double bestObj=0.0,thisObj=0.0,bestRObj=0.0,
bestHObj=0.0,*thisCut,*bestCut,*randVec,
*dblPtr,*sinve,*ei,hth=0.0,beta=0.0,*u,rtmp,ese;
orderCut *vhat;
thisCut = dAlloc(dim,"Insufficient memory to store cut");
bestCut = dAlloc(dim,"Insufficient memory to store cut");
randVec = dAlloc(dim,"Insufficient memory to store cut");
sinve = dAlloc(dim,"Insufficient memory to store cut");
vhat = OrdAlloc(dim,"Insufficient memory to store cut");
ei = dAlloc(dim,"Insufficient memory to store cut");
u = pd->rw+dim;
srand(clock()); /* Used to generate random vectors */
if (type == -3) /* For Box QP, use sqrt of the diagonal */
for (i=0;i<dim;i++)
thisCut[i]=bestCut[i]=sqrt(1-pd->rho*(pd->dy[i]-pd->y[i])/(pd->y[i]*pd->y[i]));
GetSinve(pd,sinve);
if (pd->par.findvec <= 0)
{
for (i=0; i<dim; i++)
{
memset(ei,0,sizeof(double)*dim);
ei[i]=1.0;
GetXrow(pd, sinve, randVec, ei);
for (j=0; j<dim; j++)
vhat[j].val=randVec[j];
DefineCut(thisCut,pd->ptyp,vhat,pd->nrow,pd->is,pd->it);
thisObj = EvalObjective(*(pd->cy),thisCut,dim,type);
if (i==0 || thisObj > bestHObj)
{
bestHObj=thisObj;
dblPtr=bestCut; bestCut=thisCut; thisCut=dblPtr;
}
}
}
/* We need to factor ( S + A(dy) ) when we want to
do ecut rank reduction */
if (pd->par.findvec >=0)
{
/* In case of ecut, we have to save L^-1 * e to sinve */
if(pd->ptyp == EquCut)
{
for(i=0;i<dim;i++) { randVec[i]=2.0; u[i]=0.0; }
FSubst( pd->mf,randVec,u , pd->mf->NegDiag);
for(i=0;i<dim;i++){ hth += u[i]*u[i]; }
beta = hth*( pd->dl - pd->lamda );
beta +=1.0;
if( beta < 0 )
{
printf("\n { 1 + h'h*(dl-l) } is less than zero. Error in rank reduction.");
ShutDown();
}
beta = sqrt(beta)-1.0;
beta /= hth;
}
bestRObj = -dim*dim;
for (i=0; i<dim; i++)
{
GetRandVec(randVec,dim);
if(pd->ptyp == EquCut)
{
if(pd->mf->NegDiag < 0) Uhat1(pd,u,randVec,ei,beta);
else Uhat2(pd,u,randVec,ei,beta,pd->mf->NegDiag,hth);
/* ChlSolve(cf,b,x); */
ChlSolve( pd->sf,randVec,ei);
rtmp = 0.0;
ese = 0.0;
for(j=0;j<dim;j++)
{
rtmp += ei[j];
ese += sinve[j];
}
ese = 1.0- pd->lamda*ese;
rtmp = pd->lamda*rtmp/ese;
for(j=0;j<dim;j++) randVec[j] = ei[j] + rtmp*sinve[j];
}
else
GetVhat(randVec,sinve,pd);
for (j=0; j<dim; j++) vhat[j].val=randVec[j];
DefineCut(thisCut,pd->ptyp,vhat,pd->nrow,pd->is,pd->it);
thisObj = EvalObjective(*(pd->cy),thisCut,dim,type);
if (i==dim || thisObj > bestRObj)
{
bestRObj=thisObj;
dblPtr=bestCut; bestCut=thisCut; thisCut=dblPtr;
}
}
}
else bestRObj = bestHObj;
bestObj = max(bestHObj, bestRObj);
if (pd->par.prtlev) SaveIt(bestCut,pd->ptyp,dim,pd->ss);
dFree(&thisCut);
dFree(&bestCut);
dFree(&randVec);
OrdFree(&vhat);
dFree(&sinve);
dFree(&ei);
if (pd->par.findvec == 0)
printf("\n The best solution found is : %8.6e \n",bestObj);
if (pd->par.findvec <= 0)
printf(" The best solution (heuristic) found is : %8.6e \n",bestHObj);
if (pd->par.findvec >= 0)
printf(" The best solution (randomized) found is : %8.6e \n",bestRObj);
printf("\n");
fprintf(fout," The best solution found is : %8.6e \n\n",bestObj);
}
static void *_OU_CONVENTION_CALLBACK ForwardOUMemoryAlloc(size_t nBlockSize)
{
return dAlloc(nBlockSize);
}
