Example #1
0
Matrix f_l(Matrix const& P, Matrix const& E, float k) {
  Matrix F;
  for(Matrix::size_type p=1;p<=P.num_rows();++p) {
    for(Matrix::size_type i=1;i<=E.num_rows();++i) {
      Matrix::size_type a = (Matrix::size_type)E[i][1];
      Matrix::size_type b = (Matrix::size_type)E[i][2];
      if ( a == p || b == p ) { continue; }
      Vector<float> ab = P[b] - P[a];
      float abl = ab.norm();
      Vector<float> ap = P[p] - P[a];
      float t = ( (ab[1]*ap[1]) + (ab[2]*ap[2]) ) / abl;
      if ( t > 0 && t < abl ) {
        
        Vector<float> x = P[a];
        x += (ab * (t / abl));
        Vector<float> d = P[p] - x;

        float dl = d.norm();
        if ( dl > 0.01 ) {
          F[p] += d * ((k * k) / (dl * dl * dl));
          //          F[a] += d * (- (k * k) / (dl * dl));
          //          F[b] += d * (- (k * k) / (dl * dl));
        } else {
          F[p][1] += k * k * (drand48() - 0.5);
          F[p][2] += k * k * (drand48() - 0.5);
        }      
      }
    }
  }
  return F;
}
Example #2
0
inline Vector Vector::intersection(const Vector &b) const
{
	Vector a = *this;
	if (a.normal() == b.normal())
		return (a.norm() <= b.norm()) ? a : b;
	return Vector();
}
Example #3
0
Vector Vector::v_union(const Vector &b) const
{
	Vector a = *this;
	if (a.normal() == b.normal())
		return (a.norm() >= b.norm()) ? a : b;
	return Vector();
}
Example #4
0
void
WillOWisp::active_update(float elapsed_time)
{
  Player* player = get_nearest_player();
  if (!player) return;
  Vector p1 = bbox.get_middle();
  Vector p2 = player->get_bbox().get_middle();
  Vector dist = (p2 - p1);

  switch(mystate) {
    case STATE_STOPPED:
      break;

    case STATE_IDLE:
      if (dist.norm() <= track_range) {
        mystate = STATE_TRACKING;
      }
      break;

    case STATE_TRACKING:
      if (dist.norm() > vanish_range) {
        vanish();
      } else if (dist.norm() >= 1) {
        Vector dir_ = dist.unit();
        movement = dir_ * elapsed_time * flyspeed;
      } else {
        /* We somehow landed right on top of the player without colliding.
         * Sit tight and avoid a division by zero. */
      }
      sound_source->set_position(get_pos());
      break;

    case STATE_WARPING:
      if(sprite->animation_done()) {
        remove_me();
      }

    case STATE_VANISHING: {
      Vector dir_ = dist.unit();
      movement = dir_ * elapsed_time * flyspeed;
      if(sprite->animation_done()) {
        remove_me();
      }
      break;
    }

    case STATE_PATHMOVING:
    case STATE_PATHMOVING_TRACK:
      if(walker.get() == NULL)
        return;
      movement = walker->advance(elapsed_time) - get_pos();
      if(mystate == STATE_PATHMOVING_TRACK && dist.norm() <= track_range) {
        mystate = STATE_TRACKING;
      }
      break;

    default:
      assert(false);
  }
}
Example #5
0
template<typename Scalar> void lines()
{
  typedef Hyperplane<Scalar, 2> HLine;
  typedef ParametrizedLine<Scalar, 2> PLine;
  typedef Matrix<Scalar,2,1> Vector;
  typedef Matrix<Scalar,3,1> CoeffsType;

  for(int i = 0; i < 10; i++)
  {
    Vector center = Vector::Random();
    Vector u = Vector::Random();
    Vector v = Vector::Random();
    Scalar a = ei_random<Scalar>();
    while (ei_abs(a-1) < 1e-4) a = ei_random<Scalar>();
    while (u.norm() < 1e-4) u = Vector::Random();
    while (v.norm() < 1e-4) v = Vector::Random();

    HLine line_u = HLine::Through(center + u, center + a*u);
    HLine line_v = HLine::Through(center + v, center + a*v);

    // the line equations should be normalized so that a^2+b^2=1
    VERIFY_IS_APPROX(line_u.normal().norm(), Scalar(1));
    VERIFY_IS_APPROX(line_v.normal().norm(), Scalar(1));

    Vector result = line_u.intersection(line_v);

    // the lines should intersect at the point we called "center"
    VERIFY_IS_APPROX(result, center);

    // check conversions between two types of lines
    CoeffsType converted_coeffs(HLine(PLine(line_u)).coeffs());
    converted_coeffs *= line_u.coeffs()(0)/converted_coeffs(0);
    VERIFY(line_u.coeffs().isApprox(converted_coeffs));
  }
}
void Objective<Real>::hessVec( Vector<Real> &hv, const Vector<Real> &v, const Vector<Real> &x, Real &tol ) {
  // Get Step Length
  if ( v.norm() == 0. ) {
    hv.zero();
  }
  else {
    Real gtol = std::sqrt(ROL_EPSILON);

    Real h = std::max(1.0,x.norm()/v.norm())*tol;
    //Real h = 2.0/(v.norm()*v.norm())*tol;

    // Compute Gradient at x
    Teuchos::RCP<Vector<Real> > g = hv.clone();
    this->gradient(*g,x,gtol);

    // Compute New Step x + h*v
    Teuchos::RCP<Vector<Real> > xnew = x.clone();
    xnew->set(x);
    xnew->axpy(h,v);  
    this->update(*xnew);

    // Compute Gradient at x + h*v
    hv.zero();
    this->gradient(hv,*xnew,gtol);
    
    // Compute Newton Quotient
    hv.axpy(-1.0,*g);
    hv.scale(1.0/h);
  }
} 
Example #7
0
double Line::sqrDistance(const Vector& p) const {
    Vector diff = p - a;
    Vector dir = getDirection();
    double sqrlen = dir.norm();
    double t = (diff * dir ) / sqrlen;
    diff = diff - t * dir;
    return diff.norm();
}
Example #8
0
int ccw(Point p0, Point p1, Point p2) {
  Vector a = p1 - p0;
  Vector b = p2 - p0;
  if (cross(a, b) > EPS) return COUNTER_CLOCKWISE;
  if (cross(a, b) < -EPS) return CLOCKWISE;
  if (dot(a, b) < -EPS) return ONLINE_BACK;
  if (a.norm() < b.norm()) return ONLINE_FRONT;
  return ON_SEGMENT;
}
Example #9
0
void EdgeExtremityGlyph::get2DTransformationMatrix(const Coord &src, const Coord &dest, const Size &glyphSize, MatrixGL& transformationMatrix, MatrixGL& scalingMatrix) {
  //  Vecteur AB
  Vector<float, 3> vAB;
  //Vecteur V
  Vector<float, 3> vV;
  //Vecteur W
  Vector<float, 3> vW;

  vAB = dest - src;
  float nAB; //|AB|
  nAB = vAB.norm();

  if (fabs(nAB) > 1E-6)
    vAB /= nAB;

  //vAB * vV = xAB * xV + yAB*yV + zAB * zV = |AB| * |V| * cos(alpha) = 0;
  if (fabs(vAB[2]) < 1E-6) {
    vV[0] = 0;
    vV[1] = 0;
    vV[2] = 1.0;
  }
  else if (fabs(vAB[1]) < 1E-6) {
    vV[0] = 0;
    vV[1] = 1.0;
    vV[2] = 0;
  }
  else {
    vV[0] = 0;
    vV[1] = 1. / vAB[1];
    vV[2] = -1. / vAB[2];
    vV /= vV.norm();
  }

  vW = vAB ^ vV;
  float nW = vW.norm();

  if (fabs(nW) > 1E-6)
    vW /= nW;

  for (unsigned int i = 0; i < 3; ++i) {
    transformationMatrix[0][i] = vAB[i];
    transformationMatrix[1][i] = vW[i];
    transformationMatrix[2][i] = vV[i];
    transformationMatrix[3][i] = dest[i] + (-glyphSize[0] * .5 * vAB[i]);
  }

  transformationMatrix[0][3] = 0;
  transformationMatrix[1][3] = 0;
  transformationMatrix[2][3] = 0;
  transformationMatrix[3][3] = 1;

  scalingMatrix.fill(0);
  scalingMatrix[0][0] = glyphSize[0];
  scalingMatrix[1][1] = glyphSize[1];
  scalingMatrix[2][2] = glyphSize[2];
  scalingMatrix[3][3] = 1;
}
Example #10
0
// Get the angle between this vector and v in degrees.
double Vector::angleTo(const Vector &v) const
{
	double rad;
	Vector u = *this;
	double dot;
	
	dot = (u * v) / (u.norm() * v.norm());
	rad = acos(dot);
	if (v.y > u.y)
		rad = 2*M_PI - rad;

	return rad * 180 / M_PI;
}
Example #11
0
Matrix matrixOrient(const Vector &direction,const Vector &up)
{
	assert(direction.norm()>0.0);
	assert(up.norm()>0.0);

	Vector d(direction);
	d.normalize();

	Vector u(up);
	u.normalize();

	return matrixOrient(xProduct(u,d),u,d);
}
Vector<Scalar,Dim> PiecewiseCubicSpline<Scalar,Dim>::gradient(const Vector<Scalar,Dim> &center_to_x, Scalar R) const
{
    PHYSIKA_ASSERT(R > 0);
    Scalar a = 1.0;
    Scalar h = 0.5*R;
    switch(Dim)
    {
    case 2:
        a = 15.0/(7*PI*h*h);
        break;
    case 3:
        a = 3.0/(2*PI*h*h*h);
        break;
    default:
        PHYSIKA_ERROR("Wrong dimension specified.");
    }
    Scalar r = center_to_x.norm();
    Vector<Scalar,Dim> direction(0);
    if(r>std::numeric_limits<Scalar>::epsilon())
        direction = center_to_x / r;
    else
        direction[0] = 1.0;//set direction to x axis when x is at center
    Scalar s = r/h;
    if(s>2)
        return 0*direction;
    else if(s>=1)
        return a*(2-s)*(2-s)*(-1)/(2.0*h)*direction;
    else if(s>=0)
        return a*(-2.0*s/h+3.0/2*s*s/h)*direction;
    else
        PHYSIKA_ERROR("r/R must be greater than zero.");
}
Example #13
0
double
Vector::angle(const Vector& vector) const throw (Exception)
{
	if (norm() * vector.norm() == 0) {
		throw Exception(__FILE__, __LINE__, "Error, nullvector has no direction ergo no angle.");
    } else {
		double cos_angle = (((*this) * vector) / (this->norm() * vector.norm()));
		if (cos_angle >= 1){ 
			return 0; 
		} else if (cos_angle <= -1){ 
			return M_PI;
		} else {		
	      	return acos(cos_angle);
	    }
    }
}
bool inter_empty(double tmp) {
    bool ok = false;
    // centros
    for (int i = 0; !ok && i < n; i++) 
        if (achieve(p[i], tmp)) ok = true;

    // intersecções dois a dois
    for (int i = 0; !ok && i < n; i++) {
        for (int j = 0; !ok && j < n; j++) {
            if (i == j) continue;

            double r1 = tmp * s[i];
            double r2 = tmp * s[j];
            double R = max(r1, r2);
            double r = min(r1, r2);

            Vector vd = p[j] - p[i];
            double d = vd.norm();

            if (r1 + r2 - d > eps && d + r - R > eps) {
                double alpha = acos((d * d + r1 * r1 - r2 * r2) / (2 * d * r1));

                Vector vr = vd / d * r1;

                Point inter = p[i] + vr.rotate(alpha);
                if (achieve(inter, tmp)) ok = true;
                inter = p[i] + vr.rotate(-alpha);
                if (!ok && achieve(inter, tmp)) ok = true;
            }
        }
    }

    return ok;
}
int main() {
#ifdef LOCAL
    freopen("C:\\Users\\TaoSama\\Desktop\\in.txt", "r", stdin);
//  freopen("C:\\Users\\TaoSama\\Desktop\\out.txt","w",stdout);
#endif
//    ios_base::sync_with_stdio(0);

    while(scanf("%d", &n) == 1 && n) {
        for(int i = 1; i <= n; ++i) ps[i].read();

        int ans = 1;
        for(int i = 1; i <= n; ++i) {
            for(int j = i + 1; j <= n; ++j) {
                if(i == j) continue;

                Vector AB = ps[j] - ps[i]; //B-A=AB
                double c = AB.length();
                if(sgn(c - 2) > 0) continue;

                double cosTheta = (c * c + 1 - 1) / (2 * c);
                double theta = acos(cosTheta);
                Vector AO = AB.rotate(theta); AO.norm();
                Point O = ps[i] + AO;

                int cnt = 0;
                for(int k = 1; k <= n; ++k)
                    if(sgn((ps[k] - O).length() - 1) <= 0) ++cnt;
                ans = max(ans, cnt);
            }
        }
        printf("%d\n", ans);
    }

    return 0;
}
Example #16
0
int KMeans:: get_closest_center_id( const Vector &X)
{
	double dis = 10000000.0;  //
	int pid ;
	for(  int j = 0 ; j < k ; j++ )
	{
		Vector temp = X;//
		temp.sub( cluster_centers[ j ] );
		if( temp.norm()< dis )
		{
			dis = temp.norm() ;	
			pid = j;
		}
	}
	return pid;
}
Example #17
0
  void update( Vector<Real> &x, const Vector<Real> &s,
               Objective<Real> &obj, BoundConstraint<Real> &bnd,
               AlgorithmState<Real> &algo_state ) {
    Real tol = std::sqrt(ROL_EPSILON<Real>());
    Teuchos::RCP<StepState<Real> > step_state = Step<Real>::getState();

    // Update iterate
    algo_state.iter++;
    x.plus(s);
    (step_state->descentVec)->set(s);
    algo_state.snorm = s.norm();

    // Compute new gradient
    if ( useSecantPrecond_ ) {
      gp_->set(*(step_state->gradientVec));
    }
    obj.update(x,true,algo_state.iter);
    if ( computeObj_ ) {
      algo_state.value = obj.value(x,tol);
      algo_state.nfval++;
    }
    obj.gradient(*(step_state->gradientVec),x,tol);
    algo_state.ngrad++;

    // Update Secant Information
    if ( useSecantPrecond_ ) {
      secant_->updateStorage(x,*(step_state->gradientVec),*gp_,s,algo_state.snorm,algo_state.iter+1);
    }

    // Update algorithm state
    (algo_state.iterateVec)->set(x);
    algo_state.gnorm = step_state->gradientVec->norm();
  }
Photon::Photon(Point pos, Vector dir, RGBColor color){
    int er, eg, eb, em, cr, cg, cb, i, d1, d2;
    double sr, sg, sb;
    x = pos.x;
    y = pos.y;
    z = pos.z;
    sr = frexp(color.r, &er);
    sg = frexp(color.g, &eg);
    sb = frexp(color.b, &eb);
    em = (er > eg ? (er > eb ? er : eb) : (eg > eb ? eg : eb));
    em = em > 127 ? 127 : em < -128 ? -128 : em;
    for (i = er; i < em; i++)
        sr /= 2;
    for (i = eg; i < em; i++)
        sg /= 2;
    for (i = eb; i < em; i++)
        sb /= 2;
    cr = sr * 256 - 0.5;
    cr = cr > 255 ? 255 : cr < 0 ? 0 : cr;
    cg = sg * 256 - 0.5;
    cg = cg > 255 ? 255 : cg < 0 ? 0 : cg;
    cb = sb * 256 - 0.5;
    cb = cb > 255 ? 255 : cb < 0 ? 0 : cb;
    r = cr;
    g = cg;
    b = cb;
    e = em;
    d1 = (atan2(dir.y, dir.x) * INVPI * 0.5 + 0.5) * 256 - 0.5;
    d1 = d1 > 255 ? 255 : d1 < 0 ? 0 : d1;
    d2 = (asin(dir.z / dir.norm()) * INVPI + 0.5) * 256 - 0.5;
    d2 = d2 > 255 ? 255 : d2 < 0 ? 0 : d2;
    theta = d1;
    phi = d2;
}
Example #19
0
void GramSchmidt::add(const Vector & rhs)
{
  Vector ortho = projectOrthogonal(rhs);

  if ( zeroVectorChecker.isZero( ortho.norm() ) )
    {
      cerr << "GramSchmidt projectiong norm is: " << ortho.norm() << endl;
      cerr << "\ttable had " << numRows() << " vectors and this was added: " << endl;
      cerr << rhs << endl << endl;

      throw ZeroVectorException();
    }

  Matrix::add( (1/ortho.norm()) * ortho);
  original.add(rhs);
}
Scalar PiecewiseCubicSpline<Scalar,Dim>::weight(const Vector<Scalar,Dim> &center_to_x, Scalar R) const
{
    PHYSIKA_ASSERT(R > 0);
    Scalar a = 1.0;
    Scalar h = 0.5*R;
    switch(Dim)
    {
    case 2:
        a = 15.0/(7*PI*h*h);
        break;
    case 3:
        a = 3.0/(2*PI*h*h*h);
        break;
    default:
        PHYSIKA_ERROR("Wrong dimension specified.");
    }
    Scalar r = center_to_x.norm();
    Scalar s = r/h;
    if(s>2)
        return 0;
    else if(s>=1)
        return a*(2.0-s)*(2.0-s)*(2.0-s)/6.0;
    else if(s>=0)
        return a*(2.0/3.0-s*s+1.0/2.0*s*s*s);
    else
        PHYSIKA_ERROR("r/R must be greater than zero.");
}
Scalar PiecewiseCubicSpline<Scalar,Dim>::laplacian(const Vector<Scalar,Dim> &center_to_x, Scalar R) const
{
    PHYSIKA_ASSERT(R > 0);
    Scalar a = 1.0;
    Scalar h = 0.5*R;
    switch(Dim)
    {
    case 2:
        a = 15.0/(7*PI*h*h);
        break;
    case 3:
        a = 3.0/(2*PI*h*h*h);
        break;
    default:
        PHYSIKA_ERROR("Wrong dimension specified.");
    }
    Scalar r = center_to_x.norm();
    Scalar s = r/h;
    if(s>2)
        return 0;
    else if(s>=1)
    {
        Scalar result = a/(-2*h)*(Dim*(4.0/r-4.0/h+r/(h*h))-4.0/r+r/(h*h));
        return result;
    }
    else if(s>=0)
    {
        Scalar result = a*(Dim*(-2.0/(h*h)+3.0/(2.0*h*h*h)*r)+3.0/(2.0*h*h*h)*r);
        return result;
    }
    else
        PHYSIKA_ERROR("r/R must be greater than zero.");
}
Example #22
0
bool IterativeMethod::Solve(Type type,Vector& x0,int& maxIters,Real& tol) const
{
  bool valid=false;
  switch(type) {
  case Jacobi: valid=IsValid_Jacobi(); break;
  case GaussSeidel: valid=IsValid_GaussSeidel(); break;
  case SOR: valid=IsValid_SOR(); break;
  default: AssertNotReached();
  }
  if(!valid) {
    cout<<"Warning: matrix in IterativeMethod::Solve() won't guarantee convergence"<<endl;
  }

  Vector r;
  for(int i=0;i<maxIters;i++) {
    switch(type) {
    case Jacobi: Iterate_Jacobi(x0); break;
    case GaussSeidel: Iterate_GaussSeidel(x0); break;
    case SOR: Iterate_SOR(x0); break;
    default: AssertNotReached();
    }

    //calculate residual
    r.setNegative(b); A.madd(x0,r);
    Real rnorm=r.norm();
    if(rnorm <= Sqr(tol)) {
      tol = rnorm;
      maxIters = i;
      return true;
    }
  }
  return false;
}
static void cb_mouse (double px, double py, int flags)
{
  if (flags & gfx::MOUSE_PRESS) {
    for (handle_s ** hh (handle); *hh != 0; ++hh) {
      Vector offset ((*hh)->point_);
      offset[0] -= px;
      offset[1] -= py;
      if (offset.norm() <= (*hh)->radius_) {
        grab_offset = offset;
        grabbed = *hh;
        break;
      }
    }
  }
  else if (flags & gfx::MOUSE_DRAG) {
    if (0 != grabbed) {
      grabbed->point_[0] = px;
      grabbed->point_[1] = py;
      grabbed->point_ += grab_offset;
    }
  }
  else if (flags & gfx::MOUSE_RELEASE) {
    grabbed = 0;
  }
}
Example #24
0
Matrix matrixRotate(const Vector axis, const double angle)
{
	Matrix rotate;

	// Page 466, Graphics Gems

	double s = sin(angle*M_PI_DEG);
	double c = cos(angle*M_PI_DEG);
	double t = 1 - c;

	Vector ax = axis/sqrt(axis.norm());

	double x = ax[0];
	double y = ax[1];
	double z = ax[2];

	rotate.set(0,0,t*x*x+c);
	rotate.set(1,0,t*y*x+s*z);
	rotate.set(2,0,t*z*x-s*y);

	rotate.set(0,1,t*x*y-s*z);
	rotate.set(1,1,t*y*y+c);
	rotate.set(2,1,t*z*y+s*x);

	rotate.set(0,2,t*x*z+s*y);
	rotate.set(1,2,t*y*z-s*x);
	rotate.set(2,2,t*z*z+c);

	return rotate;
}
Example #25
0
File: core.cpp Project: bo0ts/mango
 /**
  * Move the camera to a given position, but reorient it so that
  * the position it is pointing at does not change.
  *
  * @see lookAt
  */
 void CoreCamera::lookFrom(Vector from_point){
   GLfloat alef, bet;
   Vector v(0, 0, from_point.norm());
   position = v;
   focus_frame->orientationFor(from_point, alef, bet);
   focus_frame->setOrientation(alef, bet - 90, 0);
 }
Example #26
0
void
InfoBlock::update(float delta)
{
  Block::update(delta);

  if (delta == 0) return;

  // hide message if player is too far away
  if (dest_pct > 0) {
    Player* player = get_nearest_player();
    if (player) {
      Vector p1 = bbox.get_middle();
      Vector p2 = player->get_bbox().get_middle();
      Vector dist = (p2 - p1);
      float d = dist.norm();
      if (d > 128) dest_pct = 0;
    }
  }

  // handle soft fade-in and fade-out
  if (shown_pct != dest_pct) {
    if (dest_pct > shown_pct) shown_pct = std::min(shown_pct + 2*delta, dest_pct);
    if (dest_pct < shown_pct) shown_pct = std::max(shown_pct - 2*delta, dest_pct);
  }
}
Example #27
0
ConvergenceResult ConstrainedMinimizationProblem::LineMinimizationStep(const Vector& dx,Real& alpha0)
{
  Vector x0 = x;
  Real fx0 = fx;
  const Real lineSearchShrink = 0.5;
  Real dxnorm = dx.norm();
  Real t = alpha0;
  Real slope = Abs(dx.dot(grad));
  if(dxnorm < tolgrad) {
    return ConvergenceF;
  }

  //if(verbose>=1 && t != alpha0) cout<<"Starting t value: "<<t<<endl;
  //find a step that descends along grad
  for(int lineSearchIters=0;;lineSearchIters++) {
    if(t*dxnorm < tolx) {
      if(verbose>=1) cout<<"ConstrainedMinimzationProblem::LineSearch(): Converged on t on line search iteration "<<lineSearchIters<<", |dx|="<<dxnorm<<endl;
      //x must remain on surface... just set x to x0
      x = x0; alpha0 = 0;
      return ConvergenceX;
    }
    x = x0; x.madd(dx,t);
    for(int i=0;i<x.n;i++)
      if(IsNaN(x(i))) {
	cerr<<"ConstrainedMinimizationProblem: x is NaN!"<<endl;
	cerr<<"t is "<<t<<endl;
	cerr<<"x0 is "<<x0<<endl;
	getchar();
	return ConvergenceError;
      }
    if(SolveFeasiblePoint(x,20)) {
      Assert(CheckPoint(x));
      fx = (*f)(x);
      if(fx < fx0 - ALF*slope*t) {
	break;
      }
    }
    else {
      if(verbose>=1) cout<<"ConstrainedMinimzationProblem::StepGD(): Warning, MoveToSurface_Bounded failed on line search iteration "<<lineSearchIters<<endl;
    }
    //reduce t
    t *= lineSearchShrink;
  }
  alpha0 = t;

  if(Abs(fx-fx0) <= tolf) {
    if(verbose>=1) cout<<"ConstrainedMinimzationProblem::StepGD(): Success, change in f is "<<fx0-fx<<endl;
    return ConvergenceF;
  }

  if(verbose>=1) cout<<"StepGD(): Target "<<fx<<", length of gradient "<<dxnorm<<", step size "<<t*dxnorm<<endl;

  if(bmin.n != 0)
    Assert(AABBContains(x0,bmin,bmax));

  return MaxItersReached;
}
 Status BaseMultiPos::
  update(Model const & model)
  {
    Status st;    
    Vector cur_eepos(Vector::Zero(3));
    Vector delta;
    Vector v_delta;

    size_t ndof(model.getUnconstrainedNDOF());

    for (size_t ii(0); ii < task_table_.size(); ++ii) {
      st = task_table_[ii]->update(model);
      if ( ! st) { return st; }
    }
    for(int ii=0; ii<3; ii++) {
      cur_eepos[ii] = ee_pos_[3*cur_row_+ii];
    }
    
    delta = cur_eepos - ee_task_->getActual();
    jspace::Constraint* constraint = model.getConstraint();
    if (constraint) {
      jspace::State fullState(model.getNDOF(),model.getNDOF(),6);
      constraint->getFullState(model.getState(),fullState);
      v_delta = ee_task_->getJacobian()*fullState.velocity_;
    }
    else {
      v_delta = ee_task_->getJacobian()*model.getState().velocity_;
    }
    
    if (delta.norm() < threshold_ && v_delta.norm() < vel_threshold_) {
      if(forward_) {
	if (cur_row_ < (ee_pos_.rows()/3)-1) {
	  ++cur_row_;
	  for(size_t jj(0); jj<3; ++jj) {
	    cur_eepos[jj] = ee_pos_[3*cur_row_+jj];
	  }
	  st = ee_goal_->set(cur_eepos);
	  if (! st) { return st; } 
	}
	else { forward_ = false; }
      }
      else {
	if (cur_row_ > 0) {
	  --cur_row_;
	  for(size_t jj(0); jj<3; ++jj) {
	    cur_eepos[jj] = ee_pos_[3*cur_row_+jj];
	  }
	  st = ee_goal_->set(cur_eepos);
	  if (! st) { return st; } 
	}
	else { forward_ = true; }
      }
    }
 
      return st;
    }
Example #29
0
TEST(VectorTest, VectorMultiply)
{
    const Vector ex(1, 0, 0);
    const Vector ey(0, 1, 0);
    const Vector ez(0, 0, 1);
    const Vector ZERO(0, 0, 0);
    
    const Vector v1(1, 0, -1);
    const Vector v2(0, 1, -1);
    Vector v3(1, 1, 1);
    v3.normalize();
    v3 *= v1.norm()*v2.norm()*sin( acos( v1.normalized()*v2.normalized()) ); // area of parallelogram

    EXPECT_EQ( ez, cross_product( ex, ey ) );
    EXPECT_EQ( -ez, cross_product( ey, ex ) );
    EXPECT_EQ( ZERO, cross_product( ex, ex ) );

    EXPECT_EQ( v3, cross_product( v1, v2 ) );
}
Example #30
0
 void vectorToSpherical(const Vector &v, double *r, double *theta, double *phi) {
   *r = v.norm();
   if (fabs(*r) < EPSILON) {
     *theta = 0;
     *phi = 0;
     return;
   }
   *theta = acos(v.z() / (*r));
   *phi = atan2(v.y(), v.x());
 }