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