Commit a46e6f44 authored by Dirk Wilden's avatar Dirk Wilden
Browse files

BioVision reader. Saving skeletons disabled because its not working correct

git-svn-id: http://www.openflipper.org/svnrepo/OpenFlipper/branches/Free@10557 383ad7c9-94d9-4d36-a494-682f7c89f535
parent ca88bd63
include (plugin)
openflipper_plugin ()
/**** EulerAngles.c - Convert Euler angles to/from matrix or quat ****/
/* Ken Shoemake, 1993 */
#include <math.h>
#include <float.h>
#include "EulerAngles.hh"
EulerAngles Eul_(float ai, float aj, float ah, int order)
{
EulerAngles ea;
ea.x = ai; ea.y = aj; ea.z = ah;
ea.w = order;
return (ea);
}
/* Construct quaternion from Euler angles (in radians). */
Quat Eul_ToQuat(EulerAngles ea)
{
Quat qu;
double a[3], ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
int i,j,k,h,n,s,f;
EulGetOrd(ea.w,i,j,k,h,n,s,f);
if (f==EulFrmR) {float t = ea.x; ea.x = ea.z; ea.z = t;}
if (n==EulParOdd) ea.y = -ea.y;
ti = ea.x*0.5; tj = ea.y*0.5; th = ea.z*0.5;
ci = cos(ti); cj = cos(tj); ch = cos(th);
si = sin(ti); sj = sin(tj); sh = sin(th);
cc = ci*ch; cs = ci*sh; sc = si*ch; ss = si*sh;
if (s==EulRepYes) {
a[i] = cj*(cs + sc); /* Could speed up with */
a[j] = sj*(cc + ss); /* trig identities. */
a[k] = sj*(cs - sc);
qu.w = cj*(cc - ss);
} else {
a[i] = cj*sc - sj*cs;
a[j] = cj*ss + sj*cc;
a[k] = cj*cs - sj*sc;
qu.w = cj*cc + sj*ss;
}
if (n==EulParOdd) a[j] = -a[j];
qu.x = a[X]; qu.y = a[Y]; qu.z = a[Z];
return (qu);
}
/* Construct matrix from Euler angles (in radians). */
void Eul_ToHMatrix(EulerAngles ea, HMatrix M)
{
double ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
int i,j,k,h,n,s,f;
EulGetOrd(ea.w,i,j,k,h,n,s,f);
if (f==EulFrmR) {float t = ea.x; ea.x = ea.z; ea.z = t;}
if (n==EulParOdd) {ea.x = -ea.x; ea.y = -ea.y; ea.z = -ea.z;}
ti = ea.x; tj = ea.y; th = ea.z;
ci = cos(ti); cj = cos(tj); ch = cos(th);
si = sin(ti); sj = sin(tj); sh = sin(th);
cc = ci*ch; cs = ci*sh; sc = si*ch; ss = si*sh;
if (s==EulRepYes) {
M[i][i] = cj; M[i][j] = sj*si; M[i][k] = sj*ci;
M[j][i] = sj*sh; M[j][j] = -cj*ss+cc; M[j][k] = -cj*cs-sc;
M[k][i] = -sj*ch; M[k][j] = cj*sc+cs; M[k][k] = cj*cc-ss;
} else {
M[i][i] = cj*ch; M[i][j] = sj*sc-cs; M[i][k] = sj*cc+ss;
M[j][i] = cj*sh; M[j][j] = sj*ss+cc; M[j][k] = sj*cs-sc;
M[k][i] = -sj; M[k][j] = cj*si; M[k][k] = cj*ci;
}
M[W][X]=M[W][Y]=M[W][Z]=M[X][W]=M[Y][W]=M[Z][W]=0.0; M[W][W]=1.0;
}
/* Convert matrix to Euler angles (in radians). */
EulerAngles Eul_FromHMatrix(HMatrix M, int order)
{
EulerAngles ea;
int i,j,k,h,n,s,f;
EulGetOrd(order,i,j,k,h,n,s,f);
if (s==EulRepYes) {
double sy = sqrt(M[i][j]*M[i][j] + M[i][k]*M[i][k]);
if (sy > 16*FLT_EPSILON) {
ea.x = atan2(M[i][j], M[i][k]);
ea.y = atan2((double)sy,(double)M[i][i]);
ea.z = atan2(M[j][i], -M[k][i]);
} else {
ea.x = atan2(-M[j][k], M[j][j]);
ea.y = atan2((double)sy,(double) M[i][i]);
ea.z = 0;
}
} else {
double cy = sqrt(M[i][i]*M[i][i] + M[j][i]*M[j][i]);
if (cy > 16*FLT_EPSILON) {
ea.x = atan2(M[k][j], M[k][k]);
ea.y = atan2((double)-M[k][i],(double) cy);
ea.z = atan2(M[j][i], M[i][i]);
} else {
ea.x = atan2(-M[j][k], M[j][j]);
ea.y = atan2((double)-M[k][i],(double) cy);
ea.z = 0;
}
}
if (n==EulParOdd) {ea.x = -ea.x; ea.y = - ea.y; ea.z = -ea.z;}
if (f==EulFrmR) {float t = ea.x; ea.x = ea.z; ea.z = t;}
ea.w = order;
return (ea);
}
/* Convert quaternion to Euler angles (in radians). */
EulerAngles Eul_FromQuat(Quat q, int order)
{
HMatrix M;
double Nq = q.x*q.x+q.y*q.y+q.z*q.z+q.w*q.w;
double s = (Nq > 0.0) ? (2.0 / Nq) : 0.0;
double xs = q.x*s, ys = q.y*s, zs = q.z*s;
double wx = q.w*xs, wy = q.w*ys, wz = q.w*zs;
double xx = q.x*xs, xy = q.x*ys, xz = q.x*zs;
double yy = q.y*ys, yz = q.y*zs, zz = q.z*zs;
M[X][X] = 1.0 - (yy + zz); M[X][Y] = xy - wz; M[X][Z] = xz + wy;
M[Y][X] = xy + wz; M[Y][Y] = 1.0 - (xx + zz); M[Y][Z] = yz - wx;
M[Z][X] = xz - wy; M[Z][Y] = yz + wx; M[Z][Z] = 1.0 - (xx + yy);
M[W][X]=M[W][Y]=M[W][Z]=M[X][W]=M[Y][W]=M[Z][W]=0.0; M[W][W]=1.0;
return (Eul_FromHMatrix(M, order));
}
/**** EulerAngles.h - Support for 24 angle schemes ****/
/**** by Ken Shoemake, shoemake@graphics.cis.upenn.edu ****/
/**** in "Graphics Gems IV", Academic Press, 1994 ****/
#ifndef _H_EulerAngles
#define _H_EulerAngles
#include "QuatTypes.hh"
/*** Order type constants, constructors, extractors ***/
/* There are 24 possible conventions, designated by: */
/* o EulAxI = axis used initially */
/* o EulPar = parity of axis permutation */
/* o EulRep = repetition of initial axis as last */
/* o EulFrm = frame from which axes are taken */
/* Axes I,J,K will be a permutation of X,Y,Z. */
/* Axis H will be either I or K, depending on EulRep. */
/* Frame S takes axes from initial static frame. */
/* If ord = (AxI=X, Par=Even, Rep=No, Frm=S), then */
/* {a,b,c,ord} means Rz(c)Ry(b)Rx(a), where Rz(c)v */
/* rotates v around Z by c radians. */
#define EulFrmS 0
#define EulFrmR 1
#define EulFrm(ord) ((unsigned)(ord)&1)
#define EulRepNo 0
#define EulRepYes 1
#define EulRep(ord) (((unsigned)(ord)>>1)&1)
#define EulParEven 0
#define EulParOdd 1
#define EulPar(ord) (((unsigned)(ord)>>2)&1)
#define EulSafe "\000\001\002\000"
#define EulNext "\001\002\000\001"
#define EulAxI(ord) ((int)(EulSafe[(((unsigned)(ord)>>3)&3)]))
#define EulAxJ(ord) ((int)(EulNext[EulAxI(ord)+(EulPar(ord)==EulParOdd)]))
#define EulAxK(ord) ((int)(EulNext[EulAxI(ord)+(EulPar(ord)!=EulParOdd)]))
#define EulAxH(ord) ((EulRep(ord)==EulRepNo)?EulAxK(ord):EulAxI(ord))
/* EulGetOrd unpacks all useful information about order simultaneously. */
#define EulGetOrd(ord,i,j,k,h,n,s,f) {unsigned o=ord;f=o&1;o>>1;s=o&1;o>>1;\
n=o&1;o>>1;i=EulSafe[o&3];j=EulNext[i+n];k=EulNext[i+1-n];h=s?k:i;}
/* EulOrd creates an order value between 0 and 23 from 4-tuple choices. */
#define EulOrd(i,p,r,f) (((((((i)<<1)+(p))<<1)+(r))<<1)+(f))
/* Static axes */
#define EulOrdXYZs EulOrd(X,EulParEven,EulRepNo,EulFrmS)
#define EulOrdXYXs EulOrd(X,EulParEven,EulRepYes,EulFrmS)
#define EulOrdXZYs EulOrd(X,EulParOdd,EulRepNo,EulFrmS)
#define EulOrdXZXs EulOrd(X,EulParOdd,EulRepYes,EulFrmS)
#define EulOrdYZXs EulOrd(Y,EulParEven,EulRepNo,EulFrmS)
#define EulOrdYZYs EulOrd(Y,EulParEven,EulRepYes,EulFrmS)
#define EulOrdYXZs EulOrd(Y,EulParOdd,EulRepNo,EulFrmS)
#define EulOrdYXYs EulOrd(Y,EulParOdd,EulRepYes,EulFrmS)
#define EulOrdZXYs EulOrd(Z,EulParEven,EulRepNo,EulFrmS)
#define EulOrdZXZs EulOrd(Z,EulParEven,EulRepYes,EulFrmS)
#define EulOrdZYXs EulOrd(Z,EulParOdd,EulRepNo,EulFrmS)
#define EulOrdZYZs EulOrd(Z,EulParOdd,EulRepYes,EulFrmS)
/* Rotating axes */
#define EulOrdZYXr EulOrd(X,EulParEven,EulRepNo,EulFrmR)
#define EulOrdXYXr EulOrd(X,EulParEven,EulRepYes,EulFrmR)
#define EulOrdYZXr EulOrd(X,EulParOdd,EulRepNo,EulFrmR)
#define EulOrdXZXr EulOrd(X,EulParOdd,EulRepYes,EulFrmR)
#define EulOrdXZYr EulOrd(Y,EulParEven,EulRepNo,EulFrmR)
#define EulOrdYZYr EulOrd(Y,EulParEven,EulRepYes,EulFrmR)
#define EulOrdZXYr EulOrd(Y,EulParOdd,EulRepNo,EulFrmR)
#define EulOrdYXYr EulOrd(Y,EulParOdd,EulRepYes,EulFrmR)
#define EulOrdYXZr EulOrd(Z,EulParEven,EulRepNo,EulFrmR)
#define EulOrdZXZr EulOrd(Z,EulParEven,EulRepYes,EulFrmR)
#define EulOrdXYZr EulOrd(Z,EulParOdd,EulRepNo,EulFrmR)
#define EulOrdZYZr EulOrd(Z,EulParOdd,EulRepYes,EulFrmR)
EulerAngles Eul_(float ai, float aj, float ah, int order);
Quat Eul_ToQuat(EulerAngles ea);
void Eul_ToHMatrix(EulerAngles ea, HMatrix M);
EulerAngles Eul_FromHMatrix(HMatrix M, int order);
EulerAngles Eul_FromQuat(Quat q, int order);
#endif
/*===========================================================================*\
* *
* OpenFlipper *
* Copyright (C) 2001-2010 by Computer Graphics Group, RWTH Aachen *
* www.openflipper.org *
* *
*---------------------------------------------------------------------------*
* This file is part of OpenFlipper. *
* *
* OpenFlipper is free software: you can redistribute it and/or modify *
* it under the terms of the GNU Lesser General Public License as *
* published by the Free Software Foundation, either version 3 of *
* the License, or (at your option) any later version with the *
* following exceptions: *
* *
* If other files instantiate templates or use macros *
* or inline functions from this file, or you compile this file and *
* link it with other files to produce an executable, this file does *
* not by itself cause the resulting executable to be covered by the *
* GNU Lesser General Public License. This exception does not however *
* invalidate any other reasons why the executable file might be *
* covered by the GNU Lesser General Public License. *
* *
* OpenFlipper 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 Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU LesserGeneral Public *
* License along with OpenFlipper. If not, *
* see <http://www.gnu.org/licenses/>. *
* *
\*===========================================================================*/
/*===========================================================================*\
* *
* $Revision: 7936 $ *
* $Author: kremer $ *
* $Date: 2009-12-15 14:01:19 +0100 (Tue, 15 Dec 2009) $ *
* *
\*===========================================================================*/
#include <QtGui>
#include <QFileInfo>
#include "FileBVH.hh"
#include <iostream>
#include "OpenFlipper/BasePlugin/PluginFunctions.hh"
#include "OpenFlipper/common/GlobalOptions.hh"
#include <ACG/Geometry/AlgorithmsAngleT.hh>
#include "EulerAngles.hh"
#include <bitset>
#define HIERARCHY std::bitset<4>(0ul)
#define ROOT_DEFINITION std::bitset<4>(1ul)
#define OPENED_BRACKET std::bitset<4>(2ul)
#define CLOSED_BRACKET std::bitset<4>(3ul)
#define OFFSET std::bitset<4>(4ul)
#define CHANNELS std::bitset<4>(5ul)
#define JOINT std::bitset<4>(6ul)
#define ENDSITE std::bitset<4>(7ul)
#define MOTION std::bitset<4>(8ul)
#define FRAMES std::bitset<4>(9ul)
#define FRAME_TIME std::bitset<4>(10ul)
#define CHANNEL_DATA std::bitset<4>(11ul)
//-----------------------------------------------------------------------------
//data structures used for parsing
enum ChannelType{NotGiven,XP,YP,ZP,XR,YR,ZR}; //contains the informations about the use of this datafield
class JointInfo{ //for every Joint we will create one such instance during Hirachy parsing. It will contain the informations needed to generate the Transformation from the frame values
public:
ChannelType dataChannels[6]; //will state which channels information is stored in the current datafields value.
unsigned int dataOffset[6]; //will contain the index of the channel in the frame data
unsigned int channelOffset; //used top keep track of the highest channel currently used
//constructor
//will set proper default values (as the dataChannels will only be written if an according option is found in the file)
JointInfo(){
channelOffset=0; //we assign the first channelType to the first entry
for(int i=0;i<6;i++){ //all channels that are not overwritten are Unused
dataChannels[i]=NotGiven;
dataOffset[i]=0;
}
} //end ctor()
}; //end class
//end of data structure
//-----------------------------------------------------------------------------
/// Constructor
FileBVHPlugin::FileBVHPlugin()
: ignoreJointScaling_(true),
loadOptions_(0),
checkJointScaling_(0),
loadDefaultButton_(0)
{
}
//-----------------------------------------------------------------------------------------------------
void FileBVHPlugin::initializePlugin() {
}
//-----------------------------------------------------------------------------------------------------
QString FileBVHPlugin::getLoadFilters() {
return QString( tr("Biovision Format files ( *.bvh )") );
}
//-----------------------------------------------------------------------------------------------------
QString FileBVHPlugin::getSaveFilters() {
return QString( tr("Biovision Format files ( *.bvh )") );
}
//-----------------------------------------------------------------------------------------------------
DataType FileBVHPlugin::supportedType() {
DataType type = DATA_SKELETON;
return type;
}
//-----------------------------------------------------------------------------------------------------
void trimString( std::string& _string) {
// Trim Both leading and trailing spaces
size_t start = _string.find_first_not_of(" \t\r\n");
size_t end = _string.find_last_not_of(" \t\r\n");
if(( std::string::npos == start ) || ( std::string::npos == end))
_string = "";
else
_string = _string.substr( start, end-start+1 );
}
//-----------------------------------------------------------------------------------------------------
int FileBVHPlugin::loadObject(QString _filename) {
if ( checkJointScaling_ != 0 )
ignoreJointScaling_ = checkJointScaling_->isChecked();
else
ignoreJointScaling_ = OpenFlipperSettings().value("FileBVH/Load/JointScaling",true).toBool();
QString path = QFileInfo(_filename).absolutePath();
QString baseName = QFileInfo(_filename).baseName();
//setup filestream
std::fstream input( _filename.toUtf8(), std::ios_base::in );
if ( !input.is_open() || !input.good() ){
emit log(LOGERR, tr("Error: cannot open file %1").arg(_filename) );
return -1;
}
//add a skeleton
int id = -1;
emit addEmptyObject(DATA_SKELETON, id);
BaseObjectData* object = 0;
Skeleton* skeleton = 0;
if(PluginFunctions::getObject( id, object)){
skeleton = PluginFunctions::skeleton( object );
object->setName( baseName + ".bvh" );
}
if (skeleton == 0){
emit log(LOGERR, tr("Error: Unable to add skeleton!"));
return -1;
}
Skeleton::Joint* currentParent = 0;
Skeleton::Pose* refPose = skeleton->referencePose();
std::string line;
std::string keyWrd;
std::bitset<4> waitingFor = HIERARCHY;
std::map< Skeleton::Joint* , JointInfo> jointInfos;
uint dataOffset = 0; //Offset of the current channel in the frame data
AnimationHandle animHandle;
uint currentFrame = 0;
uint frameCount = 0;
while( input && !input.eof() )
{
std::getline(input,line);
if ( input.bad() ){
emit log(LOGERR, tr("Error: could not read file properly!"));
return -1;
}
// Trim Both leading and trailing spaces
trimString(line);
// ignore empty lines
if ( line.size() == 0 )
continue;
std::stringstream stream(line);
//read keyword from stream
stream >> keyWrd;
//HIERARCHY
if ( (waitingFor == HIERARCHY) && (keyWrd == "HIERARCHY") ){
waitingFor = ROOT_DEFINITION;
continue;
}
//ROOT_DEFINITION
if ( (waitingFor == ROOT_DEFINITION) && (keyWrd == "ROOT") ){
std::string name;
stream >> name;
Skeleton::Joint* rootJoint = new Skeleton::Joint(0, name);
skeleton->addJoint(0, rootJoint);
JointInfo info; //we found a new Joint, hence we need an ne JointInfo to store the channel inforamtions.
jointInfos[rootJoint]=info; //store Joint info for later use in case of CHANNELS
currentParent = rootJoint;
waitingFor = OPENED_BRACKET;
continue;
}
//OPENED_BRACKET
if ( (waitingFor == OPENED_BRACKET) && (keyWrd == "{") ){
waitingFor = OFFSET | CHANNELS | JOINT | ENDSITE | CLOSED_BRACKET;
continue;
}
//CLOSED_BRACKET
if ( (!(waitingFor&CLOSED_BRACKET).none()) && (keyWrd == "}") ){
if ( currentParent->parent() == 0 )
waitingFor = MOTION;
else{
waitingFor = JOINT | CLOSED_BRACKET;
}
currentParent = currentParent->parent();
continue;
}
//JOINT
if ( (!(waitingFor&JOINT).none()) && (keyWrd == "JOINT") ){
std::string name;
stream >> name;
Skeleton::Joint* newJoint = new Skeleton::Joint(currentParent, name);
skeleton->addJoint(currentParent, newJoint);
JointInfo info; //we found a new Joint, hence we need an ne JointInfo to store the channel inforamtions.
jointInfos[newJoint]=info; //store Joint info for later use in case of CHANNELS
currentParent = newJoint;
waitingFor = OPENED_BRACKET;
continue;
}
//OFFSET
if ( (!(waitingFor&OFFSET).none()) && (keyWrd == "OFFSET") ){
ACG::Vec3d translation;
stream >> translation[0];
stream >> translation[1];
stream >> translation[2];
refPose->setLocalTranslation(currentParent->id(), translation );
continue;
}
//CHANNELS
if ( (!(waitingFor&CHANNELS).none()) && (keyWrd == "CHANNELS") ){
uint channelCount;
stream >> channelCount;
JointInfo& info=jointInfos[ currentParent ];
if(channelCount>6) //well somethings wrong here...
std::cerr << "Error: channel count to big, will crash very soon" << std::endl;
for (uint i=0; i < channelCount; i++){ //parse channel informations
std::string channelType;
stream >> channelType;
//extract Channel position info and store in info.dataChannels
if (channelType == "Xposition")
info.dataChannels[info.channelOffset]=XP;
else if (channelType == "Yposition")
info.dataChannels[info.channelOffset]=YP;
else if (channelType == "Zposition")
info.dataChannels[info.channelOffset]=ZP;
else if (channelType == "Xrotation")
info.dataChannels[info.channelOffset]=XR;
else if (channelType == "Yrotation")
info.dataChannels[info.channelOffset]=YR;
else if (channelType == "Zrotation")
info.dataChannels[info.channelOffset]=ZR;
else
{std::cerr << "Error: Unknown channelType. Ignoring." << std::endl;}
if(info.dataChannels[info.channelOffset]!=NotGiven){ //if there is a channel assigned
info.dataOffset[info.channelOffset]=dataOffset; //the value for this channel will be found this data position
info.channelOffset++; //write next info into the next index
}
dataOffset++; // the data will be read even if there is no channel assigned to this data (e.g. unknown channel type)
}
continue;
}
// ENDSITE
if ( (!(waitingFor&ENDSITE).none()) && (keyWrd == "End") ){
std::string site;
stream >> site;
std::string name = "End";
Skeleton::Joint* newJoint = new Skeleton::Joint(currentParent, currentParent->name() + name);
skeleton->addJoint(currentParent, newJoint);
currentParent = newJoint;
waitingFor = OPENED_BRACKET;
continue;
}
//MOTION
if ( (waitingFor == MOTION) && (keyWrd == "MOTION") ){
waitingFor = FRAMES;
continue;
}
//Frames
if ( (waitingFor == FRAMES) && (keyWrd == "Frames:") ){
stream >> frameCount;
FrameAnimationT<ACG::Vec3d>* animation = new FrameAnimationT<ACG::Vec3d>(skeleton, frameCount);
animHandle = skeleton->addAnimation(baseName.toStdString(), animation);
waitingFor = FRAME_TIME;
continue;
}
//Frame Time
if ( (waitingFor == FRAME_TIME) && (keyWrd == "Frame") ){
std::string time;
stream >> time;
double frameTime;
stream >> frameTime;
skeleton->animation(animHandle)->setFps( frameTime * 1000 );
waitingFor = CHANNEL_DATA;
continue;
}
//Channel Data
if ( (waitingFor == CHANNEL_DATA) ){
// a vector to store all the data for this frame
vector<double> data(dataOffset,0.0);
Skeleton::Pose* pose = 0;
if ( currentFrame < frameCount ){
animHandle.setFrame( currentFrame );
pose = skeleton->pose(animHandle);
}
else
{std::cerr << "Warning: Too many frames specified in file." << std::endl;}
//since we dont have a keyWrd here