void ode_evaluate(
CppAD::vector<Float> &x ,
size_t m ,
CppAD::vector<Float> &fm )
{
typedef CppAD::vector<Float> Vector;
size_t n = x.size();
size_t ell;
CPPAD_ASSERT_KNOWN( m == 0 || m == 1,
"ode_evaluate: m is not zero or one"
);
CPPAD_ASSERT_KNOWN(
((m==0) & (fm.size()==n)) || ((m==1) & (fm.size()==n*n)),
"ode_evaluate: the size of fm is not correct"
);
if( m == 0 )
ell = n;
else ell = n + n * n;
// set up the case we are integrating
Float ti = 0.;
Float tf = 1.;
Float smin = 1e-5;
Float smax = 1.;
Float scur = 1.;
Float erel = 0.;
vector<Float> yi(ell), eabs(ell);
size_t i, j;
for(i = 0; i < ell; i++)
{ eabs[i] = 1e-10;
if( i < n )
yi[i] = 1.;
else yi[i] = 0.;
}
// return values
Vector yf(ell), ef(ell), maxabs(ell);
size_t nstep;
// construct ode method for taking one step
ode_evaluate_method<Float> method(m, x);
// solve differential equation
yf = OdeErrControl(method,
ti, tf, yi, smin, smax, scur, eabs, erel, ef, maxabs, nstep);
if( m == 0 )
{ for(i = 0; i < n; i++)
fm[i] = yf[i];
}
else
{ for(i = 0; i < n; i++)
for(j = 0; j < n; j++)
fm[i * n + j] = yf[n + i * n + j];
}
return;
}
void reverse() {
if (xf() == 0) {
xf() = 1;
xi()--;
} else if (xf() == 1) {
xf() = 0;
xi()++;
} else if (yf() == 0) {
yf() = 1;
yi()--;
} else if (yf() == 1) {
yf() = 0;
yi()++;
}
}
//------------------------------------------------------------------------------
// class Object base class
//------------------------------------------------------------------------------
CObject::CObject(gfxSprite *nspr1, short x, short y)
{
dead = false;
spr = nspr1;
xi(x);
yi(y);
if(spr)
{
iw = (short)spr->getWidth();
ih = (short)spr->getHeight();
}
velx = 0.0f;
vely = 0.0f;
state = 0;
collisionWidth = iw;
collisionHeight = ih;
collisionOffsetX = 0;
collisionOffsetY = 0;
iNetworkID = g_iNextNetworkID++;
}
void create_population()
{
cl_brute_generator bg;
bg.init(init_l, alphabet);
cl_brute_generator bg_yi;
bg_yi.init(init_l_yi, alphabet_yi);
while(population.size() < psize)
{
cl_term* term = new cl_term(bg.create_random(), &counter);
vector<cl_term*> yi(ansamble.size());
vector<string> yi_str(ansamble.size());
for (size_t i = 0 ; i < yi.size() ; i++)
{
yi[i] = new cl_term(bg_yi.create_random(), &counter);
yi_str[i] = yi[i]->conv2str();
}
if (uniqchecker.try_to_add(term->conv2str(), yi_str))
{
population.push_back(make_cl_rmdlga2_member(term, yi, yi_str));
}
else
{
delete term;
}
}
print_best_result();
}
ExecStatus
DistinctDoit<View0>::propagate(Space& home, const ModEventDelta&) {
if (x0.assigned()) {
GlbRanges<View0> xi(x0);
GlbRanges<ConstSetView> yi(y);
if (Iter::Ranges::equal(xi,yi)) { return ES_FAILED; }
else { return home.ES_SUBSUMED(*this); }
}
assert(x0.lubSize()-x0.glbSize() >0);
if (x0.cardMin()>y.cardMax()) { return home.ES_SUBSUMED(*this); }
if (x0.cardMax()<y.cardMin()) { return home.ES_SUBSUMED(*this); }
//These tests are too expensive, we should only do them
//in the 1 unknown left case.
GlbRanges<View0> xi1(x0);
LubRanges<ConstSetView> yi1(y);
if (!Iter::Ranges::subset(xi1,yi1)){ return home.ES_SUBSUMED(*this); }
LubRanges<View0> xi2(x0);
GlbRanges<ConstSetView> yi2(y);
if (!Iter::Ranges::subset(yi2,xi2)){ return home.ES_SUBSUMED(*this); }
// from here, we know y\subseteq lub(x) and glb(x)\subseteq y
if (x0.lubSize() == y.cardMin() && x0.lubSize() > 0) {
GECODE_ME_CHECK(x0.cardMax(home, x0.lubSize() - 1));
return home.ES_SUBSUMED(*this);
}
if (x0.glbSize() == y.cardMin()) {
GECODE_ME_CHECK(x0.cardMin(home, x0.glbSize() + 1));
return home.ES_SUBSUMED(*this);
}
return ES_FIX;
}
bool Path::PointInside(const Objects & objects, const Vec2 & pt, bool debug) const
{
vector<Vec2> x_ints;
vector<Vec2> y_ints;
for (unsigned i = m_start; i <= m_end; ++i)
{
Bezier bez(objects.beziers[objects.data_indices[i]].ToAbsolute(objects.bounds[i]));
vector<Vec2> xi(bez.SolveX(pt.x));
vector<Vec2> yi(bez.SolveY(pt.y));
x_ints.insert(x_ints.end(), xi.begin(), xi.end());
y_ints.insert(y_ints.end(), yi.begin(), yi.end());
}
//Debug("Solved for intersections");
unsigned bigger = 0;
unsigned smaller = 0;
for (unsigned i = 0; i < x_ints.size(); ++i)
{
if (debug)
Debug("X Intersection %u at %f,%f vs %f,%f", i,Double(x_ints[i].x), Double(x_ints[i].y), Double(pt.x), Double(pt.y));
if (x_ints[i].y >= pt.y)
{
++bigger;
}
}
smaller = x_ints.size() - bigger;
if (debug)
{
Debug("%u horizontal, %u bigger, %u smaller", x_ints.size(), bigger, smaller);
}
if (smaller % 2 == 0 || bigger % 2 == 0)
return false;
bigger = 0;
smaller = 0;
for (unsigned i = 0; i < y_ints.size(); ++i)
{
if (debug)
Debug("Y Intersection %u at %f,%f vs %f,%f", i,Double(y_ints[i].x), Double(y_ints[i].y), Double(pt.x), Double(pt.y));
if (y_ints[i].x >= pt.x)
{
++bigger;
}
}
smaller = y_ints.size() - bigger;
if (debug)
{
Debug("%u vertical, %u bigger, %u smaller", y_ints.size(), bigger, smaller);
}
if (smaller % 2 == 0 || bigger % 2 == 0)
return false;
return true;
}
// [[Rcpp::export]]
VectorXd bootR2(const MatrixXd X, const VectorXd y, int nBoot){
RNGScope scope;
const int n(X.rows());
// const int p(X.cols());
VectorXd R2s(nBoot);
MatrixXd Xi(X);
VectorXd yi(y);
IntegerVector prm(n);
// double R2i(R2(Xi, yi));
for(int i = 0; i < nBoot; ++i) {
prm = bootPerm(n);
Xi = shuffleMatrix(X, prm);
yi = shuffleVector(y, prm);
R2s(i) = R2(Xi, yi);
}
return R2s;
}
// [[Rcpp::export]]
MatrixXd bootCoef(const MatrixXd X, const VectorXd y, int nBoot){
RNGScope scope;
const int n(X.rows());
const int p(X.cols());
MatrixXd bootBeta(nBoot, p);
MatrixXd Xi(X);
VectorXd yi(y);
IntegerVector prm(n);
VectorXd betaHati(p);
for(int i = 0; i < nBoot; ++i) {
prm = bootPerm(n);
Xi = shuffleMatrix(X, prm);
yi = shuffleVector(y, prm);
betaHati = betaHat(Xi, yi);
for(int j = 0; j < p; ++j) {
bootBeta(i, j) = betaHati[j];
}
}
return bootBeta;
}
void cnbint(
int i,
const double pos[][3],
const double vel[][3],
const double mass[],
int nnb,
int list[],
double f[3],
double fdot[3]){
const int NBMAX = 512;
assert(nnb <= NBMAX);
double xbuf[NBMAX] __attribute__ ((aligned(16)));
double ybuf[NBMAX] __attribute__ ((aligned(16)));
double zbuf[NBMAX] __attribute__ ((aligned(16)));
float vxbuf[NBMAX] __attribute__ ((aligned(16)));
float vybuf[NBMAX] __attribute__ ((aligned(16)));
float vzbuf[NBMAX] __attribute__ ((aligned(16)));
float mbuf[NBMAX] __attribute__ ((aligned(16)));
for(int k=0; k<nnb; k++){
int j = list[k];
xbuf[k] = pos[j][0];
ybuf[k] = pos[j][1];
zbuf[k] = pos[j][2];
vxbuf[k] = vel[j][0];
vybuf[k] = vel[j][1];
vzbuf[k] = vel[j][2];
mbuf[k] = mass[j];
}
for(int k=nnb; k%4; k++){
xbuf[k] = 16.0;
ybuf[k] = 16.0;
zbuf[k] = 16.0;
vxbuf[k] = 0.0f;
vybuf[k] = 0.0f;
vzbuf[k] = 0.0f;
mbuf[k] = 0.0f;
}
v4df xi(pos[i][0]);
v4df yi(pos[i][1]);
v4df zi(pos[i][2]);
v4sf vxi(vel[i][0]);
v4sf vyi(vel[i][1]);
v4sf vzi(vel[i][2]);
v4df ax(0.0), ay(0.0), az(0.0);
v4sf jx(0.0), jy(0.0), jz(0.0);
for(int k=0; k<nnb; k+=4){
v4df xj(xbuf + k);
v4df yj(ybuf + k);
v4df zj(zbuf + k);
v4sf vxj(vxbuf + k);
v4sf vyj(vybuf + k);
v4sf vzj(vzbuf + k);
v4sf mj(mbuf + k);
v4sf dx = v4sf::_v4sf(xj - xi);
v4sf dy = v4sf::_v4sf(yj - yi);
v4sf dz = v4sf::_v4sf(zj - zi);
v4sf dvx = vxj - vxi;
v4sf dvy = vyj - vyi;
v4sf dvz = vzj - vzi;
v4sf r2 = dx*dx + dy*dy + dz*dz;
v4sf rv = dx*dvx + dy*dvy + dz*dvz;
rv *= v4sf(-3.0f);
// v4sf rinv1 = r2.rsqrt() & v4sf(mask);
v4sf rinv1 = r2.rsqrt();
v4sf rinv2 = rinv1 * rinv1;
rv *= rinv2;
v4sf rinv3 = mj * rinv1 * rinv2;
dx *= rinv3; ax += v4df(dx);
dy *= rinv3; ay += v4df(dy);
dz *= rinv3; az += v4df(dz);
dvx *= rinv3; jx += dvx;
dvy *= rinv3; jy += dvy;
dvz *= rinv3; jz += dvz;
dx *= rv; jx += dx;
dy *= rv; jy += dy;
dz *= rv; jz += dz;
}
f[0] = ax.sum();
f[1] = ay.sum();
f[2] = az.sum();
fdot[0] = (v4df(jx)).sum();
fdot[1] = (v4df(jy)).sum();
fdot[2] = (v4df(jz)).sum();
assert(f[0] == f[0]);
assert(f[1] == f[1]);
assert(f[2] == f[2]);
assert(fdot[0] == fdot[0]);
assert(fdot[1] == fdot[1]);
assert(fdot[2] == fdot[2]);
}
void step(const vb::Array<double> &o) {
int r = 0;
double oo = o.at(vb::ucoo(vb::coo{xi(), yi()}));
if (yf() == 1) {
r = 1;
oo = -1 / oo;
yf() = xf();
xf() = 0;
} else if (xf() == 1) {
r = 2;
yf() = 1 - yf();
xf() = 0;
} else if (yf() == 0) {
r = 3;
oo = -1 / oo;
yf() = 1 - xf();
xf() = 0;
}
int di = 0, dj = 0;
if (oo < -yf()) {
xf() = -yf() / oo;
yf() = 0;
dj = -1;
} else if (oo < 1 - yf()) {
xf() = 1;
yf() = yf() + oo;
di = 1;
} else {
xf() = (1 - yf()) / oo;
yf() = 1;
dj = 1;
}
if (r == 1) {
int ti = di;
di = dj;
dj = -ti;
double tx = xf();
xf() = yf();
yf() = 1 - tx;
} else if (r == 2) {
di = -di;
dj = -dj;
xf() = 1 - xf();
yf() = 1 - yf();
} else if (r == 3) {
int ti = di;
di = -dj;
dj = ti;
double tx = xf();
xf() = 1 - yf();
yf() = tx;
}
xf() -= di;
xi() += di;
yf() -= dj;
yi() += dj;
}
void IO_MovingObject::collision_detection_map()
{
xf(fx + velx);
flipsidesifneeded();
fPrecalculatedY = fy + vely; //Fixes weird float rounding error. Must be computed here before casting to int. Otherwise, this will miss the bottom collision, but then hit the side collision and the player can slide out of 1x1 spaces.
float fPlatformVelX = 0.0f;
float fPlatformVelY = 0.0f;
float fTempY = fy;
if(platform)
{
if(!onice)
{
fPlatformVelX = platform->fVelX;
xf(fx + fPlatformVelX);
flipsidesifneeded();
}
fPlatformVelY = platform->fVelY;
if(platform->fOldVelY < 0.0f)
fy += platform->fOldVelY;
fPrecalculatedY += platform->fOldVelY;
}
iHorizontalPlatformCollision = -1;
iVerticalPlatformCollision = -1;
g_map.movingPlatformCollision(this);
fy = fTempY;
if(fPrecalculatedY + collisionHeight < 0.0f)
{ // on top outside of the screen
yf(fPrecalculatedY);
vely = CapFallingVelocity(GRAVITATION + vely);
if(!platform)
{
inair = true;
onice = false;
}
return;
}
else if(fPrecalculatedY + collisionHeight >= 480.0f)
{ //on ground outside of the screen?
yi(-collisionHeight);
fOldY = fy - 1.0f;
onice = false;
return;
}
//Could be optimized with bit shift >> 5
short ty = (short)fy / TILESIZE;
short ty2 = ((short)fy + collisionHeight) / TILESIZE;
short tx = -1;
//-----------------------------------------------------------------
// x axis first (--)
//-----------------------------------------------------------------
if(fy + collisionHeight >= 0.0f)
{
if(velx + fPlatformVelX > 0.01f || iHorizontalPlatformCollision == 3)
{ //moving right
if(fx + collisionWidth >= 640.0f)
{
tx = (short)(fx + collisionWidth - 640.0f) / TILESIZE;
fOldX -= 640.0f;
}
else
tx = ((short)fx + collisionWidth) / TILESIZE;
IO_Block * topblock = g_map.block(tx, ty);
IO_Block * bottomblock = g_map.block(tx, ty2);
bool fTopBlockSolid = topblock && !topblock->isTransparent() && !topblock->isHidden();
bool fBottomBlockSolid = bottomblock && !bottomblock->isTransparent() && !bottomblock->isHidden();
if(fTopBlockSolid || fBottomBlockSolid)
{
bool processOtherBlock = true;
if(fTopBlockSolid) //collide with top block
{
if(iHorizontalPlatformCollision == 3)
{
KillObjectMapHazard();
return;
}
topblock->collide(this, 1);
flipsidesifneeded();
removeifprojectile(this, true, true);
processOtherBlock = false;
SideBounce(true);
}
if(processOtherBlock && fBottomBlockSolid) //then bottom
{
if(iHorizontalPlatformCollision == 3)
{
KillObjectMapHazard();
return;
}
bottomblock->collide(this, 1);
flipsidesifneeded();
removeifprojectile(this, true, true);
SideBounce(true);
}
}
else if((g_map.map(tx, ty) & tile_flag_solid) || (g_map.map(tx, ty2) & tile_flag_solid))
{ //collision on the right side.
if(iHorizontalPlatformCollision == 3)
{
KillObjectMapHazard();
return;
}
xf((float)((tx << 5) - collisionWidth) - 0.2f); //move to the edge of the tile (tile on the right -> mind the object width)
fOldX = fx;
if(velx > 0.0f)
velx = -velx;
flipsidesifneeded();
removeifprojectile(this, true, true);
SideBounce(true);
}
}
else if(velx + fPlatformVelX < -0.01f || iHorizontalPlatformCollision == 1)
{ //moving left
tx = (short)fx / TILESIZE;
//Just in case fx < 0 and wasn't caught by flipsidesifneeded()
if(tx < 0)
tx = 0;
IO_Block * topblock = g_map.block(tx, ty);
IO_Block * bottomblock = g_map.block(tx, ty2);
bool fTopBlockSolid = topblock && !topblock->isTransparent() && !topblock->isHidden();
bool fBottomBlockSolid = bottomblock && !bottomblock->isTransparent() && !bottomblock->isHidden();
if(fTopBlockSolid || fBottomBlockSolid)
{
bool processOtherBlock = true;
if(fTopBlockSolid) //collide with top block
{
if(iHorizontalPlatformCollision == 1)
{
KillObjectMapHazard();
return;
}
topblock->collide(this, 3);
flipsidesifneeded();
removeifprojectile(this, true, true);
processOtherBlock = false;
SideBounce(false);
}
if(processOtherBlock && fBottomBlockSolid) //then bottom
{
if(iHorizontalPlatformCollision == 1)
{
KillObjectMapHazard();
return;
}
bottomblock->collide(this, 3);
flipsidesifneeded();
removeifprojectile(this, true, true);
SideBounce(false);
}
}
else if((g_map.map(tx, ty) & tile_flag_solid) || (g_map.map(tx, ty2) & tile_flag_solid))
{
if(iHorizontalPlatformCollision == 1)
{
KillObjectMapHazard();
return;
}
xf((float)((tx << 5) + TILESIZE) + 0.2f); //move to the edge of the tile
fOldX = fx;
if(velx < 0.0f)
velx = -velx;
flipsidesifneeded();
removeifprojectile(this, true, true);
SideBounce(false);
}
}
}
short txl = -1, txr = -1;
txl = ix / TILESIZE;
if(ix + collisionWidth >= 640)
txr = (ix + collisionWidth - 640) / TILESIZE;
else
txr = (ix + collisionWidth) / TILESIZE;
//-----------------------------------------------------------------
// then y axis (|)
//-----------------------------------------------------------------
float fMovingUp = vely;
if(platform)
fMovingUp = vely + fPlatformVelY - bounce;
if(fMovingUp < -0.01f)
{
ty = (short)(fPrecalculatedY) / TILESIZE;
IO_Block * leftblock = g_map.block(txl, ty);
IO_Block * rightblock = g_map.block(txr, ty);
if(leftblock && !leftblock->isTransparent() && !leftblock->isHidden()) //then left
{
if(iVerticalPlatformCollision == 2)
KillObjectMapHazard();
leftblock->collide(this, 0);
return;
}
if(rightblock && !rightblock->isTransparent() && !rightblock->isHidden()) //then right
{
if(iVerticalPlatformCollision == 2)
KillObjectMapHazard();
rightblock->collide(this, 0);
return;
}
if((g_map.map(txl, ty) & tile_flag_solid) || (g_map.map(txr, ty) & tile_flag_solid))
{
if(iVerticalPlatformCollision == 2)
KillObjectMapHazard();
yf((float)((ty << 5) + TILESIZE) + 0.2f);
fOldY = fy - 1.0f;
if(vely < 0.0f)
vely = -vely;
}
else
{
yf(fPrecalculatedY);
vely += GRAVITATION;
}
if(!platform)
{
inair = true;
onice = false;
}
}
else
{ //moving down / on ground
ty = ((short)fPrecalculatedY + collisionHeight) / TILESIZE;
IO_Block * leftblock = g_map.block(txl, ty);
IO_Block * rightblock = g_map.block(txr, ty);
bool fLeftBlockSolid = leftblock && !leftblock->isTransparent() && !leftblock->isHidden();
bool fRightBlockSolid = rightblock && !rightblock->isTransparent() && !rightblock->isHidden();
if(fLeftBlockSolid || fRightBlockSolid)
{
bool processOtherBlock = true;
if(fLeftBlockSolid) //collide with left block
{
processOtherBlock = leftblock->collide(this, 2);
if(!platform)
{
inair = false;
onice = false;
}
}
if(processOtherBlock && fRightBlockSolid) //then right
{
rightblock->collide(this, 2);
if(!platform)
{
inair = false;
onice = false;
}
}
if(iVerticalPlatformCollision == 0)
KillObjectMapHazard();
return;
}
int leftTile = g_map.map(txl, ty);
int rightTile = g_map.map(txr, ty);
if((leftTile & tile_flag_solid_on_top) || (rightTile & tile_flag_solid_on_top))
{
if((fOldY + collisionHeight) / TILESIZE < ty)
{
vely = BottomBounce();
yf((float)((ty << 5) - collisionHeight) - 0.2f);
fOldY = fy - GRAVITATION;
if(!platform)
{
inair = false;
onice = false;
}
platform = NULL;
if(iVerticalPlatformCollision == 0)
KillObjectMapHazard();
return;
}
}
bool fSuperDeathTileUnderObject = fObjectDiesOnSuperDeathTiles && (((leftTile & tile_flag_super_death_top) && (rightTile & tile_flag_super_death_top)) ||
((leftTile & tile_flag_super_death_top) && !(rightTile & tile_flag_solid)) ||
(!(leftTile & tile_flag_solid) && (rightTile & tile_flag_super_death_top)));
if(((leftTile & tile_flag_solid) || (rightTile & tile_flag_solid)) && !fSuperDeathTileUnderObject)
{
vely = BottomBounce();
yf((float)((ty << 5) - collisionHeight) - 0.2f);
fOldY = fy;
if(!platform)
{
inair = false;
if((leftTile & tile_flag_ice && ((rightTile & tile_flag_ice) || rightTile == tile_flag_nonsolid || rightTile == tile_flag_gap)) ||
(rightTile & tile_flag_ice && ((leftTile & tile_flag_ice) || leftTile == tile_flag_nonsolid || leftTile == tile_flag_gap)))
onice = true;
else
onice = false;
}
platform = NULL;
if(iVerticalPlatformCollision == 0)
KillObjectMapHazard();
}
else if(fSuperDeathTileUnderObject)
{
KillObjectMapHazard();
return;
}
else
{
yf(fPrecalculatedY);
vely = CapFallingVelocity(GRAVITATION + vely);
if(!platform)
inair = true;
}
}
if(!platform && inair)
onice = false;
}
void
multibinpacking(Home home,
int n, int m, int k,
const IntVarArgs& y,
const IntVarArgs& x,
const IntSharedArray& D,
const IntSharedArray& B,
IntConLevel)
{
/// Check input sizes
if (n*k != D.size() )
throw ArgumentSizeMismatch("Int::multibinpacking");
if (k != B.size() )
throw ArgumentSizeMismatch("Int::multibinpacking");
if (n != x.size() )
throw ArgumentSizeMismatch("Int::multibinpacking");
if (m*k != y.size() )
throw ArgumentSizeMismatch("Int::multibinpacking");
for (int i=B.size(); i--; )
Limits::nonnegative(B[i],"Int::multibinpacking");
if (home.failed()) return;
/// Post first each single binpacking constraint
/// Capacity constraint for each dimension
for ( int j = 0; j < m; ++j )
for ( int l = 0; l < k; ++l ) {
IntView yi(y[j*k+l]);
GECODE_ME_FAIL(yi.lq(home,B[l]));
}
/// Post a binpacking constraints for each dimension
for ( int l = 0; l < k; ++l ) {
ViewArray<OffsetView> yv(home,m);
for (int j=m; j--; )
yv[j] = OffsetView(y[j*k+l],0);
ViewArray<BinPacking::Item> xs(home,x.size());
for (int i=xs.size(); i--; )
xs[i] = BinPacking::Item(x[i],D[i*k+l]);
Support::quicksort(&xs[0], xs.size());
GECODE_ES_FAIL(Int::BinPacking::Pack::post(home,yv,xs));
}
/// Clique Finding and Alldifferent posting
{
/// Firt construct the conflict graph
graph_t* g = graph_new(n);
for ( int a = 0; a < n-1; ++a ) {
for ( int b = a+1; b < n; ++b ) {
int v = a; /// The variable with smaller domain
int w = b;
unsigned int nl = 0;
if ( x[a].size() > x[b].size() ) {
v = b;
w = a;
}
IntVarValues i(x[v]);
IntVarValues j(x[w]);
while ( i() ) {
if ( i.val() != j.val() ) {
if ( i.val() < j.val() )
break;
else
++i;
} else {
for ( int l = 0; l < k; ++l )
if ( D[a*k+l] + D[b*k+l] > B[l] ) {
nl++;
break;
}
++i; ++j;
}
}
if ( nl >= x[v].size() )
GRAPH_ADD_EDGE(g,a,b);
}
}
/// Consitency cheking: look for the maximum clique
clique_options* opts;
opts = (clique_options*) malloc (sizeof(clique_options));
opts->time_function=NULL;
opts->reorder_function=reorder_by_default;
opts->reorder_map=NULL;
opts->user_function=NULL;
opts->user_data=NULL;
opts->clique_list=NULL;
opts->clique_list_length=0;
set_t s;
s = clique_find_single ( g, 0, 0, TRUE, opts);
if ( s != NULL ) {
if ( set_size(s) > m ) {
set_free(s);
free(opts);
graph_free(g);
GECODE_ES_FAIL(ES_FAILED);
}
if ( true ) { //option == 1 ) { FIXING
for ( int a = 0, j = 0; a < n; ++a ) {
if ( SET_CONTAINS_FAST(s,a) ) {
assert( x[a].in(j) );
IntView xi(x[a]);
GECODE_ME_FAIL(xi.eq(home,j++));
}
}
}
}
if ( s!=NULL )
set_free(s);
/// List every maximal clique in the conflict graph
opts->user_function=record_clique_func;
clique_find_all ( g, 2, 0, TRUE, opts);
for ( int c = 0; c < clique_count; c++ ) {
ViewArray<IntView> xv(home, set_size(clique_list[c]));
for ( int a = 0, idx = 0; a < n; ++a )
if ( SET_CONTAINS_FAST(clique_list[c],a) )
xv[idx++] = x[a];
GECODE_ES_FAIL(Distinct::Dom<IntView>::post(home,xv));
set_free(clique_list[c]);
}
free(opts);
graph_free(g);
}
}
int check()
{
if(1 == yi() || 1 == er() || 1 == san())
return 1;
return 0;
}
const size_t SpikeNeuronBase::localId() const {
return xi() + colSize()*yi();
}
void
vpPose::poseDementhonPlan(vpHomogeneousMatrix &cMo)
{
#if (DEBUG_LEVEL1)
std::cout << "begin CCalculPose::PoseDementhonPlan()" << std::endl ;
#endif
unsigned int i,j,k ;
if (c3d !=NULL) delete []c3d ;
c3d = new vpPoint[npt] ;
vpPoint p0 = listP.front() ;
vpPoint P ;
i=0 ;
for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
{
P = *it;
c3d[i] = P ;
c3d[i].set_oX(P.get_oX()-p0.get_oX()) ;
c3d[i].set_oY(P.get_oY()-p0.get_oY()) ;
c3d[i].set_oZ(P.get_oZ()-p0.get_oZ()) ;
i++ ;
}
vpMatrix a ;
try
{
a.resize(npt-1,3) ;
}
catch(...)
{
vpERROR_TRACE(" ") ;
throw ;
}
for (i=1 ; i < npt ; i++)
{
a[i-1][0]=c3d[i].get_oX();
a[i-1][1]=c3d[i].get_oY();
a[i-1][2]=c3d[i].get_oZ();
}
// calcul a^T a
vpMatrix ata ;
ata = a.t()*a ;
/* essai FC pour debug SVD */
/*
vpMatrix ata_old ;
ata_old = a.t()*a ;
vpMatrix ata((ata_old.getRows()-1),(ata_old.getCols()-1)) ;
for (i=0;i<ata.getRows();i++)
for (j=0;j<ata.getCols();j++) ata[i][j] = ata_old[i][j];
*/
vpMatrix ata_sav;
ata_sav = ata;
#if (DEBUG_LEVEL2)
{
std::cout << "a" << std::endl <<a<<std::endl ;
std::cout << "ata" << std::endl <<ata<<std::endl ;
}
#endif
// calcul (a^T a)^-1
vpMatrix ata1(ata.getRows(),ata.getCols()) ;
vpMatrix v(ata.getRows(),ata.getCols());
vpColVector sv(ata.getRows());
// ata1 = ata.i() ;
unsigned int imin = 0;
double s = 0.0;
//calcul de ata^-1
ata.svd(sv,v) ;
unsigned int nc = sv.getRows() ;
for (i=0; i < nc ; i++)
if (sv[i] > s) s = sv[i];
s *= 0.0002;
int irank = 0;
for (i=0;i<nc;i++)
if (sv[i] > s ) irank++;
double svm = 100.0;
for (i = 0; i < nc; i++)
if (sv[i] < svm) { imin = i; svm = sv[i]; }
#if (DEBUG_LEVEL2)
{
std::cout << "rang: " << irank << std::endl ;;
std::cout <<"imin = " << imin << std::endl ;
std::cout << "sv " << sv.t() << std::endl ;
}
#endif
for (i=0 ; i < ata.getRows() ; i++)
for (j=0 ; j < ata.getCols() ; j++)
{
ata1[i][j] = 0.0;
for (k=0 ; k < nc ; k++)
if (sv[k] > s)
ata1[i][j] += ((v[i][k]*ata[j][k])/sv[k]);
}
vpMatrix b ; // b=(at a)^-1*at
b = ata1*a.t() ;
//calcul de U
vpColVector U(3) ;
U = ata.column(imin+1) ;
#if (DEBUG_LEVEL2)
{
std::cout << "a" << std::endl <<a<<std::endl ;
std::cout << "ata" << std::endl <<ata_sav<<std::endl ;
std::cout << "ata1" << std::endl <<ata1<<std::endl ;
std::cout << "ata1*ata" << std::endl << ata1*ata_sav ;
std::cout << "b" << std::endl << b ;
std::cout << "U " << U.t() << std::endl ;
}
#endif
vpColVector xi(npt) ;
vpColVector yi(npt) ;
//calcul de la premiere solution
for (i = 0; i < npt; i++)
{
xi[i] = c3d[i].get_x() ;
yi[i] = c3d[i].get_y() ;
}
vpColVector I0(3) ; I0 = 0 ;
vpColVector J0(3) ; J0 = 0 ;
vpColVector I(3) ;
vpColVector J(3) ;
for (i=1;i<npt;i++)
{
I0[0] += b[0][i-1] * (xi[i]-xi[0]);
I0[1] += b[1][i-1] * (xi[i]-xi[0]);
I0[2] += b[2][i-1] * (xi[i]-xi[0]);
J0[0] += b[0][i-1] * (yi[i]-yi[0]);
J0[1] += b[1][i-1] * (yi[i]-yi[0]);
J0[2] += b[2][i-1] * (yi[i]-yi[0]);
}
#if (DEBUG_LEVEL2)
{
std::cout << "I0 "<<I0.t() ;
std::cout << "J0 "<<J0.t() ;
}
#endif
s = -2.0*vpColVector::dotProd(I0,J0);
double c = J0.sumSquare() - I0.sumSquare() ;
double r,theta,si,co ;
calculRTheta(s, c, r, theta);
co = cos(theta);
si = sin(theta);
// calcul de la premiere solution
I = I0 + U*r*co ;
J = J0 + U*r*si ;
vpHomogeneousMatrix cMo1f ;
calculSolutionDementhon(xi[0], yi[0], I, J, cMo1f);
int erreur1 = calculArbreDementhon(b, U, cMo1f);
// calcul de la deuxieme solution
I = I0 - U*r*co ;
J = J0 - U*r*si ;
vpHomogeneousMatrix cMo2f;
calculSolutionDementhon(xi[0], yi[0], I, J, cMo2f);
int erreur2 = calculArbreDementhon(b, U, cMo2f);
if ((erreur1 == 0) && (erreur2 == -1)) cMo = cMo1f ;
if ((erreur1 == -1) && (erreur2 == 0)) cMo = cMo2f ;
if ((erreur1 == 0) && (erreur2 == 0))
{
double s1 = sqrt(computeResidualDementhon(cMo1f)/npt) ;
double s2 = sqrt(computeResidualDementhon(cMo2f)/npt) ;
if (s1<=s2) cMo = cMo1f ; else cMo = cMo2f ;
}
cMo[0][3] -= p0.get_oX()*cMo[0][0]+p0.get_oY()*cMo[0][1]+p0.get_oZ()*cMo[0][2];
cMo[1][3] -= p0.get_oX()*cMo[1][0]+p0.get_oY()*cMo[1][1]+p0.get_oZ()*cMo[1][2];
cMo[2][3] -= p0.get_oX()*cMo[2][0]+p0.get_oY()*cMo[2][1]+p0.get_oZ()*cMo[2][2];
delete [] c3d ; c3d = NULL ;
#if (DEBUG_LEVEL1)
std::cout << "end CCalculPose::PoseDementhonPlan()" << std::endl ;
#endif
}
int
vpPose::calculArbreDementhon(vpMatrix &b, vpColVector &U,
vpHomogeneousMatrix &cMo)
{
#if (DEBUG_LEVEL1)
std::cout << "begin vpPose::CalculArbreDementhon() " << std::endl;
#endif
unsigned int i, k;
int erreur = 0;
unsigned int cpt;
double s,c,si,co;
double smin,smin_old, s1,s2;
double r, theta;
vpHomogeneousMatrix cMo1,cMo2,cMo_old;
unsigned int iter_max = 20;
vpMatrix eps(iter_max+1,npt) ;
// on test si tous les points sont devant la camera
for(i = 0; i < npt; i++)
{
double z ;
z = cMo[2][0]*c3d[i].get_oX()+cMo[2][1]*c3d[i].get_oY()+cMo[2][2]*c3d[i].get_oZ() + cMo[2][3];
if (z <= 0.0) erreur = -1;
}
smin = sqrt(computeResidualDementhon(cMo)/npt) ;
vpColVector xi(npt) ;
vpColVector yi(npt) ;
if (erreur==0)
{
k=0;
for(i = 0; i < npt; i++)
{
xi[k] = c3d[i].get_x();
yi[k] = c3d[i].get_y();
if (k != 0)
{ // On ne prend pas le 1er point
eps[0][k] = (cMo[2][0]*c3d[i].get_oX() +
cMo[2][1]*c3d[i].get_oY() +
cMo[2][2]*c3d[i].get_oZ())/cMo[2][3];
}
k++;
}
vpColVector I0(3) ;
vpColVector J0(3) ;
vpColVector I(3) ;
vpColVector J(3) ;
vpHomogeneousMatrix cMo_old ;
smin_old = 2*smin ;
cpt = 0;
while ((cpt<20) && (smin_old > 0.01) && (smin <= smin_old))
{
#if (DEBUG_LEVEL2)
{
std::cout << "cpt " << cpt << std::endl ;
std::cout << "smin_old " << smin_old << std::endl ;
std::cout << "smin " << smin << std::endl ;
}
#endif
smin_old = smin;
cMo_old = cMo;
I0 = 0 ;
J0 = 0 ;
for (i=1;i<npt;i++)
{
s = (1.0+eps[cpt][i])*xi[i] - xi[0];
I0[0] += b[0][i-1] * s;
I0[1] += b[1][i-1] * s;
I0[2] += b[2][i-1] * s;
s = (1.0+eps[cpt][i])*yi[i] - yi[0];
J0[0] += b[0][i-1] * s;
J0[1] += b[1][i-1] * s;
J0[2] += b[2][i-1] * s;
}
s = -2.0*(vpColVector::dotProd(I0,J0));
c = J0.sumSquare() - I0.sumSquare() ;
calculRTheta(s,c,r,theta);
co = cos(theta);
si = sin(theta);
/* 1ere branche */
I = I0 + U*r*co ;
J = J0 + U*r*si ;
#if (DEBUG_LEVEL3)
{
std::cout << "I " << I.t() ;
std::cout << "J " << J.t() ;
}
#endif
calculSolutionDementhon(xi[0],yi[0],I,J,cMo1);
s1 = sqrt(computeResidualDementhon(cMo1)/npt) ;
#if (DEBUG_LEVEL3)
std::cout << "cMo1 "<< std::endl << cMo1 << std::endl ;
#endif
/* 2eme branche */
I = I0 - U*r*co ;
J = J0 - U*r*si ;
#if (DEBUG_LEVEL3)
{
std::cout << "I " << I.t() ;
std::cout << "J " << J.t() ;
}
#endif
calculSolutionDementhon(xi[0],yi[0],I,J,cMo2);
s2 = sqrt(computeResidualDementhon(cMo2)/npt) ;
#if (DEBUG_LEVEL3)
std::cout << "cMo2 "<< std::endl << cMo2 << std::endl ;
#endif
cpt ++;
if (s1 <= s2)
{
smin = s1;
k = 0;
for(i = 0; i < npt; i++)
{
if (k != 0) { // On ne prend pas le 1er point
eps[cpt][k] = (cMo1[2][0]*c3d[i].get_oX() + cMo1[2][1]*c3d[i].get_oY()
+ cMo1[2][2]*c3d[i].get_oZ())/cMo1[2][3];
}
k++;
}
cMo = cMo1 ;
}
else
{
smin = s2;
k = 0;
for(i = 0; i < npt; i++)
{
if (k != 0) { // On ne prend pas le 1er point
eps[cpt][k] = (cMo2[2][0]*c3d[i].get_oX() + cMo2[2][1]*c3d[i].get_oY()
+ cMo2[2][2]*c3d[i].get_oZ())/cMo2[2][3];
}
k++;
}
cMo = cMo2 ;
}
if (smin > smin_old)
{
#if (DEBUG_LEVEL2)
std::cout << "Divergence " << std::endl ;
#endif
cMo = cMo_old ;
}
#if (DEBUG_LEVEL2)
{
std::cout << "s1 = " << s1 << std::endl ;
std::cout << "s2 = " << s2 << std::endl ;
std::cout << "smin = " << smin << std::endl ;
std::cout << "smin_old = " << smin_old << std::endl ;
}
#endif
}
}
#if (DEBUG_LEVEL1)
std::cout << "end vpPose::CalculArbreDementhon() return "<< erreur << std::endl;
#endif
return erreur ;
}
