//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;
}
}
