Ok I am almost done with an example of this I got it as no rotations right know but know i need to perform forward kinematics. I had a hard time re engineering this file but I think that I am almost there.
#include "Main.h"
#include <vector>
// let the compiler know we're using the AGK namespace
using namespace AGK;
using namespace std;
vector<char *> afile;
// declare our app
#include <fstream>
#include <string.h>
#include "BVH.h"
struct POINTS2
{
float xp;
float yp;
float zp;
};
POINTS2 points;
float rotationmatrix[4][4];
float inputmatrix[4][4];
float outputmatrix[4][4];
float radius= 0.01;
float slices = 8.0;
float stack = 3.0;
struct OBJECTS
{
int intobjs;
int id;
};
OBJECTS objects;
vector<OBJECTS> objectsl;
void initrotationmatrix()
{
for(int i=0; i<4;i++)
{
rotationmatrix[i][i]=1;
inputmatrix[i][i]=1;
//outputmatrix[i][i]=1;
}
}
void zeroinputmatrix()
{
for(int i=0;i<4;i++)
{
for(int j=0;j<4;j++)
{
//inputmatrix[i][j]=0;
rotationmatrix[i][j]=0;
}
}
for(int i=0; i<4;i++)
{
//inputmatrix[i][i]=1;
rotationmatrix[i][i]=1;
}
}
void setxyztransformation(float xp,float yp,float zp)
{
//l zeroinputmatrix();
for(int i=0; i<4;i++)
{
inputmatrix[i][i]=1;
}
inputmatrix[0][3]=xp;
inputmatrix[1][3]=yp;
inputmatrix[2][3]=zp;
}
void setrotationaboutz(float theta)
{
zeroinputmatrix();
rotationmatrix[0][0]=agk::Cos(theta);
rotationmatrix[0][1]=-agk::Sin(theta);
rotationmatrix[1][0]=agk::Sin(theta);
rotationmatrix[1][1]=agk::Cos(theta);
rotationmatrix[2][2]=1.0f;
rotationmatrix[3][3]=1.0f;
}
void setrotationaboutx(float theta)
{
zeroinputmatrix();
rotationmatrix[0][0]=1.0f;
rotationmatrix[1][1]=agk::Cos(theta);
rotationmatrix[1][2]=-agk::Sin(theta);
rotationmatrix[2][1]=agk::Sin(theta);
rotationmatrix[2][2]=agk::Cos(theta);
rotationmatrix[3][3]=1.0f;
}
void setrotationabouty(float theta)
{
zeroinputmatrix();
rotationmatrix[0][0]=agk::Cos(theta);
rotationmatrix[0][2]=agk::Sin(theta);
rotationmatrix[1][1]=1.0f;
rotationmatrix[2][0]=-agk::Sin(theta);
rotationmatrix[2][2]=agk::Cos(theta);
rotationmatrix[3][3]=1.0f;
}
void multiplyMatrixrot()
{
for(int i=0;i<4;i++)
{
for(int k=0;k<4;k++)
{
outputmatrix[i][k]=0;
for(int z=0;z<4;z++)
{
outputmatrix[i][k] =outputmatrix[i][k]+rotationmatrix[i][z]*inputmatrix[z][k];
}
}
}
}
void multiplyMatrixOtoI()
{
for(int i=0;i<4;i++)
{
for(int k=0;k<4;k++)
{
inputmatrix[i][k] =outputmatrix[i][k];
}
}
}
BVH::BVH()
{
motion = NULL;
Clear();
objects.id=0;
}
BVH::BVH( const char * bvh_file_name )
{
motion = NULL;
Clear();
initrotationmatrix();
Load( bvh_file_name );
}
BVH::~BVH()
{
Clear();
}
void BVH::Clear()
{
int i;
for ( i=0; i<channels.size(); i++ )
delete channels[ i ];
for ( i=0; i<joints.size(); i++ )
delete joints[ i ];
if ( motion != NULL )
delete motion;
is_load_success = false;
file_name = "";
motion_name = "";
num_channel = 0;
channels.clear();
joints.clear();
joint_index.clear();
num_frame = 0;
interval = 0.0;
motion = NULL;
}
//
//
//
void BVH::Load( const char * bvh_file_name )
{
#define BUFFER_LENGTH 1024*4
ifstream file;
char line[ BUFFER_LENGTH ];
char * token;
char separater[] = " :,\t";
vector< Joint * > joint_stack;
Joint * joint = NULL;
Joint * new_joint = NULL;
bool is_site = false;
double x, y ,z;
int i, j;
Clear();
file_name = bvh_file_name;
const char * mn_first = bvh_file_name;
const char * mn_last = bvh_file_name + strlen( bvh_file_name );
if ( strrchr( bvh_file_name, '\\' ) != NULL )
mn_first = strrchr( bvh_file_name, '\\' ) + 1;
else if ( strrchr( bvh_file_name, '/' ) != NULL )
mn_first = strrchr( bvh_file_name, '/' ) + 1;
if ( strrchr( bvh_file_name, '.' ) != NULL )
mn_last = strrchr( bvh_file_name, '.' );
if ( mn_last < mn_first )
mn_last = bvh_file_name + strlen( bvh_file_name );
motion_name.assign( mn_first, mn_last );
file.open( bvh_file_name, ios::in );
if ( file.is_open() == 0 ) return;
if(objectsl.size()!=0)
{
for(int i=0;i<objectsl.size();i++)
{
agk::DeleteObject(objectsl[i].intobjs);
}
}
objectsl.resize(0);
while ( ! file.eof() )
{
if ( file.eof() ) goto bvh_error;
file.getline( line, BUFFER_LENGTH );
token = strtok( line, separater );
if ( token == NULL ) continue;
if ( strcmp( token, "{" ) == 0 )
{
int tid=agk::LoadImage("");
objects.intobjs=agk::CreateObjectSphere(.3,5,5);
agk::SetObjectImage(objects.intobjs,tid,0);
objects.id=objects.id+1;
objectsl.push_back(objects);
joint_stack.push_back( joint );
joint = new_joint;
continue;
}
// ŠÖ߃uƒƒbƒN‚ÌI—¹
if ( strcmp( token, "}" ) == 0 )
{
agk::DeleteObject(objectsl[objectsl.size()-1].intobjs);
//agk::DeleteObject(objectsl[objectsl.size()-2].intobjs);
//agk::DeleteObject(objectsl[objectsl.size()-3].intobjs);
// objectsl.pop_back();
// Œ»Ý‚ÌŠÖß‚ðƒXƒ^ƒbƒN‚©‚çŽæ‚èo‚·
joint = joint_stack.back();
joint_stack.pop_back();
is_site = false;
continue;
}
if ( ( strcmp( token, "ROOT" ) == 0 ) ||
( strcmp( token, "JOINT" ) == 0 ) )
{
new_joint = new Joint();
new_joint->index = joints.size();
new_joint->parent = joint;
new_joint->has_site = false;
new_joint->offset[0] = 0.0; new_joint->offset[1] = 0.0; new_joint->offset[2] = 0.0;
new_joint->site[0] = 0.0; new_joint->site[1] = 0.0; new_joint->site[2] = 0.0;
joints.push_back( new_joint );
if ( joint )
joint->children.push_back( new_joint );
// ŠÖß–¼‚Ì“Ç‚Ýž‚Ý
token = strtok( NULL, "" );
while ( *token == ' ' ) token ++;
new_joint->name = token;
joint_index[ new_joint->name ] = new_joint;
continue;
}
if ( ( strcmp( token, "End" ) == 0 ) )
{
new_joint = joint;
is_site = true;
continue;
}
if ( strcmp( token, "OFFSET" ) == 0 )
{
token = strtok( NULL, separater );
x = token ? atof( token ) : 0.0;
token = strtok( NULL, separater );
y = token ? atof( token ) : 0.0;
token = strtok( NULL, separater );
z = token ? atof( token ) : 0.0;
if ( is_site )
{
joint->has_site = true;
joint->site[0] = x;
joint->site[1] = y;
joint->site[2] = z;
}
else
{
joint->offset[0] = x;
joint->offset[1] = y;
joint->offset[2] = z;
}
continue;
}
if ( strcmp( token, "CHANNELS" ) == 0 )
{
token = strtok( NULL, separater );
joint->channels.resize( token ? atoi( token ) : 0 );
for ( i=0; i<joint->channels.size(); i++ )
{
Channel * channel = new Channel();
channel->joint = joint;
channel->index = channels.size();
channels.push_back( channel );
joint->channels[ i ] = channel;
token = strtok( NULL, separater );
if ( strcmp( token, "Xrotation" ) == 0 )
channel->type = X_ROTATION;
else if ( strcmp( token, "Yrotation" ) == 0 )
channel->type = Y_ROTATION;
else if ( strcmp( token, "Zrotation" ) == 0 )
channel->type = Z_ROTATION;
else if ( strcmp( token, "Xposition" ) == 0 )
channel->type = X_POSITION;
else if ( strcmp( token, "Yposition" ) == 0 )
channel->type = Y_POSITION;
else if ( strcmp( token, "Zposition" ) == 0 )
channel->type = Z_POSITION;
}
}
if ( strcmp( token, "MOTION" ) == 0 )
break;
}
file.getline( line, BUFFER_LENGTH );
token = strtok( line, separater );
if ( strcmp( token, "Frames" ) != 0 ) goto bvh_error;
token = strtok( NULL, separater );
if ( token == NULL ) goto bvh_error;
num_frame = atoi( token );
file.getline( line, BUFFER_LENGTH );
token = strtok( line, ":" );
if ( strcmp( token, "Frame Time" ) != 0 ) goto bvh_error;
token = strtok( NULL, separater );
if ( token == NULL ) goto bvh_error;
interval = atof( token );
num_channel = channels.size();
motion = new float[ num_frame * num_channel ];
for ( i=0; i<num_frame; i++ )
{
file.getline( line, BUFFER_LENGTH );
token = strtok( line, separater );
for ( j=0; j<num_channel; j++ )
{
if ( token == NULL )
goto bvh_error;
motion[ i*num_channel + j ] = atof( token );
token = strtok( NULL, separater );
}
}
file.close();
is_load_success = true;
agk::DeleteObject(objectsl[objectsl.size()-1].intobjs);
agk::DeleteObject(objectsl[objectsl.size()-2].intobjs);
agk::DeleteObject(objectsl[objectsl.size()-3].intobjs);
agk::DeleteObject(objectsl[objectsl.size()-4].intobjs);
agk::DeleteObject(objectsl[objectsl.size()-5].intobjs);
agk::DeleteObject(objectsl[objectsl.size()-6].intobjs);
agk::DeleteObject(objectsl[objectsl.size()-7].intobjs);
return;
bvh_error:
file.close();
}
//vector BVH::XVECTOR(
//
//
//
void BVH::RenderFigure( int frame_no, float scale )
{
RenderFigure( joints[ 0 ], motion + frame_no * num_channel, scale,0 );
}
void BVH::RenderFigure( const Joint * joint, float * data, float scale,int ii )
{
glPushMatrix();
//initrotationmatrix(); //GLdouble mm[16];
int i, j;
for ( i=0; i<joint->channels.size(); i++ )
{
Channel * channel = joint->channels[ i ];
if ( channel->type == X_ROTATION )
{
setrotationaboutx(data[channel->index]);
// agk::SetObjectRotation(objectsl[joint->index].intobjs,data[channel->index],0,0);
//multiplyMatrixrot();
// multiplyMatrixOtoI();
// // agk::Print(data[ channel->index ]);
//glRotatef( data[ channel->index ], 1.0f, 0.0f, 0.0f );
}
else if ( channel->type == Y_ROTATION )
{
// agk::Print("");
//setrotationabouty(data[channel->index]);
//multiplyMatrixrot();
//multiplyMatrixOtoI();
//glRotatef( data[ channel->index ], 0.0f, 1.0f, 0.0f );
}
else if ( channel->type == Z_ROTATION )
{
// setrotationaboutz(data[channel->index]);
// multiplyMatrixrot();
// multiplyMatrixOtoI();
//setrotationaboutz(data[channel->index]);
//agk::SetObjectRotation(objectsl[joint->index].intobjs,0,0,data[channel->index]);
//multiplyMatrixrot();
//glRotatef( data[ channel->index ], 0.0f, 0.0f, 1.0f );
}
}
if ( joint->parent == NULL )
{
// setxyztransformation(data[ 0 ] * scale,data[ 1 ] * scale,data[ 2 ] * scale);
// multiplyMatrixrot();
// multiplyMatrixOtoI();
agk::SetObjectPosition(objectsl[0].intobjs,data[ 0 ] * scale,data[ 1 ] * scale,data[ 2 ] * scale);
float flo=data[ 0 ];
char buffer[255];
sprintf(buffer,"%f",flo);
agk::DeleteText(1);
agk::CreateText(1,buffer);
agk::SetTextSize(1,3);
agk::SetTextColor(1,255,255,255,255);
agk::SetTextPosition(1,10,10);
// agk::Print(data[ 0 ] * scale);
// agk::Print(data[ 1 ] * scale);
// agk::Print(data[ 2 ] * scale);
//glMultMatrixd(mm);
//glTranslatef( data[ 0 ] * scale, data[ 1 ] * scale, data[ 2 ] * scale );
}
else
{
// setxyztransformation(joint->offset[ 0 ] * scale,joint->offset[ 1 ] * scale,joint->offset[ 2 ] * scale);
// multiplyMatrixrot();
// multiplyMatrixOtoI();
//joint->channels[0]->;
// agk::SetObjectPosition(objectsl[joint->index].intobjs,joint->offset[ 0 ] * scale, joint->offset[ 1 ] * scale, joint->offset[ 2 ] * scale);
// agk::SetObjectPosition(joint->index,joint->offset[ 0 ] * scale, joint->offset[ 1 ] * scale, joint->offset[ 2 ] * scale);
float atemp=joint->offset[ 0 ] ;
// agk::Print( atemp);
//agk::SetObjectPosition(objectsl[joint->index].intobjs,outputmatrix[0][3],outputmatrix[1][3],outputmatrix[2][3]);
// glMultMatrixd(mm);
//agk::SetObjectPosition(objectsl[joint->index].intobjs,data[0]*scale,data[1]*scale,data[2]*scale);
// glTranslatef( joint->offset[ 0 ] * scale, joint->offset[ 1 ] * scale, joint->offset[ 2 ] * scale );
}
// agk::Print(joint->channels[0]->joint->channels[0]->joint->channels[0]->joint->channels[0]->joint->channels[0]->joint->channels[0]->type);
const Joint * tjoint=joint;
//forward kinematics
//to do push Joints on a vector then pop stack doing //multiplication and rotation matrix math.
if ( joint->children.size() == 0 )
{
if(joint->name=="RightToeBase"||joint->name=="LeftToeBase")
{
float xpos=joint->offset[0]*scale;
float ypos=joint->offset[1]*scale;
float zpos=joint->offset[2]*scale;
while(tjoint->parent!=NULL)
{
xpos+=tjoint->parent->offset[0]*scale;
ypos+=tjoint->parent->offset[1]*scale;
zpos+=tjoint->parent->offset[2]*scale;
tjoint=tjoint->parent;
}
agk::SetObjectPosition(objectsl[joint->index].intobjs,data[0]*scale+xpos+joint->site[ 0 ] * scale,data[1]*scale+ypos+joint->site[ 1 ] * scale,data[2]*scale+zpos+joint->site[ 2 ] * scale);
}
else
{
float xpos=0;
float ypos=0;
float zpos=0;
xpos=joint->offset[0]*scale;
ypos=joint->offset[1]*scale;
zpos=joint->offset[2]*scale;
while(tjoint->parent!=NULL)
{
xpos+=tjoint->parent->offset[0]*scale;
ypos+=tjoint->parent->offset[1]*scale;
zpos+=tjoint->parent->offset[2]*scale;
tjoint=tjoint->parent;
}
//setxyztransformation(joint->site[0]*scale+data[0]*scale,joint->site[1]*scale+data[1]*scale,joint->offset[2]*scale+data[2]*scale);//center[0]*scale+data[ 0 ] * scale,center[1]*scale+data[ 1 ] * scale,center[2]*scale+data[ 2 ] * scale);
//multiplyMatrixrot();
agk::SetObjectPosition(objectsl[joint->index].intobjs,data[0]*scale+xpos+joint->site[ 0 ] * scale,data[1]*scale+ypos+joint->site[ 1 ] * scale,data[2]*scale+zpos+joint->site[ 2 ] * scale);
}
// RenderBone( 0.0f, 0.0f, 0.0f, joint->site[ 0 ] * scale, joint->site[ 1 ] * scale, joint->site[ 2 ] * scale,100 );
}
else if ( joint->children.size() == 1 )
{
//if(joint->name=="Left
if(joint->name=="Neck1"||joint->name=="LeftArm"||joint->name=="RightArm")
{
Joint * child = joint->children[ 0 ];
Joint * parent = joint->parent->parent;
Joint * child1=parent->children[1];
Joint * child2=parent->children[2];
// float offx=(child1->offset[0]*scale+child2->offset[0]*scale+parent->offset[0]*scale)/3+data[0]*scale;
// float offy=(child1->offset[1]*scale+child2->offset[1]*scale+parent->offset[1]*scale)/3+data[1]*scale;
// float offz=(child1->offset[2]*scale+child2->offset[2]*scale+parent->offset[2]*scale)/3+data[2]*scale;
float xpos=0;
float ypos=0;
float zpos=0;
xpos=joint->offset[0]*scale;
ypos=joint->offset[1]*scale;
zpos=joint->offset[2]*scale;
while(tjoint->parent!=NULL)
{
xpos+=tjoint->parent->offset[0]*scale;
ypos+=tjoint->parent->offset[1]*scale;
zpos+=tjoint->parent->offset[2]*scale;
tjoint=tjoint->parent;
}
agk::SetObjectPosition(objectsl[joint->index].intobjs,xpos+data[0]*scale,ypos+data[1]*scale,zpos+data[2]*scale);
}
else
{
Joint * child = joint->children[ 0 ];
float xpos=0;
float ypos=0;
float zpos=0;
xpos=joint->offset[0]*scale;
ypos=joint->offset[1]*scale;
zpos=joint->offset[2]*scale;
while(tjoint->parent!=NULL)
{
xpos+=tjoint->parent->offset[0]*scale;
ypos+=tjoint->parent->offset[1]*scale;
zpos+=tjoint->parent->offset[2]*scale;
tjoint=tjoint->parent;
}
agk::SetObjectPosition(objectsl[joint->index].intobjs,xpos+data[0]*scale,ypos+data[1]*scale,zpos+data[2]*scale);
}
// setxyztransformation(child->offset[0]*scale+data[0]*scale,child->offset[1]*scale+data[1]*scale,child->offset[2]*scale+data[2]*scale);//center[0]*scale+data[ 0 ] * scale,center[1]*scale+data[ 1 ] * scale,center[2]*scale+data[ 2 ] * scale);
//multiplyMatrixrot();
//Joint * child = joint->children[ 0 ];
// agk::SetObjectPosition(objectsl[joint->index].intobjs,agk::GetObjectX(objectsl[joint->index].intobjs)+child->offset[0]*scale+data[ 0 ] * scale,agk::GetObjectY(objectsl[joint->index].intobjs)+ child->offset[ 1 ] * scale+data[ 1 ] * scale,agk::GetObjectZ(objectsl[joint->index].intobjs)+ joint->offset[ 2 ] * scale+data[ 2 ] * scale);
//agk::SetObjectPosition(objectsl[joint->index].intobjs,outputmatrix[0][3],outputmatrix[1][3],outputmatrix[2][3]);
// RenderBone( 0.0f, 0.0f, 0.0f, child->offset[ 0 ] * scale, child->offset[ 1 ] * scale, child->offset[ 2 ] * scale,101 );
}
else if ( joint->children.size() > 1 )
{
float center[ 3 ] = { 0.0f, 0.0f, 0.0f };
for ( i=0; i<joint->children.size(); i++ )
{
Joint * child = joint->children[ i ];
center[ 0 ] += child->offset[ 0 ];
center[ 1 ] += child->offset[ 1 ];
center[ 2 ] += child->offset[ 2 ];
}
center[ 0 ] /= joint->children.size() + 1;
center[ 1 ] /= joint->children.size() + 1;
center[ 2 ] /= joint->children.size() + 1;
if(joint->parent==NULL)
{
agk::Print(data[0]*scale);
agk::SetObjectPosition(objectsl[joint->index].intobjs,data[0]*scale,data[1]*scale,data[2]*scale);
}
else
{
Joint * child = joint->children[ 0 ];
float xpos=0;
float ypos=0;
float zpos=0;
xpos=center[0]*scale;
ypos=center[1]*scale;
zpos=center[2]*scale;
while(tjoint->parent!=NULL)
{
xpos+=tjoint->parent->offset[0]*scale;
ypos+=tjoint->parent->offset[1]*scale;
zpos+=tjoint->parent->offset[2]*scale;
tjoint=tjoint->parent;
}
agk::SetObjectPosition(objectsl[joint->index].intobjs,xpos+data[0]*scale,ypos+data[1]*scale,zpos+data[2]*scale);
}
//if(joint->name==leftcollar
// RenderBone( 0.0f, 0.0f, 0.0f, center[ 0 ] * scale, center[ 1 ] * scale, center[ 2 ] * scale,i );
// setxyztransformation(center[0]*scale+data[ 0 ] * scale+data[0]*scale,center[1]*scale+data[ 1 ] * scale,center[2]*scale+data[ 2 ] * scale+data[2]*scale);
// multiplyMatrixrot();
// agk::SetObjectPosition(objectsl[joint->index].intobjs,outputmatrix[0][3],outputmatrix[1][3],outputmatrix[2][3]);
}
else
{
agk::SetObjectPosition(objectsl[joint->index].intobjs,data[0]*scale,data[1]*scale,data[2]*scale);
}
for (int i=0; i<joint->children.size(); i++ )
{
RenderFigure( joint->children[ i ], data, scale,i );
}
//zeroinputmatrix();
// glPopMatrix();
}
Developer of Space Chips, pianobasic, zipzapzoom, and vet pinball apps. Developed the tiled map engine seen on the showcase. Veteran for the military.
