void Model::
computeInverseMassInertia()
{
if (ainv_upper_triangular_.empty()) {
ainv_upper_triangular_.resize(ndof_ * (ndof_ + 1) / 2);
}
deFloat const one(1);
for (size_t irow(0); irow < ndof_; ++irow) {
taoJoint * joint(kgm_tree_->info[irow].joint);
// Compute one column of Ainv by solving forward dynamics of the
// corresponding joint having a unit torque, while all the
// others remain unactuated. This works on the kgm_tree because
// it has zero speeds, thus the Coriolis-centrifgual effects are
// zero, and by using zero gravity we get pure system dynamics:
// acceleration = mass_inv * force (in matrix form).
joint->setTau(&one);
taoDynamics::fwdDynamics(kgm_tree_->root, &zero_gravity);
joint->zeroTau();
// Retrieve the column of Ainv by reading the joint
// accelerations generated by the column-selecting unit torque
// (into a flattened upper triangular matrix).
for (size_t icol(0); icol <= irow; ++icol) {
kgm_tree_->info[icol].joint->getDDQ(&ainv_upper_triangular_[squareToTriangularIndex(irow, icol, ndof_)]);
}
}
// Reset all the accelerations.
for (size_t ii(0); ii < ndof_; ++ii) {
kgm_tree_->info[ii].joint->zeroDDQ();
}
}
void Model::
computeMassInertia()
{
if (a_upper_triangular_.empty()) {
a_upper_triangular_.resize(ndof_ * (ndof_ + 1) / 2);
}
deFloat const one(1);
for (size_t irow(0); irow < ndof_; ++irow) {
taoJoint * joint(kgm_joints_[irow]);
// Compute one column of A by solving inverse dynamics of the
// corresponding joint having a unit acceleration, while all the
// others remain fixed. This works on the KGM tree because it
// has zero speeds, thus the Coriolis-centrifgual effects are
// zero, and by using zero gravity we get pure system dynamics:
// force = mass * acceleration (in matrix form).
joint->setDDQ(&one);
taoDynamics::invDynamics(kgm_root_, &zero_gravity);
joint->zeroDDQ();
// Retrieve the column of A by reading the joint torques
// required for the column-selecting unit acceleration (into a
// flattened upper triangular matrix).
for (size_t icol(0); icol <= irow; ++icol) {
kgm_joints_[icol]->getTau(&a_upper_triangular_[squareToTriangularIndex(irow, icol, ndof_)]);
}
}
// Reset all the torques.
for (size_t ii(0); ii < ndof_; ++ii) {
kgm_joints_[ii]->zeroTau();
}
}
autoConfusion Confusion_groupResponses (Confusion me, const char32 *labels, const char32 *newLabel, long newpos) {
try {
long ncondense = Melder_countTokens (labels);
autoNUMvector<long> icol (1, my numberOfColumns);
for (long i = 1; i <= my numberOfColumns; i++) {
icol[i] = i;
}
for (char32 *token = Melder_firstToken (labels); token != 0; token = Melder_nextToken ()) {
for (long i = 1; i <= my numberOfColumns; i++) {
if (Melder_equ (token, my columnLabels[i])) {
icol[i] = 0;
break;
}
}
}
long nfound = 0;
for (long i = 1; i <= my numberOfColumns; i++) {
if (icol[i] == 0) {
nfound ++;
}
}
if (nfound == 0) {
Melder_throw (U"Invalid response labels.");
}
if (nfound != ncondense) {
Melder_warning (U"One or more of the given response labels are suspect.");
}
long newnresp = my numberOfColumns - nfound + 1;
if (newpos < 1) {
newpos = 1;
}
if (newpos > newnresp) {
newpos = newnresp;
}
autoConfusion thee = Confusion_create (my numberOfRows, newnresp);
NUMstrings_copyElements (my rowLabels, thy rowLabels, 1, my numberOfRows);
TableOfReal_setColumnLabel (thee.get(), newpos, newLabel);
long inewcol = 1;
for (long i = 1; i <= my numberOfColumns; i++) {
long colpos = newpos;
if (icol[i] > 0) {
if (inewcol == newpos) {
inewcol++;
}
colpos = inewcol;
inewcol++;
TableOfReal_setColumnLabel (thee.get(), colpos, my columnLabels[i]);
}
for (long j = 1; j <= my numberOfRows; j++) {
thy data[j][colpos] += my data[j][i];
}
}
return thee;
} catch (MelderError) {
Melder_throw (me, U": responses not grouped.");
}
}
void ObjectScene::syncQuad(Kite::KQuadCom *Quad) {
auto pixItem = (QGraphicsPixmapItem *)Quad->getSceneItem();
Kite::KRectF32 brect;
Quad->getBoundingRect(brect);
if (!Quad->getAtlasTextureArray().str.empty()) {
Kite::KAtlasTextureArray *tarray = nullptr;
emit(tarray = (Kite::KAtlasTextureArray *)requestResource(Quad->getAtlasTextureArray().str.c_str()));
if (tarray) {
auto atex = tarray->getItem(Quad->getTextureArrayIndex());
if (atex) {
Kite::KTexture *tex = atex->getTexture();
if (tex) {
Kite::KImage image;
tex->getImage(image);
QImage qimage(image.getPixelsData(), image.getWidth(), image.getHeight(), QImage::Format::Format_RGBA8888);
auto procimgae = qimage.copy(Quad->getAtlasItem().xpos, Quad->getAtlasItem().ypos,
Quad->getAtlasItem().width, Quad->getAtlasItem().height)
.mirrored(Quad->getAtlasItem().getFlipH(), !Quad->getAtlasItem().getFlipV())
.scaled(Quad->getWidth(), Quad->getHeight());
//blend
if (Quad->getBlendColor() != Kite::KColor(Kite::Colors::WHITE)) {
QColor col(Quad->getBlendColor().getR(), Quad->getBlendColor().getG(),
Quad->getBlendColor().getB(), Quad->getBlendColor().getA());
auto alpha = procimgae.alphaChannel();
for (int x = 0; x != procimgae.width(); ++x) {
for (int y(0); y != procimgae.height(); ++y) {
if (qAlpha(procimgae.pixel(x, y)) == 0) continue; // transparrent pixels
QColor icol(procimgae.pixel(x, y));
icol.setRed(BLEND_Multiply(icol.red(), col.red()));
icol.setBlue(BLEND_Multiply(icol.blue(), col.blue()));
icol.setGreen(BLEND_Multiply(icol.green(), col.green()));
procimgae.setPixel(x, y, icol.rgb());
}
}
procimgae.setAlphaChannel(alpha);
}
pixItem->setPixmap(QPixmap::fromImage(procimgae));
pixItem->setOpacity(Quad->getBlendColor().getGLA());
return;
}
}
}
}
QPixmap pm(Quad->getWidth(), Quad->getHeight());
pm.fill(QColor(Quad->getBlendColor().getR(), Quad->getBlendColor().getG(), Quad->getBlendColor().getB(), Quad->getBlendColor().getA()));
pixItem->setPixmap(pm);
pixItem->setOpacity(Quad->getBlendColor().getGLA());
}
Record::FieldNames DBResult::getFieldNames() const
{
FieldNames result;
size_t nbcol(getNbColumns());
for(size_t icol(0); icol<nbcol; ++icol)
{
result.push_back(getColumnName(icol));
}
return result;
}
bool Model::
computeJacobian(taoDNode const * node,
double gx, double gy, double gz,
Matrix & jacobian) const
{
if ( ! node) {
return false;
}
#ifdef DEBUG
fprintf(stderr, "computeJacobian()\ng: [% 4.2f % 4.2f % 4.2f]\n", gx, gy, gz);
#endif // DEBUG
// \todo Implement support for more than one joint per node, and
// more than one DOF per joint.
jacobian.resize(6, ndof_);
for (size_t icol(0); icol < ndof_; ++icol) {
deVector6 Jg_col; // in NDOF case, this will become an array of deVector6...
// in NOJ case, we will have to loop over all joints of a node...
kgm_joints_[icol]->getJgColumns(&Jg_col);
#ifdef DEBUG
fprintf(stderr, "iJg[%zu]: [ % 4.2f % 4.2f % 4.2f % 4.2f % 4.2f % 4.2f]\n",
icol,
Jg_col.elementAt(0), Jg_col.elementAt(1), Jg_col.elementAt(2),
Jg_col.elementAt(3), Jg_col.elementAt(4), Jg_col.elementAt(5));
#endif // DEBUG
for (size_t irow(0); irow < 6; ++irow) {
jacobian.coeffRef(irow, icol) = Jg_col.elementAt(irow);
}
// Add the effect of the joint rotation on the translational
// velocity at the global point (column-wise cross product with
// [gx;gy;gz]). Note that Jg_col.elementAt(3) is the
// contribution to omega_x etc, because the upper 3 elements of
// Jg_col are v_x etc. (And don't ask me why we have to
// subtract the cross product, it probably got inverted
// somewhere)
jacobian.coeffRef(0, icol) -= -gz * Jg_col.elementAt(4) + gy * Jg_col.elementAt(5);
jacobian.coeffRef(1, icol) -= gz * Jg_col.elementAt(3) - gx * Jg_col.elementAt(5);
jacobian.coeffRef(2, icol) -= -gy * Jg_col.elementAt(3) + gx * Jg_col.elementAt(4);
#ifdef DEBUG
fprintf(stderr, "0Jg[%zu]: [ % 4.2f % 4.2f % 4.2f % 4.2f % 4.2f % 4.2f]\n",
icol,
jacobian.coeff(0, icol), jacobian.coeff(1, icol), jacobian.coeff(2, icol),
jacobian.coeff(3, icol), jacobian.coeff(4, icol), jacobian.coeff(5, icol));
#endif // DEBUG
}
return true;
}
bool Model::
getInverseMassInertia(Matrix & inverse_mass_inertia) const
{
if (ainv_upper_triangular_.empty()) {
return false;
}
inverse_mass_inertia.resize(ndof_, ndof_);
for (size_t irow(0); irow < ndof_; ++irow) {
for (size_t icol(0); icol <= irow; ++icol) {
inverse_mass_inertia.coeffRef(irow, icol) = ainv_upper_triangular_[squareToTriangularIndex(irow, icol, ndof_)];
if (irow != icol) {
inverse_mass_inertia.coeffRef(icol, irow) = inverse_mass_inertia.coeff(irow, icol);
}
}
}
return true;
}
T GaussJordan(VVT &a, VVT &b) {
const int n = a.size();
const int m = b[0].size();
VI irow(n), icol(n), ipiv(n);
T det = 1;
for (int i = 0; i < n; i++) {
int pj = -1, pk = -1;
for (int j = 0; j < n; j++) if (!ipiv[j])
for (int k = 0; k < n; k++) if (!ipiv[k])
if (pj == -1 || fabs(a[j][k]) > fabs(a[pj][pk])) { pj = j; pk = k; }
if (fabs(a[pj][pk]) < EPS) { return 0; }
ipiv[pk]++;
swap(a[pj], a[pk]);
swap(b[pj], b[pk]);
if (pj != pk) det *= -1;
irow[i] = pj;
icol[i] = pk;
T c = 1.0 / a[pk][pk];
det *= a[pk][pk];
a[pk][pk] = 1.0;
for (int p = 0; p < n; p++) a[pk][p] *= c;
for (int p = 0; p < m; p++) b[pk][p] *= c;
for (int p = 0; p < n; p++) if (p != pk) {
c = a[p][pk];
a[p][pk] = 0;
for (int q = 0; q < n; q++) a[p][q] -= a[pk][q] * c;
for (int q = 0; q < m; q++) b[p][q] -= b[pk][q] * c;
}
}
for (int p = n-1; p >= 0; p--) if (irow[p] != icol[p]) {
for (int k = 0; k < n; k++) swap(a[k][irow[p]], a[k][icol[p]]);
}
return det;
}
void Bayes::sample_muOmega() {
double tau = 10.0;
int d0 = p + 2;
// matrix_t S0(p, p);
ublas::matrix<double> S0(p, p);
S0 = d0 * ublas::identity_matrix<double>(p, p)/4.0;
std::vector<int> idx;
ublas::indirect_array<> irow(p);
// projection - want every row
for (size_t i=0; i<irow.size(); ++i)
irow(i) = i;
// size_t rank;
ublas::matrix<double> S(p, p);
// matrix_t SS(p, p);
ublas::matrix<double> SS(p, p);
ublas::matrix<double> Omega_inv(p, p);
// identity matrix for inverting cholesky factorization
ublas::matrix<double> I(p, p);
I.assign(ublas::identity_matrix<double> (p, p));
ublas::symmetric_adaptor<ublas::matrix<double>, ublas::upper> SH(I);
// triangular matrix for cholesky_decompose
ublas::triangular_matrix<double, ublas::lower, ublas::row_major> L(p, p);
int df;
for (int j=0; j<k; ++j) {
idx = find_all(z, j);
int n_idx = idx.size();
ublas::matrix<double> xx(p, n_idx);
ublas::matrix<double> e(p, n_idx);
// ublas::matrix<double> m(p, 1);
ublas::vector<double> m(p);
ublas::indirect_array<> icol(n_idx);
for (size_t i=0; i<idx.size(); ++i)
icol(i) = idx[i];
if (n_idx > 0) {
double a = tau/(1.0 + n_idx*tau);
//! REFACTOR - should be able to do matrix_sum directly on projection rather than make a copy?
xx.assign(project(x, irow, icol));
m.assign(ublas::matrix_sum(xx, 1)/n_idx);
// e.assign(xx - outer_prod(column(m, 0), ublas::scalar_vector<double>(n_idx, 1)));
e.assign(xx - outer_prod(m, ublas::scalar_vector<double>(n_idx, 1)));
S.assign(prod(e, trans(e)) + outer_prod(m, m) * n_idx * a/tau);
// SS = trans(S) + S0;
SS = S + S0;
df = d0 + n_idx;
// Omega(j).assign(wishart_rnd(df, SS));
Omega(j).assign(wishart_InvA_rnd(df, SS));
SH.assign(Omega(j));
cholesky_decompose(SH, L);
Omega_inv.assign(solve(SH, I, ublas::upper_tag()));
mu(j).assign(a*n_idx*m + sqrt(a)*prod(Omega_inv, Rmath::rnorm(0, 1, p)));
} else {
// Omega(j).assign(wishart_rnd(d0, S0));
Omega(j).assign(wishart_InvA_rnd(d0, S0));
SH.assign(Omega(j));
cholesky_decompose(SH, L);
Omega_inv.assign(solve(SH, I, ublas::upper_tag()));
mu(j).assign(sqrt(tau) * prod(Omega_inv, Rmath::rnorm(0, 1, p)));
}
}
}
