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;
}
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 Collision::Circle_vs_Circle(const Circle& a, const Circle& b, Vector *v)
{
const Vector apos = a.getPosition();
Vector diff = b.getPosition() - apos;
const float r = a.radius + b.radius;
bool c = diff.isLength2DIn(r);
if(c && v)
{
diff.setLength2D(a.radius);
*v = apos + diff;
}
return c;
}
void Circle::intersect(const Circle& other, he::PrimitiveList<vec2>& outIntersections) const
{
float d(length(m_Position - other.getPosition()));
if (d > m_Radius + other.m_Radius)
{
return;
}
else if (d < fabs(m_Radius - other.m_Radius))
{
return;
}
else
{
//line connecting 2 points =
const float& x1(m_Position.x),
x2(other.m_Position.x),
y1(m_Position.y),
y2(other.m_Position.y),
r1(m_Radius),
r2(other.m_Radius);
float d2(lengthSqr(other.m_Position - m_Position));
float xPart1( (x2 + x1) / 2.0f + ((x2 - x1) * (sqr(r1) - sqr(r2))) / (2.0f * d2));
float xPart2( ((y2 - y1) / (2.0f * d2)) * sqrtf( (sqr(r1 + r2) - d2) * (d2 - sqr(r2 - r1))));
float yPart1( (y2 + y1) / 2.0f + ((y2 - y1) * (sqr(r1) - sqr(r2))) / (2.0f * d2));
float yPart2( ((x2 - x1) / (2.0f * d2)) * sqrtf( (sqr(r1 + r2) - d2) * (d2 - sqr(r2 - r1))));
outIntersections.add( vec2(xPart1 + xPart2, yPart1 - yPart2) );
outIntersections.add( vec2(xPart1 - xPart2, yPart1 + yPart2) );
}
}
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;
}
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.
}
/*!
* \brief Register::setUpResult
* \param circle
* \return
*/
bool Register::setUpResult(Circle &circle){
//get and check plane
if(!this->inputElements.contains(0) || this->inputElements[0].size() != 1){
return false;
}
QPointer<Plane> plane = this->inputElements[0].at(0).plane;
if(plane.isNull() || !plane->getIsSolved()){
return false;
}
//get the position of the sphere and the plane and the normal vector
OiVec n_plane = plane->getDirection().getVector();
n_plane.normalize();
OiVec x_plane = plane->getPosition().getVector();
OiVec x_circle = circle.getPosition().getVector();
//calculate the distance of the plane from the origin
double d;
OiVec::dot(d, x_plane, n_plane);
if(d < 0.0){
n_plane = -1.0 * n_plane;
d = -d;
}
//calculate the distance of the sphere position from the plane
double s;
OiVec::dot(s, x_circle, n_plane);
s = s - d;
//project the sphere position into the plane
x_circle = x_circle - s * n_plane;
//set result
Position position = circle.getPosition();
position.setVector(x_circle);
circle.setCircle(position, circle.getDirection(), circle.getRadius());
return true;
}
/*!
* \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 Collision::Circle_vs_Line(const Circle& a, const Line& b, Vector *v)
{
const Vector dir = b.direction();
const Vector bstart = b.startpos();
const Vector apos = a.getPosition();
Vector diff = apos - bstart;
Vector closest;
if (!dir.isZero())
{
float t = diff.dot2D(dir) / dir.getLength2DSq();
t = clamp(t, 0.0f, 1.0f);
closest = bstart + (t * dir);
}
else
closest = bstart;
diff = apos - closest;
bool c = diff.isLength2DIn(a.radius);
if (v && c)
*v = closest;
return c;
}
int CASE03::start() {
init();
calibrate();
if (time != -1) {
std::time(&_start);
}
cv::Mat original;
cv::Mat resized;
cv::Mat processed[MAX_COLOR];
original = Util::readImage();
resized = Util::resize(original);
cv::cvtColor(resized, resized, cv::COLOR_BGR2HSV);
cv::Size size = resized.size();
cv::Point center = cv::Point(original.size().width / 2, original.size().height / 2);
int radiusAim = 60;
bool inside;
bool flag = true;
while (flag) {
framerate.start();
original = Util::readImage();
int textHeight = original.size().height - 10;
resized = Util::resize(original);
objects.clear();
std::vector<std::future<cv::Mat>> t_preprocessing(MAX_COLOR);
for (int i = 0; i < colorCount; i++) {
HSV color = colors[i];
t_preprocessing[i] = std::async(std::launch::async, [resized, color] { return Util::preprocessing(resized, color); });
}
for (int i = 0; i < colorCount; i++) { t_preprocessing[i].wait(); }
for (int i = 0; i < colorCount; i++) { processed[i] = t_preprocessing[i].get(); }
if (SHOW_PROCESSED) {
for (int i = 0; i < colorCount; i++) {
imshow("Color " + std::to_string(i) + " processed", processed[i]);
}
}
std::vector<std::future<std::vector<Circle*>>> t_circles(MAX_COLOR);
for (int i = 0; i < colorCount; i++) {
cv::Mat temp = processed[i];
HSV color = colors[i];
t_circles[i] = std::async(std::launch::async, [temp, color] { return Util::findCircles(temp, color); });
}
for (int i = 0; i < colorCount; i++) {
t_circles[i].wait();
}
std::vector<std::future<std::vector<Poly*>>> t_poly(MAX_COLOR);
for (int i = 0; i < colorCount; i++) {
cv::Mat temp = processed[i];
HSV color = colors[i];
t_poly[i] = std::async(std::launch::async, [temp, color] { return Util::findPoly(temp, color); });
}
for (int i = 0; i < colorCount; i++) {
t_poly[i].wait();
}
for (int i = 0; i < colorCount; i++) {
std::vector<Circle*> circles = t_circles[i].get();
objects.insert(objects.end(), circles.begin(), circles.end());
std::vector<Poly*> polys = t_poly[i].get();
for (int j = 0; j < circles.size(); j++) {
for (int k = 0; k < polys.size(); k++) {
if (Util::distance(polys[k]->getPosition(), circles[j]->getPosition()) < 10) {
polys.erase(polys.begin() + k);
}
}
}
objects.insert(objects.end(), polys.begin(), polys.end());
}
for (int i = 0; i < objects.size(); i++) {
Util::drawObject(original, objects[i]);
if (objects[i]->getShape() == Shape::CIRCLE) {
Circle* circle = dynamic_cast<Circle*>(objects[i]);
putText(original, std::to_string(circle->getRadius()*ratio), circle->getPosition(), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.6, cv::Scalar(255, 255, 255), 1, CV_AA);
}
else {
Poly* poly = dynamic_cast<Poly*>(objects[i]);
for (int k = 0; k < poly->getV().size(); k++) {
double size = Util::distance(poly->getV()[k], poly->getV()[(k + 1) % poly->getV().size()])*ratio;
std::stringstream stream;
stream << std::fixed << std::setprecision(2) <<size;
putText(original,
stream.str(),
cv::Point(
(poly->getV()[k].x + poly->getV()[(k + 1) % poly->getV().size()].x)/2,
(poly->getV()[k].y + poly->getV()[(k + 1) % poly->getV().size()].y)/2
),
cv::FONT_HERSHEY_COMPLEX_SMALL, 0.6, cv::Scalar(255, 255, 255), 1, CV_AA);
}
}
std::string temp = "";
temp += shapeString[objects[i]->getShape()];
temp += " - ";
temp += objects[i]->getColor();
temp += " - ";
temp += "(" + std::to_string(objects[i]->getPosition().x) + "," + std::to_string(objects[i]->getPosition().y) + ")";
putText(original, temp, cv::Point(10, textHeight), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.6, cv::Scalar(0, 0, 0), 1, CV_AA);
textHeight = textHeight - 12;
}
imshow("Source", original);
int qtd = objects.size();
switch (cv::waitKey(1)) {
case 27:
cv::destroyAllWindows();
for (int i = 0; i < colorCount; i++) { colors[i].save(); }
return 0;
}
if (time != 0) {
flag = std::difftime(std::time(0), _start) < time;
}
}
return 0;
}
//=============================================================================
// 円の実装
//=============================================================================
//-----------------------------------------------------------------------------
//! 衝突計算
//! 円同士
//! @param [in] circle 当たり判定のしたい円
//-----------------------------------------------------------------------------
bool Circle::isHit(Circle& circle)
{
return GmSystemCollision()->HitCircle(_position, circle.getPosition(), _radius, circle.getRadius());
}
iuiAPI bool IsInside( const Circle &circle, const Position &pos )
{
Float r = circle.getRadius();
vec2 d = circle.getPosition() - pos;
return glm::dot(d,d) < r*r;
}
bool Collision::AABB_vs_Circle(const AABB& a, const Circle& c, Vector *v)
{
const Vector cpos = c.getPosition();
const Vector upleft = a.upleft();
const Vector downright = a.downright();
const bool leftside = cpos.x >= upleft.x;
const bool rightside = cpos.x <= downright.x;
const bool topside = cpos.y >= upleft.y;
const bool bottomside = cpos.y <= downright.y;
const bool inx = leftside && rightside;
const bool iny = topside && bottomside;
/*
| |
| (x) |
----+-------+----
|#######|
(y)|#######|(y)
|#######|
----+-------+----
| (x) |
| |
*/
// First check: Center completely in AABB? [(#) region ]
if(inx && iny)
{
if(v)
*v = cpos;
return true;
}
// Quick check: If AABBs are not intersecting, the circle is definitely out of reach.
const AABB caabb = c.getAABB();
if(!AABB_vs_AABB(a, caabb, NULL))
return false;
// AABBs intersecting. Now, if the circle center is contained in at least one axis,
// they are intersecting. [ (x) or (y) region ]
if(inx || iny)
{
if(v)
{
if(inx)
{
if(bottomside) *v = Vector(cpos.x, upleft.y);
else if(topside) *v = Vector(cpos.x, downright.y);
}
else
{
if(leftside) *v = Vector(downright.x, cpos.y);
else if(rightside) *v = Vector(upleft.x, cpos.y);
}
}
return true;
}
// If we are here, the circle center must be in one of the corner regions.
// Now we need to check if the circle contains the corresponding corner point of the AABB.
Vector corner;
// Above AABB?
if(cpos.y <= upleft.y)
{
// Left of AABB?
if(cpos.x <= upleft.x)
{
// Upper left
corner = upleft;
}
else
{
// Upper right
corner = Vector(downright.x, upleft.y);
}
}
else if(cpos.y >= downright.y)
{
if(cpos.x <= upleft.x)
{
// Lower left
corner = Vector(upleft.x, downright.y);
}
else
{
// Lower right.
corner = downright;
}
}
else
{
// Circle is too far away, done here.
return false;
}
if(c.isPointInside(corner))
{
if(v)
*v = corner;
return true;
}
return false;
}
