Initial revision
[blender.git] / intern / iksolver / intern / IK_Solver.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  * Contributor(s): none yet.
28  *
29  * ***** END GPL/BL DUAL LICENSE BLOCK *****
30  */
31
32 #define IK_USE_EXPMAP
33
34
35
36 #ifdef IK_USE_EXPMAP 
37 #       include "IK_QSolver_Class.h"
38 #else 
39 #       include "IK_Solver_Class.h"
40 #endif
41 #include "../extern/IK_solver.h"
42
43 #include <iostream.h>
44
45         IK_Chain_ExternPtr  
46 IK_CreateChain(
47         void
48 ) {
49
50         MEM_SmartPtr<IK_Chain_Extern> output (new IK_Chain_Extern);
51         MEM_SmartPtr<IK_QSolver_Class> output_void (IK_QSolver_Class::New());   
52         
53
54         if (output == NULL || output_void == NULL) {
55                 return NULL;
56         }
57
58         output->chain_dof = 0;
59         output->num_segments = 0;
60         output->segments = NULL;
61         output->intern = output_void.Release();
62         return output.Release();
63 };
64
65
66         int 
67 IK_LoadChain(
68         IK_Chain_ExternPtr chain,
69         IK_Segment_ExternPtr segments,
70         int num_segs
71 ) {
72
73         if (chain == NULL || segments == NULL) return 0;
74
75         IK_QSolver_Class *intern_cpp = (IK_QSolver_Class *)chain->intern;
76         if (intern_cpp == NULL) return 0;
77
78         std::vector<IK_QSegment> & segs = intern_cpp->Chain().Segments();       
79         if (segs.size() != num_segs) {
80                 segs = std::vector<IK_QSegment>(num_segs);
81         }
82
83         std::vector<IK_QSegment>::const_iterator seg_end= segs.end();
84         std::vector<IK_QSegment>::iterator seg_begin= segs.begin();
85
86         IK_Segment_ExternPtr extern_seg_it = segments;
87         
88
89         for (;seg_begin != seg_end; ++seg_begin,++extern_seg_it) {
90         
91                 MT_Point3 tr1(extern_seg_it->seg_start);
92
93                 MT_Matrix3x3 A(
94                         extern_seg_it->basis[0],extern_seg_it->basis[1],extern_seg_it->basis[2],
95                         extern_seg_it->basis[3],extern_seg_it->basis[4],extern_seg_it->basis[5],
96                         extern_seg_it->basis[6],extern_seg_it->basis[7],extern_seg_it->basis[8]
97                 );
98
99                 MT_Scalar length(extern_seg_it->length);
100
101
102                 *seg_begin = IK_QSegment( 
103                         tr1,A,length,MT_Vector3(0,0,0)
104                 );
105
106         }
107
108
109         intern_cpp->Chain().UpdateGlobalTransformations();
110         intern_cpp->Chain().ComputeJacobian();
111         
112         chain->num_segments = num_segs;
113         chain->chain_dof = intern_cpp->Chain().DoF();
114         chain->segments = segments;
115
116         return 1;
117 };              
118                 
119         int 
120 IK_SolveChain(
121         IK_Chain_ExternPtr chain,
122         float goal[3],
123         float tolerance,
124         int max_iterations,
125         float max_angle_change, 
126         IK_Segment_ExternPtr output
127 ){
128         if (chain == NULL || output == NULL) return 0;
129         if (chain->intern == NULL) return 0;
130
131         IK_QSolver_Class *intern_cpp = (IK_QSolver_Class *)chain->intern;
132
133         IK_QJacobianSolver & solver = intern_cpp->Solver();
134
135         bool solve_result = solver.Solve(
136                 intern_cpp->Chain(),
137                 MT_Vector3(goal),
138                 MT_Vector3(0,0,0),
139                 MT_Scalar(tolerance),
140                 max_iterations,
141                 MT_Scalar(max_angle_change)     
142         );
143         
144         // turn the computed role->pitch->yaw into a quaternion and 
145         // return the result in output
146         
147         std::vector<IK_QSegment> & segs = intern_cpp->Chain().Segments();       
148         std::vector<IK_QSegment>::const_iterator seg_end= segs.end();
149         std::vector<IK_QSegment>::iterator seg_begin= segs.begin();
150
151         for (;seg_begin != seg_end; ++seg_begin, ++output) {
152                 MT_Matrix3x3 qrot = seg_begin->ExpMap().getMatrix();
153
154                 // don't forget to transpose this qrot for use by blender!
155
156                 qrot.transpose(); // blender uses transpose here ????
157
158                 output->basis_change[0] = float(qrot[0][0]);
159                 output->basis_change[1] = float(qrot[0][1]);
160                 output->basis_change[2] = float(qrot[0][2]);
161                 output->basis_change[3] = float(qrot[1][0]);
162                 output->basis_change[4] = float(qrot[1][1]);
163                 output->basis_change[5] = float(qrot[1][2]);
164                 output->basis_change[6] = float(qrot[2][0]);
165                 output->basis_change[7] = float(qrot[2][1]);
166                 output->basis_change[8] = float(qrot[2][2]);
167
168         }
169
170
171         return solve_result ? 1 : 0;
172 }
173
174         void 
175 IK_FreeChain(
176         IK_Chain_ExternPtr chain
177 ){
178         IK_QSolver_Class *intern_cpp = (IK_QSolver_Class *)chain->intern;
179
180         delete(intern_cpp);
181         delete(chain);
182
183 }       
184
185
186
187
188
189         
190         
191
192
193         
194
195
196