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