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