package com.gebauz.bauzoid.math.collision;
import com.badlogic.gdx.Gdx;
import com.gebauz.bauzoid.app.Consts;
import com.gebauz.bauzoid.math.Line2;
import com.gebauz.bauzoid.math.Vector2;
public class CollisionUtil
{
// Constants========================================================================================
// Embedded Types===================================================================================
public static class PenetrationResult
{
public boolean isPenetrating =
false;
public Vector2 penetrationVector =
null;
public Vector2 contactPoint =
null;
public int collidingEdgeIndex = -
1;
}
public static class ProjectionResult
{
public double minimum =
Float.
MAX_VALUE;
public double maximum = -
Float.
MAX_VALUE;
/** Returns true if the projection overlaps. */
public boolean isOverlapping
(ProjectionResult other
)
{
return ((maximum
> other.
minimum) && (minimum
< other.
maximum));
}
/** Return the amount of overlap. */
public double getOverlap
(ProjectionResult other
)
{
if (!isOverlapping
(other
))
return 0.0f
;
double maxEdge =
Math.
min(maximum, other.
maximum);
double minEdge =
Math.
max(minimum, other.
minimum);
return (maxEdge - minEdge
);
}
public double flipPenetrationVector
(ProjectionResult other
)
{
if (minimum
< other.
minimum)
return -
1.0;
return 1.0;
}
public boolean contains
(ProjectionResult other
)
{
return ((minimum
<= other.
minimum) && (maximum
>= other.
maximum));
}
}
// Fields===========================================================================================
// Methods==========================================================================================
/** Calculate collision and minimal penetration vector using the separating axis theorem, return penetration vector.
* Returns null if there is no collision, or the penetration vector between the polygons.
*/
public static PenetrationResult calculatePenetration
(Line2
[] poly1, Line2
[] poly2, Vector2
[] axes1, Vector2
[] axes2
)
{
PenetrationResult result =
new PenetrationResult
();
double overlap =
Float.
MAX_VALUE;
Vector2 smallestAxis =
null;
int smallestAxisIndex = -
1;
int smallestAxisFirst =
0;
boolean fromFirst =
true;
ProjectionResult x1 =
null;
ProjectionResult x2 =
null;
String info =
new String();
info =
"---Ad";
double flip =
1.0;
// project onto first axis list
for (int i =
0; i
< axes1.
length; i++
)
{
Vector2 axis = axes1
[i
];
axis.
normalize();
ProjectionResult p1 = projectPoly
(poly1, axis
);
ProjectionResult p2 = projectPoly
(poly2, axis
);
if (!p1.
isOverlapping(p2
))
return result
;
else
{
double o = p1.
getOverlap(p2
);
double f = p1.
flipPenetrationVector(p2
);
if (p1.
contains(p2
) || p2.
contains(p1
))
{
double mins =
Math.
abs(p1.
minimum - p2.
minimum);
double maxs =
Math.
abs(p1.
maximum - p2.
maximum);
if (mins
< maxs
)
o += mins
;
else
o += maxs
;
}
// get the overlap
/*double o = p1.getOverlap(p2);
// check for containment
if (p1.contains(p2) || p2.contains(p1)) {
// get the overlap plus the distance from the minimum end points
double mins = abs(p1.min - p2.min);
double maxs = abs(p1.max - p2.max);
// NOTE: depending on which is smaller you may need to
// negate the separating axis!!
if (mins < maxs) {
o += mins;
} else {
o += maxs;
}
}
// check for minimum
if (o < overlap) {
// then set this one as the smallest
overlap = o;
smallest = axis;
}*/
/*Gdx.app.log("BLA", "---A");
Gdx.app.log("BLA", "[" + i + "] Axis: " + axis.x + ", " + axis.y);
Gdx.app.log("BLA", "[" + i + "] Interval 1: " + p1.minimum + " ... " + p1.maximum);
Gdx.app.log("BLA", "[" + i + "] Interval 2: " + p2.minimum + " ... " + p2.maximum);
Gdx.app.log("BLA", "Overlap: " + o);*/
info = info +
"\n" +
"[" + i +
"] Axis: " + axis.
x +
", " + axis.
y +
"\n[" + i +
"] Interval 1: " + p1.
minimum +
" ... " + p1.
maximum +
"\n[" + i +
"] Interval 2: " + p2.
minimum +
" ... " + p2.
maximum +
"\nOverlap: " + o
;
if (o
< overlap
)
{
overlap = o
;
smallestAxis = axis
;
smallestAxisIndex = i
;
flip = f
;
x1 = p1
;
x2 = p2
;
}
}
}
info = info +
"\n---B";
// project onto second axis list
for (int i =
0; i
< axes2.
length; i++
)
{
Vector2 axis = axes2
[i
];
axis.
normalize();
ProjectionResult p1 = projectPoly
(poly1, axis
);
ProjectionResult p2 = projectPoly
(poly2, axis
);
if (!p1.
isOverlapping(p2
))
return result
;
else
{
double o = p1.
getOverlap(p2
);
double f = p1.
flipPenetrationVector(p2
);
if (p1.
contains(p2
) || p2.
contains(p1
))
{
double mins =
Math.
abs(p1.
minimum - p2.
minimum);
double maxs =
Math.
abs(p1.
maximum - p2.
maximum);
if (mins
< maxs
)
o += mins
;
else
o += maxs
;
}
/*Gdx.app.log("BLA", "---B");
Gdx.app.log("BLA", "[" + i + "] Axis: " + axis.x + ", " + axis.y);
Gdx.app.log("BLA", "[" + i + "] Interval 1: " + p1.minimum + " ... " + p1.maximum);
Gdx.app.log("BLA", "[" + i + "] Interval 2: " + p2.minimum + " ... " + p2.maximum);
Gdx.app.log("BLA", "Overlap: " + o);*/
info = info +
"\n" +
"[" + i +
"] Axis: " + axis.
x +
", " + axis.
y +
"\n[" + i +
"] Interval 1: " + p1.
minimum +
" ... " + p1.
maximum +
"\n[" + i +
"] Interval 2: " + p2.
minimum +
" ... " + p2.
maximum +
"\nOverlap: " + o
;
if (o
< overlap
)
{
overlap = o
;
smallestAxis = axis
;
smallestAxisIndex = i
;
smallestAxisFirst =
1;
fromFirst =
false;
flip = f
;
x1 = p1
;
x2 = p2
;
}
}
}
result.
isPenetrating =
true;
smallestAxis.
x *= flip
;
smallestAxis.
y *= flip
;
result.
penetrationVector =
new Vector2
((float)(smallestAxis.
x * overlap
),
(float)(smallestAxis.
y * overlap
));
Gdx.
app.
log("BLA", info
);
Gdx.
app.
log("XXXXXX",
"--");
Gdx.
app.
log("XXXXXX",
"smallestAxis: " + smallestAxis.
x +
", " + smallestAxis.
y +
" index: " + smallestAxisIndex +
" first: " + smallestAxisFirst
);
Gdx.
app.
log("XXXXXX",
"Penetration: " + result.
penetrationVector.
x +
", " + result.
penetrationVector.
y);
Gdx.
app.
log("XXXXXX",
"Interval 1: " + x1.
minimum +
" ... " + x1.
maximum);
Gdx.
app.
log("XXXXXX",
"Interval 2: " + x2.
minimum +
" ... " + x2.
maximum);
Gdx.
app.
log("XXXXXX",
"Overlap: " + overlap
);
// determine the vertex that is "deepest" in the other shape
float maxDp =
Float.
MAX_VALUE;
int whichVertex = -
1;
Vector2 contactPoint
;
// project vertices of poly that did not create the axis
if (fromFirst
)
{
for (int i =
0; i
< poly2.
length; i++
)
{
Vector2 v =
new Vector2
(poly2
[i
].
ax, poly2
[i
].
ay);
float dp = Vector2.
dotProduct(v, smallestAxis
);
if (dp
< maxDp
)
{
maxDp = dp
;
whichVertex = i
;
}
}
contactPoint =
new Vector2
(poly2
[whichVertex
].
ax, poly2
[whichVertex
].
ay);
}
else
{
for (int i =
0; i
< poly1.
length; i++
)
{
Vector2 v =
new Vector2
(poly1
[i
].
ax, poly1
[i
].
ay);
float dp = Vector2.
dotProduct(v, smallestAxis
);
if (dp
< maxDp
)
{
maxDp = dp
;
whichVertex = i
;
}
}
contactPoint =
new Vector2
(poly1
[whichVertex
].
ax, poly1
[whichVertex
].
ay);
}
// that vertex position plus penetration vector should be the contact point
//Vector2 v = new Vector2(poly1[whichVertex].ax, poly1[whichVertex].ay);
contactPoint.
addVector(result.
penetrationVector);
result.
contactPoint = contactPoint
;
return result
;
}
public static boolean isLeft
(Vector2 a, Vector2 b, Vector2 c
)
{
return ((b.
x - a.
x)*(c.
y - a.
y) -
(b.
y - a.
y)*(c.
x - a.
x)) >= 0.0f
;
}
/** Get the center point of a poly. */
public static Vector2 getCenter
(Line2
[] poly
)
{
Vector2 result =
new Vector2
(0,
0);
for (int i =
0; i
< poly.
length; i++
)
{
// need just one point because it should be a closed polygon
result.
x += poly
[i
].
ax;
result.
y += poly
[i
].
ay;
}
result.
x /=
(float)poly.
length;
result.
y /=
(float)poly.
length;
return result
;
}
/** Project the polygon onto an axis. */
public static ProjectionResult projectPoly
(Line2
[] poly, Vector2 axis
)
{
ProjectionResult result =
new ProjectionResult
();
for (int i =
0; i
< poly.
length; i++
)
{
// project the vector from origin to each of the line segment's end points onto b
// the result is the section on the axis
Vector2 a =
new Vector2
(poly
[i
].
ax, poly
[i
].
ay);
Vector2 b =
new Vector2
(poly
[i
].
bx, poly
[i
].
by);
float dpA = Vector2.
dotProduct(a, axis
);
float dpB = Vector2.
dotProduct(b, axis
);
//Gdx.app.log(Consts.LOG_TAG, "a: " + a.x + ", " + a.y + " (" + dpA + ") - b: " + b.x + ", " + b.y + " (" + dpB + ")");
if (dpA
< result.
minimum)
result.
minimum = dpA
;
if (dpB
< result.
minimum)
result.
minimum = dpB
;
if (dpA
> result.
maximum)
result.
maximum = dpA
;
if (dpB
> result.
maximum)
result.
maximum = dpB
;
}
return result
;
}
// Getters/Setters==================================================================================
}