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