Universe.Physics.OpenDynamicsEngine.ODEPhysicsScene.IsNearBody C# (CSharp) Method

        void IsNearBody(IntPtr gspace, IntPtr g1, IntPtr g2)
        {
            if (g1 == IntPtr.Zero || g2 == IntPtr.Zero || g1 == g2 || !ContinueCollisionProcessing)
                return;

            // Test if we're colliding a geom with a space.
            // If so we have to drill down into the space recursively
            if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2))
            {
                // Separating static prim geometry spaces.
                // We'll be calling near recursivly if one
                // of them is a space to find all of the
                // contact points in the space
                try
                {
                    lock(_activeprimsLock)
                        d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
                }
                catch (Exception e)
                {
                    MainConsole.Instance.WarnFormat("[ODE Physics]: SpaceCollide2 failed: {0} ", e);
                    return;
                }
                return;
            }
            IntPtr b1 = d.GeomGetBody(g1);
            IntPtr b2 = d.GeomGetBody(g2);

            // Figure out how many contact points we have
            int count;
            try
            {
                // Colliding Geom To Geom
                // This portion of the function 'was' blatantly ripped off from BoxStack.cs

                if (g1 == g2)
                    return; // Can't collide with yourself

                if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
                    return;

                count = d.CollidePtr(g1, g2, (contactsPerCollision & 0xffff), ContactgeomsArray,
                                     d.ContactGeom.unmanagedSizeOf);
            }
            catch (Exception e)
            {
                MainConsole.Instance.WarnFormat("[ODE Physics]:  ode Collide failed: {0} ", e);

                PhysicsActor badObj;
                if (actor_name_map.TryGetValue(g1, out badObj))
                    if (badObj is ODEPrim)
                        RemovePrim((ODEPrim) badObj);
                    else if (badObj is ODECharacter)
                        RemoveAvatar((ODECharacter) badObj);
                if (actor_name_map.TryGetValue(g2, out badObj))
                    if (badObj is ODEPrim)
                        RemovePrim((ODEPrim) badObj);
                    else if (badObj is ODECharacter)
                        RemoveAvatar((ODECharacter) badObj);
                return;
            }

            if (count == 0)
                return;

            PhysicsActor p1;
            PhysicsActor p2;

            if (!actor_name_map.TryGetValue(g1, out p1))
                p1 = PANull;

            if (!actor_name_map.TryGetValue(g2, out p2))
                p2 = PANull;

            if (p1.CollisionScore >= float.MaxValue - count)
                p1.CollisionScore = 0;
            p1.CollisionScore += count;

            if (p2.CollisionScore >= float.MaxValue - count)
                p2.CollisionScore = 0;
            p2.CollisionScore += count;

            ContactPoint maxDepthContact = new ContactPoint();
            d.ContactGeom curContact = new d.ContactGeom();
// 20131224 not used            d.ContactGeom maxContact = new d.ContactGeom();

            int NotSkipedCount = 0;

            //StatContactLoopTime = CollectTime(() =>

            #region Contact Loop

            for (int i = 0; i < count; i++)
            {
                if (!GetCurContactGeom(i, ref curContact))
                    break;

                if (curContact.depth > maxDepthContact.PenetrationDepth)
                {
                    maxDepthContact.PenetrationDepth = curContact.depth;
                    maxDepthContact.Position.X = curContact.pos.X;
                    maxDepthContact.Position.Y = curContact.pos.Y;
                    maxDepthContact.Position.Z = curContact.pos.Z;
                    maxDepthContact.Type = (ActorTypes)p1.PhysicsActorType;
                    maxDepthContact.SurfaceNormal.X = curContact.normal.X;
                    maxDepthContact.SurfaceNormal.Y = curContact.normal.Y;
                    maxDepthContact.SurfaceNormal.Z = curContact.normal.Z;
// 20131224 not used                    maxContact = curContact;
                }
            }
            if (p1 is ODECharacter || p2 is ODECharacter)
                //This really should be maxContact, but there are crashes that users have reported when this is used...
                //AddODECollision(maxContact, p1, p2, b1, b2, maxDepthContact, ref NotSkipedCount);
                AddODECollision(curContact, p1, p2, b1, b2, maxDepthContact, ref NotSkipedCount);
            else
            {
                for (int i = 0; i < count; i++)
                {
                    if (!GetCurContactGeom(i, ref curContact))
                        break;
                    AddODECollision(curContact, p1, p2, b1, b2, maxDepthContact, ref NotSkipedCount);
                }
            }

            #endregion

            //StatCollisionAccountingTime = CollectTime(() =>
            {
                if (NotSkipedCount > 0)
                {
                    if (NotSkipedCount > geomContactPointsStartthrottle)
                    {
                        // If there are more then 3 contact points, it's likely
                        // that we've got a pile of objects, so ...
                        // We don't want to send out hundreds of terse updates over and over again
                        // so lets throttle them and send them again after it's somewhat sorted out.
                        p2.ThrottleUpdates = true;
                    }
                }
                Collision_accounting_events(p1, p2, maxDepthContact);
            } //);
        }