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