void SimObjBase::setAxisAndAngle(double ax, double ay, double az, double angle)
{
if (dynamics()) { return; }
Rotation r;
r.setAxisAndAngle(ax, ay, az, angle);
const dReal *q = r.q();
setQ(q);
}
/*!
* @brief It rotates for the specification of the relative angle.
* @param[in] x-axis rotation weather(i of quaternion complex part)
* @param[in] y-axis rotation weather(j of quaternion complex part)
* @param[in] z-axis rotation weather(k of quaternion complex part)
* @param[in] flag for ansolute / relational (1.0=absolute, else=relational)
*/
void SimObjBase::setAxisAndAngle(double ax, double ay, double az, double angle, double direct)
{
// The angle is used now at the relative angle specification.
if (dynamics()) { return; }
Rotation r;
if (direct != 1.0) r.setQuaternion(qw(), qx(), qy(), qz());
// alculate relational angle
r.setAxisAndAngle(ax, ay, az, angle, direct);
const dReal *q = r.q();
setQ(q);
}
XERCES_CPP_NAMESPACE_USE
#define ARRAY_SIZE(ARY) ( (int)(sizeof(ARY)/sizeof(ARY[0])) )
#ifdef CONF_DUMP
#define ENABLE_DUMP
#define ENABLE_DUMP1
#endif
#include "ParserDump.h"
void JointParser::parse(DOMNode &target, Eval &eval)
{
char *name = XMLUtils::getAttribute(target, "name");
DUMP1(("name %s\n", name));
m_transform.push();
DOMNode *p = target.getFirstChild();
while (p) {
char *s = XMLString::transcode(p->getNodeName());
// printf("%s\n", s);
if (strcmp(s, "type") == 0) {
char *type = XMLUtils::parseJointType(*p);
DUMP1(("type=\"%s\"\n", type));
Joint *j = 0;
if (strcmp(type, "fixed") == 0) {
j = new FixedJoint(name);
} else if (strcmp(type, "ball") == 0) {
j = new BallJoint(name);
} else if (strcmp(type, "hinge") == 0) {
j = new HingeJoint(name, Vector3d(0, 1, 0));
}
if (j) {
m_joint = j;
}
XMLString::release(&type);
/*
} else if (strcmp(s, "limitOrientation") == 0) {
double *v = XMLUtils::parseLimitOrientation(*p);
if (m_joint) {
delete m_joint;
}
m_joint = new HingeJoint(name, Vector3d(v[0], v[1], v[2]));
*/
} else if (strcmp(s, "translation") == 0) {
double * values = XMLUtils::parseTranslation(*p, eval);
DUMP1(("translation (%f, %f, %f)\n", values[0], values[1], values[2]));
m_transform.curr().push(Vector3d(values[0], values[1], values[2]));
} else if (strcmp(s, "rotation") == 0) {
double *values = XMLUtils::parseRotation(*p);
DUMP1(("rotation (%f, %f, %f, %f)\n", values[0], values[1], values[2], values[3]));
Rotation rot;
rot.setAxisAndAngle(values[0], values[1], values[2], values[3]);
m_transform.curr().push(rot);
} else if (strcmp(s, "axis") == 0) {
double *values = XMLUtils::parseAxis(*p);
DUMP1(("axis (%f, %f, %f)\n", values[0], values[1], values[2]));
if (m_joint) {
if (m_joint->type() == Joint::TYPE_HINGE) {
HingeJoint *hinge = static_cast<HingeJoint*>(m_joint);
hinge->setAxis(values[0], values[1], values[2]);
} else {
DUMP1(("not hinge type joint. axis ingored\n"));
}
} else {
DUMP1(("no joint type"));
}
} else if (strcmp(s, "children") == 0) {
DUMP1(("JOINT TRANSFORM TEST \n"));
TRANSFORM_TEST(m_transform.curr());
parseChildren(*p, eval);
}
p = p->getNextSibling();
XMLString::release(&s);
}
{
Transform &t = m_transform.curr();
const Vector3d &v = t.translation();
if (m_joint) {
m_f.addJoint(m_joint);
DUMP(("Joint : (%f, %f, %f)\n", v.x(), v.y(), v.z()));
m_joint->setAnchor(v.x(), v.y(), v.z());
}
}
m_transform.pop();
char *strs[] = { name,};
for (int i=0; i<ARRAY_SIZE(strs); i++) {
char *p = strs[i];
XMLString::release(&p);
}
}