C++ (Cpp) SpatialForceVector::getLinearVec3示例

示例#1

文件： SpatialMotionVector.cpp 项目： smithjoshua001/idyntree

SpatialForceVector SpatialMotionVector::cross(const SpatialForceVector& other) const
{
    SpatialForceVector res;

    res.getLinearVec3()  = this->angularVec3.cross(other.getLinearVec3());
    res.getAngularVec3() = this->linearVec3.cross(other.getLinearVec3()) + this->angularVec3.cross(other.getAngularVec3());

    return res;
}

示例#2

文件： PositionRaw.cpp 项目： robotology/idyntree

SpatialForceVector PositionRaw::changePointOf(const SpatialForceVector & other) const
{
    SpatialForceVector result;

    Eigen::Map<const Eigen::Vector3d> thisPos(this->data());
    Eigen::Map<const Eigen::Vector3d> otherLinear(other.getLinearVec3().data());
    Eigen::Map<const Eigen::Vector3d> otherAngular(other.getAngularVec3().data());
    Eigen::Map<Eigen::Vector3d> resLinear(result.getLinearVec3().data());
    Eigen::Map<Eigen::Vector3d> resAngular(result.getAngularVec3().data());

    resLinear  = otherLinear;
    resAngular = thisPos.cross(otherLinear) + otherAngular;

    return result;
}

示例#3

文件： TransformDerivative.cpp 项目： PerryZh/idyntree

SpatialForceVector TransformDerivative::transform(const Transform& transform,
                                                  SpatialForceVector& other)
{
    SpatialForceVector ret;

    Eigen::Map<const Eigen::Vector3d> p(transform.getPosition().data());
    Eigen::Map<const Eigen::Matrix<double,3,3,Eigen::RowMajor> > R(transform.getRotation().data());
    Eigen::Map<const Eigen::Vector3d> dp(this->posDerivative.data());
    Eigen::Map<const Eigen::Matrix<double,3,3,Eigen::RowMajor> > dR(this->rotDerivative.data());

    toEigen(ret.getLinearVec3()) = dR*toEigen(other.getLinearVec3());
    toEigen(ret.getAngularVec3())  =  dR*toEigen(other.getAngularVec3())
                                    + p.cross(toEigen(ret.getLinearVec3()))
                                    + dp.cross(R*toEigen(other.getLinearVec3()));

    return ret;
}