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
20 /** \file
21 * \ingroup iksolver
22 */
23
24 #include "IK_QSegment.h"
25
26 // IK_QSegment
27
IK_QSegment(int num_DoF,bool translational)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
46 m_orig_basis = m_basis;
47 m_orig_translation = m_translation;
48 }
49
Reset()50 void IK_QSegment::Reset()
51 {
52 m_locked[0] = m_locked[1] = m_locked[2] = false;
53
54 m_basis = m_orig_basis;
55 m_translation = m_orig_translation;
56 SetBasis(m_basis);
57
58 for (IK_QSegment *seg = m_child; seg; seg = seg->m_sibling)
59 seg->Reset();
60 }
61
SetTransform(const Vector3d & start,const Matrix3d & rest_basis,const Matrix3d & basis,const double length)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);
76 m_orig_translation = m_translation;
77 }
78
BasisChange() const79 Matrix3d IK_QSegment::BasisChange() const
80 {
81 return m_orig_basis.transpose() * m_basis;
82 }
83
TranslationChange() const84 Vector3d IK_QSegment::TranslationChange() const
85 {
86 return m_translation - m_orig_translation;
87 }
88
~IK_QSegment()89 IK_QSegment::~IK_QSegment()
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
SetParent(IK_QSegment * parent)98 void IK_QSegment::SetParent(IK_QSegment *parent)
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
SetComposite(IK_QSegment * seg)114 void IK_QSegment::SetComposite(IK_QSegment *seg)
115 {
116 m_composite = seg;
117 }
118
RemoveChild(IK_QSegment * child)119 void IK_QSegment::RemoveChild(IK_QSegment *child)
120 {
121 if (m_child == NULL)
122 return;
123 else if (m_child == child)
124 m_child = m_child->m_sibling;
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
UpdateTransform(const Affine3d & global)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;
143 m_global_transform.translate(m_translation);
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
PrependBasis(const Matrix3d & mat)150 void IK_QSegment::PrependBasis(const Matrix3d &mat)
151 {
152 m_basis = m_rest_basis.inverse() * mat * m_rest_basis * m_basis;
153 }
154
Scale(double scale)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
IK_QSphericalSegment()167 IK_QSphericalSegment::IK_QSphericalSegment()
168 : IK_QSegment(3, false), m_limit_x(false), m_limit_y(false), m_limit_z(false)
169 {
170 }
171
Axis(int dof) const172 Vector3d IK_QSphericalSegment::Axis(int dof) const
173 {
174 return m_global_transform.linear().col(dof);
175 }
176
SetLimit(int axis,double lmin,double lmax)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
SetWeight(int axis,double weight)212 void IK_QSphericalSegment::SetWeight(int axis, double weight)
213 {
214 m_weight[axis] = weight;
215 }
216
UpdateAngle(const IK_QJacobian & jacobian,Vector3d & delta,bool * clamp)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
Lock(int dof,IK_QJacobian & jacobian,Vector3d & delta)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
UpdateAngleApply()351 void IK_QSphericalSegment::UpdateAngleApply()
352 {
353 m_basis = m_new_basis;
354 }
355
356 // IK_QNullSegment
357
IK_QNullSegment()358 IK_QNullSegment::IK_QNullSegment() : IK_QSegment(0, false)
359 {
360 }
361
362 // IK_QRevoluteSegment
363
IK_QRevoluteSegment(int axis)364 IK_QRevoluteSegment::IK_QRevoluteSegment(int axis)
365 : IK_QSegment(1, false), m_axis(axis), m_angle(0.0), m_limit(false)
366 {
367 }
368
SetBasis(const Matrix3d & basis)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
Axis(int) const381 Vector3d IK_QRevoluteSegment::Axis(int) const
382 {
383 return m_global_transform.linear().col(m_axis);
384 }
385
UpdateAngle(const IK_QJacobian & jacobian,Vector3d & delta,bool * clamp)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
Lock(int,IK_QJacobian & jacobian,Vector3d & delta)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
UpdateAngleApply()417 void IK_QRevoluteSegment::UpdateAngleApply()
418 {
419 m_angle = m_new_angle;
420 m_basis = RotationMatrix(m_angle, m_axis);
421 }
422
SetLimit(int axis,double lmin,double lmax)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
SetWeight(int axis,double weight)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
IK_QSwingSegment()446 IK_QSwingSegment::IK_QSwingSegment() : IK_QSegment(2, false), m_limit_x(false), m_limit_z(false)
447 {
448 }
449
SetBasis(const Matrix3d & basis)450 void IK_QSwingSegment::SetBasis(const Matrix3d &basis)
451 {
452 m_basis = basis;
453 RemoveTwist(m_basis);
454 }
455
Axis(int dof) const456 Vector3d IK_QSwingSegment::Axis(int dof) const
457 {
458 return m_global_transform.linear().col((dof == 0) ? 0 : 2);
459 }
460
UpdateAngle(const IK_QJacobian & jacobian,Vector3d & delta,bool * clamp)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
Lock(int,IK_QJacobian & jacobian,Vector3d & delta)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
UpdateAngleApply()562 void IK_QSwingSegment::UpdateAngleApply()
563 {
564 m_basis = m_new_basis;
565 }
566
SetLimit(int axis,double lmin,double lmax)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
SetWeight(int axis,double weight)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
IK_QElbowSegment(int axis)611 IK_QElbowSegment::IK_QElbowSegment(int axis)
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
SetBasis(const Matrix3d & basis)623 void IK_QElbowSegment::SetBasis(const Matrix3d &basis)
624 {
625 m_basis = basis;
626
627 m_twist = ComputeTwist(m_basis);
628 RemoveTwist(m_basis);
629 m_angle = EulerAngleFromMatrix(basis, m_axis);
630
631 m_basis = RotationMatrix(m_angle, m_axis) * ComputeTwistMatrix(m_twist);
632 }
633
Axis(int dof) const634 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
UpdateAngle(const IK_QJacobian & jacobian,Vector3d & delta,bool * clamp)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
Lock(int dof,IK_QJacobian & jacobian,Vector3d & delta)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
UpdateAngleApply()705 void IK_QElbowSegment::UpdateAngleApply()
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
SetLimit(int axis,double lmin,double lmax)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
SetWeight(int axis,double weight)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
IK_QTranslateSegment(int axis1)750 IK_QTranslateSegment::IK_QTranslateSegment(int axis1) : IK_QSegment(1, true)
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
IK_QTranslateSegment(int axis1,int axis2)760 IK_QTranslateSegment::IK_QTranslateSegment(int axis1, int axis2) : IK_QSegment(2, true)
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
IK_QTranslateSegment()772 IK_QTranslateSegment::IK_QTranslateSegment() : IK_QSegment(3, true)
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
Axis(int dof) const783 Vector3d IK_QTranslateSegment::Axis(int dof) const
784 {
785 return m_global_transform.linear().col(m_axis[dof]);
786 }
787
UpdateAngle(const IK_QJacobian & jacobian,Vector3d & delta,bool * clamp)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
UpdateAngleApply()826 void IK_QTranslateSegment::UpdateAngleApply()
827 {
828 m_translation = m_new_translation;
829 }
830
Lock(int dof,IK_QJacobian & jacobian,Vector3d & delta)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
SetWeight(int axis,double weight)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
SetLimit(int axis,double lmin,double lmax)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
Scale(double scale)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 }
869