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