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