예제 #1
0
        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;
        }
예제 #3
0
        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);
        }