//============================================================================== void FreeJoint::integratePositions(double _dt) { const Eigen::Isometry3d Qnext = getQ() * convertToTransform(getVelocitiesStatic() * _dt); setPositionsStatic(convertToPositions(Qnext)); }
//============================================================================== void BallJoint::integratePositions(double _dt) { Eigen::Matrix3d Rnext = getR().linear() * convertToRotation(getVelocitiesStatic() * _dt); setPositionsStatic(convertToPositions(Rnext)); }
//============================================================================== void FreeJoint::setRelativeTransform(const Eigen::Isometry3d& newTransform) { setPositionsStatic(convertToPositions( Joint::mAspectProperties.mT_ParentBodyToJoint.inverse() * newTransform * Joint::mAspectProperties.mT_ChildBodyToJoint)); }
//============================================================================== void FreeJoint::integratePositions(double _dt) { const Eigen::Vector6d& velocities = getVelocitiesStatic(); mQ.linear() = mQ.linear() * math::expMapRot( getLocalJacobianStatic().topRows<3>() * velocities * _dt); mQ.translation() = mQ.translation() + velocities.tail<3>() * _dt; setPositionsStatic(convertToPositions(mQ)); }