diff options
Diffstat (limited to 'intern/iksolver/test/ik_glut_test/intern')
5 files changed, 1094 insertions, 0 deletions
diff --git a/intern/iksolver/test/ik_glut_test/intern/ChainDrawer.h b/intern/iksolver/test/ik_glut_test/intern/ChainDrawer.h new file mode 100644 index 00000000000..a164e686e37 --- /dev/null +++ b/intern/iksolver/test/ik_glut_test/intern/ChainDrawer.h @@ -0,0 +1,387 @@ +/** + * $Id$ + * ***** BEGIN GPL/BL DUAL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. The Blender + * Foundation also sells licenses for use in proprietary software under + * the Blender License. See http://www.blender.org/BL/ for information + * about this. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Contributor(s): none yet. + * + * ***** END GPL/BL DUAL LICENSE BLOCK ***** + */ + +#ifndef NAN_INCLUDED_ChainDrawer_h + +#define NAN_INCLUDED_ChainDrawer_h + +#include "../common/GlutDrawer.h" +#include "MyGlutMouseHandler.h" +#include "MyGlutKeyHandler.h" +#include "MT_Transform.h" +#ifdef USE_QUATERNIONS +# include "IK_Qsolver.h" +# include "../intern/IK_QChain.h" +# include "../intern/IK_QSolver_Class.h" +#else +# include "IK_solver.h" +# include "../intern/IK_Chain.h" +# include "../intern/IK_Solver_Class.h" +#endif +#include <GL/glut.h> + +class ChainDrawer : public GlutDrawer +{ +public : + static + ChainDrawer * + New( + ) { + return new ChainDrawer(); + } + + void + SetMouseHandler( + MyGlutMouseHandler *mouse_handler + ) { + m_mouse_handler = mouse_handler; + } + + void + SetKeyHandler ( + MyGlutKeyHandler *key_handler + ) { + m_key_handler = key_handler; + } + + void + SetChain( + IK_Chain_ExternPtr *chains,int chain_num + ) { + m_chain_num = chain_num; + m_chains = chains; + } + + + // inherited from GlutDrawer + void + Draw( + ) { + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + glPopMatrix(); + glPushMatrix(); + glRotatef(m_mouse_handler->AngleX(), 0.0, 1.0, 0.0); + glRotatef(m_mouse_handler->AngleY(), 1.0, 0.0, 0.0); + + DrawScene(); + glutSwapBuffers(); + + } + + ~ChainDrawer( + ){ + // nothing to do + }; + +private : + + void + DrawScene( + ){ + + // draw a little cross at the position of the key handler + // coordinates + + MT_Vector3 line_x(4,0,0); + MT_Vector3 line_y(0.0,4,0); + MT_Vector3 line_z(0.0,0.0,4); + + MT_Vector3 cross_origin = m_mouse_handler->Position(); + MT_Vector3 temp; + + glDisable(GL_LIGHTING); + + + glBegin(GL_LINES); + + glColor3f (1.0f,1.0f,1.0f); + + temp = cross_origin - line_x; + glVertex3f(temp[0],temp[1],temp[2]); + temp = cross_origin + line_x; + glVertex3f(temp[0],temp[1],temp[2]); + + temp = cross_origin - line_y; + glVertex3f(temp[0],temp[1],temp[2]); + temp = cross_origin + line_y; + glVertex3f(temp[0],temp[1],temp[2]); + + temp = cross_origin - line_z; + glVertex3f(temp[0],temp[1],temp[2]); + temp = cross_origin + line_z; + glVertex3f(temp[0],temp[1],temp[2]); + + glEnd(); + glEnable(GL_LIGHTING); + + + IK_Chain_ExternPtr chain; + + int chain_num; + for (chain_num = 0; chain_num < m_chain_num; chain_num++) { + chain = m_chains[chain_num]; + + + IK_Segment_ExternPtr segs = chain->segments; + IK_Segment_ExternPtr seg_start = segs; + const IK_Segment_ExternPtr seg_end = segs + chain->num_segments; + float ogl_matrix[16]; + + glColor3f (0.0f,1.0f,0.0f); + + MT_Vector3 previous_origin(0,0,0); + + MT_Transform global_transform; + global_transform.setIdentity(); + + for (; seg_start != seg_end; ++seg_start) { + + glPushMatrix(); + + // fill ogl_matrix with zeros + + std::fill(ogl_matrix,ogl_matrix + 16,float(0)); + + // we have to do a bit of work here to compute the chain's + // bone values + + // first compute all the matrices we need + + MT_Transform translation; + translation.setIdentity(); + translation.translate(MT_Vector3(0,seg_start->length,0)); + + MT_Matrix3x3 seg_rot( + seg_start->basis_change[0],seg_start->basis_change[1],seg_start->basis_change[2], + seg_start->basis_change[3],seg_start->basis_change[4],seg_start->basis_change[5], + seg_start->basis_change[6],seg_start->basis_change[7],seg_start->basis_change[8] + ); + + seg_rot.transpose(); + + MT_Matrix3x3 seg_pre_rot( + seg_start->basis[0],seg_start->basis[1],seg_start->basis[2], + seg_start->basis[3],seg_start->basis[4],seg_start->basis[5], + seg_start->basis[6],seg_start->basis[7],seg_start->basis[8] + ); + + + MT_Transform seg_t_pre_rot( + MT_Point3( + seg_start->seg_start[0], + seg_start->seg_start[1], + seg_start->seg_start[2] + ), + seg_pre_rot + ); + // start of the bone is just the current global transform + // multiplied by the seg_start vector + + + + MT_Transform seg_t_rot(MT_Point3(0,0,0),seg_rot); + MT_Transform seg_local = seg_t_pre_rot * seg_t_rot * translation; + + MT_Vector3 bone_start = global_transform * + MT_Point3( + seg_start->seg_start[0], + seg_start->seg_start[1], + seg_start->seg_start[2] + ); + + + global_transform = global_transform * seg_local; + + global_transform.getValue(ogl_matrix); + MT_Vector3 bone_end = global_transform.getOrigin(); + + glMultMatrixf(ogl_matrix); +// glutSolidSphere(0.5,5,5); + + glPopMatrix(); + + glDisable(GL_LIGHTING); + + glBegin(GL_LINES); + + // draw lines of the principle axis of the local transform + + MT_Vector3 x_axis(1,0,0); + MT_Vector3 y_axis(0,1,0); + MT_Vector3 z_axis(0,0,1); + + x_axis = global_transform.getBasis() * x_axis * 5; + y_axis = global_transform.getBasis() * y_axis * 5; + z_axis = global_transform.getBasis() * z_axis * 5; + + + x_axis = x_axis + bone_start; + y_axis = y_axis + bone_start; + z_axis = z_axis + bone_start; + + glColor3f(1,0,0); + + glVertex3f(x_axis.x(),x_axis.y(),x_axis.z()); + glVertex3f( + bone_start.x(), + bone_start.y(), + bone_start.z() + ); + + glColor3f(0,1,0); + + glVertex3f(y_axis.x(),y_axis.y(),y_axis.z()); + glVertex3f( + bone_start.x(), + bone_start.y(), + bone_start.z() + ); + + glColor3f(0,1,1); + + glVertex3f(z_axis.x(),z_axis.y(),z_axis.z()); + glVertex3f( + bone_start.x(), + bone_start.y(), + bone_start.z() + ); + + glColor3f(0,0,1); + + glVertex3f( + bone_start.x(), + bone_start.y(), + bone_start.z() + ); + glVertex3f(bone_end[0],bone_end[1],bone_end[2]); + + glEnd(); + glEnable(GL_LIGHTING); + } +#if 0 + // draw jacobian column vectors + + // hack access to internals + + IK_Solver_Class * internals = static_cast<IK_Solver_Class *>(chain->intern); + + glDisable(GL_LIGHTING); + + glBegin(GL_LINES); + + const TNT::Matrix<MT_Scalar> & jac = internals->Chain().TransposedJacobian(); + + int i = 0; + for (i=0; i < jac.num_rows(); i++) { + glColor3f(1,1,1); + + previous_origin = internals->Chain().Segments()[i/3].GlobalSegmentStart(); + + glVertex3f(previous_origin[0],previous_origin[1],previous_origin[2]); + glVertex3f(jac[i][0] + previous_origin[0],jac[i][1] + previous_origin[1],jac[i][2] + previous_origin[2]); + + + } + glEnd(); + glEnable(GL_LIGHTING); +#endif + + } + + glColor3f(1.0,1.0,1.0); + + glDisable(GL_LIGHTING); + glBegin(GL_LINES); + + MT_Scalar cube_size = 50; + glVertex3f(cube_size,cube_size,cube_size); + glVertex3f(-cube_size,cube_size,cube_size); + + glVertex3f(cube_size,-cube_size,cube_size); + glVertex3f(-cube_size,-cube_size,cube_size); + + glVertex3f(cube_size,cube_size,-cube_size); + glVertex3f(-cube_size,cube_size,-cube_size); + + glVertex3f(cube_size,-cube_size,-cube_size); + glVertex3f(-cube_size,-cube_size,-cube_size); + + + glVertex3f(-cube_size,cube_size,cube_size); + glVertex3f(-cube_size,-cube_size,cube_size); + + glVertex3f(cube_size,cube_size,-cube_size); + glVertex3f(cube_size,-cube_size,-cube_size); + + glVertex3f(cube_size,cube_size,cube_size); + glVertex3f(cube_size,-cube_size,cube_size); + + glVertex3f(-cube_size,cube_size,-cube_size); + glVertex3f(-cube_size,-cube_size,-cube_size); + + + glVertex3f(cube_size,cube_size,cube_size); + glVertex3f(cube_size,cube_size,-cube_size); + + glVertex3f(cube_size,-cube_size,cube_size); + glVertex3f(cube_size,-cube_size,-cube_size); + + glVertex3f(-cube_size,cube_size,cube_size); + glVertex3f(-cube_size,cube_size,-cube_size); + + glVertex3f(-cube_size,-cube_size,cube_size); + glVertex3f(-cube_size,-cube_size,-cube_size); + glEnd(); + glEnable(GL_LIGHTING); + + }; + + + +private : + + MyGlutMouseHandler * m_mouse_handler; + MyGlutKeyHandler *m_key_handler; + IK_Chain_ExternPtr *m_chains; + + int m_chain_num; + ChainDrawer ( + ) : m_chains (NULL), + m_mouse_handler (NULL), + m_chain_num (0) + { + }; + +}; + + + +#endif
\ No newline at end of file diff --git a/intern/iksolver/test/ik_glut_test/intern/Makefile b/intern/iksolver/test/ik_glut_test/intern/Makefile new file mode 100644 index 00000000000..fe1f4b54182 --- /dev/null +++ b/intern/iksolver/test/ik_glut_test/intern/Makefile @@ -0,0 +1,51 @@ +# +# $Id$ +# +# ***** BEGIN GPL/BL DUAL LICENSE BLOCK ***** +# +# This program is free software; you can redistribute it and/or +# modify it under the terms of the GNU General Public License +# as published by the Free Software Foundation; either version 2 +# of the License, or (at your option) any later version. The Blender +# Foundation also sells licenses for use in proprietary software under +# the Blender License. See http://www.blender.org/BL/ for information +# about this. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program; if not, write to the Free Software Foundation, +# Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +# +# The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. +# All rights reserved. +# +# The Original Code is: all of this file. +# +# Contributor(s): none yet. +# +# ***** END GPL/BL DUAL LICENSE BLOCK ***** +# iksolver test intern Makefile +# + +LIBNAME = intern +SOURCEDIR = intern/iksolver/test/ik_glut_test/$(LIBNAME) +DIR = $(OCGDIR)/$(SOURCEDIR) + +include nan_compile.mk + +CCFLAGS += $(LEVEL_2_CPP_WARNINGS) + +CPPFLAGS += -I$(OPENGL_HEADERS) +CPPFLAGS += -I../../../extern +CPPFLAGS += -I../common +CPPFLAGS += -I$(NAN_MOTO)/include +CPPFLAGS += -I$(NAN_MEMUTIL)/include + +ifeq ($(OS),windows) + CPPFLAGS += -I$(NAN_LIBDIR)/windows/glut-3.7/include +endif + diff --git a/intern/iksolver/test/ik_glut_test/intern/MyGlutKeyHandler.h b/intern/iksolver/test/ik_glut_test/intern/MyGlutKeyHandler.h new file mode 100644 index 00000000000..5f88bda378c --- /dev/null +++ b/intern/iksolver/test/ik_glut_test/intern/MyGlutKeyHandler.h @@ -0,0 +1,84 @@ +/** + * $Id$ + * ***** BEGIN GPL/BL DUAL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. The Blender + * Foundation also sells licenses for use in proprietary software under + * the Blender License. See http://www.blender.org/BL/ for information + * about this. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Contributor(s): none yet. + * + * ***** END GPL/BL DUAL LICENSE BLOCK ***** + */ + +#ifndef NAN_INCLUDED_MyGlutKeyHandler_h + +#define NAN_INCLUDED_MyGlutKeyHandler_h + +#include "../common/GlutKeyboardManager.h" + +class MyGlutKeyHandler : public GlutKeyboardHandler +{ +public : + static + MyGlutKeyHandler * + New( + ) { + MEM_SmartPtr<MyGlutKeyHandler> output = new MyGlutKeyHandler(); + + if (output == NULL + ) { + return NULL; + } + return output.Release(); + + } + + void + HandleKeyboard( + unsigned char key, + int x, + int y + ){ + + switch (key) { + + case 27 : + + exit(0); + } + } + + ~MyGlutKeyHandler( + ) + { + }; + +private : + + MyGlutKeyHandler( + ) + { + } + +}; +#endif + diff --git a/intern/iksolver/test/ik_glut_test/intern/MyGlutMouseHandler.h b/intern/iksolver/test/ik_glut_test/intern/MyGlutMouseHandler.h new file mode 100644 index 00000000000..e7d210beffb --- /dev/null +++ b/intern/iksolver/test/ik_glut_test/intern/MyGlutMouseHandler.h @@ -0,0 +1,211 @@ +/** + * $Id$ + * ***** BEGIN GPL/BL DUAL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. The Blender + * Foundation also sells licenses for use in proprietary software under + * the Blender License. See http://www.blender.org/BL/ for information + * about this. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Contributor(s): none yet. + * + * ***** END GPL/BL DUAL LICENSE BLOCK ***** + */ + +#ifndef NAN_INCLUDED_MyGlutMouseHandler_h + +#define NAN_INCLUDED_MyGlutMouseHandler_h + +#include "../common/GlutMouseManager.h" +#include <GL/glut.h> +#include "IK_solver.h" + +class MyGlutMouseHandler : public GlutMouseHandler +{ + +public : + + static + MyGlutMouseHandler * + New( + ) { + MEM_SmartPtr<MyGlutMouseHandler> output = new MyGlutMouseHandler(); + if (output == NULL + ) { + return NULL; + } + return output.Release(); + + } + + void + SetChain( + IK_Chain_ExternPtr *chains, int num_chains + ){ + m_chains = chains; + m_num_chains = num_chains; + } + + void + Mouse( + int button, + int state, + int x, + int y + ){ + if (button == GLUT_LEFT_BUTTON && state == GLUT_DOWN) { + m_moving = true; + m_begin_x = x; + m_begin_y = y; + } + if (button == GLUT_LEFT_BUTTON && state == GLUT_UP) { + m_moving = false; + } + + if (button == GLUT_RIGHT_BUTTON && state == GLUT_DOWN) { + m_tracking = true; + } + if (button == GLUT_RIGHT_BUTTON && state == GLUT_UP) { + m_tracking = false; + } + + if (button == GLUT_MIDDLE_BUTTON && state == GLUT_DOWN) { + m_cg_on = true; + } + if (button == GLUT_MIDDLE_BUTTON && state == GLUT_UP) { + m_cg_on = false; + } + + } + + + void + Motion( + int x, + int y + ){ + if (m_moving) { + m_angle_x = m_angle_x + (x - m_begin_x); + m_begin_x = x; + + m_angle_y = m_angle_y + (y - m_begin_y); + m_begin_y = y; + + glutPostRedisplay(); + } + if (m_tracking) { + + int w_h = glutGet((GLenum)GLUT_WINDOW_HEIGHT); + + y = w_h - y; + + double mvmatrix[16]; + double projmatrix[16]; + GLint viewport[4]; + + double px, py, pz,sz; + + /* Get the matrices needed for gluUnProject */ + glGetIntegerv(GL_VIEWPORT, viewport); + glGetDoublev(GL_MODELVIEW_MATRIX, mvmatrix); + glGetDoublev(GL_PROJECTION_MATRIX, projmatrix); + + // work out the position of the end effector in screen space + + GLdouble ex,ey,ez; + ex = m_pos.x(); + ey = m_pos.y(); + ez = m_pos.z(); + + gluProject(ex, ey, ez, mvmatrix, projmatrix, viewport, &px, &py, &sz); + gluUnProject((GLdouble) x, (GLdouble) y, sz, mvmatrix, projmatrix, viewport, &px, &py, &pz); + + m_pos = MT_Vector3(px,py,pz); + + } + if (m_tracking || m_cg_on) { + float temp[3]; + m_pos.getValue(temp); + + IK_SolveChain(m_chains[0],temp,0.01,200,0.1,m_chains[1]->segments); + IK_LoadChain(m_chains[0],m_chains[0]->segments,m_chains[0]->num_segments); + + glutPostRedisplay(); + } + + + } + + const + float + AngleX( + ) const { + return m_angle_x; + } + + const + float + AngleY( + ) const { + return m_angle_y; + } + + const + MT_Vector3 + Position( + ) const { + return m_pos; + } + + +private : + + MyGlutMouseHandler ( + ) : + m_angle_x(0), + m_angle_y(0), + m_begin_x(0), + m_begin_y(0), + m_moving (false), + m_tracking (false), + m_pos(0,0,0), + m_cg_on (false), + m_chains(NULL), + m_num_chains(0) + { + }; + + float m_angle_x; + float m_angle_y; + float m_begin_x; + float m_begin_y; + + bool m_moving; + bool m_tracking; + bool m_cg_on; + MT_Vector3 m_pos; + + IK_Chain_ExternPtr *m_chains; + int m_num_chains; + +}; + + +#endif
\ No newline at end of file diff --git a/intern/iksolver/test/ik_glut_test/intern/main.cpp b/intern/iksolver/test/ik_glut_test/intern/main.cpp new file mode 100644 index 00000000000..137c9bd4470 --- /dev/null +++ b/intern/iksolver/test/ik_glut_test/intern/main.cpp @@ -0,0 +1,361 @@ +/** + * $Id$ + * ***** BEGIN GPL/BL DUAL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. The Blender + * Foundation also sells licenses for use in proprietary software under + * the Blender License. See http://www.blender.org/BL/ for information + * about this. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Contributor(s): none yet. + * + * ***** END GPL/BL DUAL LICENSE BLOCK ***** + */ + +/** + * $Id$ + * ***** BEGIN GPL/BL DUAL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. The Blender + * Foundation also sells licenses for use in proprietary software under + * the Blender License. See http://www.blender.org/BL/ for information + * about this. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Contributor(s): none yet. + * + * ***** END GPL/BL DUAL LICENSE BLOCK ***** + */ + + +#include "MEM_SmartPtr.h" + +#ifdef USE_QUATERNIONS +#include "IK_Qsolver.h" +#else +#include "IK_solver.h" +#endif + +#include <GL/glut.h> +#include "MT_Vector3.h" +#include "MT_Quaternion.h" +#include "MT_Matrix3x3.h" +#include "MyGlutMouseHandler.h" +#include "MyGlutKeyHandler.h" +#include "ChainDrawer.h" + +void +init(MT_Vector3 min,MT_Vector3 max) +{ + + GLfloat light_diffuse0[] = {1.0, 0.0, 0.0, 1.0}; /* Red diffuse light. */ + GLfloat light_position0[] = {1.0, 1.0, 1.0, 0.0}; /* Infinite light location. */ + + GLfloat light_diffuse1[] = {1.0, 1.0, 1.0, 1.0}; /* Red diffuse light. */ + GLfloat light_position1[] = {1.0, 0, 0, 0.0}; /* Infinite light location. */ + + /* Enable a single OpenGL light. */ + glLightfv(GL_LIGHT0, GL_DIFFUSE, light_diffuse0); + glLightfv(GL_LIGHT0, GL_POSITION, light_position0); + + glLightfv(GL_LIGHT1, GL_DIFFUSE, light_diffuse1); + glLightfv(GL_LIGHT1, GL_POSITION, light_position1); + + glEnable(GL_LIGHT0); + glEnable(GL_LIGHT1); + glEnable(GL_LIGHTING); + + /* Use depth buffering for hidden surface elimination. */ + glEnable(GL_DEPTH_TEST); + + /* Setup the view of the cube. */ + glMatrixMode(GL_PROJECTION); + + // centre of the box + 3* depth of box + + MT_Vector3 centre = (min + max) * 0.5; + MT_Vector3 diag = max - min; + + float depth = diag.length(); + float distance = 2; + + gluPerspective( + /* field of view in degree */ 40.0, + /* aspect ratio */ 1.0, + /* Z near */ 1.0, + /* Z far */ distance * depth * 2 + ); + glMatrixMode(GL_MODELVIEW); + + + gluLookAt( + centre.x(), centre.y(), centre.z() + distance*depth, /* eye is at (0,0,5) */ + centre.x(), centre.y(), centre.z(), /* center is at (0,0,0) */ + 0.0, 1.0, 0.); /* up is in positive Y direction */ + + glPushMatrix(); + + +} +int +main(int argc, char **argv) +{ + + + const int seg_num = 5; + const MT_Scalar seg_length = 15; + + const float seg_startA[3] = {0,0,0}; + const float seg_startB[3] = {0,-20,0}; + + // create some segments to solve with + + // First chain + ////////////// + + + IK_Segment_ExternPtr const segmentsA = new IK_Segment_Extern[seg_num]; + IK_Segment_ExternPtr const segmentsB = new IK_Segment_Extern[seg_num]; + + IK_Segment_ExternPtr seg_it = segmentsA; + IK_Segment_ExternPtr seg_itB = segmentsB; + + + { + +// MT_Quaternion qmat(MT_Vector3(0,0,1),-3.141/2); + MT_Quaternion qmat(MT_Vector3(0,0,1),0); + MT_Matrix3x3 mat(qmat); + + seg_it->seg_start[0] = seg_startA[0]; + seg_it->seg_start[1] = seg_startA[1]; + seg_it->seg_start[2] = seg_startA[2]; + + float temp[12]; + mat.getValue(temp); + + seg_it->basis[0] = temp[0]; + seg_it->basis[1] = temp[1]; + seg_it->basis[2] = temp[2]; + + seg_it->basis[3] = temp[4]; + seg_it->basis[4] = temp[5]; + seg_it->basis[5] = temp[6]; + + seg_it->basis[6] = temp[8]; + seg_it->basis[7] = temp[9]; + seg_it->basis[8] = temp[10]; + + seg_it->length = seg_length; + + MT_Quaternion q; + q.setEuler(0,0,0); + + + MT_Matrix3x3 qrot(q); + + seg_it->basis_change[0] = 1; + seg_it->basis_change[1] = 0; + seg_it->basis_change[2] = 0; + seg_it->basis_change[3] = 0; + seg_it->basis_change[4] = 1; + seg_it->basis_change[5] = 0; + seg_it->basis_change[6] = 0; + seg_it->basis_change[7] = 0; + seg_it->basis_change[8] = 1; + + + seg_it ++; + + seg_itB->seg_start[0] = seg_startA[0]; + seg_itB->seg_start[1] = seg_startA[1]; + seg_itB->seg_start[2] = seg_startA[2]; + + seg_itB->basis[0] = temp[0]; + seg_itB->basis[1] = temp[1]; + seg_itB->basis[2] = temp[2]; + + seg_itB->basis[3] = temp[4]; + seg_itB->basis[4] = temp[5]; + seg_itB->basis[5] = temp[6]; + + seg_itB->basis[6] = temp[8]; + seg_itB->basis[7] = temp[9]; + seg_itB->basis[8] = temp[10]; + + seg_itB->length = seg_length; + + seg_itB->basis_change[0] = 1; + seg_itB->basis_change[1] = 0; + seg_itB->basis_change[2] = 0; + seg_itB->basis_change[3] = 0; + seg_itB->basis_change[4] = 1; + seg_itB->basis_change[5] = 0; + seg_itB->basis_change[6] = 0; + seg_itB->basis_change[7] = 0; + seg_itB->basis_change[8] = 1; + + + seg_itB ++; + + + } + + + int i; + for (i=1; i < seg_num; ++i, ++seg_it,++seg_itB) { + + MT_Quaternion qmat(MT_Vector3(0,0,1),0.3); + MT_Matrix3x3 mat(qmat); + + seg_it->seg_start[0] = 0; + seg_it->seg_start[1] = 0; + seg_it->seg_start[2] = 0; + + float temp[12]; + mat.getValue(temp); + + seg_it->basis[0] = temp[0]; + seg_it->basis[1] = temp[1]; + seg_it->basis[2] = temp[2]; + + seg_it->basis[3] = temp[4]; + seg_it->basis[4] = temp[5]; + seg_it->basis[5] = temp[6]; + + seg_it->basis[6] = temp[8]; + seg_it->basis[7] = temp[9]; + seg_it->basis[8] = temp[10]; + + seg_it->length = seg_length; + + MT_Quaternion q; + q.setEuler(0,0,0); + + + MT_Matrix3x3 qrot(q); + + seg_it->basis_change[0] = 1; + seg_it->basis_change[1] = 0; + seg_it->basis_change[2] = 0; + seg_it->basis_change[3] = 0; + seg_it->basis_change[4] = 1; + seg_it->basis_change[5] = 0; + seg_it->basis_change[6] = 0; + seg_it->basis_change[7] = 0; + seg_it->basis_change[8] = 1; + + + /////////////////////////////// + + seg_itB->seg_start[0] = 0; + seg_itB->seg_start[1] = 0; + seg_itB->seg_start[2] = 0; + + seg_itB->basis[0] = temp[0]; + seg_itB->basis[1] = temp[1]; + seg_itB->basis[2] = temp[2]; + + seg_itB->basis[3] = temp[4]; + seg_itB->basis[4] = temp[5]; + seg_itB->basis[5] = temp[6]; + + seg_itB->basis[6] = temp[8]; + seg_itB->basis[7] = temp[9]; + seg_itB->basis[8] = temp[10]; + + seg_itB->length = seg_length; + + seg_itB->basis_change[0] = 1; + seg_itB->basis_change[1] = 0; + seg_itB->basis_change[2] = 0; + seg_itB->basis_change[3] = 0; + seg_itB->basis_change[4] = 1; + seg_itB->basis_change[5] = 0; + seg_itB->basis_change[6] = 0; + seg_itB->basis_change[7] = 0; + seg_itB->basis_change[8] = 1; + + + + } + + // create the chains + + const int num_chains = 2; + + IK_Chain_ExternPtr chains[num_chains]; + + chains[0] = IK_CreateChain(); + chains[1] = IK_CreateChain(); + + // load segments into chain + + IK_LoadChain(chains[0],segmentsA,seg_num); + IK_LoadChain(chains[1],segmentsB,seg_num); + + // make and install a mouse handler + + MEM_SmartPtr<MyGlutMouseHandler> mouse_handler (MyGlutMouseHandler::New()); + GlutMouseManager::Instance()->InstallHandler(mouse_handler); + + mouse_handler->SetChain(chains,num_chains); + + // make and install a keyhandler + MEM_SmartPtr<MyGlutKeyHandler> key_handler (MyGlutKeyHandler::New()); + GlutKeyboardManager::Instance()->InstallHandler(key_handler); + + // instantiate the drawing class + + MEM_SmartPtr<ChainDrawer> drawer (ChainDrawer::New()); + GlutDrawManager::Instance()->InstallDrawer(drawer); + + drawer->SetMouseHandler(mouse_handler); + drawer->SetChain(chains,num_chains); + drawer->SetKeyHandler(key_handler); + + glutInit(&argc, argv); + glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH); + glutCreateWindow("ik"); + glutDisplayFunc(GlutDrawManager::Draw); + glutMouseFunc(GlutMouseManager::Mouse); + glutMotionFunc(GlutMouseManager::Motion); + glutKeyboardFunc(GlutKeyboardManager::HandleKeyboard); + + init(MT_Vector3(-50,-50,-50),MT_Vector3(50,50,50)); + glutMainLoop(); + return 0; /* ANSI C requires main to return int. */ +} |