f7aeee090a1c615378f7418724d434ad6b2bc794
[blender.git] / intern / iksolver / intern / IK_CGChainSolver.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 CGChainSolver_h
33
34 #define CGChainSolver_h
35
36 /**
37  * @author Laurence Bourn
38  * @date 28/6/2001
39  */
40
41 #include "IK_ConjugateGradientSolver.h"
42 #include "IK_Chain.h"
43 #include "MT_Scalar.h"
44 #include "TNT/vec.h"
45 #include "MEM_SmartPtr.h"
46
47
48 /**
49  * This class is a concrete differentiable potenial function for
50  * an IK_Chain representing the distance to the goal. 
51  * @warning This method of solving IK problems is not as good
52  * as IK_JacobianSolver. I advise you to use that class instead.
53  */
54
55 class ChainPotential : 
56 public DifferentiablePotenialFunctionNd 
57 {
58 public :
59
60         static 
61                 ChainPotential *
62         New(
63                 IK_Chain &chain
64         );
65         
66         // End effector goal
67
68                 void
69         SetGoal(
70                 const MT_Vector3 goal
71         );
72
73         // Inherited from DifferentiablePotenialFunctionNd
74         //////////////////////////////////////////////////
75
76                 MT_Scalar 
77         Evaluate(
78                 const MT_Scalar x
79         );
80
81                 MT_Scalar
82         Derivative(
83                 const MT_Scalar x
84         );
85
86                 MT_Scalar
87         Evaluate(
88                 const TNT::Vector<MT_Scalar> &x
89         );
90
91                 void
92         Derivative(
93                 const TNT::Vector<MT_Scalar> &x,
94                 TNT::Vector<MT_Scalar> &dy
95         );
96
97         // return the dimension of the domain of the potenial
98         // function
99
100                 int
101         Dimension(
102         ) const {
103                 return m_dimension;
104         }
105         
106         ~ChainPotential(
107         ){
108         };
109         
110 private :
111
112                 MT_Scalar
113         DistancePotential(
114                 MT_Vector3 pos,
115                 MT_Vector3 goal
116         ) const;
117
118                 void
119         DistanceGradient(
120                 MT_Vector3 pos,
121                 MT_Vector3 goal
122         );
123
124         ChainPotential(
125                 IK_Chain & chain
126         ) :
127                 DifferentiablePotenialFunctionNd(),
128                 m_chain(chain),
129                 m_t_chain(chain),
130                 m_dimension (chain.Segments().size())
131         {
132         };
133
134         MT_Vector3 m_goal;
135         TNT::Vector<MT_Scalar> m_distance_grad;
136         TNT::Vector<MT_Scalar> m_angle_grad;
137         TNT::Vector<MT_Scalar> m_temp_pos;
138         TNT::Vector<MT_Scalar> m_temp_grad;
139
140         TNT::Vector<MT_Scalar> m_original_pos;
141         int m_dimension;
142
143         IK_Chain &m_chain;
144         IK_Chain m_t_chain; // deep copy 
145
146 };
147                 
148
149 class IK_CGChainSolver : public MEM_NonCopyable
150 {
151 public :
152         
153         
154         static
155                 IK_CGChainSolver *
156         New(
157         );
158
159                 bool
160         Solve(
161                 IK_Chain & chain,
162                 MT_Vector3 new_position,
163                 MT_Scalar tolerance
164         );
165
166         ~IK_CGChainSolver();    
167
168 private :
169
170         IK_CGChainSolver(
171         );
172
173         MEM_SmartPtr<ChainPotential>  m_potential;
174         MEM_SmartPtr<IK_ConjugateGradientSolver> m_grad_solver;
175 };
176
177
178 #endif