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