bool MultiSE3Seq::doReadSeq(const Mapping& archive)
{
if(BaseSeqType::doReadSeq(archive)){
const string& type = archive["type"].toString();
if(type == seqType() || type == "MultiSe3Seq" || type == "MultiAffine3Seq"){
string formatString = archive["format"].toString();
const Listing& values = *archive.findListing("frames");
if(values.isValid()){
const int nParts = archive["numParts"].toInt();
const int nFrames = values.size();
setDimension(nFrames, nParts);
if(formatString == "XYZQWQXQYQZ"){
readPosQuatSeq(nParts, nFrames, values, true);
} else if(formatString == "XYZQXQYQZQW"){
readPosQuatSeq(nParts, nFrames, values, false);
} else if(formatString == "XYZRPY"){
readPosRpySeq(nParts, nFrames, values);
} else {
addSeqMessage(
str(format(("Unknown format \"%1%\" cannot be loaded into MultiSE3Seq."))
% formatString));
return false;
}
}
}
return true;
}
return false;
}
bool MultiAffine3Seq::doReadSeq(const Mapping& archive)
{
if(BaseSeqType::doReadSeq(archive)){
const string& type = archive["type"].toString();
if((type == seqType() || type == "MultiSe3Seq") && (archive["format"].toString() == "XYZRPY")){
const Listing& values = *archive.findListing("frames");
if(values.isValid()){
const int nParts = archive["numParts"].toInt();
const int nFrames = values.size();
setDimension(nFrames, nParts);
for(int i=0; i < nFrames; ++i){
const Listing& frameNode = *values[i].toListing();
Frame f = frame(i);
const int n = std::min(frameNode.size(), nParts);
for(int j=0; j < n; ++j){
readAffine3(*frameNode[j].toListing(), f[j]);
}
}
return true;
}
}
}
return false;
}
bool PoseSeq::restore(const Mapping& archive, const BodyPtr body)
{
setTargetBodyName(archive.get("targetBody", body->name()));
const Listing& refs = *archive.findListing("refs");
if(!refs.isValid()){
return false;
}
PoseSeq::iterator current = begin();
for(int i=0; i < refs.size(); ++i){
const Mapping& ref = *refs[i].toMapping();
bool isInserted = false;
double time = ref["time"].toDouble();
const ValueNode& referred = ref["refer"];
if(referred.isScalar()){
const string& name = referred;
if(!name.empty()){
current = insert(current, time, name);
isInserted = true;
}
} else if(referred.isMapping()){
const Mapping& mReferred = *referred.toMapping();
const string& type = mReferred["type"];
PoseUnitPtr poseUnit;
if(type == "Pose"){
poseUnit = new Pose();
} else if(type == "PronunSymbol"){
poseUnit = new PronunSymbol();
}
/*
else if(type == "PoseSeq"){
poseUnit = createLocalPoseSeq();
}
*/
if(poseUnit && poseUnit->restore(mReferred, body)){
poseUnit->name_ = mReferred["name"];
current = insert(current, time, poseUnit);
isInserted = true;
}
}
if(isInserted){
current->setMaxTransitionTime(ref.get("maxTransitionTime", 0.0));
}
}
return true;
}
std::vector<std::string> AbstractMultiSeq::readSeqPartLabels(const Mapping& archive)
{
vector<string> labelStrings;
const Listing& labels = *archive.findListing("partLabels");
if(labels.isValid()){
for(int i=0; i < labels.size(); ++i){
labelStrings.push_back(labels[i].toString());
}
}
return labelStrings;
}
bool AbstractMultiSeq::readSeqPartLabels(const Mapping& archive, SetPartLabelFunction setPartLabel)
{
const Listing& labels = *archive.findListing("partLabels");
if(labels.isValid()){
for(int i=0; i < labels.size(); ++i){
setPartLabel(labels[i].toString(), i);
}
return true;
} else {
addSeqMessage("\"partLabels\" is not valid.");
}
return false;
}
bool Vector3Seq::doReadSeq(const Mapping& archive)
{
if(BaseSeqType::doReadSeq(archive)){
if(archive["type"].toString() == seqType()){
const Listing& frames = *archive.findListing("frames");
if(!frames.isValid()){
addSeqMessage(_("Valid \"frames\" field of Vector3Seq is not found."));
} else {
const int n = frames.size();
setNumFrames(n);
for(int i=0; i < n; ++i){
const Listing& frame = *frames[i].toListing();
Vector3& v = (*this)[i];
for(int j=0; j < 3; ++j){
v[j] = frame[j].toDouble();
}
}
return true;
}
}
}
return false;
}
bool PoseSeqItem::convertSub(BodyPtr orgBody, const Mapping& convInfo)
{
bool converted = false;
const Listing& jointMap = *convInfo.findListing("jointMap");
const Mapping& linkMap = *convInfo.findMapping("linkMap");
BodyPtr body = ownerBodyItem->body();
for(PoseSeq::iterator p = seq->begin(); p != seq->end(); ++p){
PosePtr pose = p->get<Pose>();
if(pose){
bool modified = false;
seq->beginPoseModification(p);
PosePtr orgPose = dynamic_cast<Pose*>(pose->duplicate());
if(jointMap.isValid()){
modified = true;
pose->setNumJoints(0);
int n = orgPose->numJoints();
for(int i=0; i < n; ++i){
if(orgPose->isJointValid(i)){
if(i < jointMap.size()){
int newJointId = jointMap[i].toInt();
if(newJointId >= 0){
pose->setJointPosition(newJointId, orgPose->jointPosition(i));
pose->setJointStationaryPoint(newJointId, orgPose->isJointStationaryPoint(i));
}
}
}
}
}
if(linkMap.isValid()){
modified = true;
pose->clearIkLinks();
int baseLinkIndex = -1;
for(Pose::LinkInfoMap::const_iterator q = orgPose->ikLinkBegin(); q != orgPose->ikLinkEnd(); ++q){
Link* orgLink = orgBody->link(q->first);
string linkName;
ValueNode* linkNameNode = linkMap.find(orgLink->name());
if(linkNameNode->isValid()){
linkName = linkNameNode->toString();
} else {
linkName = orgLink->name();
}
Link* link = body->link(linkName);
if(link){
const Pose::LinkInfo& orgLinkInfo = q->second;
Pose::LinkInfo* linkInfo = pose->addIkLink(link->index());
linkInfo->p = orgLinkInfo.p;
linkInfo->R = orgLinkInfo.R;
linkInfo->setStationaryPoint(orgLinkInfo.isStationaryPoint());
if(orgLinkInfo.isTouching()){
linkInfo->setTouching(orgLinkInfo.partingDirection());
}
linkInfo->setSlave(orgLinkInfo.isSlave());
if(orgLinkInfo.isBaseLink()){
baseLinkIndex = link->index();
}
}
}
if(baseLinkIndex >= 0){
pose->setBaseLink(baseLinkIndex);
}
}
if(modified){
seq->endPoseModification(p);
converted = true;
}
}
}
return converted;
}
