//============================================================================== void FreeJoint::integratePositions(double _dt) { const Eigen::Isometry3d Qnext = getQ() * convertToTransform(getVelocitiesStatic() * _dt); setPositionsStatic(convertToPositions(Qnext)); }
//============================================================================== void FreeJoint::setRelativeTransform(const Eigen::Isometry3d& newTransform) { setPositionsStatic(convertToPositions( Joint::mAspectProperties.mT_ParentBodyToJoint.inverse() * newTransform * Joint::mAspectProperties.mT_ChildBodyToJoint)); }
//============================================================================== void BallJoint::integratePositions(double _dt) { Eigen::Matrix3d Rnext = getR().linear() * convertToRotation(getVelocitiesStatic() * _dt); setPositionsStatic(convertToPositions(Rnext)); }
//============================================================================== Eigen::Vector3d BallJoint::getPositionDifferencesStatic( const Eigen::Vector3d& _q2, const Eigen::Vector3d& _q1) const { const Eigen::Matrix3d R1 = convertToRotation(_q1); const Eigen::Matrix3d R2 = convertToRotation(_q2); return convertToPositions(R1.transpose() * R2); }
//============================================================================== Eigen::Vector6d FreeJoint::getPositionDifferencesStatic( const Eigen::Vector6d& _q2, const Eigen::Vector6d& _q1) const { const Eigen::Isometry3d T1 = convertToTransform(_q1); const Eigen::Isometry3d T2 = convertToTransform(_q2); return convertToPositions(T1.inverse() * T2); }
//============================================================================== 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)); }