/*
* 通过 二元一次方程来解决这个问题
* axx+bx+c=0的根是(-b-sqrt(bb-4ac))/2a
*
* x(t) = o + td o是起始点，t=【0，1】，d是 p2 - p1 （参数函数）
* |x - c_p|平方 = r平方 c是中点，r是半径
* |td + (o-c_p)|平方 = r平方
* ddtt+ 2d(o-c_p)t + (o-c_p)(o-c_p) - rr = 0
* 求出的t就是需要求的第一个相交点
* s = o - c_p
* b = sd
* c = ss - rr
* 带入方程
* ddtt + 2bt + c = 0
* sigma = sqrt(delta)/2 = sqrt(bb-ddc)
* t = -c - sigma
*
*/
public static bool RaycastCircle(CircleCollider body, RayCollider line, out Vector2 normal, out float fraction)
{
normal = Vector2.zero;
fraction = 0;
Vector2 s = line.p1 - body.position;
float c = Vector2.Dot(s, s) - body.radius * body.radius;
// Solve quadratic equation.
Vector2 d = line.p2 - line.p1;
float b = Vector2.Dot(s, d);
float dd = Vector2.Dot(d, d);
float sigma = b * b - dd * c;
// Check for negative discriminant and short segment.
if (sigma < 0.0f || dd < float.MinValue)
{
return(false);
}
// Find the point of intersection of the line with the circle.
float a = -(b + Mathf.Sqrt(sigma));
// Is the intersection point on the segment?
if (0.0f <= a)
{
a /= dd;
fraction = a;
normal = s + a * d;
normal.Normalize();
return(true);
}
return(false);
}
// 切换到矩形的local坐标系
// p = p1 + a * d
// dot(normal, p - v) = 0
// dot(normal, p1 - v) + a * dot(normal, d) = 0
public static bool RaycastSquare(SquareCollider body, RayCollider line, out Vector2 normal, out float fraction)
{
normal = Vector2.zero;
fraction = 0;
Mat22 RotA = new Mat22(body.rotation);
Mat22 RotAT = RotA.Transpose();
Vector2 p1 = RotAT * (line.p1 - body.position) + body.position;
Vector2 p2 = RotAT * (line.p2 - body.position) + body.position;
Vector2 d = p2 - p1;
float lower = 0.0f, upper = 1;
int index = -1;
for (int i = 0; i < 4; ++i)
{
Vector2 v = body.position + SquareCollider.vecties[i] * body.width;
// p = p1 + a * d
// dot(normal, p - v) = 0
// dot(normal, p1 - v) + a * dot(normal, d) = 0
float numerator = Vector2.Dot(SquareCollider.normals[i], v - p1);
float denominator = Vector2.Dot(SquareCollider.normals[i], d);
if (denominator == 0.0f)
{
if (numerator < 0.0f)
{
return(false);
}
}
else
{
// Note: we want this predicate without division:
// lower < numerator / denominator, where denominator < 0
// Since denominator < 0, we have to flip the inequality:
// lower < numerator / denominator <==> denominator * lower > numerator.
if (denominator < 0.0f && numerator < lower * denominator)
{
// Increase lower.
// The segment enters this half-space.
lower = numerator / denominator;
index = i;
}
else if (denominator > 0.0f && numerator < upper * denominator)
{
// Decrease upper.
// The segment exits this half-space.
upper = numerator / denominator;
}
}
// The use of epsilon here causes the assert on lower to trip
// in some cases. Apparently the use of epsilon was to make edge
// shapes work, but now those are handled separately.
//if (upper < lower - b2_epsilon)
if (upper < lower)
{
return(false);
}
}
Assert.IsTrue(0.0f <= lower && lower <= 1);
if (index >= 0)
{
fraction = lower;
normal = RotA * SquareCollider.normals[index];
return(true);
}
return(false);
}
