int SkeletonFitting::funcSize(void* p, int m, int n, const double* x, double* fvec, int iflag)
	{
		if (!p)
			throw Exception("invalid data");

		Payload* thisPayload = (Payload*)p;
		Joint* joint = (JointHinge*)thisPayload->joint;
				
		// compute energy
		joint->addBoneSize(x[0]);
		joint->updateForward(false);
		fvec[0] = joint->computeForwardEnergy();

		// reset
		joint->addBoneSize(-x[0]);
		joint->updateForward(false);
		
		return iflag;
	}
	int SkeletonFitting::funcPos(void* p, int m, int n, const double* x, double* fvec, int iflag)
	{
		if (!p)
			throw Exception("invalid data");

		Payload* thisPayload = (Payload*)p;
		Joint* joint = thisPayload->joint;
		
		// store state
		cv::Point3d pos = joint->getPos3d();
		
		switch (thisPayload->axis) {
		case 0:
			joint->addPos3d(cv::Point3d(x[0], 0, 0));
			break;
		case 1:
			joint->addPos3d(cv::Point3d(0, x[0], 0));
			break;
		case 2:
			joint->addPos3d(cv::Point3d(0, 0, x[0]));
			break;
		default:
				throw Exception("invalid parameter");
			break;
		}

		// compute energy
		joint->updateForward(false);
		fvec[0] = joint->computeForwardEnergy();

		// reset
		joint->setPos3d(pos);
		joint->updateForward(false);
		
		return iflag;
	}