Blender  V2.93
IK_QSegment.cpp
Go to the documentation of this file.
1 /*
2  * This program is free software; you can redistribute it and/or
3  * modify it under the terms of the GNU General Public License
4  * as published by the Free Software Foundation; either version 2
5  * of the License, or (at your option) any later version.
6  *
7  * This program is distributed in the hope that it will be useful,
8  * but WITHOUT ANY WARRANTY; without even the implied warranty of
9  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10  * GNU General Public License for more details.
11  *
12  * You should have received a copy of the GNU General Public License
13  * along with this program; if not, write to the Free Software Foundation,
14  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
15  *
16  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
17  * All rights reserved.
18  */
19 
24 #include "IK_QSegment.h"
25 
26 // IK_QSegment
27 
28 IK_QSegment::IK_QSegment(int num_DoF, bool translational)
29  : m_parent(NULL),
30  m_child(NULL),
31  m_sibling(NULL),
32  m_composite(NULL),
33  m_num_DoF(num_DoF),
34  m_translational(translational)
35 {
36  m_locked[0] = m_locked[1] = m_locked[2] = false;
37  m_weight[0] = m_weight[1] = m_weight[2] = 1.0;
38 
39  m_max_extension = 0.0;
40 
41  m_start = Vector3d(0, 0, 0);
42  m_rest_basis.setIdentity();
43  m_basis.setIdentity();
44  m_translation = Vector3d(0, 0, 0);
45 
48 }
49 
51 {
52  m_locked[0] = m_locked[1] = m_locked[2] = false;
53 
57 
58  for (IK_QSegment *seg = m_child; seg; seg = seg->m_sibling)
59  seg->Reset();
60 }
61 
62 void IK_QSegment::SetTransform(const Vector3d &start,
63  const Matrix3d &rest_basis,
64  const Matrix3d &basis,
65  const double length)
66 {
67  m_max_extension = start.norm() + length;
68 
69  m_start = start;
70  m_rest_basis = rest_basis;
71 
72  m_orig_basis = basis;
73  SetBasis(basis);
74 
75  m_translation = Vector3d(0, length, 0);
77 }
78 
79 Matrix3d IK_QSegment::BasisChange() const
80 {
81  return m_orig_basis.transpose() * m_basis;
82 }
83 
85 {
87 }
88 
90 {
91  if (m_parent)
92  m_parent->RemoveChild(this);
93 
94  for (IK_QSegment *seg = m_child; seg; seg = seg->m_sibling)
95  seg->m_parent = NULL;
96 }
97 
99 {
100  if (m_parent == parent)
101  return;
102 
103  if (m_parent)
104  m_parent->RemoveChild(this);
105 
106  if (parent) {
107  m_sibling = parent->m_child;
108  parent->m_child = this;
109  }
110 
111  m_parent = parent;
112 }
113 
115 {
116  m_composite = seg;
117 }
118 
120 {
121  if (m_child == NULL)
122  return;
123  else if (m_child == child)
125  else {
126  IK_QSegment *seg = m_child;
127 
128  while (seg->m_sibling != child)
129  seg = seg->m_sibling;
130 
131  if (child == seg->m_sibling)
132  seg->m_sibling = child->m_sibling;
133  }
134 }
135 
136 void IK_QSegment::UpdateTransform(const Affine3d &global)
137 {
138  // compute the global transform at the end of the segment
139  m_global_start = global.translation() + global.linear() * m_start;
140 
141  m_global_transform.translation() = m_global_start;
142  m_global_transform.linear() = global.linear() * m_rest_basis * m_basis;
144 
145  // update child transforms
146  for (IK_QSegment *seg = m_child; seg; seg = seg->m_sibling)
147  seg->UpdateTransform(m_global_transform);
148 }
149 
150 void IK_QSegment::PrependBasis(const Matrix3d &mat)
151 {
152  m_basis = m_rest_basis.inverse() * mat * m_rest_basis * m_basis;
153 }
154 
155 void IK_QSegment::Scale(double scale)
156 {
157  m_start *= scale;
158  m_translation *= scale;
159  m_orig_translation *= scale;
160  m_global_start *= scale;
161  m_global_transform.translation() *= scale;
162  m_max_extension *= scale;
163 }
164 
165 // IK_QSphericalSegment
166 
168  : IK_QSegment(3, false), m_limit_x(false), m_limit_y(false), m_limit_z(false)
169 {
170 }
171 
172 Vector3d IK_QSphericalSegment::Axis(int dof) const
173 {
174  return m_global_transform.linear().col(dof);
175 }
176 
177 void IK_QSphericalSegment::SetLimit(int axis, double lmin, double lmax)
178 {
179  if (lmin > lmax)
180  return;
181 
182  if (axis == 1) {
183  lmin = Clamp(lmin, -M_PI, M_PI);
184  lmax = Clamp(lmax, -M_PI, M_PI);
185 
186  m_min_y = lmin;
187  m_max_y = lmax;
188 
189  m_limit_y = true;
190  }
191  else {
192  // clamp and convert to axis angle parameters
193  lmin = Clamp(lmin, -M_PI, M_PI);
194  lmax = Clamp(lmax, -M_PI, M_PI);
195 
196  lmin = sin(lmin * 0.5);
197  lmax = sin(lmax * 0.5);
198 
199  if (axis == 0) {
200  m_min[0] = -lmax;
201  m_max[0] = -lmin;
202  m_limit_x = true;
203  }
204  else if (axis == 2) {
205  m_min[1] = -lmax;
206  m_max[1] = -lmin;
207  m_limit_z = true;
208  }
209  }
210 }
211 
212 void IK_QSphericalSegment::SetWeight(int axis, double weight)
213 {
214  m_weight[axis] = weight;
215 }
216 
217 bool IK_QSphericalSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp)
218 {
219  if (m_locked[0] && m_locked[1] && m_locked[2])
220  return false;
221 
222  Vector3d dq;
223  dq.x() = jacobian.AngleUpdate(m_DoF_id);
224  dq.y() = jacobian.AngleUpdate(m_DoF_id + 1);
225  dq.z() = jacobian.AngleUpdate(m_DoF_id + 2);
226 
227  // Directly update the rotation matrix, with Rodrigues' rotation formula,
228  // to avoid singularities and allow smooth integration.
229 
230  double theta = dq.norm();
231 
232  if (!FuzzyZero(theta)) {
233  Vector3d w = dq * (1.0 / theta);
234 
235  double sine = sin(theta);
236  double cosine = cos(theta);
237  double cosineInv = 1 - cosine;
238 
239  double xsine = w.x() * sine;
240  double ysine = w.y() * sine;
241  double zsine = w.z() * sine;
242 
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;
249 
250  Matrix3d M = CreateMatrix(cosine + xxcosine,
251  -zsine + xycosine,
252  ysine + xzcosine,
253  zsine + xycosine,
254  cosine + yycosine,
255  -xsine + yzcosine,
256  -ysine + xzcosine,
257  xsine + yzcosine,
258  cosine + zzcosine);
259 
260  m_new_basis = m_basis * M;
261  }
262  else
263  m_new_basis = m_basis;
264 
265  if (m_limit_y == false && m_limit_x == false && m_limit_z == false)
266  return false;
267 
268  Vector3d a = SphericalRangeParameters(m_new_basis);
269 
270  if (m_locked[0])
271  a.x() = m_locked_ax;
272  if (m_locked[1])
273  a.y() = m_locked_ay;
274  if (m_locked[2])
275  a.z() = m_locked_az;
276 
277  double ax = a.x(), ay = a.y(), az = a.z();
278 
279  clamp[0] = clamp[1] = clamp[2] = false;
280 
281  if (m_limit_y) {
282  if (a.y() > m_max_y) {
283  ay = m_max_y;
284  clamp[1] = true;
285  }
286  else if (a.y() < m_min_y) {
287  ay = m_min_y;
288  clamp[1] = true;
289  }
290  }
291 
292  if (m_limit_x && m_limit_z) {
293  if (EllipseClamp(ax, az, m_min, m_max))
294  clamp[0] = clamp[2] = true;
295  }
296  else if (m_limit_x) {
297  if (ax < m_min[0]) {
298  ax = m_min[0];
299  clamp[0] = true;
300  }
301  else if (ax > m_max[0]) {
302  ax = m_max[0];
303  clamp[0] = true;
304  }
305  }
306  else if (m_limit_z) {
307  if (az < m_min[1]) {
308  az = m_min[1];
309  clamp[2] = true;
310  }
311  else if (az > m_max[1]) {
312  az = m_max[1];
313  clamp[2] = true;
314  }
315  }
316 
317  if (clamp[0] == false && clamp[1] == false && clamp[2] == false) {
318  if (m_locked[0] || m_locked[1] || m_locked[2])
319  m_new_basis = ComputeSwingMatrix(ax, az) * ComputeTwistMatrix(ay);
320  return false;
321  }
322 
323  m_new_basis = ComputeSwingMatrix(ax, az) * ComputeTwistMatrix(ay);
324 
325  delta = MatrixToAxisAngle(m_basis.transpose() * m_new_basis);
326 
327  if (!(m_locked[0] || m_locked[2]) && (clamp[0] || clamp[2])) {
328  m_locked_ax = ax;
329  m_locked_az = az;
330  }
331 
332  if (!m_locked[1] && clamp[1])
333  m_locked_ay = ay;
334 
335  return true;
336 }
337 
338 void IK_QSphericalSegment::Lock(int dof, IK_QJacobian &jacobian, Vector3d &delta)
339 {
340  if (dof == 1) {
341  m_locked[1] = true;
342  jacobian.Lock(m_DoF_id + 1, delta[1]);
343  }
344  else {
345  m_locked[0] = m_locked[2] = true;
346  jacobian.Lock(m_DoF_id, delta[0]);
347  jacobian.Lock(m_DoF_id + 2, delta[2]);
348  }
349 }
350 
352 {
353  m_basis = m_new_basis;
354 }
355 
356 // IK_QNullSegment
357 
359 {
360 }
361 
362 // IK_QRevoluteSegment
363 
365  : IK_QSegment(1, false), m_axis(axis), m_angle(0.0), m_limit(false)
366 {
367 }
368 
369 void IK_QRevoluteSegment::SetBasis(const Matrix3d &basis)
370 {
371  if (m_axis == 1) {
372  m_angle = ComputeTwist(basis);
373  m_basis = ComputeTwistMatrix(m_angle);
374  }
375  else {
376  m_angle = EulerAngleFromMatrix(basis, m_axis);
377  m_basis = RotationMatrix(m_angle, m_axis);
378  }
379 }
380 
381 Vector3d IK_QRevoluteSegment::Axis(int) const
382 {
383  return m_global_transform.linear().col(m_axis);
384 }
385 
386 bool IK_QRevoluteSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp)
387 {
388  if (m_locked[0])
389  return false;
390 
391  m_new_angle = m_angle + jacobian.AngleUpdate(m_DoF_id);
392 
393  clamp[0] = false;
394 
395  if (m_limit == false)
396  return false;
397 
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;
402  else
403  return false;
404 
405  clamp[0] = true;
406  m_new_angle = m_angle + delta[0];
407 
408  return true;
409 }
410 
411 void IK_QRevoluteSegment::Lock(int, IK_QJacobian &jacobian, Vector3d &delta)
412 {
413  m_locked[0] = true;
414  jacobian.Lock(m_DoF_id, delta[0]);
415 }
416 
418 {
419  m_angle = m_new_angle;
420  m_basis = RotationMatrix(m_angle, m_axis);
421 }
422 
423 void IK_QRevoluteSegment::SetLimit(int axis, double lmin, double lmax)
424 {
425  if (lmin > lmax || m_axis != axis)
426  return;
427 
428  // clamp and convert to axis angle parameters
429  lmin = Clamp(lmin, -M_PI, M_PI);
430  lmax = Clamp(lmax, -M_PI, M_PI);
431 
432  m_min = lmin;
433  m_max = lmax;
434 
435  m_limit = true;
436 }
437 
438 void IK_QRevoluteSegment::SetWeight(int axis, double weight)
439 {
440  if (axis == m_axis)
441  m_weight[0] = weight;
442 }
443 
444 // IK_QSwingSegment
445 
446 IK_QSwingSegment::IK_QSwingSegment() : IK_QSegment(2, false), m_limit_x(false), m_limit_z(false)
447 {
448 }
449 
450 void IK_QSwingSegment::SetBasis(const Matrix3d &basis)
451 {
452  m_basis = basis;
454 }
455 
456 Vector3d IK_QSwingSegment::Axis(int dof) const
457 {
458  return m_global_transform.linear().col((dof == 0) ? 0 : 2);
459 }
460 
461 bool IK_QSwingSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp)
462 {
463  if (m_locked[0] && m_locked[1])
464  return false;
465 
466  Vector3d dq;
467  dq.x() = jacobian.AngleUpdate(m_DoF_id);
468  dq.y() = 0.0;
469  dq.z() = jacobian.AngleUpdate(m_DoF_id + 1);
470 
471  // Directly update the rotation matrix, with Rodrigues' rotation formula,
472  // to avoid singularities and allow smooth integration.
473 
474  double theta = dq.norm();
475 
476  if (!FuzzyZero(theta)) {
477  Vector3d w = dq * (1.0 / theta);
478 
479  double sine = sin(theta);
480  double cosine = cos(theta);
481  double cosineInv = 1 - cosine;
482 
483  double xsine = w.x() * sine;
484  double zsine = w.z() * sine;
485 
486  double xxcosine = w.x() * w.x() * cosineInv;
487  double xzcosine = w.x() * w.z() * cosineInv;
488  double zzcosine = w.z() * w.z() * cosineInv;
489 
490  Matrix3d M = CreateMatrix(cosine + xxcosine,
491  -zsine,
492  xzcosine,
493  zsine,
494  cosine,
495  -xsine,
496  xzcosine,
497  xsine,
498  cosine + zzcosine);
499 
500  m_new_basis = m_basis * M;
501 
502  RemoveTwist(m_new_basis);
503  }
504  else
505  m_new_basis = m_basis;
506 
507  if (m_limit_x == false && m_limit_z == false)
508  return false;
509 
510  Vector3d a = SphericalRangeParameters(m_new_basis);
511  double ax = 0, az = 0;
512 
513  clamp[0] = clamp[1] = false;
514 
515  if (m_limit_x && m_limit_z) {
516  ax = a.x();
517  az = a.z();
518 
519  if (EllipseClamp(ax, az, m_min, m_max))
520  clamp[0] = clamp[1] = true;
521  }
522  else if (m_limit_x) {
523  if (ax < m_min[0]) {
524  ax = m_min[0];
525  clamp[0] = true;
526  }
527  else if (ax > m_max[0]) {
528  ax = m_max[0];
529  clamp[0] = true;
530  }
531  }
532  else if (m_limit_z) {
533  if (az < m_min[1]) {
534  az = m_min[1];
535  clamp[1] = true;
536  }
537  else if (az > m_max[1]) {
538  az = m_max[1];
539  clamp[1] = true;
540  }
541  }
542 
543  if (clamp[0] == false && clamp[1] == false)
544  return false;
545 
546  m_new_basis = ComputeSwingMatrix(ax, az);
547 
548  delta = MatrixToAxisAngle(m_basis.transpose() * m_new_basis);
549  delta[1] = delta[2];
550  delta[2] = 0.0;
551 
552  return true;
553 }
554 
555 void IK_QSwingSegment::Lock(int, IK_QJacobian &jacobian, Vector3d &delta)
556 {
557  m_locked[0] = m_locked[1] = true;
558  jacobian.Lock(m_DoF_id, delta[0]);
559  jacobian.Lock(m_DoF_id + 1, delta[1]);
560 }
561 
563 {
564  m_basis = m_new_basis;
565 }
566 
567 void IK_QSwingSegment::SetLimit(int axis, double lmin, double lmax)
568 {
569  if (lmin > lmax)
570  return;
571 
572  // clamp and convert to axis angle parameters
573  lmin = Clamp(lmin, -M_PI, M_PI);
574  lmax = Clamp(lmax, -M_PI, M_PI);
575 
576  lmin = sin(lmin * 0.5);
577  lmax = sin(lmax * 0.5);
578 
579  // put center of ellispe in the middle between min and max
580  double offset = 0.5 * (lmin + lmax);
581  // lmax = lmax - offset;
582 
583  if (axis == 0) {
584  m_min[0] = -lmax;
585  m_max[0] = -lmin;
586 
587  m_limit_x = true;
588  m_offset_x = offset;
589  m_max_x = lmax;
590  }
591  else if (axis == 2) {
592  m_min[1] = -lmax;
593  m_max[1] = -lmin;
594 
595  m_limit_z = true;
596  m_offset_z = offset;
597  m_max_z = lmax;
598  }
599 }
600 
601 void IK_QSwingSegment::SetWeight(int axis, double weight)
602 {
603  if (axis == 0)
604  m_weight[0] = weight;
605  else if (axis == 2)
606  m_weight[1] = weight;
607 }
608 
609 // IK_QElbowSegment
610 
612  : IK_QSegment(2, false),
613  m_axis(axis),
614  m_twist(0.0),
615  m_angle(0.0),
616  m_cos_twist(1.0),
617  m_sin_twist(0.0),
618  m_limit(false),
619  m_limit_twist(false)
620 {
621 }
622 
623 void IK_QElbowSegment::SetBasis(const Matrix3d &basis)
624 {
625  m_basis = basis;
626 
627  m_twist = ComputeTwist(m_basis);
629  m_angle = EulerAngleFromMatrix(basis, m_axis);
630 
631  m_basis = RotationMatrix(m_angle, m_axis) * ComputeTwistMatrix(m_twist);
632 }
633 
634 Vector3d IK_QElbowSegment::Axis(int dof) const
635 {
636  if (dof == 0) {
637  Vector3d v;
638  if (m_axis == 0)
639  v = Vector3d(m_cos_twist, 0, m_sin_twist);
640  else
641  v = Vector3d(-m_sin_twist, 0, m_cos_twist);
642 
643  return m_global_transform.linear() * v;
644  }
645  else
646  return m_global_transform.linear().col(1);
647 }
648 
649 bool IK_QElbowSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp)
650 {
651  if (m_locked[0] && m_locked[1])
652  return false;
653 
654  clamp[0] = clamp[1] = false;
655 
656  if (!m_locked[0]) {
657  m_new_angle = m_angle + jacobian.AngleUpdate(m_DoF_id);
658 
659  if (m_limit) {
660  if (m_new_angle > m_max) {
661  delta[0] = m_max - m_angle;
662  m_new_angle = m_max;
663  clamp[0] = true;
664  }
665  else if (m_new_angle < m_min) {
666  delta[0] = m_min - m_angle;
667  m_new_angle = m_min;
668  clamp[0] = true;
669  }
670  }
671  }
672 
673  if (!m_locked[1]) {
674  m_new_twist = m_twist + jacobian.AngleUpdate(m_DoF_id + 1);
675 
676  if (m_limit_twist) {
677  if (m_new_twist > m_max_twist) {
678  delta[1] = m_max_twist - m_twist;
679  m_new_twist = m_max_twist;
680  clamp[1] = true;
681  }
682  else if (m_new_twist < m_min_twist) {
683  delta[1] = m_min_twist - m_twist;
684  m_new_twist = m_min_twist;
685  clamp[1] = true;
686  }
687  }
688  }
689 
690  return (clamp[0] || clamp[1]);
691 }
692 
693 void IK_QElbowSegment::Lock(int dof, IK_QJacobian &jacobian, Vector3d &delta)
694 {
695  if (dof == 0) {
696  m_locked[0] = true;
697  jacobian.Lock(m_DoF_id, delta[0]);
698  }
699  else {
700  m_locked[1] = true;
701  jacobian.Lock(m_DoF_id + 1, delta[1]);
702  }
703 }
704 
706 {
707  m_angle = m_new_angle;
708  m_twist = m_new_twist;
709 
710  m_sin_twist = sin(m_twist);
711  m_cos_twist = cos(m_twist);
712 
713  Matrix3d A = RotationMatrix(m_angle, m_axis);
714  Matrix3d T = RotationMatrix(m_sin_twist, m_cos_twist, 1);
715 
716  m_basis = A * T;
717 }
718 
719 void IK_QElbowSegment::SetLimit(int axis, double lmin, double lmax)
720 {
721  if (lmin > lmax)
722  return;
723 
724  // clamp and convert to axis angle parameters
725  lmin = Clamp(lmin, -M_PI, M_PI);
726  lmax = Clamp(lmax, -M_PI, M_PI);
727 
728  if (axis == 1) {
729  m_min_twist = lmin;
730  m_max_twist = lmax;
731  m_limit_twist = true;
732  }
733  else if (axis == m_axis) {
734  m_min = lmin;
735  m_max = lmax;
736  m_limit = true;
737  }
738 }
739 
740 void IK_QElbowSegment::SetWeight(int axis, double weight)
741 {
742  if (axis == m_axis)
743  m_weight[0] = weight;
744  else if (axis == 1)
745  m_weight[1] = weight;
746 }
747 
748 // IK_QTranslateSegment
749 
751 {
752  m_axis_enabled[0] = m_axis_enabled[1] = m_axis_enabled[2] = false;
753  m_axis_enabled[axis1] = true;
754 
755  m_axis[0] = axis1;
756 
757  m_limit[0] = m_limit[1] = m_limit[2] = false;
758 }
759 
761 {
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;
765 
766  m_axis[0] = axis1;
767  m_axis[1] = axis2;
768 
769  m_limit[0] = m_limit[1] = m_limit[2] = false;
770 }
771 
773 {
774  m_axis_enabled[0] = m_axis_enabled[1] = m_axis_enabled[2] = true;
775 
776  m_axis[0] = 0;
777  m_axis[1] = 1;
778  m_axis[2] = 2;
779 
780  m_limit[0] = m_limit[1] = m_limit[2] = false;
781 }
782 
783 Vector3d IK_QTranslateSegment::Axis(int dof) const
784 {
785  return m_global_transform.linear().col(m_axis[dof]);
786 }
787 
788 bool IK_QTranslateSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp)
789 {
790  int dof_id = m_DoF_id, dof = 0, i, clamped = false;
791 
792  Vector3d dx(0.0, 0.0, 0.0);
793 
794  for (i = 0; i < 3; i++) {
795  if (!m_axis_enabled[i]) {
796  m_new_translation[i] = m_translation[i];
797  continue;
798  }
799 
800  clamp[dof] = false;
801 
802  if (!m_locked[dof]) {
803  m_new_translation[i] = m_translation[i] + jacobian.AngleUpdate(dof_id);
804 
805  if (m_limit[i]) {
806  if (m_new_translation[i] > m_max[i]) {
807  delta[dof] = m_max[i] - m_translation[i];
808  m_new_translation[i] = m_max[i];
809  clamped = clamp[dof] = true;
810  }
811  else if (m_new_translation[i] < m_min[i]) {
812  delta[dof] = m_min[i] - m_translation[i];
813  m_new_translation[i] = m_min[i];
814  clamped = clamp[dof] = true;
815  }
816  }
817  }
818 
819  dof_id++;
820  dof++;
821  }
822 
823  return clamped;
824 }
825 
827 {
828  m_translation = m_new_translation;
829 }
830 
831 void IK_QTranslateSegment::Lock(int dof, IK_QJacobian &jacobian, Vector3d &delta)
832 {
833  m_locked[dof] = true;
834  jacobian.Lock(m_DoF_id + dof, delta[dof]);
835 }
836 
837 void IK_QTranslateSegment::SetWeight(int axis, double weight)
838 {
839  int i;
840 
841  for (i = 0; i < m_num_DoF; i++)
842  if (m_axis[i] == axis)
843  m_weight[i] = weight;
844 }
845 
846 void IK_QTranslateSegment::SetLimit(int axis, double lmin, double lmax)
847 {
848  if (lmax < lmin)
849  return;
850 
851  m_min[axis] = lmin;
852  m_max[axis] = lmax;
853  m_limit[axis] = true;
854 }
855 
856 void IK_QTranslateSegment::Scale(double scale)
857 {
858  int i;
859 
860  IK_QSegment::Scale(scale);
861 
862  for (i = 0; i < 3; i++) {
863  m_min[0] *= scale;
864  m_max[1] *= scale;
865  }
866 
867  m_new_translation *= scale;
868 }
#define M_PI
Definition: BLI_math_base.h:38
static void RemoveTwist(Eigen::Matrix3d &R)
Definition: IK_Math.h:146
static bool EllipseClamp(double &ax, double &az, double *amin, double *amax)
Definition: IK_Math.h:205
static Eigen::Matrix3d CreateMatrix(double xx, double xy, double xz, double yx, double yy, double yz, double zx, double zy, double zz)
Definition: IK_Math.h:46
static Eigen::Vector3d MatrixToAxisAngle(const Eigen::Matrix3d &R)
Definition: IK_Math.h:192
static Eigen::Matrix3d RotationMatrix(double sine, double cosine, int axis)
Definition: IK_Math.h:69
static Eigen::Matrix3d ComputeTwistMatrix(double tau)
Definition: IK_Math.h:141
static Eigen::Vector3d SphericalRangeParameters(const Eigen::Matrix3d &R)
Definition: IK_Math.h:158
static Eigen::Matrix3d ComputeSwingMatrix(double ax, double az)
Definition: IK_Math.h:180
static bool FuzzyZero(double x)
Definition: IK_Math.h:36
Group RGB to Bright Vector Camera Clamp
ATTR_WARN_UNUSED_RESULT const BMVert * v
#define A
btAngularLimit m_limit
SIMD_FORCE_INLINE const btScalar & w() const
Return the w value.
Definition: btQuadWord.h:119
SIMD_FORCE_INLINE btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Definition: btQuaternion.h:895
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)
Definition: IK_QSegment.h:189
Vector3d m_start
Definition: IK_QSegment.h:216
virtual EIGEN_MAKE_ALIGNED_OPERATOR_NEW ~IK_QSegment()
Definition: IK_QSegment.cpp:89
Vector3d m_translation
Definition: IK_QSegment.h:219
Matrix3d m_basis
Definition: IK_QSegment.h:218
Vector3d m_orig_translation
Definition: IK_QSegment.h:223
Vector3d m_global_start
Definition: IK_QSegment.h:229
void UpdateTransform(const Affine3d &global)
IK_QSegment * m_sibling
Definition: IK_QSegment.h:211
bool m_locked[3]
Definition: IK_QSegment.h:235
Matrix3d m_orig_basis
Definition: IK_QSegment.h:222
Affine3d m_global_transform
Definition: IK_QSegment.h:230
double m_max_extension
Definition: IK_QSegment.h:226
virtual void Scale(double scale)
IK_QSegment * m_child
Definition: IK_QSegment.h:210
double m_weight[3]
Definition: IK_QSegment.h:237
Matrix3d m_rest_basis
Definition: IK_QSegment.h:217
IK_QSegment * m_parent
Definition: IK_QSegment.h:209
void SetComposite(IK_QSegment *seg)
Matrix3d BasisChange() const
Definition: IK_QSegment.cpp:79
IK_QSegment(int num_DoF, bool translational)
Definition: IK_QSegment.cpp:28
void PrependBasis(const Matrix3d &mat)
void SetParent(IK_QSegment *parent)
Definition: IK_QSegment.cpp:98
IK_QSegment * m_composite
Definition: IK_QSegment.h:212
void RemoveChild(IK_QSegment *child)
Vector3d TranslationChange() const
Definition: IK_QSegment.cpp:84
void SetTransform(const Vector3d &start, const Matrix3d &rest_basis, const Matrix3d &basis, const double length)
Definition: IK_QSegment.cpp:62
void Reset()
Definition: IK_QSegment.cpp:50
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 &)
void Scale(double scale)
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)
#define M
#define T
static unsigned a[3]
Definition: RandGen.cpp:92
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:319
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:311
ccl_device_inline int clamp(int a, int mn, int mx)
Definition: util_math.h:283