dAnimIKController* CreateSkeletonRig(DemoEntity* const character) { DemoEntity* const rootEntity = FindRigRoot(character); dAnimIKController* const controller = CreateIKController(); controller->SetUserData(rootEntity); int stack = 0; DemoEntity* entityStack[32]; dAnimIKRigJoint* parentJointStack[32]; for (DemoEntity* node = rootEntity->GetChild(); node; node = node->GetSibling()) { entityStack[stack] = node; parentJointStack[stack] = controller->GetAsIKRigJoint(); stack ++; } while (stack) { stack--; DemoEntity* const entity = entityStack[stack]; dAnimIKRigJoint* const parentJoint = parentJointStack[stack]; dSkeletonRigDefinition* const definitions = FindDefinition(entity); if (definitions) { dAnimIK3dofJoint* const joint = new dAnimIK3dofJoint(parentJoint); joint->SetUserData(entity); for (DemoEntity* node = entity->GetChild(); node; node = node->GetSibling()) { entityStack[stack] = node; parentJointStack[stack] = joint; stack++; } } } return controller; }
DemoEntity* FindRigRoot(DemoEntity* const character) { int stack = 1; DemoEntity* pool[32]; pool[0] = character; const int nodesCount = sizeof(inverseKinematicsRidParts) / sizeof(inverseKinematicsRidParts[0]); while (stack) { stack--; DemoEntity* const entity = pool[stack]; for (int i = 0; i < nodesCount; i++) { if (entity->GetName() == inverseKinematicsRidParts[i].m_name) { if (inverseKinematicsRidParts[i].m_type == dRigType::m_root) { return entity; } break; } } for (DemoEntity* node = entity->GetChild(); node; node = node->GetSibling()) { pool[stack] = node; stack++; } } dAssert(0); return NULL; }
DemoEntity* FindMesh(const DemoEntity* const bodyPart) const { for (DemoEntity* child = bodyPart->GetChild(); child; child = child->GetSibling()) { if (child->GetMesh()) { return child; } } dAssert(0); return NULL; }
void PopulateBasePose(dAnimPose& basePose, DemoEntity* const character) { basePose.Clear(); int stack = 1; DemoEntity* pool[32]; pool[0] = character; while (stack) { stack--; DemoEntity* const entity = pool[stack]; dAnimKeyframe& transform = basePose.Append()->GetInfo(); dMatrix matrix(entity->GetCurrentMatrix()); transform.m_posit = matrix.m_posit; transform.m_rotation = dQuaternion(matrix); transform.m_userData = entity; for (DemoEntity* node = entity->GetChild(); node; node = node->GetSibling()) { pool[stack] = node; stack++; } } }
void CreateRagdollExperiment_0(const dMatrix& location) { static dJointDefinition jointsDefinition[] = { { "body" }, { "leg", 100.0f,{ -15.0f, 15.0f, 30.0f },{ 0.0f, 0.0f, 180.0f }, dAnimationRagdollJoint::m_threeDof }, { "foot", 100.0f,{ -15.0f, 15.0f, 30.0f },{ 0.0f, 90.0f, 0.0f }, dAnimationRagdollJoint::m_twoDof }, }; const int definitionCount = sizeof (jointsDefinition)/sizeof (jointsDefinition[0]); NewtonWorld* const world = GetWorld(); DemoEntityManager* const scene = (DemoEntityManager*)NewtonWorldGetUserData(world); DemoEntity* const modelEntity = DemoEntity::LoadNGD_mesh("selfbalance_01.ngd", GetWorld(), scene->GetShaderCache()); dMatrix matrix0(modelEntity->GetCurrentMatrix()); //matrix0.m_posit = location; //modelEntity->ResetMatrix(*scene, matrix0); scene->Append(modelEntity); // add the root childBody NewtonBody* const rootBody = CreateBodyPart(modelEntity); // build the rag doll with rigid bodies connected by joints dDynamicsRagdoll* const dynamicRagdoll = new dDynamicsRagdoll(rootBody, dGetIdentityMatrix()); AddModel(dynamicRagdoll); dynamicRagdoll->SetCalculateLocalTransforms(true); // save the controller as the collision user data, for collision culling NewtonCollisionSetUserData(NewtonBodyGetCollision(rootBody), dynamicRagdoll); int stackIndex = 0; DemoEntity* childEntities[32]; dAnimationJoint* parentBones[32]; for (DemoEntity* child = modelEntity->GetChild(); child; child = child->GetSibling()) { parentBones[stackIndex] = dynamicRagdoll; childEntities[stackIndex] = child; stackIndex++; } // walk model hierarchic adding all children designed as rigid body bones. while (stackIndex) { stackIndex--; DemoEntity* const entity = childEntities[stackIndex]; dAnimationJoint* parentNode = parentBones[stackIndex]; const char* const name = entity->GetName().GetStr(); for (int i = 0; i < definitionCount; i++) { if (!strcmp(jointsDefinition[i].m_boneName, name)) { NewtonBody* const childBody = CreateBodyPart(entity); // connect this body part to its parent with a rag doll joint parentNode = CreateChildNode(childBody, parentNode, jointsDefinition[i]); NewtonCollisionSetUserData(NewtonBodyGetCollision(childBody), parentNode); break; } } for (DemoEntity* child = entity->GetChild(); child; child = child->GetSibling()) { parentBones[stackIndex] = parentNode; childEntities[stackIndex] = child; stackIndex++; } } SetModelMass (100.0f, dynamicRagdoll); // set the collision mask // note this container work best with a material call back for setting bit field //dAssert(0); //controller->SetDefaultSelfCollisionMask(); // transform the entire contraction to its location dMatrix worldMatrix(modelEntity->GetCurrentMatrix() * location); worldMatrix.m_posit = location.m_posit; NewtonBodySetMatrixRecursive(rootBody, &worldMatrix[0][0]); // attach effectors here for (dAnimationJoint* joint = GetFirstJoint(dynamicRagdoll); joint; joint = GetNextJoint(joint)) { if (joint->GetAsRoot()) { dAssert(dynamicRagdoll == joint); dynamicRagdoll->SetHipEffector(joint); } else if (joint->GetAsLeaf()) { dynamicRagdoll->SetFootEffector(joint); } } dynamicRagdoll->Finalize(); //return controller; }
void MakeKukaRobot(DemoEntityManager* const scene, DemoEntity* const model) { m_kinematicSolver = NewtonCreateInverseDynamics(scene->GetNewton()); NewtonBody* const baseFrameBody = CreateBodyPart(model, armRobotConfig[0]); void* const baseFrameNode = NewtonInverseDynamicsAddRoot(m_kinematicSolver, baseFrameBody); NewtonBodySetMassMatrix(baseFrameBody, 0.0f, 0.0f, 0.0f, 0.0f); dMatrix boneAligmentMatrix( dVector(0.0f, 1.0f, 0.0f, 0.0f), dVector(0.0f, 0.0f, 1.0f, 0.0f), dVector(1.0f, 0.0f, 0.0f, 0.0f), dVector(0.0f, 0.0f, 0.0f, 1.0f)); int stackIndex = 0; DemoEntity* childEntities[32]; void* parentBones[32]; for (DemoEntity* child = model->GetChild(); child; child = child->GetSibling()) { parentBones[stackIndex] = baseFrameNode; childEntities[stackIndex] = child; stackIndex++; } dKukaEffector* effector = NULL; const int partCount = sizeof(armRobotConfig) / sizeof(armRobotConfig[0]); while (stackIndex) { stackIndex--; DemoEntity* const entity = childEntities[stackIndex]; void* const parentJoint = parentBones[stackIndex]; const char* const name = entity->GetName().GetStr(); for (int i = 0; i < partCount; i++) { if (!strcmp(armRobotConfig[i].m_partName, name)) { if (strstr(name, "bone")) { // add a bone and all it children dMatrix matrix; NewtonBody* const limbBody = CreateBodyPart(entity, armRobotConfig[i]); NewtonBodyGetMatrix(limbBody, &matrix[0][0]); NewtonBody* const parentBody = NewtonInverseDynamicsGetBody(m_kinematicSolver, parentJoint); dCustomInverseDynamics* const rotatingColumnHinge = new dCustomInverseDynamics(boneAligmentMatrix * matrix, limbBody, parentBody); rotatingColumnHinge->SetJointTorque(armRobotConfig[i].m_mass * DEMO_GRAVITY * 50.0f); rotatingColumnHinge->SetTwistAngle(armRobotConfig[i].m_minLimit * dDegreeToRad, armRobotConfig[i].m_maxLimit * dDegreeToRad); void* const limbJoint = NewtonInverseDynamicsAddChildNode(m_kinematicSolver, parentJoint, rotatingColumnHinge->GetJoint()); for (DemoEntity* child = entity->GetChild(); child; child = child->GetSibling()) { parentBones[stackIndex] = limbJoint; childEntities[stackIndex] = child; stackIndex++; } } else if (strstr(name, "effector")) { // add effector dMatrix gripperMatrix(entity->CalculateGlobalMatrix()); effector = new dKukaEffector(m_kinematicSolver, parentJoint, baseFrameBody, gripperMatrix); effector->SetAsThreedof(); effector->SetLinearSpeed(2.0f); effector->SetMaxLinearFriction(armRobotConfig[i].m_mass * DEMO_GRAVITY * 50.0f); } break; } } } // create the Animation tree for manipulation DemoEntity* const effectoBone = model->Find("effector_arm"); dMatrix baseFrameMatrix(model->GetCurrentMatrix()); dMatrix effectorLocalMatrix(effectoBone->CalculateGlobalMatrix(model)); dVector upAxis(baseFrameMatrix.UnrotateVector(dVector(0.0f, 1.0f, 0.0f, 1.0f))); dVector planeAxis(baseFrameMatrix.UnrotateVector(dVector(0.0f, 0.0f, 1.0f, 1.0f))); dEffectorTreeFixPose* const fixPose = new dEffectorTreeFixPose(baseFrameBody); m_inputModifier = new dEffectorTreeInputModifier(fixPose, upAxis, planeAxis); m_animTreeNode = new dEffectorTreeRoot(baseFrameBody, m_inputModifier); // set base pose dEffectorTreeInterface::dEffectorTransform frame; frame.m_effector = effector; frame.m_posit = effectorLocalMatrix.m_posit; frame.m_rotation = dQuaternion(effectorLocalMatrix); fixPose->GetPose().Append(frame); m_animTreeNode->GetPose().Append(frame); NewtonInverseDynamicsEndBuild(m_kinematicSolver); }