public static int side(Vector2f lineA, Vector2f lineB, Vector2f point)
{
Vector2f diff = lineB - lineA;
Vector2f perp = new Vector2f(-diff.Y, diff.X);
float d = AppMath.Vec2Dot(point - lineA, perp);
return(Math.Sign(d));
}
private static bool IntervalIntersect(Vector2f[] A, int Anum,
Vector2f[] B, int Bnum,
Vector2f xAxis,
Vector2f xOffset, Vector2f xVel,
ref float taxis, float tmax)
{
float min0, max0;
float min1, max1;
GetInterval(A, Anum, xAxis, out min0, out max0);
GetInterval(B, Bnum, xAxis, out min1, out max1);
float h = AppMath.Vec2Dot(xOffset, xAxis);
min0 += h;
max0 += h;
float d0 = min0 - max1; // if overlapped, do < 0
float d1 = min1 - max0; // if overlapped, d1 > 0
// separated, test dynamic intervals
if (d0 > 0.0f || d1 > 0.0f)
{
float v = AppMath.Vec2Dot(xVel, xAxis);
// small velocity, so only the overlap test will be relevant.
if (Math.Abs(v) < 0.000001f) //0.0000001f
{
return(false);
}
float t0 = -d0 / v; // time of impact to d0 reaches 0
float t1 = d1 / v; // time of impact to d0 reaches 1
if (t0 > t1)
{
float temp = t0;
t0 = t1;
t1 = temp;
}
taxis = (t0 > 0.0f) ? t0 : t1;
if (taxis < 0.0f || taxis > tmax)
{
return(false);
}
return(true);
}
else
{
// overlap. get the interval, as a the smallest of |d0| and |d1|
// return negative number to mark it as an overlap
taxis = (d0 > d1) ? d0 : d1;
return(true);
}
}
public static Vector2f processWorldObjCollision(Vector2f velocity, Vector2f N)
{
Vector2f D = velocity;
float n = AppMath.Vec2Dot(D, N);
Vector2f Dn = N * n;
Vector2f Dt = D - Dn;
if (n > 0.0f)
{
Dn = new Vector2f(0, 0);
}
float dt = AppMath.Vec2Dot(Dt, Dt);
float CoF = Constants.FRICTION;
if (dt < Constants.GLUE * Constants.GLUE)
{
CoF = 1.01f;
}
//D = -(1.0f + s_fRestitution) * Dn - (CoF) * Dt;
D.X = -(1.0f + Constants.RESTITUTION) * Dn.X - (CoF) * Dt.X;
D.Y = -(1.0f + Constants.RESTITUTION) * Dn.Y - (CoF) * Dt.Y;
float m0 = 1.0f;
float m1 = 1.0f;
float m = m0 + m1;
float r0 = m0 / m; //m
float r1 = m1 / m; //m
//if (m0 > 0.0f)
{
velocity.X += D.X * r0;
velocity.Y += D.Y * r0;
}
return(velocity);
/*
* if (m1 > 0.0f)
* {
* objB->Velocity.x += D.x * -r1;
* objB->Velocity.y += D.y * -r1;
* }
*/
}
// calculate the projection range of a polygon along an axis
private static void GetInterval(Vector2f[] axVertices, int iNumVertices, Vector2f xAxis, out float min, out float max)
{
min = max = AppMath.Vec2Dot(axVertices[0], xAxis);
for (int i = 1; i < iNumVertices; i++)
{
float d = AppMath.Vec2Dot(axVertices[i], xAxis);
if (d < min)
{
min = d;
}
else if (d > max)
{
max = d;
}
}
}
// two objects overlapped. push them away from each other
private static void ProcessCollision(cGameObject objA, cGameObject objB, Vector2f N, float t)
{
//Vector2f D = m_xDisplacement - xBody.m_xDisplacement;
Vector2f D = objA.Velocity - objB.Velocity;
float n = AppMath.Vec2Dot(D, N);
Vector2f Dn = N * n;
Vector2f Dt = D - Dn;
if (n > 0.0f)
{
Dn = new Vector2f(0, 0);
}
float dt = AppMath.Vec2Dot(Dt, Dt);
float CoF = Constants.FRICTION;
if (dt < Constants.GLUE * Constants.GLUE)
{
CoF = 1.01f;
}
//D = -(1.0f + Constants.RESTITUTION) * Dn - (CoF) * Dt;
D.X = -(1.0f + Constants.RESTITUTION) * Dn.X - (CoF) * Dt.X;
D.Y = -(1.0f + Constants.RESTITUTION) * Dn.Y - (CoF) * Dt.Y;
float m0 = objA.Mass;
float m1 = objB.Mass;
float m = m0 + m1;
float r0 = m0 / m; //m
float r1 = m1 / m; //m
if (m0 > 0.0f)
{
objA.AddVelocity(D * r0);
/*
* Vector2f velA = objA.Velocity + D * r0;
* objA.Velocity = velA;
*/
/*
* objA.Velocity.X += D.X * r0;
* objA.Velocity.Y += D.Y * r0;
*/
}
if (m1 > 0.0f)
{
objB.AddVelocity(D * (-r1));
/*
* Vector2f velB = objB.Velocity + D * -r1;
* objB.Velocity = velB;
*/
/*
* objB.Velocity.X += D.X * -r1;
* objB.Velocity.Y += D.Y * -r1;
*/
}
}
private static bool Collide(Vector2f[] A, int Anum,
Vector2f[] B, int Bnum,
Vector2f xOffset,
Vector2f xVel,
ref Vector2f N,
ref float t)
{
if (A == null || B == null)
{
return(false);
}
// All the separation axes
const int _max = 64; //64
Vector2f[] xAxis = new Vector2f[_max]; // note : a maximum of (if 64 - 32) 4 vertices per poly is supported
float[] taxis = new float[_max];
int iNumAxes = 0;
xAxis[iNumAxes] = new Vector2f(-xVel.Y, xVel.X);
float fVel2 = AppMath.Vec2Dot(xVel, xVel);
if (fVel2 > 0.000001f)
{
if (!IntervalIntersect(A, Anum,
B, Bnum,
xAxis[iNumAxes],
xOffset, xVel,
ref taxis[iNumAxes], t))
{
return(false);
}
iNumAxes++;
}
// test separation axes of A
for (int j = Anum - 1, i = 0; i < Anum; j = i, i++)
{
Vector2f E0 = A[j];
Vector2f E1 = A[i];
Vector2f E = E1 - E0;
xAxis[iNumAxes] = new Vector2f(-E.Y, E.X);
if (!IntervalIntersect(A, Anum,
B, Bnum,
xAxis[iNumAxes],
xOffset, xVel,
ref taxis[iNumAxes], t))
{
return(false);
}
iNumAxes++;
}
// test separation axes of B
for (int j = Bnum - 1, i = 0; i < Bnum; j = i, i++)
{
Vector2f E0 = B[j];
Vector2f E1 = B[i];
Vector2f E = E1 - E0;
xAxis[iNumAxes] = new Vector2f(-E.Y, E.X);
if (!IntervalIntersect(A, Anum,
B, Bnum,
xAxis[iNumAxes],
xOffset, xVel,
ref taxis[iNumAxes], t))
{
return(false);
}
iNumAxes++;
}
// special case for segments
if (Bnum == 2)
{
Vector2f E = B[1] - B[0];
xAxis[iNumAxes] = E;
if (!IntervalIntersect(A, Anum,
B, Bnum,
xAxis[iNumAxes],
xOffset, xVel,
ref taxis[iNumAxes], t))
{
return(false);
}
iNumAxes++;
}
// special case for segments
if (Anum == 2)
{
Vector2f E = A[1] - A[0];
xAxis[iNumAxes] = E;
if (!IntervalIntersect(A, Anum,
B, Bnum,
xAxis[iNumAxes],
xOffset, xVel,
ref taxis[iNumAxes], t))
{
return(false);
}
iNumAxes++;
}
if (!FindMTD(xAxis, taxis, iNumAxes, ref N, ref t))
{
return(false);
}
// make sure the polygons gets pushed away from each other.
if (AppMath.Vec2Dot(N, xOffset) < 0.0f)
{
N = -N;
}
return(true);
}