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;
}
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 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 AbstractMultiSeq::writeSeqPartLabels(YAMLWriter& writer)
{
writer.putKey("partLabels");
writer.startFlowStyleListing();
int n = getNumParts();
for(int i=0; i < n; ++i){
writer.putDoubleQuotedString(partLabel(i));
}
writer.endListing();
return true;
}
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();
});
}
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();
}
static inline void writeAffine3(YAMLWriter& writer, const Affine3& value)
{
writer.startFlowStyleListing();
writer.putScalar(value.translation().x());
writer.putScalar(value.translation().y());
writer.putScalar(value.translation().z());
Vector3 rpy(rpyFromRot(value.linear()));
writer.putScalar(rpy[0]);
writer.putScalar(rpy[1]);
writer.putScalar(rpy[2]);
writer.endListing();
}
static inline void writeSE3(YAMLWriter& writer, const SE3& value)
{
writer.startFlowStyleListing();
const Vector3& p = value.translation();
writer.putScalar(p.x());
writer.putScalar(p.y());
writer.putScalar(p.z());
const Quat& q = value.rotation();
writer.putScalar(q.w());
writer.putScalar(q.x());
writer.putScalar(q.y());
writer.putScalar(q.z());
writer.endListing();
}
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();
});
}