Eigen::MatrixXd Kinematics::transform(double x, double y, double z, double roll, double pitch, double yaw) const{
Eigen::MatrixXd trans(4, 4);
trans << 1.0, 0.0, 0.0, x,
0.0, 1.0, 0.0, y,
0.0, 0.0, 1.0, z,
0.0, 0.0, 0.0, 1.0;
Eigen::MatrixXd ro(4, 4);
ro << 1.0, 0.0, 0.0, 0.0,
0.0, cos(roll), -sin(roll), 0.0,
0.0, sin(roll), cos(roll), 0.0,
0.0, 0.0, 0.0, 1.0;
Eigen::MatrixXd pi(4, 4);
pi << cos(pitch), 0.0, sin(pitch), 0.0,
0.0, 1.0, 0.0, 0.0,
-sin(pitch), 0.0, cos(pitch), 0.0,
0.0, 0.0, 0.0, 1.0;
Eigen::MatrixXd ya(4, 4);
ya << cos(yaw), -sin(yaw), 0.0, 0.0,
sin(yaw), cos(yaw), 0.0, 0.0,
0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 1.0;
Eigen::MatrixXd res = ro * pi * ya * trans;
return res;
}
ExecStatus
Val<View,Offset,shared>::propagate(Space& home, const ModEventDelta&) {
Region r(home);
ProcessStack xa(r,n);
ProcessStack ya(r,n);
ValInfo<View>* x = xy;
ValInfo<View>* y = xy+n;
// Scan x and y for assigned but not yet propagated views
for (int i = n; i--; ) {
if (x[i].doval()) xa.push(i);
if (y[i].doval()) ya.push(i);
}
do {
// Propagate assigned views for x
GECODE_ES_CHECK((prop_val<View,Offset,ValInfo<View> >
(home,n,x,ox,y,oy,n_na,xa,ya)));
// Propagate assigned views for y
GECODE_ES_CHECK((prop_val<View,Offset,ValInfo<View> >
(home,n,y,oy,x,ox,n_na,ya,xa)));
assert(ya.empty());
} while (!xa.empty());
if (n_na == 0)
return home.ES_SUBSUMED(*this);
return shared ? ES_NOFIX : ES_FIX;
}
// Fit model to this random selection of data points.
void
vpHomography::computeTransformation(vpColVector &x, unsigned int *ind, vpColVector &M)
{
unsigned int i ;
unsigned int n = x.getRows()/4 ;
std::vector<double> xa(4), xb(4);
std::vector<double> ya(4), yb(4);
unsigned int n2 = n * 2;
unsigned int ind2;
for(i=0 ; i < 4 ; i++)
{
ind2 = 2 * ind[i];
xb[i] = x[ind2] ;
yb[i] = x[ind2+1] ;
xa[i] = x[n2+ind2] ;
ya[i] = x[n2+ind2+1] ;
}
vpHomography aHb ;
try {
vpHomography::HLM(xb, yb, xa, ya, true, aHb);
}
catch(...)
{
aHb.setIdentity();
}
M.resize(9);
for (i=0 ; i <9 ; i++)
{
M[i] = aHb.data[i] ;
}
aHb /= aHb[2][2] ;
}
void
channel(Home home, const IntVarArgs& x, const SetVarArgs& y) {
if (home.failed()) return;
ViewArray<Int::CachedView<Int::IntView> > xa(home,x.size());
for (int i=x.size(); i--;)
new (&xa[i]) Int::CachedView<Int::IntView>(x[i]);
ViewArray<Set::CachedView<Set::SetView> > ya(home,y.size());
for (int i=y.size(); i--;)
new (&ya[i]) Set::CachedView<Set::SetView>(y[i]);
GECODE_ES_FAIL((Set::Int::ChannelInt<Set::SetView>::post(home,xa,ya)));
}
void apply( stk_classic::mesh::Bucket::iterator bucket_i ,
stk_classic::mesh::Bucket::iterator bucket_j ,
reduce_type * inout ) const
{
stk_classic::mesh::BucketArray<field_type> xa( field_x , bucket_i , bucket_j );
stk_classic::mesh::BucketArray<field_type> ya( field_y , bucket_i , bucket_j );
scalar * xi = xa.contiguous_data();
scalar * yi = ya.contiguous_data();
scalar * ye = yi + ya.size();
scalar tmp = 0 ;
while ( yi != ye ) { tmp += *xi++ * *yi++ ; }
*inout += tmp ;
}
void apply( stk_classic::mesh::Bucket::iterator bucket_i ,
stk_classic::mesh::Bucket::iterator bucket_j ) const
{
typedef typename stk_classic::mesh::FieldTraits<field_type>::data_type scalar ;
stk_classic::mesh::BucketArray<field_type> xa( x_ , bucket_i , bucket_j );
stk_classic::mesh::BucketArray<field_type> ya( y_ , bucket_i , bucket_j );
scalar * xi = xa.contiguous_data();
scalar * yi = ya.contiguous_data();
scalar * ye = yi + ya.size();
//is it worthwhile to optimize by adding special loops for
//cases where alpha or beta equal 1.0 or 0.0, or where x==y?
while(yi < ye) {
*yi = alpha_* *xi++ + beta_* *yi;
++yi;
}
}
void createAtomicLib() {
typedef CG<double> CGD;
typedef AD<CGD> ADCGD;
/**
* Tape model
*/
std::vector<ADCGD> xa(na_);
for (size_t j = 0; j < na_; j++)
xa[j] = xa_[j];
CppAD::Independent(xa);
std::vector<ADCGD> ya(ns_);
atomicFunction(xa, ya);
ADFun<CGD> fun;
fun.Dependent(ya);
/**
* Compile
*/
std::string lName = getAtomicLibName()+(ignoreParameters_ ? "" : "All");
ModelCSourceGen<double> compHelpL(fun, lName);
compHelpL.setCreateForwardZero(true);
compHelpL.setCreateForwardOne(true);
compHelpL.setCreateReverseOne(true);
compHelpL.setCreateReverseTwo(true);
compHelpL.setTypicalIndependentValues(xa_);
if (ignoreParameters_) {
std::vector<std::set<size_t> > jacSparAll = extra::jacobianSparsitySet<std::vector<std::set<size_t> > >(fun);
std::vector<std::set<size_t> > jacSpar(jacSparAll.size());
for (size_t i = 0; i < jacSparAll.size(); i++) {
// only differential information for states and controls
std::set<size_t>::const_iterator itEnd = jacSparAll[i].upper_bound(ns_ + nm_ - 1);
if (itEnd != jacSparAll[i].begin())
jacSpar[i].insert(jacSparAll[i].begin(), itEnd);
}
compHelpL.setCustomSparseJacobianElements(jacSpar);
std::vector<std::set<size_t> > hessSparAll = extra::hessianSparsitySet<std::vector<std::set<size_t> > >(fun);
std::vector<std::set<size_t> > hessSpar(hessSparAll.size());
for (size_t i = 0; i < ns_ + nm_; i++) {
std::set<size_t>::const_iterator it = hessSparAll[i].upper_bound(i); // only the lower left side
if (it != hessSparAll[i].begin())
hessSpar[i].insert(hessSparAll[i].begin(), it);
}
compHelpL.setCustomSparseHessianElements(hessSpar);
}
ModelLibraryCSourceGen<double> compDynHelpL(compHelpL);
compDynHelpL.setVerbose(verbose_);
SaveFilesModelLibraryProcessor<double>::saveLibrarySourcesTo(compDynHelpL, "sources_" + lName);
DynamicModelLibraryProcessor<double> p(compDynHelpL, lName);
GccCompiler<double> compiler;
if (!compilerFlags_.empty())
compiler.setCompileFlags(compilerFlags_);
atomicDynamicLib_.reset(p.createDynamicLibrary(compiler));
/**
* load the model
*/
atomicModel_.reset(atomicDynamicLib_->model(lName));
}
QQuickStateOperation::ActionList QQuickParentChange::actions()
{
Q_D(QQuickParentChange);
if (!d->target || !d->parent)
return ActionList();
ActionList actions;
QQuickAction a;
a.event = this;
actions << a;
if (d->xString.isValid()) {
bool ok = false;
qreal x = d->xString.value.numberLiteral(&ok);
if (ok) {
QQuickAction xa(d->target, QLatin1String("x"), x);
actions << xa;
} else {
QQmlBinding *newBinding = new QQmlBinding(d->xString.value, d->target, qmlContext(this));
QQmlProperty property(d->target, QLatin1String("x"));
newBinding->setTarget(property);
QQuickAction xa;
xa.property = property;
xa.toBinding = QQmlAbstractBinding::getPointer(newBinding);
xa.fromValue = xa.property.read();
xa.deletableToBinding = true;
actions << xa;
}
}
if (d->yString.isValid()) {
bool ok = false;
qreal y = d->yString.value.numberLiteral(&ok);
if (ok) {
QQuickAction ya(d->target, QLatin1String("y"), y);
actions << ya;
} else {
QQmlBinding *newBinding = new QQmlBinding(d->yString.value, d->target, qmlContext(this));
QQmlProperty property(d->target, QLatin1String("y"));
newBinding->setTarget(property);
QQuickAction ya;
ya.property = property;
ya.toBinding = QQmlAbstractBinding::getPointer(newBinding);
ya.fromValue = ya.property.read();
ya.deletableToBinding = true;
actions << ya;
}
}
if (d->scaleString.isValid()) {
bool ok = false;
qreal scale = d->scaleString.value.numberLiteral(&ok);
if (ok) {
QQuickAction sa(d->target, QLatin1String("scale"), scale);
actions << sa;
} else {
QQmlBinding *newBinding = new QQmlBinding(d->scaleString.value, d->target, qmlContext(this));
QQmlProperty property(d->target, QLatin1String("scale"));
newBinding->setTarget(property);
QQuickAction sa;
sa.property = property;
sa.toBinding = QQmlAbstractBinding::getPointer(newBinding);
sa.fromValue = sa.property.read();
sa.deletableToBinding = true;
actions << sa;
}
}
if (d->rotationString.isValid()) {
bool ok = false;
qreal rotation = d->rotationString.value.numberLiteral(&ok);
if (ok) {
QQuickAction ra(d->target, QLatin1String("rotation"), rotation);
actions << ra;
} else {
QQmlBinding *newBinding = new QQmlBinding(d->rotationString.value, d->target, qmlContext(this));
QQmlProperty property(d->target, QLatin1String("rotation"));
newBinding->setTarget(property);
QQuickAction ra;
ra.property = property;
ra.toBinding = QQmlAbstractBinding::getPointer(newBinding);
ra.fromValue = ra.property.read();
ra.deletableToBinding = true;
actions << ra;
}
}
if (d->widthString.isValid()) {
bool ok = false;
qreal width = d->widthString.value.numberLiteral(&ok);
if (ok) {
QQuickAction wa(d->target, QLatin1String("width"), width);
actions << wa;
} else {
QQmlBinding *newBinding = new QQmlBinding(d->widthString.value, d->target, qmlContext(this));
QQmlProperty property(d->target, QLatin1String("width"));
newBinding->setTarget(property);
QQuickAction wa;
wa.property = property;
wa.toBinding = QQmlAbstractBinding::getPointer(newBinding);
wa.fromValue = wa.property.read();
wa.deletableToBinding = true;
actions << wa;
}
}
if (d->heightString.isValid()) {
bool ok = false;
qreal height = d->heightString.value.numberLiteral(&ok);
if (ok) {
QQuickAction ha(d->target, QLatin1String("height"), height);
actions << ha;
} else {
QQmlBinding *newBinding = new QQmlBinding(d->heightString.value, d->target, qmlContext(this));
QQmlProperty property(d->target, QLatin1String("height"));
newBinding->setTarget(property);
QQuickAction ha;
ha.property = property;
ha.toBinding = QQmlAbstractBinding::getPointer(newBinding);
ha.fromValue = ha.property.read();
ha.deletableToBinding = true;
actions << ha;
}
}
return actions;
}
