34 m_translational(translational)
63 const Matrix3d &rest_basis,
64 const Matrix3d &basis,
168 :
IK_QSegment(3, false), m_limit_x(false), m_limit_y(false), m_limit_z(false)
196 lmin =
sin(lmin * 0.5);
197 lmax =
sin(lmax * 0.5);
204 else if (axis == 2) {
230 double theta = dq.norm();
233 Vector3d
w = dq * (1.0 / theta);
235 double sine =
sin(theta);
236 double cosine =
cos(theta);
237 double cosineInv = 1 - cosine;
239 double xsine =
w.x() * sine;
240 double ysine =
w.y() * sine;
241 double zsine =
w.z() * sine;
243 double xxcosine =
w.x() *
w.x() * cosineInv;
244 double xycosine =
w.x() *
w.y() * cosineInv;
245 double xzcosine =
w.x() *
w.z() * cosineInv;
246 double yycosine =
w.y() *
w.y() * cosineInv;
247 double yzcosine =
w.y() *
w.z() * cosineInv;
248 double zzcosine =
w.z() *
w.z() * cosineInv;
265 if (m_limit_y ==
false && m_limit_x ==
false && m_limit_z ==
false)
277 double ax =
a.x(), ay =
a.y(), az =
a.z();
282 if (
a.y() > m_max_y) {
286 else if (
a.y() < m_min_y) {
292 if (m_limit_x && m_limit_z) {
296 else if (m_limit_x) {
301 else if (ax > m_max[0]) {
306 else if (m_limit_z) {
311 else if (az > m_max[1]) {
395 if (m_limit ==
false)
398 if (m_new_angle > m_max)
399 delta[0] = m_max - m_angle;
400 else if (m_new_angle < m_min)
401 delta[0] = m_min - m_angle;
406 m_new_angle = m_angle + delta[0];
419 m_angle = m_new_angle;
425 if (lmin > lmax || m_axis != axis)
474 double theta = dq.norm();
477 Vector3d
w = dq * (1.0 / theta);
479 double sine =
sin(theta);
480 double cosine =
cos(theta);
481 double cosineInv = 1 - cosine;
483 double xsine =
w.x() * sine;
484 double zsine =
w.z() * sine;
486 double xxcosine =
w.x() *
w.x() * cosineInv;
487 double xzcosine =
w.x() *
w.z() * cosineInv;
488 double zzcosine =
w.z() *
w.z() * cosineInv;
507 if (m_limit_x ==
false && m_limit_z ==
false)
511 double ax = 0, az = 0;
515 if (m_limit_x && m_limit_z) {
522 else if (m_limit_x) {
527 else if (ax > m_max[0]) {
532 else if (m_limit_z) {
537 else if (az > m_max[1]) {
576 lmin =
sin(lmin * 0.5);
577 lmax =
sin(lmax * 0.5);
580 double offset = 0.5 * (lmin + lmax);
591 else if (axis == 2) {
639 v = Vector3d(m_cos_twist, 0, m_sin_twist);
641 v = Vector3d(-m_sin_twist, 0, m_cos_twist);
660 if (m_new_angle > m_max) {
661 delta[0] = m_max - m_angle;
665 else if (m_new_angle < m_min) {
666 delta[0] = m_min - m_angle;
677 if (m_new_twist > m_max_twist) {
678 delta[1] = m_max_twist - m_twist;
679 m_new_twist = m_max_twist;
682 else if (m_new_twist < m_min_twist) {
683 delta[1] = m_min_twist - m_twist;
684 m_new_twist = m_min_twist;
707 m_angle = m_new_angle;
708 m_twist = m_new_twist;
710 m_sin_twist =
sin(m_twist);
711 m_cos_twist =
cos(m_twist);
731 m_limit_twist =
true;
733 else if (axis == m_axis) {
752 m_axis_enabled[0] = m_axis_enabled[1] = m_axis_enabled[2] =
false;
753 m_axis_enabled[axis1] =
true;
757 m_limit[0] = m_limit[1] = m_limit[2] =
false;
762 m_axis_enabled[0] = m_axis_enabled[1] = m_axis_enabled[2] =
false;
763 m_axis_enabled[axis1] =
true;
764 m_axis_enabled[axis2] =
true;
769 m_limit[0] = m_limit[1] = m_limit[2] =
false;
774 m_axis_enabled[0] = m_axis_enabled[1] = m_axis_enabled[2] =
true;
780 m_limit[0] = m_limit[1] = m_limit[2] =
false;
790 int dof_id =
m_DoF_id, dof = 0, i, clamped =
false;
792 Vector3d dx(0.0, 0.0, 0.0);
794 for (i = 0; i < 3; i++) {
795 if (!m_axis_enabled[i]) {
806 if (m_new_translation[i] > m_max[i]) {
808 m_new_translation[i] = m_max[i];
809 clamped =
clamp[dof] =
true;
811 else if (m_new_translation[i] < m_min[i]) {
813 m_new_translation[i] = m_min[i];
814 clamped =
clamp[dof] =
true;
842 if (m_axis[i] == axis)
853 m_limit[axis] =
true;
862 for (i = 0; i < 3; i++) {
867 m_new_translation *= scale;
static void RemoveTwist(Eigen::Matrix3d &R)
static bool EllipseClamp(double &ax, double &az, double *amin, double *amax)
static Eigen::Matrix3d CreateMatrix(double xx, double xy, double xz, double yx, double yy, double yz, double zx, double zy, double zz)
static Eigen::Vector3d MatrixToAxisAngle(const Eigen::Matrix3d &R)
static Eigen::Matrix3d RotationMatrix(double sine, double cosine, int axis)
static Eigen::Matrix3d ComputeTwistMatrix(double tau)
static Eigen::Vector3d SphericalRangeParameters(const Eigen::Matrix3d &R)
static Eigen::Matrix3d ComputeSwingMatrix(double ax, double az)
static bool FuzzyZero(double x)
Group RGB to Bright Vector Camera Clamp
ATTR_WARN_UNUSED_RESULT const BMVert * v
SIMD_FORCE_INLINE const btScalar & w() const
Return the w value.
SIMD_FORCE_INLINE btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Vector3d Axis(int dof) const
void SetLimit(int axis, double lmin, double lmax)
IK_QElbowSegment(int axis)
void Lock(int dof, IK_QJacobian &jacobian, Vector3d &delta)
void SetWeight(int axis, double weight)
bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp)
void SetBasis(const Matrix3d &basis)
void Lock(int dof_id, double delta)
double AngleUpdate(int dof_id) const
void SetWeight(int axis, double weight)
bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp)
Vector3d Axis(int dof) const
IK_QRevoluteSegment(int axis)
void SetBasis(const Matrix3d &basis)
void Lock(int dof, IK_QJacobian &jacobian, Vector3d &delta)
void SetLimit(int axis, double lmin, double lmax)
virtual void SetBasis(const Matrix3d &basis)
virtual EIGEN_MAKE_ALIGNED_OPERATOR_NEW ~IK_QSegment()
Vector3d m_orig_translation
void UpdateTransform(const Affine3d &global)
Affine3d m_global_transform
virtual void Scale(double scale)
void SetComposite(IK_QSegment *seg)
Matrix3d BasisChange() const
IK_QSegment(int num_DoF, bool translational)
void PrependBasis(const Matrix3d &mat)
void SetParent(IK_QSegment *parent)
IK_QSegment * m_composite
void RemoveChild(IK_QSegment *child)
Vector3d TranslationChange() const
void SetTransform(const Vector3d &start, const Matrix3d &rest_basis, const Matrix3d &basis, const double length)
void SetWeight(int axis, double weight)
Vector3d Axis(int dof) const
void Lock(int dof, IK_QJacobian &jacobian, Vector3d &delta)
bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp)
void SetLimit(int axis, double lmin, double lmax)
void SetWeight(int axis, double weight)
void Lock(int dof, IK_QJacobian &jacobian, Vector3d &delta)
Vector3d Axis(int dof) const
bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp)
void SetLimit(int axis, double lmin, double lmax)
void SetBasis(const Matrix3d &basis)
void SetWeight(int axis, double weight)
void Lock(int, IK_QJacobian &, Vector3d &)
Vector3d Axis(int dof) const
bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp)
void SetLimit(int axis, double lmin, double lmax)
static double ComputeTwist(const KDL::Rotation &R)
static double EulerAngleFromMatrix(const KDL::Rotation &R, int axis)
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
ccl_device_inline int clamp(int a, int mn, int mx)