bool BodyMotion::write(YAMLWriter& writer) { writer.startListing(); if(jointPosSeq_->numFrames() > 0){ if(!jointPosSeq_->writeSeq(writer)){ addSeqMessage(jointPosSeq_->seqMessage()); return false; } } if(linkPosSeq_->numFrames() > 0){ if(!linkPosSeq_->writeSeq(writer)){ addSeqMessage(linkPosSeq_->seqMessage()); return false; } } for(ExtraSeqMap::iterator p = extraSeqs.begin(); p != extraSeqs.end(); ++p){ AbstractSeqPtr& seq = p->second; if(!seq->writeSeq(writer)){ addSeqMessage(seq->seqMessage()); return false; } } writer.endListing(); return true; }
bool MultiAffine3Seq::doWriteSeq(YAMLWriter& writer) { if(BaseSeqType::doWriteSeq(writer)){ writer.putKeyValue("format", "XYZRPY"); writer.putKey("frames"); writer.startListing(); const int m = numParts(); const int n = numFrames(); for(int i=0; i < n; ++i){ Frame f = frame(i); writer.startFlowStyleListing(); for(int j=0; j < m; ++j){ writeAffine3(writer, f[j]); } writer.endListing(); } writer.endListing(); return true; } return false; }
void CollisionSeq::writeCollsionData(YAMLWriter& writer, std::shared_ptr<const CollisionLinkPairList> ptr) { writer.startMapping(); writer.putKey("LinkPairs"); writer.startListing(); for(auto it = ptr->begin(); it != ptr->end(); ++it){ CollisionLinkPairPtr linkPair = *it; writer.startMapping(); writer.putKeyValue("body0",linkPair->body[0]->name()); writer.putKeyValue("link0",linkPair->link[0]->name()); writer.putKeyValue("body1",linkPair->body[1]->name()); writer.putKeyValue("link1",linkPair->link[1]->name()); int numCollisions = linkPair->collisions.size(); writer.putKey("Collisions"); writer.startListing(); for(int j=0; j<numCollisions; j++){ Collision& collision = linkPair->collisions[j]; writer.startFlowStyleListing(); const Vector3& point = collision.point; writer.putScalar(point.x()); writer.putScalar(point.y()); writer.putScalar(point.z()); const Vector3& normal = collision.normal; writer.putScalar(normal.x()); writer.putScalar(normal.y()); writer.putScalar(normal.z()); writer.putScalar(collision.depth); writer.endListing(); } writer.endListing(); writer.endMapping(); } writer.endListing(); writer.endMapping(); }
bool CollisionSeq::doWriteSeq(YAMLWriter& writer, std::function<void()> additionalPartCallback) { return BaseSeqType::doWriteSeq( writer, [&](){ writer.putKeyValue("format", "PxPyPzNxNyNzD"); if(additionalPartCallback) additionalPartCallback(); writer.putKey("frames"); writer.startListing(); const int n = numFrames(); for(int i=0; i < n; ++i){ Frame f = frame(i); writeCollsionData(writer, f[0]); } writer.endListing(); }); }
bool Vector3Seq::doWriteSeq(YAMLWriter& writer) { if(BaseSeqType::doWriteSeq(writer)){ writer.putKey("frames"); writer.startListing(); const int n = numFrames(); for(int i=0; i < n; ++i){ writer.startFlowStyleListing(); const Vector3& v = (*this)[i]; for(int j=0; j < 3; ++j){ writer.putScalar(v[j]); } writer.endListing(); } writer.endListing(); return true; } return false; }
bool MultiValueSeq::doWriteSeq(YAMLWriter& writer, std::function<void()> additionalPartCallback) { return BaseSeqType::doWriteSeq( writer, [&](){ if(additionalPartCallback) additionalPartCallback(); writer.putKey("frames"); writer.startListing(); const int n = numFrames(); const int m = numParts(); for(int i=0; i < n; ++i){ writer.startFlowStyleListing(); Frame v = frame(i); for(int j=0; j < m; ++j){ writer.putScalar(v[j]); } writer.endListing(); } writer.endListing(); }); }