//comprobamos que la distancia entre los centros de los dos circulos sea menos que la suma de sus radios bool CollisionManager::CircleToCircle(double x1, double y1, double r1, double x2, double y2, double r2) const { double distance = SquareDistance(x1,y1,x2,y2); if (distance < (r1 + r2) * (r1 + r2)) return true; else return false; }
bool CollisionManager::CircleToRect(double cx, double cy, double cr, double rx, double ry, double rw, double rh) const { double closestX,closestY; //buscamos el punto mas cercano desde el centro hasta el rectangulo ClosestPointToRect(cx,cy,rx,ry,rw,rh,&closestX,&closestY); //si la distancia desde el centro al punto mas cercano es menor que el radio colisiona return SquareDistance(cx,cy,closestX, closestY) < (cr * cr); }
//recoremos el buffer de pixeles y comprobamos si los pixeles opacos estan a una distancia menos que el radio del circulo bool CollisionManager::CircleToPixels(double cx, double cy, double cr, const CollisionPixelData* pixels, double px, double py) const { for (int i = 0; i < pixels->GetWidth(); i++) { for (int j = 0; j < pixels->GetHeight(); j++) { if (pixels->GetData(i,j) && SquareDistance(cx, cy, i + px, j + py) < (cr * cr)) return true; } } return false; }
CColor CLandmarks::GetFloorColor(const CVector2& c_position_on_plane) { if((c_position_on_plane.GetX() >= -2.0f && c_position_on_plane.GetX() <= 2.0f) && (c_position_on_plane.GetY() >= -5.0f && c_position_on_plane.GetY() <= -1.0f)) { return CColor::GRAY90; } for(size_t i = 0; i < TARGETS; ++i) { if(SquareDistance(c_position_on_plane, m_vecTargets[i]) < TARGET_RADIUS2) { return CColor::BLACK; } } return CColor::WHITE; }
SInt32 CLandmarks::AtTarget(CFootBotEntity& c_fb) { /* * Go through the targets * If the square distance between the foot-bot center and the target center * is less than the target radius squared, return the id of the current target * Otherwise, return -1 */ for(size_t i = 0; i < TARGETS; ++i) { if(SquareDistance(CVector2(c_fb.GetEmbodiedEntity().GetOriginAnchor().Position.GetX(), c_fb.GetEmbodiedEntity().GetOriginAnchor().Position.GetY()), m_vecTargets[i]) < TARGET_RADIUS2) { return i; } } return -1; }
int Cad::CLoop3D::NumIntersecRay(const Com::CVector3D& org0, const Com::CVector3D& dir0) const { double t0 = Dot(normal,org-org0); double t1 = Dot(normal,dir0); if( fabs(t0) < 1.0e-10*fabs(t1) ){ Com::CVector2D vf2 = this->Project(org0); Com::CVector3D vf3 = this->UnProject(vf2); const double sqheight = SquareDistance(org0,vf3); if( sqheight < 1.0e-10 ) return -1; return 0; // pararell } const double r = t0/t1; if( r < -1.0e-10 ) return 0; Com::CVector3D vi3 = org0+r*dir0; Com::CVector2D vi2 = this->Project(vi3); bool is_inside = this->CheckPointInside2D(vi2); if( is_inside ){ if( r < 1.0e-10 ) return -1; return 1; } return 0; }
bool CKinematics2DEngine::CheckCollisions( const CKinematics2DCollisionCircle* pc_circle, const CKinematics2DCollisionRectangle* pc_rectangle ) { /* Rototranslate the plane so that the rectangle is axis aligned and centered in O */ CVector2 center = pc_circle->GetPosition() - pc_rectangle->GetPosition(); center.Rotate(pc_rectangle->GetOrientation() ); center.Absolute(); /* Find the Voronoi Region that the circle is in, exploiting the symmetries */ CVector2 c_half_size = pc_rectangle->GetHalfSize(); Real f_radius = pc_circle->GetRadius(); if( center.GetX() <= c_half_size.GetX() ) { /* The circle is in the top or bottom region */ return (center.GetY() <= c_half_size.GetY() + f_radius); } if( center.GetY() <= c_half_size.GetY() ) { /* The circle is in the left or right region */ return (center.GetX() <= c_half_size.GetX() + f_radius); } /* The circle is in one of the four corner regions */ return (SquareDistance( c_half_size, center ) <= f_radius*f_radius); }
/* virtual */ void CAnimation_SpawnUnit::Action(CUnit &unit, int &/*move*/, int /*scale*/) const { Assert(unit.Anim.Anim == this); const int offX = ParseAnimInt(unit, this->offXStr.c_str()); const int offY = ParseAnimInt(unit, this->offYStr.c_str()); const int range = ParseAnimInt(unit, this->rangeStr.c_str()); const int playerId = ParseAnimInt(unit, this->playerStr.c_str()); const SpawnUnit_Flags flags = (SpawnUnit_Flags)(ParseAnimFlags(unit, this->flagsStr.c_str())); CPlayer &player = Players[playerId]; const Vec2i pos(unit.tilePos.x + offX, unit.tilePos.y + offY); CUnitType *type = UnitTypeByIdent(this->unitTypeStr.c_str()); Assert(type); Vec2i resPos; DebugPrint("Creating a %s\n" _C_ type->Name.c_str()); FindNearestDrop(*type, pos, resPos, LookingW); if (SquareDistance(pos, resPos) <= square(range)) { CUnit *target = MakeUnit(*type, &player); if (target != NULL) { target->tilePos = resPos; target->Place(resPos); if (flags & SU_Summoned) { target->Summoned = 1; } if ((flags & SU_JoinToAIForce) && unit.Player->AiEnabled) { int force = unit.Player->Ai->Force.GetForce(unit); if (force != -1) { unit.Player->Ai->Force[force].Insert(*target); target->GroupId = unit.GroupId; CommandDefend(*target, unit, FlushCommands); } } //DropOutOnSide(*target, LookingW, NULL); } else { DebugPrint("Unable to allocate Unit"); } } }
bool CKinematics2DEngine::CheckCollisions( const CKinematics2DCollisionCircle* pc_circle_1, const CKinematics2DCollisionCircle* pc_circle_2 ) { Real min_distance = pc_circle_1->GetRadius() + pc_circle_2->GetRadius(); return (SquareDistance(pc_circle_1->GetPosition(), pc_circle_2->GetPosition()) <= min_distance*min_distance); }
RF_Type::Float32 Point2D<T>::Distance(const Point2D<T>& Other) const { return Float32::Sqrt(SquareDistance(Other)); }
template<class Real> double Distance (const Point3D<Real>& p1, const Point3D<Real>& p2) { return sqrt (SquareDistance (p1, p2)); }
template<class Real> void TriangleCollapse (const Real& edgeRatio, std::vector<TriangleIndex>& triangles, std::vector<Point3D<Real> >& positions, std::vector<Point3D<Real> >* normals) { int i, j, *remapTable, *pointCount, idx[3]; Point3D<Real> p[3], q[2], c; double d[3], a; double Ratio = 12.0 / sqrt (3.0); // (Sum of Squares Length / Area) for and equilateral triangle remapTable = new int[positions.size ()]; pointCount = new int[positions.size ()]; for (i = 0; i < int (positions.size ()); i++) { remapTable[i] = i; pointCount[i] = 1; } for (i = int (triangles.size () - 1); i >= 0; i--) { for (j = 0; j < 3; j++) { idx[j] = triangles[i].idx[j]; while (remapTable[idx[j]] < idx[j]) { idx[j] = remapTable[idx[j]]; } } if (idx[0] == idx[1] || idx[0] == idx[2] || idx[1] == idx[2]) { triangles[i] = triangles[triangles.size () - 1]; triangles.pop_back (); continue; } for (j = 0; j < 3; j++) { p[j].coords[0] = positions[idx[j]].coords[0] / pointCount[idx[j]]; p[j].coords[1] = positions[idx[j]].coords[1] / pointCount[idx[j]]; p[j].coords[2] = positions[idx[j]].coords[2] / pointCount[idx[j]]; } for (j = 0; j < 3; j++) { q[0].coords[j] = p[1].coords[j] - p[0].coords[j]; q[1].coords[j] = p[2].coords[j] - p[0].coords[j]; d[j] = SquareDistance (p[j], p[(j + 1) % 3]); } CrossProduct (q[0], q[1], c); a = Length (c) / 2; if ((d[0] + d[1] + d[2]) * edgeRatio > a * Ratio) { // Find the smallest edge j = 0; if (d[1] < d[j]) { j = 1; } if (d[2] < d[j]) { j = 2; } int idx1, idx2, idx3; if (idx[0] < idx[1]) { if (idx[0] < idx[2]) { idx1 = idx[0]; idx2 = idx[2]; idx3 = idx[1]; } else { idx1 = idx[2]; idx2 = idx[0]; idx3 = idx[1]; } } else { if (idx[1] < idx[2]) { idx1 = idx[1]; idx2 = idx[2]; idx3 = idx[0]; } else { idx1 = idx[2]; idx2 = idx[1]; idx3 = idx[0]; } } positions[idx1].coords[0] += positions[idx2].coords[0] + positions[idx3].coords[0]; positions[idx1].coords[1] += positions[idx2].coords[1] + positions[idx3].coords[1]; positions[idx1].coords[2] += positions[idx2].coords[2] + positions[idx3].coords[2]; if (normals) { (*normals)[idx1].coords[0] += (*normals)[idx2].coords[0] + (*normals)[idx3].coords[0]; (*normals)[idx1].coords[1] += (*normals)[idx2].coords[1] + (*normals)[idx3].coords[1]; (*normals)[idx1].coords[2] += (*normals)[idx2].coords[2] + (*normals)[idx3].coords[2]; } pointCount[idx1] += pointCount[idx2] + pointCount[idx3]; remapTable[idx2] = idx1; remapTable[idx3] = idx1; triangles[i] = triangles[triangles.size () - 1]; triangles.pop_back (); } } int pCount = 0; for (i = 0; i < int (positions.size ()); i++) { for (j = 0; j < 3; j++) { positions[i].coords[j] /= pointCount[i]; } if (normals) { Real l = Real (Length ((*normals)[i])); for (j = 0; j < 3; j++) { (*normals)[i].coords[j] /= l; } } if (remapTable[i] == i) { // If vertex i is being used positions[pCount] = positions[i]; if (normals) { (*normals)[pCount] = (*normals)[i]; } pointCount[i] = pCount; pCount++; } } positions.resize (pCount); for (i = int (triangles.size () - 1); i >= 0; i--) { for (j = 0; j < 3; j++) { idx[j] = triangles[i].idx[j]; while (remapTable[idx[j]] < idx[j]) { idx[j] = remapTable[idx[j]]; } triangles[i].idx[j] = pointCount[idx[j]]; } if (idx[0] == idx[1] || idx[0] == idx[2] || idx[1] == idx[2]) { triangles[i] = triangles[triangles.size () - 1]; triangles.pop_back (); } } delete[] pointCount; delete[] remapTable; }
void Stroker::Line(const Pointf& p3) { LLOG("Stroker::Line " << p3); if(IsNull(p1)) { Move(p3); return; } if(IsNull(p2)) { Pointf v = p3 - p1; double l = Length(v); if(l < 1e-30) return; p2 = p3; v1 = v; o1 = Ortogonal(v1) * w2 / l; a1 = p1 + o1; b1 = p1 - o1; if(IsNull(p0)) { p0 = p1; v0 = v1; o0 = o1; a0 = a1; b0 = b1; } return; } Pointf v2 = p3 - p2; double l = Length(v2); if(l < 1e-30) return; Pointf o2 = Ortogonal(v2) * w2 / l; Pointf a2 = p2 + o2; Pointf b2 = p2 - o2; double d = v1.y * v2.x - v2.y * v1.x; if(d > 1e-30) { Pointf ts = a1 + v1 * (v2.y * (a1.x - a2.x) - v2.x * (a1.y - a2.y)) / d; PutMove(a1); if(linejoin != LINEJOIN_MITER || SquareDistance(ts, p2) > qmiter) { PutLine(a1 + v1); if(linejoin == LINEJOIN_ROUND) Round(p2, o1, o2, w2); } else PutLine(ts); PutLine(a2); PutMove(b2); PutLine(p2); PutLine(b1 + v1); PutLine(b1); } else if(d < -1e-30) { Pointf ts = b2 + v2 * (v1.y * (a2.x - a1.x) - v1.x * (a2.y - a1.y)) / d; PutMove(b2); if(linejoin != LINEJOIN_MITER || SquareDistance(ts, p2) > qmiter) { if(linejoin == LINEJOIN_ROUND) Round(p2, -o2, -o1, w2); PutLine(b1 + v1); } else PutLine(ts); PutLine(b1); PutMove(a1); PutLine(a1 + v1); PutLine(p2); PutLine(a2); } else { PutMove(a1); PutLine(a1 + v1); if(linejoin == LINEJOIN_ROUND) Round(p2, o1, o2, w2); PutLine(a2); PutMove(b2); PutLine(b1 + v1); PutLine(b1); } p1 = p2; v1 = v2; o1 = o2; a1 = a2; b1 = b2; p2 = p3; }
void Collider::CircleToPolygon(Manifold *m,Body *a,Body *b) { CircleShape *A=reinterpret_cast<CircleShape*>(a->m_shape); PolygonShape *B=reinterpret_cast<PolygonShape*>(b->m_shape); m->m_contactCount=0; Vector2f center=a->m_position; center=B->u.Transpose()*(center-b->m_position); float separation=MPACK::Math::Misc<float>::c_Min; int faceNormal=0; for(int i=0;i<B->m_vertexCount;++i) { float s=Dot(B->m_normals[i],center-B->m_vertices[i]); if(s>A->radius) { return; } if(s>separation) { separation=s; faceNormal=i; } } Vector2f v1=B->m_vertices[faceNormal]; int i2=faceNormal+1<B->m_vertexCount?faceNormal+1:0; Vector2f v2 = B->m_vertices[i2]; if(separation<MPACK::Math::Misc<float>::c_Epsilon) { m->m_contactCount=1; m->m_normal=-(B->u*B->m_normals[faceNormal]); m->m_contacts[0]=m->m_normal*A->radius+a->m_position; m->m_penetration=A->radius; return; } float dot1=Dot(center-v1,v2-v1); float dot2=Dot(center-v2,v1-v2); m->m_penetration=A->radius-separation; if(dot1<=0.0f) { if(SquareDistance(center,v1)>A->radius*A->radius) { return; } m->m_contactCount=1; Vector2f n=v1-center; n=B->u*n; n.Normalize(); m->m_normal=n; v1=B->u*v1+b->m_position; m->m_contacts[0]=v1; } else if(dot2<=0.0f) { if(SquareDistance(center,v2)>A->radius*A->radius) { return; } m->m_contactCount=1; Vector2f n=v2-center; v2=B->u*v2+b->m_position; m->m_contacts[0]=v2; n=B->u*n; n.Normalize(); m->m_normal=n; } else { Vector2f n=B->m_normals[faceNormal]; if(Dot(center-v1,n)>A->radius) { return; } n=B->u*n; m->m_normal=-n; m->m_contacts[0]=m->m_normal*A->radius+a->m_position; m->m_contactCount=1; } }