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