f2863806112fccaa102df71a29fc897a4f1219a3
[blender.git] / intern / iksolver / test / ik_glut_test / intern / ChainDrawer.h
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 #ifndef __CHAINDRAWER_H__
29 #define __CHAINDRAWER_H__
30
31 #include "../common/GlutDrawer.h"
32 #include "MyGlutMouseHandler.h"
33 #include "MyGlutKeyHandler.h"
34 #include "MT_Transform.h"
35 #       include "IK_Qsolver.h"
36 #       include "../intern/IK_QChain.h"
37 #       include "../intern/IK_QSolver_Class.h"
38 #include <GL/glut.h>
39
40 class ChainDrawer : public GlutDrawer
41 {
42 public :
43         static
44                 ChainDrawer *
45         New(
46         ) {
47                 return new ChainDrawer();
48         }
49         
50                 void
51         SetMouseHandler(
52                 MyGlutMouseHandler *mouse_handler
53         ) {
54                 m_mouse_handler = mouse_handler;
55         }
56
57                 void
58         SetKeyHandler (
59                 MyGlutKeyHandler *key_handler
60         ) {
61                 m_key_handler = key_handler;
62         }
63
64                 void
65         SetChain(
66                 IK_Chain_ExternPtr *chains,int chain_num
67         ) {
68                 m_chain_num = chain_num;
69                 m_chains = chains;
70         }
71
72
73         // inherited from GlutDrawer
74                 void
75         Draw(
76         ) {
77           glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
78                   glPopMatrix();
79                   glPushMatrix();
80                   glRotatef(m_mouse_handler->AngleX(), 0.0, 1.0, 0.0);
81                   glRotatef(m_mouse_handler->AngleY(), 1.0, 0.0, 0.0);
82                         
83           DrawScene();
84           glutSwapBuffers();
85
86         }
87
88         ~ChainDrawer(
89         ){
90                 // nothing to do
91         };              
92         
93 private :
94
95                 void
96         DrawScene(
97         ){
98
99                 // draw a little cross at the position of the key handler
100                 // coordinates
101
102                 MT_Vector3 line_x(4,0,0);
103                 MT_Vector3 line_y(0.0,4,0);
104                 MT_Vector3 line_z(0.0,0.0,4);
105
106                 MT_Vector3 cross_origin = m_mouse_handler->Position();
107                 MT_Vector3 temp;
108                 
109                 glDisable(GL_LIGHTING);
110
111
112                 glBegin(GL_LINES);
113
114                 glColor3f (1.0f,1.0f,1.0f);
115
116                 temp = cross_origin - line_x;
117                 glVertex3f(temp[0],temp[1],temp[2]);
118                 temp = cross_origin + line_x;
119                 glVertex3f(temp[0],temp[1],temp[2]);
120
121                 temp = cross_origin - line_y;
122                 glVertex3f(temp[0],temp[1],temp[2]);
123                 temp = cross_origin + line_y;
124                 glVertex3f(temp[0],temp[1],temp[2]);
125
126                 temp = cross_origin - line_z;
127                 glVertex3f(temp[0],temp[1],temp[2]);
128                 temp = cross_origin + line_z;
129                 glVertex3f(temp[0],temp[1],temp[2]);
130
131                 glEnd();
132                 glEnable(GL_LIGHTING);
133
134
135                 IK_Chain_ExternPtr chain;
136
137                 int chain_num;
138                 for (chain_num = 0; chain_num < m_chain_num; chain_num++) {
139                         chain = m_chains[chain_num];
140
141
142                         IK_Segment_ExternPtr segs = chain->segments;
143                         IK_Segment_ExternPtr seg_start = segs;
144                         const IK_Segment_ExternPtr seg_end = segs + chain->num_segments;
145                         float ogl_matrix[16];
146
147                         glColor3f (0.0f,1.0f,0.0f);
148
149                         MT_Vector3 previous_origin(0,0,0);
150
151                         MT_Transform global_transform;
152                         global_transform.setIdentity();
153                         
154                         for (; seg_start != seg_end; ++seg_start) {
155                                 
156                                 glPushMatrix();
157
158                                 // fill ogl_matrix with zeros
159
160                                 std::fill(ogl_matrix,ogl_matrix + 16,float(0));
161
162                                 // we have to do a bit of work here to compute the chain's
163                                 // bone values
164                                         
165                                 // first compute all the matrices we need
166                                 
167                                 MT_Transform translation;
168                                 translation.setIdentity();
169                                 translation.translate(MT_Vector3(0,seg_start->length,0));
170
171                                 MT_Matrix3x3 seg_rot(
172                                         seg_start->basis_change[0],seg_start->basis_change[1],seg_start->basis_change[2],
173                                         seg_start->basis_change[3],seg_start->basis_change[4],seg_start->basis_change[5],
174                                         seg_start->basis_change[6],seg_start->basis_change[7],seg_start->basis_change[8]
175                                 );
176
177                                 seg_rot.transpose();
178
179                                 MT_Matrix3x3 seg_pre_rot(
180                                         seg_start->basis[0],seg_start->basis[1],seg_start->basis[2],
181                                         seg_start->basis[3],seg_start->basis[4],seg_start->basis[5],
182                                         seg_start->basis[6],seg_start->basis[7],seg_start->basis[8]
183                                 );
184
185                 
186                                 MT_Transform seg_t_pre_rot(
187                                         MT_Point3(
188                                                 seg_start->seg_start[0],
189                                                 seg_start->seg_start[1],
190                                                 seg_start->seg_start[2]
191                                         ),
192                                         seg_pre_rot
193                                 );
194                                 // start of the bone is just the current global transform
195                                 // multiplied by the seg_start vector
196
197                                 
198
199                                 MT_Transform seg_t_rot(MT_Point3(0,0,0),seg_rot);
200                                 MT_Transform seg_local = seg_t_pre_rot * seg_t_rot * translation;
201
202                                 MT_Vector3 bone_start = global_transform *      
203                                         MT_Point3(
204                                                 seg_start->seg_start[0],
205                                                 seg_start->seg_start[1],
206                                                 seg_start->seg_start[2]
207                                         );
208
209
210                                 global_transform = global_transform * seg_local;
211
212                                 global_transform.getValue(ogl_matrix);
213                                 MT_Vector3 bone_end = global_transform.getOrigin();
214
215                                 glMultMatrixf(ogl_matrix);
216 //                              glutSolidSphere(0.5,5,5);
217
218                                 glPopMatrix();
219                 
220                                 glDisable(GL_LIGHTING);
221
222                                 glBegin(GL_LINES);
223
224                                 // draw lines of the principle axis of the local transform
225
226                                 MT_Vector3 x_axis(1,0,0);
227                                 MT_Vector3 y_axis(0,1,0);
228                                 MT_Vector3 z_axis(0,0,1);
229
230                                 x_axis = global_transform.getBasis() * x_axis * 5;
231                                 y_axis = global_transform.getBasis() * y_axis * 5;
232                                 z_axis = global_transform.getBasis() * z_axis * 5;
233
234
235                                 x_axis = x_axis + bone_start;
236                                 y_axis = y_axis + bone_start;
237                                 z_axis = z_axis + bone_start;
238
239                                 glColor3f(1,0,0);
240
241                                 glVertex3f(x_axis.x(),x_axis.y(),x_axis.z());
242                                 glVertex3f(
243                                                 bone_start.x(),
244                                                 bone_start.y(),
245                                                 bone_start.z()
246                                 );
247
248                                 glColor3f(0,1,0);
249
250                                 glVertex3f(y_axis.x(),y_axis.y(),y_axis.z());
251                                 glVertex3f(
252                                                 bone_start.x(),
253                                                 bone_start.y(),
254                                                 bone_start.z()
255                                 );
256
257                                 glColor3f(0,1,1);
258
259                                 glVertex3f(z_axis.x(),z_axis.y(),z_axis.z());
260                                 glVertex3f(
261                                                 bone_start.x(),
262                                                 bone_start.y(),
263                                                 bone_start.z()
264                                 );
265
266                                 glColor3f(0,0,1);
267
268                                 glVertex3f(
269                                                 bone_start.x(),
270                                                 bone_start.y(),
271                                                 bone_start.z()
272                                 );
273                                 glVertex3f(bone_end[0],bone_end[1],bone_end[2]);
274
275                                 glEnd();
276                                 glEnable(GL_LIGHTING);
277                         }
278 #if 0
279                         // draw jacobian column vectors
280
281                         // hack access to internals
282
283                         IK_Solver_Class * internals = static_cast<IK_Solver_Class *>(chain->intern);
284
285                         glDisable(GL_LIGHTING);
286
287                         glBegin(GL_LINES);
288
289                         const TNT::Matrix<MT_Scalar> & jac = internals->Chain().TransposedJacobian();
290
291                         int i = 0;
292                         for (i=0; i < jac.num_rows(); i++) {
293                                 glColor3f(1,1,1);
294
295                                 previous_origin = internals->Chain().Segments()[i/3].GlobalSegmentStart();
296
297                                 glVertex3f(previous_origin[0],previous_origin[1],previous_origin[2]);
298                                 glVertex3f(jac[i][0] + previous_origin[0],jac[i][1] + previous_origin[1],jac[i][2] + previous_origin[2]);
299                         
300                                 
301                         }
302                         glEnd();
303                         glEnable(GL_LIGHTING);
304 #endif
305                         
306                 }
307
308                 glColor3f(1.0,1.0,1.0);
309
310                 glDisable(GL_LIGHTING);
311                 glBegin(GL_LINES);
312
313                 MT_Scalar cube_size = 50;
314                 glVertex3f(cube_size,cube_size,cube_size);
315                 glVertex3f(-cube_size,cube_size,cube_size);
316
317                 glVertex3f(cube_size,-cube_size,cube_size);
318                 glVertex3f(-cube_size,-cube_size,cube_size);
319                 
320                 glVertex3f(cube_size,cube_size,-cube_size);
321                 glVertex3f(-cube_size,cube_size,-cube_size);
322
323                 glVertex3f(cube_size,-cube_size,-cube_size);
324                 glVertex3f(-cube_size,-cube_size,-cube_size);
325
326
327                 glVertex3f(-cube_size,cube_size,cube_size);
328                 glVertex3f(-cube_size,-cube_size,cube_size);
329
330                 glVertex3f(cube_size,cube_size,-cube_size);
331                 glVertex3f(cube_size,-cube_size,-cube_size);
332
333                 glVertex3f(cube_size,cube_size,cube_size);
334                 glVertex3f(cube_size,-cube_size,cube_size);
335
336                 glVertex3f(-cube_size,cube_size,-cube_size);
337                 glVertex3f(-cube_size,-cube_size,-cube_size);
338
339
340                 glVertex3f(cube_size,cube_size,cube_size);
341                 glVertex3f(cube_size,cube_size,-cube_size);
342
343                 glVertex3f(cube_size,-cube_size,cube_size);
344                 glVertex3f(cube_size,-cube_size,-cube_size);
345
346                 glVertex3f(-cube_size,cube_size,cube_size);
347                 glVertex3f(-cube_size,cube_size,-cube_size);
348
349                 glVertex3f(-cube_size,-cube_size,cube_size);
350                 glVertex3f(-cube_size,-cube_size,-cube_size);
351                 glEnd();
352                 glEnable(GL_LIGHTING);
353
354         };
355
356
357
358 private :
359
360         MyGlutMouseHandler * m_mouse_handler;
361         MyGlutKeyHandler *m_key_handler;
362         IK_Chain_ExternPtr *m_chains;
363
364         int m_chain_num;
365         ChainDrawer (
366         ) : m_chains (NULL),
367                 m_mouse_handler (NULL),
368                 m_chain_num (0)
369         {
370         };
371         
372 };
373
374 #endif
375