예제 #1
0
returnValue PlotWindow::addSubplot(	const Expression& _expressionX,
									const Expression& _expressionY,
									const char* const _title,
									const char* const _xLabel,
									const char* const _yLabel,
									PlotMode _plotMode,
									double _xRangeLowerLimit,
									double _xRangeUpperLimit,
									double _yRangeLowerLimit,
									double _yRangeUpperLimit
									)
{

    ASSERT( _expressionX.getDim() == _expressionY.getDim() );

    uint run1;

    for( run1 = 0; run1 < _expressionX.getDim(); run1++ ){

		PlotWindowSubplot* newSubplot = new PlotWindowSubplot(	_expressionX(run1),_expressionY(run1),
																_title,_xLabel,_yLabel,_plotMode,
																_xRangeLowerLimit,_xRangeUpperLimit,
																_yRangeLowerLimit,_yRangeUpperLimit );

		if ( number == 0 )
		{
			first = newSubplot;
			last = newSubplot;
		}
		else
		{
			if ( last->setNext( newSubplot ) != SUCCESSFUL_RETURN )
				return ACADOERROR( RET_OPTIONS_LIST_CORRUPTED );
			last = newSubplot;
		}

        Expression tmpX( _expressionX(run1) );

		if ( addPlotDataItem( &tmpX ) != SUCCESSFUL_RETURN )
			return ACADOERROR( RET_LOG_RECORD_CORRUPTED );

        Expression tmpY( _expressionY(run1) );

		if ( addPlotDataItem( &tmpY ) != SUCCESSFUL_RETURN )
			return ACADOERROR( RET_LOG_RECORD_CORRUPTED );

		++number;
    }

	return SUCCESSFUL_RETURN;
}
예제 #2
0
void quotRemainder(const BigReal &x,  const BigReal &y, BigInt *quotient, BigReal *remainder) {
  DEFINEMETHODNAME;
  if(y.isZero()) {
    throwBigRealInvalidArgumentException(method, _T("Division by zero. (%s / 0)."), x.toString().cstr());
  }
  if(quotient == remainder) { // also takes care of the stupid situation where both are NULL
    throwBigRealInvalidArgumentException(method, _T("quotient is the same variable as remainder"));
  }
  if(quotient == &x || quotient == &y) {
    throwBigRealInvalidArgumentException(method, _T("quotient cannot be the same variable as x or y"));
  }
  if(remainder == &x || remainder == &y) {
    throwBigRealInvalidArgumentException(method, _T("remainder cannot be the same variable as x or y"));
  }

  if(x.isZero()) {
    if(quotient ) quotient->setToZero();
    if(remainder) remainder->setToZero();
    return;
  }

  DigitPool *pool = x.getDigitPool();
  const int cmpAbs = compareAbs(x, y);
  if(cmpAbs < 0) {
    if(remainder) *remainder = x;
    if(quotient)  quotient->setToZero();
    return;
  } else if(cmpAbs == 0) {
    if(remainder) remainder->setToZero();
    if(quotient) {
      *quotient = quotient->getDigitPool()->get1();
    }
    return;
  }

  BigReal tmpX(x);
  tmpX.setPositive();
  BigReal tmpY(y);
  tmpY.setPositive();
  BigInt q   = floor(quot(tmpX, tmpY, modulusC1));
  BigReal mod = tmpX - q * tmpY;

  if(mod.isNegative()) {
    if(remainder) {
      *remainder = mod + tmpY;
    }
    if(quotient) {
      --q;
      *quotient = q;
    }
  } else if(mod >= tmpY) {
    if(remainder) {
      *remainder = mod - tmpY;
    }
    if(quotient) {
      ++q;
      *quotient = q;
    }
  } else {
    if(remainder) {
      *remainder = mod;
    }
    if(quotient) {
      *quotient = q;
    }
  }

  if(remainder && (remainder->m_negative != x.m_negative)) {
    remainder->m_negative = x.m_negative; // sign(x % y) = sign(x), equivalent to build-in % operator
  }
}
예제 #3
0
bool ArmMap::query(const YVector &arm, const YVector &head)
{
	_headKinematics.update(head);
	// _fkinematics.update(arm, head);

	const Y3DVector &cart = _headKinematics.fixationPolar();
	Y3DVector tmp;
	tmp = cart;

	if (!_checkReachability(cart))
		return false;

//	tmp(1) = tmp(1) + __azimuthOffset;
//	tmp(2) = tmp(2) + __elevationOffset;
//	tmp(3) = tmp(3) + __distanceOffset;

	int x, y;	//retinal position (for closed loop)
	x = 128;
	y = 128;

	if (_mode == atnr)
		_command = _atr.query(head) + _noise.query();
	else if (_mode == learning)
	{
//		_nnet.sim(tmp.data(), _command.data());
		_command = _command + _noise.query();
	}
	else
	{

//		_nnet.sim(tmp.data(), _command.data());
		YVector tmpY(3);
		_rfnet.Simulate(tmp, 0.001, tmpY);
		_command = 0.0;
		_command(1) = tmpY(1);
		_command(2) = tmpY(2);
		_command(3) = tmpY(3);

#define USE_JACOBIAN 1
#if USE_JACOBIAN
		_fkinematics.update(_command, head);
		_fkinematics.computeJacobian(x,y);		// compute from center
		YVector tmpArm(6);
	//	tmpArm(1) = _command(1);
	//  tmpArm(2) = _command(2);
	//	tmpArm(3) = _command(3);

//		tmpArm(1) = _command(1);	 	//copy 1 joint from map
//		tmpArm(2) = arm(2);
//		tmpArm(3) = arm(3);
//		tmpArm(4) = arm(4);
//		tmpArm(5) = arm(5);
//		tmpArm(6) = arm(6);

		tmpArm=_command;
		_commandCL = _fkinematics.computeCommandThreshold(tmpArm , x, y);	// to center
#endif
	//	_sendTrajectory();
		
#ifndef TEST_REACHING
			_formTrajectory(_commandCL, _commandCL);
#else
		_formTrajectory(_command, _commandCL);	
#endif
	}

	return true;
}