#include <hpp/fcl/ccd/motion.h>
Public Member Functions | |
ScrewMotion () | |
Default transformations are all identities. | |
ScrewMotion (const Matrix3f &R1, const Vec3f &T1, const Matrix3f &R2, const Vec3f &T2) | |
Construct motion from the initial rotation/translation and goal rotation/translation. | |
ScrewMotion (const Transform3f &tf1_, const Transform3f &tf2_) | |
Construct motion from the initial transform and goal transform. | |
bool | integrate (double dt) const |
Integrate the motion from 0 to dt We compute the current transformation from zero point instead of from last integrate time, for precision. | |
FCL_REAL | computeMotionBound (const BVMotionBoundVisitor &mb_visitor) const |
Compute the motion bound for a bounding volume along a given direction n, which is defined in the visitor. | |
FCL_REAL | computeMotionBound (const TriangleMotionBoundVisitor &mb_visitor) const |
Compute the motion bound for a triangle along a given direction n, which is defined in the visitor. | |
void | getCurrentTransform (Transform3f &tf_) const |
Get the rotation and translation in current step. | |
void | getTaylorModel (TMatrix3 &tm, TVector3 &tv) const |
FCL_REAL | getLinearVelocity () const |
FCL_REAL | getAngularVelocity () const |
const Vec3f & | getAxis () const |
const Vec3f & | getAxisOrigin () const |
Protected Member Functions | |
void | computeScrewParameter () |
Quaternion3f | deltaRotation (FCL_REAL dt) const |
Quaternion3f | absoluteRotation (FCL_REAL dt) const |
Protected Attributes | |
Transform3f | tf1 |
The transformation at time 0. | |
Transform3f | tf2 |
The transformation at time 1. | |
Transform3f | tf |
The transformation at current time t. | |
Vec3f | axis |
screw axis | |
Vec3f | p |
A point on the axis S. | |
FCL_REAL | linear_vel |
linear velocity along the axis | |
FCL_REAL | angular_vel |
angular velocity |
fcl::ScrewMotion::ScrewMotion | ( | ) | [inline] |
Default transformations are all identities.
References angular_vel, axis, linear_vel, and fcl::Vec3fX< T >::setValue().
fcl::ScrewMotion::ScrewMotion | ( | const Matrix3f & | R1, |
const Vec3f & | T1, | ||
const Matrix3f & | R2, | ||
const Vec3f & | T2 | ||
) | [inline] |
Construct motion from the initial rotation/translation and goal rotation/translation.
References computeScrewParameter().
fcl::ScrewMotion::ScrewMotion | ( | const Transform3f & | tf1_, |
const Transform3f & | tf2_ | ||
) | [inline] |
Construct motion from the initial transform and goal transform.
References computeScrewParameter().
Quaternion3f fcl::ScrewMotion::absoluteRotation | ( | FCL_REAL | dt | ) | const [inline, protected] |
References deltaRotation(), fcl::Transform3f::getQuatRotation(), and tf1.
Referenced by integrate().
FCL_REAL fcl::ScrewMotion::computeMotionBound | ( | const BVMotionBoundVisitor & | mb_visitor | ) | const [inline, virtual] |
Compute the motion bound for a bounding volume along a given direction n, which is defined in the visitor.
Implements fcl::MotionBase.
References fcl::BVMotionBoundVisitor::visit().
FCL_REAL fcl::ScrewMotion::computeMotionBound | ( | const TriangleMotionBoundVisitor & | mb_visitor | ) | const [inline, virtual] |
Compute the motion bound for a triangle along a given direction n, which is defined in the visitor.
Implements fcl::MotionBase.
References fcl::TriangleMotionBoundVisitor::visit().
void fcl::ScrewMotion::computeScrewParameter | ( | ) | [inline, protected] |
Quaternion3f fcl::ScrewMotion::deltaRotation | ( | FCL_REAL | dt | ) | const [inline, protected] |
References angular_vel, axis, and fcl::Quaternion3f::fromAxisAngle().
Referenced by absoluteRotation(), and integrate().
FCL_REAL fcl::ScrewMotion::getAngularVelocity | ( | ) | const [inline] |
References angular_vel.
void fcl::ScrewMotion::getCurrentTransform | ( | Transform3f & | tf_ | ) | const [inline, virtual] |
FCL_REAL fcl::ScrewMotion::getLinearVelocity | ( | ) | const [inline] |
References linear_vel.
Implements fcl::MotionBase.
References angular_vel, axis, fcl::generateTaylorModelForCosFunc(), fcl::generateTaylorModelForLinearFunc(), fcl::generateTaylorModelForSinFunc(), fcl::Transform3f::getRotation(), fcl::MotionBase::getTimeInterval(), fcl::Transform3f::getTranslation(), fcl::hat(), linear_vel, p, and tf1.
bool fcl::ScrewMotion::integrate | ( | double | dt | ) | const [inline, virtual] |
Integrate the motion from 0 to dt We compute the current transformation from zero point instead of from last integrate time, for precision.
Implements fcl::MotionBase.
References absoluteRotation(), axis, deltaRotation(), fcl::Transform3f::getTranslation(), linear_vel, p, fcl::Transform3f::setQuatRotation(), fcl::Transform3f::setTranslation(), tf, tf1, and fcl::Quaternion3f::transform().
FCL_REAL fcl::ScrewMotion::angular_vel [protected] |
angular velocity
Referenced by computeScrewParameter(), deltaRotation(), getAngularVelocity(), getTaylorModel(), and ScrewMotion().
Vec3f fcl::ScrewMotion::axis [protected] |
screw axis
Referenced by computeScrewParameter(), deltaRotation(), getAxis(), getTaylorModel(), integrate(), and ScrewMotion().
FCL_REAL fcl::ScrewMotion::linear_vel [protected] |
linear velocity along the axis
Referenced by computeScrewParameter(), getLinearVelocity(), getTaylorModel(), integrate(), and ScrewMotion().
Vec3f fcl::ScrewMotion::p [protected] |
A point on the axis S.
Referenced by computeScrewParameter(), getAxisOrigin(), getTaylorModel(), and integrate().
Transform3f fcl::ScrewMotion::tf [mutable, protected] |
The transformation at current time t.
Referenced by getCurrentTransform(), and integrate().
Transform3f fcl::ScrewMotion::tf1 [protected] |
The transformation at time 0.
Referenced by absoluteRotation(), computeScrewParameter(), getTaylorModel(), and integrate().
Transform3f fcl::ScrewMotion::tf2 [protected] |
The transformation at time 1.
Referenced by computeScrewParameter().