15 m_poleconstraint =
false;
16 m_getpoleangle =
false;
17 m_rootmatrix.setIdentity();
20 double IK_QJacobianSolver::ComputeScale()
22 std::vector<IK_QSegment *>::iterator seg;
25 for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
26 length += (*seg)->MaxExtension();
34 void IK_QJacobianSolver::Scale(
double scale, std::list<IK_QTask *> &tasks)
36 std::list<IK_QTask *>::iterator
task;
37 std::vector<IK_QSegment *>::iterator seg;
40 (*task)->Scale(scale);
42 for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
45 m_rootmatrix.translation() *= scale;
50 void IK_QJacobianSolver::AddSegmentList(
IK_QSegment *seg)
52 m_segments.push_back(seg);
55 for (child = seg->
Child(); child; child = child->
Sibling())
56 AddSegmentList(child);
65 std::vector<IK_QSegment *>::iterator seg;
68 for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
70 num_dof += (*seg)->NumberOfDoF();
77 int primary_size = 0, primary = 0;
78 int secondary_size = 0, secondary = 0;
79 double primary_weight = 0.0, secondary_weight = 0.0;
80 std::list<IK_QTask *>::iterator
task;
82 for (
task = tasks.begin();
task != tasks.end();
task++) {
86 qtask->
SetId(primary_size);
87 primary_size += qtask->
Size();
88 primary_weight += qtask->
Weight();
92 qtask->
SetId(secondary_size);
93 secondary_size += qtask->
Size();
94 secondary_weight += qtask->
Weight();
99 if (primary_size == 0 ||
FuzzyZero(primary_weight))
102 m_secondary_enabled = (secondary > 0);
105 double primary_rescale = 1.0 / primary_weight;
106 double secondary_rescale;
108 secondary_rescale = 0.0;
110 secondary_rescale = 1.0 / secondary_weight;
112 for (
task = tasks.begin();
task != tasks.end();
task++) {
124 m_jacobian_sub.
ArmMatrices(num_dof, secondary_size);
129 for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
130 for (i = 0; i < (*seg)->NumberOfDoF(); i++)
131 m_jacobian.
SetDoFWeight((*seg)->DoFId() + i, (*seg)->Weight(i));
137 IK_QSegment *tip, Vector3d &goal, Vector3d &polegoal,
float poleangle,
bool getangle)
139 m_poleconstraint =
true;
142 m_polegoal = polegoal;
143 m_poleangle = (getangle) ? 0.0f : poleangle;
144 m_getpoleangle = getangle;
147 void IK_QJacobianSolver::ConstrainPoleVector(
IK_QSegment *root, std::list<IK_QTask *> &tasks)
153 if (!m_poleconstraint)
157 std::list<IK_QTask *>::iterator
task;
158 int positiontasks = 0;
161 if ((*task)->PositionTask())
164 if (positiontasks >= 2) {
165 m_poleconstraint =
false;
173 const Vector3d endpos = m_poletip->
GlobalEnd();
179 Vector3d dir =
normalize(endpos - rootpos);
180 Vector3d rootx = rootbasis.col(0);
181 Vector3d rootz = rootbasis.col(2);
182 Vector3d up = rootx *
cos(m_poleangle) + rootz *
sin(m_poleangle);
185 Vector3d poledir = (m_getpoleangle) ? dir :
normalize(m_goal - rootpos);
186 Vector3d poleup =
normalize(m_polegoal - rootpos);
188 Matrix3d mat, polemat;
191 mat.row(1) = mat.row(0).cross(dir);
194 polemat.row(0) =
normalize(poledir.cross(poleup));
195 polemat.row(1) = polemat.row(0).cross(poledir);
196 polemat.row(2) = -poledir;
198 if (m_getpoleangle) {
200 m_poleangle =
angle(mat.row(1), polemat.row(1));
202 double dt = rootz.dot(mat.row(1) *
cos(m_poleangle) + mat.row(0) *
sin(m_poleangle));
204 m_poleangle = -m_poleangle;
207 m_getpoleangle =
false;
208 ConstrainPoleVector(root, tasks);
216 trans.linear() = polemat.transpose() * mat;
217 trans.translation() = Vector3d(0, 0, 0);
218 m_rootmatrix = trans * m_rootmatrix;
222 bool IK_QJacobianSolver::UpdateAngles(
double &
norm)
225 std::vector<IK_QSegment *>::iterator seg;
227 double minabsdelta = 1e10, absdelta;
228 Vector3d delta, mindelta;
229 bool locked =
false,
clamp[3];
235 for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
240 absdelta =
fabs(delta[i]);
243 qseg->
Lock(i, m_jacobian, delta);
246 else if (absdelta < minabsdelta) {
247 minabsdelta = absdelta;
259 minseg->
Lock(mindof, m_jacobian, mindelta);
262 if (minabsdelta >
norm)
268 for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
270 (*seg)->UpdateAngleApply();
278 std::list<IK_QTask *> tasks,
280 const int max_iterations)
282 float scale = ComputeScale();
288 ConstrainPoleVector(root, tasks);
293 for (
int iterations = 0; iterations < max_iterations; iterations++) {
297 std::list<IK_QTask *>::iterator
task;
300 for (
task = tasks.begin();
task != tasks.end();
task++) {
301 if ((*task)->Primary())
302 (*task)->ComputeJacobian(m_jacobian);
304 (*task)->ComputeJacobian(m_jacobian_sub);
313 if (m_secondary_enabled)
314 m_jacobian.
SubTask(m_jacobian_sub);
317 fprintf(stderr,
"IK Exception\n");
322 }
while (UpdateAngles(
norm));
325 std::vector<IK_QSegment *>::iterator seg;
326 for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
335 if (norm < 1e-3 && iterations > 10) {
341 if (m_poleconstraint)
344 Scale(1.0f / scale, tasks);
static const double IK_EPSILON
static bool FuzzyZero(double x)
SIMD_FORCE_INLINE btScalar norm() const
Return the norm (length) of the vector.
SIMD_FORCE_INLINE btScalar angle(const btVector3 &v) const
Return the angle between this and another vector.
void SetPoleVectorConstraint(IK_QSegment *tip, Vector3d &goal, Vector3d &polegoal, float poleangle, bool getangle)
bool Solve(IK_QSegment *root, std::list< IK_QTask * > tasks, const double tolerance, const int max_iterations)
bool Setup(IK_QSegment *root, std::list< IK_QTask * > &tasks)
void SetDoFWeight(int dof, double weight)
void SubTask(IK_QJacobian &jacobian)
double AngleUpdateNorm() const
void ArmMatrices(int dof, int task_size)
const Vector3d GlobalStart() const
const Affine3d & GlobalTransform() const
const Vector3d GlobalEnd() const
virtual void Lock(int, IK_QJacobian &, Vector3d &)
virtual bool UpdateAngle(const IK_QJacobian &, Vector3d &, bool *)=0
IK_QSegment * Child() const
IK_QSegment * Sibling() const
bool Locked(int dof) const
void UpdateTransform(const Affine3d &global)
void PrependBasis(const Matrix3d &mat)
void SetDoFId(int dof_id)
void SetWeight(double weight)
ccl_device_inline float2 fabs(const float2 &a)
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
struct blender::compositor::@179::@181 task
T clamp(const T &a, const T &min, const T &max)
T length(const vec_base< T, Size > &a)
vec_base< T, Size > normalize(const vec_base< T, Size > &v)