This is non-static for faster pooling. To get an instance, use the {@link SingletonPool}, don't construct a distance object.
Example #1
0
        // TODO_ERIN might not need to return the separation
        public virtual float initialize(Distance.SimplexCache cache, Distance.DistanceProxy proxyA, Sweep sweepA, Distance.DistanceProxy proxyB, Sweep sweepB, float t1)
        {
            m_proxyA = proxyA;
            m_proxyB = proxyB;
            int count = cache.count;
            Debug.Assert(0 < count && count < 3);

            m_sweepA = sweepA;
            m_sweepB = sweepB;

            m_sweepA.getTransform(xfa, t1);
            m_sweepB.getTransform(xfb, t1);

            // log.debug("initializing separation.\n" +
            // "cache: "+cache.count+"-"+cache.metric+"-"+cache.indexA+"-"+cache.indexB+"\n"
            // "distance: "+proxyA.

            if (count == 1)
            {
                m_type = Type.POINTS;
                /*
                * Vec2 localPointA = m_proxyA.GetVertex(cache.indexA[0]); Vec2 localPointB =
                * m_proxyB.GetVertex(cache.indexB[0]); Vec2 pointA = Mul(transformA, localPointA); Vec2
                * pointB = Mul(transformB, localPointB); m_axis = pointB - pointA; m_axis.Normalize();
                */
                localPointA.set_Renamed(m_proxyA.getVertex(cache.indexA[0]));
                localPointB.set_Renamed(m_proxyB.getVertex(cache.indexB[0]));
                Transform.mulToOutUnsafe(xfa, localPointA, pointA);
                Transform.mulToOutUnsafe(xfb, localPointB, pointB);
                m_axis.set_Renamed(pointB).subLocal(pointA);
                float s = m_axis.normalize();
                return s;
            }
            else if (cache.indexA[0] == cache.indexA[1])
            {
                // Two points on B and one on A.
                m_type = Type.FACE_B;

                localPointB1.set_Renamed(m_proxyB.getVertex(cache.indexB[0]));
                localPointB2.set_Renamed(m_proxyB.getVertex(cache.indexB[1]));

                temp.set_Renamed(localPointB2).subLocal(localPointB1);
                Vec2.crossToOutUnsafe(temp, 1f, m_axis);
                m_axis.normalize();

                Rot.mulToOutUnsafe(xfb.q, m_axis, normal);

                m_localPoint.set_Renamed(localPointB1).addLocal(localPointB2).mulLocal(.5f);
                Transform.mulToOutUnsafe(xfb, m_localPoint, pointB);

                localPointA.set_Renamed(proxyA.getVertex(cache.indexA[0]));
                Transform.mulToOutUnsafe(xfa, localPointA, pointA);

                temp.set_Renamed(pointA).subLocal(pointB);
                float s = Vec2.dot(temp, normal);
                if (s < 0.0f)
                {
                    m_axis.negateLocal();
                    s = -s;
                }
                return s;
            }
            else
            {
                // Two points on A and one or two points on B.
                m_type = Type.FACE_A;

                localPointA1.set_Renamed(m_proxyA.getVertex(cache.indexA[0]));
                localPointA2.set_Renamed(m_proxyA.getVertex(cache.indexA[1]));

                temp.set_Renamed(localPointA2).subLocal(localPointA1);
                Vec2.crossToOutUnsafe(temp, 1.0f, m_axis);
                m_axis.normalize();

                Rot.mulToOutUnsafe(xfa.q, m_axis, normal);

                m_localPoint.set_Renamed(localPointA1).addLocal(localPointA2).mulLocal(.5f);
                Transform.mulToOutUnsafe(xfa, m_localPoint, pointA);

                localPointB.set_Renamed(m_proxyB.getVertex(cache.indexB[0]));
                Transform.mulToOutUnsafe(xfb, localPointB, pointB);

                temp.set_Renamed(pointB).subLocal(pointA);
                float s = Vec2.dot(temp, normal);
                if (s < 0.0f)
                {
                    m_axis.negateLocal();
                    s = -s;
                }
                return s;
            }
        }
Example #2
0
        // TODO_ERIN might not need to return the separation
        public float Initialize(Distance.SimplexCache cache, Distance.DistanceProxy proxyA, Sweep sweepA, Distance.DistanceProxy proxyB, Sweep sweepB, float t1)
        {
            ProxyA = proxyA;
            ProxyB = proxyB;
            int count = cache.Count;
            Debug.Assert(0 < count && count < 3);

            SweepA = sweepA;
            SweepB = sweepB;

            SweepA.GetTransform(xfa, t1);
            SweepB.GetTransform(xfb, t1);

            // log.debug("initializing separation.\n" +
            // "cache: "+cache.count+"-"+cache.metric+"-"+cache.indexA+"-"+cache.indexB+"\n"
            // "distance: "+proxyA.

            if (count == 1)
            {
                Type = Type.Points;
                /*
                * Vec2 localPointA = m_proxyA.GetVertex(cache.indexA[0]); Vec2 localPointB =
                * m_proxyB.GetVertex(cache.indexB[0]); Vec2 pointA = Mul(transformA, localPointA); Vec2
                * pointB = Mul(transformB, localPointB); m_axis = pointB - pointA; m_axis.Normalize();
                */
                localPointA.Set(ProxyA.GetVertex(cache.IndexA[0]));
                localPointB.Set(ProxyB.GetVertex(cache.IndexB[0]));
                Transform.MulToOutUnsafe(xfa, localPointA, pointA);
                Transform.MulToOutUnsafe(xfb, localPointB, pointB);
                Axis.Set(pointB).SubLocal(pointA);
                float s = Axis.Normalize();
                return s;
            }
            else if (cache.IndexA[0] == cache.IndexA[1])
            {
                // Two points on B and one on A.
                Type = Type.FaceB;

                localPointB1.Set(ProxyB.GetVertex(cache.IndexB[0]));
                localPointB2.Set(ProxyB.GetVertex(cache.IndexB[1]));

                temp.Set(localPointB2).SubLocal(localPointB1);
                Vec2.CrossToOutUnsafe(temp, 1f, Axis);
                Axis.Normalize();

                Rot.MulToOutUnsafe(xfb.Q, Axis, normal);

                LocalPoint.Set(localPointB1).AddLocal(localPointB2).MulLocal(.5f);
                Transform.MulToOutUnsafe(xfb, LocalPoint, pointB);

                localPointA.Set(proxyA.GetVertex(cache.IndexA[0]));
                Transform.MulToOutUnsafe(xfa, localPointA, pointA);

                temp.Set(pointA).SubLocal(pointB);
                float s = Vec2.Dot(temp, normal);
                if (s < 0.0f)
                {
                    Axis.NegateLocal();
                    s = -s;
                }
                return s;
            }
            else
            {
                // Two points on A and one or two points on B.
                Type = Type.FaceA;

                localPointA1.Set(ProxyA.GetVertex(cache.IndexA[0]));
                localPointA2.Set(ProxyA.GetVertex(cache.IndexA[1]));

                temp.Set(localPointA2).SubLocal(localPointA1);
                Vec2.CrossToOutUnsafe(temp, 1.0f, Axis);
                Axis.Normalize();

                Rot.MulToOutUnsafe(xfa.Q, Axis, normal);

                LocalPoint.Set(localPointA1).AddLocal(localPointA2).MulLocal(.5f);
                Transform.MulToOutUnsafe(xfa, LocalPoint, pointA);

                localPointB.Set(ProxyB.GetVertex(cache.IndexB[0]));
                Transform.MulToOutUnsafe(xfb, localPointB, pointB);

                temp.Set(pointB).SubLocal(pointA);
                float s = Vec2.Dot(temp, normal);
                if (s < 0.0f)
                {
                    Axis.NegateLocal();
                    s = -s;
                }
                return s;
            }
        }
Example #3
0
        public DefaultWorldPool(int argSize, int argContainerSize)
        {
            args = new object[] { this };

            pcstack = new MutableStack<Contact, PolygonContact>(Settings.CONTACT_STACK_INIT_SIZE, classes, args);
            ccstack = new MutableStack<Contact, CircleContact>(Settings.CONTACT_STACK_INIT_SIZE, classes, args);
            cpstack = new MutableStack<Contact, PolygonAndCircleContact>(Settings.CONTACT_STACK_INIT_SIZE, classes, args);
            ecstack = new MutableStack<Contact, EdgeAndCircleContact>(Settings.CONTACT_STACK_INIT_SIZE, classes, args);
            epstack = new MutableStack<Contact, EdgeAndPolygonContact>(Settings.CONTACT_STACK_INIT_SIZE, classes, args);
            chcstack = new MutableStack<Contact, ChainAndCircleContact>(Settings.CONTACT_STACK_INIT_SIZE, classes, args);
            chpstack = new MutableStack<Contact, ChainAndPolygonContact>(Settings.CONTACT_STACK_INIT_SIZE, classes, args);

            vecs = new OrderedStack<Vec2>(argSize, argContainerSize);
            vec3s = new OrderedStack<Vec3>(argSize, argContainerSize);
            mats = new OrderedStack<Mat22>(argSize, argContainerSize);
            aabbs = new OrderedStack<AABB>(argSize, argContainerSize);
            rots = new OrderedStack<Rot>(argSize, argContainerSize);
            mat33s = new OrderedStack<Mat33>(argSize, argContainerSize);

            dist = new Distance();
            collision = new Collision.Collision(this);
            toi = new TimeOfImpact(this);
        }