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