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