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