style cleanup
[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 #include <cmath>
36
37 // Utility functions
38
39 static MT_Matrix3x3 RotationMatrix(MT_Scalar sine, MT_Scalar cosine, int axis)
40 {
41         if (axis == 0)
42                 return MT_Matrix3x3(1.0, 0.0, 0.0,
43                                     0.0, cosine, -sine,
44                                     0.0, sine, cosine);
45         else if (axis == 1)
46                 return MT_Matrix3x3(cosine, 0.0, sine,
47                                     0.0, 1.0, 0.0,
48                                     -sine, 0.0, cosine);
49         else
50                 return MT_Matrix3x3(cosine, -sine, 0.0,
51                                     sine, cosine, 0.0,
52                                     0.0, 0.0, 1.0);
53 }
54
55 static MT_Matrix3x3 RotationMatrix(MT_Scalar angle, int axis)
56 {
57         return RotationMatrix(sin(angle), cos(angle), axis);
58 }
59
60
61 static MT_Scalar EulerAngleFromMatrix(const MT_Matrix3x3& R, int axis)
62 {
63         MT_Scalar t = sqrt(R[0][0] * R[0][0] + R[0][1] * R[0][1]);
64
65         if (t > 16.0 * MT_EPSILON) {
66                 if      (axis == 0) return -atan2(R[1][2], R[2][2]);
67                 else if (axis == 1) return  atan2(-R[0][2], t);
68                 else                return -atan2(R[0][1], R[0][0]);
69         }
70         else {
71                 if      (axis == 0) return -atan2(-R[2][1], R[1][1]);
72                 else if (axis == 1) return  atan2(-R[0][2], t);
73                 else                return 0.0f;
74         }
75 }
76
77 static MT_Scalar safe_acos(MT_Scalar f)
78 {
79         if (f <= -1.0)
80                 return MT_PI;
81         else if (f >= 1.0)
82                 return 0.0;
83         else
84                 return acos(f);
85 }
86
87 static MT_Scalar ComputeTwist(const MT_Matrix3x3& R)
88 {
89         // qy and qw are the y and w components of the quaternion from R
90         MT_Scalar qy = R[0][2] - R[2][0];
91         MT_Scalar qw = R[0][0] + R[1][1] + R[2][2] + 1;
92
93         MT_Scalar tau = 2.0 * atan2(qy, qw);
94
95         return tau;
96 }
97
98 static MT_Matrix3x3 ComputeTwistMatrix(MT_Scalar tau)
99 {
100         return RotationMatrix(tau, 1);
101 }
102
103 static void RemoveTwist(MT_Matrix3x3& R)
104 {
105         // compute twist parameter
106         MT_Scalar tau = ComputeTwist(R);
107
108         // compute twist matrix
109         MT_Matrix3x3 T = ComputeTwistMatrix(tau);
110
111         // remove twist
112         R = R * T.transposed();
113 }
114
115 static MT_Vector3 SphericalRangeParameters(const MT_Matrix3x3& R)
116 {
117         // compute twist parameter
118         MT_Scalar tau = ComputeTwist(R);
119
120         // compute swing parameters
121         MT_Scalar num = 2.0 * (1.0 + R[1][1]);
122
123         // singularity at pi
124         if (MT_abs(num) < MT_EPSILON)
125                 // TODO: this does now rotation of size pi over z axis, but could
126                 // be any axis, how to deal with this i'm not sure, maybe don't
127                 // enforce limits at all then
128                 return MT_Vector3(0.0, tau, 1.0);
129
130         num = 1.0 / sqrt(num);
131         MT_Scalar ax = -R[2][1] * num;
132         MT_Scalar az =  R[0][1] * num;
133
134         return MT_Vector3(ax, tau, az);
135 }
136
137 static MT_Matrix3x3 ComputeSwingMatrix(MT_Scalar ax, MT_Scalar az)
138 {
139         // length of (ax, 0, az) = sin(theta/2)
140         MT_Scalar sine2 = ax * ax + az * az;
141         MT_Scalar cosine2 = sqrt((sine2 >= 1.0) ? 0.0 : 1.0 - sine2);
142
143         // compute swing matrix
144         MT_Matrix3x3 S(MT_Quaternion(ax, 0.0, az, -cosine2));
145
146         return S;
147 }
148
149 static MT_Vector3 MatrixToAxisAngle(const MT_Matrix3x3& R)
150 {
151         MT_Vector3 delta = MT_Vector3(R[2][1] - R[1][2],
152                                       R[0][2] - R[2][0],
153                                       R[1][0] - R[0][1]);
154
155         MT_Scalar c = safe_acos((R[0][0] + R[1][1] + R[2][2] - 1) / 2);
156         MT_Scalar l = delta.length();
157         
158         if (!MT_fuzzyZero(l))
159                 delta *= c / l;
160         
161         return delta;
162 }
163
164 static bool EllipseClamp(MT_Scalar& ax, MT_Scalar& az, MT_Scalar *amin, MT_Scalar *amax)
165 {
166         MT_Scalar xlim, zlim, x, z;
167
168         if (ax < 0.0) {
169                 x = -ax;
170                 xlim = -amin[0];
171         }
172         else {
173                 x = ax;
174                 xlim = amax[0];
175         }
176
177         if (az < 0.0) {
178                 z = -az;
179                 zlim = -amin[1];
180         }
181         else {
182                 z = az;
183                 zlim = amax[1];
184         }
185
186         if (MT_fuzzyZero(xlim) || MT_fuzzyZero(zlim)) {
187                 if (x <= xlim && z <= zlim)
188                         return false;
189
190                 if (x > xlim)
191                         x = xlim;
192                 if (z > zlim)
193                         z = zlim;
194         }
195         else {
196                 MT_Scalar invx = 1.0 / (xlim * xlim);
197                 MT_Scalar invz = 1.0 / (zlim * zlim);
198
199                 if ((x * x * invx + z * z * invz) <= 1.0)
200                         return false;
201
202                 if (MT_fuzzyZero(x)) {
203                         x = 0.0;
204                         z = zlim;
205                 }
206                 else {
207                         MT_Scalar rico = z / x;
208                         MT_Scalar old_x = x;
209                         x = sqrt(1.0 / (invx + invz * rico * rico));
210                         if (old_x < 0.0)
211                                 x = -x;
212                         z = rico * x;
213                 }
214         }
215
216         ax = (ax < 0.0) ? -x : x;
217         az = (az < 0.0) ? -z : z;
218
219         return true;
220 }
221
222 // IK_QSegment
223
224 IK_QSegment::IK_QSegment(int num_DoF, bool translational)
225         : m_parent(NULL), m_child(NULL), m_sibling(NULL), m_composite(NULL),
226         m_num_DoF(num_DoF), m_translational(translational)
227 {
228         m_locked[0] = m_locked[1] = m_locked[2] = false;
229         m_weight[0] = m_weight[1] = m_weight[2] = 1.0;
230
231         m_max_extension = 0.0;
232
233         m_start = MT_Vector3(0, 0, 0);
234         m_rest_basis.setIdentity();
235         m_basis.setIdentity();
236         m_translation = MT_Vector3(0, 0, 0);
237
238         m_orig_basis = m_basis;
239         m_orig_translation = m_translation;
240 }
241
242 void IK_QSegment::Reset()
243 {
244         m_locked[0] = m_locked[1] = m_locked[2] = false;
245
246         m_basis = m_orig_basis;
247         m_translation = m_orig_translation;
248         SetBasis(m_basis);
249
250         for (IK_QSegment *seg = m_child; seg; seg = seg->m_sibling)
251                 seg->Reset();
252 }
253
254 void IK_QSegment::SetTransform(
255     const MT_Vector3& start,
256     const MT_Matrix3x3& rest_basis,
257     const MT_Matrix3x3& basis,
258     const MT_Scalar length
259     )
260 {
261         m_max_extension = start.length() + length;      
262
263         m_start = start;
264         m_rest_basis = rest_basis;
265
266         m_orig_basis = basis;
267         SetBasis(basis);
268
269         m_translation = MT_Vector3(0, length, 0);
270         m_orig_translation = m_translation;
271 }
272
273 MT_Matrix3x3 IK_QSegment::BasisChange() const
274 {
275         return m_orig_basis.transposed() * m_basis;
276 }
277
278 MT_Vector3 IK_QSegment::TranslationChange() const
279 {
280         return m_translation - m_orig_translation;
281 }
282
283 IK_QSegment::~IK_QSegment()
284 {
285         if (m_parent)
286                 m_parent->RemoveChild(this);
287
288         for (IK_QSegment *seg = m_child; seg; seg = seg->m_sibling)
289                 seg->m_parent = NULL;
290 }
291
292 void IK_QSegment::SetParent(IK_QSegment *parent)
293 {
294         if (m_parent == parent)
295                 return;
296         
297         if (m_parent)
298                 m_parent->RemoveChild(this);
299         
300         if (parent) {
301                 m_sibling = parent->m_child;
302                 parent->m_child = this;
303         }
304
305         m_parent = parent;
306 }
307
308 void IK_QSegment::SetComposite(IK_QSegment *seg)
309 {
310         m_composite = seg;
311 }
312
313 void IK_QSegment::RemoveChild(IK_QSegment *child)
314 {
315         if (m_child == NULL)
316                 return;
317         else if (m_child == child)
318                 m_child = m_child->m_sibling;
319         else {
320                 IK_QSegment *seg = m_child;
321
322                 while (seg->m_sibling != child)
323                         seg = seg->m_sibling;
324
325                 if (child == seg->m_sibling)
326                         seg->m_sibling = child->m_sibling;
327         }
328 }
329
330 void IK_QSegment::UpdateTransform(const MT_Transform& global)
331 {
332         // compute the global transform at the end of the segment
333         m_global_start = global.getOrigin() + global.getBasis() * m_start;
334
335         m_global_transform.setOrigin(m_global_start);
336         m_global_transform.setBasis(global.getBasis() * m_rest_basis * m_basis);
337         m_global_transform.translate(m_translation);
338
339         // update child transforms
340         for (IK_QSegment *seg = m_child; seg; seg = seg->m_sibling)
341                 seg->UpdateTransform(m_global_transform);
342 }
343
344 void IK_QSegment::PrependBasis(const MT_Matrix3x3& mat)
345 {
346         m_basis = m_rest_basis.inverse() * mat * m_rest_basis * m_basis;
347 }
348
349 void IK_QSegment::Scale(MT_Scalar scale)
350 {
351         m_start *= scale;
352         m_translation *= scale;
353         m_orig_translation *= scale;
354         m_global_start *= scale;
355         m_global_transform.getOrigin() *= scale;
356         m_max_extension *= scale;
357 }
358
359 // IK_QSphericalSegment
360
361 IK_QSphericalSegment::IK_QSphericalSegment()
362         : IK_QSegment(3, false), m_limit_x(false), m_limit_y(false), m_limit_z(false)
363 {
364 }
365
366 MT_Vector3 IK_QSphericalSegment::Axis(int dof) const
367 {
368         return m_global_transform.getBasis().getColumn(dof);
369 }
370
371 void IK_QSphericalSegment::SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax)
372 {
373         if (lmin > lmax)
374                 return;
375         
376         if (axis == 1) {
377                 lmin = MT_clamp(lmin, -MT_PI, MT_PI);
378                 lmax = MT_clamp(lmax, -MT_PI, MT_PI);
379
380                 m_min_y = lmin;
381                 m_max_y = lmax;
382
383                 m_limit_y = true;
384         }
385         else {
386                 // clamp and convert to axis angle parameters
387                 lmin = MT_clamp(lmin, -MT_PI, MT_PI);
388                 lmax = MT_clamp(lmax, -MT_PI, MT_PI);
389
390                 lmin = sin(lmin * 0.5);
391                 lmax = sin(lmax * 0.5);
392
393                 if (axis == 0) {
394                         m_min[0] = -lmax;
395                         m_max[0] = -lmin;
396                         m_limit_x = true;
397                 }
398                 else if (axis == 2) {
399                         m_min[1] = -lmax;
400                         m_max[1] = -lmin;
401                         m_limit_z = true;
402                 }
403         }
404 }
405
406 void IK_QSphericalSegment::SetWeight(int axis, MT_Scalar weight)
407 {
408         m_weight[axis] = weight;
409 }
410
411 bool IK_QSphericalSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp)
412 {
413         if (m_locked[0] && m_locked[1] && m_locked[2])
414                 return false;
415
416         MT_Vector3 dq;
417         dq.x() = jacobian.AngleUpdate(m_DoF_id);
418         dq.y() = jacobian.AngleUpdate(m_DoF_id + 1);
419         dq.z() = jacobian.AngleUpdate(m_DoF_id + 2);
420
421         // Directly update the rotation matrix, with Rodrigues' rotation formula,
422         // to avoid singularities and allow smooth integration.
423
424         MT_Scalar theta = dq.length();
425
426         if (!MT_fuzzyZero(theta)) {
427                 MT_Vector3 w = dq * (1.0 / theta);
428
429                 MT_Scalar sine = sin(theta);
430                 MT_Scalar cosine = cos(theta);
431                 MT_Scalar cosineInv = 1 - cosine;
432
433                 MT_Scalar xsine = w.x() * sine;
434                 MT_Scalar ysine = w.y() * sine;
435                 MT_Scalar zsine = w.z() * sine;
436
437                 MT_Scalar xxcosine = w.x() * w.x() * cosineInv;
438                 MT_Scalar xycosine = w.x() * w.y() * cosineInv;
439                 MT_Scalar xzcosine = w.x() * w.z() * cosineInv;
440                 MT_Scalar yycosine = w.y() * w.y() * cosineInv;
441                 MT_Scalar yzcosine = w.y() * w.z() * cosineInv;
442                 MT_Scalar zzcosine = w.z() * w.z() * cosineInv;
443
444                 MT_Matrix3x3 M(
445                     cosine + xxcosine, -zsine + xycosine, ysine + xzcosine,
446                     zsine + xycosine, cosine + yycosine, -xsine + yzcosine,
447                     -ysine + xzcosine, xsine + yzcosine, cosine + zzcosine);
448
449                 m_new_basis = m_basis * M;
450         }
451         else
452                 m_new_basis = m_basis;
453
454         
455         if (m_limit_y == false && m_limit_x == false && m_limit_z == false)
456                 return false;
457
458         MT_Vector3 a = SphericalRangeParameters(m_new_basis);
459
460         if (m_locked[0])
461                 a.x() = m_locked_ax;
462         if (m_locked[1])
463                 a.y() = m_locked_ay;
464         if (m_locked[2])
465                 a.z() = m_locked_az;
466
467         MT_Scalar ax = a.x(), ay = a.y(), az = a.z();
468
469         clamp[0] = clamp[1] = clamp[2] =  false;
470         
471         if (m_limit_y) {
472                 if (a.y() > m_max_y) {
473                         ay = m_max_y;
474                         clamp[1] = true;
475                 }
476                 else if (a.y() < m_min_y) {
477                         ay = m_min_y;
478                         clamp[1] = true;
479                 }
480         }
481
482         if (m_limit_x && m_limit_z) {
483                 if (EllipseClamp(ax, az, m_min, m_max))
484                         clamp[0] = clamp[2] = true;
485         }
486         else if (m_limit_x) {
487                 if (ax < m_min[0]) {
488                         ax = m_min[0];
489                         clamp[0] = true;
490                 }
491                 else if (ax > m_max[0]) {
492                         ax = m_max[0];
493                         clamp[0] = true;
494                 }
495         }
496         else if (m_limit_z) {
497                 if (az < m_min[1]) {
498                         az = m_min[1];
499                         clamp[2] = true;
500                 }
501                 else if (az > m_max[1]) {
502                         az = m_max[1];
503                         clamp[2] = true;
504                 }
505         }
506
507         if (clamp[0] == false && clamp[1] == false && clamp[2] == false) {
508                 if (m_locked[0] || m_locked[1] || m_locked[2])
509                         m_new_basis = ComputeSwingMatrix(ax, az) * ComputeTwistMatrix(ay);
510                 return false;
511         }
512         
513         m_new_basis = ComputeSwingMatrix(ax, az) * ComputeTwistMatrix(ay);
514
515         delta = MatrixToAxisAngle(m_basis.transposed() * m_new_basis);
516
517         if (!(m_locked[0] || m_locked[2]) && (clamp[0] || clamp[2])) {
518                 m_locked_ax = ax;
519                 m_locked_az = az;
520         }
521
522         if (!m_locked[1] && clamp[1])
523                 m_locked_ay = ay;
524         
525         return true;
526 }
527
528 void IK_QSphericalSegment::Lock(int dof, IK_QJacobian& jacobian, MT_Vector3& delta)
529 {
530         if (dof == 1) {
531                 m_locked[1] = true;
532                 jacobian.Lock(m_DoF_id + 1, delta[1]);
533         }
534         else {
535                 m_locked[0] = m_locked[2] = true;
536                 jacobian.Lock(m_DoF_id, delta[0]);
537                 jacobian.Lock(m_DoF_id + 2, delta[2]);
538         }
539 }
540
541 void IK_QSphericalSegment::UpdateAngleApply()
542 {
543         m_basis = m_new_basis;
544 }
545
546 // IK_QNullSegment
547
548 IK_QNullSegment::IK_QNullSegment()
549         : IK_QSegment(0, false)
550 {
551 }
552
553 // IK_QRevoluteSegment
554
555 IK_QRevoluteSegment::IK_QRevoluteSegment(int axis)
556         : IK_QSegment(1, false), m_axis(axis), m_angle(0.0), m_limit(false)
557 {
558 }
559
560 void IK_QRevoluteSegment::SetBasis(const MT_Matrix3x3& basis)
561 {
562         if (m_axis == 1) {
563                 m_angle = ComputeTwist(basis);
564                 m_basis = ComputeTwistMatrix(m_angle);
565         }
566         else {
567                 m_angle = EulerAngleFromMatrix(basis, m_axis);
568                 m_basis = RotationMatrix(m_angle, m_axis);
569         }
570 }
571
572 MT_Vector3 IK_QRevoluteSegment::Axis(int) const
573 {
574         return m_global_transform.getBasis().getColumn(m_axis);
575 }
576
577 bool IK_QRevoluteSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp)
578 {
579         if (m_locked[0])
580                 return false;
581
582         m_new_angle = m_angle + jacobian.AngleUpdate(m_DoF_id);
583
584         clamp[0] = false;
585
586         if (m_limit == false)
587                 return false;
588
589         if (m_new_angle > m_max)
590                 delta[0] = m_max - m_angle;
591         else if (m_new_angle < m_min)
592                 delta[0] = m_min - m_angle;
593         else
594                 return false;
595         
596         clamp[0] = true;
597         m_new_angle = m_angle + delta[0];
598
599         return true;
600 }
601
602 void IK_QRevoluteSegment::Lock(int, IK_QJacobian& jacobian, MT_Vector3& delta)
603 {
604         m_locked[0] = true;
605         jacobian.Lock(m_DoF_id, delta[0]);
606 }
607
608 void IK_QRevoluteSegment::UpdateAngleApply()
609 {
610         m_angle = m_new_angle;
611         m_basis = RotationMatrix(m_angle, m_axis);
612 }
613
614 void IK_QRevoluteSegment::SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax)
615 {
616         if (lmin > lmax || m_axis != axis)
617                 return;
618         
619         // clamp and convert to axis angle parameters
620         lmin = MT_clamp(lmin, -MT_PI, MT_PI);
621         lmax = MT_clamp(lmax, -MT_PI, MT_PI);
622
623         m_min = lmin;
624         m_max = lmax;
625
626         m_limit = true;
627 }
628
629 void IK_QRevoluteSegment::SetWeight(int axis, MT_Scalar weight)
630 {
631         if (axis == m_axis)
632                 m_weight[0] = weight;
633 }
634
635 // IK_QSwingSegment
636
637 IK_QSwingSegment::IK_QSwingSegment()
638         : IK_QSegment(2, false), m_limit_x(false), m_limit_z(false)
639 {
640 }
641
642 void IK_QSwingSegment::SetBasis(const MT_Matrix3x3& basis)
643 {
644         m_basis = basis;
645         RemoveTwist(m_basis);
646 }
647
648 MT_Vector3 IK_QSwingSegment::Axis(int dof) const
649 {
650         return m_global_transform.getBasis().getColumn((dof == 0) ? 0 : 2);
651 }
652
653 bool IK_QSwingSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp)
654 {
655         if (m_locked[0] && m_locked[1])
656                 return false;
657
658         MT_Vector3 dq;
659         dq.x() = jacobian.AngleUpdate(m_DoF_id);
660         dq.y() = 0.0;
661         dq.z() = jacobian.AngleUpdate(m_DoF_id + 1);
662
663         // Directly update the rotation matrix, with Rodrigues' rotation formula,
664         // to avoid singularities and allow smooth integration.
665
666         MT_Scalar theta = dq.length();
667
668         if (!MT_fuzzyZero(theta)) {
669                 MT_Vector3 w = dq * (1.0 / theta);
670
671                 MT_Scalar sine = sin(theta);
672                 MT_Scalar cosine = cos(theta);
673                 MT_Scalar cosineInv = 1 - cosine;
674
675                 MT_Scalar xsine = w.x() * sine;
676                 MT_Scalar zsine = w.z() * sine;
677
678                 MT_Scalar xxcosine = w.x() * w.x() * cosineInv;
679                 MT_Scalar xzcosine = w.x() * w.z() * cosineInv;
680                 MT_Scalar zzcosine = w.z() * w.z() * cosineInv;
681
682                 MT_Matrix3x3 M(
683                     cosine + xxcosine, -zsine, xzcosine,
684                     zsine, cosine, -xsine,
685                     xzcosine, xsine, cosine + zzcosine);
686
687                 m_new_basis = m_basis * M;
688
689                 RemoveTwist(m_new_basis);
690         }
691         else
692                 m_new_basis = m_basis;
693
694         if (m_limit_x == false && m_limit_z == false)
695                 return false;
696
697         MT_Vector3 a = SphericalRangeParameters(m_new_basis);
698         MT_Scalar ax = 0, az = 0;
699
700         clamp[0] = clamp[1] = false;
701         
702         if (m_limit_x && m_limit_z) {
703                 ax = a.x();
704                 az = a.z();
705
706                 if (EllipseClamp(ax, az, m_min, m_max))
707                         clamp[0] = clamp[1] = true;
708         }
709         else if (m_limit_x) {
710                 if (ax < m_min[0]) {
711                         ax = m_min[0];
712                         clamp[0] = true;
713                 }
714                 else if (ax > m_max[0]) {
715                         ax = m_max[0];
716                         clamp[0] = true;
717                 }
718         }
719         else if (m_limit_z) {
720                 if (az < m_min[1]) {
721                         az = m_min[1];
722                         clamp[1] = true;
723                 }
724                 else if (az > m_max[1]) {
725                         az = m_max[1];
726                         clamp[1] = true;
727                 }
728         }
729
730         if (clamp[0] == false && clamp[1] == false)
731                 return false;
732
733         m_new_basis = ComputeSwingMatrix(ax, az);
734
735         delta = MatrixToAxisAngle(m_basis.transposed() * m_new_basis);
736         delta[1] = delta[2]; delta[2] = 0.0;
737
738         return true;
739 }
740
741 void IK_QSwingSegment::Lock(int, IK_QJacobian& jacobian, MT_Vector3& delta)
742 {
743         m_locked[0] = m_locked[1] = true;
744         jacobian.Lock(m_DoF_id, delta[0]);
745         jacobian.Lock(m_DoF_id + 1, delta[1]);
746 }
747
748 void IK_QSwingSegment::UpdateAngleApply()
749 {
750         m_basis = m_new_basis;
751 }
752
753 void IK_QSwingSegment::SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax)
754 {
755         if (lmin > lmax)
756                 return;
757         
758         // clamp and convert to axis angle parameters
759         lmin = MT_clamp(lmin, -MT_PI, MT_PI);
760         lmax = MT_clamp(lmax, -MT_PI, MT_PI);
761
762         lmin = sin(lmin * 0.5);
763         lmax = sin(lmax * 0.5);
764
765         // put center of ellispe in the middle between min and max
766         MT_Scalar offset = 0.5 * (lmin + lmax);
767         //lmax = lmax - offset;
768
769         if (axis == 0) {
770                 m_min[0] = -lmax;
771                 m_max[0] = -lmin;
772
773                 m_limit_x = true;
774                 m_offset_x = offset;
775                 m_max_x = lmax;
776         }
777         else if (axis == 2) {
778                 m_min[1] = -lmax;
779                 m_max[1] = -lmin;
780
781                 m_limit_z = true;
782                 m_offset_z = offset;
783                 m_max_z = lmax;
784         }
785 }
786
787 void IK_QSwingSegment::SetWeight(int axis, MT_Scalar weight)
788 {
789         if (axis == 0)
790                 m_weight[0] = weight;
791         else if (axis == 2)
792                 m_weight[1] = weight;
793 }
794
795 // IK_QElbowSegment
796
797 IK_QElbowSegment::IK_QElbowSegment(int axis)
798         : IK_QSegment(2, false), m_axis(axis), m_twist(0.0), m_angle(0.0),
799         m_cos_twist(1.0), m_sin_twist(0.0), m_limit(false), m_limit_twist(false)
800 {
801 }
802
803 void IK_QElbowSegment::SetBasis(const MT_Matrix3x3& basis)
804 {
805         m_basis = basis;
806
807         m_twist = ComputeTwist(m_basis);
808         RemoveTwist(m_basis);
809         m_angle = EulerAngleFromMatrix(basis, m_axis);
810
811         m_basis = RotationMatrix(m_angle, m_axis) * ComputeTwistMatrix(m_twist);
812 }
813
814 MT_Vector3 IK_QElbowSegment::Axis(int dof) const
815 {
816         if (dof == 0) {
817                 MT_Vector3 v;
818                 if (m_axis == 0)
819                         v = MT_Vector3(m_cos_twist, 0, m_sin_twist);
820                 else
821                         v = MT_Vector3(-m_sin_twist, 0, m_cos_twist);
822
823                 return m_global_transform.getBasis() * v;
824         }
825         else
826                 return m_global_transform.getBasis().getColumn(1);
827 }
828
829 bool IK_QElbowSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp)
830 {
831         if (m_locked[0] && m_locked[1])
832                 return false;
833
834         clamp[0] = clamp[1] = false;
835
836         if (!m_locked[0]) {
837                 m_new_angle = m_angle + jacobian.AngleUpdate(m_DoF_id);
838
839                 if (m_limit) {
840                         if (m_new_angle > m_max) {
841                                 delta[0] = m_max - m_angle;
842                                 m_new_angle = m_max;
843                                 clamp[0] = true;
844                         }
845                         else if (m_new_angle < m_min) {
846                                 delta[0] = m_min - m_angle;
847                                 m_new_angle = m_min;
848                                 clamp[0] = true;
849                         }
850                 }
851         }
852
853         if (!m_locked[1]) {
854                 m_new_twist = m_twist + jacobian.AngleUpdate(m_DoF_id + 1);
855
856                 if (m_limit_twist) {
857                         if (m_new_twist > m_max_twist) {
858                                 delta[1] = m_max_twist - m_twist;
859                                 m_new_twist = m_max_twist;
860                                 clamp[1] = true;
861                         }
862                         else if (m_new_twist < m_min_twist) {
863                                 delta[1] = m_min_twist - m_twist;
864                                 m_new_twist = m_min_twist;
865                                 clamp[1] = true;
866                         }
867                 }
868         }
869
870         return (clamp[0] || clamp[1]);
871 }
872
873 void IK_QElbowSegment::Lock(int dof, IK_QJacobian& jacobian, MT_Vector3& delta)
874 {
875         if (dof == 0) {
876                 m_locked[0] = true;
877                 jacobian.Lock(m_DoF_id, delta[0]);
878         }
879         else {
880                 m_locked[1] = true;
881                 jacobian.Lock(m_DoF_id + 1, delta[1]);
882         }
883 }
884
885 void IK_QElbowSegment::UpdateAngleApply()
886 {
887         m_angle = m_new_angle;
888         m_twist = m_new_twist;
889
890         m_sin_twist = sin(m_twist);
891         m_cos_twist = cos(m_twist);
892
893         MT_Matrix3x3 A = RotationMatrix(m_angle, m_axis);
894         MT_Matrix3x3 T = RotationMatrix(m_sin_twist, m_cos_twist, 1);
895
896         m_basis = A * T;
897 }
898
899 void IK_QElbowSegment::SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax)
900 {
901         if (lmin > lmax)
902                 return;
903
904         // clamp and convert to axis angle parameters
905         lmin = MT_clamp(lmin, -MT_PI, MT_PI);
906         lmax = MT_clamp(lmax, -MT_PI, MT_PI);
907
908         if (axis == 1) {
909                 m_min_twist = lmin;
910                 m_max_twist = lmax;
911                 m_limit_twist = true;
912         }
913         else if (axis == m_axis) {
914                 m_min = lmin;
915                 m_max = lmax;
916                 m_limit = true;
917         }
918 }
919
920 void IK_QElbowSegment::SetWeight(int axis, MT_Scalar weight)
921 {
922         if (axis == m_axis)
923                 m_weight[0] = weight;
924         else if (axis == 1)
925                 m_weight[1] = weight;
926 }
927
928 // IK_QTranslateSegment
929
930 IK_QTranslateSegment::IK_QTranslateSegment(int axis1)
931         : IK_QSegment(1, true)
932 {
933         m_axis_enabled[0] = m_axis_enabled[1] = m_axis_enabled[2] = false;
934         m_axis_enabled[axis1] = true;
935
936         m_axis[0] = axis1;
937
938         m_limit[0] = m_limit[1] = m_limit[2] = false;
939 }
940
941 IK_QTranslateSegment::IK_QTranslateSegment(int axis1, int axis2)
942         : IK_QSegment(2, true)
943 {
944         m_axis_enabled[0] = m_axis_enabled[1] = m_axis_enabled[2] = false;
945         m_axis_enabled[axis1] = true;
946         m_axis_enabled[axis2] = true;
947
948         m_axis[0] = axis1;
949         m_axis[1] = axis2;
950
951         m_limit[0] = m_limit[1] = m_limit[2] = false;
952 }
953
954 IK_QTranslateSegment::IK_QTranslateSegment()
955         : IK_QSegment(3, true)
956 {
957         m_axis_enabled[0] = m_axis_enabled[1] = m_axis_enabled[2] = true;
958
959         m_axis[0] = 0;
960         m_axis[1] = 1;
961         m_axis[2] = 2;
962
963         m_limit[0] = m_limit[1] = m_limit[2] = false;
964 }
965
966 MT_Vector3 IK_QTranslateSegment::Axis(int dof) const
967 {
968         return m_global_transform.getBasis().getColumn(m_axis[dof]);
969 }
970
971 bool IK_QTranslateSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp)
972 {
973         int dof_id = m_DoF_id, dof = 0, i, clamped = false;
974
975         MT_Vector3 dx(0.0, 0.0, 0.0);
976
977         for (i = 0; i < 3; i++) {
978                 if (!m_axis_enabled[i]) {
979                         m_new_translation[i] = m_translation[i];
980                         continue;
981                 }
982
983                 clamp[dof] = false;
984
985                 if (!m_locked[dof]) {
986                         m_new_translation[i] = m_translation[i] + jacobian.AngleUpdate(dof_id);
987
988                         if (m_limit[i]) {
989                                 if (m_new_translation[i] > m_max[i]) {
990                                         delta[dof] = m_max[i] - m_translation[i];
991                                         m_new_translation[i] = m_max[i];
992                                         clamped = clamp[dof] = true;
993                                 }
994                                 else if (m_new_translation[i] < m_min[i]) {
995                                         delta[dof] = m_min[i] - m_translation[i];
996                                         m_new_translation[i] = m_min[i];
997                                         clamped = clamp[dof] = true;
998                                 }
999                         }
1000                 }
1001
1002                 dof_id++;
1003                 dof++;
1004         }
1005
1006         return clamped;
1007 }
1008
1009 void IK_QTranslateSegment::UpdateAngleApply()
1010 {
1011         m_translation = m_new_translation;
1012 }
1013
1014 void IK_QTranslateSegment::Lock(int dof, IK_QJacobian& jacobian, MT_Vector3& delta)
1015 {
1016         m_locked[dof] = true;
1017         jacobian.Lock(m_DoF_id + dof, delta[dof]);
1018 }
1019
1020 void IK_QTranslateSegment::SetWeight(int axis, MT_Scalar weight)
1021 {
1022         int i;
1023
1024         for (i = 0; i < m_num_DoF; i++)
1025                 if (m_axis[i] == axis)
1026                         m_weight[i] = weight;
1027 }
1028
1029 void IK_QTranslateSegment::SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax)
1030 {
1031         if (lmax < lmin)
1032                 return;
1033
1034         m_min[axis] = lmin;
1035         m_max[axis] = lmax;
1036         m_limit[axis] = true;
1037 }
1038
1039 void IK_QTranslateSegment::Scale(MT_Scalar scale)
1040 {
1041         int i;
1042
1043         IK_QSegment::Scale(scale);
1044
1045         for (i = 0; i < 3; i++) {
1046                 m_min[0] *= scale;
1047                 m_max[1] *= scale;
1048         }
1049
1050         m_new_translation *= scale;
1051 }
1052