array ADFun<Base>::Hessian(array& x, array& w)
{ vec<Base> x_vec(x);
vec<Base> w_vec(w);
vec<Base> result = f_.Hessian(x_vec, w_vec);
// Kludge: return a vector which is reshaped by cppad.py
return vec2array(result);
}
void update_arrows() {
geometry_msgs::Point origin, arrow_x_tip, arrow_y_tip, arrow_z_tip;
Eigen::Matrix3d R;
Eigen::Quaterniond quat;
quat.x() = g_stamped_pose.pose.orientation.x;
quat.y() = g_stamped_pose.pose.orientation.y;
quat.z() = g_stamped_pose.pose.orientation.z;
quat.w() = g_stamped_pose.pose.orientation.w;
R = quat.toRotationMatrix();
Eigen::Vector3d x_vec, y_vec, z_vec;
double veclen = 0.2; //make the arrows this long
x_vec = R.col(0) * veclen;
y_vec = R.col(1) * veclen;
z_vec = R.col(2) * veclen;
//update the arrow markers w/ new pose:
origin = g_stamped_pose.pose.position;
arrow_x_tip = origin;
arrow_x_tip.x += x_vec(0);
arrow_x_tip.y += x_vec(1);
arrow_x_tip.z += x_vec(2);
arrow_marker_x.points.clear();
arrow_marker_x.points.push_back(origin);
arrow_marker_x.points.push_back(arrow_x_tip);
arrow_marker_x.header = g_stamped_pose.header;
arrow_y_tip = origin;
arrow_y_tip.x += y_vec(0);
arrow_y_tip.y += y_vec(1);
arrow_y_tip.z += y_vec(2);
arrow_marker_y.points.clear();
arrow_marker_y.points.push_back(origin);
arrow_marker_y.points.push_back(arrow_y_tip);
arrow_marker_y.header = g_stamped_pose.header;
arrow_z_tip = origin;
arrow_z_tip.x += z_vec(0);
arrow_z_tip.y += z_vec(1);
arrow_z_tip.z += z_vec(2);
arrow_marker_z.points.clear();
arrow_marker_z.points.push_back(origin);
arrow_marker_z.points.push_back(arrow_z_tip);
arrow_marker_z.header = g_stamped_pose.header;
}
PetscErrorCode __feel_petsc_preconditioner_apply( void *ctx, Vec x, Vec y )
{
Preconditioner<double> * preconditioner = static_cast<Preconditioner<double>*>( ctx );
VectorPetsc<double> x_vec( x );
VectorPetsc<double> y_vec( y );
preconditioner->apply( x_vec,y_vec );
return 0;
}
PetscErrorCode __feel_petsc_preconditioner_apply( PC pc, Vec x, Vec y )
{
void *ctx;
PetscErrorCode ierr = PCShellGetContext( pc,&ctx );
CHKERRQ( ierr );
Preconditioner<double> * preconditioner = static_cast<Preconditioner<double>*>( ctx );
VectorPetsc<double> x_vec( x );
VectorPetsc<double> y_vec( y );
preconditioner->apply( x_vec,y_vec );
return 0;
}
void Vector3<T>::rotate_inverse(enum Rotation rotation)
{
Vector3<T> x_vec(1.0f,0.0f,0.0f);
Vector3<T> y_vec(0.0f,1.0f,0.0f);
Vector3<T> z_vec(0.0f,0.0f,1.0f);
x_vec.rotate(rotation);
y_vec.rotate(rotation);
z_vec.rotate(rotation);
Matrix3<T> M(
x_vec.x, y_vec.x, z_vec.x,
x_vec.y, y_vec.y, z_vec.y,
x_vec.z, y_vec.z, z_vec.z
);
(*this) = M.mul_transpose(*this);
}
Matrixd NavHandler::getMovement() {
static bool inAir = false;
Vec3 x_vec(MAX_VELOCITY,0,0);
Vec3 y_vec(0,MAX_VELOCITY,0);
Vec3 mov;
if (goLeft) mov -= x_vec;
if (goRight) mov += x_vec;
if (goUp) mov += y_vec;
if (goDown) mov -= y_vec;
if (jumping && !inAir) {
std::cout << "Jumping\n";
inAir = true;
//bh->setLinearVelocity( person, Vec3(0,0,MAX_VELOCITY) );
} else if (!jumping && inAir) {
std::cout << "Can Jump\n";
inAir = false;
}
bh->translate( person, mov );
Matrixd m;
bh->getWorldTransform( person, m );
return (m);
}
array ADFun<Base>::Jacobian(array& x)
{ vec<Base> x_vec(x);
vec<Base> result = f_.Jacobian(x_vec);
// Kludge: return a vector which is reshaped by cppad.py
return vec2array(result);
}
ADFun<Base>::ADFun(array& x_array, array& y_array)
{ vec< CppAD::AD<Base> > x_vec(x_array);
vec< CppAD::AD<Base> > y_vec(y_array);
f_.Dependent(x_vec, y_vec);
}
void ExampleTridiagSerialLinearOp<Scalar>::applyImpl(
const Thyra::EOpTransp M_trans,
const Thyra::MultiVectorBase<Scalar> &X_in,
const Teuchos::Ptr<Thyra::MultiVectorBase<Scalar> > &Y_inout,
const Scalar alpha,
const Scalar beta
) const
{
typedef Teuchos::ScalarTraits<Scalar> ST;
typedef Thyra::Ordinal Ordinal;
const Ordinal dim = space_->dim();
// Loop over the input columns
const Ordinal m = X_in.domain()->dim();
for (Ordinal col_j = 0; col_j < m; ++col_j) {
// Get access the the elements of column j
Thyra::ConstDetachedVectorView<Scalar> x_vec(X_in.col(col_j));
Thyra::DetachedVectorView<Scalar> y_vec(Y_inout->col(col_j));
const Teuchos::ArrayRCP<const Scalar> x = x_vec.sv().values();
const Teuchos::ArrayRCP<Scalar> y = y_vec.sv().values();
// Perform y = beta*y (being careful to set y=0 if beta=0 in case y is
// uninitialized on input!)
if( beta == ST::zero() ) {
for( Ordinal k = 0; k < dim; ++k ) y[k] = ST::zero();
}
else if( beta != ST::one() ) {
for( Ordinal k = 0; k < dim; ++k ) y[k] *= beta;
}
// Perform y = alpha*op(M)*x
Ordinal k = 0;
if( M_trans == Thyra::NOTRANS ) {
y[k] += alpha * ( diag_[k]*x[k] + upper_[k]*x[k+1] ); // First row
for( k = 1; k < dim - 1; ++k ) // Middle rows
y[k] += alpha * ( lower_[k-1]*x[k-1] + diag_[k]*x[k] + upper_[k]*x[k+1] );
y[k] += alpha * ( lower_[k-1]*x[k-1] + diag_[k]*x[k] ); // Last row
}
else if( M_trans == Thyra::CONJ ) {
y[k] += alpha * ( ST::conjugate(diag_[k])*x[k] + ST::conjugate(upper_[k])*x[k+1] );
for( k = 1; k < dim - 1; ++k )
y[k] += alpha * ( ST::conjugate(lower_[k-1])*x[k-1]
+ ST::conjugate(diag_[k])*x[k] + ST::conjugate(upper_[k])*x[k+1] );
y[k] += alpha * ( ST::conjugate(lower_[k-1])*x[k-1] + ST::conjugate(diag_[k])*x[k] );
}
else if( M_trans == Thyra::TRANS ) {
y[k] += alpha * ( diag_[k]*x[k] + lower_[k]*x[k+1] );
for( k = 1; k < dim - 1; ++k )
y[k] += alpha * ( upper_[k-1]*x[k-1] + diag_[k]*x[k] + lower_[k]*x[k+1] );
y[k] += alpha * ( upper_[k-1]*x[k-1] + diag_[k]*x[k] );
}
else if( M_trans == Thyra::CONJTRANS ) {
y[k] += alpha * ( ST::conjugate(diag_[k])*x[k] + ST::conjugate(lower_[k])*x[k+1] );
for( k = 1; k < dim - 1; ++k )
y[k] += alpha * ( ST::conjugate(upper_[k-1])*x[k-1]
+ ST::conjugate(diag_[k])*x[k] + ST::conjugate(lower_[k])*x[k+1] );
y[k] += alpha * ( ST::conjugate(upper_[k-1])*x[k-1] + ST::conjugate(diag_[k])*x[k] );
}
else {
TEUCHOS_TEST_FOR_EXCEPT(true); // Throw exception if we get here!
}
}
}
