SpatialMotionVector SpatialMotionVector::cross(const SpatialMotionVector& other) const
{
SpatialMotionVector res;
res.getLinearVec3() = this->angularVec3.cross(other.getLinearVec3()) + this->linearVec3.cross(other.getAngularVec3());
res.getAngularVec3() = this->angularVec3.cross(other.getAngularVec3());
return res;
}
SpatialMotionVector PositionRaw::changePointOf(const SpatialMotionVector & other) const
{
SpatialMotionVector 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 + thisPos.cross(otherAngular);
resAngular = otherAngular;
return result;
}
void checkInertiaAccProduct(const ArticulatedBodyInertia & inertia, const SpatialMotionVector & twist)
{
SpatialForceVector momentum = inertia*twist;
Vector6 momentumCheck;
Matrix6x6 I = inertia.asMatrix();
Vector6 twistPlain = twist.asVector();
toEigen(momentumCheck) = toEigen(I)*toEigen(twistPlain);
ASSERT_EQUAL_VECTOR(momentum.asVector(),momentumCheck);
}
SpatialMotionVector TransformDerivative::transform(const Transform& transform,
SpatialMotionVector& other)
{
SpatialMotionVector 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.getAngularVec3()) = dR*toEigen(other.getAngularVec3());
toEigen(ret.getLinearVec3()) = dR*toEigen(other.getLinearVec3())
+ p.cross(toEigen(ret.getAngularVec3()))
+ dp.cross(R*toEigen(other.getAngularVec3()));
return ret;
}