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;
}
double PathTo(Vector2D * tar)
{
double ang = AngleToTarget();
//if (
return 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;
}
double AngleToTarget()
{
return AngleToTarget(target);
}
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());
}