18 m_translational(translational)
47 const Matrix3d &rest_basis,
48 const Matrix3d &basis,
152 :
IK_QSegment(3, false), m_limit_x(false), m_limit_y(false), m_limit_z(false)
180 lmin =
sin(lmin * 0.5);
181 lmax =
sin(lmax * 0.5);
188 else if (axis == 2) {
214 double theta = dq.norm();
217 Vector3d
w = dq * (1.0 / theta);
219 double sine =
sin(theta);
220 double cosine =
cos(theta);
221 double cosineInv = 1 - cosine;
223 double xsine =
w.x() * sine;
224 double ysine =
w.y() * sine;
225 double zsine =
w.z() * sine;
227 double xxcosine =
w.x() *
w.x() * cosineInv;
228 double xycosine =
w.x() *
w.y() * cosineInv;
229 double xzcosine =
w.x() *
w.z() * cosineInv;
230 double yycosine =
w.y() *
w.y() * cosineInv;
231 double yzcosine =
w.y() *
w.z() * cosineInv;
232 double zzcosine =
w.z() *
w.z() * cosineInv;
249 if (m_limit_y ==
false && m_limit_x ==
false && m_limit_z ==
false)
261 double ax =
a.x(), ay =
a.y(), az =
a.z();
266 if (
a.y() > m_max_y) {
270 else if (
a.y() < m_min_y) {
276 if (m_limit_x && m_limit_z) {
280 else if (m_limit_x) {
285 else if (ax > m_max[0]) {
290 else if (m_limit_z) {
295 else if (az > m_max[1]) {
379 if (m_limit ==
false)
382 if (m_new_angle > m_max)
383 delta[0] = m_max - m_angle;
384 else if (m_new_angle < m_min)
385 delta[0] = m_min - m_angle;
390 m_new_angle = m_angle + delta[0];
403 m_angle = m_new_angle;
409 if (lmin > lmax || m_axis != axis)
458 double theta = dq.norm();
461 Vector3d
w = dq * (1.0 / theta);
463 double sine =
sin(theta);
464 double cosine =
cos(theta);
465 double cosineInv = 1 - cosine;
467 double xsine =
w.x() * sine;
468 double zsine =
w.z() * sine;
470 double xxcosine =
w.x() *
w.x() * cosineInv;
471 double xzcosine =
w.x() *
w.z() * cosineInv;
472 double zzcosine =
w.z() *
w.z() * cosineInv;
491 if (m_limit_x ==
false && m_limit_z ==
false)
495 double ax = 0, az = 0;
499 if (m_limit_x && m_limit_z) {
506 else if (m_limit_x) {
511 else if (ax > m_max[0]) {
516 else if (m_limit_z) {
521 else if (az > m_max[1]) {
560 lmin =
sin(lmin * 0.5);
561 lmax =
sin(lmax * 0.5);
564 double offset = 0.5 * (lmin + lmax);
575 else if (axis == 2) {
623 v = Vector3d(m_cos_twist, 0, m_sin_twist);
625 v = Vector3d(-m_sin_twist, 0, m_cos_twist);
644 if (m_new_angle > m_max) {
645 delta[0] = m_max - m_angle;
649 else if (m_new_angle < m_min) {
650 delta[0] = m_min - m_angle;
661 if (m_new_twist > m_max_twist) {
662 delta[1] = m_max_twist - m_twist;
663 m_new_twist = m_max_twist;
666 else if (m_new_twist < m_min_twist) {
667 delta[1] = m_min_twist - m_twist;
668 m_new_twist = m_min_twist;
691 m_angle = m_new_angle;
692 m_twist = m_new_twist;
694 m_sin_twist =
sin(m_twist);
695 m_cos_twist =
cos(m_twist);
715 m_limit_twist =
true;
717 else if (axis == m_axis) {
736 m_axis_enabled[0] = m_axis_enabled[1] = m_axis_enabled[2] =
false;
737 m_axis_enabled[axis1] =
true;
741 m_limit[0] = m_limit[1] = m_limit[2] =
false;
746 m_axis_enabled[0] = m_axis_enabled[1] = m_axis_enabled[2] =
false;
747 m_axis_enabled[axis1] =
true;
748 m_axis_enabled[axis2] =
true;
753 m_limit[0] = m_limit[1] = m_limit[2] =
false;
758 m_axis_enabled[0] = m_axis_enabled[1] = m_axis_enabled[2] =
true;
764 m_limit[0] = m_limit[1] = m_limit[2] =
false;
774 int dof_id =
m_DoF_id, dof = 0, i, clamped =
false;
776 Vector3d dx(0.0, 0.0, 0.0);
778 for (i = 0; i < 3; i++) {
779 if (!m_axis_enabled[i]) {
790 if (m_new_translation[i] > m_max[i]) {
792 m_new_translation[i] = m_max[i];
793 clamped =
clamp[dof] =
true;
795 else if (m_new_translation[i] < m_min[i]) {
797 m_new_translation[i] = m_min[i];
798 clamped =
clamp[dof] =
true;
826 if (m_axis[i] == axis)
837 m_limit[axis] =
true;
846 for (i = 0; i < 3; i++) {
851 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 Output data from inside of a node group A color picker Mix two input colors RGB to Convert a color s luminance to a grayscale value Generate a normal vector and a dot product Bright Control the brightness and contrast of the input color Vector Map an input vectors to used to fine tune the interpolation of the input Camera Retrieve information about the camera and how it relates to the current shading point s position Clamp
ATTR_WARN_UNUSED_RESULT const BMVert * v
SIMD_FORCE_INLINE const btScalar & w() const
Return the w value.
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)
ccl_gpu_kernel_postfix ccl_global float int int int int float bool int offset
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
T clamp(const T &a, const T &min, const T &max)
T length(const vec_base< T, Size > &a)