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