Initial revision
[blender.git] / intern / iksolver / intern / IK_QJacobianSolver.h
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  * Contributor(s): none yet.
28  *
29  * ***** END GPL/BL DUAL LICENSE BLOCK *****
30  */
31
32 #ifndef NAN_INCLUDED_IK_QJacobianSolver_h
33
34 #define NAN_INCLUDED_IK_QJacobianSolver_h
35
36 /**
37  * @author Laurence Bourn
38  * @date 28/6/2001
39  */
40
41 #include "TNT/cmat.h"
42 #include <vector>
43 #include "MT_Vector3.h"
44 #include "IK_QChain.h"
45
46 class IK_QJacobianSolver {
47
48 public :
49
50         /**
51          * Create a new IK_QJacobianSolver on the heap
52          * @return A newly created IK_QJacobianSolver you take ownership of the object
53      * and responsibility for deleting it
54          */
55
56
57         static 
58                 IK_QJacobianSolver *
59         New(
60         );
61
62         /**
63          * Compute a solution for a chain.
64      * @param chain Reference to the chain to modify
65          * @param g_position Reference to the goal position.
66          * @param g_pose -not used- Reference to the goal pose. 
67          * @param tolerance The maximum allowed distance between solution
68      * and goal for termination.
69          * @param max_iterations should be in the range (50 - 500) 
70      * @param max_angle_change The maximum change in the angle vector 
71      * of the chain (0.1 is a good value)
72          *
73      * @return True iff goal position reached.
74      */
75
76                 bool
77         Solve(
78                 IK_QChain &chain,
79                 const MT_Vector3 &g_position,
80                 const MT_Vector3 &g_pose,
81                 const MT_Scalar tolerance,
82                 const int max_iterations,
83                 const MT_Scalar max_angle_change
84         );
85
86         ~IK_QJacobianSolver(
87         );
88
89
90 private :
91
92         /**
93          * Private constructor to force use of New()
94          */
95
96         IK_QJacobianSolver(
97         );
98
99
100         /** 
101          * Compute the inverse jacobian matrix of the chain.
102          * Uses a damped least squares solution when the matrix is 
103          * is approaching singularity
104      */
105
106                 int
107         ComputeInverseJacobian(
108                 IK_QChain &chain,
109                 const MT_Scalar x_length,
110                 const MT_Scalar max_angle_change
111         );
112
113                 void
114         ComputeBetas(
115                 IK_QChain &chain,
116                 const MT_Vector3 d_pos
117         );
118
119         /** 
120          * Updates the angles of the chain with the newly
121          * computed values
122      */
123
124                 void
125         UpdateChain(
126                 IK_QChain &chain
127         );
128         
129         /**
130          * Make sure all the matrices are of the correct size
131      */
132
133                 void
134         ArmMatrices(
135                 int dof
136         );
137
138         /**
139          * Quick check to see if all the segments are parallel
140          * to the goal direction.
141          */
142
143                 bool
144         ParallelCheck(
145                 const IK_QChain &chain,
146                 const MT_Vector3 goal
147         ) const;
148         
149
150
151 private :
152
153         /// the vector of intermediate betas
154         TNT::Vector<MT_Scalar> m_beta;
155
156         /// the vector of computed angle changes
157         TNT::Vector<MT_Scalar> m_d_theta;
158
159         /// the constraint gradients for each angle.
160         TNT::Vector<MT_Scalar> m_dh;
161
162         /// Space required for SVD computation
163
164         TNT::Vector<MT_Scalar> m_svd_w;
165     TNT::Vector<MT_Scalar> m_svd_work_space;        
166         TNT::Matrix<MT_Scalar> m_svd_v;
167         TNT::Matrix<MT_Scalar> m_svd_u;
168
169         TNT::Matrix<MT_Scalar> m_svd_w_diag;
170         TNT::Matrix<MT_Scalar> m_svd_v_t;
171         TNT::Matrix<MT_Scalar> m_svd_u_t;
172         TNT::Matrix<MT_Scalar> m_svd_inverse;
173         TNT::Matrix<MT_Scalar> m_svd_temp1;
174
175
176 };
177
178
179
180
181 #endif
182
183
184