Beispiel #1
0
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;
}
Beispiel #2
0
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);

}
Beispiel #5
0
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]);

}