private static bool OnRectIntersection(RayComponent ray, ColliderComponent col, TransformComponent tr,
out float2 hitPoint)
{
hitPoint = float2.zero;
float minDist = float.MaxValue;
float2x2 rotate = float2x2.Rotate(tr.Rotation);
float2x4 vertices = float2x4.zero;
for (int i = 0; i < 4; i++)
{
vertices[i] = MathHelper.Mul(rotate, col.Vertices[i]) + tr.Position;
}
for (int i = 0; i < 4; i++)
{
int j = i + 1;
if (j == 4)
{
j = 0;
}
float2 p1 = vertices[i];
float2 p2 = vertices[j];
float2 b = ray.Target - ray.Source;
float2 d = p2 - p1;
float cross = b.x * d.y - b.y * d.x;
if (MathHelper.Equal(cross, 0))
{
continue;
}
float2 c = p1 - ray.Source;
float t = (c.x * d.y - c.y * d.x) / cross;
if (t < 0 || t > 1)
{
continue;
}
float u = (c.x * b.y - c.y * b.x) / cross;
if (u < 0 || u > 1)
{
continue;
}
float2 p = ray.Source + t * b;
float dist = math.distancesq(ray.Source, p);
if (!(dist < minDist))
{
continue;
}
minDist = dist;
hitPoint = p;
}
return(minDist < float.MaxValue);
}
private static void OnCircleRectCollision(ColliderComponent ca, TransformComponent ta,
ColliderComponent cb, TransformComponent tb, out ContactInfo contactInfo)
{
contactInfo = new ContactInfo {
Hit = false
};
float2x2 rotate = float2x2.Rotate(tb.Rotation);
float2 center = MathHelper.Mul(MathHelper.Transpose(rotate), ta.Position - tb.Position);
float radius = ca.Size.x;
float separation = float.MinValue;
int faceNormal = 0;
for (int i = 0; i < 4; ++i)
{
float s = math.dot(cb.Normals[i], center - cb.Vertices[i]);
if (s > radius)
{
return;
}
if (!(s > separation))
{
continue;
}
separation = s;
faceNormal = i;
}
if (separation < MathHelper.EPSILON)
{
contactInfo.Hit = true;
contactInfo.Normal = -MathHelper.Mul(rotate, cb.Normals[faceNormal]);
contactInfo.HitPoint = contactInfo.Normal * radius + ta.Position;
contactInfo.Penetration = radius;
return;
}
contactInfo.Penetration = radius - separation;
float2 v1 = cb.Vertices[faceNormal];
float2 v2 = cb.Vertices[faceNormal + 1 < 4 ? faceNormal + 1 : 0];
if (math.dot(center - v1, v2 - v1) <= 0.0f)
{
if (math.distancesq(center, v1) > radius * radius)
{
return;
}
contactInfo.Hit = true;
contactInfo.Normal = math.normalizesafe(MathHelper.Mul(rotate, v1 - center));
contactInfo.HitPoint = MathHelper.Mul(rotate, v1) + tb.Position;
return;
}
if (math.dot(center - v2, v1 - v2) <= 0.0f)
{
if (math.distancesq(center, v2) > radius * radius)
{
return;
}
contactInfo.Hit = true;
contactInfo.Normal = math.normalizesafe(MathHelper.Mul(rotate, v2 - center));
contactInfo.HitPoint = MathHelper.Mul(rotate, v2) + tb.Position;
return;
}
float2 n = cb.Normals[faceNormal];
if (math.dot(center - v1, n) > radius)
{
return;
}
contactInfo.Normal = -MathHelper.Mul(rotate, n);
contactInfo.HitPoint = contactInfo.Normal * radius + ta.Position;
contactInfo.Hit = true;
}
private static bool OnCircleIntersection(RayComponent ray, ColliderComponent col, TransformComponent tr,
out float2 hitPoint)
{
hitPoint = float2.zero;
float2 source = ray.Source;
float2 target = ray.Target;
float2 pos = tr.Position;
float r = col.Size.x;
float t;
float dx = target.x - source.x;
float dy = target.y - source.y;
float a = dx * dx + dy * dy;
float spDx = source.x - pos.x;
float spDy = source.y - pos.y;
float b = 2 * (dx * spDx + dy * spDy);
float c = spDx * spDx + spDy * spDy - r * r;
float det = b * b - 4 * a * c;
if (a <= MathHelper.EPSILON || det < 0)
{
return(false);
}
if (MathHelper.Equal(det, 0))
{
t = -b / (2 * a);
hitPoint = new float2(source.x + t * dx, source.y + t * dy);
return(true);
}
float sqrtDet = math.sqrt(det);
t = (-b + sqrtDet) / (2 * a);
float2 p1 = new float2(source.x + t * dx, source.y + t * dy);
t = (-b - sqrtDet) / (2 * a);
float2 p2 = new float2(source.x + t * dx, source.y + t * dy);
hitPoint = math.distancesq(ray.Source, p1) < math.distancesq(ray.Source, p2) ? p1 : p2;
return(true);
}
