void
Polygon::initialize(QXmlStreamReader& xml, UnitsType units)
{
while (!xml.atEnd() && !xml.hasError()) {
xml.readNext();
if (xml.isStartElement()) {
if (xml.name() == "PolyBegin") {
m_polyBegin.rx() = toInch(
getDoubleAttribute(xml, "PolyBegin", "x"), units);
m_polyBegin.ry() = toInch(
getDoubleAttribute(xml, "PolyBegin", "y"), units);
}
else if (isSubstitutionGroupPolyStep(xml.name())) {
PolyStep *p = PolyStepFactory().create(xml.name());
p->initialize(xml, units);
m_polySteps.append(p);
}
}
else if (isEndElementWithName(xml, "Polygon") || // </Polygon> the end
isEndElementWithName(xml, "Cutout")) { // another possible name
if (!isClosedShape()) {
throw new InvalidElementError("Polygon");
}
return;
}
}
}
void
Features::initialize(QXmlStreamReader& xml, UnitsType units)
{
m_xform = NULL;
m_feature = NULL;
while (!xml.atEnd() && !xml.hasError()) {
xml.readNext();
if (xml.isStartElement()) {
if (xml.name() == "Xform") {
m_xform = new Xform();
m_xform->initialize(xml);
}
else if (xml.name() == "Location") {
m_location = QPointF(
toInch(getDoubleAttribute(xml, "Location", "x"), units),
toInch(getDoubleAttribute(xml, "Location", "y"), units));
}
else if (isSubstitutionGroupFeature(xml.name())) {
m_feature = FeatureFactory().create(xml.name());
m_feature->initialize(xml, units);
}
}
else if (isEndElementWithName(xml, "Features")) { // </Features>
if (m_feature == NULL) {
throw new InvalidElementError("Features");
}
return;
}
}
}
void
Target::initialize(QXmlStreamReader& xml, UnitsType units)
{
m_xform = NULL;
while (!xml.atEnd() && !xml.hasError()) {
xml.readNext();
if (xml.isStartElement()) {
if (xml.name() == "Xform") {
m_xform = new Xform();
m_xform->initialize(xml);
}
else if (xml.name() == "Location") {
m_location = QPointF(getDoubleAttribute(xml, "Location", "x"),
getDoubleAttribute(xml, "Location", "y"));
}
else if (isSubstitutionGroupStandardShape(xml.name())) {
m_standardShape = StandardShapeFactory().create(xml.name());
m_standardShape->initialize(xml, units);
}
}
else if (isEndElementWithName(xml, "Target")) { // </Target>
return;
}
}
}
void
StepRepeat::initialize(QXmlStreamReader& xml)
{
m_stepRef = getStringAttribute(xml, "StepRepeat", "stepRef");
m_x = getDoubleAttribute(xml, "StepRepeat", "x");
m_y = getDoubleAttribute(xml, "StepRepeat", "y");
m_nx = getNonNegativeIntAttribute(xml, "StepRepeat", "nx");
m_ny = getNonNegativeIntAttribute(xml, "StepRepeat", "ny");
m_dx = getNonNegativeDoubleAttribute(xml, "StepRepeat", "dx");
m_dy = getNonNegativeDoubleAttribute(xml, "StepRepeat", "dy");
m_angle = getDoubleAttribute(xml, "StepRepeat", "angle");
m_mirror = getBoolAttribute(xml, "mirror");
}
/*===========================================================================*/
static int parseState(xmlDocPtr doc, xmlNodePtr cur, ghmm_xmlfile* f, int * inDegree, int * outDegree, int modelNo) {
#define CUR_PROC "parseState"
int i, error, order=0, state=-1442, fixed=-985, tied=-9354, M, aprox, label;
int curX=0, curY=0;
double pi, prior;
double *emissions = NULL;
char *desc = NULL;
char *s = NULL, *estr;
int rev, stateFixed=1;
ghmm_cstate *newcstate;
ghmm_c_emission *emission;
xmlNodePtr elem, child, multichild;
state = getIntAttribute(cur, "id", &error);
pi = getDoubleAttribute(cur, "initial", &error);
if (error) {
estr = ighmm_mprintf(NULL, 0, "can't read required intial probability for"
"state %d", state);
GHMM_LOG(LERROR, estr);
goto STOP;
} else
desc = xmlGetProp(cur, BAD_CAST "desc");
elem = cur->children;
while (elem!=NULL) {
/* ======== silent state ============================================== */
if ((!xmlStrcmp(elem->name, BAD_CAST "silent"))) {
switch (f->modelType & PTR_TYPE_MASK) {
case (GHMM_kDiscreteHMM):
f->model.d[modelNo]->silent[state] = 1;
break;
case (GHMM_kDiscreteHMM+GHMM_kTransitionClasses):
f->model.ds[modelNo]->silent[state] = 1;
break;
case (GHMM_kDiscreteHMM+GHMM_kPairHMM):
case (GHMM_kDiscreteHMM+GHMM_kPairHMM+GHMM_kTransitionClasses):
f->model.dp[modelNo]->silent[state] = 1;
break;
default:
GHMM_LOG(LERROR, "invalid modelType");
goto STOP;
}
}
/* ======== discrete state (possible higher order) ==================== */
if ((!xmlStrcmp(elem->name, BAD_CAST "discrete"))) {
assert((f->modelType & GHMM_kDiscreteHMM) && ((f->modelType & GHMM_kPairHMM) == 0));
/* fixed is a propety of the distribution and optional */
fixed = getIntAttribute(elem, "fixed", &error);
if (error)
fixed = 0;
/* order is optional for discrete */
if (f->modelType & GHMM_kHigherOrderEmissions) {
order = getIntAttribute(elem, "order", &error);
if (error)
order = 0;
}
rev = getIntAttribute(cur, "rev", &error);
if (error)
rev = 0;
/* parsing emission probabilities */
s = (char *)xmlNodeGetContent(elem);
switch (f->modelType & PTR_TYPE_MASK) {
case (GHMM_kDiscreteHMM):
f->model.d[modelNo]->s[state].desc = desc;
f->model.d[modelNo]->s[state].pi = pi;
f->model.d[modelNo]->s[state].fix = fixed;
if (f->modelType & GHMM_kHigherOrderEmissions) {
f->model.d[modelNo]->order[state] = order;
if (f->model.d[modelNo]->maxorder < order) {
f->model.d[modelNo]->maxorder = order;
estr = ighmm_mprintf(NULL, 0, "Updated maxorder to %d\n",
f->model.d[modelNo]->maxorder);
GHMM_LOG(LDEBUG, estr);
m_free(estr);
}
}
ARRAY_MALLOC(emissions, pow(f->model.d[modelNo]->M, order+1));
parseCSVList(s, pow(f->model.d[modelNo]->M, order+1), emissions, rev);
free(f->model.d[modelNo]->s[state].b);
f->model.d[modelNo]->s[state].b = emissions;
break;
case (GHMM_kDiscreteHMM+GHMM_kTransitionClasses):
f->model.ds[modelNo]->s[state].desc = desc;
f->model.ds[modelNo]->s[state].pi = pi;
f->model.ds[modelNo]->s[state].fix = fixed;
if (f->modelType & GHMM_kHigherOrderEmissions)
f->model.ds[modelNo]->order[state] = order;
ARRAY_MALLOC(emissions, pow(f->model.ds[modelNo]->M, order+1));
parseCSVList(s, pow(f->model.ds[modelNo]->M, order+1), emissions, rev);
f->model.ds[modelNo]->s[state].b = emissions;
break;
default:
GHMM_LOG(LERROR, "invalid modelType");
goto STOP;
}
m_free(s);
}
/* ======== continuous state ========================================== */
if ((!xmlStrcmp(elem->name, BAD_CAST "mixture"))) {
assert(f->modelType & GHMM_kContinuousHMM);
M = 0;
child = elem->children;
while (child != NULL) {
if ((!xmlStrcmp(child->name, BAD_CAST "normal")) ||
(!xmlStrcmp(child->name, BAD_CAST "normalLeftTail")) ||
(!xmlStrcmp(child->name, BAD_CAST "normalRightTail")) ||
(!xmlStrcmp(child->name, BAD_CAST "multinormal")) ||
(!xmlStrcmp(child->name, BAD_CAST "uniform"))){
M ++;
}
child = child->next;
}
ghmm_cstate_alloc(f->model.c[modelNo]->s + state, M, inDegree[state], outDegree[state], f->model.c[modelNo]->cos);
newcstate = f->model.c[modelNo]->s + state;
newcstate->desc = desc;
newcstate->M = M;
newcstate->pi = pi;
if( f->model.c[modelNo]->M < M)
f->model.c[modelNo]->M = M;
child = elem->children;
i = 0;
while (child != NULL) {
emission = newcstate->e+i;
/* common attributes */
if ((!xmlStrcmp(child->name, BAD_CAST "normal")) ||
(!xmlStrcmp(child->name, BAD_CAST "normalLeftTail")) ||
(!xmlStrcmp(child->name, BAD_CAST "normalRightTail")) ||
(!xmlStrcmp(child->name, BAD_CAST "multinormal")) ||
(!xmlStrcmp(child->name, BAD_CAST "uniform"))){
fixed = getIntAttribute(child, "fixed", &error);
if (error)
fixed = 0;
stateFixed = fixed && stateFixed;
/* allocate emission */
emission->fixed = fixed;
prior = getDoubleAttribute(child, "prior", &error);
if (error)
prior = 1.0;
newcstate->c[i] = prior;
}
/* child is not a density, continue with the next child */
else {
child = child->next;
continue;
}
/* density type dependent attributes */
if ((!xmlStrcmp(child->name, BAD_CAST "normal"))) {
emission->mean.val = getDoubleAttribute(child, "mean", &error);
emission->variance.val = getDoubleAttribute(child, "variance", &error);
/* should the normal distribution be approximated? */
aprox = getIntAttribute(child, "approx", &error);
if (error)
aprox = 0;
emission->type = aprox ? normal_approx : normal;
emission->dimension = 1;
if (f->model.c[modelNo]->dim > 1) {
GHMM_LOG(LERROR, "All emissions must have same dimension.");
goto STOP;
}
}
if ((!xmlStrcmp(child->name, BAD_CAST "normalLeftTail"))) {
emission->mean.val = getDoubleAttribute(child, "mean", &error);
emission->variance.val = getDoubleAttribute(child, "variance", &error);
emission->min = getDoubleAttribute(child, "max", &error);
emission->type = normal_left;
emission->dimension = 1;
if (f->model.c[modelNo]->dim > 1) {
GHMM_LOG(LERROR, "All emissions must have same dimension.");
goto STOP;
}
}
if ((!xmlStrcmp(child->name, BAD_CAST "normalRightTail"))) {
emission->mean.val = getDoubleAttribute(child, "mean", &error);
emission->variance.val = getDoubleAttribute(child, "variance", &error);
emission->max = getDoubleAttribute(child, "min", &error);
emission->type = normal_right;
emission->dimension = 1;
if (f->model.c[modelNo]->dim > 1) {
GHMM_LOG(LERROR, "All emissions must have same dimension.");
goto STOP;
}
}
if ((!xmlStrcmp(child->name, BAD_CAST "uniform"))) {
emission->max = getDoubleAttribute(child, "max", &error);
emission->min = getDoubleAttribute(child, "min", &error);
emission->type = uniform;
emission->dimension = 1;
if (f->model.c[modelNo]->dim > 1) {
GHMM_LOG(LERROR, "All emissions must have same dimension.");
goto STOP;
}
}
if ((!xmlStrcmp(child->name, BAD_CAST "multinormal"))) {
emission->type = multinormal;
emission->dimension = getIntAttribute(child, "dimension", &error);
/* check that all emissions in all states have same dimension or
set when first emission is read*/
if (f->model.c[modelNo]->dim <= 1)
f->model.c[modelNo]->dim = emission->dimension;
else if (f->model.c[modelNo]->dim != emission->dimension) {
GHMM_LOG(LERROR, "All emissions must have same dimension.");
goto STOP;
}
if (0 != ghmm_c_emission_alloc(emission, emission->dimension)) {
GHMM_LOG(LERROR, "Can not allocate multinormal emission.");
goto STOP;
}
multichild = child->children;
while (multichild != NULL) {
if ((!xmlStrcmp(multichild->name, BAD_CAST "mean"))) {
s = (char *)xmlNodeGetContent(multichild);
if (-1 == parseCSVList(s, emission->dimension, emission->mean.vec, 0)) {
GHMM_LOG(LERROR, "Can not parse mean CSV list.");
goto STOP;
}
}
if ((!xmlStrcmp(multichild->name, BAD_CAST "variance"))) {
s = (char *)xmlNodeGetContent(multichild);
if (-1 == parseCSVList(s, emission->dimension * emission->dimension,
emission->variance.mat, 0)) {
GHMM_LOG(LERROR, "Can not parse variance CSV list.");
goto STOP;
}
if (0 != ighmm_invert_det(emission->sigmainv, &emission->det,
emission->dimension, emission->variance.mat))
{
GHMM_LOG(LERROR, "Can not calculate inverse of covariance matrix.");
goto STOP;
}
if (0 != ighmm_cholesky_decomposition(emission->sigmacd,
emission->dimension,
emission->variance.mat))
{
GHMM_LOG(LERROR, "Can not calculate cholesky decomposition of covariance matrix.");
goto STOP;
}
}
multichild = multichild->next;
}
}
i++;
child = child->next;
}
newcstate->fix = stateFixed;
}
/* ======== pair hmm state ============================================ */
if ((!xmlStrcmp(elem->name, BAD_CAST "pair"))) {
}
/* -------- background name ------------------------------------------ */
if ((!xmlStrcmp(elem->name, BAD_CAST "backgroundKey"))) {
assert(f->modelType & GHMM_kBackgroundDistributions);
s = (char *)xmlNodeGetContent(elem);
for (i=0; i<f->model.d[modelNo]->bp->n; i++) {
if (0 == strcmp(s, f->model.d[modelNo]->bp->name[i])) {
if (order != f->model.d[modelNo]->bp->order[i]) {
estr = ighmm_mprintf(NULL, 0, "order of background %s and state %d"
" does not match",
f->model.d[modelNo]->bp->name[i], state);
GHMM_LOG(LERROR, estr);
m_free(estr);
goto STOP;
} else {
f->model.d[modelNo]->background_id[state] = i;
break;
}
}
}
if (i == f->model.d[modelNo]->bp->n) {
estr = ighmm_mprintf(NULL, 0, "can't find background with name %s in"
" state %d", s, state);
GHMM_LOG(LERROR, estr);
m_free(estr);
goto STOP;
}
m_free(s);
}
/* -------- tied to --------------------------------------------------- */
if ((!xmlStrcmp(elem->name, BAD_CAST "class"))) {
assert(f->modelType & GHMM_kLabeledStates);
s = (char *)xmlNodeGetContent(elem);
label = atoi(s);
m_free(s);
if ((f->modelType & PTR_TYPE_MASK) == GHMM_kDiscreteHMM) {
if (f->model.d[modelNo]->label_alphabet->size > label)
f->model.d[modelNo]->label[state] = label;
else
GHMM_LOG(LWARN, "Invalid label");
}
}
/* -------- tied to --------------------------------------------------- */
if ((!xmlStrcmp(elem->name, BAD_CAST "tiedTo"))) {
assert(f->modelType & GHMM_kTiedEmissions);
s = (char *)xmlNodeGetContent(elem);
tied = atoi(s);
if (state>=tied) {
f->model.d[modelNo]->tied_to[state] = tied;
if (f->model.d[modelNo]->tied_to[tied] != tied) {
estr = ighmm_mprintf(NULL, 0, "state %d not tied to tie group leader", state);
GHMM_LOG(LERROR, estr);
m_free(estr);
goto STOP;
}
} else {
estr = ighmm_mprintf(NULL, 0, "state %d tiedTo (%d) is invalid", state, tied);
GHMM_LOG(LERROR, estr);
m_free(estr);
goto STOP;
}
m_free(s);
}
/* -------- position for graphical editing ---------------------------- */
if ((!xmlStrcmp(elem->name, BAD_CAST "position"))) {
curX = getIntAttribute(elem, "x", &error);
if (error)
GHMM_LOG(LWARN, "failed to read x position");
curY = getIntAttribute(elem, "y", &error);
if (error)
GHMM_LOG(LWARN, "failed to read y position");
switch (f->modelType & PTR_TYPE_MASK) {
case GHMM_kDiscreteHMM:
f->model.d[modelNo]->s[state].xPosition = curX;
f->model.d[modelNo]->s[state].yPosition = curY;
break;
case GHMM_kDiscreteHMM+GHMM_kTransitionClasses:
f->model.ds[modelNo]->s[state].xPosition = curX;
f->model.ds[modelNo]->s[state].yPosition = curY;
break;
case GHMM_kDiscreteHMM+GHMM_kPairHMM:
case GHMM_kDiscreteHMM+GHMM_kPairHMM+GHMM_kTransitionClasses:
f->model.dp[modelNo]->s[state].xPosition = curX;
f->model.dp[modelNo]->s[state].yPosition = curY;
break;
case GHMM_kContinuousHMM:
case GHMM_kContinuousHMM+GHMM_kTransitionClasses:
case (GHMM_kContinuousHMM+GHMM_kMultivariate):
case (GHMM_kContinuousHMM+GHMM_kMultivariate+GHMM_kTransitionClasses):
f->model.c[modelNo]->s[state].xPosition = curX;
f->model.c[modelNo]->s[state].yPosition = curY;
break;
default:
GHMM_LOG(LERROR, "invalid modelType");
goto STOP;
}
}
elem = elem->next;
}
return 0;
STOP:
m_free(s);
m_free(desc);
m_free(emissions)
return -1;
#undef CUR_PROC
}
void SBCollisionManager::start()
{
#ifndef SB_NO_ODE_PHYSICS
_singleChrCapsuleMode = getBoolAttribute("singleChrCapsuleMode");
float jointBVLenRadRatio = (float)(getDoubleAttribute("jointBVLenRadRatio"));
SR_CLIP(jointBVLenRadRatio, 0.001f, 1000.0f);
_jointBVLenRadRatio = jointBVLenRadRatio;
SBScene* scene = SmartBody::SBScene::getScene();
SBSteerManager* steerManager = scene->getSteerManager();
_characterRadius = (float) steerManager->getDoubleAttribute("initialConditions.radius");
double sceneScale = scene->getDoubleAttribute("scale");
if (sceneScale != 0.0)
{
_characterRadius *= (float) (1.0 / sceneScale);
}
_positions.clear();
_velocities.clear();
if (!collisionSpace)
{
collisionSpace = new ODECollisionSpace();
}
const std::vector<std::string>& characterNames = scene->getCharacterNames();
for (std::vector<std::string>::const_iterator iter = characterNames.begin();
iter != characterNames.end();
iter++)
{
_positions.insert(std::pair<std::string, SrVec>((*iter), SrVec()));
_velocities.insert(std::pair<std::string, SrVec>((*iter), SrVec()));
SBCharacter* character = scene->getCharacter(*iter);
if (!character->getGeomObject() || character->getGeomObject()->geomType() == "null") // no collision geometry setup for the character
{
if(_singleChrCapsuleMode)
{
//SBGeomObject* obj = new SBGeomCapsule()
SrBox bbox = character->getBoundingBox();
float yoffset = bbox.getMinimum().y - character->get_world_offset().get_translation().y;
SrVec size = SrVec(0,_characterRadius,0);
SBGeomObject* obj = createCollisionObject(character->getGeomObjectName(),"capsule",size,SrVec(0,yoffset,0),SrVec(0,yoffset+character->getHeight(),0));
obj->attachToObj(character);
addObjectToCollisionSpace(character->getGeomObjectName());
//new SBGeomCapsule(SrVec(0,yoffset,0),SrVec(0,yoffset+character->getHeight(),0),_characterRadius);
//character->setGeomObject(obj);
//collisionSpace->addCollisionObjects(obj);
}
else // create collision capsules based on skel bones
{
LOG(character->getName().c_str());
SkSkeleton* sk = character->getSkeleton();
const std::vector<SkJoint*>& origJnts = sk->joints();
sk->update_global_matrices();
std::vector<SkJoint*> jnt_excld_list;
for(unsigned int i=0; i<origJnts.size(); i++)
{
SkJoint* j = origJnts[i];
SrString jname(j->jointName().c_str());
if(jname.search("world_offset")>=0) { jnt_excld_list.push_back(j); continue; } // skip world_offset
if(jname.search("face")>=0) { jnt_excld_list.push_back(j); continue; }
if(jname.search("brow")>=0) { jnt_excld_list.push_back(j); continue; }
if(jname.search("eye")>=0) { jnt_excld_list.push_back(j); continue; }
if(jname.search("nose")>=0) { jnt_excld_list.push_back(j); continue; }
if(jname.search("lid")>=0) { jnt_excld_list.push_back(j); continue; }
if(jname.search("jaw")>=0) { jnt_excld_list.push_back(j); continue; }
if(jname.search("tongue")>=0) { jnt_excld_list.push_back(j); continue; }
if(jname.search("lip")>=0) { jnt_excld_list.push_back(j); continue; }
if(jname.search("cheek")>=0) { jnt_excld_list.push_back(j); continue; }
if(jname.search("finger")>=0) { jnt_excld_list.push_back(j); continue; }
if(jname.search("thumb")>=0) { jnt_excld_list.push_back(j); continue; }
if(jname.search("index")>=0) { jnt_excld_list.push_back(j); continue; }
if(jname.search("middle")>=0) { jnt_excld_list.push_back(j); continue; }
if(jname.search("pinky")>=0) { jnt_excld_list.push_back(j); continue; }
if(jname.search("ring")>=0) { jnt_excld_list.push_back(j); continue; }
}
std::string chrName = character->getGeomObjectName();
float chrHeight = character->getHeight();
for(unsigned int i=0; i<origJnts.size(); i++)
{
SkJoint* j = origJnts[i];
if(isJointExcluded(j, jnt_excld_list)) continue;
SrString jname(j->jointName().c_str());
for(int k=0; k<j->num_children(); k++)
{
SkJoint* j_ch = j->child(k);
if(isJointExcluded(j_ch, jnt_excld_list)) continue;
const SrVec& offset = j_ch->offset();
float offset_len = offset.norm();
float radius = offset_len / jointBVLenRadRatio;
if(offset_len < 0.03*chrHeight) continue; // skip short bones
std::string colObjName = chrName + ":" + j->jointName();
if(k>0) colObjName = colObjName + ":" + boost::lexical_cast<std::string>(k);
SBGeomObject* obj = createCollisionObject(colObjName,"capsule",SrVec(0, radius, 0),SrVec::null,offset);
LOG("SBColMan: col primitive added: %s, len: %f, radius: %f", colObjName.c_str(), offset_len, radius);
obj->attachToObj(dynamic_cast<SBTransformObjInterface*>(j));
addObjectToCollisionSpace(colObjName);
}
}
}
}
}
const std::vector<std::string>& pawnNames = scene->getPawnNames();
for (std::vector<std::string>::const_iterator iter = pawnNames.begin();
iter != pawnNames.end(); iter++)
{
SBPawn* pawn = scene->getPawn(*iter);
if(pawn->getGeomObject()->geomType() != "null")
{
//SBGeomObject* obj = pawn->getGeomObject();
SBGeomObject* obj = createCollisionObject(pawn->getGeomObjectName(),pawn->getGeomObject()->geomType(),pawn->getGeomObject()->getGeomSize(),SrVec::null,SrVec::null);
obj->attachToObj(pawn);
addObjectToCollisionSpace(pawn->getGeomObjectName());
}
}
#endif
}
