void MyWidget::process()
{
ui->plainTextEdit->clear();
variant = ui->comboBox->currentIndex();
print(QString("Variant %1").arg(variant));
print(QString("[%1; %2]").arg(x0(), 0, 'g', 5).arg(xFinish(), 0, 'g', 3));
//calc
qreal x = x0(), y = y0(), z = z0();
for (int i = 1; x < xFinish() || qFuzzyCompare(x, xFinish()); ++i)
{
qreal k1 = h() * f(x, y, z);
qreal q1 = h() * g(x, y, z);
qreal k2 = h() * f(x + h() / 2.0, y + k1 / 2.0, z + q1 / 2.0);
qreal q2 = h() * g(x + h() / 2.0, y + k1 / 2.0, z + q1 / 2.0);
qreal k3 = h() * f(x + h() / 2.0, y + k2 / 2.0, z + q2 / 2.0);
qreal q3 = h() * g(x + h() / 2.0, y + k2 / 2.0, z + q2 / 2.0);
qreal k4 = h() * f(x + h(), y + k3, z + q3);
qreal q4 = h() * g(x + h(), y + k3, z + q3);
print(QString("#%1").arg(i));
print(QString("y(%1) = %2").arg(x, 0, 'g', 5).arg(y, 0, 'g', 5));
print(QString("y_ex(%1) = %2").arg(x, 0, 'g',5).arg(yt(x), 0, 'g', 5));
print(QString("error(%1) = %2").arg(x, 0, 'g',5).arg(ypo(y, x), 0, 'g', 5));
x += h();
y += (k1 + 2.0 * k2 + 2.0 * k3 + k4) / 6.0;
z += (q1 + 2.0 * q2 + 2.0 * q3 + q4) / 6.0;
}
}
vector<CoeffT> Polynomial<CoeffT>::roots(size_t num_iterations, CoeffT ztol) const
{
assert(c.size() >= 1);
size_t n = c.size() - 1;
// initial guess
vector<complex<CoeffT>> z0(n);
for (size_t j = 0; j < n; j++) {
z0[j] = pow(complex<double>(0.4, 0.9), j);
}
// durand-kerner-weierstrass iterations
Polynomial<CoeffT> p = this->normalize();
for (size_t k = 0; k < num_iterations; k++) {
complex<CoeffT> num, den;
for (size_t i = 0; i < n; i++) {
num = p.evaluate(z0[i]);
den = 1.0;
for (size_t j = 0; j < n; j++) {
if (j == i) { continue; }
den = den * (z0[i] - z0[j]);
}
z0[i] = z0[i] - num / den;
}
}
vector<CoeffT> roots(n);
for (size_t j = 0; j < n; j++) {
roots[j] = abs(z0[j]) < ztol ? 0.0 : real(z0[j]);
}
sort(roots.begin(), roots.end());
return roots;
}
// Spatial second derivatives of effective sound speed
double NCPA::AtmosphericProfile::ddceffdzdz(double z, double phi) {
if (z-eps_z < z0()) {
return (this->ceff(z+2*eps_z,phi) - 2*this->ceff(z+eps_z,phi) + this->ceff(z,phi)) / (std::pow(eps_z,2.0));
} else {
return (this->ceff(z + eps_z, phi) + this->ceff(z - eps_z, phi)- 2.0*this->ceff(z,phi))/(std::pow(eps_z,2.0));
}
}
// Spatial second derivatives of sound speed
double NCPA::AtmosphericProfile::ddc0dzdz(double z) {
if (z-eps_z < z0()) {
return (this->c0(z+2*eps_z) - 2*this->c0(z+eps_z) + this->c0(z)) / (std::pow(eps_z,2.0));
} else {
return (this->c0(z + eps_z) + this->c0(z - eps_z)- 2.0*this->c0(z))/(std::pow(eps_z,2.0));
}
}
// Spatial derivatives of effective sound speed
double NCPA::AtmosphericProfile::dceffdz( double z, double phi ) {
if (z-eps_z < z0()) {
return (this->ceff(z+eps_z,phi) - this->ceff(z,phi)) / eps_z;
} else {
return (this->ceff(z+eps_z,phi) - this->ceff(z-eps_z,phi)) / (2.0*eps_z);
}
}
// Spatial derivatives of c0
double NCPA::AtmosphericProfile::dc0dz( double z ) {
if (z-eps_z < z0()) {
return (this->c0(z+eps_z) - this->c0(z)) / eps_z;
} else {
return (this->c0(z+eps_z) - this->c0(z-eps_z)) / (2.0*eps_z);
}
}
RVector DC1dModelling::pot1d(const RVector & R, const RVector & rho, const RVector & thk) {
RVector z0(R.size());
double rabs;
for (size_t i = 0; i < R.size(); i++) {
rabs = std::fabs(R[i]);
z0[i] = sum(myw_ * kern1d(myx_ / rabs, rho, thk) * 2.0) / rabs;
}
return z0;
}
void beijing2()
{ int m,n;
for(n=0;n<22;n++)
{
for(m=0;m<38;m++)
{
blackground z0(66,7,m,n,BACKGROUND_INTENSITY|FOREGROUND_INTENSITY);
z0.display1();
}
}
}
void mukuai()
{ int m,n;
for(n=0;n<3;n++)
{ for(m=0;m<15;m++)
{ blackground z0(86,8,m,n,BACKGROUND_GREEN|FOREGROUND_INTENSITY);
z0.display1();
}
}
SetConsoleTextAttribute(GetStdHandle(STD_OUTPUT_HANDLE),FOREGROUND_INTENSITY|BACKGROUND_GREEN|BACKGROUND_INTENSITY);
COORD pos1;
pos1.X=88;
pos1.Y=9;
SetConsoleCursorPosition(GetStdHandle(STD_OUTPUT_HANDLE),pos1);
printf("play again!");
for(n=0;n<3;n++)
{ for(m=0;m<8;m++)
{ blackground z1(74,25,m,n,BACKGROUND_GREEN|FOREGROUND_INTENSITY);
z1.display1();
}
}
SetConsoleTextAttribute(GetStdHandle(STD_OUTPUT_HANDLE),FOREGROUND_INTENSITY|BACKGROUND_GREEN|BACKGROUND_INTENSITY);
COORD pos2;
pos2.X=76;
pos2.Y=26;
SetConsoleCursorPosition(GetStdHandle(STD_OUTPUT_HANDLE),pos2);
printf("save");
for(n=0;n<3;n++)
{ for(m=0;m<8;m++)
{ blackground z2(84,25,m,n,BACKGROUND_GREEN|FOREGROUND_INTENSITY);
z2.display1();
}
}
SetConsoleTextAttribute(GetStdHandle(STD_OUTPUT_HANDLE),FOREGROUND_INTENSITY|BACKGROUND_GREEN|BACKGROUND_INTENSITY);
COORD pos3;
pos3.X=86;
pos3.Y=26;
SetConsoleCursorPosition(GetStdHandle(STD_OUTPUT_HANDLE),pos3);
printf("open");
for(n=0;n<3;n++)
{ for(m=0;m<8;m++)
{ blackground z2(94,25,m,n,BACKGROUND_GREEN|FOREGROUND_INTENSITY);
z2.display1();
}
}
SetConsoleTextAttribute(GetStdHandle(STD_OUTPUT_HANDLE),FOREGROUND_INTENSITY|BACKGROUND_GREEN|BACKGROUND_INTENSITY);
COORD pos4;
pos4.X=96 ;
pos4.Y=26;
SetConsoleCursorPosition(GetStdHandle(STD_OUTPUT_HANDLE),pos4);
printf("over");
}
void mu2(int x,int y,char po[24],int f)
{ int m,n;
int i=0;
for(m=0,n=0;m<6;m++)
{ blackground z0(x,y,m,n,BACKGROUND_BLUE|BACKGROUND_GREEN|BACKGROUND_INTENSITY|FOREGROUND_INTENSITY);
z0.display1();
if(f&&i<24)
{
po[i++]=x+m; // x
po[i++]=y+n; // y
po[i++]=1; // 颜色
}
}
}
int main() {
// Initial data z0 = [y(0), y'(0)]
Eigen::Vector2d z0;
z0 << 1,0;
// Final time
const double T = 10;
// Parameter
const double gamma = (3.+std::sqrt(3.)) / 6.;
// Mesh sizes
std::vector<int> N = {20, 40, 80, 160, 320, 640, 1280, 2560, 5120, 10240};
// Exact solution (only y(t)) given z0 = [y(0), y'(0)] and t
auto yex = [&z0] (double t) {
return 1./3.*std::exp(-t/2.) * ( 3.*z0(0) * std::cos( std::sqrt(3.)*t/2. ) +
std::sqrt(3.)*z0(0) * std::sin( std::sqrt(3.)*t/2. ) +
2.*std::sqrt(3.)*z0(1) * std::sin( std::sqrt(3.)*t/2. ) );
};
// Store old error for rate computation
double errold = 0;
std::cout << std::setw(15) << "n" << std::setw(15) << "maxerr" << std::setw(15) << "rate" << std::endl;
// Loop over all meshes
for(unsigned int i = 0; i < N.size(); ++i) {
int n = N.at(i);
// Get solution
auto sol = sdirkSolve(z0, n, T, gamma);
// Compute error
double err = std::abs(sol.back()(0) - yex(T));
// I/O
std::cout << std::setw(15) << n << std::setw(15) << err;
if(i > 0) std::cout << std::setw(15) << std::log2(errold /err);
std::cout << std::endl;
// Store old error
errold = err;
}
}
// ----------------------------------------------------------------
fpnpoly_t fpnpoly_from_qpoly(qpoly_t q, fppoly_t im)
{
int d = q.find_degree();
int p = im.get_char();
intmod_t z0(0, p);
fppoly_t z1(z0);
fppolymod_t z2(z0, im);
fpnpoly_t rv(z2);
for (int i = d; i >= 0; i--) {
intmod_t m = intmod_from_rat(q.get_coeff(i), p);
fppolymod_t c = fppolymod_t::prime_sfld_elt(m.get_residue(), im);
rv.set_coeff(i, c);
}
return rv;
}
void mu1(int x,int y,char po[24],int f)
{ int m,n;
int i=0;
//char po[12]={0};
for(m=0;m<2;m++)
{
for(n=0;n<3;n++)
{ blackground z0(x,y,m,n,BACKGROUND_BLUE|BACKGROUND_GREEN|BACKGROUND_INTENSITY|FOREGROUND_INTENSITY);
z0.display0();
if(f&&i<24)
{
po[i++]=x+m; // x
po[i++]=y+n; // y
po[i++]=1; // 颜色
}
}
}
}
void print(struct node * head)
{ int m,n,x1,y1,c1;
for(n=0;n<22;n++)
{
for(m=0;m<42;m++)
{
blackground z0(10,32,m,n,BACKGROUND_INTENSITY|FOREGROUND_INTENSITY);
z0.display1();
}
}
SetConsoleTextAttribute(GetStdHandle(STD_OUTPUT_HANDLE),FOREGROUND_BLUE|FOREGROUND_INTENSITY|BACKGROUND_INTENSITY);
COORD pos2;
pos2.X=15;
pos2.Y=33;
SetConsoleCursorPosition(GetStdHandle(STD_OUTPUT_HANDLE),pos2);
printf("------> 上次操作的记录 <------");
struct node * p;
p=head;
if(head!=0)
do
{ x1=p->x;
y1=p->y;
c1=p->color;
if(c1==1)
{blackground z(x1,y1+25,0,0,BACKGROUND_BLUE|BACKGROUND_GREEN|BACKGROUND_INTENSITY|FOREGROUND_INTENSITY);
z.display1();}
else if(c1==2)
{blackground z(x1,y1+25,0,0,BACKGROUND_BLUE|BACKGROUND_INTENSITY|FOREGROUND_INTENSITY);
z.display1();}
else if(c1==3)
{blackground z(x1,y1+25,0,0,BACKGROUND_GREEN|BACKGROUND_RED|BACKGROUND_INTENSITY|FOREGROUND_INTENSITY);
z.display1();}
else if(c1==4)
{blackground z(x1,y1+25,0,0,BACKGROUND_BLUE|BACKGROUND_RED|BACKGROUND_INTENSITY|FOREGROUND_INTENSITY);
z.display1();}
else if(c1==5)
{blackground z(x1,y1+25,0,0,BACKGROUND_RED|BACKGROUND_INTENSITY|FOREGROUND_INTENSITY);
z.display1();}
// printf("x=%d,y=%d,color=%d",p->x,p->y,p->color);
p=p->next;
} while(p!=0);
}
/*
* Numerical Recipes quotes a formula due to Rybicki for evaluating
* Dawson's Integral:
*
* exp(-x^2) integral exp(t^2).dt = 1/sqrt(pi) lim sum exp(-(z-n.h)^2) / n
* 0 to x h->0 n odd
*
* This can be adapted to erf(z).
*/
std::complex<double> cerf_rybicki(const std::complex<double>& z)
{
double h = 0.2; // numerical experiment suggests this is small enough
// choose an even n0, and then shift z->z-n0.h and n->n-h.
// n0 is chosen so that real((z-n0.h)^2) is as small as possible.
int n0 = 2 * static_cast<int>(z.imag() / (2 * h) + 0.5);
std::complex<double> z0(0.0, n0 * h);
std::complex<double> zp(z - z0);
std::complex<double> sum(0.0, 0.0);
// limits of sum chosen so that the end sums of the sum are
// fairly small. In this case exp(-(35.h)^2)=5e-22
for (int np = -35; np <= 35; np += 2) {
std::complex<double> t(zp.real(), zp.imag() - np * h);
std::complex<double> b(exp(t * t) / static_cast<double>(np + n0));
sum += b;
}
sum *= 2.0 * exp(-z * z) / pi;
return std::complex<double>(-sum.imag(), sum.real());
}
int
Munkres::step4(void) {
int rows = matrix.rows();
int cols = matrix.columns();
std::list<std::pair<int,int> > seq;
// use saverow, savecol from step 3.
std::pair<int,int> z0(saverow, savecol);
std::pair<int,int> z1(-1,-1);
std::pair<int,int> z2n(-1,-1);
seq.insert(seq.end(), z0);
int row, col = savecol;
/*
Increment Set of Starred Zeros
1. Construct the ``alternating sequence'' of primed and starred zeros:
Z0 : Unpaired Z' from Step 4.2
Z1 : The Z* in the column of Z0
Z[2N] : The Z' in the row of Z[2N-1], if such a zero exists
Z[2N+1] : The Z* in the column of Z[2N]
The sequence eventually terminates with an unpaired Z' = Z[2N] for some N.
*/
bool madepair;
do {
madepair = false;
for ( row = 0 ; row < rows ; row++ )
if ( mask_matrix(row,col) == STAR ) {
z1.first = row;
z1.second = col;
if ( pair_in_list(z1, seq) )
continue;
madepair = true;
seq.insert(seq.end(), z1);
break;
}
if ( !madepair )
break;
madepair = false;
for ( col = 0 ; col < cols ; col++ )
if ( mask_matrix(row,col) == PRIME ) {
z2n.first = row;
z2n.second = col;
if ( pair_in_list(z2n, seq) )
continue;
madepair = true;
seq.insert(seq.end(), z2n);
break;
}
} while ( madepair );
for ( std::list<std::pair<int,int> >::iterator i = seq.begin() ;
i != seq.end() ;
i++ ) {
// 2. Unstar each starred zero of the sequence.
if ( mask_matrix(i->first,i->second) == STAR )
mask_matrix(i->first,i->second) = NORMAL;
// 3. Star each primed zero of the sequence,
// thus increasing the number of starred zeros by one.
if ( mask_matrix(i->first,i->second) == PRIME )
mask_matrix(i->first,i->second) = STAR;
}
// 4. Erase all primes, uncover all columns and rows,
for ( int row = 0 ; row < mask_matrix.rows() ; row++ )
for ( int col = 0 ; col < mask_matrix.columns() ; col++ )
if ( mask_matrix(row,col) == PRIME )
mask_matrix(row,col) = NORMAL;
for ( int i = 0 ; i < rows ; i++ ) {
row_mask[i] = false;
}
for ( int i = 0 ; i < cols ; i++ ) {
col_mask[i] = false;
}
// and return to Step 2.
return 2;
}
/*!
Increment Set of Starred Zeros
1. Construct the ``alternating sequence'' of primed and starred zeros:
Z0 : Unpaired Z' from Step 4.2
Z1 : The Z* in the column of Z0
Z[2N] : The Z' in the row of Z[2N-1], if such a zero exists
Z[2N+1] : The Z* in the column of Z[2N]
The sequence eventually terminates with an unpaired Z' = Z[2N] for some N.
*/
int Munkres::step4(void)
{
std::list<std::pair<int,int> > seq;
// use saverow, savecol from step 3.
std::pair<int,int> z0(saverow, savecol);
std::pair<int,int> z1(-1,-1);
std::pair<int,int> z2n(-1,-1);
seq.insert(seq.end(), z0);
unsigned row, col = savecol;
bool madepair;
do {
madepair = false;
for ( row = 0 ; row < matrix.rows() ; row++ )
if ( mask_matrix(row,col) == Z_STAR ) {
z1.first = row;
z1.second = col;
if ( pair_in_list(z1, seq) )
continue;
madepair = true;
seq.insert(seq.end(), z1);
break;
}
if ( !madepair )
break;
madepair = false;
for ( col = 0 ; col < matrix.columns() ; col++ )
if ( mask_matrix(row,col) == Z_PRIME ) {
z2n.first = row;
z2n.second = col;
if ( pair_in_list(z2n, seq) )
continue;
madepair = true;
seq.insert(seq.end(), z2n);
break;
}
} while ( madepair );
for ( std::list<std::pair<int,int> >::iterator i = seq.begin() ;
i != seq.end() ;
i++ ) {
// 2. Unstar each starred zero of the sequence.
if ( mask_matrix(i->first,i->second) == Z_STAR )
mask_matrix(i->first,i->second) = Z_NORMAL;
// 3. Star each primed zero of the sequence,
// thus increasing the number of starred zeros by one.
if ( mask_matrix(i->first,i->second) == Z_PRIME )
mask_matrix(i->first,i->second) = Z_STAR;
}
// 4. Erase all primes, uncover all columns and rows,
for ( unsigned row = 0 ; row < mask_matrix.rows() ; row++ )
for ( unsigned col = 0 ; col < mask_matrix.columns() ; col++ )
if ( mask_matrix(row,col) == Z_PRIME )
mask_matrix(row,col) = Z_NORMAL;
for ( unsigned i = 0 ; i < matrix.rows() ; i++ ) {
row_mask[i] = false;
}
for ( unsigned i = 0 ; i < matrix.columns() ; i++ ) {
col_mask[i] = false;
}
// and return to Step 2.
return 2;
}
// This does a tri-linear interpolation of the derivatives at each of the 8
// grid points around the given point. This avoids additional calls to the
// function and speeds things up by a factor of 2 or so.
GLvoid MarchingCubes::surfaceNormal(GLvector &norm, GLfloat const& x, GLfloat const& y,
GLfloat const& z)
{
// needs offloading to the Grid class
GLvector V000, V001, V010, V011, V100, V101, V110, V111;
GLfloat gx(x/m_stepSize);
GLfloat gy(y/m_stepSize);
GLfloat gz(z/m_stepSize);
GLint x0( std::floor(gx) );
GLint y0( std::floor(gy) );
GLint z0( std::floor(gz) );
GLint x1(x0+1);
GLint y1(y0+1);
GLint z1(z0+1);
int i = x0; int j = y0; int k = z0;
V000.fX = (*m_grid)(i+1, j, k ) - (*m_grid)(i-1, j, k );
V000.fY = (*m_grid)(i, j+1, k ) - (*m_grid)(i, j-1, k );
V000.fZ = (*m_grid)(i, j, k+1) - (*m_grid)(i, j, k-1);
i = x0; j = y0; k = z1;
V001.fX = (*m_grid)(i+1, j, k ) - (*m_grid)(i-1, j, k );
V001.fY = (*m_grid)(i, j+1, k ) - (*m_grid)(i, j-1, k );
V001.fZ = (*m_grid)(i, j, k+1) - (*m_grid)(i, j, k-1);
i = x0; j = y1; k = z0;
V010.fX = (*m_grid)(i+1, j, k ) - (*m_grid)(i-1, j, k );
V010.fY = (*m_grid)(i, j+1, k ) - (*m_grid)(i, j-1, k );
V010.fZ = (*m_grid)(i, j, k+1) - (*m_grid)(i, j, k-1);
i = x0; j = y1; k = z1;
V011.fX = (*m_grid)(i+1, j, k ) - (*m_grid)(i-1, j, k );
V011.fY = (*m_grid)(i, j+1, k ) - (*m_grid)(i, j-1, k );
V011.fZ = (*m_grid)(i, j, k+1) - (*m_grid)(i, j, k-1);
i = x1; j = y0; k = z0;
V100.fX = (*m_grid)(i+1, j, k ) - (*m_grid)(i-1, j, k );
V100.fY = (*m_grid)(i, j+1, k ) - (*m_grid)(i, j-1, k );
V100.fZ = (*m_grid)(i, j, k+1) - (*m_grid)(i, j, k-1);
i = x1; j = y0; k = z1;
V101.fX = (*m_grid)(i+1, j, k ) - (*m_grid)(i-1, j, k );
V101.fY = (*m_grid)(i, j+1, k ) - (*m_grid)(i, j-1, k );
V101.fZ = (*m_grid)(i, j, k+1) - (*m_grid)(i, j, k-1);
i = x1; j = y1; k = z0;
V110.fX = (*m_grid)(i+1, j, k ) - (*m_grid)(i-1, j, k );
V110.fY = (*m_grid)(i, j+1, k ) - (*m_grid)(i, j-1, k );
V110.fZ = (*m_grid)(i, j, k+1) - (*m_grid)(i, j, k-1);
i = x1; j = y1; k = z1;
V111.fX = (*m_grid)(i+1, j, k ) - (*m_grid)(i-1, j, k );
V111.fY = (*m_grid)(i, j+1, k ) - (*m_grid)(i, j-1, k );
V111.fZ = (*m_grid)(i, j, k+1) - (*m_grid)(i, j, k-1);
GLvector p0;
p0.fX = gx - x0;
p0.fY = gy - y0;
p0.fZ = gz - z0;
GLvector p1;
p1.fX = x1 - gx;
p1.fY = y1 - gy;
p1.fZ = z1 - gz;
GLfloat dX, dY, dZ;
dX = V000.fX * p1.fX * p1.fY * p1.fZ
+ V001.fX * p1.fX * p1.fY * p0.fZ
+ V010.fX * p1.fX * p0.fY * p1.fZ
+ V011.fX * p1.fX * p0.fY * p0.fZ
+ V100.fX * p0.fX * p1.fY * p1.fZ
+ V101.fX * p0.fX * p1.fY * p0.fZ
+ V110.fX * p0.fX * p0.fY * p1.fZ
+ V111.fX * p0.fX * p0.fY * p0.fZ;
dY = V000.fY * p1.fX * p1.fY * p1.fZ
+ V001.fY * p1.fX * p1.fY * p0.fZ
+ V010.fY * p1.fX * p0.fY * p1.fZ
+ V011.fY * p1.fX * p0.fY * p0.fZ
+ V100.fY * p0.fX * p1.fY * p1.fZ
+ V101.fY * p0.fX * p1.fY * p0.fZ
+ V110.fY * p0.fX * p0.fY * p1.fZ
+ V111.fY * p0.fX * p0.fY * p0.fZ;
dZ = V000.fZ * p1.fX * p1.fY * p1.fZ
+ V001.fZ * p1.fX * p1.fY * p0.fZ
+ V010.fZ * p1.fX * p0.fY * p1.fZ
+ V011.fZ * p1.fX * p0.fY * p0.fZ
+ V100.fZ * p0.fX * p1.fY * p1.fZ
+ V101.fZ * p0.fX * p1.fY * p0.fZ
+ V110.fZ * p0.fX * p0.fY * p1.fZ
+ V111.fZ * p0.fX * p0.fY * p0.fZ;
norm.fX = -dX;
norm.fY = -dY;
norm.fZ = -dZ;
normalizeVector(norm, norm);
}
int CProblem::setModel()
{
//time_t start, end;
IloEnv env;
try
{
IloModel model(env);
IloCplex cplex(env);
/*Variables*/
IloNumVar lambda(env, "lambda");
IloNumVarArray c(env, n); //
for (unsigned int u = 0; u < n; u++)
{
std::stringstream ss;
ss << u;
std::string str = "c" + ss.str();
c[u] = IloNumVar(env, str.c_str());
}
IloIntVarArray z0(env, Info.numAddVar);
for (int i = 0; i < Info.numAddVar; i++)
{
std::stringstream ss;
ss << i;
std::string str = "z0_" + ss.str();
z0[i] = IloIntVar(env, 0, 1, str.c_str());
}
/* Objective*/
model.add(IloMinimize(env, lambda));
/*Constrains*/
/* d=function of the distance */
IloArray<IloNumArray> Par_d(env, n);
for (unsigned int u = 0; u < n; u++)
{
Par_d[u] = IloNumArray(env, n);
for (unsigned int v = 0; v < n; v++)
{
Par_d[u][v] = d[u][v];
}
}
int M = (max_distance + 1) * n;
for (int i = 0; i < Info.numAddVar; i++)
{
int u = Info.ConstrIndex[i * 2];
int v = Info.ConstrIndex[i * 2 + 1];
model.add(c[u] - c[v] + M * (z0[i]) >= Par_d[u][v]);
model.add(c[v] - c[u] + M * (1 - z0[i]) >= Par_d[u][v]);
}
// d(x) = 3 - x
if (max_distance == 2)
{
// Gridgraphen 1x2 (6 Knoten)
for (int i = 0; i < sqrt(n)-1; i+=1)
{
for (int j = 0; j < sqrt(n)-2; j+=2)
{
model.add(c[i * sqrt(n) + j] + c[i * sqrt(n) + j+2] +
c[(i+1) * sqrt(n) + j] + c[(i+1) * sqrt(n) + j+2] >= 16.2273);
}
}
//
}
//d(x) = 4 - x
if (max_distance == 3)
{
// Gridgraphen 1x2 (6 Knoten)
for (int i = 0; i < sqrt(n)-1; i+=1)
{
for (int j = 0; j < sqrt(n)-2; j+=2)
{
model.add(c[i * sqrt(n) + j] + c[i * sqrt(n) + j+2] +
c[(i+1) * sqrt(n) + j] + c[(i+1) * sqrt(n) + j+2] >= 30.283);
}
}
}
for (unsigned int v = 0; v < n; v++)
{
model.add(c[v] <= lambda);
model.add(c[v] >= 0);
}
std::cout << "Number of variables " << Info.numVar << "\n";
/* solve the Model*/
cplex.extract(model);
cplex.exportModel("L-Labeling.lp");
IloTimer timer(env);
timer.start();
int solveError = cplex.solve();
timer.stop();
if (!solveError)
{
std::cout << "STATUS : " << cplex.getStatus() << "\n";
env.error() << "Failed to optimize LP \n";
exit(1);
}
//Info.time = (double)(end-start)/(double)CLOCKS_PER_SEC;
Info.time = timer.getTime();
std::cout << "STATUS : " << cplex.getStatus() << "\n";
/* get the solution*/
env.out() << "Solution status = " << cplex.getStatus() << "\n";
Info.numConstr = cplex.getNrows();
env.out() << " Number of constraints = " << Info.numConstr << "\n";
lambda_G_d = cplex.getObjValue();
env.out() << "Solution value = " << lambda_G_d << "\n";
for (unsigned int u = 0; u < n; u++)
{
C.push_back(cplex.getValue(c[u]));
std::cout << "c(" << u << ")=" << C[u] << " ";
}
} // end try
catch (IloException& e)
{
std::cerr << "Concert exception caught: " << e << std::endl;
} catch (...)
{
std::cerr << "Unknown exception caught" << std::endl;
}
env.end();
return 0;
}
int CProblem::setModel()
{
//time_t start, end;
IloEnv env;
try
{
IloModel model(env);
IloCplex cplex(env);
/*Variables*/
IloNumVar lambda(env, "lambda");
IloNumVarArray c(env, n); //
for (unsigned int u = 0; u < n; u++)
{
std::stringstream ss;
ss << u;
std::string str = "c" + ss.str();
c[u] = IloNumVar(env, str.c_str());
}
IloArray<IloIntVarArray> z0(env,n);
for (unsigned int i=0; i<n; i++)
{
std::stringstream ss;
ss << i;
std::string str = "z0_" + ss.str();
z0[i]= IloIntVarArray(env, n, 0, 1);
}
/*
IloIntVarArray z0(env, Info.numAddVar);
for (int i = 0; i < Info.numAddVar; i++)
{
std::stringstream ss;
ss << i;
std::string str = "z0_" + ss.str();
z0[i] = IloIntVar(env, 0, 1, str.c_str());
}
*/
/* Objective*/
model.add(IloMinimize(env, lambda));
/*Constrains*/
/* d=function of the distance */
IloArray<IloNumArray> Par_d(env, n);
for (unsigned int u = 0; u < n; u++)
{
Par_d[u] = IloNumArray(env, n);
for (unsigned int v = 0; v < n; v++)
{
Par_d[u][v] = d[u][v];
}
}
int M = (max_distance + 1) * n;
/*
for (int i = 0; i < Info.numAddVar; i++)
{
int u = Info.ConstrIndex[i * 2];
int v = Info.ConstrIndex[i * 2 + 1];
model.add(c[u] - c[v] + M * (z0[i]) >= Par_d[u][v]);
model.add(c[v] - c[u] + M * (1 - z0[i]) >= Par_d[u][v]);
}
*/
for (unsigned u=0; u<n; u++)
{
for (unsigned v=0; v<u; v++)
{
if (Par_d[u][v] <= max_distance)
{
model.add(c[v]-c[u]+M*z0[u][v] >=Par_d[u][v]);
model.add(c[u]-c[v]+M*(1-z0[u][v]) >=Par_d[u][v]);
//Info.numvar++;
}
}
}
// d(x) = 3 - x
if (max_distance == 2)
{
// Sechsecke mit der Seitenlaenge 1
model.add(c[0]+c[2] +c[3] +c[5] +c[6] +c[9]>=16.6077);
model.add(c[1]+c[3] +c[4] +c[6] +c[7] +c[10]>=16.6077);
model.add(c[5]+c[8] +c[9] +c[12]+c[13]+c[16]>=16.6077);
model.add(c[6]+c[9] +c[10]+c[13]+c[14]+c[17]>=16.6077);
model.add(c[7]+c[10]+c[11]+c[14]+c[15]+c[18]>=16.6077);
model.add(c[13]+c[16]+c[17]+c[19]+c[20]+c[22]>=16.6077);
model.add(c[14]+c[17]+c[18]+c[20]+c[21]+c[23]>=16.6077);
}
//d(x) = 4 - x
if (max_distance == 3)
{
// Sechsecke mit der Seitenlaenge 1
model.add(c[0]+c[2] +c[3] +c[5] +c[6] +c[9]>=31.6077);
model.add(c[1]+c[3] +c[4] +c[6] +c[7] +c[10]>=31.6077);
model.add(c[5]+c[8] +c[9] +c[12]+c[13]+c[16]>=31.6077);
model.add(c[6]+c[9] +c[10]+c[13]+c[14]+c[17]>=31.6077);
model.add(c[7]+c[10]+c[11]+c[14]+c[15]+c[18]>=31.6077);
model.add(c[13]+c[16]+c[17]+c[19]+c[20]+c[22]>=31.6077);
model.add(c[14]+c[17]+c[18]+c[20]+c[21]+c[23]>=31.6077);
}
for (unsigned int v = 0; v < n; v++)
{
model.add(c[v] <= lambda);
model.add(c[v] >= 0);
}
std::cout << "Number of variables " << Info.numVar << "\n";
/* solve the Model*/
cplex.extract(model);
cplex.exportModel("L-Labeling.lp");
cplex.setParam(IloCplex::ClockType,1);
IloTimer timer(env);
timer.start();
int solveError = cplex.solve();
timer.stop();
if (!solveError)
{
std::cout << "STATUS : " << cplex.getStatus() << "\n";
env.error() << "Failed to optimize LP \n";
exit(1);
}
//Info.time = (double)(end-start)/(double)CLOCKS_PER_SEC;
Info.time = timer.getTime();
std::cout << "STATUS : " << cplex.getStatus() << "\n";
/* get the solution*/
env.out() << "Solution status = " << cplex.getStatus() << "\n";
Info.numConstr = cplex.getNrows();
env.out() << " Number of constraints = " << Info.numConstr << "\n";
lambda_G_d = cplex.getObjValue();
env.out() << "Solution value = " << lambda_G_d << "\n";
for (unsigned int u = 0; u < n; u++)
{
C.push_back(cplex.getValue(c[u]));
std::cout << "c(" << u << ")=" << C[u] << " ";
}
} // end try
catch (IloException& e)
{
std::cerr << "Concert exception caught: " << e << std::endl;
} catch (...)
{
std::cerr << "Unknown exception caught" << std::endl;
}
env.end();
return 0;
}
int main(int argc, char** argv)
{
std::cout << "%{\n";
// Parameters.
Opm::parameter::ParameterGroup param(argc, argv);
// Parser.
std::string ecl_file = param.get<std::string>("filename");
Opm::EclipseGridParser parser(ecl_file);
// Look at the BlackoilFluid behaviour
Opm::BlackoilFluid fluid;
fluid.init(parser);
Opm::BlackoilFluid::CompVec z0(0.0);
z0[Opm::BlackoilFluid::Water] = param.getDefault("z_w", 0.0);
z0[Opm::BlackoilFluid::Oil] = param.getDefault("z_o", 1.0);
z0[Opm::BlackoilFluid::Gas] = param.getDefault("z_g", 0.0);
int num_pts_p = param.getDefault("num_pts_p", 41);
int num_pts_z = param.getDefault("num_pts_z", 51);
double min_press = param.getDefault("min_press", 1e7);
double max_press = param.getDefault("max_press", 3e7);
int changing_component = param.getDefault("changing_component", int(Opm::BlackoilFluid::Gas));
double min_z = param.getDefault("min_z", 0.0);
double max_z = param.getDefault("max_z", 500.0);
int variable = param.getDefault("variable", 0);
Opm::BlackoilFluid::CompVec z = z0;
std::cout << "%}\n"
<< "data = [\n";
for (int i = 0; i < num_pts_p; ++i) {
double pfactor = num_pts_p < 2 ? 0.0 : double(i)/double(num_pts_p - 1);
double p = (1.0 - pfactor)*min_press + pfactor*max_press;
for (int j = 0; j < num_pts_z; ++j) {
double zfactor = num_pts_z < 2 ? 0.0 : double(j)/double(num_pts_z - 1);
z[changing_component] = (1.0 - zfactor)*min_z + zfactor*max_z;
// std::cout << p << ' ' << z << '\n';
Opm::BlackoilFluid::FluidState state = fluid.computeState(Opm::BlackoilFluid::PhaseVec(p), z);
std::cout.precision(6);
std::cout.width(15);
std::cout.fill(' ');
double var = 0.0;
switch (variable) {
case 0:
var = state.total_compressibility_;
break;
case 1:
var = state.experimental_term_;
break;
case 2:
var = state.saturation_[0];
break;
case 3:
var = state.saturation_[1];
break;
case 4:
var = state.saturation_[2];
break;
case 5:
var = state.formation_volume_factor_[0];
break;
case 6:
var = state.formation_volume_factor_[1];
break;
case 7:
var = state.formation_volume_factor_[2];
break;
case 8:
var = state.solution_factor_[0];
break;
case 9:
var = state.solution_factor_[1];
break;
case 10:
var = state.solution_factor_[2];
break;
default:
THROW("Unknown varable specification: " << variable);
break;
}
std::cout << var << ' ';
}
std::cout << '\n';
}
std::cout << "];\n\n"
<< "paxis = [\n";
for (int i = 0; i < num_pts_p; ++i) {
double pfactor = double(i)/double(num_pts_p - 1);
double p = (1.0 - pfactor)*min_press + pfactor*max_press;
std::cout << p << '\n';
}
std::cout << "];\n\n"
<< "zaxis = [\n";
for (int j = 0; j < num_pts_z; ++j) {
double zfactor = double(j)/double(num_pts_z - 1);
std::cout << (1.0 - zfactor)*min_z + zfactor*max_z << '\n';
}
std::cout << "];\n";
}
void POP06::runProblem()
{
// Problem 6 (Nataraj)
// 5 2-D B-splines
int dim = 4+2;
// x1,x2,x3,x4,l1,l2
std::vector<double> costs = {0, 0, 0, 0, 1, 0};
std::vector<double> lb = {1,0.625,47.5,90,-INF,-INF};
std::vector<double> ub = {1.1375,1,52.5,112,INF,INF};
std::vector<double> z0(dim,0);
// x1,x2,x3,x4,l1,l2,l3,l4,l5
// std::vector<double> lb = {1,0.625,47.5,90,-INF,-INF,-INF,-INF,-INF};
// std::vector<double> ub = {1.1375,1,52.5,112,INF,INF,INF,INF,INF};
std::vector<VariablePtr> vars;
for (int i = 0; i < dim; i++)
{
auto var = std::make_shared<Variable>(costs.at(i), lb.at(i), ub.at(i));
vars.push_back(var);
}
ConstraintSetPtr cs = std::make_shared<ConstraintSet>();
{ // obj = l1
std::vector<VariablePtr> cvars = {
vars.at(0),
vars.at(1),
vars.at(2),
vars.at(3),
vars.at(4)
};
std::vector<double> thislb = {
cvars.at(0)->getLowerBound(),
cvars.at(1)->getLowerBound(),
cvars.at(2)->getLowerBound(),
cvars.at(3)->getLowerBound()
};
std::vector<double> thisub = {
cvars.at(0)->getUpperBound(),
cvars.at(1)->getUpperBound(),
cvars.at(2)->getUpperBound(),
cvars.at(3)->getUpperBound()
};
std::vector<unsigned int> deg = {2,1,2,1};
// Poly coeffs
DenseVector c(4);
c.setZero();
c(0) = 0.6224;
c(1) = 1.7781;
c(2) = 3.1661;
c(3) = 19.84;
// Poly exponents
DenseMatrix E(4,4);
E.setZero();
E(0,2) = 1; E(0,3) = 1;
E(1,1) = 1; E(1,2) = 2;
E(2,0) = 2; E(2,3) = 1;
E(3,0) = 2; E(3,2) = 1;
DenseMatrix coeffs = getBSplineBasisCoefficients(c, E, thislb, thisub);
std::vector< std::vector<double> > knots = getRegularKnotVectors(deg, thislb, thisub);
BSpline bs(coeffs, knots, deg);
ConstraintPtr cbs = std::make_shared<ConstraintBSpline>(cvars, bs, true);
cs->add(cbs);
//DenseMatrix cpoints = bs.getControlPoints();
//cout << cpoints << endl;
}
{ // noncon = l2
std::vector<VariablePtr> cvars = {
vars.at(2),
vars.at(3),
vars.at(5)
};
std::vector<double> thislb = {
cvars.at(0)->getLowerBound(),
cvars.at(1)->getLowerBound()
};
std::vector<double> thisub = {
cvars.at(0)->getUpperBound(),
cvars.at(1)->getUpperBound()
};
std::vector<unsigned int> deg = {3,1};
// Poly coeffs
DenseVector c(2);
c.setZero();
c(0) = -1;
c(1) = -(4.0/3.0);
// Poly exponents
DenseMatrix E(2,2);
E.setZero();
E(0,0) = 2; E(0,1) = 1;
E(1,0) = 3;
DenseMatrix coeffs = getBSplineBasisCoefficients(c, E, thislb, thisub);
std::vector< std::vector<double> > knots = getRegularKnotVectors(deg, thislb, thisub);
BSpline bs(coeffs, knots, deg);
ConstraintPtr cbs = std::make_shared<ConstraintBSpline>(cvars, bs, true);
cs->add(cbs);
}
{ // Linear constraints of auxiliary variables
DenseMatrix A(4,6);
A.setZero();
A(0,0) = -1; A(0,2) = 0.0193;
A(1,1) = -1; A(1,2) = 0.00954;
A(2,5) = 1;
A(3,3) = 1;
DenseVector b;
b.setZero(4);
b(2) = -750.1728/3.14159265359;
b(3) = 240;
ConstraintPtr lincon = std::make_shared<ConstraintLinear>(vars, A, b, false);
cs->add(lincon);
}
BB::BranchAndBound bnb(cs);
Timer timer;
timer.start();
SolverResult res = bnb.optimize();
timer.stop();
cout << "Time: " << timer.getMilliSeconds() << " (ms)" << endl;
cout << "Time: " << timer.getMicroSeconds() << " (us)" << endl;
//1 0.625 47.5 90 6395.50783 -345958.333
if (res.status == SolverStatus::OPTIMAL)
fopt_found = res.objectiveValue;
}
inline void
internal::TrsvUN
( UnitOrNonUnit diag,
const DistMatrix<F,MC,MR>& U,
DistMatrix<F,MC,MR>& x )
{
#ifndef RELEASE
PushCallStack("internal::TrsvUN");
if( U.Grid() != x.Grid() )
throw std::logic_error("{U,x} must be distributed over the same grid");
if( U.Height() != U.Width() )
throw std::logic_error("U must be square");
if( x.Width() != 1 && x.Height() != 1 )
throw std::logic_error("x must be a vector");
const int xLength = ( x.Width() == 1 ? x.Height() : x.Width() );
if( U.Width() != xLength )
throw std::logic_error("Nonconformal TrsvUN");
#endif
const Grid& g = U.Grid();
if( x.Width() == 1 )
{
// Matrix views
DistMatrix<F,MC,MR>
UTL(g), UTR(g), U00(g), U01(g), U02(g),
UBL(g), UBR(g), U10(g), U11(g), U12(g),
U20(g), U21(g), U22(g);
DistMatrix<F,MC,MR>
xT(g), x0(g),
xB(g), x1(g),
x2(g);
// Temporary distributions
DistMatrix<F,STAR,STAR> U11_STAR_STAR(g);
DistMatrix<F,STAR,STAR> x1_STAR_STAR(g);
DistMatrix<F,MR, STAR> x1_MR_STAR(g);
DistMatrix<F,MC, STAR> z0_MC_STAR(g);
// Start the algorithm
LockedPartitionUpDiagonal
( U, UTL, UTR,
UBL, UBR, 0 );
PartitionUp
( x, xT,
xB, 0 );
while( xT.Height() > 0 )
{
LockedRepartitionUpDiagonal
( UTL, /**/ UTR, U00, U01, /**/ U02,
/**/ U10, U11, /**/ U12,
/*************/ /******************/
UBL, /**/ UBR, U20, U21, /**/ U22 );
RepartitionUp
( xT, x0,
x1,
/**/ /**/
xB, x2 );
x1_MR_STAR.AlignWith( U01 );
z0_MC_STAR.AlignWith( U01 );
z0_MC_STAR.ResizeTo( x0.Height(), 1 );
//----------------------------------------------------------------//
x1_STAR_STAR = x1;
U11_STAR_STAR = U11;
Trsv
( UPPER, NORMAL, diag,
U11_STAR_STAR.LockedLocalMatrix(),
x1_STAR_STAR.LocalMatrix() );
x1 = x1_STAR_STAR;
x1_MR_STAR = x1_STAR_STAR;
Gemv
( NORMAL, (F)-1,
U01.LockedLocalMatrix(),
x1_MR_STAR.LockedLocalMatrix(),
(F)0, z0_MC_STAR.LocalMatrix() );
x0.SumScatterUpdate( (F)1, z0_MC_STAR );
//----------------------------------------------------------------//
x1_MR_STAR.FreeAlignments();
z0_MC_STAR.FreeAlignments();
SlideLockedPartitionUpDiagonal
( UTL, /**/ UTR, U00, /**/ U01, U02,
/*************/ /******************/
/**/ U10, /**/ U11, U12,
UBL, /**/ UBR, U20, /**/ U21, U22 );
SlidePartitionUp
( xT, x0,
/**/ /**/
x1,
xB, x2 );
}
}
else
{
// Matrix views
DistMatrix<F,MC,MR>
UTL(g), UTR(g), U00(g), U01(g), U02(g),
UBL(g), UBR(g), U10(g), U11(g), U12(g),
U20(g), U21(g), U22(g);
DistMatrix<F,MC,MR>
xL(g), xR(g),
x0(g), x1(g), x2(g);
// Temporary distributions
DistMatrix<F,STAR,STAR> U11_STAR_STAR(g);
DistMatrix<F,STAR,STAR> x1_STAR_STAR(g);
DistMatrix<F,STAR,MR > x1_STAR_MR(g);
DistMatrix<F,STAR,MC > z0_STAR_MC(g);
DistMatrix<F,MR, MC > z0_MR_MC(g);
DistMatrix<F,MC, MR > z0(g);
// Start the algorithm
LockedPartitionUpDiagonal
( U, UTL, UTR,
UBL, UBR, 0 );
PartitionLeft( x, xL, xR, 0 );
while( xL.Width() > 0 )
{
LockedRepartitionUpDiagonal
( UTL, /**/ UTR, U00, U01, /**/ U02,
/**/ U10, U11, /**/ U12,
/*************/ /******************/
UBL, /**/ UBR, U20, U21, /**/ U22 );
RepartitionLeft
( xL, /**/ xR,
x0, x1, /**/ x2 );
x1_STAR_MR.AlignWith( U01 );
z0_STAR_MC.AlignWith( U01 );
z0.AlignWith( x0 );
z0_STAR_MC.ResizeTo( 1, x0.Width() );
//----------------------------------------------------------------//
x1_STAR_STAR = x1;
U11_STAR_STAR = U11;
Trsv
( UPPER, NORMAL, diag,
U11_STAR_STAR.LockedLocalMatrix(),
x1_STAR_STAR.LocalMatrix() );
x1 = x1_STAR_STAR;
x1_STAR_MR = x1_STAR_STAR;
Gemv
( NORMAL, (F)-1,
U01.LockedLocalMatrix(),
x1_STAR_MR.LockedLocalMatrix(),
(F)0, z0_STAR_MC.LocalMatrix() );
z0_MR_MC.SumScatterFrom( z0_STAR_MC );
z0 = z0_MR_MC;
Axpy( (F)1, z0, x0 );
//----------------------------------------------------------------//
x1_STAR_MR.FreeAlignments();
z0_STAR_MC.FreeAlignments();
z0.FreeAlignments();
SlideLockedPartitionUpDiagonal
( UTL, /**/ UTR, U00, /**/ U01, U02,
/*************/ /******************/
/**/ U10, /**/ U11, U12,
UBL, /**/ UBR, U20, /**/ U21, U22 );
SlidePartitionLeft
( xL, /**/ xR,
x0, /**/ x1, x2 );
}
}
#ifndef RELEASE
PopCallStack();
#endif
}
Localizer::Localizer(exchangeData *_commData, const unsigned int _period) :
RateThread(_period), commData(_commData), period(_period)
{
iCubHeadCenter eyeC(commData->head_version>1.0?"right_v2":"right");
eyeL=new iCubEye(commData->head_version>1.0?"left_v2":"left");
eyeR=new iCubEye(commData->head_version>1.0?"right_v2":"right");
// remove constraints on the links
// we use the chains for logging purpose
eyeL->setAllConstraints(false);
eyeR->setAllConstraints(false);
// release links
eyeL->releaseLink(0); eyeC.releaseLink(0); eyeR->releaseLink(0);
eyeL->releaseLink(1); eyeC.releaseLink(1); eyeR->releaseLink(1);
eyeL->releaseLink(2); eyeC.releaseLink(2); eyeR->releaseLink(2);
// add aligning matrices read from configuration file
getAlignHN(commData->rf_cameras,"ALIGN_KIN_LEFT",eyeL->asChain(),true);
getAlignHN(commData->rf_cameras,"ALIGN_KIN_RIGHT",eyeR->asChain(),true);
// overwrite aligning matrices iff specified through tweak values
if (commData->tweakOverwrite)
{
getAlignHN(commData->rf_tweak,"ALIGN_KIN_LEFT",eyeL->asChain(),true);
getAlignHN(commData->rf_tweak,"ALIGN_KIN_RIGHT",eyeR->asChain(),true);
}
// get the absolute reference frame of the head
Vector q(eyeC.getDOF(),0.0);
eyeCAbsFrame=eyeC.getH(q);
// ... and its inverse
invEyeCAbsFrame=SE3inv(eyeCAbsFrame);
// get the length of the half of the eyes baseline
eyesHalfBaseline=0.5*norm(eyeL->EndEffPose().subVector(0,2)-eyeR->EndEffPose().subVector(0,2));
bool ret;
// get camera projection matrix
ret=getCamPrj(commData->rf_cameras,"CAMERA_CALIBRATION_LEFT",&PrjL,true);
if (commData->tweakOverwrite)
{
Matrix *Prj;
if (getCamPrj(commData->rf_tweak,"CAMERA_CALIBRATION_LEFT",&Prj,true))
{
delete PrjL;
PrjL=Prj;
}
}
if (ret)
{
cxl=(*PrjL)(0,2);
cyl=(*PrjL)(1,2);
invPrjL=new Matrix(pinv(PrjL->transposed()).transposed());
}
else
PrjL=invPrjL=NULL;
// get camera projection matrix
ret=getCamPrj(commData->rf_cameras,"CAMERA_CALIBRATION_RIGHT",&PrjR,true);
if (commData->tweakOverwrite)
{
Matrix *Prj;
if (getCamPrj(commData->rf_tweak,"CAMERA_CALIBRATION_RIGHT",&Prj,true))
{
delete PrjR;
PrjR=Prj;
}
}
if (ret)
{
cxr=(*PrjR)(0,2);
cyr=(*PrjR)(1,2);
invPrjR=new Matrix(pinv(PrjR->transposed()).transposed());
}
else
PrjR=invPrjR=NULL;
Vector Kp(1,0.001), Ki(1,0.001), Kd(1,0.0);
Vector Wp(1,1.0), Wi(1,1.0), Wd(1,1.0);
Vector N(1,10.0), Tt(1,1.0);
Matrix satLim(1,2);
satLim(0,0)=0.05;
satLim(0,1)=10.0;
pid=new parallelPID(0.05,Kp,Ki,Kd,Wp,Wi,Wd,N,Tt,satLim);
Vector z0(1,0.5);
pid->reset(z0);
dominantEye="left";
port_xd=NULL;
}
void
karatsuba_rec (unit_vec_type & dest,
const_iterator const a_begin, const_iterator const a_end,
const_iterator const b_begin, const_iterator const b_end)
{
std::cout << "karatsuba_rec(" << print_range (a_begin, a_end) << ", "
<< print_range (b_begin, b_end) << ")" << std::endl;
unsigned const a_size = a_end - a_begin;
unsigned const b_size = b_end - b_begin;
std::cout << "a_size: " << a_size << " b_size: " << b_size << std::endl;
if (a_size == 1 && *a_begin == 0
|| b_size == 1 && *b_begin == 0)
{
dest = zero_value;
std::cout << "dest: " << print_vec (dest) << std::endl;
return;
}
else if (a_size == 1 && b_size == 1)
{
accum_type prod = static_cast<accum_type>(*a_begin) * *b_begin;
dest[0] = static_cast<unit_type>(prod % modulus);
unit_type high = static_cast<unit_type>(prod / modulus);
if (high != 0)
{
dest.resize (2);
dest[1] = high;
}
remove_leading_zeroes (dest);
std::cout << "dest: " << print_vec (dest) << std::endl;
return;
}
unsigned split_size;
if (a_size > b_size && b_size != 1)
split_size = b_size / 2;
else
split_size = a_size / 2;
std::cout << "split_size: " << split_size << std::endl;
const_iterator const & a0_begin = a_begin;
const_iterator const a1_begin = a_begin + split_size;
const_iterator const a0_end = a1_begin;
const_iterator const & a1_end = a_end;
unsigned a0_size = a0_end - a0_begin;
unsigned a1_size = a1_end - a1_begin;
const_iterator const & b0_begin = b_begin;
const_iterator const b1_begin = b_begin + split_size;
const_iterator const b0_end = b1_begin;
const_iterator const & b1_end = b_end;
unsigned b0_size = b0_end - b0_begin;
unsigned b1_size = b1_end - b1_begin;
// z2 = x1 * y1
unit_vec_type z2 (zero_value);
if (a1_size != 0 && b1_size != 0)
karatsuba_rec (z2, a1_begin, a1_end, b1_begin, b1_end);
// z0 = x0 * y0
unit_vec_type z0 (zero_value);
if (a0_size != 0 && b1_size != 0)
karatsuba_rec (z0, a0_begin, a0_end, b0_begin, b0_end);
// z1 = (x1 + x0)(y1 + y0) - (z2 + z0)
unit_vec_type z1, a01_sum, b01_sum, z02_sum;
resize_for_add (a01_sum, a0_begin, a0_end, a1_begin, a1_end);
add (a01_sum, a0_begin, a0_end, a1_begin, a1_end);
resize_for_add (b01_sum, b0_begin, b0_end, b1_begin, b1_end);
add (b01_sum, b0_begin, b0_end, b1_begin, b1_end);
add (z02_sum, z0, z2);
unit_vec_type z1_ab_prod;
resize_for_mul (z1_ab_prod, a01_sum.begin (), a01_sum.end (),
b01_sum.begin (), b01_sum.end ());
karatsuba_rec (z1_ab_prod, a01_sum.begin (), a01_sum.end (),
b01_sum.begin (), b01_sum.end ());
sub (z1, z1_ab_prod, z02_sum);
modulus_power (z2, split_size * 2);
modulus_power (z1, split_size);
dest = zero_value;
add (dest, dest, z2);
add (dest, dest, z1);
add (dest, dest, z0);
std::cout << "dest: " << print_vec (dest) << std::endl;
}
int main(int argc, char *argv[])
{
# include "setRootCase.H"
# include "createTime.H"
# include "createMesh.H"
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
// Read in the existing solution files.
Info << "Reading field U" << endl;
volVectorField U
(
IOobject
(
"U",
runTime.timeName(),
mesh,
IOobject::MUST_READ,
IOobject::NO_WRITE
),
mesh
);
Info << "Reading field T" << endl;
volScalarField T
(
IOobject
(
"T",
runTime.timeName(),
mesh,
IOobject::MUST_READ,
IOobject::NO_WRITE
),
mesh
);
Info << "Reading field p_rgh" << endl;
volScalarField p_rgh
(
IOobject
(
"p_rgh",
runTime.timeName(),
mesh,
IOobject::MUST_READ,
IOobject::NO_WRITE
),
mesh
);
// Compute the velocity flux at the faces. This is needed
// by the laminar transport model.
Info<< "Creating/Calculating face flux field, phi..." << endl;
surfaceScalarField phi
(
IOobject
(
"phi",
runTime.timeName(),
mesh,
IOobject::READ_IF_PRESENT,
IOobject::AUTO_WRITE
),
linearInterpolate(U) & mesh.Sf()
);
// Read the gravitational acceleration. This is needed
// for calculating dp/dn on boundaries.
Info << "Reading gravitational acceleration..." << endl;
uniformDimensionedVectorField g
(
IOobject
(
"g",
runTime.constant(),
mesh,
IOobject::MUST_READ,
IOobject::NO_WRITE
)
);
// Read the value of TRef in the transportProperties file.
singlePhaseTransportModel laminarTransport(U, phi);
dimensionedScalar TRef(laminarTransport.lookup("TRef"));
// Use Tref and the T field to compute rhok, which is needed
// to calculate dp/dn on boundaries.
Info<< "Creating the kinematic density field, rhok..." << endl;
volScalarField rhok
(
IOobject
(
"rhok",
runTime.timeName(),
mesh
),
1.0 - (T - TRef)/TRef
);
// Get access to the input dictionary.
IOdictionary setFieldsABLDict
(
IOobject
(
"setFieldsABLDict",
runTime.time().system(),
runTime,
IOobject::MUST_READ,
IOobject::NO_WRITE
)
);
// Read in the setFieldsABLDict entries.
word velocityInitType(setFieldsABLDict.lookup("velocityInitType"));
word temperatureInitType(setFieldsABLDict.lookup("temperatureInitType"));
word tableInterpTypeU(setFieldsABLDict.lookupOrDefault<word>("tableInterpTypeU","linear"));
word tableInterpTypeT(setFieldsABLDict.lookupOrDefault<word>("tableInterpTypeT","linear"));
scalar deltaU(setFieldsABLDict.lookupOrDefault<scalar>("deltaU",1.0));
scalar deltaV(setFieldsABLDict.lookupOrDefault<scalar>("deltaV",1.0));
scalar zPeak(setFieldsABLDict.lookupOrDefault<scalar>("zPeak",0.03));
scalar Uperiods(setFieldsABLDict.lookupOrDefault<scalar>("Uperiods",4));
scalar Vperiods(setFieldsABLDict.lookupOrDefault<scalar>("Vperiods",4));
scalar xMin(setFieldsABLDict.lookupOrDefault<scalar>("xMin",0.0));
scalar yMin(setFieldsABLDict.lookupOrDefault<scalar>("yMin",0.0));
scalar zMin(setFieldsABLDict.lookupOrDefault<scalar>("zMin",0.0));
scalar xMax(setFieldsABLDict.lookupOrDefault<scalar>("xMax",3000.0));
scalar yMax(setFieldsABLDict.lookupOrDefault<scalar>("yMax",3000.0));
scalar zMax(setFieldsABLDict.lookupOrDefault<scalar>("zMax",1000.0));
scalar zRef(setFieldsABLDict.lookupOrDefault<scalar>("zRef",600.0));
bool useWallDistZ(setFieldsABLDict.lookupOrDefault<bool>("useWallDistZ",false));
bool scaleVelocityWithHeight(setFieldsABLDict.lookupOrDefault<bool>("scaleVelocityWithHeight",false));
scalar zInversion(setFieldsABLDict.lookupOrDefault<scalar>("zInversion",600.0));
scalar Ug(setFieldsABLDict.lookupOrDefault<scalar>("Ug",15.0));
scalar UgDir(setFieldsABLDict.lookupOrDefault<scalar>("UgDir",270.0));
scalar Tbottom(setFieldsABLDict.lookupOrDefault<scalar>("Tbottom",300.0));
scalar Ttop(setFieldsABLDict.lookupOrDefault<scalar>("Ttop",304.0));
scalar dTdz(setFieldsABLDict.lookupOrDefault<scalar>("dTdz",0.003));
scalar widthInversion(setFieldsABLDict.lookupOrDefault<scalar>("widthInversion",80.0));
scalar TPrimeScale(setFieldsABLDict.lookupOrDefault<scalar>("TPrimeScale",0.0));
scalar z0(setFieldsABLDict.lookupOrDefault<scalar>("z0",0.016));
scalar kappa(setFieldsABLDict.lookupOrDefault<scalar>("kappa",0.40));
List<List<scalar> > profileTable(setFieldsABLDict.lookup("profileTable"));
bool updateInternalFields(setFieldsABLDict.lookupOrDefault<bool>("updateInternalFields",true));
bool updateBoundaryFields(setFieldsABLDict.lookupOrDefault<bool>("updateBoundaryFields",true));
// Change the table profiles from scalar lists to scalar fields
scalarField zProfile(profileTable.size(),0.0);
scalarField UProfile(profileTable.size(),0.0);
scalarField VProfile(profileTable.size(),0.0);
scalarField TProfile(profileTable.size(),0.0);
forAll(zProfile,i)
{
zProfile[i] = profileTable[i][0];
UProfile[i] = profileTable[i][1];
VProfile[i] = profileTable[i][2];
TProfile[i] = profileTable[i][3];
}
void POP03::runProblem()
{
// Problem 3 (Nataraj)
// cout << "\n\nSolving problem P03..." << endl;
int dim = 4;
// x1,x2,l1,l2
std::vector<double> costs = {1, 0, 0, 0};
std::vector<double> lb = {-10,-10,0,-1000};
std::vector<double> ub = {10,10,100,1000};
std::vector<double> z0(dim,0);
std::vector<VariablePtr> vars;
for (int i = 0; i < dim; i++)
{
auto var = std::make_shared<Variable>(costs.at(i), lb.at(i), ub.at(i));
vars.push_back(var);
}
ConstraintSetPtr cs = std::make_shared<ConstraintSet>();
{ // x1^2 = l1
VariablePtr var = vars.at(0);
std::vector<double> thislb = {var->getLowerBound()};
std::vector<double> thisub = {var->getUpperBound()};
std::vector<unsigned int> deg = {2};
DenseVector c(3);
c.setZero();
c(0) = 0;
c(1) = 0;
c(2) = 1;
DenseMatrix T = getTransformationMatrix(deg, thislb, thisub);
DenseMatrix coeffs = T*c;
std::vector< std::vector<double> > knots = getRegularKnotVectors(deg, thislb, thisub);
BSpline bs(coeffs.transpose(), knots, deg);
std::vector<VariablePtr> cvars = {var, vars.at(2)};
ConstraintPtr cbs = std::make_shared<ConstraintBSpline>(cvars, bs, true);
cs->add(cbs);
}
{ // x1^3 = l1
VariablePtr var = vars.at(0);
std::vector<double> thislb = {var->getLowerBound()};
std::vector<double> thisub = {var->getUpperBound()};
std::vector<unsigned int> deg = {3};
DenseVector c(4); c.setZero();
c(0) = 0;
c(1) = 0;
c(2) = 0;
c(3) = 1;
DenseMatrix T = getTransformationMatrix(deg, thislb, thisub);
DenseMatrix coeffs = T*c;
std::vector< std::vector<double> > knots = getRegularKnotVectors(deg, thislb, thisub);
BSpline bs(coeffs.transpose(), knots, deg);
std::vector<VariablePtr> cvars = {var, vars.at(3)};
ConstraintPtr cbs = std::make_shared<ConstraintBSpline>(cvars, bs, true);
cs->add(cbs);
}
{ // x2 - l1 <= 0, x2 - l2 <= 0
DenseMatrix A(2,3);
A.setZero();
A(0,0) = -1; A(0,1) = 1;
A(1,0) = 1; A(1,1) = 2; A(1,2) = -1;
DenseVector b; b.setZero(2); b(1) = -1e-5;
std::vector<VariablePtr> cvars = {
vars.at(1),
vars.at(2),
vars.at(3)
};
ConstraintPtr lincon = std::make_shared<ConstraintLinear>(cvars, A, b, false);
cs->add(lincon);
}
BB::BranchAndBound bnb(cs);
Timer timer;
timer.start();
SolverResult res = bnb.optimize();
timer.stop();
cout << "Time: " << timer.getMilliSeconds() << " (ms)" << endl;
if (res.status == SolverStatus::OPTIMAL)
fopt_found = res.objectiveValue;
}
inline void
internal::TrsvLT
( Orientation orientation,
UnitOrNonUnit diag,
const DistMatrix<F,MC,MR>& L,
DistMatrix<F,MC,MR>& x )
{
#ifndef RELEASE
PushCallStack("internal::TrsvLT");
if( L.Grid() != x.Grid() )
throw std::logic_error("{L,x} must be distributed over the same grid");
if( orientation == NORMAL )
throw std::logic_error("TrsvLT expects a (conjugate-)transpose option");
if( L.Height() != L.Width() )
throw std::logic_error("L must be square");
if( x.Width() != 1 && x.Height() != 1 )
throw std::logic_error("x must be a vector");
const int xLength = ( x.Width() == 1 ? x.Height() : x.Width() );
if( L.Width() != xLength )
throw std::logic_error("Nonconformal TrsvLT");
#endif
const Grid& g = L.Grid();
if( x.Width() == 1 )
{
// Matrix views
DistMatrix<F,MC,MR>
LTL(g), LTR(g), L00(g), L01(g), L02(g),
LBL(g), LBR(g), L10(g), L11(g), L12(g),
L20(g), L21(g), L22(g);
DistMatrix<F,MC,MR>
xT(g), x0(g),
xB(g), x1(g),
x2(g);
// Temporary distributions
DistMatrix<F,STAR,STAR> L11_STAR_STAR(g);
DistMatrix<F,STAR,STAR> x1_STAR_STAR(g);
DistMatrix<F,MC, STAR> x1_MC_STAR(g);
DistMatrix<F,MR, STAR> z0_MR_STAR(g);
DistMatrix<F,MR, MC > z0_MR_MC(g);
DistMatrix<F,MC, MR > z0(g);
// Start the algorithm
LockedPartitionUpDiagonal
( L, LTL, LTR,
LBL, LBR, 0 );
PartitionUp
( x, xT,
xB, 0 );
while( xT.Height() > 0 )
{
LockedRepartitionUpDiagonal
( LTL, /**/ LTR, L00, L01, /**/ L02,
/**/ L10, L11, /**/ L12,
/*************/ /******************/
LBL, /**/ LBR, L20, L21, /**/ L22 );
RepartitionUp
( xT, x0,
x1,
/**/ /**/
xB, x2 );
x1_MC_STAR.AlignWith( L10 );
z0_MR_STAR.AlignWith( L10 );
z0_MR_STAR.ResizeTo( x0.Height(), 1 );
z0.AlignWith( x0 );
//----------------------------------------------------------------//
x1_STAR_STAR = x1;
L11_STAR_STAR = L11;
Trsv
( LOWER, orientation, diag,
L11_STAR_STAR.LockedLocalMatrix(),
x1_STAR_STAR.LocalMatrix() );
x1 = x1_STAR_STAR;
x1_MC_STAR = x1_STAR_STAR;
Gemv
( orientation, (F)-1,
L10.LockedLocalMatrix(),
x1_MC_STAR.LockedLocalMatrix(),
(F)0, z0_MR_STAR.LocalMatrix() );
z0_MR_MC.SumScatterFrom( z0_MR_STAR );
z0 = z0_MR_MC;
Axpy( (F)1, z0, x0 );
//----------------------------------------------------------------//
x1_MC_STAR.FreeAlignments();
z0_MR_STAR.FreeAlignments();
z0.FreeAlignments();
SlideLockedPartitionUpDiagonal
( LTL, /**/ LTR, L00, /**/ L01, L02,
/*************/ /******************/
/**/ L10, /**/ L11, L12,
LBL, /**/ LBR, L20, /**/ L21, L22 );
SlidePartitionUp
( xT, x0,
/**/ /**/
x1,
xB, x2 );
}
}
else
{
// Matrix views
DistMatrix<F,MC,MR>
LTL(g), LTR(g), L00(g), L01(g), L02(g),
LBL(g), LBR(g), L10(g), L11(g), L12(g),
L20(g), L21(g), L22(g);
DistMatrix<F,MC,MR>
xL(g), xR(g),
x0(g), x1(g), x2(g);
// Temporary distributions
DistMatrix<F,STAR,STAR> L11_STAR_STAR(g);
DistMatrix<F,STAR,STAR> x1_STAR_STAR(g);
DistMatrix<F,STAR,MC > x1_STAR_MC(g);
DistMatrix<F,STAR,MR > z0_STAR_MR(g);
// Start the algorithm
LockedPartitionUpDiagonal
( L, LTL, LTR,
LBL, LBR, 0 );
PartitionLeft( x, xL, xR, 0 );
while( xL.Width() > 0 )
{
LockedRepartitionUpDiagonal
( LTL, /**/ LTR, L00, L01, /**/ L02,
/**/ L10, L11, /**/ L12,
/*************/ /******************/
LBL, /**/ LBR, L20, L21, /**/ L22 );
RepartitionLeft
( xL, /**/ xR,
x0, x1, /**/ x2 );
x1_STAR_MC.AlignWith( L10 );
z0_STAR_MR.AlignWith( L10 );
//----------------------------------------------------------------//
x1_STAR_STAR = x1;
L11_STAR_STAR = L11;
Trsv
( LOWER, orientation, diag,
L11_STAR_STAR.LockedLocalMatrix(),
x1_STAR_STAR.LocalMatrix() );
x1 = x1_STAR_STAR;
x1_STAR_MC = x1_STAR_STAR;
Gemv
( orientation, (F)-1,
L10.LockedLocalMatrix(),
x1_STAR_MC.LockedLocalMatrix(),
(F)0, z0_STAR_MR.LocalMatrix() );
x0.SumScatterUpdate( (F)1, z0_STAR_MR );
//----------------------------------------------------------------//
x1_STAR_MC.FreeAlignments();
z0_STAR_MR.FreeAlignments();
SlideLockedPartitionUpDiagonal
( LTL, /**/ LTR, L00, /**/ L01, L02,
/*************/ /******************/
/**/ L10, /**/ L11, L12,
LBL, /**/ LBR, L20, /**/ L21, L22 );
SlidePartitionLeft
( xL, /**/ xR,
x0, /**/ x1, x2 );
}
}
#ifndef RELEASE
PopCallStack();
#endif
}
float CIKJoint::getValueOverLimitation(bool deactivate)
{
if (!active)
{ // The case (!active)&&spherical&&topSpherical&&(!sphericalUp) should not return here!
if (!spherical)
return(-1.0f); // No limitation violation
if (!topSpherical)
return(-1.0f);
if (sphericalUp)
return(-1.0f);
}
if (spherical)
{
if (!topSpherical)
return(-1.0f);
if (range>179.9f*degToRad)
return(-1.0f);
if (sphericalUp)
{ // Bottom-up
if (!parent->active)
return(-1.0f); // Was already deactivated
C4Vector rotYm90(C3Vector(0.0f,-piValue/2.0f,0.0f));
C4Vector tr0(C3Vector(parent->parent->tempParameter,parent->tempParameter,0.0f));
C4Vector tr(C3Vector(parent->parent->tempParameter+parent->parent->probableDeltaValue,parent->tempParameter+parent->probableDeltaValue,0.0f));
C4Vector sphericalTr(parent->parent->transformation.Q*rotYm90);
tr0=sphericalTr*tr0;
tr=sphericalTr*tr;
C3X3Matrix m0(tr0.getMatrix());
C3X3Matrix m(tr.getMatrix());
C3Vector z0(m0(0,2),m0(1,2),m0(2,2));
C3Vector z(m(0,2),m(1,2),m(2,2));
C3Vector zVertical(0.0f,0.0f,1.0f);
C4Vector zVerticalToZ0(zVertical,z0);
C4Vector zVerticalToZ(zVertical,z);
C4Vector angleAndAxis0(zVerticalToZ0.getAngleAndAxis());
C4Vector angleAndAxis(zVerticalToZ.getAngleAndAxis());
float p=angleAndAxis(0);
float deltaV=p-angleAndAxis0(0);
float tolerance=range*0.0001f;
bool respected=true;
if (p>range+tolerance)
{
p=range;
respected=false;
}
if (!respected)
{
float retVal=fabs((p-angleAndAxis0(0)+deltaV)/deltaV);
if (deactivate)
{
tr.setAngleAndAxis(p,C3Vector(angleAndAxis(1),angleAndAxis(2),angleAndAxis(3)));
tr=sphericalTr.getInverse()*tr;
C3Vector euler(tr.getEulerAngles());
parent->tempParameter=euler(1);
parent->active=false;
parent->copyStateToAvatarKids();
parent->parent->tempParameter=euler(0);
parent->parent->active=false;
parent->parent->copyStateToAvatarKids();
parent->parent->parent->active=true;
parent->parent->parent->copyStateToAvatarKids();
}
return(retVal);
}
}
else
{ // Top-down
if (active)
return(-1.0f); // Was already deactivated
C4Vector rotY180(C3Vector(0.0f,piValue,0.0f));
C4Vector rotXm90(C3Vector(-piValue/2.0f,0.0f,0.0f));
C4Vector rotZ90(C3Vector(0.0f,0.0f,piValue/2.0f));
C4Vector tr0(C3Vector(parent->tempParameter,parent->parent->tempParameter,0.0f));
C4Vector tr(C3Vector(parent->tempParameter+parent->probableDeltaValue,parent->parent->tempParameter+parent->parent->probableDeltaValue,0.0f));
C4Vector sphericalTr((rotZ90*rotXm90*transformation.Q*rotY180).getInverse());
tr0=sphericalTr*tr0;
tr=sphericalTr*tr;
C3X3Matrix m0(tr0.getMatrix());
C3X3Matrix m(tr.getMatrix());
C3Vector z0(m0(0,2),m0(1,2),m0(2,2));
C3Vector z(m(0,2),m(1,2),m(2,2));
C3Vector zVertical(0.0f,0.0f,1.0f);
C4Vector zVerticalToZ0(zVertical,z0);
C4Vector zVerticalToZ(zVertical,z);
C4Vector angleAndAxis0(zVerticalToZ0.getAngleAndAxis());
C4Vector angleAndAxis(zVerticalToZ.getAngleAndAxis());
float p=angleAndAxis(0);
float deltaV=p-angleAndAxis0(0);
float tolerance=range*0.0001f;
bool respected=true;
if (p>range+tolerance)
{
p=range;
respected=false;
}
if (!respected)
{
float retVal=fabs((p-angleAndAxis0(0)+deltaV)/deltaV);
if (deactivate)
{
tr.setAngleAndAxis(p,C3Vector(angleAndAxis(1),angleAndAxis(2),angleAndAxis(3)));
tr=sphericalTr.getInverse()*tr;
C3Vector euler(tr.getEulerAngles());
active=true;
copyStateToAvatarKids();
parent->tempParameter=euler(0);
parent->active=false;
parent->copyStateToAvatarKids();
parent->parent->tempParameter=euler(1);
parent->parent->active=false;
parent->parent->copyStateToAvatarKids();
}
return(retVal);
}
}
return(-1.0f);
}
else
{
float p=tempParameter+probableDeltaValue;
float tolerance=range*0.0001f;
bool respected=true;
if (revolute)
{
if ( (range<359.9f*degToRad)&&(!cyclic) )
{
if (p<minValue-tolerance)
{
p=minValue;
respected=false;
}
else if (p>minValue+range+tolerance)
{
p=minValue+range;
respected=false;
}
}
}
else
{
if (p<minValue-tolerance)
{
p=minValue;
respected=false;
}
else if (p>minValue+range+tolerance)
{
p=minValue+range;
respected=false;
}
}
if (!respected)
{
float retVal=fabs((p-tempParameter+probableDeltaValue)/probableDeltaValue);
if (deactivate)
{
tempParameter=p;
active=false;
copyStateToAvatarKids();
}
return(retVal);
}
return(-1.0f); // Respected
}
}
void Robot::dqp_torque(const ColumnVector & q, const ColumnVector & qp,
const ColumnVector & dqp,
ColumnVector & ltorque, ColumnVector & dtorque)
{
int i;
ColumnVector z0(3);
Matrix Rt, temp;
Matrix Q(3,3);
ColumnVector *w, *wp, *vp, *a, *f, *n, *F, *N, *p;
ColumnVector *dw, *dwp, *dvp, *da, *df, *dn, *dF, *dN, *dp;
if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension");
if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension");
ltorque = ColumnVector(dof);
dtorque = ColumnVector(dof);
set_q(q);
w = new ColumnVector[dof+1];
wp = new ColumnVector[dof+1];
vp = new ColumnVector[dof+1];
a = new ColumnVector[dof+1];
f = new ColumnVector[dof+1];
n = new ColumnVector[dof+1];
F = new ColumnVector[dof+1];
N = new ColumnVector[dof+1];
p = new ColumnVector[dof+1];
dw = new ColumnVector[dof+1];
dwp = new ColumnVector[dof+1];
dvp = new ColumnVector[dof+1];
da = new ColumnVector[dof+1];
df = new ColumnVector[dof+1];
dn = new ColumnVector[dof+1];
dF = new ColumnVector[dof+1];
dN = new ColumnVector[dof+1];
dp = new ColumnVector[dof+1];
w[0] = ColumnVector(3);
wp[0] = ColumnVector(3);
vp[0] = gravity;
dw[0] = ColumnVector(3);
dwp[0] = ColumnVector(3);
dvp[0] = ColumnVector(3);
z0 = 0.0;
Q = 0.0;
Q(1,2) = -1.0;
Q(2,1) = 1.0;
z0(3) = 1.0;
w[0] = 0.0;
wp[0] = 0.0;
dw[0] = 0.0;
dwp[0] = 0.0;
dvp[0] = 0.0;
for(i = 1; i <= dof; i++) {
Rt = links[i].R.t();
p[i] = ColumnVector(3);
p[i](1) = links[i].get_a();
p[i](2) = links[i].get_d() * Rt(2,3);
p[i](3) = links[i].get_d() * Rt(3,3);
if(links[i].get_joint_type() != 0) {
dp[i] = ColumnVector(3);
dp[i](1) = 0.0;
dp[i](2) = Rt(2,3);
dp[i](3) = Rt(3,3);
}
if(links[i].get_joint_type() == 0) {
w[i] = Rt*(w[i-1] + z0*qp(i));
dw[i] = Rt*(dw[i-1] + z0*dqp(i));
wp[i] = Rt*(wp[i-1] + vec_x_prod(w[i-1],z0*qp(i)));
dwp[i] = Rt*(dwp[i-1]
+ vec_x_prod(dw[i-1],z0*qp(i))
+ vec_x_prod(w[i-1],z0*dqp(i))
);
vp[i] = vec_x_prod(wp[i],p[i])
+ vec_x_prod(w[i],vec_x_prod(w[i],p[i]))
+ Rt*(vp[i-1]);
dvp[i] = vec_x_prod(dwp[i],p[i])
+ vec_x_prod(dw[i],vec_x_prod(w[i],p[i]))
+ vec_x_prod(w[i],vec_x_prod(dw[i],p[i]))
+ Rt*dvp[i-1];
} else {
w[i] = Rt*w[i-1];
dw[i] = Rt*dw[i-1];
wp[i] = Rt*wp[i-1];
dwp[i] = Rt*dwp[i-1];
vp[i] = Rt*(vp[i-1]
+ vec_x_prod(w[i],z0*qp(i))) * 2.0
+ vec_x_prod(wp[i],p[i])
+ vec_x_prod(w[i],vec_x_prod(w[i],p[i]));
dvp[i] = Rt*(dvp[i-1]
+ (vec_x_prod(dw[i],z0*qp(i)) * 2.0
+ vec_x_prod(w[i],z0*dqp(i))))
+ vec_x_prod(dwp[i],p[i])
+ vec_x_prod(dw[i],vec_x_prod(w[i],p[i]))
+ vec_x_prod(w[i],vec_x_prod(dw[i],p[i]));
}
a[i] = vec_x_prod(wp[i],links[i].r)
+ vec_x_prod(w[i],vec_x_prod(w[i],links[i].r))
+ vp[i];
da[i] = vec_x_prod(dwp[i],links[i].r)
+ vec_x_prod(dw[i],vec_x_prod(w[i],links[i].r))
+ vec_x_prod(w[i],vec_x_prod(dw[i],links[i].r))
+ dvp[i];
}
for(i = dof; i >= 1; i--) {
F[i] = a[i] * links[i].m;
N[i] = links[i].I*wp[i] + vec_x_prod(w[i],links[i].I*w[i]);
dF[i] = da[i] * links[i].m;
dN[i] = links[i].I*dwp[i] + vec_x_prod(dw[i],links[i].I*w[i])
+ vec_x_prod(w[i],links[i].I*dw[i]);
if(i == dof) {
f[i] = F[i];
n[i] = vec_x_prod(p[i],f[i])
+ vec_x_prod(links[i].r,F[i]) + N[i];
df[i] = dF[i];
dn[i] = vec_x_prod(p[i],df[i])
+ vec_x_prod(links[i].r,dF[i]) + dN[i];
} else {
f[i] = links[i+1].R*f[i+1] + F[i];
n[i] = links[i+1].R*n[i+1] + vec_x_prod(p[i],f[i])
+ vec_x_prod(links[i].r,F[i]) + N[i];
df[i] = links[i+1].R*df[i+1] + dF[i];
dn[i] = links[i+1].R*dn[i+1] + vec_x_prod(p[i],df[i])
+ vec_x_prod(links[i].r,dF[i]) + dN[i];
}
if(links[i].get_joint_type() == 0) {
temp = ((z0.t()*links[i].R)*n[i]);
ltorque(i) = temp(1,1);
temp = ((z0.t()*links[i].R)*dn[i]);
dtorque(i) = temp(1,1);
} else {
temp = ((z0.t()*links[i].R)*f[i]);
ltorque(i) = temp(1,1);
temp = ((z0.t()*links[i].R)*df[i]);
dtorque(i) = temp(1,1);
}
}
delete []dp;
delete []dN;
delete []dF;
delete []dn;
delete []df;
delete []da;
delete []dvp;
delete []dwp;
delete []dw;
delete []p;
delete []N;
delete []F;
delete []n;
delete []f;
delete []a;
delete []vp;
delete []wp;
delete []w;
}
