Unverified Commit 4f7332c6 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

core: fix cast

parent 9e69f005
//
// Copyright (c) 2016-2018 CNRS
// Copyright (c) 2016-2020 CNRS INRIA
//
#ifndef __pinocchio_vector_space_operation_hpp__
#define __pinocchio_vector_space_operation_hpp__
#include <stdexcept>
#include "pinocchio/multibody/liegroup/liegroup-base.hpp"
#include <stdexcept>
#include <boost/integer/static_min_max.hpp>
namespace pinocchio
......@@ -80,10 +79,10 @@ namespace pinocchio
template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
void dDifference_impl (const Eigen::MatrixBase<ConfigL_t> &,
const Eigen::MatrixBase<ConfigR_t> &,
const Eigen::MatrixBase<JacobianOut_t>& J) const
const Eigen::MatrixBase<JacobianOut_t> & J) const
{
if (arg == ARG0)
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J).noalias() = - JacobianMatrix_t::Identity();
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J) = -JacobianMatrix_t::Identity();
else if (arg == ARG1)
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J).setIdentity();
}
......@@ -104,9 +103,9 @@ namespace pinocchio
}
template <class Config_t, class Tangent_t, class JacobianOut_t>
static void dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
const Eigen::MatrixBase<Tangent_t> & /*v*/,
const Eigen::MatrixBase<JacobianOut_t>& J,
static void dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
const Eigen::MatrixBase<Tangent_t> & /*v*/,
const Eigen::MatrixBase<JacobianOut_t> & J,
const AssignmentOperatorType op)
{
Eigen::MatrixBase<JacobianOut_t>& Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
......@@ -116,10 +115,10 @@ namespace pinocchio
Jout.setIdentity();
break;
case ADDTO:
Jout.diagonal().array() += 1.;
Jout.diagonal().array() += Scalar(1);
break;
case RMTO:
Jout.diagonal().array() -= 1.;
Jout.diagonal().array() -= Scalar(1);
break;
default:
assert(false && "Wrong Op requesed value");
......@@ -128,9 +127,9 @@ namespace pinocchio
}
template <class Config_t, class Tangent_t, class JacobianOut_t>
static void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
const Eigen::MatrixBase<Tangent_t> & /*v*/,
const Eigen::MatrixBase<JacobianOut_t>& J,
static void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
const Eigen::MatrixBase<Tangent_t> & /*v*/,
const Eigen::MatrixBase<JacobianOut_t> & J,
const AssignmentOperatorType op)
{
Eigen::MatrixBase<JacobianOut_t>& Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
......@@ -140,10 +139,10 @@ namespace pinocchio
Jout.setIdentity();
break;
case ADDTO:
Jout.diagonal().array() += 1.;
Jout.diagonal().array() += Scalar(1);
break;
case RMTO:
Jout.diagonal().array() -= 1.;
Jout.diagonal().array() -= Scalar(1);
break;
default:
assert(false && "Wrong Op requesed value");
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment