Point trilaterate(const Circle& c1, const Circle& c2, const Circle& c3)
{
Point p1, p2, p3;
p1 = c1.getCenter();
p2 = c2.getCenter();
p3 = c3.getCenter();
float r1, r2, r3;
r1 = c1.getRadius();
r2 = c2.getRadius();
r3 = c3.getRadius();
float d12, d13, d23;
d12 = getDistance(p1,p2);
d13 = getDistance(p1,p3);
d23 = getDistance(p2,p3);
DEBUG("Attempting to trilaterate:\n");
DEBUG("Centre:(%f, %f) Radius:%f\n",p1.getX(), p1.getY(), r1);
DEBUG("Centre:(%f, %f) Radius:%f\n",p2.getX(), p2.getY(), r2);
DEBUG("Centre:(%f, %f) Radius:%f\n",p3.getX(), p3.getY(), r3);
bool inter12, inter13, inter23;
inter12 = d12-r1 < r2 && r2 < d12+r1;
inter13 = d13-r1 < r3 && r3 < d13+r1;
inter23 = d23-r2 < r3 && r3 < d23+r3;
if(!(inter12 && inter13 && inter23))
{
DEBUG("Error: each pair of circles must intersect with at least two points\n");
throw CIRCLES_DO_NOT_INTERSECT;
}
return __trilaterate(c1, c2, c3);
}
Point __trilaterate(const Circle& a, const Circle& b, const Circle& c)
{
//radii of each circle
float ra, rb, rc;
ra = a.getRadius();
rb = b.getRadius();
rc = c.getRadius();
//center of each circle
Point ca, cb, cc;
ca = a.getCenter();
cb = b.getCenter();
cc = c.getCenter();
//x basis vector of transformed co-ordinate space in original space
Point xUnitVector = getXUnit(ca, cb);
//"horizontal" component (transformed space) of difference between circle centers
float i = signedMagnitudeX(ca, cc, xUnitVector);
//y basis vector of transformed co-ordinate space in original space
Point yUnitVector = getYUnit(ca, cc, i, xUnitVector);
//"vertical" component (transformed space) of difference between circle centers
float j = signedMagnitudeY(ca, cc, yUnitVector);
float d = getDistance(ca, cb);
//the intersection point relative to origin in transformed co-ordinate space
float x = (powf(ra,2)-powf(rb,2)+powf(d,2))/(2*d);
float y = ((powf(ra, 2)-powf(rc,2)+powf(i,2)+powf(j,2))/(2*j)) - x*(i)/(j);
//transform x, y to original co-ordinate frame
Point xMagVector = xUnitVector*x, yMagVector = yUnitVector*y;
Point p = ca + xMagVector + yMagVector;
return p;
}
bool checkCollision(Circle granade,Circle obstacle){
float dt = distance(granade.getX(),granade.getY(),obstacle.getX(),obstacle.getY());
if(obstacle.getRadius()+granade.getRadius() >= dt){
return true;
}
return false;
}
int main()
{
//circle tests
Circle newCircle = Circle(2);
cout << "newCircle radius is: " << newCircle.getRadius() << endl;
newCircle.incSides(3);
cout << "newCircle radius is: " << newCircle.getRadius() << endl;
std::cin.get();
//rectangle tests
Rectangle newRectangle = Rectangle(2, 3);
cout << "newRectangle side1, side2: " << newRectangle.getSide1() << " , " << newRectangle.getSide2() << endl;
newRectangle.incSides(3);
cout << "newRectangle side1, side2: " << newRectangle.getSide1() << " , " << newRectangle.getSide2() << endl;
std::cin.get();
//triangle tests
Triangle newTriangle = Triangle(5, 5, 5);
cout << "newTriangle side1, side2, side3: " << newTriangle.getSide1() << " , " << newTriangle.getSide2() << " , " << newTriangle.getSide3() << endl;
cout << "newTriangle's area: " << newTriangle.area() << endl;
newTriangle.incSides(3);
cout << "newTriangle side1, side2, side3: " << newTriangle.getSide1() << " , " << newTriangle.getSide2() << " , " << newTriangle.getSide3() << endl;
std::cin.get();
return 0;
}
bool TestCircleCircle ( Circle const & A, Circle const & B )
{
float dist2 = Distance2d::Distance2PointPoint( A.getCenter(), B.getCenter() );
float rad = A.getRadius() * B.getRadius();
float rad2 = rad * rad;
return dist2 <= rad2;
}
bool Circle::overlaps(Circle c) {
double radiiSum = pow(c.getRadius() + radius, 2);
double radiiDiff = pow(c.getRadius() - radius, 2);
double centerDist = pow(c.getX() - x, 2) + pow(c.getY() - y, 2);
if(radiiDiff <= centerDist && centerDist <= radiiSum) {
return true;
}
return false;
}
void Ball::geOutOf(Object* object) {
Circle* bouncer = object->getCircleBounds();
Circle* ball = getCircleBounds();
sf::Vector2f* normal = vectors->computeNormToSurface(ball, bouncer);
vectors->setLength(normal, bouncer->getRadius() + ball->getRadius());
sf::Vector2f* bouncerCenter = bouncer->getCenter();
setPosition(bouncerCenter->x + normal->x - ball->getRadius(), bouncerCenter->y + normal->y - ball->getRadius());
}
bool circleCollision(Circle c1, Circle c2)
{
vec2 pos1 = c1.getPosition();
vec2 pos2 = c2.getPosition();
float r1 = c1.getRadius();
float r2 = c2.getRadius();
float distance = getDistance(pos1, pos2);
if (distance - r1 - r2 <= 0) {
return true;
}
return false;
}
bool isCircleInCircle(Circle& circ1, Circle& circ2)
{
if(!circ2.isPointInsideFigure(circ1.getCenter()))
return false;
if(!circ1.isPointInsideFigure(circ2.getCenter()))
return false;
Point OA(circ1.getCenter().x, circ1.getCenter().y + circ1.getRadius());
Point OB (circ1.getCenter().x + circ1.getRadius(), circ1.getCenter().y);
Point OC(circ1.getCenter().x, circ1.getCenter().y - circ1.getRadius());
Point OD(circ1.getCenter().x - circ1.getRadius(), circ1.getCenter().y);
if(circ2.isPointInsideFigure(OA) && circ2.isPointInsideFigure(OB) && circ2.isPointInsideFigure(OC) && circ2.isPointInsideFigure(OD))
return true;
Point OA2(circ2.getCenter().x, circ2.getCenter().y + circ2.getRadius());
Point OB2(circ2.getCenter().x + circ2.getRadius(), circ2.getCenter().y);
Point OC2(circ2.getCenter().x, circ2.getCenter().y - circ2.getRadius());
Point OD2(circ2.getCenter().x - circ2.getRadius(), circ2.getCenter().y);
if(circ1.isPointInsideFigure(OA2) && circ1.isPointInsideFigure(OB2) && circ1.isPointInsideFigure(OC2) && circ1.isPointInsideFigure(OD2))
return true;
return false;
}
void Collision::CircleToCircle(Circle &a, Circle &b)
{
ManifoldCtoC m;
Vector2D diff = a.getPosition().subtract(b.getPosition());//!< Origin of position subtracting origin of other position;
float dist = diff.magnitude();//!< Gets the length between the two origins.
float sumOfRadii = a.getRadius() + b.getRadius();//!< Sum of the radii
if (dist > sumOfRadii)//!< Exits early if no collision detection.
{
return;
}
//!send circles and the calculated distance to the manifold.
m.Initialize(a,b, dist);//!< Initializes the manifold.
m.ApplyImpulse(a,b);
}
bool Circle::operator>=(Circle C1)
{
if (radius >= C1.getRadius())
return true;
else
return false;
}
bool isCircleInRectangle(Circle& circ, Rectangle& rect)
{
Point O(circ.getCenter());
Point OA(circ.getCenter().x, circ.getCenter().y + circ.getRadius());
Point OB (circ.getCenter().x + circ.getRadius(), circ.getCenter().y);
Point OC(circ.getCenter().x, circ.getCenter().y - circ.getRadius());
Point OD(circ.getCenter().x - circ.getRadius(), circ.getCenter().y);
if(rect.isPointInsideFigure(OA) && rect.isPointInsideFigure(OB) && rect.isPointInsideFigure(OC) && rect.isPointInsideFigure(OD))
return true;
return false;
}
bool Collision::CircleToAABB(Circle &a, Box &b, bool &hitFloor,bool &hitSide, bool &hitUnder)
{
Vector2D extents(b.getWidth()/2,b.getHeight()/2);//!< puts the extents of the box into a vector2d.
Vector2D disti = b.getPosition().subtract(a.getPosition());//!< Finds the distance between the two origins.
Vector2D clamp = clamp.minMax(disti,extents);//!<Clamp closet point?
disti = disti.subtract(clamp);//!< Calculates the distance from the box. In the X and Y.
distance = disti.magnitude() - a.getRadius();//!< Gets the length between the two objects.
if(distance > 0)
{
return false; //!<Exits if the distance is greater than 0
}
//return true;
normal2 = disti.getUnitVector();//!< Calculates the normal
//!Used to find out if ive hit top side or under
if(normal2.getY() > 0.2f)
{
hitFloor = true;
}
else hitFloor = false;
if(normal2.getY() < -0.5f)
{
hitUnder = true;
}
if(normal2.getX() == -1.f || normal2.getX() == 1.f )
{
hitSide = true;
}
return true;
}
double Circle::intersectionArea(const Circle & otherCircle) {
double d = center.distanceFrom(otherCircle.getCenter());
//fprintf(stderr, "d=%f\n", d);
double r = radius;
//fprintf(stderr, "r=%f\n", r);
double R = otherCircle.getRadius();
//fprintf(stderr, "R=%f\n", R);
// error handle: if a circle has no radius
if (r==0 || R==0 || r+R<=d) {
return 0;
}
if (d==0){
r = r<R ? r:R;
return M_PI*r*r;
}
if ( ((((d+r <= r)||(-d-R >= -r)) && (r>R))) || \
((((d+r <= R)||(-d-r >= -R)) && (r<R)))){
//if overlap completely, do pi*r^2 with the smaller radius
r = r<R ? r:R;
return M_PI*r*r;
}
double part1 = r*r*acos( (d*d+r*r-R*R)/(2*d*r));
//fprintf(stderr, "part1=%f\n", part1);
double part2 = R*R*acos( (d*d+R*R-r*r)/(2*d*R));
//fprintf(stderr, "part2=%f\n", part2);
double part3 = 0.5*sqrt( (-d+r+R)*(d+r-R)*(d-r+R)*(d+r+R));
//fprintf(stderr, "part3=%f\n", part3);
return part1+part2-part3;
}
Point trilaterate(const Circle& a, const Circle& b, const Circle& c)
{
Point ca, cb, cc;
ca = a.getCenter();
cb = b.getCenter();
cc = c.getCenter();
float dab = getDistance(ca, cb); //distance from a to b
float dac = getDistance(ca, cc); //distance from a to c
float dbc = getDistance(cb, cc); //distance from b to c
float ra, rb, rc;
ra = a.getRadius(); rb = b.getRadius(); rc = c.getRadius();
//does a,b intersect? a,c intersect? b,c intersect?
bool abIntersect, acIntersect, bcIntersect;
abIntersect = fabs(dab-ra) < rb && rb < dab+ra;
acIntersect = fabs(dac-ra) < rc && rc < dac+ra;
bcIntersect = fabs(dbc-rb) < rc && rc < dbc+rb;
//debug statements
#ifdef VERBOSE
PRINT_ERR("Attempting to trilaterate:\n");
PRINT_ERR("Centre:(%f, %f) Radius:%f\n",ca.getX(), ca.getY(), ra);
PRINT_ERR("Centre:(%f, %f) Radius:%f\n",cb.getX(), cb.getY(), rb);
PRINT_ERR("Centre:(%f, %f) Radius:%f\n",cc.getX(), cc.getY(), rc);
#endif
//if colinear will cause nan errors in calculation
if(areColinear(ca,cb,cc))
{
#ifdef VERBOSE
PRINT_ERR("Error: circle centres are colinear\n");
#endif
throw CIRCLE_CENTRES_ARE_COLINEAR;
}
//insufficient number of intersections will result in throwing exception
if(!abIntersect || !acIntersect || !bcIntersect)
{
#ifdef VERBOSE
PRINT_ERR("Error: each pair of circles must intersect with at least two points\n");
#endif
throw CIRCLES_DO_NOT_INTERSECT;
}
return __trilaterate(a, b, c);
}
bool isCircleInTriangle(Circle& circ, Triangle& trig)
{
if(!trig.isPointInsideFigure(circ.getCenter()))
return false;
Point O(circ.getCenter());
Point OA(circ.getCenter().x, circ.getCenter().y + circ.getRadius());
Point OB (circ.getCenter().x + circ.getRadius(), circ.getCenter().y);
Point OC(circ.getCenter().x, circ.getCenter().y - circ.getRadius());
Point OD(circ.getCenter().x - circ.getRadius(), circ.getCenter().y);
if(trig.isPointInsideFigure(OA) && trig.isPointInsideFigure(OB) && trig.isPointInsideFigure(OC) && trig.isPointInsideFigure(OD))
return true;
return false;
}
bool Line::within(FigureType type, const double * coordinates, unsigned size)
{
if (type == FigureType::rect)
{
if (size < 4)
return false;
Rectangle polygon;
polygon.setCoord(coordinates, size);
if ( !(p1.getX() >= polygon.getX() && p1.getX() <= polygon.getX() + polygon.getWidth()) )
return false;
if ( !(p2.getX() >= polygon.getX() && p2.getX() <= polygon.getX() + polygon.getWidth()) )
return false;
if ( !(p1.getY() >= polygon.getY() && p1.getY() <= polygon.getY() + polygon.getHeight()) )
return false;
if ( !(p2.getY() >= polygon.getY() && p2.getY() <= polygon.getY() + polygon.getHeight()) )
return false;
return true;
}
else if (type == FigureType::circle)
{
if (size < 3)
return false;
Circle polygon;
polygon.setCoord(coordinates, size);
double x = p1.getX() - polygon.getX();
double y = p1.getY() - polygon.getY();
double radius = polygon.getRadius();
if ( !(x*x + y*y < radius*radius) )
return false;
x = p2.getX() - polygon.getX();
y = p2.getY() - polygon.getY();
if ( !(x*x + y*y < radius*radius) )
return false;
return true;
}
return false;
}
bool Circle::operator==(const Circle& circle) const {
if (this->getX() == circle.getX() && this->getY() == circle.getY() &&
this->getRadius() == circle.getRadius() &&
this->isVisible() == circle.isVisible() &&
this->getRed() == circle.getRed() &&
this->getGreen() == circle.getGreen() &&
this->getBlue() == circle.getBlue())
return true;
else return false;
}
bool Collision::CircleToLine(Circle &a, Line &b)
{
Vector2D normal = b.lineNormal();//!< Gets the normal of the line and sets to a new Vector2D
float mag = normal.magnitude();//!< Finds the magnitude of the normal.
float c = b.lineOrigin();//!< Find the origin value of the line.
c /= mag;//!< Normalises the origin value.
Vector2D normal2 = normal.getUnitVector();//!< normalises the normal.
float distance = (a.getPosition().dotProduct(normal2) + (c));//!< Finds the distance away from the point. distance = p.n +
return (distance >= 0 - a.getRadius()); //!< If the distance is 0 or less than 0. Return true.
}
void TestCircle::testPerimeter()
{
this->testInit(__func__);
Point origin = Point(100, 100);
double radius = 100;
Circle circle = Circle(origin, radius);
if (circle.getPerimeter() != (2 * M_PI * circle.getRadius())) {
this->testFailed();
}
this->testInterpret();
}
int Line::getCircleIntersectionPoints( Circle circle,
Vector2D *posSolution1, Vector2D *posSolution2 ){
int iSol;
float dSol1, dSol2;
float h = circle.getCenter().getX();
float k = circle.getCenter().getY();
// line: x = -c/b (if a = 0)
// circle: (x-h)^2 + (y-k)^2 = r^2, with h = center.x and k = center.y
// fill in:(-c/b-h)^2 + y^2 -2ky + k^2 - r^2 = 0
// y^2 -2ky + (-c/b-h)^2 + k^2 - r^2 = 0
// and determine solutions for y using abc-formula
if( fabs(a) < EPSILON ){
iSol = Geometry::abcFormula( 1, -2*k, ((-c/b) - h)*((-c/b) - h)
+ k*k - circle.getRadius()*circle.getRadius(), &dSol1, &dSol2);
posSolution1->setVector2D( (-c/b), dSol1 );
posSolution2->setVector2D( (-c/b), dSol2 );
return iSol;
}
// ay + bx + c = 0 => y = -b/a x - c/a, with da = -b/a and db = -c/a
// circle: (x-h)^2 + (y-k)^2 = r^2, with h = center.x and k = center.y
// fill in:x^2 -2hx + h^2 + (da*x-db)^2 -2k(da*x-db) + k^2 - r^2 = 0
// x^2 -2hx + h^2 + da^2*x^2 + 2da*db*x + db^2 -2k*da*x -2k*db
// + k^2 - r^2 = 0
// (1+da^2)*x^2 + 2(da*db-h-k*da)*x + h2 + db^2 -2k*db + k^2 - r^2 = 0
// and determine solutions for x using abc-formula
// fill in x in original line equation to get y coordinate
float da = -b/a;
float db = -c/a;
float dA = 1 + da*da;
float dB = 2*( da*db - h - k*da );
float dC = h*h + db*db - 2*k*db + k*k - circle.getRadius()*circle.getRadius();
iSol = Geometry::abcFormula( dA, dB, dC, &dSol1, &dSol2 );
posSolution1->setVector2D( dSol1, da*dSol1 + db );
posSolution2->setVector2D( dSol2, da*dSol2 + db );
return iSol;
}
int Circle::getIntersectionPoints( const Circle &c, Vector2D *p1,
Vector2D *p2) const {
float x0, y0, r0;
float x1, y1, r1;
x0 = getCenter( ).getX();
y0 = getCenter( ).getY();
r0 = getRadius( );
x1 = c.getCenter( ).getX();
y1 = c.getCenter( ).getY();
r1 = c.getRadius( );
float d, dx, dy, h, a, x, y, p2_x, p2_y;
// first calculate distance between two centers circles P0 and P1.
dx = x1 - x0;
dy = y1 - y0;
d = sqrt(dx*dx + dy*dy);
// normalize differences
dx /= d; dy /= d;
// a is distance between p0 and point that is the intersection point P2
// that intersects P0-P1 and the line that crosses the two intersection
// points P3 and P4.
// Define two triangles: P0,P2,P3 and P1,P2,P3.
// with distances a, h, r0 and b, h, r1 with d = a + b
// We know a^2 + h^2 = r0^2 and b^2 + h^2 = r1^2 which then gives
// a^2 + r1^2 - b^2 = r0^2 with d = a + b ==> a^2 + r1^2 - (d-a)^2 = r0^2
// ==> r0^2 + d^2 - r1^2 / 2*d
a = (r0*r0 + d*d - r1*r1) / (2.0 * d);
// h is then a^2 + h^2 = r0^2 ==> h = sqrt( r0^2 - a^2 )
float arg = r0*r0 - a*a;
h = (arg > 0.0) ? sqrt(arg) : 0.0;
// First calculate P2
p2_x = x0 + a * dx;
p2_y = y0 + a * dy;
// and finally the two intersection points
x = p2_x - h * dy;
y = p2_y + h * dx;
p1->setVector2D( x, y );
x = p2_x + h * dy;
y = p2_y - h * dx;
p2->setVector2D( x, y );
return (arg < 0.0) ? 0 : ((arg == 0.0 ) ? 1 : 2);
}
/* Considérons le segment de ligne l (AB) et le cercre centré en C
Le point D est la projection de AC sur AB.
DC est le vecteur entre D et C.
Lorsque que la norme de DC est plus petite que le rayon, nous savons qu'il
y a une intersection sur la ligne AB. Il reste a determiner si le point
d'intersection est sur le segment AB.
Ensuite on détermine s'il y a 1 ou 2 points d'intersections. Lorsque la
norme de DC est strictement égale au rayon, il n'y a qu'une intersection parce
que AB est tangente au cercle.
Si la norme de DC est plus petite que le rayon, il y aura 2 intersections.
Ensuite, pour chaque intersection, il faut d'éterminer le point précis de
l'intersection (E) et déterminer si ce point est sur le segment AB.
Dans le cas de la tangente, le point d'intersection est déterminer par la
projection AC sur AB.
Dans l'autre cas, on trouve le point d'intersection par trigonométrie. On
connais le rayon et le point D. Par c^2 = a^2 + b^2 (triangle rectangle), on
peut trouver la norme de ED. Ensuite les deux points d'intersections sont
défini comme AD - ED et AD + ED.
Pour terminer, afin de determiner si E est sur le segment AB on s'assure
que le produit scalaire AB * AE est positif et que la norme de AB est plus
grande qye la norme de AE
*/
Intersection2d intersect( const LineSegment2d& l, const Circle& c)
{
Intersection2d r;
Vector2d ab(l.a(), l.b());
Vector2d abUnit = ab.getUnit();
Vector2d ac(l.a(), c.getCenter());
double adNorm = ac * abUnit;
Vector2d ad = adNorm * abUnit;
Vector2d dc( l.a() + ad, c.getCenter() );
double dcNorm = dc.norm();
if( dcNorm <= c.getRadius() )
{
if( isEqual( dcNorm, c.getRadius(), 1.0e-5 ) ) //l'intersection est tangente
{
Point2d e = l.a() + ad;
Vector2d ae(l.a(), e);
Vector2d ec( c.getCenter(), e );
if( ab.normSquare() >= ae.normSquare() && ab * ae >= 0.0 )
r.add( e, ec );
}
else
{
double edNorm = sqrt( std::abs( dc.normSquare() - c.getRadius() * c.getRadius() ) );
Point2d e = l.a() + (adNorm - edNorm) * abUnit;
Vector2d ae(l.a(), e);
Vector2d ec( c.getCenter(), e );
if( ab.normSquare() >= ae.normSquare() && ab * ae >= 0.0 )
r.add( e, ec );
e = l.a() + (adNorm +edNorm) * abUnit;
ae = Vector2d(l.a(), e);
ec = Vector2d( c.getCenter(), e );
if( ab.normSquare() >= ae.normSquare() && ab * ae >= 0.0 )
r.add( e, ec );
}
}
return r;
}
bool Circle::within(FigureType type, const double * coordinates, unsigned size)
{
if (type == FigureType::rect)
{
if (size < 4)
return false;
Rectangle polygon;
polygon.setCoord(coordinates, size);
if ( !(center.getX() - radius >= polygon.getX() && center.getX() + radius <= polygon.getX() + polygon.getWidth()) )
return false;
if ( !(center.getY() - radius >= polygon.getY() && center.getY() + radius <= polygon.getY() + polygon.getHeight()) )
return false;
return true;
}
else if (type == FigureType::circle)
{
if (size < 3)
return false;
Circle polygon;
polygon.setCoord(coordinates, size);
double distX = polygon.getX() - center.getX();
double distY = polygon.getY() - center.getY();
double radiusDifference = polygon.getRadius() - radius;
if (radiusDifference < 0)
return false;
if ( !(distX*distX + distY*distY <= radiusDifference*radiusDifference) )
return false;
return true;
}
return false;
}
Point centroidIntersection(const Circle& a, const Circle& b)
{
Point ca, cb;
ca = a.getCenter();
cb = b.getCenter();
float ra, rb;
ra = a.getRadius();
rb = b.getRadius();
float deltaX, deltaY;
deltaX = cb.getX() - ca.getX();
deltaY = cb.getY() - ca.getY();
float d = getDistance(ca,cb);
float s = (pow(d,2)+pow(ra,2)-pow(rb,2))/(2*d);
float newX, newY;
newX = ca.getX()+deltaX * s/d;
newY = ca.getY()+deltaY * s/d;
return Point(newX,newY);
}
Size3D Circle::getIntersection(Circle circle)
{
float circlesDistance = _location.getDistance(circle.getLocation());
float intersection = _radius + circle.getRadius() - circlesDistance;
return Size3D(intersection,intersection);
/*Point2D2 circleLocation = circle.getLocation();
float xDistance = fmaxf(_location.getX(),circleLocation.getX()) - fminf(_location.getX(),circleLocation.getX());
float xIntersection = _radius + circle.getRadius() - xDistance;
if(xIntersection > 0)
{
float yDistance = fmaxf(_location.getY(),circleLocation.getY()) - fminf(_location.getY(),circleLocation.getY());
float yIntersection = _radius + circle.getRadius() - yDistance;
return Size2D(xIntersection,yIntersection);
}
return Size2D(xIntersection);*/
}
/*!
* \brief RectifyToVector::setUpResult
* \param circle
* \return
*/
bool RectifyToVector::setUpResult(Circle &circle){
//get and check point
if(!this->inputElements.contains(0) || this->inputElements[0].size() != 1){
return false;
}
QPointer<Geometry> geometry = this->inputElements[0].at(0).geometry;
if(geometry.isNull() || !geometry->getIsSolved() || !geometry->hasDirection()){
return false;
}
//get the sense (positive or negative)
double sense = 1.0;
if(this->scalarInputParams.stringParameter.contains("sense")){
if(this->scalarInputParams.stringParameter.value("sense").compare("negative") == 0){
sense = -1.0;
}
}
//get the direction to compare
OiVec r_reference = geometry->getDirection().getVector();
r_reference.normalize();
OiVec r_circle = circle.getDirection().getVector();
r_circle.normalize();
//calculate the angle between both directions
double angle = 0.0;
OiVec::dot(angle, r_reference, r_circle);
angle = qAbs(qAcos(angle));
//invert the normal vector if the angle is greater than 90°
if(angle > PI/2.0){
r_circle = -1.0 * r_circle;
}
//invert the normal vector if sense is negative
r_circle = sense * r_circle;
//set result
Direction direction = circle.getDirection();
direction.setVector(r_circle);
circle.setCircle(circle.getPosition(), direction, circle.getRadius());
return true;
}
bool checkCollision(Circle granade,pair<float,float> pt1 ,pair<float,float> pt2){
pair<float,float> centre= granade.getCentre();
pair< pair<float,float> , float > line = getLine(pt1,pt2);
// output2(centre.F,centre.S);
//output2(line.F.F,line.F.S);
//output1(line.S);
pair<float,float> foot = getFoot(centre , line.F.F,line.F.S , line.S);
//output2(foot.F,foot.S);
double dt = distance(foot.F,foot.S, centre.F, centre.S) ;
//output1(dt);
if( dt <= 2*granade.getRadius() ){
if( isInBW(foot,pt1,pt2) ){
return true;
}
else
return false;
}
}
void findIntersections(std::vector<Point>& p, const Circle& a, const Circle& b)
{
Point mid = centroidIntersection(a,b);
DEBUG("Circles intersect at: (%f %f)\n",mid.getX(),mid.getY());
Point ca = a.getCenter();
Point cb = b.getCenter();
float deltaX, deltaY;
deltaX = cb.getX() - ca.getX();
deltaY = cb.getY() - ca.getY();
float s = getDistance(ca,mid);
float ra = a.getRadius();
float d = getDistance(ca, cb);
float u = sqrt(pow(ra,2)-pow(s,2));
p.push_back(Point(ca.getX() - deltaX * u / d, ca.getY() + deltaY * u / d));
p.push_back(Point(ca.getX() + deltaX * u / d, ca.getY() - deltaY * u / d));
}
void CinderBoxFunApp::draw()
{
// clear out the window with black
gl::clear( Color( 0, 0, 0 ) );
for( vector<Body*>::const_iterator boxIt = mBodies.begin(); boxIt != mBodies.end(); ++boxIt ) {
Body *body = *boxIt;
Vec2f pos = body->getPosition();
float t = toDegrees(body->getRotation());
glPushMatrix();
gl::translate( pos );
gl::rotate( t );
if (body->getBodyType() == BodyTypeBox) {
Box *boxBody = (Box *)body;
Rectf rect( -boxBody->getWidth(), -boxBody->getHeight(), boxBody->getWidth(), boxBody->getHeight() );
gl::color( Color( 1, 0.5f, 0.25f ) );
gl::drawSolidRect( rect );
} else if (body->getBodyType() == BodyTypeCircle) {
Circle *circleBody = (Circle *)body;
gl::color( Color( 0.25, 0.5f, 1.f ) );
gl::drawSolidCircle(Vec2f(0,0), circleBody->getRadius());
} else if (body->getBodyType() == BodyTypePolygon) {
// Physics::Polygon *polyBody = (Physics::Polygon *)body;
gl::color(0.5f, 1, 0.25);
ConstVec pt1 = Vec2f(TRIANGLE_EDGE_LENGTH * 0.5f, 0);
ConstVec pt2 = Vec2f(TRIANGLE_EDGE_LENGTH, TRIANGLE_EDGE_LENGTH);
ConstVec pt3 = Vec2f( 0, TRIANGLE_EDGE_LENGTH);
gl::drawSolidTriangle(pt1, pt2, pt3);
}
glPopMatrix();
}
drawHelpText();
}
