Example #1
0
 Vector3D * rightPart(Vector3D * X)
 {
     Vector3D * Y = new Vector3D(0, 0, 0);
     
     double omega = 0;
     double ang = AngleToTarget();
     if (ang > EPS) omega = OMax;
     if (ang < -EPS) omega = -OMax;
     
     //omega = 0;
     
     (*Y)[0] = new Scalar(VMax * (*X)[2]->cos());
     (*Y)[1] = new Scalar(VMax * (*X)[2]->sin());
     (*Y)[2] = new Scalar(omega);
     
     double gamma = (*X)[2]->get();
     while (gamma > 2 * PI)
     {
         gamma -= 2 * PI;
         (*X)[2]->set(gamma);
     }
     
     while (gamma < 0)
     {
         gamma += 2 * PI;
         (*X)[2]->set(gamma);
     }
     
     position = X;
     return Y;
 }
Example #2
0
 double PathTo(Vector2D * tar)
 {
     double ang = AngleToTarget();
     
     //if (
     
     return 0;
 }
Example #3
0
 double PathToTarget(Vector2D * targ)
 {
     bool isLeft = false;
     double ang = AngleToTarget(targ);
     if (ang > 0) isLeft = true; // else false
     
     double theta;
     
     if (isLeft)
     {
         theta = position->getZ() + PI / 2.0;
     }
     else
     {
         theta = position->getZ() - PI / 2.0;
     }
     
     Vector2D * P = new Vector2D(position->getX(), position->getY());
     Vector2D * n = new Vector2D(r * cos(theta), r * sin(theta));
     
                 // R = targ - (P + n)
     Vector2D * G = P->add(n);
     Vector2D * R = targ->extract(G);
     Vector2D * ni = n->inv();
     
     double beta = R->angleTo(n->inv());
     double beta1 = ni->anticlockAngle(R);
     if (!isLeft) beta1 *= -1;
     double e = acos(r / R->norm());
     
     if (beta1 < 0)
         beta = 2 * PI - beta;
     
     /*
     if (beta1 > 0 && !isLeft)
         beta = 2 * PI - beta;
     */
      
     double eps = beta - e;
     double L = r * eps;
     
     double Rn = R->norm();
     double T = sqrt(Rn * Rn + r * r);
     return L + T;
 }
Example #4
0
 double AngleToTarget()
 {
     return AngleToTarget(target);
 }
Example #5
0
	float Math::AngleToTarget(const Vector2& vec1,const Vector2& vec2)
	{
		return AngleToTarget(vec1.GetX(),vec1.GetY(),vec2.GetX(),vec2.GetY());
	}
	double Math::AngleToTarget(Vector3& source,Vector3& target)
	{
		return AngleToTarget(source.getX(),source.getY(),target.getX(),target.getY());
	}