void initPhysics(double rad, double speed, double angle)
{
clocktime = 0.f;
double vx = speed * cos(angle);
double vy = speed * sin(angle);
simBall1.set(rad, rad, rad, 2, vx, vy, 128, 128, 0);
realBall.set(rad, rad, rad, 2, vx, vy, 20, 160, 60);
printf("initPhysics: ball(%f, %f)\n", simBall1.cx, simBall1.cy);
printf("initPhysics: realball(%f, %f)\n", realBall.cx, realBall.cy);
}
bool
xpcc::Polygon2D<T>::intersects(const Circle2D<T>& circle) const
{
SizeType n = this->points.getSize();
for (SizeType i = 0; i < n; ++i)
{
LineSegment2D<T> segment(this->points[i], this->points[(i + 1) % n]);
T distance = segment.getDistanceTo(circle.getCenter());
if (distance <= circle.getRadius()) {
return true;
}
}
return false;
}
bool Stage::isShipInShip(int shipIndex, double x, double y) {
Circle2D *shipCircle = new Circle2D(x, y, SHIP_RADIUS);
bool shipInShip = false;
for (int z = 0; z < numShips_ && !shipInShip; z++) {
if (shipIndex != z && ships_[z]->alive) {
Ship *otherShip = ships_[z];
Circle2D *otherShipCircle =
new Circle2D(otherShip->x, otherShip->y, SHIP_RADIUS);
if (shipCircle->overlaps(otherShipCircle)) {
shipInShip = true;
}
delete otherShipCircle;
}
}
delete shipCircle;
return shipInShip;
}
void renderScene()
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
gluOrtho2D(0., WorldWidth, 0., (GLdouble)WorldHeight);
////////////////////////// Drawing The Coordinate Plane Starts Here.
// We Will Draw Horizontal And Vertical Lines With A Space Of 1 Meter Between Them.
/* glColor3ub(0, 0, 255); // Draw In Blue
glBegin(GL_LINES);
glLineWidth(0.5);
// Draw The Vertical Lines
for (float x = 0.; x <= WorldWidth; x += 1.0f) // x += 1.0f Stands For 1 Meter Of Space In This Example
{
glVertex3f(x, WorldHeight, 0);
glVertex3f(x, -WorldHeight, 0);
}
// Draw The Horizontal Lines
for (float y = 0.; y <= WorldHeight; y += 1.0f) // y += 1.0f Stands For 1 Meter Of Space In This Example
{
glVertex3f(0, y, 0);
glVertex3f(WorldWidth, y, 0);
}
glEnd(); */
/////////////////////////// Drawing The Coordinate Plane Ends Here.
/////////////////////////draw the hallow 2d disc /////////////
simBall1.DrawCircle(0, circleSections);
realBall.DrawCircle(0, circleSections);
///////////// draw the overlay HUD /////////////////////
glColor3ub(127, 127, 127);
char str[256];
sprintf(str, "simBall1: pos(%f, %f), velocity(%f, %f)", simBall1.cx, simBall1.cy, simBall1.vx, simBall1.vy);
glRasterPos2i(32, height-32);
glCallLists(strlen(str), GL_UNSIGNED_BYTE, str);
sprintf(str, "real: pos(%f, %f), velocity(%f, %f)", realBall.cx, realBall.cy, realBall.vx, realBall.vy);
glRasterPos2i(32, height - 64);
glCallLists(strlen(str), GL_UNSIGNED_BYTE, str);
FsSwapBuffers();
}
/*!
*/
int
Circle2D::intersection( const Circle2D & circle,
Vector2D * sol1,
Vector2D * sol2 ) const
{
double rel_x = circle.center().x - this->center().x;
double rel_y = circle.center().y - this->center().y;
double center_dist2 = rel_x * rel_x + rel_y * rel_y;
double center_dist = std::sqrt( center_dist2 );
if ( center_dist < std::fabs( this->radius() - circle.radius() )
|| this->radius() + circle.radius() < center_dist )
{
return 0;
}
//std::cerr << "must exist intersection C1: " << this->center() << this->radius()
// << " C2: " << circle.center() << circle.radius()
// << std::endl;
// line that passes through the intersection points
Line2D line( -2.0 * rel_x,
-2.0 * rel_y,
circle.center().r2()
- circle.radius() * circle.radius()
- this->center().r2()
+ this->radius() * this->radius() );
return this->intersection( line, sol1, sol2 );
}
bool Stage::isShipInWall(double x, double y) {
for (int z = 0; z < numWalls_; z++) {
Wall *wall = walls_[z];
double left = wall->getLeft();
double bottom = wall->getBottom();
if (x > left && x < left + wall->getWidth() && y > bottom
&& y < bottom + wall->getHeight()) {
return true;
}
}
Circle2D *shipCircle = new Circle2D(x, y, SHIP_RADIUS);
for (int z = 0; z < numWallLines_; z++) {
Line2D* line = wallLines_[z];
if (shipCircle->intersects(line)) {
delete shipCircle;
return true;
}
}
delete shipCircle;
return false;
}
// ==================================================================================
Vector2D TacticTransferObject::AvoidtoEnterCircle(Circle2D Ci, Vector2D pnt, Vector2D finPOS)
{
// Avoided=false;
Segment2D S(pnt,finPOS);
if(Ci.HasIntersection(S) || Ci.contains(pnt) || Ci.contains(finPOS))
{
// qDebug() << " HAS INTERSECTION WITH CIRCLE !! DO NOT ENTER THIS CIRCLE PLEASE !!!";
// qDebug() << " HAS INTERSECTION WITH CIRCLE ( " << Ci.center().x << "," << Ci.center().y << " ) , Radius : " << Ci.radius() ;
Vector2D c2r = pnt - Ci.center() ;
Vector2D c2f = finPOS - Ci.center() ;
c2r.setLength(Ci.radius()+50);
AngleDeg angle=c2r.dir() - c2f.dir();
//double ang=c2r.dir().radian() - c2f.dir().radian();
if (angle.radian() > M_PI) angle -= AngleDeg(M_PI) *= 2;
if (angle.radian() < -M_PI) angle += AngleDeg(M_PI) *= 2;
//if (ang > M_PI) ang -= 2 * M_PI;
//if (ang < -M_PI) ang += 2 * M_PI;
//qDebug() << "c2r DIRECTION = " << c2r.dir().degree() << "c2f DIRECTION = " << c2f.dir().degree() << " ANGLE = " << angle.degree() << " ANG =" << ang*57.32;
int div=int(Ci.radius()/50);
// qDebug() << "div = " << div ;
angle=angle/=min(div,6);
angle=c2r.dir() - angle;
c2r.setDir(angle);// c2f.dir()));
Vector2D p = Ci.center() + c2r ;
// Avoided=true;
// qDebug() << " Avoided point is : (" << finPOS.x << "," <<finPOS.y << ") & Corrected is : ("<<
// p.x << "," << p.y << ")" ;
return p;
}
else
{
return finPOS;
}
}
Circle2D Circle2D::OptimalEnclosingCircle(const float2 *pointArray, int numPoints)
{
assert(pointArray || numPoints == 0);
// Special case handling for 0-3 points.
switch(numPoints)
{
case 0: return Circle2D(float2::nan, -FLOAT_INF);
case 1: return Circle2D(pointArray[0], 0.f);
case 2:
{
float2 center = (pointArray[0] + pointArray[1]) * 0.5f;
float r = Sqrt(Max(center.DistanceSq(pointArray[0]), center.DistanceSq(pointArray[1]))) + 1e-5f;
return Circle2D(center, r);
}
case 3: return Circle2D::OptimalEnclosingCircle(pointArray[0], pointArray[1], pointArray[2]);
}
// Start off by computing the convex hull of the points, which prunes many points off from the problem space.
float2 *pts = new float2[numPoints];
memcpy(pts, pointArray, sizeof(float2)*numPoints);
numPoints = float2_ConvexHullInPlace(pts, numPoints);
// Use initial bounding box extents (min/max x and y) as fast guesses for the optimal
// bounding sphere extents.
for(int i = 0; i < numPoints; ++i)
{
if (pts[0].x < pts[i].x) Swap(pts[0], pts[i]);
if (pts[1].x > pts[i].x) Swap(pts[1], pts[i]);
if (pts[2].y < pts[i].y) Swap(pts[2], pts[i]);
if (pts[3].y > pts[i].y) Swap(pts[3], pts[i]);
}
// Compute the minimal enclosing circle for the first three points.
Circle2D minCircle = OptimalEnclosingCircle(pts[0], pts[1], pts[2]);
float r2 = minCircle.r*minCircle.r;
// Iteratively include the remaining points to the minimal circle.
for(int i = 3; i < numPoints; ++i)
{
// If the new point is already inside the current bounding circle, it can be skipped
float d2 = (pts[i] - minCircle.pos).LengthSq();
if (d2 <= r2)
continue;
// The new point is outside the current bounding circle defined by pts[0]-pts[2].
// Compute a new bounding circle that encloses pts[0]-pts[2] and the new point pts[i].
// A circle is defined by at most three points, so one of the resulting points is redundant.
// Swap points around so that pts[0]-pts[2] define the new minimum circle, and pts[i] will
// have the redundant point.
int redundantIndex;
minCircle = SmallestCircleSqEnclosing4Points(pts[i], pts[0], pts[1], pts[2], redundantIndex);
minCircle.pos += pts[i];
Swap(pts[i], pts[redundantIndex]);
// For robustness, apply epsilon after square root.
minCircle.r = Sqrt(minCircle.r) + 1e-3f;
r2 = minCircle.r*minCircle.r;
// Start again from scratch: pts[0]-pts[2] now has the new candidate.
i = 2;
mathassert1(minCircle.Contains(pts[0], 1e-3f), minCircle.SignedDistance(pts[0]));
mathassert1(minCircle.Contains(pts[1], 1e-3f), minCircle.SignedDistance(pts[1]));
mathassert1(minCircle.Contains(pts[2], 1e-3f), minCircle.SignedDistance(pts[2]));
}
delete[] pts;
return minCircle;
}
bool Circle2D::intersects(Line2D *line, bool inverted, bool assignPoints,
Point2D **p1, Point2D **p2) {
if (h_ - line->xMax() > r_ || line->xMin() - h_ > r_ || k_ - line->yMax() > r_
|| line->yMin() - k_ > r_) {
return false;
}
double linem = line->m();
double lineb = line->b();
if (linem == DBL_MAX) {
Line2D *invLine = line->getInverse();
Circle2D *invCircle = this->getInverse();
return invCircle->intersects(invLine, !inverted, assignPoints, p1, p2);
} else {
bool intersects = false;
if (contains(line->x1(), line->y1())) {
if (assignPoints) {
saveToNextPoint(p1, p2, line->x1(), line->y1(), inverted);
intersects = true;
} else {
return true;
}
}
if (contains(line->x2(), line->y2())) {
if (assignPoints) {
saveToNextPoint(p1, p2, line->x2(), line->y2(), inverted);
intersects = true;
if (*p2 != 0) {
return true;
}
} else {
return true;
}
}
double a = square(linem) + 1;
double b = 2 * ((lineb * linem) - (k_ * linem) - h_);
double c = square(h_) + square(k_) + square(lineb) - (2 * lineb * k_)
- square(r_);
double discrim = (b * b) - (4 * a * c);
if (discrim < 0) {
return false;
}
double sqrtDiscrim = sqrt(discrim);
double x1 = (-b + sqrtDiscrim) / (2 * a);
double y1 = (linem * x1) + lineb;
if (x1 >= line->xMin() && x1 <= line->xMax()) {
if (assignPoints) {
saveToNextPoint(p1, p2, x1, y1, inverted);
intersects = true;
if (*p2 != 0) {
return true;
}
} else {
return true;
}
}
if (sqrtDiscrim > 0) {
double x2 = (-b - sqrtDiscrim) / (2 * a);
double y2 = (linem * x2) + lineb;
if (x2 >= line->xMin() && x2 <= line->xMax()) {
if (assignPoints) {
saveToNextPoint(p1, p2, x2, y2, inverted);
intersects = true;
} else {
return true;
}
}
}
return intersects;
}
}
const Circle2D Pose2D::operator/(const Circle2D& circle) const
{
return Circle2D(*this / circle.origin(), circle.radius());
}
