/// <summary> /// Gets the variable linear variable angular position. /// </summary> /// <param name="v">Velocity.</param> /// <param name="a">Acceleration.</param> /// <param name="position">Position.</param> /// <param name="w">Angular velocity.</param> /// <param name="n">Angular acceleration.</param> /// <param name="b">Facing.</param> /// <param name="t">Time.</param> public static void GetVarLinearVarAngularPosition(ref Vector2 velocity, float maxVelocity, Vector2 accelerationDirection, float acceleration, ref Vector2 position, ref float angularVelocity, float angularAcceleration, ref float facing, float time) { // if (Math.Abs(angularAcceleration) < Epsilon) { GetVarLinearConstantAngularPosition(ref velocity, maxVelocity, accelerationDirection, acceleration, ref position, angularVelocity, ref facing, time); return; } Vector2 endVelocity = velocity + accelerationDirection * acceleration * time; //Vector2 endVelocity = accelerationDirection*(Vector2.Dot(velocity, accelerationDirection) + acceleration * time); // Vector2 endVelocity = v + (a > 0 ? Vector2.Rotation(b) : Vector2.Normalize(v)) * t * a; float endAngularVelocity = angularVelocity + angularAcceleration * time; float endFacing = facing + (angularVelocity + endAngularVelocity) * time / 2.0f; float sn = (float)Math.Sqrt(Math.Abs(angularAcceleration)); float spi = (float)Math.Sqrt(Math.PI); float divider = acceleration / (sn * Math.Abs(angularAcceleration)); // common denominator, a/n^1.5 float bwn = facing - angularVelocity * angularVelocity / (2 * angularAcceleration); // b - w^2/2n, some kind of angle? float nvaw = angularVelocity * spi; float cosBwn = (float)Math.Cos(bwn); float sinBwn = (float)Math.Sin(bwn); float ws = angularVelocity / (spi * sn); // StartAngularVelocity / sqrt(2*PI*n) float ntws = (angularAcceleration * time + angularVelocity) / (spi * sn); // float frenC1, frenC2, frenS1, frenS2; Fresnel(ws, out frenC1, out frenS1); Fresnel(ntws, out frenC2, out frenS2); float frenC = (float)(frenC1 - frenC2); float frenS = (float)(frenS1 - frenS2); float sinDiff = sn * ((float)Math.Sin(facing) - (float)Math.Sin(endFacing)); float cosDiff = sn * ((float)Math.Cos(facing) - (float)Math.Cos(endFacing)); float x, y; if (angularAcceleration > 0) { x = (nvaw * (cosBwn * frenC - sinBwn * frenS) - sinDiff) * divider; y = (nvaw * (cosBwn * frenS + sinBwn * frenC) + cosDiff) * divider; } else { x = (nvaw * (cosBwn * frenC + sinBwn * frenS) + sinDiff) * divider; y = (nvaw * (-cosBwn * frenS + sinBwn * frenC) - cosDiff) * divider; } position += velocity * time + new Vector2(x, y); velocity = endVelocity; angularVelocity = endAngularVelocity; facing = endFacing; }
internal static bool CollideCircles(Vector2 oneCenter, float oneRadius, Transform oneTransform, Vector2 anotherCenter, float anotherRadius, Transform anotherTransform, out CollisionResult result) { Vector2 onePoint = oneTransform.Mul(oneCenter); Vector2 anotherPoint = anotherTransform.Mul(anotherCenter); float radius = oneRadius + anotherRadius; Vector2 dir = anotherPoint - onePoint; float distanceSqrd = dir.LengthSquared(); if (distanceSqrd > radius * radius) { result = default(CollisionResult); return(false); } float distance = (float)Math.Sqrt(distanceSqrd); dir /= distance; float collisionLength = (oneRadius + anotherRadius - distance); Vector2 point = onePoint + dir * (oneRadius + collisionLength / 2); // center of the collision result = new CollisionResult(point, dir * collisionLength); // we need to shift one of circles to collision length distance return(true); }
private static void SinCos(float facing, out float sin, out float cos) { #if !USE_MATHF sin = (float)Math.Sin(facing); cos = (float)Math.Cos(facing); #else Math.SinCos(facing, out sin, out cos); #endif }
/// <summary> /// Calculates position with variable linear speed and zero angular speed /// </summary> /// <param name="velocity">Linear velocity</param> /// <param name="acceleration">Linear acceleration</param> /// <param name="position">Position</param> /// <param name="facing">Angle</param> /// <param name="time">Time</param> public static void GetVarLinearPosition(ref Vector2 velocity, float maxVelocity, Vector2 accelerationDirection, float acceleration, ref Vector2 position, float facing, float time) { float exp = (float)Math.Exp(-acceleration * time / maxVelocity); Vector2 endVelocity = accelerationDirection * maxVelocity * (1 - exp) + velocity * exp; float k = maxVelocity * (1 - exp) / acceleration; position += accelerationDirection * maxVelocity * (time - k) + velocity * k; velocity = endVelocity; }
/// <summary> /// Calculates position with variable linear speed and constant angular speed /// </summary> /// <returns>The variable linear constant angular shift.</returns> /// <param name="velocity">Linear velocity</param> /// <param name="acceleration">Linear acceleration</param> /// <param name="position">Position</param> /// <param name="angularVelocity">Constant angular velocity</param> /// <param name="facing">Angle</param> /// <param name="time">Time</param> public static void GetVarLinearConstantAngularPosition(ref Vector2 velocity, float maxVelocity, Vector2 accelerationDirection, float acceleration, ref Vector2 position, float angularVelocity, ref float facing, float time) { if (Math.Abs(angularVelocity) < Epsilon) { GetVarLinearPosition(ref velocity, maxVelocity, accelerationDirection, acceleration, ref position, facing, time); return; } float sinb; float cosb; SinCos(facing, out sinb, out cosb); float endFacing = facing + angularVelocity * time; float sinend; float cosend; SinCos(endFacing, out sinend, out cosend); float exp = (float)Math.Exp(-acceleration * time / maxVelocity); float vw = maxVelocity * angularVelocity; float divider = maxVelocity * maxVelocity / (acceleration * acceleration + vw * vw); float vx = maxVelocity * cosend - (maxVelocity * cosend - velocity.X) * exp; float vy = maxVelocity * sinend - (maxVelocity * sinend - velocity.Y) * exp; Vector2 endVelocity = new Vector2(vx, vy); Vector2 shift = velocity * maxVelocity * (1 - exp) / acceleration; float x = (-sinb + sinend) * maxVelocity / angularVelocity + divider * (exp * (acceleration * cosend - vw * sinend) - (acceleration * cosb - vw * sinb)); float y = (cosb - cosend) * maxVelocity / angularVelocity - divider * (-exp * (acceleration * sinend + vw * cosend) + (acceleration * sinb + vw * cosb)); position += shift + new Vector2(x, y); velocity = endVelocity; facing = endFacing; }
/// <summary> /// Calculates average movement time to the point /// </summary> /// <returns>The average time.</returns> /// <param name="velocity">Velocity.</param> /// <param name="maxVelocity">Max velocity.</param> /// <param name="position">Position.</param> /// <param name="target">Target.</param> public static float GetAverageMoveTime(Vector velocity, float maxVelocity, float damping, float acceleration, Vector position, Vector target, float epsilon) { Vector direction = (target - position); float distance = direction.Normalize(); float v = Vector.Dot(velocity, direction); float dampingDistance = maxVelocity * maxVelocity * s_oneMinusLog2 / damping; float t; if (distance > dampingDistance) { t = (distance - dampingDistance) / maxVelocity + (maxVelocity - v) / acceleration; // rought acceleration time, will be smaller than actual time } else { t = 0.1f; } float ndistance; float u; int iterations = 10; // limit the cycle do { float exp = (float)Math.Exp(-acceleration * t / maxVelocity); u = maxVelocity - (maxVelocity - v) * exp; ndistance = maxVelocity * (t - (u - v) / acceleration) + GetDampingDistance(u, maxVelocity, damping); if (ndistance >= distance + epsilon) // average goal { break; } t += 0.1f; } while(ndistance < distance && --iterations >= 0); return(t + maxVelocity * (float)Math.Log(1.0f + u / maxVelocity) / damping); }
//This part knows nothing about shapes and works only with raw data internal static bool CollidePoints(Vector2 one, Transform oneTransform, Vector2 another, Transform anotherTransform, out CollisionResult result) { Vector2 onePoint = oneTransform.Mul(one); Vector2 anotherPoint = anotherTransform.Mul(another); Vector2 dir = onePoint - anotherPoint; float distanceSqrd = dir.LengthSquared(); if (distanceSqrd > float.Epsilon) { result = default(CollisionResult); return(false); } #if !USE_MATHF dir /= (float)Math.Sqrt(distanceSqrd); // that function requires define and additional check #else dir *= Math.InvSqrt(distanceSqrd); #endif result = new CollisionResult(onePoint, dir); return(true); }
internal static bool CollideRectangleAndPoint(Vector2 anotherCenter, float anotherWidth, float anotherHeight, Transform anotherTransform, Vector2 one, Transform oneTransform, out CollisionResult result) { Vector2 worldPoint = oneTransform.Mul(one); Vector2 anotherPoint = (anotherTransform.MulT(worldPoint) - anotherCenter) * 2; // note the * 2 here, that is done to use width/height and not width*0.5 if (anotherPoint.X < -anotherWidth || anotherPoint.X > anotherWidth || anotherPoint.Y < -anotherHeight || anotherPoint.Y > anotherHeight) { result = default(CollisionResult); return(false); } float[] distances = { Math.Abs(-anotherWidth - worldPoint.X), // left Math.Abs(anotherHeight - worldPoint.Y), // top Math.Abs(anotherWidth - worldPoint.X), // right Math.Abs(-anotherHeight - worldPoint.Y), // bottom }; int index = -1; float dist = float.MaxValue; for (int i = 0; i < distances.Length; i++) { if (distances[i] < dist) { dist = distances[i]; index = i; } } Vector2 normal = anotherTransform.MulR(RectangleShape.RectangleNormals[index]) * dist; result = new CollisionResult(worldPoint, normal); return(true); }
private static void Fresnel(float x, out float c, out float s) { double xxa = 0; double f = 0; double g = 0; double t = 0; double u = 0; double sn = 0; double sd = 0; double cn = 0; double cd = 0; double fn = 0; double fd = 0; double gn = 0; double gd = 0; xxa = x; x = Math.Abs(x); float x2 = x * x; if (x2 < 2.5625) { t = x2 * x2; sn = -2.99181919401019853726E3; sn = sn * t + 7.08840045257738576863E5; sn = sn * t - 6.29741486205862506537E7; sn = sn * t + 2.54890880573376359104E9; sn = sn * t - 4.42979518059697779103E10; sn = sn * t + 3.18016297876567817986E11; sd = 1.00000000000000000000E0; sd = sd * t + 2.81376268889994315696E2; sd = sd * t + 4.55847810806532581675E4; sd = sd * t + 5.17343888770096400730E6; sd = sd * t + 4.19320245898111231129E8; sd = sd * t + 2.24411795645340920940E10; sd = sd * t + 6.07366389490084639049E11; cn = -4.98843114573573548651E-8; cn = cn * t + 9.50428062829859605134E-6; cn = cn * t - 6.45191435683965050962E-4; cn = cn * t + 1.88843319396703850064E-2; cn = cn * t - 2.05525900955013891793E-1; cn = cn * t + 9.99999999999999998822E-1; cd = 3.99982968972495980367E-12; cd = cd * t + 9.15439215774657478799E-10; cd = cd * t + 1.25001862479598821474E-7; cd = cd * t + 1.22262789024179030997E-5; cd = cd * t + 8.68029542941784300606E-4; cd = cd * t + 4.12142090722199792936E-2; cd = cd * t + 1.00000000000000000118E0; s = (float)(Math.Sign(xxa) * x * x2 * sn / sd); c = (float)(Math.Sign(xxa) * x * cn / cd); return; } if (x > 36974.0) { c = Math.Sign(xxa) * 0.5f; s = Math.Sign(xxa) * 0.5f; return; } t = Math.PI * x2; u = 1 / (t * t); t = 1 / t; fn = 4.21543555043677546506E-1; fn = fn * u + 1.43407919780758885261E-1; fn = fn * u + 1.15220955073585758835E-2; fn = fn * u + 3.45017939782574027900E-4; fn = fn * u + 4.63613749287867322088E-6; fn = fn * u + 3.05568983790257605827E-8; fn = fn * u + 1.02304514164907233465E-10; fn = fn * u + 1.72010743268161828879E-13; fn = fn * u + 1.34283276233062758925E-16; fn = fn * u + 3.76329711269987889006E-20; fd = 1.00000000000000000000E0; fd = fd * u + 7.51586398353378947175E-1; fd = fd * u + 1.16888925859191382142E-1; fd = fd * u + 6.44051526508858611005E-3; fd = fd * u + 1.55934409164153020873E-4; fd = fd * u + 1.84627567348930545870E-6; fd = fd * u + 1.12699224763999035261E-8; fd = fd * u + 3.60140029589371370404E-11; fd = fd * u + 5.88754533621578410010E-14; fd = fd * u + 4.52001434074129701496E-17; fd = fd * u + 1.25443237090011264384E-20; gn = 5.04442073643383265887E-1; gn = gn * u + 1.97102833525523411709E-1; gn = gn * u + 1.87648584092575249293E-2; gn = gn * u + 6.84079380915393090172E-4; gn = gn * u + 1.15138826111884280931E-5; gn = gn * u + 9.82852443688422223854E-8; gn = gn * u + 4.45344415861750144738E-10; gn = gn * u + 1.08268041139020870318E-12; gn = gn * u + 1.37555460633261799868E-15; gn = gn * u + 8.36354435630677421531E-19; gn = gn * u + 1.86958710162783235106E-22; gd = 1.00000000000000000000E0; gd = gd * u + 1.47495759925128324529E0; gd = gd * u + 3.37748989120019970451E-1; gd = gd * u + 2.53603741420338795122E-2; gd = gd * u + 8.14679107184306179049E-4; gd = gd * u + 1.27545075667729118702E-5; gd = gd * u + 1.04314589657571990585E-7; gd = gd * u + 4.60680728146520428211E-10; gd = gd * u + 1.10273215066240270757E-12; gd = gd * u + 1.38796531259578871258E-15; gd = gd * u + 8.39158816283118707363E-19; gd = gd * u + 1.86958710162783236342E-22; f = 1 - u * fn / fd; g = t * gn / gd; float tt = Mathf.HalfPI * x2; float cc = (float)Math.Cos((float)t); float ss = (float)Math.Sin((float)t); tt = Mathf.HalfPI * x; c = (float)(0.5f + (f * ss - g * cc) / tt); s = (float)(0.5f - (f * cc + g * ss) / tt); c = c * Math.Sign(xxa); s = s * Math.Sign(xxa); }
/// <summary> /// Calculates the time untill full stop with specified speed and damping /// </summary> /// <returns>The damping time.</returns> /// <param name="velocity">Velocity.</param> /// <param name="maxVelocity">Max velocity.</param> /// <param name="damping">Damping.</param> public static float GetDampingTime(float velocity, float maxVelocity, float damping) { float speedPct = velocity / maxVelocity; return(maxVelocity * (speedPct - (float)Math.Log(1 + speedPct)) / Math.Abs(damping)); }
internal static bool CollidePolygons(Vector2[] one, Vector2[] oneNormals, Transform oneTransform, Vector2[] nanotherPoints, Vector2[] nanotherNormals, Transform anotherTransform, out CollisionResult result) { result = default(CollisionResult); if (s_cachePoints == null || nanotherPoints.Length != s_cachePoints.Length) { s_cachePoints = new Vector2[nanotherPoints.Length]; s_cacheNormals = new Vector2[nanotherNormals.Length]; } Vector2[] anotherPoints = s_cachePoints; Vector2[] anotherNormals = s_cacheNormals; for (int i = 0; i < anotherPoints.Length; i++) { anotherPoints[i] = oneTransform.MulT(anotherTransform.Mul(nanotherPoints[i])); anotherNormals[i] = oneTransform.MulTR(anotherTransform.MulR(nanotherNormals[i])); } float minDistance = float.MaxValue; Vector2 minNormal = Vector2.Zero; // Loop through all the edges of both polygons for (int i = 0; i < oneNormals.Length; i++) { Vector2 normal = oneNormals[i]; // Find the projection of the polygon on the current axis float minA; float maxA; ProjectPolygon(normal, one, out minA, out maxA); float minB; float maxB; ProjectPolygon(normal, anotherPoints, out minB, out maxB); if (minA > maxB || maxA < minB) { return(false); } float distance = Math.Max(minA, minB) - Math.Min(maxA, maxB); if (distance < minDistance) { minDistance = distance; minNormal = normal; } } for (int i = 0; i < anotherNormals.Length; i++) { Vector2 normal = anotherNormals[i]; // Find the projection of the polygon on the current axis float minA; float maxA; ProjectPolygon(normal, one, out minA, out maxA); float minB; float maxB; ProjectPolygon(normal, anotherPoints, out minB, out maxB); if (minA > maxB || maxA < minB) { return(false); } float distance = Math.Max(minA, minB) - Math.Min(maxA, maxB); if (distance < minDistance) { minDistance = distance; minNormal = normal; } } float x; { Vector2 axisX = new Vector2(1, 0); float minA; float maxA; ProjectPolygon(axisX, one, out minA, out maxA); float minB; float maxB; ProjectPolygon(axisX, anotherPoints, out minB, out maxB); x = (Math.Max(minA, minB) + Math.Min(maxA, maxB)) / 2; } float y; { Vector2 axisY = new Vector2(0, 1); float minA; float maxA; ProjectPolygon(axisY, one, out minA, out maxA); float minB; float maxB; ProjectPolygon(axisY, anotherPoints, out minB, out maxB); y = (Math.Max(minA, minB) + Math.Min(maxA, maxB)) / 2; } // TODO: correct polygon-polygon collision result result = new CollisionResult(oneTransform.Mul(new Vector2(x, y)), oneTransform.MulR(minNormal)); return(true); }
internal static bool CollidePolygonAndCircle(Vector2[] one, Vector2[] oneNormals, Transform oneTransform, Vector2 anotherCenter, float anotherRadius, Transform anotherTransform, out CollisionResult result) { result = default(CollisionResult); Vector2 worldCenter = anotherTransform.Mul(anotherCenter); Vector2 anotherPoint = oneTransform.MulT(worldCenter); #if OLD int index = -1; float dist = float.MaxValue; // Loop through all the edges for (int i = 0; i < oneNormals.Length; i++) { Vector2 normal = oneNormals[i]; float pointA = Vector2.Dot(anotherPoint, normal); // Find the projection of the polygon on the current axis float minA = 0; float maxA = 0; ProjectPolygon(normal, one, out minA, out maxA); if (pointA + anotherRadius < minA || pointA - anotherRadius > maxA) { return(false); } float distance = Vector2.Dot(one[i] - anotherPoint, normal); if (-distance > anotherRadius) { return(false); } if (distance < dist) { dist = distance; index = i; } } float x; { Vector2 axisX = new Vector2(1, 0); float minA = 0; float maxA = 0; ProjectPolygon(axisX, one, out minA, out maxA); float pointA = Vector2.Dot(anotherPoint, axisX); float minB = pointA - anotherRadius; float maxB = pointA + anotherRadius; x = (Math.Max(minA, minB) + Math.Min(maxA, maxB)) / 2; } float y; { Vector2 axisY = new Vector2(0, 1); float minA = 0; float maxA = 0; ProjectPolygon(axisY, one, out minA, out maxA); float pointA = Vector2.Dot(anotherPoint, axisY); float minB = pointA - anotherRadius; float maxB = pointA + anotherRadius; y = (Math.Max(minA, minB) + Math.Min(maxA, maxB)) / 2; } Vector2 resultPoint = oneTransform.Mul(new Vector2(x, y)); Vector2 resultNormal = oneTransform.MulR(oneNormals[index]); result = new CollisionResult(resultPoint, resultNormal); return(true); #elif true int index = -1; float dist = float.MaxValue; // Loop through all the edges for (int i = 0; i < oneNormals.Length; i++) { Vector2 normal = oneNormals[i]; float pointA = Vector2.Dot(anotherPoint, normal); // Find the projection of the polygon on the current axis float minA = 0; float maxA = 0; ProjectPolygon(normal, one, out minA, out maxA); if (pointA + anotherRadius < minA || pointA - anotherRadius > maxA) { return(false); } float distance = Vector2.Dot(one[i] - anotherPoint, normal); if (-distance > anotherRadius) { return(false); } if (distance < dist) { dist = distance; index = i; } } // Vertices that subtend the incident face. int vertIndex1 = index; int vertIndex2 = (vertIndex1 + 1) % one.Length; Vector2 v1 = one[vertIndex1]; Vector2 v2 = one[vertIndex2]; // If the center is inside the polygon ... if (-dist < float.Epsilon) { Vector2 localNormal = oneNormals[index]; Vector2 localPoint = (v1 + v2) / 2; Vector2 resultPoint = oneTransform.Mul(localPoint); Vector2 resultNormal = oneTransform.MulR(localNormal); result = new CollisionResult(resultPoint, resultNormal); return(true); } // Compute barycentric coordinates float u1 = Vector2.Dot(anotherPoint - v1, v2 - v1); float u2 = Vector2.Dot(anotherPoint - v2, v1 - v2); if (u1 <= 0.0f) { float r = Vector2.DistanceSquared(anotherPoint, v1); if (r > anotherRadius * anotherRadius) { return(false); } Vector2 localNormal = (anotherPoint - v1) / r; Vector2 resultPoint = oneTransform.Mul(v1); Vector2 resultNormal = oneTransform.MulR(localNormal);// * Math.Sqrt(r); result = new CollisionResult(resultPoint, resultNormal); } else if (u2 <= 0.0f) { float r = Vector2.DistanceSquared(anotherPoint, v2); if (r > anotherRadius * anotherRadius) { return(false); } Vector2 localNormal = (anotherPoint - v2) / r; Vector2 resultPoint = oneTransform.Mul(v2); Vector2 resultNormal = oneTransform.MulR(localNormal);// * Math.Sqrt(r); result = new CollisionResult(resultPoint, resultNormal); } else { Vector2 faceCenter = (v1 * u1 + v2 * u2) / (u1 + u2); //if (Vector2.DistanceSquared(anotherPoint, faceCenter) > anotherRadius * anotherRadius) //return false; float s = Vector2.Dot(oneNormals[vertIndex1], anotherPoint - faceCenter); if (s > anotherRadius) { return(false); } Vector2 localNormal = oneNormals[vertIndex1]; Vector2 resultPoint = oneTransform.Mul(faceCenter); Vector2 resultNormal = oneTransform.MulR(localNormal);// * s; result = new CollisionResult(resultPoint, resultNormal); } return(true); #else // Compute circle position in the frame of the polygon. // Find the min separating edge. int index = 0; float separation = float.MinValue; float radius = anotherRadius + Mathf.Epsilon; int vertexCount = one.Length; for (int i = 0; i < vertexCount; ++i) { float distance = Vector2.Dot(oneNormals[i], anotherPoint - one[i]); if (distance > radius) { //Early out. return(false); } if (distance > separation) { separation = distance; index = i; } } int vertIndex1 = index; int vertIndex2 = (vertIndex1 + 1) % one.Length; Vector2 v1 = one[vertIndex1]; Vector2 v2 = one[vertIndex2]; // If the center is inside the polygon ... if (separation < Mathf.Epsilon) { Vector2 localNormal = oneNormals[index]; Vector2 localPoint = (v1 + v2) / 2; Vector2 resultPoint = oneTransform.Mul(localPoint); Vector2 resultNormal = oneTransform.MulR(localNormal); result = new CollisionResult(resultPoint, resultNormal); return(true); } float u1 = Vector2.Dot(anotherPoint - v1, v2 - v1); float u2 = Vector2.Dot(anotherPoint - v2, v1 - v2); if (u1 <= 0.0f) { float r = Vector2.DistanceSquared(anotherPoint, v1); if (r > anotherRadius * anotherRadius) { return(false); } Vector2 localNormal = anotherPoint - v1; localNormal.Normalize(); Vector2 resultPoint = oneTransform.Mul(v1); Vector2 resultNormal = oneTransform.MulR(localNormal); result = new CollisionResult(resultPoint, resultNormal); } else if (u2 <= 0.0f) { float r = Vector2.DistanceSquared(anotherPoint, v2); if (r > anotherRadius * anotherRadius) { return(false); } Vector2 localNormal = anotherPoint - v2; localNormal.Normalize(); Vector2 resultPoint = oneTransform.Mul(v2); Vector2 resultNormal = oneTransform.MulR(localNormal); result = new CollisionResult(resultPoint, resultNormal); } else { Vector2 faceCenter = //(v1 + v2) / 2; (v1 * u1 + v2 * u2) / (u1 + u2); if (Vector2.DistanceSquared(anotherPoint, faceCenter) > anotherRadius * anotherRadius) { return(false); } float s = Vector2.Dot(oneNormals[vertIndex1], anotherPoint - faceCenter); if (s > anotherRadius) { return(false); } Vector2 localNormal = oneNormals[vertIndex1]; Vector2 resultPoint = oneTransform.Mul(faceCenter); Vector2 resultNormal = oneTransform.MulR(localNormal); result = new CollisionResult(resultPoint, resultNormal); } return(true); #endif }
internal static bool CollideRectangleAndCircle(Vector2 anotherCenter, float anotherWidth, float anotherHeight, Transform anotherTransform, Vector2 oneCenter, float oneRadius, Transform oneTransform, out CollisionResult result) { Vector2 anotherPoint = anotherTransform.MulT(oneTransform.Mul(oneCenter)) - anotherCenter; result = default(CollisionResult); // resulting X and Y are invalid in some rare cases, that why we need to rewrite this method or use RECT_AS_POLY float x; { Vector2 axisX = new Vector2(1, 0); float minA = -anotherWidth / 2; float maxA = anotherWidth / 2; float pointA = Vector2.Dot(anotherPoint, axisX); float minB = pointA - oneRadius; float maxB = pointA + oneRadius; if (minA > maxB || maxA < minB) { return(false); } x = (Math.Max(minA, minB) + Math.Min(maxA, maxB)) / 2; } float y; { Vector2 axisY = new Vector2(0, 1); float minA = -anotherHeight / 2; float maxA = anotherHeight / 2; float pointA = Vector2.Dot(anotherPoint, axisY); float minB = pointA - oneRadius; float maxB = pointA + oneRadius; if (minA > maxB || maxA < minB) { return(false); } y = (Math.Max(minA, minB) + Math.Min(maxA, maxB)) / 2; } if (Vector2.DistanceSquared(anotherPoint, new Vector2(x, y)) > oneRadius * oneRadius) { return(false); } float left = Math.Abs(-anotherWidth / 2 - x); // left float top = Math.Abs(anotherHeight / 2 - y); // top float right = Math.Abs(anotherWidth / 2 - x); // right float bottom = Math.Abs(-anotherHeight / 2 - y); // bottom int index = 0; float dist = left; if (top < dist) { dist = top; index = 1; } if (right < dist) { dist = right; index = 2; } if (bottom < dist) { dist = bottom; index = 3; } //for (int i = 0; i < distances.Length; i++) //{ // if (distances[i] < dist) // { // dist = distances[i]; // index = i; // } //} Vector2 normal = anotherTransform.MulR(RectangleShape.RectangleNormals[index]) * dist; result = new CollisionResult(anotherTransform.Mul(new Vector2(x, y) + anotherCenter), normal); return(true); }
public override bool RayCast(Transform ownTransform, Vector2 from, Vector2 to, out float fraction, out Vector2 normal) { // TODO: I'm copying this code from FarSeer engine. That means we need either to open source this engine // or rewrite this method completely // Collision Detection in Interactive 3D Environments by Gino van den Bergen // From Section 3.1.2 // x = s + a * r // norm(x) = radius fraction = 0; normal = Vector2.Zero; Vector2 center = ownTransform.Mul(m_center); Vector2 s = from - center; Vector2 r = to - from; bool invert = false; float b = s.LengthSquared() - Radius * Radius; if (b < 0) { s = to - center; b = s.LengthSquared() - Radius * Radius; r = -r; invert = true; } // Solve quadratic equation. float c = Vector2.Dot(s, r); float rr = r.LengthSquared(); float sigma = c * c - rr * b; // Check for negative discriminant and short segment. if (sigma < 0.0f || rr < float.Epsilon) { return(false); } // Find the point of intersection of the line with the circle. float a = -(c + (float)Math.Sqrt(sigma)); //TODO: Move to mathhelper? // Is the intersection point on the segment? if (0.0f <= a && a <= rr) { a /= rr; fraction = a; if (invert) { fraction = 1.0f - fraction; } normal = s + r * a; normal = Vector2.Normalize(normal); return(true); } return(false); }