示例#1
0
 private static Vec2 ClosestEnd(CapsuleCache subject, CapsuleCache reference)
 {
     if (subject.circle)
     {
         return(subject.center);
     }
     return(Mathf.Abs((subject.capsule.p0 - reference.center) * reference.normal) <=
            Mathf.Abs((subject.capsule.p1 - reference.center) * reference.normal)
         ? subject.capsule.p0
         : subject.capsule.p1);
 }
示例#2
0
        private static ClosestPoints CollideCircles(CapsuleCache c0, CapsuleCache c1)
        {
            Vec2  dir  = c1.center - c0.center;
            float dist = dir.Len();

            return(new ClosestPoints(
                       point0: c0.center,
                       point1: c1.center,
                       normal: dir.SafeDivPositive(dist),
                       distance: dist - c0.capsule.radius - c1.capsule.radius
                       ));
        }
示例#3
0
        public static ClosestPoints Collide(CapsuleCache c0, CapsuleCache c1, Action <Vec2[]> debug = null)
        {
            if (Mathf.Abs(Vec2.Cross(c0.normal, c1.normal)) < Mathf.LARGE_EPS && !c0.circle && !c1.circle)
            {
                return(CollideParallel(c0, c1));
            }
            float r0 = c0.capsule.radius;
            float r1 = c1.capsule.radius;

            Vec2 c0nearend = ClosestEnd(c0, c1);
            Vec2 c1nearend = ClosestEnd(c1, c0);
            Vec2 c0closest = ClosestAxisPoint(c0, c1nearend);
            Vec2 c1closest = ClosestAxisPoint(c1, c0nearend);

            if (debug != null)
            {
                debug(new [] { c0nearend, c1nearend, c0closest, c1closest });
            }

            Vec2  dirA    = c1closest - c0nearend;
            float distAsq = dirA.Len2();
            Vec2  dirB    = c1nearend - c0closest;
            float distBsq = dirB.Len2();

            Vec2  contact_dir, contact_p0, contact_p1;
            float contact_distsq;

            if (distBsq < distAsq)
            {
                contact_dir    = dirB;
                contact_distsq = distBsq;
                contact_p0     = c0closest;
                contact_p1     = c1nearend;
            }
            else
            {
                contact_dir    = dirA;
                contact_distsq = distAsq;
                contact_p0     = c0nearend;
                contact_p1     = c1closest;
            }

            float dist = Mathf.Sqrt(contact_distsq);
            Vec2  n    = contact_dir.SafeDivPositive(dist);

            return(new ClosestPoints(
                       point0: contact_p0 + r0 * n,
                       point1: contact_p1 - r1 * n,
                       normal: n,
                       distance: dist - (r0 + r1)
                       ));
        }
示例#4
0
        private static Vec2 ClosestAxisPoint(CapsuleCache subject, Vec2 reference)
        {
            // ((center + (p1 - p0) * j) - reference) * (p1 - p0) = 0
            //    dir = p1 - p0
            // (center + dir * j - reference) * dir = 0
            // j * dir * dir = (reference - p0) * dir
            // j = ((reference - p0) * dir) / (dir * dir)

            Vec2  dir = subject.capsule.p1 - subject.capsule.p0;
            float j1  = (reference - subject.center) * dir;

            float dir2 = dir * dir;

            float jclamped = dir2 > Mathf.EPS ? Mathf.ClampSymmetric(j1 / dir2, 0.5f) : 0.5f * Mathf.EpsSign(j1);

            return(subject.center + jclamped * dir);
        }
示例#5
0
        public void Step(float full_dt)
        {
            var dt         = full_dt / substeps;
            var gravity_dp = gravity * Mathf.Sq(dt);

            var positionDampCoeff = Mathf.Pow(1f - positionDamping, dt);
            var angularDampCoeff  = Mathf.Pow(1f - angularDamping, dt);

            foreach (var body in tree.GetAll())
            {
                body.positionVelocity = body.positionVelocity.Clamp(maxPositionVelocity);
                body.angularVelocity  = Mathf.ClampSymmetric(body.angularVelocity, maxAngularVelocity);
                body.lastPosition     = body.position - body.positionVelocity * dt;
                body.lastAngle        = body.angle - body.angularVelocity * dt;
            }
            for (var substep = 0; substep < substeps; substep++)
            {
                foreach (var body in tree.GetAll())
                {
                    if (body.type == BodyType.Dynamic)
                    {
                        body.position += gravity_dp;
                        var positionDelta = body.position - body.lastPosition;
                        body.lastPosition = body.position;
                        body.position    += positionDelta * positionDampCoeff;
                        var angleDelta = body.angle - body.lastAngle;
                        body.lastAngle = body.angle;
                        body.angle    += angleDelta * angularDampCoeff;
                        var angleWrapper = DiffToMod(body.angle, Mathf.TWO_PI);
                        body.angle     += angleWrapper;
                        body.lastAngle += angleWrapper;
                    }
                    UpdateBodyCache(body);
                }

                foreach (var body in tree.GetAll())
                {
                    tree.Add(body);
                }

                contacts.Clear();
                notables.Clear();

                foreach (var body0 in tree.GetAll())
                {
                    tempList.Clear();
                    tree.TraverseOverlapping(body0, _addToTempList);

                    foreach (var body1 in tempList)
                    {
                        if (body0 == body1)
                        {
                            continue;
                        }
                        ClosestPoints p = CapsuleCache.Collide(
                            body0.fixtureCache,
                            body1.fixtureCache,
                            null
                            );

                        if (!p.degenerate)
                        {
                            StoreContact(body0, body1, p);
                            if (p.distance < 0)
                            {
                                CorrectContact(body0, body1, p);
                                UpdateBodyCache(body0);
                                UpdateBodyCache(body1);
                            }
                        }
                    }
                }
                foreach (var body in tree.GetAll())
                {
                    tree.Add(body);
                }
            }
            foreach (var body in tree.GetAll())
            {
                var invDt = 1f / dt;
                body.positionVelocity = (body.position - body.lastPosition) * invDt;
                body.angularVelocity  = (body.angle - body.lastAngle) * invDt;
            }
        }
示例#6
0
        private static ClosestPoints CollideParallel(CapsuleCache c0, CapsuleCache c1)
        {
            var  c0p0 = c0.capsule.p0;
            var  c0p1 = c0.capsule.p1;
            Vec2 c1p0, c1p1;

            if (c1.normal * c0.normal >= 0)
            {
                c1p0 = c1.capsule.p0;
                c1p1 = c1.capsule.p1;
            }
            else
            {
                c1p0 = c1.capsule.p1;
                c1p1 = c1.capsule.p0;
            }

            Vec2  dir = -c0.normal.Rot90();
            float x00 = 0;
            float x01 = c0.length;
            float x10 = (c1p0 - c0p0) * dir;
            float x11 = (c1p1 - c0p0) * dir;

            if (x01 < x10)
            {
                return(Circle.Collide(
                           new Circle {
                    center = c0p1, radius = c0.capsule.radius
                },
                           new Circle {
                    center = c1p0, radius = c1.capsule.radius
                }
                           ));
            }
            if (x11 < x00)
            {
                return(Circle.Collide(
                           new Circle {
                    center = c0p0, radius = c0.capsule.radius
                },
                           new Circle {
                    center = c1p1, radius = c1.capsule.radius
                }
                           ));
            }

            Vec2  n           = c0.normal;
            Vec2  centerDelta = c1.center - c0.center;
            float dist        = centerDelta * n;

            if (dist < 0)
            {
                n    = -n;
                dist = -dist;
            }

            if (dist < Mathf.EPS)
            {
                return(new ClosestPoints
                {
                    degenerate = true
                });
            }

            float smax   = Math.Min(x01, x11);
            float smin   = Math.Max(x00, x10);
            float spread = smax - smin;
            float smid   = 0.5f * (smax + smin);

            return(new ClosestPoints(
                       point0: c0p0 + (c0p1 - c0p0) * (smid * c0.invLength) + n * c0.capsule.radius,
                       point1: c1p0 + (c1p1 - c1p0) * ((smid - x10) * c1.invLength) - n * c1.capsule.radius,
                       normal: n,
                       distance: dist - (c0.capsule.radius + c1.capsule.radius)
                       )
            {
                spreaded = true,
                spread = spread,
            });
        }