forked from bartvdbraak/blender
322 lines
7.8 KiB
C++
322 lines
7.8 KiB
C++
/**
|
|
* ***** BEGIN GPL 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.
|
|
*
|
|
* 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, 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 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);
|
|
|
|
// center of the box + 3* depth of box
|
|
|
|
MT_Vector3 center = (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(center.x(), center.y(), center.z() + distance*depth, /* eye is at (0,0,5) */
|
|
center.x(), center.y(), center.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. */
|
|
}
|