//============================================================== // Calculates collision time between i and image of j using quadratic formula //============================================================== double box::CalculateCollision(int i, int j, vector<DIM> pboffset) { // calculate updated position and velocity of i and j vector<DIM> xi = s[i].x + s[i].v*(gtime - s[i].lutime); vector<DIM> vi = s[i].v; vector<DIM> xj = s[j].x + pboffset*SIZE + s[j].v*(gtime - s[j].lutime); vector<DIM> vj = s[j].v; double r_now = r + gtime*growthrate; double A,B,C; A = vector<DIM>::norm_squared(vi - vj) - 4*growthrate*growthrate; B = vector<DIM>::dot(xi - xj, vi - vj) - 4*r_now*growthrate; C = vector<DIM>::norm_squared(xi - xj) - 4*r_now*r_now; if (C < -1E-12*2.*r_now) { std::cout << "error, " << i << " and " << j << " are overlapping at time "<< gtime << " and A, B, C = " << A << " " << " " << B << " " << " " << C << std::endl; std::cout << "velocity i= " << s[i].v << ", velocity j= " << s[j].v << ", gtime= " << gtime << ", det= " << B*B - A*C << std::endl; exit(-1); } return QuadraticFormula(A, B, C); }
static bool CalcT(float frame, float k1X, float k1rhX, float k2lhX, float k2X, float& r) { auto isValid = [](float v) -> bool { if (std::isnan(v)) return false; auto small_ = -0.00001f; auto big_ = 1.000001f; return (small_ <= v) && (v <= big_); }; auto c3_ = k2X - k1X + 3.0f * (k1rhX - k2lhX); auto c2_ = 3.0f * (k1X - 2.0f * k1rhX + k2lhX); auto c1_ = 3.0f * (k1rhX - k1X); auto c0_ = k1X - frame; if (c0_ == 0.0) { r = 0.0f; return true; } if (c3_ != 0.0) { auto c2 = c2_ / c3_; auto c1 = c1_ / c3_; auto c0 = c0_ / c3_; auto p = c1 / 3.0f - c2 * c2 / (3.0f * 3.0f); auto q = (2.0f * c2 * c2 * c2 / (3.0f*3.0f*3.0f) - c2 / 3.0f * c1 + c0) / 2.0f; auto p3q2 = q * q + p * p * p; if (p3q2 > 0.0) { auto t = sqrt(p3q2); auto u = sqrt3(-q + t); auto v = sqrt3(-q - t); r = u + v - c2 / 3.0f; if (isValid(r)) return true; return false; } else if (p3q2 == 0.0) { auto u = sqrt3(-q); auto v = sqrt3(-q); r = u + v - c2 / 3.0f; if (isValid(r)) return true; u = -sqrt3(-q); v = -sqrt3(-q); r = u + v - c2 / 3.0f; if (isValid(r)) return true; return false; } else { // ビエタの解 auto phi = acos(-q / sqrt(-(p * p * p))); auto pd3 = cos(phi / 3); auto rmp = sqrt(-p); r = 2.0f * rmp * cos(phi / 3) - c2 / 3.0f; if (isValid(r)) return true; auto q2 = sqrt(3 - 3 * pd3 * pd3); //r = -rmp * (pd3 + q2) - a; r = 2.0f * rmp * cos(phi / 3 + 2.0 * M_PI / 3.0) - c2 / 3.0f; if (isValid(r)) return true; //r = -rmp * (pd3 - q2) - a; r = 2.0f * rmp * cos(phi / 3 + 4.0 * M_PI / 3.0) - c2 / 3.0f; if (isValid(r)) return true; return false; } } else { // 2次方程式のケース float r1; float r2; QuadraticFormula(c2_, c1_, c0_, r1, r2); r = r1; if (isValid(r)) return true; r = r2; if (isValid(r)) return true; r = 0.0f; return false; } }