uint16_t BipedController::CalculateLinearPath(Point3D * leftFootTarget, Point3D * rightFootTarget){
	uint16_t resultActions = 0;
	int16_t difx_l, dify_l, difz_l;
	int16_t xinc_l, yinc_l, zinc_l;
	int16_t xxx_l, yyy_l, zzz_l;
	double hinc_l, hhh_l;
	int16_t difx_r, dify_r, difz_r;
	int16_t xinc_r, yinc_r, zinc_r;
	int16_t xxx_r, yyy_r, zzz_r;
	double hinc_r, hhh_r;
	uint16_t maxSteps_l = 0, maxSteps_r = 0, maxSteps = 0;
	uint8_t i;
	
	// avoid null pointers
	if(!leftFootTarget || !rightFootTarget)
		return 0;
	
	// check if first invocation in a multi-path calculation
	if(!this->keepBresenhamCalculations){
		this->lastLeftFoot->CopyFrom(this->leftFoot);
		this->lfootPath->numSteps = 0;
		this->lastRightFoot->CopyFrom(this->rightFoot);
		this->rfootPath->numSteps = 0;
	}

	// if movement in one step (no bresenham calculation)	
	if(!this->stepDistance){
		this->lfootPath->path[this->lfootPath->numSteps]->x = leftFootTarget->x;
		this->lfootPath->path[this->lfootPath->numSteps]->y = leftFootTarget->y;
		this->lfootPath->path[this->lfootPath->numSteps]->z = leftFootTarget->z;
		this->lfootPath->path[this->lfootPath->numSteps]->heading = leftFootTarget->heading;
		this->lfootPath->numSteps++;
		this->rfootPath->path[this->rfootPath->numSteps]->x = rightFootTarget->x;
		this->rfootPath->path[this->rfootPath->numSteps]->y = rightFootTarget->y;
		this->rfootPath->path[this->rfootPath->numSteps]->z = rightFootTarget->z;
		this->rfootPath->path[this->rfootPath->numSteps]->heading = rightFootTarget->heading;
		this->rfootPath->numSteps++;
		
		if(this->lfootPath->numSteps > resultActions){
			resultActions = this->lfootPath->numSteps;
		}
		if(this->rfootPath->numSteps > resultActions){
			resultActions = this->rfootPath->numSteps;
		}
		this->keepBresenhamCalculations = 1;
		return resultActions;
	}		
	
	// left effector calculation
	xinc_l = (10*(leftFootTarget->x - this->lastLeftFoot->x))/this->stepDistance;
	yinc_l = (10*(leftFootTarget->y - this->lastLeftFoot->y))/this->stepDistance;
	zinc_l = (10*(leftFootTarget->z - this->lastLeftFoot->z))/this->stepDistance;
	difx_l = (xinc_l < 0)? (-xinc_l) : xinc_l;
	dify_l = (yinc_l < 0)? (-yinc_l) : yinc_l;
	difz_l = (zinc_l < 0)? (-zinc_l) : zinc_l;
	maxSteps_l = (uint16_t)((difx_l > (int16_t)maxSteps_l)? difx_l : maxSteps_l);
	maxSteps_l = (uint16_t)((dify_l > (int16_t)maxSteps_l)? dify_l : maxSteps_l);
	maxSteps_l = (uint16_t)((difz_l > (int16_t)maxSteps_l)? difz_l : maxSteps_l);
	maxSteps_l = maxSteps_l/10;
	
	// right effector calculation
	xinc_r = (10*(rightFootTarget->x - this->lastRightFoot->x))/this->stepDistance;
	yinc_r = (10*(rightFootTarget->y - this->lastRightFoot->y))/this->stepDistance;
	zinc_r = (10*(rightFootTarget->z - this->lastRightFoot->z))/this->stepDistance;
	difx_r = (xinc_r < 0)? (-xinc_r) : xinc_r;
	dify_r = (yinc_r < 0)? (-yinc_r) : yinc_r;
	difz_r = (zinc_r < 0)? (-zinc_r) : zinc_r;
	maxSteps_r = (uint16_t)((difx_r > (int16_t)maxSteps_r)? difx_r : maxSteps_r);
	maxSteps_r = (uint16_t)((dify_r > (int16_t)maxSteps_r)? dify_r : maxSteps_r);
	maxSteps_r = (uint16_t)((difz_r > (int16_t)maxSteps_r)? difz_r : maxSteps_r);
	maxSteps_r = maxSteps_r/10;
	
	// get the max number of steps (of both legs)
	maxSteps = maxSteps_r;
	if(maxSteps_l > maxSteps_r)
		maxSteps = maxSteps_l;

	// calculate left min. step
	xinc_l = (10*(leftFootTarget->x - this->lastLeftFoot->x))/maxSteps;
	yinc_l = (10*(leftFootTarget->y - this->lastLeftFoot->y))/maxSteps;
	zinc_l = (10*(leftFootTarget->z - this->lastLeftFoot->z))/maxSteps;
	hinc_l = (leftFootTarget->heading - this->lastLeftFoot->heading)/maxSteps;
	//scaling to get decs of mm.
	xxx_l = 10*this->lastLeftFoot->x;
	yyy_l = 10*this->lastLeftFoot->y;
	zzz_l = 10*this->lastLeftFoot->z;
	hhh_l = this->lastLeftFoot->heading;

	// calculate right min. step
	xinc_r = (10*(rightFootTarget->x - this->lastRightFoot->x))/maxSteps;
	yinc_r = (10*(rightFootTarget->y - this->lastRightFoot->y))/maxSteps;
	zinc_r = (10*(rightFootTarget->z - this->lastRightFoot->z))/maxSteps;
	hinc_r = (leftFootTarget->heading - this->lastRightFoot->heading)/maxSteps;
	//scaling to get decs of mm.
	xxx_r = 10*this->lastRightFoot->x;
	yyy_r = 10*this->lastRightFoot->y;
	zzz_r = 10*this->lastRightFoot->z;
	hhh_r = this->lastRightFoot->heading;
	
	// if more data than buffer availability, raise error
	if( (this->lfootPath->numSteps+maxSteps) > BipedController_MAX_MOVEMENT_STEPS || 
		(this->rfootPath->numSteps+maxSteps) > BipedController_MAX_MOVEMENT_STEPS){
		this->keepBresenhamCalculations = 0;		
		return 0;
	}					
  
	// compute path steps for both effectors
	for(i=0; i<maxSteps; i++){
		xxx_l+=xinc_l;
		yyy_l+=yinc_l;
		zzz_l+=zinc_l;
		hhh_l+=hinc_l;
		xxx_r+=xinc_r;
		yyy_r+=yinc_r;
		zzz_r+=zinc_r;
		hhh_r+=hinc_r;
		// store step
		this->lfootPath->path[this->lfootPath->numSteps]->x = xxx_l/10;
		this->lfootPath->path[this->lfootPath->numSteps]->y = yyy_l/10;
		this->lfootPath->path[this->lfootPath->numSteps]->z = zzz_l/10;
		this->lfootPath->path[this->lfootPath->numSteps]->heading = hhh_l;
		this->lfootPath->numSteps++;	
		this->rfootPath->path[this->rfootPath->numSteps]->x = xxx_r/10;
		this->rfootPath->path[this->rfootPath->numSteps]->y = yyy_r/10;
		this->rfootPath->path[this->rfootPath->numSteps]->z = zzz_r/10;
		this->rfootPath->path[this->rfootPath->numSteps]->heading = hhh_r;
		this->rfootPath->numSteps++;	
	}
	
	// adjust min tolerances
	if( this->lfootPath->path[this->lfootPath->numSteps-1]->x != leftFootTarget->x ||
		this->lfootPath->path[this->lfootPath->numSteps-1]->y != leftFootTarget->y ||
		this->lfootPath->path[this->lfootPath->numSteps-1]->z != leftFootTarget->z ||
		this->lfootPath->path[this->lfootPath->numSteps-1]->heading != leftFootTarget->heading){
		// store target point 
		this->lfootPath->path[this->lfootPath->numSteps]->x = leftFootTarget->x;
		this->lfootPath->path[this->lfootPath->numSteps]->y = leftFootTarget->y;
		this->lfootPath->path[this->lfootPath->numSteps]->z = leftFootTarget->z;
		this->lfootPath->path[this->lfootPath->numSteps]->heading = leftFootTarget->heading;
		this->lfootPath->numSteps++;						
	}					
	if( this->rfootPath->path[this->rfootPath->numSteps-1]->x != rightFootTarget->x ||
		this->rfootPath->path[this->rfootPath->numSteps-1]->y != rightFootTarget->y ||
		this->rfootPath->path[this->rfootPath->numSteps-1]->z != rightFootTarget->z ||
		this->rfootPath->path[this->rfootPath->numSteps-1]->heading != rightFootTarget->heading){
		// store target point
		this->rfootPath->path[this->rfootPath->numSteps]->x = rightFootTarget->x;
		this->rfootPath->path[this->rfootPath->numSteps]->y = rightFootTarget->y;
		this->rfootPath->path[this->rfootPath->numSteps]->z = rightFootTarget->z;
		this->rfootPath->path[this->rfootPath->numSteps]->heading = rightFootTarget->heading;
		this->rfootPath->numSteps++;						
	}					
	if(this->lfootPath->numSteps > resultActions){
		resultActions = this->lfootPath->numSteps;
	}		
	if(this->rfootPath->numSteps > resultActions){
		resultActions = this->rfootPath->numSteps;
	}		
   
   // update state-machine for multi-path calculation
	this->lastLeftFoot->CopyFrom(leftFootTarget);						
	this->lastRightFoot->CopyFrom(rightFootTarget);						
	this->keepBresenhamCalculations = 1;   
	return resultActions;
}