static int AngleToBezier(Point2d* pts, float radius)
{
const Vector2d vec1 (pts[1] - pts[0]); // 第一条边
const Vector2d vec2 (pts[2] - pts[1]); // 第二条边
const float dHalfAngle = 0.5f * fabs(vec1.angleTo2(vec2)); // 夹角的一半
if (dHalfAngle < 1e-4f || fabs(dHalfAngle - _M_PI_2) < 1e-4f) // 两条边平行
return 0;
const float dDist1 = 0.5f * vec1.length();
const float dDist2 = 0.5f * vec2.length();
float dArc = radius / tan(dHalfAngle); // 圆弧在边上的投影长度
if (dArc > dDist1 || dArc > dDist2)
{
float dArcOld = dArc;
dArc = mgMin(dDist1, dDist2);
if (dArc < dArcOld * 0.5f)
return 3;
}
int count = 0;
Point2d ptCenter, ptStart, ptEnd;
float startAngle, sweepAngle;
ptStart = pts[1].rulerPoint(pts[0], dArc, 0);
ptEnd = pts[1].rulerPoint(pts[2], dArc, 0);
if (mgArcTan(ptStart, ptEnd, pts[1] - ptStart,
ptCenter, radius, &startAngle, &sweepAngle))
{
count = mgAngleArcToBezier(
pts, ptCenter, radius, radius, startAngle, sweepAngle);
}
return count;
}
long double angleBetween(const Vector2d &V1, const Vector2d &V2)
{
long double dotproduct = V1.dot(V2);
long double length = V1.length() * V2.length();
if (length==0) return 0;
long double quot = dotproduct / length;
if (quot > 1 && quot < 1.0001) quot = 1;
if (quot < -1 && quot > -1.0001) quot = -1;
long double result = acosl( quot ); // 0 .. pi
if (isleftof(Vector2d(0,0), V2, V1))
result = -result;
return result;
}
// 求本矢量投影到矢量xAxis上的垂直距离
// 在xAxis的逆时针方向时返回正值，顺时针则返回负值
float Vector2d::distanceToVector(const Vector2d& xAxis) const
{
float len = xAxis.length();
if (len < _MGZERO)
return length();
return xAxis.crossProduct(*this) / len;
}
long double angleBetween(Vector2d V1, Vector2d V2)
{
long double result, dotproduct, lengtha, lengthb;
dotproduct = (V1.x * V2.x) + (V1.y * V2.y);
lengtha = V1.length();//sqrt(V1.x * V1.x + V1.y * V1.y);
lengthb = V2.length();//sqrt(V2.x * V2.x + V2.y * V2.y);
result = acosl( dotproduct / (lengtha * lengthb) );
if(result < 0)
result += M_PI;
else
result -= M_PI;
return result;
}
// split at length 0 < t < 1
uint Printlines::divideline(uint lineindex, const double t)
{
PLine *l = &lines[lineindex];
Vector2d d = l->to - l->from;
vector<Vector2d> points(1);
points[0] = l->from + d * t * d.length();
uint nlines = divideline(lineindex, points);
delete l;
return nlines;
}
// Douglas-Peucker algorithm
vector<Vector2d> simplified(const vector<Vector2d> &vert, double epsilon)
{
if (epsilon == 0) return vert;
uint n_vert = vert.size();
if (n_vert<3) return vert;
double dmax = 0;
//Find the point with the maximum distance from line start-end
uint index = 0;
Vector2d normal = normalV(vert.back()-vert.front());
normal.normalize();
if( (normal.length()==0) || ((abs(normal.length())-1)>epsilon) ) return vert;
for (uint i = 1; i < n_vert-1 ; i++)
{
double dist = abs((vert[i]-vert.front()).dot(normal));
if (dist >= epsilon && dist > dmax) {
index = i;
dmax = dist;
}
}
vector<Vector2d> newvert;
if (index > 0) // there is a point > epsilon
{
// divide at max dist point and cleanup both parts recursively
vector<Vector2d> part1;
part1.insert(part1.end(), vert.begin(), vert.begin()+index+1);
vector<Vector2d> c1 = simplified(part1, epsilon);
vector<Vector2d> part2;
part2.insert(part2.end(), vert.begin()+index, vert.end());
vector<Vector2d> c2 = simplified(part2, epsilon);
newvert.insert(newvert.end(), c1.begin(), c1.end()-1);
newvert.insert(newvert.end(), c2.begin(), c2.end());
}
else
{ // all points are nearer than espilon
newvert.push_back(vert.front());
newvert.push_back(vert.back());
}
return newvert;
}
void Tank::evalPfield(GameConstants &gc,
Polygon &base,
vector<Tank*> &tanks,
vector<Flag*> &flags,
vector<Tank*> &enemy_tanks,
vector<Flag*> &enemy_flags,
vector<Polygon*> &obstacles
){
double pi = 3.1415926435;
Vector2d result = Vector2d(0);
for (int i=0; i < obstacles.size(); i++){
result += 0.4*obstacles[i]->potentialField(loc,dir);
}
/*for (int i=0; i < enemy_tanks.size(); i++)
{
result += enemy_tanks[i]->potentialField(loc,dir);
}
for (int i=0; i < enemy_flags.size(); i++)
{
result -= 30 * enemy_flags[i]->potentialField(loc,dir);
cout << enemy_flags[i]->loc[0] << ", " << enemy_flags[i]->loc[1] << endl;
}
*/
result -= 60*goals[goals.size()-1]->potentialField(loc,dir);
double desiredAngle = atan2(result[1], result[0]);
double desiredMagnitude = result.length();
double currentAngle = atan2(dir[1], dir[0]);
Tank::protocol.speed(idx, 1); //desiredMagnitude*.4
//cout << desiredMagnitude *.1 << endl;
double angDiff = currentAngle - desiredAngle;
while (angDiff < -1 * pi){
angDiff += (2*pi);
}
while (angDiff > pi){
angDiff -= (2*pi);
}
Tank::protocol.angvel(idx, -angDiff/pi);
Tank::protocol.shoot(idx);
}
double Vector2d::angleTo(Vector2d that)
{
return acos(this->dotProduct(that) / (this->length() * that.length()));
}
