13 : m_size(
size), m_primary(primary), m_active(
active), m_segment(
segment), m_weight(1.0)
34 m_clamp_length /= 2 * num;
42 Vector3d d_pos = m_goal -
pos;
43 double length = d_pos.norm();
45 if (
length > m_clamp_length)
46 d_pos = (m_clamp_length /
length) * d_pos;
63 Vector3d pa = p.cross(axis);
73 Vector3d d_pos = m_goal -
pos;
91 Matrix3d d_rotm = (m_goal *
rot.transpose()).
transpose();
94 d_rot = -0.5 * Vector3d(d_rotm(2, 1) - d_rotm(1, 2),
95 d_rotm(0, 2) - d_rotm(2, 0),
96 d_rotm(1, 0) - d_rotm(0, 1));
98 m_distance = d_rot.norm();
123 const Vector3d &goal_center)
126 m_total_mass_inv = ComputeTotalMass(
m_segment);
128 m_total_mass_inv = 1.0 / m_total_mass_inv;
137 mass += ComputeTotalMass(seg);
148 center += ComputeCenter(seg);
153 void IK_QCenterOfMassTask::JacobianSegment(
IK_QJacobian &jacobian,
160 for (i = 0; i <
segment->NumberOfDoF(); i++) {
162 axis *= m_total_mass_inv;
167 Vector3d pa = axis.cross(p);
174 JacobianSegment(jacobian,
center, seg);
182 Vector3d d_pos = m_goal_center -
center;
184 m_distance = d_pos.norm();
187 if (m_distance > m_clamp_length)
188 d_pos = (m_clamp_length / m_distance) * d_pos;
NSNotificationCenter * center
static bool FuzzyZero(double x)
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
btMatrix3x3 transpose() const
Return the transpose of the matrix.
void ComputeJacobian(IK_QJacobian &jacobian)
IK_QCenterOfMassTask(bool primary, const IK_QSegment *segment, const Vector3d ¢er)
void SetDerivatives(int id, int dof_id, const Vector3d &v, double norm_weight)
void SetBetas(int id, int size, const Vector3d &v)
IK_QOrientationTask(bool primary, const IK_QSegment *segment, const Matrix3d &goal)
void ComputeJacobian(IK_QJacobian &jacobian)
void ComputeJacobian(IK_QJacobian &jacobian)
IK_QPositionTask(bool primary, const IK_QSegment *segment, const Vector3d &goal)
const Vector3d GlobalStart() const
const Affine3d & GlobalTransform() const
const Vector3d GlobalEnd() const
IK_QSegment * Sibling() const
bool Translational() const
const double MaxExtension() const
virtual Vector3d Axis(int dof) const =0
IK_QSegment * Parent() const
const IK_QSegment * m_segment
IK_QTask(int size, bool primary, bool active, const IK_QSegment *segment)
Segment< FEdge *, Vec3r > segment
bool active
all scheduled work for the GPU.
T length(const vec_base< T, Size > &a)