static inline ULONG mmax(ULONG a,ULONG b)
{
return a>b ? a : b;
}
VOID ObtainShineShadowPens (struct ColorMap *cmap, ULONG rgb, LONG &shine, LONG &shadow)
{
ULONG r = (rgb & 0xff0000) >> 16;
ULONG g = (rgb & 0x00ff00) >> 8;
ULONG b = rgb & 0x0000ff;
ULONG dark_r = mmax(64, r)-64;
ULONG dark_g = mmax(64, g)-64;
ULONG dark_b = mmax(64, b)-64;
ULONG bright_r = mmin(191, r)+64;
ULONG bright_g = mmin(191, g)+64;
ULONG bright_b = mmin(191, b)+64;
ULONG intencity = (r+g+b)/3;
if(intencity >= 215)
{
bright_r = mmin(255, dark_r+32);
bright_g = mmin(255, dark_g+32);
bright_b = mmin(255, dark_b+32);
dark_r = mmax(0, (LONG)dark_r-64);
dark_g = mmax(0, (LONG)dark_g-64);
dark_b = mmax(0, (LONG)dark_b-64);
}
else
static void find_mismatch(const vpx_image_t *const img1,
const vpx_image_t *const img2,
int yloc[4], int uloc[4], int vloc[4]) {
const uint32_t bsize = 64;
const uint32_t bsizey = bsize >> img1->y_chroma_shift;
const uint32_t bsizex = bsize >> img1->x_chroma_shift;
const uint32_t c_w =
(img1->d_w + img1->x_chroma_shift) >> img1->x_chroma_shift;
const uint32_t c_h =
(img1->d_h + img1->y_chroma_shift) >> img1->y_chroma_shift;
int match = 1;
uint32_t i, j;
yloc[0] = yloc[1] = yloc[2] = yloc[3] = -1;
for (i = 0, match = 1; match && i < img1->d_h; i += bsize) {
for (j = 0; match && j < img1->d_w; j += bsize) {
int k, l;
const int si = mmin(i + bsize, img1->d_h) - i;
const int sj = mmin(j + bsize, img1->d_w) - j;
for (k = 0; match && k < si; ++k) {
for (l = 0; match && l < sj; ++l) {
if (*(img1->planes[VPX_PLANE_Y] +
(i + k) * img1->stride[VPX_PLANE_Y] + j + l) !=
*(img2->planes[VPX_PLANE_Y] +
(i + k) * img2->stride[VPX_PLANE_Y] + j + l)) {
yloc[0] = i + k;
yloc[1] = j + l;
yloc[2] = *(img1->planes[VPX_PLANE_Y] +
(i + k) * img1->stride[VPX_PLANE_Y] + j + l);
yloc[3] = *(img2->planes[VPX_PLANE_Y] +
(i + k) * img2->stride[VPX_PLANE_Y] + j + l);
match = 0;
break;
}
}
}
}
}
uloc[0] = uloc[1] = uloc[2] = uloc[3] = -1;
for (i = 0, match = 1; match && i < c_h; i += bsizey) {
for (j = 0; match && j < c_w; j += bsizex) {
int k, l;
const int si = mmin(i + bsizey, c_h - i);
const int sj = mmin(j + bsizex, c_w - j);
for (k = 0; match && k < si; ++k) {
for (l = 0; match && l < sj; ++l) {
if (*(img1->planes[VPX_PLANE_U] +
(i + k) * img1->stride[VPX_PLANE_U] + j + l) !=
*(img2->planes[VPX_PLANE_U] +
(i + k) * img2->stride[VPX_PLANE_U] + j + l)) {
uloc[0] = i + k;
uloc[1] = j + l;
uloc[2] = *(img1->planes[VPX_PLANE_U] +
(i + k) * img1->stride[VPX_PLANE_U] + j + l);
uloc[3] = *(img2->planes[VPX_PLANE_U] +
(i + k) * img2->stride[VPX_PLANE_U] + j + l);
match = 0;
break;
}
}
}
}
}
vloc[0] = vloc[1] = vloc[2] = vloc[3] = -1;
for (i = 0, match = 1; match && i < c_h; i += bsizey) {
for (j = 0; match && j < c_w; j += bsizex) {
int k, l;
const int si = mmin(i + bsizey, c_h - i);
const int sj = mmin(j + bsizex, c_w - j);
for (k = 0; match && k < si; ++k) {
for (l = 0; match && l < sj; ++l) {
if (*(img1->planes[VPX_PLANE_V] +
(i + k) * img1->stride[VPX_PLANE_V] + j + l) !=
*(img2->planes[VPX_PLANE_V] +
(i + k) * img2->stride[VPX_PLANE_V] + j + l)) {
vloc[0] = i + k;
vloc[1] = j + l;
vloc[2] = *(img1->planes[VPX_PLANE_V] +
(i + k) * img1->stride[VPX_PLANE_V] + j + l);
vloc[3] = *(img2->planes[VPX_PLANE_V] +
(i + k) * img2->stride[VPX_PLANE_V] + j + l);
match = 0;
break;
}
}
}
}
}
}
void CONDOR( double rhoStart, double rhoEnd, int niter,
ObjectiveFunction *of, int nnode)
{
rhoStart=mmax(rhoStart,rhoEnd);
int dim=of->dim(), info, k, t, nerror;
double rho=rhoStart, delta=rhoStart, rhoNew,
lambda1, normD=rhoEnd+1.0, modelStep, reduction, r, valueOF, valueFk, bound, noise;
Vector d, tmp;
bool improvement, forceTRStep=true, evalNeeded;
initConstrainedStep(of);
// pre-create the MultInd indexes to prevent multi-thread problems:
cacheMultInd.get(dim,1); cacheMultInd.get(dim,2);
parallelInit(nnode, dim, of);
of->initData();
// points=getFirstPoints(&ValuesF, &nPtsTotal, rhoStart,of);
InterPolynomial poly(2, rho, Vector::emptyVector, of->data, of);
if (poly.NewtonBasis==NULL)
{
printf("cannot construct lagrange basis.\n");
exit(255);
}
//Base=poly.vBase;
k=poly.kbest;
valueFk=poly.valuesF[k];
fprintf(stderr,"init part 1 finished.\n");
/* InterPolynomial poly(dim,2,of);
for (;;)
{
// find index k of the best (lowest) value of the function
k=findK(ValuesF, nPtsTotal, of, points);
Base=points[k].clone();
// Base=of->xStart.clone();
valueFk=ValuesF[k];
// translation:
t=nPtsTotal; while (t--) points[t]-=Base;
// exchange index 0 and index k (to be sure best point is inside poly):
tmp=points[k];
points[k]=points[0];
points[0]=tmp;
ValuesF[k]=ValuesF[0];
ValuesF[0]=valueFk;
k=0;
poly=InterPolynomial(2, nPtsTotal, points, ValuesF);
if (poly.NewtonBasis!=NULL) break;
// the construction of the first polynomial has failed
// delete[] points;
free(ValuesF);
int kbest=findBest(of->data, of); // return 0 if startpoint is given
Vector BaseStart=of->data.getLine(kbest,dim);
double vBaseStart=((double**)of->data)[kbest][dim];
points=GenerateData(rho, BaseStart, vBaseStart, of, &ValuesF);
nPtsTotal=n;
}
*/
// update M:
fullUpdateOfM(rho,poly.vBase,of->data,poly);
fprintf(stderr,"init part 2 finished.\n");
fprintf(stderr,"init finished.\n");
// first of init all variables:
parallelImprove(&poly, &k, rho, &valueFk, poly.vBase);
// really start in parallel:
startParallelThread();
while (true)
{
// fprintf(stderr,"rho=%e; fo=%e; NF=%i\n", rho,valueFk,QP_NF);
while (true)
{
// trust region step
while (true)
{
// poly.print();
parallelImprove(&poly, &k, rho, &valueFk, poly.vBase);
niter--;
if ((niter==0)
||(of->isConstrained&&checkForTermination(poly.NewtonPoints[k], poly.vBase, rhoEnd)))
{
poly.vBase+=poly.NewtonPoints[k];
fprintf(stderr,"rho=%e; fo=%e; NF=%i\n", rho,valueFk,of->getNFE());
of->valueBest=valueFk;
of->xBest=poly.vBase;
// to do : compute H and Lambda
Vector vG(dim);
Matrix mH(dim,dim);
poly.gradientHessian(poly.NewtonPoints[k],vG,mH);
of->finalize(vG,mH,FullLambda.clone());
return;
}
// to debug:
fprintf(stderr,"Best Value Objective=%e (nfe=%i)\n", valueFk, of->getNFE());
d=ConstrainedL2NormMinimizer(poly,k,delta,&info,1000,&lambda1,poly.vBase,of);
// if (d.euclidianNorm()>delta)
// {
// printf("Warning d to long: (%e > %e)\n", d.euclidianNorm(), delta);
// }
normD=mmin(d.euclidianNorm(), delta);
d+=poly.NewtonPoints[k];
// next line is equivalent to reduction=valueFk-poly(d);
// BUT is more precise (no rounding error)
reduction=-poly.shiftedEval(d,valueFk);
//if (normD<0.5*rho) { evalNeeded=true; break; }
if ((normD<0.5*rho)&&(!forceTRStep)) { evalNeeded=true; break; }
// IF THE MODEL REDUCTION IS SMALL, THEN WE DO NOT SAMPLE FUNCTION
// AT THE NEW POINT. WE THEN WILL TRY TO IMPROVE THE MODEL.
noise=0.5*mmax(of->noiseAbsolute*(1+of->noiseRelative), abs(valueFk)*of->noiseRelative);
if ((reduction<noise)&&(!forceTRStep)) { evalNeeded=true; break; }
forceTRStep=false; evalNeeded=false;
if (quickHack) (*quickHack)(poly,k);
tmp=poly.vBase+d; nerror=0; valueOF=of->eval(tmp, &nerror);
of->saveValue(tmp,valueOF,nerror);
if (nerror)
{
// evaluation failed
delta*=0.5;
if (normD>=2*rho) continue;
break;
}
if (!of->isFeasible(tmp, &r))
{
fprintf(stderr, "violation: %e\n",r);
}
// update of delta:
r=(valueFk-valueOF)/reduction;
if (r<=0.1) delta=0.5*normD;
else if (r<0.7) delta=mmax(0.5*delta, normD);
else delta=mmax(rho+ normD, mmax(1.25*normD, delta));
// powell's heuristics:
if (delta<1.5*rho) delta=rho;
if (valueOF<valueFk)
{
t=poly.findAGoodPointToReplace(-1, rho, d,&modelStep);
k=t; valueFk=valueOF;
improvement=true;
// fprintf(stderr,"Value Objective=%e\n", valueOF);
} else
{
t=poly.findAGoodPointToReplace(k, rho, d,&modelStep);
improvement=false;
// fprintf(stderr,".");
};
if (t<0) { poly.updateM(d, valueOF); break; }
// If we are along constraints, it's more important to update
// the polynomial with points which increase its quality.
// Thus, we will skip this update to use only points coming
// from checkIfValidityIsInBound
if ((!of->isConstrained)||(improvement)||(reduction>0.0)||(normD<rho)) poly.replace(t, d, valueOF);
if (improvement) continue;
// if (modelStep>4*rho*rho) continue;
if (modelStep>2*rho) continue;
if (normD>=2*rho) continue;
break;
}
// model improvement step
forceTRStep=true;
// fprintf(stderr,"improvement step\n");
bound=0.0;
if (normD<0.5*rho)
{
bound=0.5*sqr(rho)*lambda1;
if (poly.nUpdateOfM<10) bound=0.0;
}
parallelImprove(&poly, &k, rho, &valueFk, poly.vBase);
// !! change d (if needed):
t=poly.checkIfValidityIsInBound(d, k, bound, rho );
if (t>=0)
{
if (quickHack) (*quickHack)(poly,k);
tmp=poly.vBase+d; nerror=0; valueOF=of->eval(tmp, &nerror);
if (nerror)
{
Vector GXk(dim);
Matrix H(dim,dim);
poly.NewtonBasis[t].gradientHessian(poly.NewtonPoints[k],GXk,H);
double rhot=rho,vmax;
while (nerror)
{
rhot*=.5;
d=LAGMAXModified(GXk,H,rhot,vmax);
d+=poly.NewtonPoints[k];
tmp=poly.vBase+d; nerror=0; valueOF=of->eval(tmp, &nerror);
of->saveValue(tmp,valueOF,nerror);
}
}
poly.replace(t, d, valueOF);
if ((valueOF<valueFk)&&
(of->isFeasible(tmp))) { k=t; valueFk=valueOF; };
continue;
}
// the model is perfect for this value of rho:
// OR
// we have crossed a non_linear constraint which prevent us to advance
if ((normD<=rho)||(reduction<0.0)) break;
}
// change rho because no improvement can now be made:
if (rho<=rhoEnd) break;
fprintf(stderr,"rho=%e; fo=%e; NF=%i\n", rho,valueFk,of->getNFE());
if (rho<16*rhoEnd) rhoNew=rhoEnd;
else if (rho<250*rhoEnd) rhoNew=sqrt(rho*rhoEnd);
else rhoNew=0.1*rho;
delta=mmax(0.5*rho,rhoNew);
rho=rhoNew;
// update of the polynomial: translation of x[k].
// replace BASE by BASE+x[k]
poly.translate(poly.NewtonPoints[k]);
}
parallelFinish();
Vector vG(dim);
Matrix mH(dim,dim);
if (evalNeeded)
{
tmp=poly.vBase+d; nerror=0; valueOF=of->eval(tmp,&nerror);
of->saveValue(tmp,valueOF,nerror);
if ((nerror)||(valueOF<valueFk))
{
poly.vBase+=poly.NewtonPoints[k];
poly.gradientHessian(poly.NewtonPoints[k],vG,mH);
}
else
{
valueFk=valueOF; poly.vBase=tmp;
poly.gradientHessian(d,vG,mH);
}
} else
{
poly.vBase+=poly.NewtonPoints[k];
poly.gradientHessian(poly.NewtonPoints[k],vG,mH);
}
// delete[] points; :not necessary: done in destructor of poly which is called automatically:
fprintf(stderr,"rho=%e; fo=%e; NF=%i\n", rho,valueFk,of->getNFE());
of->valueBest=valueFk;
of->xBest=poly.vBase;
of->finalize(vG,mH,FullLambda.clone());
}
XMLNode xmlLoadFromResource(const TCHAR* lpName,
LPCTSTR tag,
XMLResults *pResults) {
LPTSTR lpRes;
HRSRC hResInfo;
HGLOBAL hRes;
int l, len;
// Find the xml resource.
hResInfo = FindResource (hInst, lpName, TEXT("XMLDialog"));
if (hResInfo == NULL) {
MessageBoxX(hWndMainWindow,
TEXT("Can't find resource"),
TEXT("Dialog error"),
MB_OK|MB_ICONEXCLAMATION);
// unable to find the resource
return XMLNode::emptyXMLNode;
}
// Load the wave resource.
hRes = LoadResource (hInst, hResInfo);
if (hRes == NULL) {
MessageBoxX(hWndMainWindow,
TEXT("Can't load resource"),
TEXT("Dialog error"),
MB_OK|MB_ICONEXCLAMATION);
// unable to load the resource
return XMLNode::emptyXMLNode;
}
// Lock the wave resource and do something with it.
lpRes = (LPTSTR)LockResource (hRes);
if (lpRes) {
l = SizeofResource(hInst,hResInfo);
if (l>0) {
char *buf= (char*)malloc(l+2);
if (!buf) {
//StartupStore(_T("------ LoadFromRes malloc error%s"),NEWLINE); // 100101
MessageBoxX(hWndMainWindow,
TEXT("Can't allocate memory"),
TEXT("Dialog error"),
MB_OK|MB_ICONEXCLAMATION);
// unable to allocate memory
return XMLNode::emptyXMLNode;
}
strncpy(buf,(char*)lpRes,l);
buf[l]=0; // need to explicitly null-terminate.
buf[l+1]=0;
len = l;
#if defined(WIN32) || defined(UNDER_CE)
#ifdef _UNICODE
#if !defined(UNDER_CE) && (WINDOWSPC<1)
if (!IsTextUnicode(buf,mmin(l,10000),NULL))
{
#endif
LPTSTR b2=(LPTSTR)malloc(l*2+2);
if (b2==NULL) StartupStore(_T(".... LoadFromRes Malloc1 failed\n")); // 100101
MultiByteToWideChar(CP_ACP, // code page
MB_PRECOMPOSED, // character-type options
buf, // string to map
l, // number of bytes in string
b2, // wide-character buffer
l*2+2); // size of buffer
free(buf);
buf=(char*)b2;
buf[l*2]= 0;
buf[l*2+1]= 0;
#if !defined(UNDER_CE) && (WINDOWSPC<1)
}
#endif
#else
if (IsTextUnicode(buf,mmin(l,10000),NULL))
{
l>>=1;
LPTSTR b2=(LPTSTR)malloc(l+2);
if (b2==NULL) StartupStore(_T(".... LoadFromRes Malloc2 failed\n")); // 100101
WideCharToMultiByte(CP_ACP, // code page
0, // performance and mapping flags
(const WCHAR*)buf, // wide-character string
l, // number of chars in string
b2, // buffer for new string
l+2, // size of buffer
NULL, // default for unmappable chars
NULL // set when default char used
);
free(buf);
buf=(char*)b2;
}
#endif
#endif
XMLNode x=XMLNode::parseString((LPTSTR)buf,tag,pResults);
free(buf);
return x;
}
//------------------------------------------------------------------
// Storm3D_Mesh_CollisionTable::ReBuild
//------------------------------------------------------------------
void Storm3D_Mesh_CollisionTable::ReBuild(Storm3D_Mesh *mesh)
{
// TEMP!!!
//return;
// Set owner
owner=mesh;
// Allocate memory for collision table
delete tree;
tree = 0;
if(!owner->vertexes || !owner->faces[0])
return;
face_amount=owner->face_amount[0];
SAFE_DELETE_ARRAY(faces);
faces = new CollisionFace[face_amount];
VC2 mmin(10000000.f, 10000000.f);
VC2 mmax(-10000000.f, -10000000.f);
// Create collision table
for(int fi = 0; fi < face_amount; ++fi)
{
// Set vertexes
faces[fi].vertex0=owner->vertexes[owner->faces[0][fi].vertex_index[0]].position;
faces[fi].vertex1=owner->vertexes[owner->faces[0][fi].vertex_index[1]].position;
faces[fi].vertex2=owner->vertexes[owner->faces[0][fi].vertex_index[2]].position;
// Create edges
faces[fi].e01=owner->vertexes[owner->faces[0][fi].vertex_index[1]].position-owner->vertexes[owner->faces[0][fi].vertex_index[0]].position;
faces[fi].e02=owner->vertexes[owner->faces[0][fi].vertex_index[2]].position-owner->vertexes[owner->faces[0][fi].vertex_index[0]].position;
VC3 e12=owner->vertexes[owner->faces[0][fi].vertex_index[2]].position-owner->vertexes[owner->faces[0][fi].vertex_index[1]].position;
VC3 cross_e01_e02 = faces[fi].e01.GetCrossWith(faces[fi].e02);
if(cross_e01_e02.GetSquareLength() < 0.0001f)
{
CollisionFace &face = faces[fi];
face.sphere.radius = 0;
continue;
}
// Create plane
faces[fi].plane=PLANE(cross_e01_e02.GetNormalized(),faces[fi].vertex0);
// psd
CollisionFace *face = &faces[fi];
face->sphere.position = (face->vertex0 + face->vertex1 + face->vertex2) / 3;
face->sphere.radius = face->sphere.position.GetSquareRangeTo(face->vertex0);
float tmp = face->sphere.position.GetSquareRangeTo(face->vertex1);
if(tmp > face->sphere.radius)
face->sphere.radius = tmp;
tmp = face->sphere.position.GetSquareRangeTo(face->vertex2);
if(tmp > face->sphere.radius)
face->sphere.radius = tmp;
face->sphere.radius = sqrtf(face->sphere.radius);
if(face->sphere.position.x - face->sphere.radius < mmin.x)
mmin.x = face->sphere.position.x - face->sphere.radius;
if(face->sphere.position.z - face->sphere.radius < mmin.y)
mmin.y = face->sphere.position.z - face->sphere.radius;
if(face->sphere.position.x + face->sphere.radius > mmax.x)
mmax.x = face->sphere.position.x + face->sphere.radius;
if(face->sphere.position.z + face->sphere.radius > mmax.y)
mmax.y = face->sphere.position.z + face->sphere.radius;
}
tree = new StaticQuadtree<CollisionFace>(mmin, mmax);
//tree = new Quadtree<CollisionFace>(mmin, mmax);
for(int i = 0; i < face_amount; ++i)
{
CollisionFace &face = faces[i];
if(face.sphere.radius < 0.001f)
continue;
//tree->insert(&face, face.sphere.position, face.sphere.radius);
float mmin = std::min(face.vertex0.y, face.vertex1.y);
mmin = std::min(mmin, face.vertex2.y);
float mmax = std::max(face.vertex0.y, face.vertex1.y);
mmax = std::max(mmax, face.vertex2.y);
tree->insert(face, face.sphere.position, face.sphere.radius, mmin, mmax);
}
}
static
void fallbackQSort3(uint32_t* fmap,
uint32_t* eclass,
int32_t loSt,
int32_t hiSt)
{
int32_t unLo, unHi, ltLo, gtHi, n, m;
int32_t sp, lo, hi;
uint32_t med, r, r3;
int32_t stackLo[FALLBACK_QSORT_STACK_SIZE];
int32_t stackHi[FALLBACK_QSORT_STACK_SIZE];
r = 0;
sp = 0;
fpush(loSt, hiSt);
while (sp > 0) {
AssertH(sp < FALLBACK_QSORT_STACK_SIZE - 1, 1004);
fpop(lo, hi);
if (hi - lo < FALLBACK_QSORT_SMALL_THRESH) {
fallbackSimpleSort(fmap, eclass, lo, hi);
continue;
}
/* Random partitioning. Median of 3 sometimes fails to
* avoid bad cases. Median of 9 seems to help but
* looks rather expensive. This too seems to work but
* is cheaper. Guidance for the magic constants
* 7621 and 32768 is taken from Sedgewick's algorithms
* book, chapter 35.
*/
r = ((r * 7621) + 1) % 32768;
r3 = r % 3;
if (r3 == 0)
med = eclass[fmap[lo]];
else if (r3 == 1)
med = eclass[fmap[(lo+hi)>>1]];
else
med = eclass[fmap[hi]];
unLo = ltLo = lo;
unHi = gtHi = hi;
while (1) {
while (1) {
if (unLo > unHi) break;
n = (int32_t)eclass[fmap[unLo]] - (int32_t)med;
if (n == 0) {
mswap(fmap[unLo], fmap[ltLo]);
ltLo++;
unLo++;
continue;
};
if (n > 0) break;
unLo++;
}
while (1) {
if (unLo > unHi) break;
n = (int32_t)eclass[fmap[unHi]] - (int32_t)med;
if (n == 0) {
mswap(fmap[unHi], fmap[gtHi]);
gtHi--; unHi--;
continue;
};
if (n < 0) break;
unHi--;
}
if (unLo > unHi) break;
mswap(fmap[unLo], fmap[unHi]); unLo++; unHi--;
}
AssertD(unHi == unLo-1, "fallbackQSort3(2)");
if (gtHi < ltLo) continue;
n = mmin(ltLo-lo, unLo-ltLo); mvswap(fmap, lo, unLo-n, n);
m = mmin(hi-gtHi, gtHi-unHi); mvswap(fmap, unLo, hi-m+1, m);
n = lo + unLo - ltLo - 1;
m = hi - (gtHi - unHi) + 1;
if (n - lo > hi - m) {
fpush(lo, n);
fpush(m, hi);
} else {
fpush(m, hi);
fpush(lo, n);
}
}
void simpleQPSolve(Matrix mH, Vector vG, Matrix mA, Vector vB, // in
Vector vP, Vector vLambda, int *info) // out
{
const double tolRelFeasibility=1e-6;
// int *info=NULL;
int dim=mA.nColumn(), nc=mA.nLine(), ncr, i,j,k, lastJ=-2, *ii;
MatrixTriangle M(dim);
Matrix mAR, mZ(dim,dim-1), mHZ(dim,dim-1), mZHZ(dim-1,dim-1);
Vector vTmp(mmax(nc,dim)), vYB(dim), vD(dim), vTmp2(dim), vTmp3(nc), vLast(dim),
vBR_QP, vLambdaScale(nc);
VectorChar vLB(nc);
double *lambda=vLambda, *br, *b=vB, violationMax, violationMax2,
*rlambda,ax,al,dviolation, **a=mA, **ar=mAR, mymin, mymax, maxb, *llambda,
**r,t, *scaleLambda=vLambdaScale;
char finished=0, feasible=0, *lb=vLB;
if (info) *info=0;
// remove lines which are null
k=0; vi_QP.setSize(nc); maxb=0.0;
for (i=0; i<nc; i++)
{
mymin=INF; mymax=-INF;
for (j=0; j<dim; j++)
{
mymin=mmin(mymin,a[i][j]);
mymax=mmax(mymax,a[i][j]);
if ((mymin!=mymax)||(mymin!=0.0)||(mymax!=0.0)) break;
}
if ((mymin!=mymax)||(mymin!=0.0)||(mymax!=0.0))
{
lambda[k]=lambda[i];
maxb=mmax(maxb,condorAbs(b[i]));
scaleLambda[k]=mA.euclidianNorm(i);
vi_QP[k]=i;
k++;
}
}
nc=k; vi_QP.setSize(nc); ii=vi_QP; maxb=(1.0+maxb)*tolRelFeasibility;
vLast.zero();
for (i=0; i<nc; i++) if (lambda[i]!=0.0) lb[i]=2; else lb[i]=1;
while (!finished)
{
finished=1;
mAR.setSize(dim,dim); mAR.zero(); ar=mAR;
vBR_QP.setSize(mmin(nc,dim)); br=vBR_QP;
ncr=0;
for (i=0; i<nc; i++)
if (lambda[i]!=0.0)
{
// mAR.setLines(ncr,mA,ii[i],1);
k=ii[i];
t=scaleLambda[ncr];
for (j=0; j<dim; j++) ar[ncr][j]=a[k][j]*t;
br[ncr]=b[ii[i]]*t;
ncr++;
}
mAR.setSize(ncr,dim);
vBR_QP.setSize(ncr);
vLastLambda_QP.copyFrom(vLambda); llambda=vLastLambda_QP;
if (ncr==0)
{
// compute step
vYB.copyFrom(vG);
vYB.multiply(-1.0);
if (mH.cholesky(M))
{
M.solveInPlace(vYB);
M.solveTransposInPlace(vYB);
} else
{
printf("warning: cholesky factorisation failed.\n");
if (info) *info=2;
}
vLambda.zero();
} else
{
Matrix mAR2=mAR.clone(), mQ2;
MatrixTriangle mR2;
mAR2.QR(mQ2,mR2);
mAR.QR(mQ_QP,mR_QP, viPermut_QP); // content of mAR is destroyed here !
bQRFailed_QP=0;
r=mR_QP;
for (i=0; i<ncr; i++)
if (r[i][i]==0.0)
{
// one constraint has been wrongly added.
bQRFailed_QP=1;
QPReconstructLambda(vLambda,vLambdaScale); vP.copyFrom(vLast); return;
}
for (i=0; i<ncr; i++)
if (viPermut_QP[i]!=i)
{
// printf("whoups.\n");
}
if (ncr<dim)
{
mQ_QP.getSubMatrix(mZ,0,ncr);
// Z^t H Z
mH.multiply(mHZ,mZ);
mZ.transposeAndMultiply(mZHZ,mHZ);
mQ_QP.setSize(dim,ncr);
}
// form Yb
vBR_QP.permutIn(vTmp,viPermut_QP);
mR_QP.solveInPlace(vTmp);
mQ_QP.multiply(vYB,vTmp);
if (ncr<dim)
{
// ( vG + H vYB )^t Z : result in vD
mH.multiply(vTmp,vYB);
vTmp+=vG;
vTmp.transposeAndMultiply(vD,mZ);
// calculate current delta (result in vD)
vD.multiply(-1.0);
if (mZHZ.cholesky(M))
{
M.solveInPlace(vD);
M.solveTransposInPlace(vD);
}
else
{
printf("warning: cholesky factorisation failed.\n");
if (info) *info=2;
};
// evaluate vX* (result in vYB):
mZ.multiply(vTmp, vD);
vYB+=vTmp;
}
// evaluate vG* (result in vTmp2)
mH.multiply(vTmp2,vYB);
vTmp2+=vG;
// evaluate lambda star (result in vTmp):
mQ2.transposeAndMultiply(vTmp,vTmp2);
mR2.solveTransposInPlace(vTmp);
// evaluate lambda star (result in vTmp):
mQ_QP.transposeAndMultiply(vTmp3,vTmp2);
mR_QP.solveTransposInPlace(vTmp3);
vTmp3.permutOut(vTmp,viPermut_QP);
rlambda=vTmp;
ncr=0;
for (i=0; i<nc; i++)
if (lambda[i]!=0.0)
{
lambda[i]=rlambda[ncr];
ncr++;
}
} // end of test on ncr==0
// find the most violated constraint j among non-active Linear constraints:
j=-1;
if (nc>0)
{
k=-1; violationMax=-INF; violationMax2=-INF;
for (i=0; i<nc; i++)
{
if (lambda[i]<=0.0) // test to see if this constraint is not active
{
ax=mA.scalarProduct(ii[i],vYB);
dviolation=b[ii[i]]-ax;
if (llambda[i]==0.0)
{
// the constraint was not active this round
// thus, it can enter the competition for the next
// active constraint
if (dviolation>maxb)
{
// the constraint should be activated
if (dviolation>violationMax2)
{ k=i; violationMax2=dviolation; }
al=mA.scalarProduct(ii[i],vLast)-ax;
if (al>0.0) // test to see if we are going closer
{
dviolation/=al;
if (dviolation>violationMax )
{ j=i; violationMax =dviolation; }
}
}
} else
{
lb[i]--;
if (feasible)
{
if (lb[i]==0)
{
vLambda.copyFrom(vLastLambda_QP);
if (lastJ>=0) lambda[lastJ]=0.0;
QPReconstructLambda(vLambda,vLambdaScale);
vP.copyFrom(vYB);
return;
}
} else
{
if (lb[i]==0)
{
if (info) *info=1;
QPReconstructLambda(vLambda,vLambdaScale);
vP.zero();
return;
}
}
finished=0; // this constraint was wrongly activated.
lambda[i]=0.0;
}
}
}
// !!! the order the tests is important here !!!
if ((j==-1)&&(!feasible))
{
feasible=1;
for (i=0; i<nc; i++) if (llambda[i]!=0.0) lb[i]=2; else lb[i]=1;
}
if (j==-1) { j=k; violationMax=violationMax2; } // change j to k after feasible is set
if (ncr==mmin(dim,nc)) {
if (feasible) { // feasible must have been checked before
QPReconstructLambda(vLambda,vLambdaScale); vP.copyFrom(vYB); return;
} else
{
if (info) *info=1;
QPReconstructLambda(vLambda,vLambdaScale); vP.zero(); return;
}
}
// activation of constraint only if ncr<mmin(dim,nc)
if (j>=0) { lambda[j]=1e-5; finished=0; } // we need to activate a new constraint
// else if (ncr==dim){
// QPReconstructLambda(vLambda); vP.copyFrom(vYB); return; }
}
// to prevent rounding error
if (j==lastJ) {
// if (0) {
QPReconstructLambda(vLambda,vLambdaScale); vP.copyFrom(vYB); return; }
lastJ=j;
vLast.copyFrom(vYB);
}
QPReconstructLambda(vLambda,vLambdaScale); vP.copyFrom(vYB); return;
}
void FindPMV(MotionVector *MV_ptr, int x, int y,
int *pmv0, int *pmv1, int block, int newgob, int half_pel)
{
int p1,p2,p3;
int xin1,xin2,xin3;
int yin1,yin2,yin3;
int vec1,vec2,vec3;
int l8,o8,or8;
l8 = o8 = or8 = 0;
vec1 = (l8 ? 2 : 0) ; yin1 = y ; xin1 = x-1;
vec2 = (o8 ? 3 : 0) ; yin2 = y-1; xin2 = x;
vec3 = (or8? 3 : 0) ; yin3 = y-1; xin3 = x+1;
if (half_pel) {
p1 = (x > 0) ?
2*((*(MV_ptr + yin1*Global::mbc + xin1)).x) + (*(MV_ptr+ yin1*Global::mbc + xin1)).x_half : 0;
p2 = (y > 0) ?
2*((*(MV_ptr + yin2*Global::mbc + xin2)).x) + (*(MV_ptr + yin2*Global::mbc + xin2)).x_half : 2*NO_VEC;
if((y > 0) && (x < Global::mbc - 1))
p3 = 2*((*(MV_ptr+yin3*Global::mbc + xin3)).x) + ((*(MV_ptr + yin3*Global::mbc + xin3)).x_half);
else if(x == Global::mbc - 1)
p3 = 0;
else /* y == 0 && x != Global::mbc - 1 */
p3 = 2*NO_VEC;
}
else {
p1 = (x > 0) ?
(2*((*(MV_ptr + yin1*Global::mbc + xin1)).x)) : 0;
p2 = (y > 0) ?
(2*((*(MV_ptr + yin2*Global::mbc + xin2)).x)) : 2*NO_VEC;
if((y > 0) && (x < Global::mbc - 1))
p3 = 2*((*(MV_ptr + yin3*Global::mbc + xin3)).x);
else if(x == Global::mbc - 1)
p3 = 0;
else /* y == 0 && x != Global::mbc - 1 */
p3 = 2*NO_VEC;
}
if (newgob && (block == 0 || block == 1 || block == 2))
p2 = 2*NO_VEC;
if (p2 == 2*NO_VEC) { p2 = p3 = p1; }
*pmv0 = p1+p2+p3 - mmax(p1,mmax(p2,p3)) - mmin(p1,mmin(p2,p3));
if (half_pel) {
p1 = (x > 0) ?
(2*((*(MV_ptr + yin1*Global::mbc + xin1)).y)) + ((*(MV_ptr + yin1*Global::mbc + xin1)).y_half) : 0;
p2 = (y > 0) ?
2*((*(MV_ptr + yin2*Global::mbc + xin2)).y) + ((*(MV_ptr + yin2*Global::mbc + xin2)).y_half) : 2*NO_VEC;
if((y > 0) && (x < Global::mbc - 1))
p3 = 2*((*(MV_ptr + yin3*Global::mbc + xin3)).y) + ((*(MV_ptr + Global::mbc*yin3 + xin3)).y_half);
else if(x == Global::mbc - 1)
p3 = 0;
else /* y == 0 && x != Global::mbc - 1 */
p3 = 2*NO_VEC;
}
else {
p1 = (x > 0) ?
(2*((*(MV_ptr + yin1*Global::mbc + xin1)).y)) : 0;
p2 = (y > 0) ?
(2*((*(MV_ptr + yin2*Global::mbc + xin2)).y)) : 2*NO_VEC;
if((y > 0) && (x < Global::mbc - 1))
p3 = 2*((*(MV_ptr + yin3*Global::mbc + xin3)).y);
else if(x == Global::mbc - 1)
p3 = 0;
else /* y == 0 && x != Global::mbc - 1 */
p3 = 2*NO_VEC;
}
if (newgob && (block == 0 || block == 1 || block == 2))
p2 = 2*NO_VEC;
if (p2 == 2*NO_VEC) { p2 = p3 = p1; }
*pmv1 = p1+p2+p3 - mmax(p1,mmax(p2,p3)) - mmin(p1,mmin(p2,p3));
return;
}
InterPolynomial::InterPolynomial( unsigned _deg, double rho,Vector _vBase, Matrix data, ObjectiveFunction *of)
// unsigned _nPtsTotal, Vector *_Pp, double *_Yp )
: Polynomial(data.nColumn()-2, _deg), M(0.0), nUpdateOfM(0), NewtonBasis(NULL), NewtonPoints(NULL), kbest(-1)
{
nPtsUsed=choose( _deg+d->dim,d->dim );
if (data.nLine()==0)
{
printf( "InterPolynomial::InterPolynomial( double *) : No data\n");
getchar(); exit(-1);
}
// Generate basis
if (_vBase==Vector::emptyVector)
{
double rhosmall=rho*ROOT2*.5;
GenerateBasis(rho,rhosmall, data,NULL);
if (!NewtonBasis)
{
GenerateBasis(rho,rho*.5, data,of);
}
} else
{
int i,ii,j=0,k,mdim=dim();
double norm, *p, *pb=_vBase;
KeepBests kb(nPtsUsed*2,mdim);
// fprintf(stderr,"Value Objective=%e\n", vBase);
ii=data.lineIndex(_vBase);
if (ii!=-1)
{
p=data[ii];
kb.add(0.0,p[mdim],p); j++;
}
i=data.nLine();
while (i--)
{
if (i==ii) continue;
p=data[i];
if (p[mdim+1]) continue;
norm=0; k=mdim;
while (k--) norm+=sqr(p[k]-pb[k]);
if ((rho==0.0)||
((norm<=4.00*rho*rho))//&&(norm>.25*rhomin*rhomin))
)
{
kb.add(norm,p[mdim], p); j++;
}
}
if (j<(int)nPtsUsed)
{
printf("Not Enough points in DB to build interpolation polynomial\n");
return;
}
// we have retained only the 2*nPtsUsed best points:
j=mmin(j,(int)(2*nPtsUsed));
NewtonPoints=new Vector[j];
valuesF=(double*)malloc(j*sizeof(double));
for (i=0; i<j; i++)
{
valuesF[i]=kb.getValue(i);
NewtonPoints[i]=Vector(mdim, kb.getOptValue(i));
}
//kbest=findK(valuesF,j,NULL,NewtonPoints);
//vBase=NewtonPoints[kbest].clone();
vBase=_vBase;
for (i=0; i<j; i++) NewtonPoints[i]-=vBase;
ComputeLagrangeBasis(valuesF, j);
}
if (NewtonBasis==NULL) return;
// test();
// Compute Interpolant
// double *NewtonCoefPoly=NewtonCoefficient(_Yp);
double *NewtonCoefPoly=valuesF;
double *coc= NewtonCoefPoly+nPtsUsed-1;
Polynomial *ppc= NewtonBasis+nPtsUsed-1;
this->copyFrom((Polynomial)(*coc * *ppc)); //take highest degree
int i=nPtsUsed-1;
if (i)
while(i--)
(*this) += *(--coc) * *(--ppc);
// No reallocation here because of the order of
// the summation
//free(NewtonCoefPoly);
}
if (Mode == MODE_INTRA) { /* Intra */
qcoeff[0] = mmax(1,mmin(254,coeff[0] >> 3)); /* was: /8 */
if(QP == 8) {
for(i = 1; i < 64; i++) {
level = (abs(coeff[i])) >> 4;
qcoeff[i] = mmin(127, mmax(-127,sign(coeff[i]) * level));
if(qcoeff[i])
CBP = CBP_Mask;
}
} else { /* QP != 8 */
for(i = 1; i < 64; i++) {
level = (abs(coeff[i])) / (2*QP);
qcoeff[i] = mmin(127,mmax(-127,sign(coeff[i]) * level));
if(qcoeff[i])
CBP = CBP_Mask;
}
} /* End QP == 8 */
}
else { /* non Intra */
if(QP == 8) {
for(i = 0; i < 64; i++) {
tempje = abs(coeff[i]) - 4;
if(tempje < 0) {
*(qcoeff++) = 0;
} else {
level = tempje >> 4; /* Note 2*QP == 2*8 = 2^4 == ">>4" */
if((*(qcoeff++) = mmin(127,mmax(-127,(sign(coeff[i]) > 0) ?
