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())); }