MeshSkin* MeshSkin::clone(NodeCloneContext &context) const
{
MeshSkin* skin = new MeshSkin();
skin->_bindShape = _bindShape;
if (_rootNode && _rootJoint)
{
const unsigned int jointCount = getJointCount();
skin->setJointCount(jointCount);
GP_ASSERT(skin->_rootNode == NULL);
// Check if the root node has already been cloned.
if (Node* rootNode = context.findClonedNode(_rootNode))
{
skin->_rootNode = rootNode;
rootNode->addRef();
}
else
{
skin->_rootNode = _rootNode->cloneRecursive(context);
}
Node* node = NULL;
if (strcmp(skin->_rootNode->getId(), _rootJoint->getId()) == 0)
{
node = skin->_rootNode;
}
else
{
node = skin->_rootNode->findNode(_rootJoint->getId());
}
GP_ASSERT(node);
skin->_rootJoint = static_cast<Joint*>(node);
for (unsigned int i = 0; i < jointCount; ++i)
{
Joint* oldJoint = getJoint(i);
GP_ASSERT(oldJoint);
Joint* newJoint = static_cast<Joint*>(skin->_rootNode->findNode(oldJoint->getId()));
if (!newJoint)
{
if (strcmp(skin->_rootJoint->getId(), oldJoint->getId()) == 0)
newJoint = static_cast<Joint*>(skin->_rootJoint);
}
GP_ASSERT(newJoint);
skin->setJoint(newJoint, i);
}
}
return skin;
}
Joint* MeshSkin::getJoint(const char* id) const
{
GP_ASSERT(id);
for (size_t i = 0, count = _joints.size(); i < count; ++i)
{
Joint* j = _joints[i];
if (j && j->getId() != NULL && strcmp(j->getId(), id) == 0)
{
return j;
}
}
return NULL;
}
/* normal construction test */
void JointTest::normalConstruction() {
int id = 1;
std::string name = "test";
float mass = 2;
float length = 3;
using namespace RoboticArm;
Joint createdPart = Joint(id, name, mass, length);
if (createdPart.getId() == id && createdPart.getName() == name &&
createdPart.getLength() == length && createdPart.getMass() == mass) {
CPPUNIT_ASSERT(true);
} else {
CPPUNIT_ASSERT(false);
}
}
void JointTest::validParReadBack(){
bool success = true;
int id = 111;
std::string name = "test";
float mass = 123.54566;
float length = 14676.54;
using namespace RoboticArm;
try {
Joint createdPart = Joint(id, name, mass, length);
if (id != createdPart.getId()) {
std::cout << "ID is wrong." << std::endl;
success = false;
}
if (name != createdPart.getName()) {
std::cout << "Name is wrong." << std::endl;
success = false;
}
if (mass != createdPart.getMass()) {
std::cout << "Mass is wrong." << std::endl;
success = false;
}
if (length != createdPart.getLength()) {
std::cout << "Length is wrong." << std::endl;
success = false;
}
} catch (const std::invalid_argument& ia) {
success = false;
}
CPPUNIT_ASSERT(success);
}
void NiutHumanReader::projectJoint(Joint& joint, double* kinectPos) {
double translation[4][4], rotX[4][4], rotY[4][4], rotZ[4][4], transformation[4][4], tmp1[4][4], tmp2[4][4];
Joint oldJoint(joint.getId(), joint.getAgentId());
oldJoint = joint;
//translation transformation matrice
translation[0][0] = 1;
translation[0][1] = 0;
translation[0][2] = 0;
translation[0][3] = kinectPos[0];
translation[1][0] = 0;
translation[1][1] = 1;
translation[1][2] = 0;
translation[1][3] = kinectPos[1];
translation[2][0] = 0;
translation[2][1] = 0;
translation[2][2] = 1;
translation[2][3] = kinectPos[2];
translation[3][0] = 0;
translation[3][1] = 0;
translation[3][2] = 0;
translation[3][3] = 1;
//rotation in x axe transformation matrice
rotX[0][0] = 1;
rotX[0][1] = 0;
rotX[0][2] = 0;
rotX[0][3] = 0;
rotX[1][0] = 0;
rotX[1][1] = cos(kinectPos[3]);
rotX[1][2] = -sin(kinectPos[3]);
rotX[1][3] = 0;
rotX[2][0] = 0;
rotX[2][1] = sin(kinectPos[3]);
rotX[2][2] = cos(kinectPos[3]);
rotX[2][3] = 0;
rotX[3][0] = 0;
rotX[3][1] = 0;
rotX[3][2] = 0;
rotX[3][3] = 1;
//rotation in y axe transformation matrice
rotY[0][0] = cos(kinectPos[4]);
rotY[0][1] = 0;
rotY[0][2] = sin(kinectPos[4]);
rotY[0][3] = 0;
rotY[1][0] = 0;
rotY[1][1] = 1;
rotY[1][2] = 0;
rotY[1][3] = 0;
rotY[2][0] = -sin(kinectPos[4]);
rotY[2][1] = 0;
rotY[2][2] = cos(kinectPos[4]);
rotY[2][3] = 0;
rotY[3][0] = 0;
rotY[3][1] = 0;
rotY[3][2] = 0;
rotY[3][3] = 1;
//rotation in z axe transformation matrice
rotZ[0][0] = cos(kinectPos[5]);
rotZ[0][1] = -sin(kinectPos[5]);
rotZ[0][2] = 0;
rotZ[0][3] = 0;
rotZ[1][0] = sin(kinectPos[5]);
rotZ[1][1] = cos(kinectPos[5]);
rotZ[1][2] = 0;
rotZ[1][3] = 0;
rotZ[2][0] = 0;
rotZ[2][1] = 0;
rotZ[2][2] = 1;
rotZ[2][3] = 0;
rotZ[3][0] = 0;
rotZ[3][1] = 0;
rotZ[3][2] = 0;
rotZ[3][3] = 1;
//transformation matrice = translation*rotX*rotY*rotZ
MathFunctions::multiplyMatrices4x4(tmp1[0], rotX[0], rotY[0]);
MathFunctions::multiplyMatrices4x4(tmp2[0], rotZ[0], tmp1[0]);
MathFunctions::multiplyMatrices4x4(transformation[0], translation[0], tmp2[0]);
//final transformation
// Y X Z Y
// | / | /
// Kinect |/ ____ Z , World |/_____X
joint.position_.set<0>(transformation[0][0] * oldJoint.position_.get<2>() + transformation[0][1] * oldJoint.position_.get<0>() + transformation[0][2] * oldJoint.position_.get<1>() + transformation[0][3]);
joint.position_.set<1>(transformation[1][0] * oldJoint.position_.get<2>() + transformation[1][1] * oldJoint.position_.get<0>() + transformation[1][2] * oldJoint.position_.get<1>() + transformation[1][3]);
joint.position_.set<2>(transformation[2][0] * oldJoint.position_.get<2>() + transformation[2][1] * oldJoint.position_.get<0>() + transformation[2][2] * oldJoint.position_.get<1>() + transformation[2][3]);
}
