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]); }