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