bfb9d8fa1a05c468dfc7c0308bfc7764adfa856b
[blender.git] / intern / iksolver / test / ik_glut_test / intern / main.cpp
1 /**
2  * ***** BEGIN GPL LICENSE BLOCK *****
3  *
4  * This program is free software; you can redistribute it and/or
5  * modify it under the terms of the GNU General Public License
6  * as published by the Free Software Foundation; either version 2
7  * of the License, or (at your option) any later version.
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12  * GNU General Public License for more details.
13  *
14  * You should have received a copy of the GNU General Public License
15  * along with this program; if not, write to the Free Software Foundation,
16  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
17  *
18  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
19  * All rights reserved.
20  *
21  * The Original Code is: all of this file.
22  *
23  * Contributor(s): none yet.
24  *
25  * ***** END GPL LICENSE BLOCK *****
26  */
27
28 #include "MEM_SmartPtr.h"
29
30 #ifdef USE_QUATERNIONS
31 #include "IK_Qsolver.h"
32 #else 
33 #include "IK_solver.h"
34 #endif
35
36 #include <GL/glut.h>
37 #include "MT_Vector3.h"
38 #include "MT_Quaternion.h"
39 #include "MT_Matrix3x3.h"
40 #include "MyGlutMouseHandler.h" 
41 #include "MyGlutKeyHandler.h"
42 #include "ChainDrawer.h"
43
44 void init(MT_Vector3 min,MT_Vector3 max)
45 {
46         GLfloat light_diffuse0[] = {1.0, 0.0, 0.0, 1.0};  /* Red diffuse light. */
47         GLfloat light_position0[] = {1.0, 1.0, 1.0, 0.0};  /* Infinite light location. */
48
49         GLfloat light_diffuse1[] = {1.0, 1.0, 1.0, 1.0};  /* Red diffuse light. */
50         GLfloat light_position1[] = {1.0, 0, 0, 0.0};  /* Infinite light location. */
51
52         /* Enable a single OpenGL light. */
53         glLightfv(GL_LIGHT0, GL_DIFFUSE, light_diffuse0);
54         glLightfv(GL_LIGHT0, GL_POSITION, light_position0);
55
56         glLightfv(GL_LIGHT1, GL_DIFFUSE, light_diffuse1);
57         glLightfv(GL_LIGHT1, GL_POSITION, light_position1);
58
59         glEnable(GL_LIGHT0);
60         glEnable(GL_LIGHT1);
61         glEnable(GL_LIGHTING);
62
63         /* Use depth buffering for hidden surface elimination. */
64         glEnable(GL_DEPTH_TEST);
65
66         /* Setup the view of the cube. */
67         glMatrixMode(GL_PROJECTION);
68
69         // center of the box + 3* depth of box
70
71         MT_Vector3 center = (min + max) * 0.5;
72         MT_Vector3 diag = max - min;
73
74         float depth = diag.length();
75         float distance = 2;
76
77         gluPerspective(/* field of view in degree */ 40.0,
78                        /* aspect ratio */ 1.0,
79                        /* Z near */ 1.0,
80                        /* Z far */ distance * depth * 2
81                        );
82         glMatrixMode(GL_MODELVIEW);
83
84
85         gluLookAt(center.x(), center.y(), center.z() + distance*depth,  /* eye is at (0,0,5) */
86                   center.x(), center.y(), center.z(),      /* center is at (0,0,0) */
87                   0.0, 1.0, 0.);      /* up is in positive Y direction */
88
89         glPushMatrix();
90
91
92 }
93
94 int main(int argc, char **argv)
95 {
96
97
98         const int seg_num = 5;
99         const MT_Scalar seg_length = 15;
100
101         const float seg_startA[3] = {0,0,0};
102         const float seg_startB[3] = {0,-20,0};
103
104         // create some segments to solve with
105
106         // First chain
107         //////////////
108
109
110         IK_Segment_ExternPtr const segmentsA = new IK_Segment_Extern[seg_num];  
111         IK_Segment_ExternPtr const segmentsB = new IK_Segment_Extern[seg_num];  
112
113         IK_Segment_ExternPtr seg_it = segmentsA;        
114         IK_Segment_ExternPtr seg_itB = segmentsB;       
115
116         
117         {
118
119 //              MT_Quaternion qmat(MT_Vector3(0,0,1),-3.141/2);
120                 MT_Quaternion qmat(MT_Vector3(0,0,1),0);
121                 MT_Matrix3x3 mat(qmat);
122
123                 seg_it->seg_start[0] = seg_startA[0];
124                 seg_it->seg_start[1] = seg_startA[1];
125                 seg_it->seg_start[2] = seg_startA[2];
126
127                 float temp[12];
128                 mat.getValue(temp);
129  
130                 seg_it->basis[0] = temp[0];
131                 seg_it->basis[1] = temp[1];
132                 seg_it->basis[2] = temp[2];
133
134                 seg_it->basis[3] = temp[4];
135                 seg_it->basis[4] = temp[5];
136                 seg_it->basis[5] = temp[6];
137
138                 seg_it->basis[6] = temp[8];
139                 seg_it->basis[7] = temp[9];
140                 seg_it->basis[8] = temp[10];
141  
142                 seg_it->length = seg_length;
143
144                 MT_Quaternion q;
145                 q.setEuler(0,0,0);
146
147                         
148                 MT_Matrix3x3 qrot(q);
149
150                 seg_it->basis_change[0] = 1;
151                 seg_it->basis_change[1] = 0;
152                 seg_it->basis_change[2] = 0;
153                 seg_it->basis_change[3] = 0;
154                 seg_it->basis_change[4] = 1;
155                 seg_it->basis_change[5] = 0;
156                 seg_it->basis_change[6] = 0;
157                 seg_it->basis_change[7] = 0;
158                 seg_it->basis_change[8] = 1;
159
160
161                 seg_it ++;                      
162
163                 seg_itB->seg_start[0] = seg_startA[0];
164                 seg_itB->seg_start[1] = seg_startA[1];
165                 seg_itB->seg_start[2] = seg_startA[2];
166  
167                 seg_itB->basis[0] = temp[0];
168                 seg_itB->basis[1] = temp[1];
169                 seg_itB->basis[2] = temp[2];
170
171                 seg_itB->basis[3] = temp[4];
172                 seg_itB->basis[4] = temp[5];
173                 seg_itB->basis[5] = temp[6];
174
175                 seg_itB->basis[6] = temp[8];
176                 seg_itB->basis[7] = temp[9];
177                 seg_itB->basis[8] = temp[10];
178  
179                 seg_itB->length = seg_length;
180
181                 seg_itB->basis_change[0] = 1;
182                 seg_itB->basis_change[1] = 0;
183                 seg_itB->basis_change[2] = 0;
184                 seg_itB->basis_change[3] = 0;
185                 seg_itB->basis_change[4] = 1;
186                 seg_itB->basis_change[5] = 0;
187                 seg_itB->basis_change[6] = 0;
188                 seg_itB->basis_change[7] = 0;
189                 seg_itB->basis_change[8] = 1;
190
191
192                 seg_itB ++;                     
193
194
195         }
196
197
198         int i;
199         for (i=1; i < seg_num; ++i, ++seg_it,++seg_itB) {
200
201                 MT_Quaternion qmat(MT_Vector3(0,0,1),0.3);
202                 MT_Matrix3x3 mat(qmat);
203
204                 seg_it->seg_start[0] = 0;
205                 seg_it->seg_start[1] = 0;
206                 seg_it->seg_start[2] = 0;
207
208                 float temp[12];
209                 mat.getValue(temp);
210  
211                 seg_it->basis[0] = temp[0];
212                 seg_it->basis[1] = temp[1];
213                 seg_it->basis[2] = temp[2];
214
215                 seg_it->basis[3] = temp[4];
216                 seg_it->basis[4] = temp[5];
217                 seg_it->basis[5] = temp[6];
218
219                 seg_it->basis[6] = temp[8];
220                 seg_it->basis[7] = temp[9];
221                 seg_it->basis[8] = temp[10];
222  
223                 seg_it->length = seg_length;
224
225                 MT_Quaternion q;
226                 q.setEuler(0,0,0);
227
228                         
229                 MT_Matrix3x3 qrot(q);
230
231                 seg_it->basis_change[0] = 1;
232                 seg_it->basis_change[1] = 0;
233                 seg_it->basis_change[2] = 0;
234                 seg_it->basis_change[3] = 0;
235                 seg_it->basis_change[4] = 1;
236                 seg_it->basis_change[5] = 0;
237                 seg_it->basis_change[6] = 0;
238                 seg_it->basis_change[7] = 0;
239                 seg_it->basis_change[8] = 1;
240
241
242                 ///////////////////////////////
243
244                 seg_itB->seg_start[0] = 0;
245                 seg_itB->seg_start[1] = 0;
246                 seg_itB->seg_start[2] = 0;
247  
248                 seg_itB->basis[0] = temp[0];
249                 seg_itB->basis[1] = temp[1];
250                 seg_itB->basis[2] = temp[2];
251
252                 seg_itB->basis[3] = temp[4];
253                 seg_itB->basis[4] = temp[5];
254                 seg_itB->basis[5] = temp[6];
255
256                 seg_itB->basis[6] = temp[8];
257                 seg_itB->basis[7] = temp[9];
258                 seg_itB->basis[8] = temp[10];
259  
260                 seg_itB->length = seg_length;
261
262                 seg_itB->basis_change[0] = 1;
263                 seg_itB->basis_change[1] = 0;
264                 seg_itB->basis_change[2] = 0;
265                 seg_itB->basis_change[3] = 0;
266                 seg_itB->basis_change[4] = 1;
267                 seg_itB->basis_change[5] = 0;
268                 seg_itB->basis_change[6] = 0;
269                 seg_itB->basis_change[7] = 0;
270                 seg_itB->basis_change[8] = 1;
271
272
273
274         }
275
276         // create the chains
277
278         const int num_chains = 2;
279
280         IK_Chain_ExternPtr chains[num_chains];
281
282         chains[0] = IK_CreateChain();
283         chains[1] = IK_CreateChain();
284
285         // load segments into chain
286
287         IK_LoadChain(chains[0],segmentsA,seg_num);
288         IK_LoadChain(chains[1],segmentsB,seg_num);
289
290         // make and install a mouse handler
291
292         MEM_SmartPtr<MyGlutMouseHandler> mouse_handler (MyGlutMouseHandler::New());
293         GlutMouseManager::Instance()->InstallHandler(mouse_handler);
294
295         mouse_handler->SetChain(chains,num_chains);
296
297         // make and install a keyhandler
298         MEM_SmartPtr<MyGlutKeyHandler> key_handler (MyGlutKeyHandler::New());
299         GlutKeyboardManager::Instance()->InstallHandler(key_handler);
300
301         // instantiate the drawing class        
302
303         MEM_SmartPtr<ChainDrawer> drawer (ChainDrawer::New());
304         GlutDrawManager::Instance()->InstallDrawer(drawer);
305
306         drawer->SetMouseHandler(mouse_handler);
307         drawer->SetChain(chains,num_chains);
308         drawer->SetKeyHandler(key_handler);
309
310         glutInit(&argc, argv);
311         glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
312         glutCreateWindow("ik");
313         glutDisplayFunc(GlutDrawManager::Draw);
314         glutMouseFunc(GlutMouseManager::Mouse);
315         glutMotionFunc(GlutMouseManager::Motion);
316         glutKeyboardFunc(GlutKeyboardManager::HandleKeyboard);
317
318         init(MT_Vector3(-50,-50,-50),MT_Vector3(50,50,50));
319         glutMainLoop();
320         return 0;             /* ANSI C requires main to return int. */
321 }