Exemplo n.º 1
0
        protected override void Test(NarrowPhase test, Part a, Part b)
        {
            int thread = TaskManager.CurrentThreadIndex;

            _a[thread] = a;
            _b[thread] = b;

            RigidBody ba = ((BodySkin)a.Owner).Owner, bb = ((BodySkin)b.Owner).Owner;

            if (ba.IsFast || bb.IsFast)
            {
                var delta = Vector3.Zero;
                if (ba.IsFast)
                {
                    Vector3.Add(ref delta, ref ba.Velocity.Linear, out delta);
                }
                if (bb.IsFast)
                {
                    Vector3.Subtract(ref delta, ref bb.Velocity.Linear, out delta);
                }
                Vector3.Multiply(ref delta, bb.Manager.TimeStep, out delta);
                test.SweptTest(this, a, b, delta);
            }
            else
            {
                test.OverlapTest(this, a, b);
            }

            var current = _contact[thread];

            if (current != null)
            {
                var points = current.Points;
                var normal = current.Normal;

                // normalize contact normal if required
                if (Math.Abs(normal.LengthSquared() - 1f) >= Constants.Epsilon)
                {
                    normal.Normalize();
                    current.Normal = normal;
                }

                for (int i = 0; i < current.Count; i++)
                {
                    // transform points to old position
                    current.BodyA.Skin.UndoTransform(ref points[i].OffsetA);
                    current.BodyB.Skin.UndoTransform(ref points[i].OffsetB);

                    // calculate penetration depth of each point
                    float ax, bx;
                    Vector3.Dot(ref normal, ref points[i].OffsetA, out ax);
                    Vector3.Dot(ref normal, ref points[i].OffsetB, out bx);
                    points[i].Depth = bx - ax;

                    // convert world points to world offsets
                    Vector3.Subtract(ref points[i].OffsetA, ref current.BodyA.World.Position, out points[i].OffsetA);
                    Vector3.Subtract(ref points[i].OffsetB, ref current.BodyB.World.Position, out points[i].OffsetB);
                }

                _items[thread].Add(current);
                _contact[thread] = null;
            }
        }