コード例 #1
0
	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;
	}
コード例 #2
0
	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;
	}
コード例 #3
0
void LoadLumberYardMesh(DemoEntityManager* const scene, const dVector& location, int shapeid)
{
	DemoEntity* const entity = DemoEntity::LoadNGD_mesh ("lumber.ngd", scene->GetNewton(), scene->GetShaderCache());

	dTree<NewtonCollision*, DemoMesh*> filter;
	NewtonWorld* const world = scene->GetNewton();

	dFloat density = 15.0f;

	int defaultMaterialID = NewtonMaterialGetDefaultGroupID(scene->GetNewton());
	for (DemoEntity* child = entity->GetFirst(); child; child = child->GetNext()) {
		DemoMesh* const mesh = (DemoMesh*)child->GetMesh();
		if (mesh) {
			dAssert(mesh->IsType(DemoMesh::GetRttiType()));
			dTree<NewtonCollision*, DemoMesh*>::dTreeNode* node = filter.Find(mesh);
			if (!node) {
				// make a collision shape only for and instance
				dFloat* const array = mesh->m_vertex;
				dVector minBox(1.0e10f, 1.0e10f, 1.0e10f, 1.0f);
				dVector maxBox(-1.0e10f, -1.0e10f, -1.0e10f, 1.0f);

				for (int i = 0; i < mesh->m_vertexCount; i++) {
					dVector p(array[i * 3 + 0], array[i * 3 + 1], array[i * 3 + 2], 1.0f);
					minBox.m_x = dMin(p.m_x, minBox.m_x);
					minBox.m_y = dMin(p.m_y, minBox.m_y);
					minBox.m_z = dMin(p.m_z, minBox.m_z);

					maxBox.m_x = dMax(p.m_x, maxBox.m_x);
					maxBox.m_y = dMax(p.m_y, maxBox.m_y);
					maxBox.m_z = dMax(p.m_z, maxBox.m_z);
				}

				dVector size(maxBox - minBox);
				dMatrix offset(dGetIdentityMatrix());
				offset.m_posit = (maxBox + minBox).Scale(0.5f);
				NewtonCollision* const shape = NewtonCreateBox(world, size.m_x, size.m_y, size.m_z, shapeid, &offset[0][0]);
				node = filter.Insert(shape, mesh);
			}

			// create a body and add to the world
			NewtonCollision* const shape = node->GetInfo();
			dMatrix matrix(child->GetMeshMatrix() * child->CalculateGlobalMatrix());
			matrix.m_posit += location;
			dFloat mass = density * NewtonConvexCollisionCalculateVolume(shape);
			CreateSimpleSolid(scene, mesh, mass, matrix, shape, defaultMaterialID);
		}
	}

	// destroy all shapes
	while (filter.GetRoot()) {
		NewtonCollision* const shape = filter.GetRoot()->GetInfo();
		NewtonDestroyCollision(shape);
		filter.Remove(filter.GetRoot());
	}
	delete entity;
}
コード例 #4
0
	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;
	}
コード例 #5
0
static void AddNonUniformScaledPrimitives(DemoEntityManager* const scene, dFloat mass, const dVector& origin, const dVector& size, int xCount, int zCount, dFloat spacing, PrimitiveType type, int materialID, const dMatrix& shapeOffsetMatrix)
{
	// create the shape and visual mesh as a common data to be re used
	NewtonWorld* const world = scene->GetNewton();
	//     NewtonCollision* const collision = CreateConvexCollision (world, &shapeOffsetMatrix[0][0], size, type, materialID);
	NewtonCollision* const collision = CreateConvexCollision(world, dGetIdentityMatrix(), size, type, materialID);

	dFloat startElevation = 1000.0f;
	dMatrix matrix(dGetIdentityMatrix());
	//matrix = dPitchMatrix(-45.0f * 3.141592f/180.0f);
	for (int i = 0; i < xCount; i++) {
		dFloat x = origin.m_x + (i - xCount / 2) * spacing;
		for (int j = 0; j < zCount; j++) {

			dFloat scalex = 1.0f + 1.5f * (dFloat(dRand()) / dFloat(dRAND_MAX) - 0.5f);
			dFloat scaley = 1.0f + 1.5f * (dFloat(dRand()) / dFloat(dRAND_MAX) - 0.5f);
			dFloat scalez = 1.0f + 1.5f * (dFloat(dRand()) / dFloat(dRAND_MAX) - 0.5f);

			dFloat z = origin.m_z + (j - zCount / 2) * spacing;
			matrix.m_posit.m_x = x;
			matrix.m_posit.m_z = z;
			dVector floor(FindFloor(world, dVector(matrix.m_posit.m_x, startElevation, matrix.m_posit.m_z, 0.0f), 2.0f * startElevation));
			matrix.m_posit.m_y = floor.m_y + 8.0f;

			// create a solid
			//NewtonBody* const body = CreateSimpleSolid (scene, geometry, mass, matrix, collision, materialID);
			NewtonBody* const body = CreateSimpleSolid(scene, NULL, mass, matrix, collision, materialID);

			DemoEntity* entity = (DemoEntity*)NewtonBodyGetUserData(body);
			NewtonCollisionSetScale(collision, scalex, scaley, scalez);
			DemoMesh* const geometry = new DemoMesh("cylinder_1", collision, "smilli.tga", "smilli.tga", "smilli.tga");
			entity->SetMesh(geometry, dGetIdentityMatrix());
			NewtonBodySetCollisionScale(body, scalex, scaley, scalez);

			dVector omega(0.0f);
			NewtonBodySetOmega(body, &omega[0]);

			// release the mesh
			geometry->Release();
		}
	}

	// do not forget to delete the collision
	NewtonDestroyCollision(collision);
}
コード例 #6
0
	void SetCamera()
	{
		if (m_player) {
			DemoEntityManager* const scene = (DemoEntityManager*)NewtonWorldGetUserData(GetWorld());
			DemoCamera* const camera = scene->GetCamera();
			dMatrix camMatrix(camera->GetNextMatrix());

			DemoEntity* player = (DemoEntity*)NewtonBodyGetUserData(m_player->GetBody());
			dMatrix playerMatrix(player->GetNextMatrix());

			dFloat height = 2.0f;
			dVector frontDir(camMatrix[0]);
			dVector upDir(0.0f, 1.0f, 0.0f, 0.0f);
			dVector camOrigin = playerMatrix.TransformVector(upDir.Scale(height));
			camOrigin -= frontDir.Scale(PLAYER_THIRD_PERSON_VIEW_DIST);

			camera->SetNextMatrix(*scene, camMatrix, camOrigin);
		}
	}
コード例 #7
0
	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++;
			}
		}
	}
コード例 #8
0
void ScaledMeshCollision (DemoEntityManager* const scene)
{
	// load the skybox
	scene->CreateSkyBox();

	// load the scene from a ngd file format
	CreateLevelMesh (scene, "flatPlane.ngd", 1);
	//CreateLevelMesh (scene, "flatPlaneDoubleFace.ngd", 1);
	//CreateLevelMesh (scene, "sponza.ngd", 0);
	//CreateLevelMesh (scene, "cattle.ngd", fileName);
	//CreateLevelMesh (scene, "playground.ngd", 0);

	//dMatrix camMatrix (dRollMatrix(-20.0f * 3.1416f /180.0f) * dYawMatrix(-45.0f * 3.1416f /180.0f));
	dMatrix camMatrix (dGetIdentityMatrix());
	dQuaternion rot (camMatrix);
	dVector origin (-15.0f, 5.0f, 0.0f, 0.0f);
	//origin = origin.Scale (0.25f);
	scene->SetCameraMatrix(rot, origin);


	NewtonWorld* const world = scene->GetNewton();
	int defaultMaterialID = NewtonMaterialGetDefaultGroupID (world);
	dVector location (0.0f, 0.0f, 0.0f, 0.0f);

	dMatrix matrix (dGetIdentityMatrix());
	matrix.m_posit = location;
	matrix.m_posit.m_x = 0.0f;
	matrix.m_posit.m_y = 0.0f;
	matrix.m_posit.m_z = 0.0f;
	matrix.m_posit.m_w = 1.0f;

	DemoEntity teaPot (dGetIdentityMatrix(), NULL);
	teaPot.LoadNGD_mesh("teapot.ngd", world);
	//teaPot.LoadNGD_mesh("box.ngd", world);

	NewtonCollision* const staticCollision = CreateCollisionTree (world, &teaPot, 0, true);
	CreateScaleStaticMesh (&teaPot, staticCollision, scene, matrix, dVector (1.0f, 1.0f, 1.0f, 0.0f));
//	CreateScaleStaticMesh (&teaPot, staticCollision, scene, matrix, dVector (0.5f, 0.5f, 2.0f, 0.0f));

	matrix.m_posit.m_z = -5.0f;
	CreateScaleStaticMesh (&teaPot, staticCollision, scene, matrix, dVector (0.5f, 0.5f, 2.0f, 0.0f));

	matrix.m_posit.m_z = 5.0f;
	CreateScaleStaticMesh (&teaPot, staticCollision, scene, matrix, dVector (3.0f, 3.0f, 1.5f, 0.0f));

	matrix.m_posit.m_z = 0.0f;
	matrix.m_posit.m_x = -5.0f;
	CreateScaleStaticMesh (&teaPot, staticCollision, scene, matrix, dVector (0.5f, 0.5f, 0.5f, 0.0f));

	matrix.m_posit.m_x = 5.0f;
	CreateScaleStaticMesh (&teaPot, staticCollision, scene, matrix, dVector (2.0f, 2.0f, 2.0f, 0.0f));

	// do not forget to destroy the collision mesh helper
	NewtonDestroyCollision(staticCollision);

	dVector size0 (1.0f, 1.0f, 1.0f, 0.0f);
	dVector size1 (0.5f, 1.0f, 1.0f, 0.0f);
	dMatrix shapeOffsetMatrix (dRollMatrix(3.141592f/2.0f));

	int count = 3;
	AddNonUniformScaledPrimitives(scene, 10.0f, location, size0, count, count, 5.0f, _SPHERE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
	AddNonUniformScaledPrimitives(scene, 10.0f, location, size0, count, count, 5.0f, _BOX_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
	AddNonUniformScaledPrimitives(scene, 10.0f, location, size0, count, count, 5.0f, _CAPSULE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
	AddNonUniformScaledPrimitives(scene, 10.0f, location, size1, count, count, 5.0f, _CAPSULE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
	AddNonUniformScaledPrimitives(scene, 10.0f, location, size1, count, count, 5.0f, _CYLINDER_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
	AddNonUniformScaledPrimitives(scene, 10.0f, location, size0, count, count, 5.0f, _CYLINDER_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
	AddNonUniformScaledPrimitives(scene, 10.0f, location, size0, count, count, 5.0f, _CHAMFER_CYLINDER_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
	AddNonUniformScaledPrimitives(scene, 10.0f, location, size0, count, count, 5.0f, _CONE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
	AddNonUniformScaledPrimitives(scene, 10.0f, location, size0, count, count, 5.0f, _REGULAR_CONVEX_HULL_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
	AddNonUniformScaledPrimitives(scene, 10.0f, location, size0, count, count, 5.0f, _RANDOM_CONVEX_HULL_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);

	origin.m_x -= 4.0f;
	origin.m_y += 1.0f;
	scene->SetCameraMatrix(rot, origin);	
}
コード例 #9
0
NewtonCollision* CreateCollisionTree (NewtonWorld* world, DemoEntity* const entity, int materialID, bool optimize)
{
	// measure the time to build a collision tree
	unsigned64 timer0 = dGetTimeInMicrosenconds();

	// create the collision tree geometry
	NewtonCollision* collision = NewtonCreateTreeCollision(world, materialID);

	// set the application level callback
#ifdef USE_STATIC_MESHES_DEBUG_COLLISION
	NewtonStaticCollisionSetDebugCallback (collision, ShowMeshCollidingFaces);
#endif

	// prepare to create collision geometry
	NewtonTreeCollisionBeginBuild(collision);

	// iterate the entire geometry an build the collision
	for (DemoEntity* model = entity->GetFirst(); model; model = model->GetNext()) {

		dMatrix matrix (model->GetMeshMatrix() * model->CalculateGlobalMatrix(entity));
		DemoMesh* const mesh = (DemoMesh*)model->GetMesh();
		dAssert (mesh->IsType(DemoMesh::GetRttiType()));

		dFloat* const vertex = mesh->m_vertex;
		for (DemoMesh::dListNode* nodes = mesh->GetFirst(); nodes; nodes = nodes->GetNext()) {
			DemoSubMesh& segment = nodes->GetInfo();
			int matID = segment.m_textureHandle;
			for (int i = 0; i < segment.m_indexCount; i += 3) {
				int index;	
				dVector face[3];

				index = segment.m_indexes[i + 0] * 3;
				face[0] = dVector (vertex[index + 0], vertex[index + 1], vertex[index + 2]);

				index = segment.m_indexes[i + 1] * 3;
				face[1] = dVector (vertex[index + 0], vertex[index + 1], vertex[index + 2]);

				index = segment.m_indexes[i + 2] * 3;
				face[2] = dVector (vertex[index + 0], vertex[index + 1], vertex[index + 2]);

				matrix.TransformTriplex (&face[0].m_x, sizeof (dVector), &face[0].m_x, sizeof (dVector), 3);

				// use material ids as physics materials 
				NewtonTreeCollisionAddFace(collision, 3, &face[0].m_x, sizeof (dVector), matID);
			}
		}
	}
	NewtonTreeCollisionEndBuild(collision, optimize ? 1 : 0);


	// test Serialization
#if 0
	FILE* file = fopen ("serialize.bin", "wb");
	NewtonCollisionSerialize (world, collision, DemoEntityManager::SerializeFile, file);
	fclose (file);
	NewtonDestroyCollision (collision);

	file = fopen ("serialize.bin", "rb");
	collision = NewtonCreateCollisionFromSerialization (world, DemoEntityManager::DeserializeFile, file);
	fclose (file);
#endif	



	// measure the time to build a collision tree
	timer0 = (dGetTimeInMicrosenconds() - timer0) / 1000;

	return collision;
}
コード例 #10
0
	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;
	}
コード例 #11
0
	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);
	}