void ofxBvh::load(string path)
{
path = ofToDataPath(path);
string data = ofBufferFromFile(path).getText();
const size_t HIERARCHY_BEGIN = data.find("HIERARCHY", 0);
const size_t MOTION_BEGIN = data.find("MOTION", 0);
if (HIERARCHY_BEGIN == string::npos
|| MOTION_BEGIN == string::npos)
{
ofLogError("ofxBvh", "invalid bvh format");
return;
}
parseHierarchy(data.substr(HIERARCHY_BEGIN, MOTION_BEGIN));
parseMotion(data.substr(MOTION_BEGIN));
currentFrame = frames[0];
int index = 0;
updateJoint(index, currentFrame, root);
frame_new = false;
}
void PackageTarget::update (uint32_t deltaTime)
{
IEntity::update(deltaTime);
if (_package == nullptr)
return;
if (_joint == nullptr) {
applyJoint(_package);
} else {
updateJoint(deltaTime);
}
}
int processCommand(struct InputParserC * ipc , struct motionStats * st ,char * line , unsigned int words_count , unsigned int startAtFrame)
{
if (InputParser_WordCompareAuto(ipc,0,"MOVE"))
{
int frameNum = InputParser_GetWordInt(ipc,2);
if (frameNum<startAtFrame) { return 0; }
float x = InputParser_GetWordFloat(ipc,3);
float y = InputParser_GetWordFloat(ipc,4);
float z = InputParser_GetWordFloat(ipc,5);
float qW = InputParser_GetWordFloat(ipc,6);
float qX = InputParser_GetWordFloat(ipc,7);
float qY = InputParser_GetWordFloat(ipc,8);
float qZ = InputParser_GetWordFloat(ipc,9);
updatePosition(st,x,y,z,qW,qX,qY,qZ);
}
else
if (InputParser_WordCompareAuto(ipc,0,"POSE"))
{
int frameNum = InputParser_GetWordInt(ipc,2);
if (frameNum<startAtFrame) { return 0; }
//fprintf(stdout,"Found Frame %u \n",InputParser_GetWordInt(ipc,1));
char str[128];
if (InputParser_GetWord(ipc,3,str,128)!=0)
{//Flush next frame
float x = InputParser_GetWordFloat(ipc,4);
float y = InputParser_GetWordFloat(ipc,5);
float z = InputParser_GetWordFloat(ipc,6);
unsigned int where2work=0;
if ( getJointMemoryID(st,str,&where2work) )
{
updateJoint(st,where2work,x,y,z);
}
//fprintf(stdout," %0.2f %0.2f %0.2f \n",x,y,z);
return 1;
}
}
return 0;
}
OSGSegment::OSGSegment(KDL::Segment seg)
{
isSelected_=false;
seg_ = seg;
jointPos_ = 0;
label_ = 0;
visual_ = 0;
post_transform_ = new osg::Group();
toTipOsg_ = new osg::PositionAttitudeTransform();
toTipOsg_->addChild(post_transform_);
toTipOsg_->setName(seg_.getJoint().getName());
post_transform_->setUserData( this );
post_transform_->setUpdateCallback(new OSGSegmentCallback);
toTipOsg_->setUserData(this);
setupTextLabel();
updateJoint();
}
void ofxBvh::updateJoint(int& index, const FrameData& frame_data, ofxBvhJoint *joint)
{
ofVec3f translate;
ofQuaternion rotate;
for (int i = 0; i < joint->channel_type.size(); i++)
{
float v = frame_data[index++];
ofxBvhJoint::CHANNEL t = joint->channel_type[i];
if (t == ofxBvhJoint::X_POSITION)
translate.x = v;
else if (t == ofxBvhJoint::Y_POSITION)
translate.y = v;
else if (t == ofxBvhJoint::Z_POSITION)
translate.z = v;
else if (t == ofxBvhJoint::X_ROTATION)
rotate = ofQuaternion(v, ofVec3f(1, 0, 0)) * rotate;
else if (t == ofxBvhJoint::Y_ROTATION)
rotate = ofQuaternion(v, ofVec3f(0, 1, 0)) * rotate;
else if (t == ofxBvhJoint::Z_ROTATION)
rotate = ofQuaternion(v, ofVec3f(0, 0, 1)) * rotate;
}
translate += joint->initial_offset;
joint->matrix.makeIdentityMatrix();
joint->matrix.glTranslate(translate);
joint->matrix.glRotate(rotate);
joint->global_matrix = joint->matrix;
joint->offset = translate;
if (joint->parent)
{
joint->global_matrix.postMult(joint->parent->global_matrix);
}
for (int i = 0; i < joint->children.size(); i++)
{
updateJoint(index, frame_data, joint->children[i]);
}
}
void ofxBvh::update()
{
frame_new = false;
if (playing && ofGetFrameNum() > 1)
{
int last_index = getFrame();
play_head += ofGetLastFrameTime() * rate;
int index = getFrame();
if (index != last_index)
{
need_update = true;
if (index >= frames.size())
{
currentFrame = frames[0];
if (loop)
play_head = 0;
else
playing = false;
}
else {
currentFrame = frames[index];
}
if (play_head < 0)
play_head = 0;
}
}
if (need_update)
{
need_update = false;
frame_new = true;
int index = 0;
updateJoint(index, currentFrame, root);
}
}
void sbsRobSys::robotAccuracyTest(position pos[5],
char* filename, int noOfStep)
{
vector<double> temp[30];
position step;
moveToPosition(pos[0]);
moveJointTo(pos[0].joint_1, pos[0].joint_2, pos[0].joint_3, pos[0].joint_4, pos[0].joint_5, pos[0].joint_6);
for (int i = 0; i<4; i++)
{
step.joint_1 = (pos[i+1].joint_1-pos[i].joint_1)/noOfStep;
step.joint_2 = (pos[i+1].joint_2-pos[i].joint_2)/noOfStep;
step.joint_3 = (pos[i+1].joint_3-pos[i].joint_3)/noOfStep;
step.joint_4 = (pos[i+1].joint_4-pos[i].joint_4)/noOfStep;
step.joint_5 = (pos[i+1].joint_5-pos[i].joint_5)/noOfStep;
step.joint_6 = (pos[i+1].joint_6-pos[i].joint_6)/noOfStep;
for(int j = 0; j < noOfStep; j++)
{
updateJoint();
updateTCP();
temp[0].push_back(m_robot.m_joint[0]);
temp[1].push_back(m_robot.m_joint[1]);
temp[2].push_back(m_robot.m_joint[2]);
temp[3].push_back(m_robot.m_joint[3]);
temp[4].push_back(m_robot.m_joint[4]);
temp[5].push_back(m_robot.m_joint[5]);
temp[6].push_back(m_robot.m_tcp.o.x);
temp[7].push_back(m_robot.m_tcp.o.y);
temp[8].push_back(m_robot.m_tcp.o.z);
temp[9].push_back(m_robot.m_tcpEuler.x);
temp[10].push_back(m_robot.m_tcpEuler.y);
temp[11].push_back(m_robot.m_tcpEuler.z);
temp[12].push_back(step.joint_1);
temp[13].push_back(step.joint_2);
temp[14].push_back(step.joint_3);
temp[15].push_back(step.joint_4);
temp[16].push_back(step.joint_5);
temp[17].push_back(step.joint_6);
moveJointBy(step.joint_1, step.joint_2, step.joint_3, step.joint_4, step.joint_5, step.joint_6);
boost::this_thread::sleep(boost::posix_time::seconds(5));
updateJoint();
updateTCP();
temp[18].push_back(m_robot.m_joint[0]);
temp[19].push_back(m_robot.m_joint[1]);
temp[20].push_back(m_robot.m_joint[2]);
temp[21].push_back(m_robot.m_joint[3]);
temp[22].push_back(m_robot.m_joint[4]);
temp[23].push_back(m_robot.m_joint[5]);
temp[24].push_back(m_robot.m_tcp.o.x);
temp[25].push_back(m_robot.m_tcp.o.y);
temp[26].push_back(m_robot.m_tcp.o.z);
temp[27].push_back(m_robot.m_tcpEuler.x);
temp[28].push_back(m_robot.m_tcpEuler.y);
temp[29].push_back(m_robot.m_tcpEuler.z);
cout << i << " " << j << endl;
}
}
char* option;
FILE* processed = NULL;
option = "w";
processed = fopen(filename,option);
if (processed == NULL)
{
printf("Error: Opening file\n");
return;
}
for (unsigned int k = 0; k < temp[0].size(); k++)
{
for(int l = 0; l < 30; l++)
{
fprintf(processed, "%f,", temp[l][k]);
}
fprintf(processed, "\n");
}
fclose(processed);
}
void NiutHumanReader::humanJointCallBack(const niut_msgs::niut_HUMAN_LIST::ConstPtr& msg) {
Joint curJoint(10000, 100);
std::vector<int> trackedJoints;
trackedJoints.push_back(0);
trackedJoints.push_back(3);
trackedJoints.push_back(9);
trackedJoints.push_back(15);
trackedJoints.push_back(22);
//msg->filtered_users[i];
for (int i = 0; i < NB_MAX_NIUT; i++) {
if (msg->filtered_users[i].trackedId > -1 && msg->filtered_users[i].date.t_sec != 0) {
unsigned int toasterId = humanIdOffset_ + msg->filtered_users[i].trackedId;
//If this human was not assigned yet, we create it.
if (lastConfig_[toasterId] == NULL) {
Human* tmpHuman = new Human(toasterId);
lastConfig_[toasterId] = tmpHuman;
}
if (fullHuman_) {
for (unsigned int j = 0; j < trackedJoints.size(); j++) {
updateJoint(i, j, curJoint, toasterId, trackedJoints, msg);
lastConfig_[toasterId]->skeleton_[curJoint.getName()]->position_.set<0>(curJoint.position_.get<0>());
lastConfig_[toasterId]->skeleton_[curJoint.getName()]->position_.set<1>(curJoint.position_.get<1>());
lastConfig_[toasterId]->skeleton_[curJoint.getName()]->position_.set<2>(curJoint.position_.get<2>());
lastConfig_[toasterId]->skeleton_[curJoint.getName()]->setTime(msg->filtered_users[i].date.t_sec * pow(10, 9) +
msg->filtered_users[i].date.t_usec);
// We update the human position with the torso (joint 3)
if (j == 3) {
lastConfig_[toasterId]->position_.set<0>(curJoint.position_.get<0>());
lastConfig_[toasterId]->position_.set<1>(curJoint.position_.get<1>());
lastConfig_[toasterId]->position_.set<2>(curJoint.position_.get<2>());
lastConfig_[toasterId]->setTime(msg->filtered_users[i].date.t_sec * pow(10, 9) +
msg->filtered_users[i].date.t_usec);
// TODO; Compute the human orientation
}
}
} else {// not fullHuman_
// We update the human position with the torso (joint 3)
updateJoint(i, 3, curJoint, toasterId, trackedJoints, msg);
lastConfig_[toasterId]->position_.set<0>(curJoint.position_.get<0>());
lastConfig_[toasterId]->position_.set<1>(curJoint.position_.get<1>());
lastConfig_[toasterId]->position_.set<2>(curJoint.position_.get<2>());
lastConfig_[toasterId]->setTime(msg->filtered_users[i].date.t_sec * pow(10, 9) +
msg->filtered_users[i].date.t_usec);
// TODO: compute the human orientation
}
}
}
}
//--------------------------------------------------------------
void ofxFatLine::pushNewVertex(ofVec3f v, ofVec3f p, ofVec3f r1, ofVec3f r2, float maxLength, int index, float cos, bool bFlipped){
ofFloatColor c(colors[index]);
c.a =0;
if (cos == 0){
cos = FLT_EPSILON;
}
r1.normalize();
r2.normalize();
bool bAligned = false;
if (abs(cos) == 1) {
bAligned = true;
p = r1;
}
cos = 1/cos;
midVectors.push_back(p);
if (bFlipped) {
p *=-1;
}
flippepMidVectors.push_back(p);
if (bAligned) {
cout << "vertexAligned" << endl;
pushNewAnchors(v, p*-1, colors[index], weights[index], feathering, true);
pushNewAnchor(v, colors[index]);
pushNewAnchors(v, p, colors[index], weights[index], feathering, false);
if (index != 0) {
if (meshVertices.size() >5) {
for (int i = meshVertices.size()-4; i<meshVertices.size(); i++) {
pushQuadIndices(i);
}
}
}
}else{
if (midVectors.back().dot(r1)>0) {
r1 *=-1;
}
if (midVectors.back().dot(r2)>0) {
r2 *=-1;
}
float midLength = weights[index]*cos;
if (midLength > maxLength) {
midLength = maxLength;
}
if (bFlipped) {
pushNewAnchors(v, r1, colors[index], weights[index], feathering, !bFlipped);
pushNewAnchors(v, midVectors.back(), colors[index], midLength, feathering*cos, bFlipped);
pushNewAnchor(v, colors[index]);
pushNewAnchors(v, r2, colors[index], weights[index], feathering, !bFlipped);
}else{
pushNewAnchors(v, r1, colors[index], weights[index], feathering, !bFlipped);
pushNewAnchors(v, r2, colors[index], weights[index], feathering, !bFlipped);
pushNewAnchor(v, colors[index]);
pushNewAnchors(v, midVectors.back(), colors[index], midLength, feathering*cos, bFlipped);
}
int l = meshVertices.size();
if (l >11) {
if (bFlipped) {
pushQuadIndices(l - 12, l -11, l - 5, l - 4);
pushQuadIndices(l - 11, l -10, l - 4, l - 3);
pushQuadIndices(l - 10, l - 9, l - 3, l - 7);
pushQuadIndices(l - 9, l - 8, l - 7, l - 6);
}else{
pushQuadIndices(l - 12, l -11, l - 7, l - 6);
pushQuadIndices(l - 11, l -10, l - 6, l - 3);
pushQuadIndices(l - 10, l - 9, l - 3, l - 2);
pushQuadIndices(l - 9, l - 8, l - 2, l - 1);
}
}
updateJoint(index,bFlipped);
}
/*
ofVec3f pp = p * 50 * cos;
ofVec3f pa = pp + p * 2 * cos;
meshVertices.push_back(pa + v);
meshColors.push_back(c);
meshVertices.push_back(pp + v);
meshColors.push_back(colors[index]);
meshVertices.push_back(v);
meshColors.push_back(colors[index]);
meshVertices.push_back(v - pp);
meshColors.push_back(colors[index]);
meshVertices.push_back(v - pa);
meshColors.push_back(c);
if (meshVertices.size() >5) {
for (int i = meshVertices.size()-4; i<meshVertices.size(); i++) {
pushQuadIndices(i);
}
}
//*/
}
void ofxBvh::update(const vector<float>& data)
{
int index = 0;
updateJoint(index, data, root);
}