void Skeleton::addNode(BodyNode *_b, bool _addParentJoint) {
mNodes.push_back(_b);
mNodeNameMap[_b->getName()] = mNodes.size()-1;
_b->setSkelIndex(mNodes.size()-1);
if (_addParentJoint)
addJoint(_b->getParentJoint());
}
void Framework::loadJoint( MyGUI::xml::ElementPtr node,RigidPtr parent )
{
ObjectFactory& factory = ObjectFactory::getSingleton();
string typeName = node->findAttribute("type");
JointPtr jpt = boost::dynamic_pointer_cast<Joint>(factory.createObject(typeName));
if(jpt)
{
if( parent )
{
jpt->mRigid[0] = parent;
}
addJoint(jpt);
jpt->load(node);
MyGUI::xml::ElementEnumerator ce = node->getElementEnumerator();
while(ce.next())
{
MyGUI::xml::ElementPtr child = ce.current();
if( child->getName() == "rigid" )
{
loadRigid(child, jpt);
}
break;
}
}
else
{
WARNING_LOG("Factory can't make "<<typeName);
}
}
void Skeleton::addBodyNode(BodyNode* _body, bool _addParentJoint)
{
assert(_body != NULL);
mBodyNodes.push_back(_body);
_body->setSkeletonIndex(mBodyNodes.size() - 1);
// The parent joint possibly be null
if (_addParentJoint)
addJoint(_body->getParentJoint());
}
void IKTree::addJoint(BVHNode *node)
{
bone[numBones].node = node;
bone[numBones].offset.setValue((float)node->offset[0],
(float)node->offset[1],
(float)node->offset[2]);
bone[numBones].weight = node->ikWeight;
bone[numBones].numChildren = 0;
int myNumBones = numBones++;
for (int i=0; i<node->numChildren(); i++) {
bone[myNumBones].child[bone[myNumBones].numChildren++] = numBones;
addJoint(node->child(i));
}
}
static void
handleLButtonUp(HWND hDlg, LPARAM lParam)
{
#if OPENGL
double p1x,p1y,p2x,p2y;
float red, green, blue;
#endif
/* These two lines are the only real platform dependent code in here.
* Once the mouse position is grabbed, the math is similar.
*/
ptNew.x = LOWORD(lParam); ptNew.y = HIWORD(lParam);
DPtoLP(hdc, &ptNew, 1);
/* We insist on actually being inside the drawing area...
*/
if(ptNew.x <= maxSize &&
ptNew.x >= 0 &&
ptNew.y <= maxSize &&
ptNew.y >= 0 &&
inside)
{
SetROP2(hdc, R2_COPYPEN);
SelectObject(hdc, hCurrentPen);
/* TODO: Turn this into a swicth on tool. */
if(tool==joint)
addJoint(hDlg);
else if (tool==fixedpoint || tool==loadpoint)
addTriangle(hDlg);
else if (tool==measpoint || tool==holepoint)
addCircle(hDlg);
}
else /* outside drawing area */
{
SetROP2(hdc, R2_NOT);
if(!tool && inside)
{
MoveToEx(hdc, ptBegin.x, ptBegin.y, NULL);
LineTo(hdc, ptOld.x, ptOld.y);
}
}
ReleaseCapture();
// ReleaseDC(hDlg, hdc);
} /* close handleMouseUp() */
void URDFProperty::on_treeWidget_customContextMenuRequested(const QPoint &pos)
{
QTreeWidgetItem *sel = tree_widget_->selectedItems()[0];
QMenu *menu = new QMenu(tree_widget_.get());
menu->addAction("Add");
menu->addAction("Remove");
QAction *selected_item = menu->exec(tree_widget_->mapToGlobal(pos));
if (selected_item)
{
if (selected_item->text() == "Add")
{
if (sel == link_root_ || sel->parent() == link_root_)
{
addLink();
}
else if (sel == joint_root_ || isJoint(sel))
{
addJoint(sel);
}
}
else
{
if (sel->parent() == link_root_)
{
link_names_.removeOne(sel->text(0));
link_root_->removeChild(sel);
}
else if (isJoint(sel))
{
joint_names_.removeOne(sel->text(0));
sel->parent()->removeChild(sel);
}
}
}
delete menu;
}
void HelloWorld::addNewSpriteAtPos(Vec2 pos)
{
auto sp1=Sprite::create("BoxA2.png");
auto body1=PhysicsBody::createBox(sp1->getContentSize());
sp1->setPhysicsBody(body1);
sp1->setPosition(pos);
sp1->setTag(Tag);
this->addChild(sp1);
auto sp2=Sprite::create("BoxB2.png");
auto body2=PhysicsBody::createBox(sp2->getContentSize());
sp2->setPhysicsBody(body2);
sp2->setPosition(pos+Vec2(-10,100));
sp2->setTag(Tag);
this->addChild(sp2);
auto joint=PhysicsJointDistance::construct(body1,body2,Vec2(0,0),Vec2(0,sp2->getContentSize().height/2));
auto world=this->getScene()->getPhysicsWorld();
world->addJoint(joint);
}
void PhysicsRagdoll::initProperties(const vec3& position, const RagdollDesc& desc, const PhysicsMaterial& material,
uint32 collisionGroup, uint32 collisionGroupAgainst )
{
m_StartPosition = position;
m_Aggregate = PHYSICS->getSDK()->createAggregate(20, false);
addBodyPart(&m_pArmL1, desc.armL1, material, collisionGroup, collisionGroupAgainst);
addBodyPart(&m_pArmL2, desc.armL2, material, collisionGroup, collisionGroupAgainst);
addBodyPart(&m_pHandL, desc.handL, material, collisionGroup, collisionGroupAgainst);
addBodyPart(&m_pArmR1, desc.armR1, material, collisionGroup, collisionGroupAgainst);
addBodyPart(&m_pArmR2, desc.armR2, material, collisionGroup, collisionGroupAgainst);
addBodyPart(&m_pHandR, desc.handR, material, collisionGroup, collisionGroupAgainst);
addBodyPart(&m_pHead, desc.head, material, collisionGroup, collisionGroupAgainst);
addBodyPart(&m_pNeck, desc.neck, material, collisionGroup, collisionGroupAgainst);
addBodyPart(&m_pShoulderL, desc.shoulderL, material, collisionGroup, collisionGroupAgainst);
addBodyPart(&m_pShoulderR, desc.shoulderR, material, collisionGroup, collisionGroupAgainst);
addBodyPart(&m_pSpine1, desc.spine1, material, collisionGroup, collisionGroupAgainst);
addBodyPart(&m_pSpine2, desc.spine2, material, collisionGroup, collisionGroupAgainst);
addBodyPart(&m_pSpine3, desc.spine3, material, collisionGroup, collisionGroupAgainst);
addBodyPart(&m_pPelvis, desc.pelvis, material, collisionGroup, collisionGroupAgainst);
addBodyPart(&m_pLegL1, desc.legL1, material, collisionGroup, collisionGroupAgainst);
addBodyPart(&m_pLegL2, desc.legL2, material, collisionGroup, collisionGroupAgainst);
addBodyPart(&m_pFootL, desc.footL, material, collisionGroup, collisionGroupAgainst);
addBodyPart(&m_pLegR1, desc.legR1, material, collisionGroup, collisionGroupAgainst);
addBodyPart(&m_pLegR2, desc.legR2, material, collisionGroup, collisionGroupAgainst);
addBodyPart(&m_pFootR, desc.footR, material, collisionGroup, collisionGroupAgainst);
//Spine
addJoint(&m_pPelvisSpine3Joint, m_pPelvis, m_pSpine3, desc.pelvis, desc.spine3,
physx::PxD6Motion::eLIMITED, physx::PxD6Motion::eLIMITED, physx::PxD6Motion::eLIMITED,
-pi / 16.0f, pi / 16.0f, pi / 8.0f, pi / 8.0f);
addJoint(&m_pSpine3Spine2Joint, m_pSpine3, m_pSpine2, desc.spine3, desc.spine2,
physx::PxD6Motion::eLIMITED, physx::PxD6Motion::eLIMITED, physx::PxD6Motion::eLIMITED,
-pi / 8.0f, pi / 8.0f, pi / 8.0f, pi / 8.0f);
addJoint(&m_pSpine2Spine1Joint, m_pSpine2, m_pSpine1, desc.spine2, desc.spine1,
physx::PxD6Motion::eLIMITED, physx::PxD6Motion::eLIMITED, physx::PxD6Motion::eLIMITED,
-pi / 8.0f, pi / 8.0f, pi / 8.0f, pi / 8.0f);
//Shoulders
addJoint(&m_pSpine1ShoulderLJoint, m_pSpine1, m_pShoulderL, desc.spine1, desc.shoulderL,
physx::PxD6Motion::eLOCKED, physx::PxD6Motion::eLOCKED, physx::PxD6Motion::eLOCKED,
0, 0, pi / 6.0f, pi / 9.0f);
addJoint(&m_pSpine1ShoulderRJoint, m_pSpine1, m_pShoulderR, desc.spine1, desc.shoulderR,
physx::PxD6Motion::eLOCKED, physx::PxD6Motion::eLOCKED, physx::PxD6Motion::eLOCKED,
0, 0, pi / 6.0f, pi / 9.0f);
//Neck/Head
addJoint(&m_pSpine1NeckJoint, m_pSpine1, m_pNeck, desc.spine1, desc.neck,
physx::PxD6Motion::eLOCKED, physx::PxD6Motion::eLOCKED, physx::PxD6Motion::eLOCKED,
0, 0, pi/6, pi/6);
addJoint(&m_pNeckHeadJoint, m_pNeck, m_pHead, desc.neck, desc.head,
physx::PxD6Motion::eLIMITED, physx::PxD6Motion::eLIMITED, physx::PxD6Motion::eLIMITED,
-pi/6, pi/6, pi/6, pi/6);
//Arms
addJoint(&m_pShouldersArmL1Joint, m_pShoulderL, m_pArmL1, desc.shoulderL, desc.armL1,
physx::PxD6Motion::eLIMITED, physx::PxD6Motion::eLIMITED, physx::PxD6Motion::eLIMITED,
-pi / 8.0f, pi / 8.0f, pi / 6, pi / 6);
addJoint(&m_pShouldersArmR1Joint, m_pShoulderR, m_pArmR1, desc.shoulderR, desc.armR1,
physx::PxD6Motion::eLIMITED, physx::PxD6Motion::eLIMITED, physx::PxD6Motion::eLIMITED,
-pi / 8.0f, pi / 8.0f, pi / 6, pi / 6);
addJoint(&m_pArmL1ArmL2Joint, m_pArmL1, m_pArmL2, desc.armL1, desc.armL2,
physx::PxD6Motion::eLIMITED, physx::PxD6Motion::eLIMITED, physx::PxD6Motion::eLOCKED,
-pi / 8.0f, pi / 8.0f, pi / 6, pi / 6);
addJoint(&m_pArmR1ArmR2Joint, m_pArmR1, m_pArmR2, desc.armR1, desc.armR2,
physx::PxD6Motion::eLIMITED, physx::PxD6Motion::eLIMITED, physx::PxD6Motion::eLOCKED,
-pi / 8.0f, pi / 8.0f, pi / 6, pi / 6);
addJoint(&m_pArmL2HandJoint, m_pArmL2, m_pHandL, desc.armL2, desc.handL,
physx::PxD6Motion::eLOCKED, physx::PxD6Motion::eLIMITED, physx::PxD6Motion::eLIMITED,
0, 0, piOverFour, piOverTwo);
addJoint(&m_pArmR2HandJoint, m_pArmR2, m_pHandR, desc.armR2, desc.handR,
physx::PxD6Motion::eLOCKED, physx::PxD6Motion::eLIMITED, physx::PxD6Motion::eLIMITED,
0, 0, piOverFour, piOverTwo);
//Legs
addJoint(&m_pPelvisLegL1Joint, m_pPelvis, m_pLegL1, desc.pelvis, desc.legL1,
physx::PxD6Motion::eLIMITED, physx::PxD6Motion::eLIMITED, physx::PxD6Motion::eLIMITED,
-pi / 6.0f, pi / 6.0f, piOverTwo, piOverTwo);
addJoint(&m_pPelvisLegR1Joint, m_pPelvis, m_pLegR1, desc.pelvis, desc.legR1,
physx::PxD6Motion::eLIMITED, physx::PxD6Motion::eLIMITED, physx::PxD6Motion::eLIMITED,
-pi / 6.0f, pi / 6.0f, piOverTwo, piOverTwo);
addJoint(&m_pLegL1LegL2Joint, m_pLegL1, m_pLegL2, desc.legL1, desc.legL2,
physx::PxD6Motion::eLOCKED, physx::PxD6Motion::eLOCKED, physx::PxD6Motion::eLIMITED,
0, 0, pi / 3, pi / 3);
addJoint(&m_pLegR1LegR2Joint, m_pLegR1, m_pLegR2, desc.legR1, desc.legR2,
physx::PxD6Motion::eLOCKED, physx::PxD6Motion::eLOCKED, physx::PxD6Motion::eLIMITED,
0, 0, pi / 3, pi / 3);
addJoint(&m_pLegL2FootJoint, m_pLegL2, m_pFootL, desc.legL2, desc.footL,
physx::PxD6Motion::eLOCKED, physx::PxD6Motion::eLIMITED, physx::PxD6Motion::eLIMITED,
0, 0, piOverFour, piOverFour);
addJoint(&m_pLegR2FootJoint, m_pLegR2, m_pFootR, desc.legR2, desc.footR,
physx::PxD6Motion::eLOCKED, physx::PxD6Motion::eLIMITED, physx::PxD6Motion::eLIMITED,
0, 0, piOverFour, piOverFour);
PHYSICS->getScene()->addAggregate(*m_Aggregate);
}
BOOL CALLBACK
BlockDlgProc (HWND hDlg, UINT message, WPARAM wParam, LPARAM lParam)
{
double xl, yl, xu, yu;
extern POINT ptBegin, ptNew, ptOld;
extern int type;
extern double ScaleX, ScaleY;
extern int maxSize;
extern JOINTLIST *jointlist;
switch (message) {
case WM_INITDIALOG:
SetDlgItemInt (hDlg, IDC_XL, 0, FALSE);
SetDlgItemInt (hDlg, IDC_YL, 0, FALSE);
SetDlgItemInt (hDlg, IDC_XU, 0, FALSE);
SetDlgItemInt (hDlg, IDC_YU, 0, FALSE);
SetDlgItemInt (hDlg, IDC_TYPE, 1, FALSE);
return TRUE;
case WM_COMMAND:
switch (LOWORD (wParam)) {
case IDOK:
xl = GetDlgItemInt (hDlg, IDC_XL, NULL, TRUE);
yl = GetDlgItemInt (hDlg, IDC_YL, NULL, TRUE);
xu = GetDlgItemInt (hDlg, IDC_XU, NULL, TRUE);
yu = GetDlgItemInt (hDlg, IDC_YU, NULL, TRUE);
type = GetDlgItemInt (hDlg, IDC_TYPE, NULL, TRUE);
ptBegin.x = (long) floor (xl / (ScaleX / maxSize));
ptBegin.y = (long) floor (yl / (ScaleY / maxSize));
ptNew.x = (long) floor (xu / (ScaleX / maxSize));
ptNew.y = (long) floor (yl / (ScaleY / maxSize));
addJoint (hDlg);
ptBegin.x = (long) floor (xu / (ScaleX / maxSize));
ptBegin.y = (long) floor (yl / (ScaleY / maxSize));
ptNew.x = (long) floor (xu / (ScaleX / maxSize));
ptNew.y = (long) floor (yu / (ScaleY / maxSize));
addJoint (hDlg);
ptBegin.x = (long) floor (xu / (ScaleX / maxSize));
ptBegin.y = (long) floor (yu / (ScaleY / maxSize));
ptNew.x = (long) floor (xl / (ScaleX / maxSize));
ptNew.y = (long) floor (yu / (ScaleY / maxSize));
addJoint (hDlg);
ptBegin.x = (long) floor (xl / (ScaleX / maxSize));
ptBegin.y = (long) floor (yu / (ScaleY / maxSize));
ptNew.x = (long) floor (xl / (ScaleX / maxSize));
ptNew.y = (long) floor (yl / (ScaleY / maxSize));
addJoint (hDlg);
return FALSE;
case IDCANCEL:
EndDialog (hDlg, 0);
return TRUE;
}
break;
}
return FALSE;
}
void Skeleton::addNode(BodyNode *_b) {
mNodes.push_back(_b);
_b->setSkelIndex(mNodes.size()-1);
addJoint(_b->getParentJoint());
}
//!
MakeHSkeleton::MakeHSkeleton()
{
mJoints.clear();
mBones.clear();
vec3 zero(0,0,0);
addJoint(zero); //0: head
addJoint(zero); //1: neck
addJoint(zero); //2: left shoulder
addJoint(zero); //3: right shoulder
addJoint(zero); //4: torso
addJoint(zero); //5: left elbow
addJoint(zero); //6: right elbow
addJoint(zero); //7: left hand
addJoint(zero); //8: right hand
addJoint(zero); //9: left hip
addJoint(zero); //10: right hip
addJoint(zero); //11: left knee
addJoint(zero); //12: right knee
addJoint(zero); //13: left foot
addJoint(zero); //14: right foot
addJoint(zero); //15: left finger
addJoint(zero); //16: right finger
addJoint(zero); //17: left toe
addJoint(zero); //18: right toe
addBone(0,1,-1); //0: head
addBone(1,2,0); //1: left shoulder
addBone(1,3,0); //2: right shoulder
addBone(1,4,0); //3: breast
addBone(4,9,3); //4: left abdomen
addBone(4,10,3); //5: right abdomen
addBone(2,5,1); //6: left upper arm
addBone(3,6,2); //7: right upper arm
addBone(9,11,4); //8: left upper leg
addBone(10,12,5); //9: right upper leg
addBone(5,7,6); //10: left lower arm
addBone(6,8,7); //11: right lower arm
addBone(11,13,8); //12: left lower leg
addBone(12,14,9); //13: right lower leg
addBone(7,15,10); //14: left hand
addBone(8,16,11); //15: right hand
addBone(13,17,12); //16: left foot
addBone(14,18,13); //17: right foot
}
Joint* ODESimulator::createJoint()
{
Joint * newJoint = new ODEJoint(mWorldID);
addJoint(newJoint);
return newJoint;
}
void IKTree::set(BVHNode *root)
{
numBones = 0;
if (root)
addJoint(root);
}
void SimEntity::setInitialPose(bool reset) {
bool worldAnchor = false;
if(!control) return;
if(config.find("rootNode") != config.end()) {
if(config.find("anchor") != config.end()) {
if((std::string)config["anchor"] == "world") {
worldAnchor = true;
}
}
NodeId id = getNode((std::string)config["rootNode"]);
NodeData myNode = control->nodes->getFullNode(id);
utils::Quaternion tmpQ(1, 0, 0, 0);
utils::Vector tmpV;
if(config.find("position") != config.end()) {
myNode.pos.x() = config["position"][0];
myNode.pos.y() = config["position"][1];
myNode.pos.z() = config["position"][2];
control->nodes->editNode(&myNode, EDIT_NODE_POS | EDIT_NODE_MOVE_ALL);
}
if(config.find("rotation") != config.end()) {
// check if euler angles or quaternion is provided; rotate around z
// if only one angle is provided
switch (config["rotation"].size()) {
case 1: tmpV[0] = 0;
tmpV[1] = 0;
tmpV[2] = config["rotation"][0];
tmpQ = utils::eulerToQuaternion(tmpV);
break;
case 3: tmpV[0] = config["rotation"][0];
tmpV[1] = config["rotation"][1];
tmpV[2] = config["rotation"][2];
tmpQ = utils::eulerToQuaternion(tmpV);
break;
case 4: tmpQ.x() = (sReal)config["rotation"][1];
tmpQ.y() = (sReal)config["rotation"][2];
tmpQ.z() = (sReal)config["rotation"][3];
tmpQ.w() = (sReal)config["rotation"][0];
break;
}
myNode.rot = tmpQ;
control->nodes->editNode(&myNode, EDIT_NODE_ROT | EDIT_NODE_MOVE_ALL);
}
if(worldAnchor) {
if (reset) {
fprintf(stderr, "Resetting initial entity pose.\n");
std::map<unsigned long, std::string>::iterator it;
JointId anchorJointId = 0;
for (it=jointIds.begin(); it!=jointIds.end(); ++it) {
if (it->second == "anchor_"+name) {
anchorJointId = it->first;
break;
}
}
SimJoint* anchorjoint = control->joints->getSimJoint(anchorJointId);
if (anchorjoint != NULL) {
anchorjoint->reattachJoint();
}
else fprintf(stderr, "Could not reset anchor of entity %s.\n", name.c_str());
}
else {
JointData anchorjoint;
anchorjoint.nodeIndex1 = id;
anchorjoint.nodeIndex2 = 0;
anchorjoint.type = JOINT_TYPE_FIXED;
anchorjoint.name = "anchor_"+name;
JointId anchorJointId = control->joints->addJoint(&anchorjoint);
addJoint(anchorJointId, anchorjoint.name);
}
}
// set Joints
configmaps::ConfigVector::iterator it;
configmaps::ConfigMap::iterator joint_it;
for (it = config["poses"].begin(); it!= config["poses"].end(); ++it) {
if ((std::string)(*it)["name"] == (std::string)config["pose"]) {
for (joint_it = (*it)["joints"][0].children.begin();
joint_it!= (*it)["joints"][0].children.end(); ++joint_it) {
//fprintf(stderr, "setMotorValue: joint: %s, id: %lu, value: %f\n", ((std::string)joint_it->first).c_str(), motorIDMap[joint_it->first], (double)joint_it->second);
control->motors->setMotorValue(getMotor(joint_it->first),
joint_it->second);
}
}
}
}
}