void Write_A(void)
{
//////////////A//////////////////////
u8 cola_A[]={1,0,0,0,0,0,0,0};
u8 rowa_A[]={0,0,0,1,1,1,1,1};
u8 colb_A[]={0,1,0,0,0,0,0,0};
u8 rowb_A[]={0,0,1,0,0,0,0,0};
u8 colc_A[]={0,0,1,0,0,0,0,0};
u8 rowc_A[]={0,1,0,0,0,0,0,0};
u8 cold_A[]={0,0,0,1,0,0,0,0};
u8 rowd_A[]={1,0,0,0,0,0,0,0};
u8 cole_A[]={0,0,0,0,1,0,0,0};
u8 rowe_A[]={0,1,0,0,0,0,0,0};
u8 colf_A[]={0,0,0,0,0,1,0,0};
u8 rowf_A[]={0,0,1,0,0,0,0,0};
u8 colg_A[]={0,0,0,0,0,0,1,0};
u8 rowg_A[]={0,0,0,1,1,1,1,1};
u8 colh_A[]={1,1,1,1,1,1,1,0};
u8 rowh_A[]={0,0,0,0,0,1,0,0};
u8 x = 30 ;
while(x)
{
cols(cola_A);
rows(rowa_A);
DIO_u8WritePinVal(latch,1);
DELAY(0.001);
DIO_u8WritePinVal(latch,0);
cols(colb_A);
rows(rowb_A);
DIO_u8WritePinVal(latch,1);
DELAY(0.001);
DIO_u8WritePinVal(latch,0);
DELAY(0.001);
cols(colc_A);
rows(rowc_A);
DIO_u8WritePinVal(latch,1);
DELAY(0.001);
DIO_u8WritePinVal(latch,0);
cols(cold_A);
rows(rowd_A);
DIO_u8WritePinVal(latch,1);
DELAY(0.001);
DIO_u8WritePinVal(latch,0);
cols(cole_A);
rows(rowe_A);
DIO_u8WritePinVal(latch,1);
DELAY(0.001);
DIO_u8WritePinVal(latch,0);
cols(colf_A);
rows(rowf_A);
DIO_u8WritePinVal(latch,1);
DELAY(0.001);
DIO_u8WritePinVal(latch,0);
cols(colg_A);
rows(rowg_A);
DIO_u8WritePinVal(latch,1);
DELAY(0.001);
DIO_u8WritePinVal(latch,0);
cols(colh_A);
rows(rowh_A);
DIO_u8WritePinVal(latch,1);
DELAY(0.001);
DIO_u8WritePinVal(latch,0);
x-- ;
}
}
/*!
\since 4.5
Appends \a count rows at the bottom of the table.
\sa insertColumns() insertRows() resize() removeRows() removeColumns() appendColumns()
*/
void QTextTable::appendRows(int count)
{
insertRows(rows(), count);
}
void vector3N::assign(const vector3N& other)
{
setSize(other.rows());
for(int i=0; i<rows(); i++)
row(i)=other.row(i);
}
Matrix Matrix::inverseGaussianElimination (int significantDigits,
MatrixConsistent &matrixConsistent) const
{
// From https://en.wikipedia.org/wiki/Gaussian_elimination
int row, col, rowFrom, rowTo;
Matrix inv (rows ());
// Track the row switches that may or may not be performed below
QVector<int> rowIndexes (rows ());
for (row = 0; row < rows (); row++) {
rowIndexes [row] = row;
}
// Create the working matrix and populate the left half. Number of columns in the working matrix is twice the number
// of cols in this matrix, but we will not populate the right half until after the bubble sort below
Matrix working (rows (), 2 * cols ());
for (row = 0; row < rows (); row++) {
for (col = 0; col < cols (); col++) {
double value = get (row, col);
working.set (row, col, value);
}
}
// Simple bubble sort to rearrange the rows with 0 leading zeros at start, followed by rows with 1 leading zero at start, ...
for (int row1 = 0; row1 < rows (); row1++) {
for (int row2 = row1 + 1; row2 < rows (); row2++) {
if (working.leadingZeros (row1) > working.leadingZeros (row2)) {
working.switchRows (row1, row2);
// Remember this switch
int row1Index = rowIndexes [row1];
int row2Index = rowIndexes [row2];
rowIndexes [row1] = row2Index;
rowIndexes [row2] = row1Index;
}
}
}
// Populate the right half now
for (row = 0; row < rows (); row++) {
for (col = cols (); col < 2 * cols (); col++) {
int colIdentitySubmatrix = col - cols ();
double value = (row == colIdentitySubmatrix) ? 1 : 0;
working.set (row, col, value);
}
}
// Loop through the "from" row going down. This results in the lower off-diagonal terms becoming zero, in the left half
for (rowFrom = 0; rowFrom < rows (); rowFrom++) {
int colFirstWithNonZero = rowFrom;
// In pathological situations we have (rowFrom, colFirstWithNonzero) = 0 in which case the solution cannot be obtained
// so we exit
if (working.get (rowFrom, colFirstWithNonZero) == 0) {
matrixConsistent = MATRIX_INCONSISTENT;
return inv;
}
// Normalize the 'from' row with first nonzero term set to 1
working.normalizeRow (rowFrom, colFirstWithNonZero, significantDigits, matrixConsistent);
if (matrixConsistent == MATRIX_INCONSISTENT) {
return inv;
}
// Apply the 'from' row to all the 'to' rows
for (rowTo = rowFrom + 1; rowTo < rows (); rowTo++) {
if (working.get (rowTo, colFirstWithNonZero) != 0) {
// We need to merge rowFrom and rowTo into rowTo
double denominator = working.get (rowFrom, colFirstWithNonZero);
double factor = -1.0 * working.get (rowTo, colFirstWithNonZero) / denominator;
working.addRowToAnotherWithScaling (rowFrom, rowTo, factor);
}
}
}
// Loop through the "from" row doing up. This results in the upper off-diagonal terms becoming zero, in the left half
for (rowFrom = rows () - 1; rowFrom >= 0; rowFrom--) {
int colFirstWithNonZero = rowFrom; // This is true since we should have 1s all down the diagonal at this point
// Normalize the 'from' row with diagonal term set to 1. The first term should be like 0.9999 or 1.0001 but we want exactly one
MatrixConsistent matrixConsistent;
working.normalizeRow (rowFrom, colFirstWithNonZero, significantDigits, matrixConsistent);
if (matrixConsistent == MATRIX_INCONSISTENT) {
return inv;
}
// Apply the 'from' row to all the 'to' rows
for (rowTo = rowFrom - 1; rowTo >= 0; rowTo--) {
if (working.get (rowTo, colFirstWithNonZero) != 0) {
// We need to merge rowFro and rowTo into rowTo
double denominator = working.get (rowFrom, colFirstWithNonZero);
double factor = -1.0 * working.get (rowTo, colFirstWithNonZero) / denominator;
working.addRowToAnotherWithScaling (rowFrom, rowTo, factor);
}
}
}
// Extract right half of rectangular matrix which is the inverse
for (row = 0; row < working.rows (); row++) {
int rowBeforeSort = rowIndexes [row];
for (col = 0; col < working.rows (); col++) {
int colRightHalf = col + inv.cols ();
double value = working.get (rowBeforeSort, colRightHalf);
inv.set (row, col, value);
}
}
return inv;
}
int MSLayout::idealHeight(void) const
{
int offset=highlightThickness()+shadowThickness()+margin();
return (vectorHeight()+innerHeight()+(rows()-1)*rowSpacing()+2*offset);
}
void GSparseMatrix::singularValueDecompositionHelper(GSparseMatrix** ppU, double** ppDiag, GSparseMatrix** ppV, int maxIters)
{
int m = rows();
int n = cols();
if(m < n)
ThrowError("Expected at least as many rows as columns");
int i, j, k;
int l = 0;
int q, iter;
double c, f, h, s, x, y, z;
double norm = 0.0;
double g = 0.0;
double scale = 0.0;
GSparseMatrix* pU = new GSparseMatrix(m, m);
Holder<GSparseMatrix> hU(pU);
pU->copyFrom(this);
double* pSigma = new double[n];
ArrayHolder<double> hSigma(pSigma);
GSparseMatrix* pV = new GSparseMatrix(n, n);
Holder<GSparseMatrix> hV(pV);
GTEMPBUF(double, temp, n + m);
double* temp2 = temp + n;
// Householder reduction to bidiagonal form
for(int i = 0; i < n; i++)
{
// Left-hand reduction
temp[i] = scale * g;
l = i + 1;
g = 0.0;
s = 0.0;
scale = 0.0;
if(i < m)
{
Iter kend = pU->colEnd(i);
for(Iter kk = pU->colBegin(i); kk != kend; kk++)
{
if(kk->first >= (unsigned int)i)
scale += ABS(kk->second);
}
if(scale != 0.0)
{
for(Iter kk = pU->colBegin(i); kk != kend; kk++)
{
if(kk->first >= (unsigned int)i)
{
double t = kk->second / scale;
pU->set(kk->first, i, t);
s += (t * t);
}
}
f = pU->get(i, i);
g = -GSparseMatrix_takeSign(sqrt(s), f);
h = f * g - s;
pU->set(i, i, f - g);
if(i != n - 1)
{
for(j = l; j < n; j++)
{
s = 0.0;
for(Iter kk = pU->colBegin(i); kk != kend; kk++)
{
if(kk->first >= (unsigned int)i)
s += kk->second * pU->get(kk->first, j);
}
f = s / h;
for(Iter kk = pU->colBegin(i); kk != kend; kk++)
{
if(kk->first >= (unsigned int)i)
pU->set(kk->first, j, pU->get(kk->first, j) + f * kk->second);
}
}
}
for(Iter kk = pU->colBegin(i); kk != kend; kk++)
{
if(kk->first >= (unsigned int)i)
pU->set(kk->first, i, pU->get(kk->first, i) * scale);
}
}
}
pSigma[i] = scale * g;
// Right-hand reduction
g = 0.0;
s = 0.0;
scale = 0.0;
if(i < m && i != n - 1)
{
Iter kend = pU->rowEnd(i);
for(Iter kk = pU->rowBegin(i); kk != kend; kk++)
{
if(kk->first >= (unsigned int)n)
break;
if(kk->first >= (unsigned int)l)
scale += ABS(kk->second);
}
if(scale != 0.0)
{
for(Iter kk = pU->rowBegin(i); kk != kend; kk++)
{
if(kk->first >= (unsigned int)n)
break;
if(kk->first >= (unsigned int)l)
{
double t = kk->second / scale;
pU->set(i, kk->first, t);
s += (t * t);
}
}
f = pU->get(i, l);
g = -GSparseMatrix_takeSign(sqrt(s), f);
h = f * g - s;
pU->set(i, l, f - g);
for(k = l; k < n; k++)
temp[k] = pU->get(i, k) / h;
if(i != m - 1)
{
for(j = l; j < m; j++)
{
s = 0.0;
for(Iter kk = pU->rowBegin(i); kk != kend; kk++)
{
if(kk->first >= (unsigned int)n)
break;
if(kk->first >= (unsigned int)l)
s += pU->get(j, kk->first) * kk->second;
}
Iter kend2 = pU->rowEnd(j);
for(Iter kk = pU->rowBegin(j); kk != kend2; kk++)
{
if(kk->first >= (unsigned int)n)
break;
if(kk->first >= (unsigned int)l)
pU->set(j, kk->first, pU->get(j, kk->first) + s * temp[kk->first]);
}
}
}
for(Iter kk = pU->rowBegin(i); kk != kend; kk++)
{
if(kk->first >= (unsigned int)n)
break;
if(kk->first >= (unsigned int)l)
pU->set(i, kk->first, kk->second * scale);
}
}
}
norm = MAX(norm, ABS(pSigma[i]) + ABS(temp[i]));
}
// Accumulate right-hand transform
for(int i = n - 1; i >= 0; i--)
{
if(i < n - 1)
{
if(g != 0.0)
{
Iter jend = pU->rowEnd(i);
for(Iter jj = pU->rowBegin(i); jj != jend; jj++)
{
if(jj->first >= (unsigned int)n)
break;
if(jj->first >= (unsigned int)l)
pV->set(i, jj->first, (jj->second / pU->get(i, l)) / g); // (double-division to avoid underflow)
}
for(j = l; j < n; j++)
{
s = 0.0;
Iter kend = pU->rowEnd(i);
for(Iter kk = pU->rowBegin(i); kk != kend; kk++)
{
if(kk->first >= (unsigned int)n)
break;
if(kk->first >= (unsigned int)l)
s += kk->second * pV->get(j, kk->first);
}
kend = pV->rowEnd(i);
for(Iter kk = pV->rowBegin(i); kk != kend; kk++)
{
if(kk->first >= (unsigned int)n)
break;
if(kk->first >= (unsigned int)l)
pV->set(j, kk->first, pV->get(j, kk->first) + s * kk->second);
}
}
}
for(j = l; j < n; j++)
{
pV->set(i, j, 0.0);
pV->set(j, i, 0.0);
}
}
pV->set(i, i, 1.0);
g = temp[i];
l = i;
}
// Accumulate left-hand transform
for(i = n - 1; i >= 0; i--)
{
l = i + 1;
g = pSigma[i];
if(i < n - 1)
{
for(j = l; j < n; j++)
pU->set(i, j, 0.0);
}
if(g != 0.0)
{
g = 1.0 / g;
if(i != n - 1)
{
for(j = l; j < n; j++)
{
s = 0.0;
Iter kend = pU->colEnd(i);
for(Iter kk = pU->colBegin(i); kk != kend; kk++)
{
if(kk->first >= (unsigned int)l)
s += kk->second * pU->get(kk->first, j);
}
f = (s / pU->get(i, i)) * g;
if(f != 0.0)
{
for(Iter kk = pU->colBegin(i); kk != kend; kk++)
{
if(kk->first >= (unsigned int)i)
pU->set(kk->first, j, pU->get(kk->first, j) + f * kk->second);
}
}
}
}
for(j = i; j < m; j++)
pU->set(j, i, pU->get(j, i) * g);
}
else
{
for(j = i; j < m; j++)
pU->set(j, i, 0.0);
}
pU->set(i, i, pU->get(i, i) + 1.0);
}
// Diagonalize the bidiagonal matrix
for(k = n - 1; k >= 0; k--) // For each singular value
{
for(iter = 1; iter <= maxIters; iter++)
{
// Test for splitting
bool flag = true;
for(l = k; l >= 0; l--)
{
q = l - 1;
if(ABS(temp[l]) + norm == norm)
{
flag = false;
break;
}
if(ABS(pSigma[q]) + norm == norm)
break;
}
if(flag)
{
c = 0.0;
s = 1.0;
for(i = l; i <= k; i++)
{
f = s * temp[i];
temp[i] *= c;
if(ABS(f) + norm == norm)
break;
g = pSigma[i];
h = GSparseMatrix_pythag(f, g);
pSigma[i] = h;
h = 1.0 / h;
c = g * h;
s = -f * h;
Iter jendi = pU->colEnd(i);
Iter jendq = pU->colEnd(q);
Iter jji = pU->colBegin(i);
Iter jjq = pU->colBegin(q);
int tpos;
for(tpos = 0; jji != jendi || jjq != jendq; tpos++)
{
if(jjq == jendq || (jji != jendi && jji->first < jjq->first))
{
temp2[tpos] = jji->first;
jji++;
}
else
{
temp2[tpos] = jjq->first;
if(jji != jendi && jjq->first == jji->first)
jji++;
jjq++;
}
}
for(int tpos2 = 0; tpos2 < tpos; tpos2++)
{
y = pU->get((unsigned int)temp2[tpos2], q);
z = pU->get((unsigned int)temp2[tpos2], i);
pU->set((unsigned int)temp2[tpos2], q, y * c + z * s);
pU->set((unsigned int)temp2[tpos2], i, z * c - y * s);
}
}
}
z = pSigma[k];
if(l == k)
{
// Detect convergence
if(z < 0.0)
{
// Singular value should be positive
pSigma[k] = -z;
for(j = 0; j < n; j++)
pV->set(k, j, pV->get(k, j) * -1.0);
}
break;
}
if(iter >= maxIters)
ThrowError("failed to converge");
// Shift from bottom 2x2 minor
x = pSigma[l];
q = k - 1;
y = pSigma[q];
g = temp[q];
h = temp[k];
f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0 * h * y);
g = GSparseMatrix_pythag(f, 1.0);
f = ((x - z) * (x + z) + h * ((y / (f + GSparseMatrix_takeSign(g, f))) - h)) / x;
// QR transform
c = 1.0;
s = 1.0;
for(j = l; j <= q; j++)
{
i = j + 1;
g = temp[i];
y = pSigma[i];
h = s * g;
g = c * g;
z = GSparseMatrix_pythag(f, h);
temp[j] = z;
c = f / z;
s = h / z;
f = x * c + g * s;
g = g * c - x * s;
h = y * s;
y = y * c;
Iter pendi = pV->rowEnd(i);
Iter pendj = pV->rowEnd(j);
Iter ppi = pV->rowBegin(i);
Iter ppj = pV->rowBegin(j);
int tpos;
for(tpos = 0; ppi != pendi || ppj != pendj; tpos++)
{
if(ppj == pendj || (ppi != pendi && ppi->first < ppj->first))
{
temp2[tpos] = ppi->first;
ppi++;
}
else
{
temp2[tpos] = ppj->first;
if(ppi != pendi && ppj->first == ppi->first)
ppi++;
ppj++;
}
}
for(int tpos2 = 0; tpos2 < tpos; tpos2++)
{
x = pV->get(j, (unsigned int)temp2[tpos2]);
z = pV->get(i, (unsigned int)temp2[tpos2]);
pV->set(j, (unsigned int)temp2[tpos2], x * c + z * s);
pV->set(i, (unsigned int)temp2[tpos2], z * c - x * s);
}
z = GSparseMatrix_pythag(f, h);
pSigma[j] = z;
if(z != 0.0)
{
z = 1.0 / z;
c = f * z;
s = h * z;
}
f = c * g + s * y;
x = c * y - s * g;
pendi = pU->colEnd(i);
pendj = pU->colEnd(j);
ppi = pU->colBegin(i);
ppj = pU->colBegin(j);
for(tpos = 0; ppi != pendi || ppj != pendj; tpos++)
{
if(ppj == pendj || (ppi != pendi && ppi->first < ppj->first))
{
temp2[tpos] = ppi->first;
ppi++;
}
else
{
temp2[tpos] = ppj->first;
if(ppi != pendi && ppj->first == ppi->first)
ppi++;
ppj++;
}
}
for(int tpos2 = 0; tpos2 < tpos; tpos2++)
{
y = pU->get((unsigned int)temp2[tpos2], j);
z = pU->get((unsigned int)temp2[tpos2], i);
pU->set((unsigned int)temp2[tpos2], j, y * c + z * s);
pU->set((unsigned int)temp2[tpos2], i, z * c - y * s);
}
}
temp[l] = 0.0;
temp[k] = f;
pSigma[k] = x;
}
}
// Sort the singular values from largest to smallest
for(i = 1; i < n; i++)
{
for(j = i; j > 0; j--)
{
if(pSigma[j - 1] >= pSigma[j])
break;
pU->swapColumns(j - 1, j);
pV->swapRows(j - 1, j);
std::swap(pSigma[j - 1], pSigma[j]);
}
}
// Return results
*ppU = hU.release();
*ppDiag = hSigma.release();
*ppV = hV.release();
}
unsigned columns() const
{
return data_.size() / rows();
}
/// check out sawConstraintController
void updateKinematics(){
jointHandles_ = VrepRobotArmDriverP_->getJointHandles();
auto eigentestJacobian=::grl::vrep::getJacobian(*VrepRobotArmDriverP_);
/// The row/column major order is swapped between cisst and VREP!
this->currentKinematicsStateP_->Jacobian.SetSize(eigentestJacobian.cols(),eigentestJacobian.rows());
Eigen::Map<Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> > mckp2(this->currentKinematicsStateP_->Jacobian.Pointer(),this->currentKinematicsStateP_->Jacobian.cols(),this->currentKinematicsStateP_->Jacobian.rows());
mckp2 = eigentestJacobian.cast<double>();
//Eigen::Map<Eigen::Matrix<float,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> > mf(eigentestJacobian,eigentestJacobian.cols(),eigentestJacobian.rows());
//Eigen::MatrixXf eigenJacobian = mf;
Eigen::MatrixXf eigenJacobian = eigentestJacobian;
///////////////////////////////////////////////////////////
// Copy Joint Interval, the range of motion for each joint
// lower limits
auto & llim = std::get<vrep::VrepRobotArmDriver::JointLowerPositionLimit>(currentArmState_);
std::vector<double> llimits(llim.begin(),llim.end());
jointPositionLimitsVFP_->LowerLimits = vctDoubleVec(llimits.size(),&llimits[0]);
// upper limits
auto & ulim = std::get<vrep::VrepRobotArmDriver::JointUpperPositionLimit>(currentArmState_);
std::vector<double> ulimits(ulim.begin(),ulim.end());
jointPositionLimitsVFP_->UpperLimits = vctDoubleVec(ulimits.size(),&ulimits[0]);
// current position
auto & currentJointPos = std::get<vrep::VrepRobotArmDriver::JointPosition>(currentArmState_);
std::vector<double> currentJointPosVec(currentJointPos.begin(),currentJointPos.end());
vctDoubleVec vctDoubleVecCurrentJoints(currentJointPosVec.size(),¤tJointPosVec[0]);
// update limits
/// @todo does this leak memory when called every time around?
UpdateJointPosLimitsVF(positionLimitsName,jointPositionLimitsVFP_->UpperLimits,jointPositionLimitsVFP_->LowerLimits,vctDoubleVecCurrentJoints);
const auto& handleParams = VrepRobotArmDriverP_->getVrepHandleParams();
Eigen::Affine3d currentEndEffectorPose =
getObjectTransform(
std::get<vrep::VrepRobotArmDriver::RobotTipName>(handleParams)
,std::get<vrep::VrepRobotArmDriver::RobotTargetBaseName>(handleParams)
);
auto currentEigenT = currentEndEffectorPose.translation();
auto& currentCisstT = currentKinematicsStateP_->Frame.Translation();
currentCisstT[0] = currentEigenT(0);
currentCisstT[1] = currentEigenT(1);
currentCisstT[2] = currentEigenT(2);
#ifndef IGNORE_ROTATION
Eigen::Map<Eigen::Matrix<double,3,3,Eigen::ColMajor>> ccr(currentKinematicsStateP_->Frame.Rotation().Pointer());
ccr = currentEndEffectorPose.rotation();
#endif // IGNORE_ROTATION
/// @todo set rotation component of current position
Eigen::Affine3d desiredEndEffectorPose =
getObjectTransform(
std::get<vrep::VrepRobotArmDriver::RobotTargetName>(handleParams)
,std::get<vrep::VrepRobotArmDriver::RobotTargetBaseName>(handleParams)
);
auto desiredEigenT = desiredEndEffectorPose.translation();
auto& desiredCisstT = desiredKinematicsStateP_->Frame.Translation();
desiredCisstT[0] = desiredEigenT(0);
desiredCisstT[1] = desiredEigenT(1);
desiredCisstT[2] = desiredEigenT(2);
#ifndef IGNORE_ROTATION
Eigen::Map<Eigen::Matrix<double,3,3,Eigen::ColMajor>> dcr(desiredKinematicsStateP_->Frame.Rotation().Pointer());
dcr = desiredEndEffectorPose.rotation();
#endif // IGNORE_ROTATION
/// @todo set rotation component of desired position
// for debugging, the translation between the current and desired position in cartesian coordinates
auto inputDesired_dx = desiredCisstT - currentCisstT;
vct3 dx_translation, dx_rotation;
// Rotation part
vctAxAnRot3 dxRot;
vct3 dxRotVec;
dxRot.FromNormalized((currentKinematicsStateP_->Frame.Inverse() * desiredKinematicsStateP_->Frame).Rotation());
dxRotVec = dxRot.Axis() * dxRot.Angle();
dx_rotation[0] = dxRotVec[0];
dx_rotation[1] = dxRotVec[1];
dx_rotation[2] = dxRotVec[2];
//dx_rotation.SetAll(0.0);
dx_rotation = currentKinematicsStateP_->Frame.Rotation() * dx_rotation;
Eigen::AngleAxis<float> tipToTarget_cisstToEigen;
Eigen::Matrix3f rotmat;
double theta = std::sqrt(dx_rotation[0]*dx_rotation[0]+dx_rotation[1]*dx_rotation[1]+dx_rotation[2]*dx_rotation[2]);
rotmat= Eigen::AngleAxisf(theta,Eigen::Vector3f(dx_rotation[0]/theta,dx_rotation[1]/theta,dx_rotation[2]/theta));
// std::cout << "\ntiptotarget \n" << tipToTarget.matrix() << "\n";
// std::cout << "\ntiptotargetcisst\n" << rotmat.matrix() << "\n";
//BOOST_LOG_TRIVIAL(trace) << "\n test desired dx: " << inputDesired_dx << " " << dx_rotation << "\noptimizer Calculated dx: " << optimizerCalculated_dx;
SetKinematics(*currentKinematicsStateP_); // replaced by name of object
// fill these out in the desiredKinematicsStateP_
//RotationType RotationMember; // vcRot3
//TranslationType TranslationMember; // vct3
SetKinematics(*desiredKinematicsStateP_); // replaced by name of object
// call setKinematics with the new kinematics
// sawconstraintcontroller has kinematicsState
// set the jacobian here
//////////////////////
/// @todo move code below here back under run_one updateKinematics() call
/// @todo need to provide tick time in double seconds and get from vrep API call
float simulationTimeStep = simGetSimulationTimeStep();
UpdateOptimizer(simulationTimeStep);
vctDoubleVec jointAngles_dt;
auto returncode = Solve(jointAngles_dt);
/// @todo check the return code, if it doesn't have a result, use the VREP version as a fallback and report an error.
if(returncode != nmrConstraintOptimizer::NMR_OK) BOOST_THROW_EXCEPTION(std::runtime_error("VrepInverseKinematicsController: constrained optimization error, please investigate"));
/// @todo: rethink where/when/how to send command for the joint angles. Return to LUA? Set Directly? Distribute via vrep send message command?
std::string str;
// str = "";
for (std::size_t i=0 ; i < jointHandles_.size() ; i++)
{
float currentAngle;
auto ret = simGetJointPosition(jointHandles_[i],¤tAngle);
BOOST_VERIFY(ret!=-1);
float futureAngle = currentAngle + jointAngles_dt[i];
//simSetJointTargetVelocity(jointHandles_[i],jointAngles_dt[i]/simulationTimeStep);
//simSetJointTargetPosition(jointHandles_[i],jointAngles_dt[i]);
//simSetJointTargetPosition(jointHandles_[i],futureAngle);
simSetJointPosition(jointHandles_[i],futureAngle);
str+=boost::lexical_cast<std::string>(jointAngles_dt[i]);
if (i<jointHandles_.size()-1)
str+=", ";
}
BOOST_LOG_TRIVIAL(trace) << "jointAngles_dt: "<< str;
auto optimizerCalculated_dx = this->currentKinematicsStateP_->Jacobian * jointAngles_dt;
BOOST_LOG_TRIVIAL(trace) << "\n desired dx: " << inputDesired_dx << " " << dx_rotation << "\noptimizer Calculated dx: " << optimizerCalculated_dx;
}
bool
LevenbergMarquart(Function& f,
real_type t,
Vector& x,
real_type atol, real_type rtol,
unsigned *itCount,
unsigned maxit)
{
Log(NewtonMethod, Debug3) << "Start guess\nx = " << trans(x) << endl;
Matrix J;
LinAlg::MatrixFactors<real_type,0,0,LinAlg::LUTag> jacFactors;
bool converged = false;
real_type tau = 1e-1;
real_type nu = 2;
// Compute in each step a new jacobian
f.jac(t, x, J);
Log(NewtonMethod, Debug3) << "Jacobian is:\n" << J << endl;
real_type mu = tau*norm1(J);
Vector fx;
// Compute the actual error
f.eval(t, x, fx);
Vector g = trans(J)*fx;
do {
jacFactors = trans(J)*J + mu*LinAlg::Eye<real_type,0,0>(rows(x), rows(x));
Log(NewtonMethod, Debug) << "Jacobian is "
<< (jacFactors.singular() ? "singular" : "ok")
<< endl;
// Compute the search direction
Vector h = jacFactors.solve(-g);
Log(NewtonMethod, Debug) << "Solve Residual "
<< norm(trans(J)*J*h + mu*h + g)/norm(g) << endl;
// Get a better search guess
Vector xnew = x + h;
// check convergence
converged = equal(x, xnew, atol, rtol);
Log(NewtonMethod, Debug) << "Convergence test: ||h||_1 = " << norm1(h)
<< ", converged = " << converged << endl;
if (converged)
break;
f.eval(t, x, fx);
real_type Fx = norm(fx);
f.eval(t, xnew, fx);
real_type Fxnew = norm(fx);
real_type rho = (Fx - Fxnew)/(0.5*dot(h, mu*h - g));
Log(NewtonMethod, Debug) << "Rho = " << rho
<< ", Fxnew = " << Fxnew
<< ", Fx = " << Fx
<< endl;
if (0 < rho) {
Log(NewtonMethod, Debug) << "Accepted step!" << endl;
Log(NewtonMethod, Debug3) << "xnew = " << trans(xnew) << endl;
Log(NewtonMethod, Debug3) << "h = " << trans(h) << endl;
// New guess is the better one
x = xnew;
f.jac(t, x, J);
Log(NewtonMethod, Debug3) << "Jacobian is:\n" << J << endl;
// Compute the actual error
f.eval(t, x, fx);
g = trans(J)*fx;
converged = norm1(g) < atol;
Log(NewtonMethod, Debug) << "||g||_1 = " << norm1(g) << endl;
mu = mu * max(real_type(1)/3, 1-pow(2*rho-1, real_type(3)));
nu = 2;
} else {
mu = mu * nu;
nu = 2 * nu;
}
} while (!converged);
return converged;
}
double& operator()(unsigned i, unsigned j) {
return v_[i*rows() + j];
}
TEST_F(MeshLibProperties, AddVariousDifferentProperties)
{
ASSERT_TRUE(mesh != nullptr);
std::string const& prop_name("GroupVectorProperty");
// check if the property is already assigned to the mesh
ASSERT_FALSE(mesh->getProperties().hasPropertyVector(prop_name));
const std::size_t n_prop_val_groups(10);
const std::size_t n_items(mesh_size*mesh_size*mesh_size);
std::vector<std::size_t> prop_item2group_mapping(n_items);
// create simple mat_group to index mapping
for (std::size_t j(0); j<n_prop_val_groups; j++) {
std::size_t const lower(
static_cast<std::size_t>(
(static_cast<double>(j)/n_prop_val_groups)*n_items
)
);
std::size_t const upper(
static_cast<std::size_t>(
(static_cast<double>(j+1)/n_prop_val_groups)*n_items
)
);
for (std::size_t k(lower); k<upper; k++) {
prop_item2group_mapping[k] = j;
}
}
// create data structure for the property
auto* const group_properties =
mesh->getProperties().createNewPropertyVector<std::array<double, 3>*>(
prop_name, n_prop_val_groups, prop_item2group_mapping,
MeshLib::MeshItemType::Cell);
// initialize the property values
for (std::size_t i(0); i<n_prop_val_groups; i++) {
group_properties->initPropertyValue(i,
std::array<double,3>({{static_cast<double>(i),
static_cast<double>(i+1),
static_cast<double>(i+2)}}
)
);
}
// the mesh should have the property assigned to cells
ASSERT_TRUE(mesh->getProperties().hasPropertyVector(prop_name));
// fetch the vector filled with property values from mesh
auto const* const group_properties_cpy =
mesh->getProperties().getPropertyVector<std::array<double, 3>*>(
prop_name);
ASSERT_FALSE(!group_properties_cpy);
// compare the content
const std::size_t n_elements(mesh_size*mesh_size*mesh_size);
for (std::size_t k(0); k<n_elements; k++) {
ASSERT_EQ((*((*group_properties)[k]))[0],
(*((*group_properties_cpy)[k]))[0]);
ASSERT_EQ((*((*group_properties)[k]))[1],
(*((*group_properties_cpy))[k])[1]);
ASSERT_EQ((*((*group_properties)[k]))[2],
(*((*group_properties_cpy)[k]))[2]);
}
// *** add a 2nd property ***
std::string const& prop_name_2("ItemwiseMatrixProperties");
// check if the property is already assigned to the mesh
ASSERT_FALSE(mesh->getProperties().hasPropertyVector(prop_name_2));
const std::size_t n_items_2(mesh_size*mesh_size*mesh_size);
auto* const array_properties =
mesh->getProperties().createNewPropertyVector<std::array<float, 9>>(
prop_name_2, MeshLib::MeshItemType::Cell);
array_properties->resize(n_items_2);
// initialize the property values
for (std::size_t i(0); i<n_items_2; i++) {
// init property value
for (std::size_t k(0); k<(*array_properties)[i].size(); k++) {
(*array_properties)[i][k] = static_cast<float>(i+k);
}
}
EXPECT_EQ(9, (*array_properties)[0].size());
// the mesh should have the property assigned to cells
ASSERT_TRUE(mesh->getProperties().hasPropertyVector(prop_name_2));
// fetch the vector in order to compare the content
auto const* const array_properties_cpy =
mesh->getProperties().getPropertyVector<std::array<float, 9>>(
prop_name_2);
ASSERT_FALSE(!array_properties_cpy);
// compare the values/matrices
for (std::size_t k(0); k<n_items_2; k++) {
for (std::size_t j(0); j<(*array_properties)[k].size(); j++) {
ASSERT_EQ((*array_properties)[k][j], (*array_properties_cpy)[k][j]);
}
}
// *** add a 3rd property ***
std::string const& prop_name_3("ItemwiseEigenMatrixProperties");
// check if the property is already assigned to the mesh
ASSERT_FALSE(mesh->getProperties().hasPropertyVector(prop_name_3));
auto* const matrix_properties =
mesh->getProperties()
.createNewPropertyVector<
Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>(
prop_name_3, MeshLib::MeshItemType::Cell);
// init property values
for (auto it = matrix_properties->begin(); it != matrix_properties->end();
it++)
{
for (int r(0); r<it->rows(); r++) {
for (int c(0); c<it->cols(); c++) {
(*it)(r,c) = static_cast<double>(
std::distance(matrix_properties->begin(),it)+r*it->cols()+c+1);
}
}
}
// the mesh should have the property assigned to cells
ASSERT_TRUE(mesh->getProperties().hasPropertyVector(prop_name_3));
// fetch the vector in order to compare the content
auto const* const matrix_properties_cpy =
mesh->getProperties()
.getPropertyVector<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>(
prop_name_3);
ASSERT_FALSE(!matrix_properties_cpy);
// compare the values/matrices
auto it_cpy = matrix_properties_cpy->begin();
for (auto it = matrix_properties->begin(); it != matrix_properties->end();
it++, it_cpy++)
{
for (int r(0); r<it->rows(); r++) {
for (int c(0); c<it->cols(); c++) {
ASSERT_EQ((*it)(r,c), (*it_cpy)(r,c));
}
}
}
}
/** Create the dual of a linear program.
* The function can only dualize programs of the form
* <code>Ax <= b, x >= 0</code>. The data in <code>primalVars</code> and
* <code>dualRows</code> as well as in <code>primalRows</code> and
* <code>dualVars</code> is in 1-to-1-correspondence.
* @param primalObj Objective function of primal problem.
* @param primalVars Variables in primal problem.
* @param primalRows Rows in primal problem.
* @param dualObj Objective function of dual will be stored here.
* @param dualVars All dual variables will be stored here.
* @param dualRows All dual rows will be stored here.
*/
void BendersOpt::makeDual(IloObjective const &primalObj,
IloNumVarArray const &primalVars,
IloRangeArray const &primalRows,
IloObjective *dualObj,
IloNumVarArray *dualVars,
IloRangeArray *dualRows)
{
// To keep the code simple we only support problems
// of the form Ax <= b, b >= 0 here. We leave it as a reader's
// exercise to extend the function to something that can handle
// any kind of linear model.
for (IloInt j = 0; j < primalVars.getSize(); ++j)
if ( primalVars[j].getLB() != 0 ||
primalVars[j].getUB() < IloInfinity )
{
std::stringstream s;
s << "Cannot dualize variable " << primalVars[j];
throw s.str();
}
for (IloInt i = 0; i < primalRows.getSize(); ++i)
if ( primalRows[i].getLB() > -IloInfinity ||
primalRows[i].getUB() >= IloInfinity )
{
std::stringstream s;
s << "Cannot dualize constraint " << primalRows[i];
std::cerr << s.str() << std::endl;
throw s.str();
}
// The dual of
// min/max c^T x
// Ax <= b
// x >= 0
// is
// max/min y^T b
// y^T A <= c
// y <= 0
// We scale y by -1 to get >= 0
IloEnv env = primalVars.getEnv();
IloObjective obj(env, 0.0,
primalObj.getSense() == IloObjective::Minimize ?
IloObjective::Maximize : IloObjective::Minimize);
IloRangeArray rows(env);
IloNumVarArray y(env);
std::map<IloNumVar,IloInt,ExtractableLess<IloNumVar> > v2i;
for (IloInt j = 0; j < primalVars.getSize(); ++j) {
IloNumVar x = primalVars[j];
v2i.insert(std::map<IloNumVar,IloInt,ExtractableLess<IloNumVar> >::value_type(x, j));
rows.add(IloRange(env, -IloInfinity, 0, x.getName()));
}
for (IloExpr::LinearIterator it = primalObj.getLinearIterator(); it.ok(); ++it)
rows[v2i[it.getVar()]].setUB(it.getCoef());
for (IloInt i = 0; i < primalRows.getSize(); ++i) {
IloRange r = primalRows[i];
IloNumColumn col(env);
col += obj(-r.getUB());
for (IloExpr::LinearIterator it = r.getLinearIterator(); it.ok(); ++it)
col += rows[v2i[it.getVar()]](-it.getCoef());
y.add(IloNumVar(col, 0, IloInfinity, IloNumVar::Float, r.getName()));
}
*dualObj = obj;
*dualVars = y;
*dualRows = rows;
}
void Write_M(void)
{
////////////M//////////////////////
u8 cola_M[]={1,0,0,0,0,0,0,0};
u8 rowa_M[]={1,1,1,1,1,1,1,1};
u8 colb_M[]={0,1,0,0,0,0,0,0};
u8 rowb_M[]={0,1,0,0,0,0,0,0};
u8 colc_M[]={0,0,1,0,0,0,0,0};
u8 rowc_M[]={0,0,1,0,0,0,0,0};
u8 cold_M[]={0,0,0,1,0,0,0,0};
u8 rowd_M[]={0,0,0,1,0,0,0,0};
u8 cole_M[]={0,0,0,0,1,0,0,0};
u8 rowe_M[]={0,0,1,0,0,0,0,0};
u8 colf_M[]={0,0,0,0,0,1,0,0};
u8 rowf_M[]={0,1,0,0,0,0,0,0};
u8 colg_M[]={0,0,0,0,0,0,1,0};
u8 rowg_M[]={1,1,1,1,1,1,1,1};
u8 x = 30;
while(x)
{
cols(cola_M);
rows(rowa_M);
DIO_u8WritePinVal(latch,1);
DELAY(10);
DIO_u8WritePinVal(latch,0);
cols(colb_M);
rows(rowb_M);
DIO_u8WritePinVal(latch,1);
DELAY(10);
DIO_u8WritePinVal(latch,0);
cols(colc_M);
rows(rowc_M);
DIO_u8WritePinVal(latch,1);
DELAY(10);
DIO_u8WritePinVal(latch,0);
cols(cold_M);
rows(rowd_M);
DIO_u8WritePinVal(latch,1);
DELAY(10);
DIO_u8WritePinVal(latch,0);
cols(cole_M);
rows(rowe_M);
DIO_u8WritePinVal(latch,1);
DELAY(10);
DIO_u8WritePinVal(latch,0);
cols(colf_M);
rows(rowf_M);
DIO_u8WritePinVal(latch,1);
DELAY(10);
DIO_u8WritePinVal(latch,0);
cols(colg_M);
rows(rowg_M);
DIO_u8WritePinVal(latch,1);
DELAY(10);
DIO_u8WritePinVal(latch,0);
x-- ;
}
}
void Write_O(void)
{
//////////////D//////////////////////
u8 cola_O[]={1,0,0,0,0,0,0,0};
u8 rowa_O[]={0,0,1,1,1,1,0,0};
u8 colb_O[]={0,1,0,0,0,0,0,0};
u8 rowb_O[]={0,1,0,0,0,0,1,0};
u8 colc_O[]={0,0,1,0,0,0,0,0};
u8 rowc_O[]={1,0,0,0,0,0,0,1};
u8 cold_O[]={0,0,0,1,0,0,0,0};
u8 rowd_O[]={1,0,0,0,0,0,0,1};
u8 cole_O[]={0,0,0,0,1,0,0,0};
u8 rowe_O[]={1,0,0,0,0,0,0,1};
u8 colf_O[]={0,0,0,0,0,1,0,0};
u8 rowf_O[]={0,1,0,0,0,0,1,0};
u8 colg_O[]={0,0,0,0,0,0,1,0};
u8 rowg_O[]={0,0,1,1,1,1,0,0};
u8 x = 30 ;
while(x)
{
cols(cola_O);
rows(rowa_O);
DIO_u8WritePinVal(latch,1);
DELAY(0.001);
DIO_u8WritePinVal(latch,0);
cols(colb_O);
rows(rowb_O);
DIO_u8WritePinVal(latch,1);
DELAY(0.001);
DIO_u8WritePinVal(latch,0);
cols(colc_O);
rows(rowc_O);
DIO_u8WritePinVal(latch,1);
DELAY(0.001);
DIO_u8WritePinVal(latch,0);
cols(cold_O);
rows(rowd_O);
DIO_u8WritePinVal(latch,1);
DELAY(0.001);
DIO_u8WritePinVal(latch,0);
cols(cole_O);
rows(rowe_O);
DIO_u8WritePinVal(latch,1);
DELAY(0.001);
DIO_u8WritePinVal(latch,0);
cols(colf_O);
rows(rowf_O);
DIO_u8WritePinVal(latch,1);
DELAY(0.001);
DIO_u8WritePinVal(latch,0);
cols(colg_O);
rows(rowg_O);
DIO_u8WritePinVal(latch,1);
DELAY(0.001);
DIO_u8WritePinVal(latch,0);
x--;
}
}
void quaterN::c1stitch(quaterN const& a, quaterN const& b)
{
// use hermite curve();
c0stitch(a, b);
/////////////////////////////////
// 123 : time
// _____ src motion
// ++++ add motion
// | con
// - guess velocity of srcMotion of this frame
// **** make displacementmap
// m21
// p 01
//disp1
// ***
//disp2
// ***
////////////////////////
int con=a.rows()-1;
quater sp1(row(con-1)); // src position at time 1
quater p2(row(con)); // center position at time 2
quater ap3(row(con+1)); // add position at time3
quater sv;
quater av;
quater cv;
sv.difference(sp1, p2);
av.difference(p2, ap3);
cv.interpolate(0.5f, sv, av); // desired velocity at con.
// guess position of desired curve
quater p1, p3;
p1.mult( cv.inverse(), p2);
p3.mult( cv, p2);
quater sp3;
sp3.mult(sv, p2);
quater ap1;
ap1.mult(av.inverse(), p2);
static quaterN disp1, disp2;
quater disp_sp2, disp_sp3;
disp_sp2.identity(); // p2->p2
disp_sp3.difference(sp3, p3);
quater disp_ap1, disp_ap2;
disp_ap1.difference(ap1, p1);
disp_ap2.identity();
// only half of velocity difference is stitched
disp_sp3.scale(0.5);
disp_ap1.scale(0.5);
disp1.hermite(quater(1,0,0,0), quater(1,0,0,0), a.rows(), disp_sp2, disp_sp3);
disp2.hermite(disp_ap1, disp_ap2, b.rows(), quater(1,0,0,0), quater(1,0,0,0));
// forward filling
for(int i=0; i<disp1.rows(); i++)
row(i)*=disp1.row(i);
// backward filling
for(int i=0; i<disp2.rows(); i++)
row(rows()-i-1)*=disp2.row(disp2.rows()-i-1);
}
int hint(int y, int x) const {
assert(0 <= y && y < rows() - 1);
assert(0 <= x && x < cols() - 1);
return hint_[y][x];
}
/**
***************************************************************************************************
* DX12ImageRenderer::CpuImageToPng
*
* @brief
* Take an RGBA CpuImage ptr and put it in PNG form resident in system memory.
*
* IMPORTANT: Memory inside ppImage is allocated on behalf of the caller, so it is their
* responsibility to free it.
*
* @return
* True if successful.
***************************************************************************************************
*/
bool DX12ImageRenderer::CpuImageToPng(
CpuImage* pImage,
UCHAR** ppPngMem,
UINT* pMemSize,
const std::string& fileName) // Optional
{
bool success = false;
if ((pImage != nullptr) && (ppPngMem != nullptr) && (pMemSize != nullptr))
{
png_structp png_ptr = png_create_write_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL);
if (png_ptr)
{
png_infop info_ptr = png_create_info_struct(png_ptr);
png_set_IHDR(png_ptr, info_ptr, pImage->width, pImage->height, 8, PNG_COLOR_TYPE_RGBA, PNG_INTERLACE_NONE, PNG_COMPRESSION_TYPE_DEFAULT, PNG_FILTER_TYPE_DEFAULT);
std::vector<UCHAR*> rows(pImage->height);
for (UINT i = 0; i < pImage->height; i++)
{
rows[i] = (UCHAR*)pImage->pData + i * pImage->width * 4;
}
png_set_rows(png_ptr, info_ptr, &rows[0]);
std::vector<UCHAR> pngData;
png_set_write_fn(png_ptr, &pngData, CustomPngWriteCb, NULL);
png_write_png(png_ptr, info_ptr, PNG_TRANSFORM_IDENTITY, NULL);
png_destroy_write_struct(&png_ptr, NULL);
const UINT memSize = (UINT)pngData.size();
if (memSize != 0)
{
UCHAR* pOut = new UCHAR[memSize];
if (pOut != nullptr)
{
memcpy(pOut, &pngData[0], memSize);
*ppPngMem = pOut;
*pMemSize = memSize;
success = true;
// Optionally, dump it out to file
if (fileName != "")
{
std::ofstream outfile(fileName.c_str(), std::ios::out | std::ios::binary);
outfile.write((const char*)pOut, memSize);
outfile.close();
}
}
}
}
}
return success;
}
uint32_t rows(int zoomlevel) const {
return 1+(rows()-1)/zoom_rowpixelsize(zoomlevel);
}
//! Assign cluster number
uvec labeltree(const mat& X, uvec conn)
{
uword n = X.n_rows;
uword nleaves = n + 1;
uvec T = ones<uvec>(n + 1);
// Each cut potentially yeild as additional cluster
uvec todo = ones<uvec>(n);
// Define cluster numbers for each side of each non-leaf node
umat clustlist = reshape(arma_ext::colon<uvec>(1, 2 * n), n, 2);
// Propagate cluster numbers down the tree
while (any(todo)) {
// Work on rows that are now split but not yet processed
// rows = find(todo & ~conn);
uvec rows = find(todo % logical_not(conn));
if (rows.empty()) break;
for (uword j = 0 ; j < 2 ; j++) { // 0: left, 1: right
uvec children = conv_to<uvec>::from(X.col(j)).elem(rows);
// Assign cluster number to child leaf node
uvec leaf = (children <= nleaves);
if (any(leaf)) {
uvec leafi = find(leaf);
#ifdef ARMA_EXT_USE_CPP11
std::for_each(leafi.begin(), leafi.end(), [&](uword index) {
#else
for (size_type i = 0 ; i < leafi.size() ; i++) {
uword index = leafi[i];
#endif
T[children[index] - 1] = clustlist.at(rows[index], j);
#ifdef ARMA_EXT_USE_CPP11
});
#else
}
#endif
}
// Also assign it to both children of any joined child non-leaf nodes
uvec joint = logical_not(leaf); // ~leaf
uvec jointi = find(joint);
joint(jointi) = conn(children(jointi) - nleaves - 1);
if (any(joint)) {
#ifdef ARMA_EXT_USE_CPP11
std::for_each(jointi.begin(), jointi.end(), [&](uword index) {
#else
for (size_type i = 0 ; i < jointi.size() ; i++) {
uword index = jointi[i];
#endif
uword clustnum = clustlist(rows(index), j);
uword childnum = children(index) - nleaves - 1;
clustlist.row(childnum).fill(clustnum);
conn(childnum) = 0;
#ifdef ARMA_EXT_USE_CPP11
});
#else
}
#endif
}
}
int zooms() const {
int z = 0;
while (zoom_rowpixelsize(z) < rows() || zoom_colpixelsize(z) < cols()) z++;
return z;
}
TEST_F(SQLiteUtilTests, test_simple_query_execution) {
// Access to the internal SQL implementation is only available in core.
auto sql = SQL("SELECT * FROM time");
EXPECT_TRUE(sql.ok());
EXPECT_EQ(sql.rows().size(), 1U);
}
void LpSub::initialize()
{
// LpSub::initialize(): local variables
Array<double> obj(sub_->nVar());
Array<double> lBound(sub_->nVar());
Array<double> uBound(sub_->nVar());
Array<Row*> rows(sub_->nCon());
Array<LPVARSTAT::STATUS> lpVarStat(sub_->nVar());
Array<SlackStat::STATUS> slackStat(sub_->nCon());
Row row(master_, sub_->nVar()); //!< buffer to store generated row
int conNnz; //!< number of nonzeros of constraint \a c
int c; //!< loop index
// generate the row format of the active constraints
/* After the generation of the row format we allocate a row of
* the correct length and make a copy in order to safe memory.
*/
int nRow = 0;
const int nCon = sub_->nCon();
for (c = 0; c < nCon; c++) {
conNnz = sub_->constraint(c)->genRow(sub_->actVar(), row);
rows[nRow] = new Row(master_, conNnz);
rows[nRow]->copy(row);
slackStat[nRow] = sub_->slackStat(c)->status();
++nRow;
row.clear();
}
// eliminate set and fixed variables and initialize the columns
Variable *v; //!< pointer to variable of subproblem
Array<bool> marked(0,sub_->nVar()-1, false); //!< \a true if variable can be eliminated
nOrigVar_ = sub_->nVar();
valueAdd_ = 0.0;
// LpSub: mark variables to eliminate, build objective function and bounds
/* We mark all variables which can be eliminated, add them to the
* ArrayBuffer \a delVar, compute the mappings from the original variable
* set to the actual variable set in the \a LP, and vice versa, and
* determine the correction term for the LP-value.
* If all variables can be eliminated then we do not eliminate the last
* variable for simpification. Otherwise it would be necessary to load
* an problem with 0 variables to the LP-solver which is, e.g., for
* Cplex not possible. Although the emulation of the optimization would
* still be simple, but extra work would have to be performed if later
* constraints were added.
*/
const int nVar = sub_->nVar();
int nCol = 0;
for (int i = 0; i < nVar; i++) {
v = sub_->variable(i);
if(sub_->fsVarStat(i)->fixedOrSet()) {
if (eliminable(i) && (nCol || (i != sub_->nVar() - 1))) {
//! eliminate variable \a i from the LP
marked[i] = true;
valueAdd_ += v->obj() * elimVal(i);
orig2lp_[i] = -1;
}
else {
// fix variable \a i in the LP
/* As variable \a i could not be eliminated we set both its upper and lower
* bound to the value it is fixed or set to.
*/
orig2lp_[i] = nCol;
lp2orig_[nCol] = i;
obj[nCol] = v->obj();
lBound[nCol] = elimVal(i);
uBound[nCol] = elimVal(i);
lpVarStat[nCol] = sub_->lpVarStat(i)->status();
++nCol;
}
}
else {
// add variable \a i to the LP
orig2lp_[i] = nCol;
lp2orig_[nCol] = i;
obj[nCol] = v->obj();
lBound[nCol] = sub_->lBound(i);
uBound[nCol] = sub_->uBound(i);
lpVarStat[nCol] = sub_->lpVarStat(i)->status();
++nCol;
}
}
// LpSub: update the constraints
/* If all active variables of a constraint are eliminated then
* its left hand side is void (implicitly 0), but its right hand side
* can be nonzero. Depending on the sense of the constraint it can be
* infeasible.
* If the elimination of variables from constraints causes an infeasible
* \a LP, the constraint is memorized in \a infeasCons_.
*/
ArrayBuffer<int> delVar(sub_->nVar(),false); //!< buffer of deletable components of row format
double rhsDelta; //!< correction of right hand side due to eliminations
InfeasCon::INFEAS infeas; //!< infeasibility mode (TooLarge, TooSmall)
for (c = 0; c < nCon; c++) {
// eliminate the variables from the constraint
delVar.clear();
rhsDelta = 0.0;
const int rNnz = rows[c]->nnz();
for(int i = 0; i < rNnz; i++)
if(marked[rows[c]->support(i)]) {
delVar.push(i);
rhsDelta += rows[c]->coeff(i)*elimVal(rows[c]->support(i));
}
rows[c]->delInd(delVar, rhsDelta);
// check if the constraint is now infeasible
if (rows[c]->nnz() == 0) {
infeas = sub_->constraint(c)->voidLhsViolated(rows[c]->rhs());
if (infeas != InfeasCon::Feasible)
infeasCons_.push(new InfeasCon(master_, sub_->constraint(c), infeas));
}
rows[c]->rename(orig2lp_);
}
// initialize the LP-solver and clean up
LP::initialize(*master_->optSense(), nRow, sub_->maxCon(), nCol,
sub_->maxVar(), obj, lBound, uBound, rows,
lpVarStat, slackStat);
for (c = 0; c < nCon; c++)
delete rows[c];
}
int MSLayout::realHeight(void) const
{
int offset=highlightThickness()+shadowThickness()+margin();
return (height()-innerHeight()-(rows()-1)*rowSpacing()-2*offset);
}
void quaterN::displacement(const quater& sp1, const quater& sp2, const quater& ap22, const quater& ap33, int start, int end)
{
/////////////////////////////////
// sp123
// ____ src motion
// ++++ add motion
// ap123
// -- guess position and velocity of srcMotion of these frames
// **** make displacementmap
// m21
// p 01
//disp1
// ***
//disp2
// ***
////////////////////////
quater ap2(ap22);
quater ap3(ap33);
//ASSERT(sp2%sp1>0);
ap2.align(sp2);
ap3.align(ap2);
quater center,center_halfvel, center_3halfvel;
quater sp3, ap1;
quater sv;
quater av;
sv.difference(sp1, sp2);
av.difference(ap2, ap3);
center.interpolate(0.5f, sp2,ap2);
center_halfvel.interpolate(0.5f, sv, av);
// the following two lines are heuristic velocity adjustment. (Think about it by drawing the situation by yourself.)
quater center_halfvel2;
center_halfvel2.difference(sp2, ap2);
center_halfvel2.align(quater(1,0,0,0));
center_halfvel2.scale(1.f/((float)(end-start)*8.f));
center_halfvel.leftMult(center_halfvel2);
center_3halfvel=center_halfvel;
center_3halfvel.scale(0.5f*3.f);
center_halfvel.scale(0.5f);
// guess position
quater m2,m1,p0,p1;
m2.mult( center_3halfvel.inverse(), center);
m1.mult( center_halfvel.inverse(), center);
p0.mult(center_halfvel, center);
p1.mult(center_3halfvel, center);
static quaterN disp1, disp2;
quater disp_sp1, disp_sp2;
disp_sp1.difference(sp1, m2);
disp_sp2.difference(sp2, m1);
quater disp_ap2, disp_ap3;
disp_ap2.difference(ap2, p0);
disp_ap3.difference(ap3, p1);
disp1.hermite(quater(1,0,0,0), quater(1,0,0,0), ABS(start)-1, disp_sp1, disp_sp2);
disp2.hermite(disp_ap2, disp_ap3, end-1, quater(1,0,0,0), quater(1,0,0,0));
ASSERT(end-start==disp1.rows()+disp2.rows()+2);
setSize(end-start);
// forward filling
for(int i=0; i<disp1.rows(); i++)
row(i)=disp1.row(i);
// center filling
row(ABS(start)-1)=disp_sp2;
row(ABS(start))=disp_ap2;
// backward filling
for(int i=0; i<disp2.rows(); i++)
row(rows()-i-1)=disp2.row(disp2.rows()-i-1);
///////////////////
//testing code backup
/*
quaterN test;
test.setSize(40);
for(int i=0; i<20; i++)
{
test[i].setRotation(vector3(0,1,0), (20-i)/20.f);
}
for(int i=20; i<40; i++)
{
test[i].setRotation(vector3(0,1,0), (-i+10)/20.f);
}
matrixn temp;
temp.assign(test);
temp.op0(m0::drawSignals("qtemp1.bmp",-1.f,1.f,true));
quaterN disp;
disp.displacement(test[18], test[19], test[20],test[21], -20,20);
for(int i=0; i<40; i++)
test[i].leftMult(disp[i]);
temp.assign(test);
temp.op0(m0::drawSignals("qtemp2.bmp",-1.f,1.f,true));*/
}
/*! Reverses the order of the rows in the latin square. */
void LatinSquare::reverseRows(void) {
std::vector< std::vector<unsigned int> > copy = square;
for (unsigned int i = 0, j = rows() - 1; i < rows(); i++, j--) {
square[i] = copy[j];
}
}
void quaterN::align()
{
for(int i=0; i<rows()-1; i++)
row(i+1).align(row(i));
}
/*!
\since 4.1
Merges the cell at the specified \a row and \a column with the adjacent cells
into one cell. The new cell will span \a numRows rows and \a numCols columns.
If \a numRows or \a numCols is less than the current number of rows or columns
the cell spans then this method does nothing.
\sa splitCell()
*/
void QTextTable::mergeCells(int row, int column, int numRows, int numCols)
{
Q_D(QTextTable);
if (d->dirty)
d->update();
QTextDocumentPrivate *p = d->pieceTable;
QTextFormatCollection *fc = p->formatCollection();
const QTextTableCell cell = cellAt(row, column);
if (!cell.isValid() || row != cell.row() || column != cell.column())
return;
QTextCharFormat fmt = cell.format();
const int rowSpan = fmt.tableCellRowSpan();
const int colSpan = fmt.tableCellColumnSpan();
numRows = qMin(numRows, rows() - cell.row());
numCols = qMin(numCols, columns() - cell.column());
// nothing to merge?
if (numRows < rowSpan || numCols < colSpan)
return;
// check the edges of the merge rect to make sure no cell spans the edge
for (int r = row; r < row + numRows; ++r) {
if (cellAt(r, column) == cellAt(r, column - 1))
return;
if (cellAt(r, column + numCols) == cellAt(r, column + numCols - 1))
return;
}
for (int c = column; c < column + numCols; ++c) {
if (cellAt(row, c) == cellAt(row - 1, c))
return;
if (cellAt(row + numRows, c) == cellAt(row + numRows - 1, c))
return;
}
p->beginEditBlock();
const int origCellPosition = cell.firstPosition() - 1;
const int cellFragment = d->grid[row * d->nCols + column];
// find the position at which to insert the contents of the merged cells
QFragmentFindHelper helper(origCellPosition, p->fragmentMap());
QList<int>::Iterator it = qBinaryFind(d->cells.begin(), d->cells.end(), helper);
Q_ASSERT(it != d->cells.end());
Q_ASSERT(*it == cellFragment);
const int insertCellIndex = it - d->cells.begin();
int insertFragment = d->cells.value(insertCellIndex + 1, d->fragment_end);
uint insertPos = p->fragmentMap().position(insertFragment);
d->blockFragmentUpdates = true;
bool rowHasText = cell.firstCursorPosition().block().length();
bool needsParagraph = rowHasText && colSpan == numCols;
// find all cells that will be erased by the merge
for (int r = row; r < row + numRows; ++r) {
int firstColumn = r < row + rowSpan ? column + colSpan : column;
// don't recompute the cell index for the first row
int firstCellIndex = r == row ? insertCellIndex + 1 : -1;
int cellIndex = firstCellIndex;
for (int c = firstColumn; c < column + numCols; ++c) {
const int fragment = d->grid[r * d->nCols + c];
// already handled?
if (fragment == cellFragment)
continue;
QTextDocumentPrivate::FragmentIterator it(&p->fragmentMap(), fragment);
uint pos = it.position();
if (firstCellIndex == -1) {
QFragmentFindHelper helper(pos, p->fragmentMap());
QList<int>::Iterator it = qBinaryFind(d->cells.begin(), d->cells.end(), helper);
Q_ASSERT(it != d->cells.end());
Q_ASSERT(*it == fragment);
firstCellIndex = cellIndex = it - d->cells.begin();
}
++cellIndex;
QTextCharFormat fmt = fc->charFormat(it->format);
const int cellRowSpan = fmt.tableCellRowSpan();
const int cellColSpan = fmt.tableCellColumnSpan();
// update the grid for this cell
for (int i = r; i < r + cellRowSpan; ++i)
for (int j = c; j < c + cellColSpan; ++j)
d->grid[i * d->nCols + j] = cellFragment;
// erase the cell marker
p->remove(pos, 1);
const int nextFragment = d->cells.value(cellIndex, d->fragment_end);
const uint nextPos = p->fragmentMap().position(nextFragment);
Q_ASSERT(nextPos >= pos);
// merge the contents of the cell (if not empty)
if (nextPos > pos) {
if (needsParagraph) {
needsParagraph = false;
QTextCursor(p, insertPos++).insertBlock();
p->move(pos + 1, insertPos, nextPos - pos);
} else if (rowHasText) {
QTextCursor(p, insertPos++).insertText(QLatin1String(" "));
p->move(pos + 1, insertPos, nextPos - pos);
} else {
p->move(pos, insertPos, nextPos - pos);
}
insertPos += nextPos - pos;
rowHasText = true;
}
}
if (rowHasText) {
needsParagraph = true;
rowHasText = false;
}
// erase cells from last row
if (firstCellIndex >= 0) {
d->cellIndices.remove(firstCellIndex, cellIndex - firstCellIndex);
d->cells.erase(d->cells.begin() + firstCellIndex, d->cells.begin() + cellIndex);
}
}
d->fragment_start = d->cells.first();
fmt.setTableCellRowSpan(numRows);
fmt.setTableCellColumnSpan(numCols);
p->setCharFormat(origCellPosition, 1, fmt);
d->blockFragmentUpdates = false;
d->dirty = false;
p->endEditBlock();
}
void quaterN::combine(const quaterN& rotY, const quaterN& offset)
{
for(int i=0; i<rows(); i++)
row(i).mult(rotY[i], offset[i]);
}
void vector3N::displacement(const vector3& sp1, const vector3& sp2, const vector3& ap2, const vector3& ap3, int start, int end)
{
/////////////////////////////////
// sp123
// ____ src motion
// ++++ add motion
// ap123
// -- guess position and velocity of srcMotion of these frames
// **** make displacementmap
// m21
// p 01
//disp1
// ***
//disp2
// ***
////////////////////////
vector3 center,center_halfvel, center_3halfvel;
vector3 sp3, ap1;
vector3 sv;
vector3 av;
sv.difference(sp1, sp2);
av.difference(ap2, ap3);
center.add(sp2,ap2);
center/=2.f;
center_halfvel.add(sv, av);
center_halfvel/=4.f;
// the following two lines are heuristic velocity adjustment. (Think about it by drawing the situation by yourself.)
vector3 center_halfvel2=(ap2-sp2)/((float)(end-start)*8.f);
center_halfvel+=center_halfvel2;
center_3halfvel.mult(center_halfvel,3);
// guess position
vector3 m2,m1,p0,p1;
m2.sub(center, center_3halfvel);
m1.sub(center, center_halfvel);
p0.add(center, center_halfvel);
p1.add(center, center_3halfvel);
static vector3N disp1, disp2;
vector3 disp_sp1, disp_sp2;
disp_sp1.difference(sp1, m2);
disp_sp2.difference(sp2, m1);
vector3 disp_ap2, disp_ap3;
disp_ap2.difference(ap2, p0);
disp_ap3.difference(ap3, p1);
disp1.hermite(vector3(0,0,0), vector3(0,0,0), ABS(start)-1, disp_sp1, disp_sp2);
disp2.hermite(disp_ap2, disp_ap3, end-1, vector3(0,0,0), vector3(0,0,0));
ASSERT(end-start==disp1.rows()+disp2.rows()+2);
setSize(end-start);
// forward filling
for(int i=0; i<disp1.rows(); i++)
row(i)=disp1.row(i);
// center filling
row(ABS(start)-1)=disp_sp2;
row(ABS(start))=disp_ap2;
// backward filling
for(int i=0; i<disp2.rows(); i++)
row(rows()-i-1)=disp2.row(disp2.rows()-i-1);
}
/// Get value
const Real & operator()(int c,int r) const
{
return Base::get(data,cols(),rows(),c,r);
}