|
Blender
V2.59
|
00001 00029 #include "MEM_SmartPtr.h" 00030 00031 #ifdef USE_QUATERNIONS 00032 #include "IK_Qsolver.h" 00033 #else 00034 #include "IK_solver.h" 00035 #endif 00036 00037 #include <GL/glut.h> 00038 #include "MT_Vector3.h" 00039 #include "MT_Quaternion.h" 00040 #include "MT_Matrix3x3.h" 00041 #include "MyGlutMouseHandler.h" 00042 #include "MyGlutKeyHandler.h" 00043 #include "ChainDrawer.h" 00044 00045 void 00046 init(MT_Vector3 min,MT_Vector3 max) 00047 { 00048 00049 GLfloat light_diffuse0[] = {1.0, 0.0, 0.0, 1.0}; /* Red diffuse light. */ 00050 GLfloat light_position0[] = {1.0, 1.0, 1.0, 0.0}; /* Infinite light location. */ 00051 00052 GLfloat light_diffuse1[] = {1.0, 1.0, 1.0, 1.0}; /* Red diffuse light. */ 00053 GLfloat light_position1[] = {1.0, 0, 0, 0.0}; /* Infinite light location. */ 00054 00055 /* Enable a single OpenGL light. */ 00056 glLightfv(GL_LIGHT0, GL_DIFFUSE, light_diffuse0); 00057 glLightfv(GL_LIGHT0, GL_POSITION, light_position0); 00058 00059 glLightfv(GL_LIGHT1, GL_DIFFUSE, light_diffuse1); 00060 glLightfv(GL_LIGHT1, GL_POSITION, light_position1); 00061 00062 glEnable(GL_LIGHT0); 00063 glEnable(GL_LIGHT1); 00064 glEnable(GL_LIGHTING); 00065 00066 /* Use depth buffering for hidden surface elimination. */ 00067 glEnable(GL_DEPTH_TEST); 00068 00069 /* Setup the view of the cube. */ 00070 glMatrixMode(GL_PROJECTION); 00071 00072 // center of the box + 3* depth of box 00073 00074 MT_Vector3 center = (min + max) * 0.5; 00075 MT_Vector3 diag = max - min; 00076 00077 float depth = diag.length(); 00078 float distance = 2; 00079 00080 gluPerspective( 00081 /* field of view in degree */ 40.0, 00082 /* aspect ratio */ 1.0, 00083 /* Z near */ 1.0, 00084 /* Z far */ distance * depth * 2 00085 ); 00086 glMatrixMode(GL_MODELVIEW); 00087 00088 00089 gluLookAt( 00090 center.x(), center.y(), center.z() + distance*depth, /* eye is at (0,0,5) */ 00091 center.x(), center.y(), center.z(), /* center is at (0,0,0) */ 00092 0.0, 1.0, 0.); /* up is in positive Y direction */ 00093 00094 glPushMatrix(); 00095 00096 00097 } 00098 int 00099 main(int argc, char **argv) 00100 { 00101 00102 00103 const int seg_num = 5; 00104 const MT_Scalar seg_length = 15; 00105 00106 const float seg_startA[3] = {0,0,0}; 00107 const float seg_startB[3] = {0,-20,0}; 00108 00109 // create some segments to solve with 00110 00111 // First chain 00113 00114 00115 IK_Segment_ExternPtr const segmentsA = new IK_Segment_Extern[seg_num]; 00116 IK_Segment_ExternPtr const segmentsB = new IK_Segment_Extern[seg_num]; 00117 00118 IK_Segment_ExternPtr seg_it = segmentsA; 00119 IK_Segment_ExternPtr seg_itB = segmentsB; 00120 00121 00122 { 00123 00124 // MT_Quaternion qmat(MT_Vector3(0,0,1),-3.141/2); 00125 MT_Quaternion qmat(MT_Vector3(0,0,1),0); 00126 MT_Matrix3x3 mat(qmat); 00127 00128 seg_it->seg_start[0] = seg_startA[0]; 00129 seg_it->seg_start[1] = seg_startA[1]; 00130 seg_it->seg_start[2] = seg_startA[2]; 00131 00132 float temp[12]; 00133 mat.getValue(temp); 00134 00135 seg_it->basis[0] = temp[0]; 00136 seg_it->basis[1] = temp[1]; 00137 seg_it->basis[2] = temp[2]; 00138 00139 seg_it->basis[3] = temp[4]; 00140 seg_it->basis[4] = temp[5]; 00141 seg_it->basis[5] = temp[6]; 00142 00143 seg_it->basis[6] = temp[8]; 00144 seg_it->basis[7] = temp[9]; 00145 seg_it->basis[8] = temp[10]; 00146 00147 seg_it->length = seg_length; 00148 00149 MT_Quaternion q; 00150 q.setEuler(0,0,0); 00151 00152 00153 MT_Matrix3x3 qrot(q); 00154 00155 seg_it->basis_change[0] = 1; 00156 seg_it->basis_change[1] = 0; 00157 seg_it->basis_change[2] = 0; 00158 seg_it->basis_change[3] = 0; 00159 seg_it->basis_change[4] = 1; 00160 seg_it->basis_change[5] = 0; 00161 seg_it->basis_change[6] = 0; 00162 seg_it->basis_change[7] = 0; 00163 seg_it->basis_change[8] = 1; 00164 00165 00166 seg_it ++; 00167 00168 seg_itB->seg_start[0] = seg_startA[0]; 00169 seg_itB->seg_start[1] = seg_startA[1]; 00170 seg_itB->seg_start[2] = seg_startA[2]; 00171 00172 seg_itB->basis[0] = temp[0]; 00173 seg_itB->basis[1] = temp[1]; 00174 seg_itB->basis[2] = temp[2]; 00175 00176 seg_itB->basis[3] = temp[4]; 00177 seg_itB->basis[4] = temp[5]; 00178 seg_itB->basis[5] = temp[6]; 00179 00180 seg_itB->basis[6] = temp[8]; 00181 seg_itB->basis[7] = temp[9]; 00182 seg_itB->basis[8] = temp[10]; 00183 00184 seg_itB->length = seg_length; 00185 00186 seg_itB->basis_change[0] = 1; 00187 seg_itB->basis_change[1] = 0; 00188 seg_itB->basis_change[2] = 0; 00189 seg_itB->basis_change[3] = 0; 00190 seg_itB->basis_change[4] = 1; 00191 seg_itB->basis_change[5] = 0; 00192 seg_itB->basis_change[6] = 0; 00193 seg_itB->basis_change[7] = 0; 00194 seg_itB->basis_change[8] = 1; 00195 00196 00197 seg_itB ++; 00198 00199 00200 } 00201 00202 00203 int i; 00204 for (i=1; i < seg_num; ++i, ++seg_it,++seg_itB) { 00205 00206 MT_Quaternion qmat(MT_Vector3(0,0,1),0.3); 00207 MT_Matrix3x3 mat(qmat); 00208 00209 seg_it->seg_start[0] = 0; 00210 seg_it->seg_start[1] = 0; 00211 seg_it->seg_start[2] = 0; 00212 00213 float temp[12]; 00214 mat.getValue(temp); 00215 00216 seg_it->basis[0] = temp[0]; 00217 seg_it->basis[1] = temp[1]; 00218 seg_it->basis[2] = temp[2]; 00219 00220 seg_it->basis[3] = temp[4]; 00221 seg_it->basis[4] = temp[5]; 00222 seg_it->basis[5] = temp[6]; 00223 00224 seg_it->basis[6] = temp[8]; 00225 seg_it->basis[7] = temp[9]; 00226 seg_it->basis[8] = temp[10]; 00227 00228 seg_it->length = seg_length; 00229 00230 MT_Quaternion q; 00231 q.setEuler(0,0,0); 00232 00233 00234 MT_Matrix3x3 qrot(q); 00235 00236 seg_it->basis_change[0] = 1; 00237 seg_it->basis_change[1] = 0; 00238 seg_it->basis_change[2] = 0; 00239 seg_it->basis_change[3] = 0; 00240 seg_it->basis_change[4] = 1; 00241 seg_it->basis_change[5] = 0; 00242 seg_it->basis_change[6] = 0; 00243 seg_it->basis_change[7] = 0; 00244 seg_it->basis_change[8] = 1; 00245 00246 00248 00249 seg_itB->seg_start[0] = 0; 00250 seg_itB->seg_start[1] = 0; 00251 seg_itB->seg_start[2] = 0; 00252 00253 seg_itB->basis[0] = temp[0]; 00254 seg_itB->basis[1] = temp[1]; 00255 seg_itB->basis[2] = temp[2]; 00256 00257 seg_itB->basis[3] = temp[4]; 00258 seg_itB->basis[4] = temp[5]; 00259 seg_itB->basis[5] = temp[6]; 00260 00261 seg_itB->basis[6] = temp[8]; 00262 seg_itB->basis[7] = temp[9]; 00263 seg_itB->basis[8] = temp[10]; 00264 00265 seg_itB->length = seg_length; 00266 00267 seg_itB->basis_change[0] = 1; 00268 seg_itB->basis_change[1] = 0; 00269 seg_itB->basis_change[2] = 0; 00270 seg_itB->basis_change[3] = 0; 00271 seg_itB->basis_change[4] = 1; 00272 seg_itB->basis_change[5] = 0; 00273 seg_itB->basis_change[6] = 0; 00274 seg_itB->basis_change[7] = 0; 00275 seg_itB->basis_change[8] = 1; 00276 00277 00278 00279 } 00280 00281 // create the chains 00282 00283 const int num_chains = 2; 00284 00285 IK_Chain_ExternPtr chains[num_chains]; 00286 00287 chains[0] = IK_CreateChain(); 00288 chains[1] = IK_CreateChain(); 00289 00290 // load segments into chain 00291 00292 IK_LoadChain(chains[0],segmentsA,seg_num); 00293 IK_LoadChain(chains[1],segmentsB,seg_num); 00294 00295 // make and install a mouse handler 00296 00297 MEM_SmartPtr<MyGlutMouseHandler> mouse_handler (MyGlutMouseHandler::New()); 00298 GlutMouseManager::Instance()->InstallHandler(mouse_handler); 00299 00300 mouse_handler->SetChain(chains,num_chains); 00301 00302 // make and install a keyhandler 00303 MEM_SmartPtr<MyGlutKeyHandler> key_handler (MyGlutKeyHandler::New()); 00304 GlutKeyboardManager::Instance()->InstallHandler(key_handler); 00305 00306 // instantiate the drawing class 00307 00308 MEM_SmartPtr<ChainDrawer> drawer (ChainDrawer::New()); 00309 GlutDrawManager::Instance()->InstallDrawer(drawer); 00310 00311 drawer->SetMouseHandler(mouse_handler); 00312 drawer->SetChain(chains,num_chains); 00313 drawer->SetKeyHandler(key_handler); 00314 00315 glutInit(&argc, argv); 00316 glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH); 00317 glutCreateWindow("ik"); 00318 glutDisplayFunc(GlutDrawManager::Draw); 00319 glutMouseFunc(GlutMouseManager::Mouse); 00320 glutMotionFunc(GlutMouseManager::Motion); 00321 glutKeyboardFunc(GlutKeyboardManager::HandleKeyboard); 00322 00323 init(MT_Vector3(-50,-50,-50),MT_Vector3(50,50,50)); 00324 glutMainLoop(); 00325 return 0; /* ANSI C requires main to return int. */ 00326 }