/// <summary> /// Adds a contact to the arbiter (threadsafe). No more than four contacts /// are stored in the contactList. When adding a new contact /// to the arbiter the existing are checked and the best are kept. /// </summary> /// <param name="point1">Point on body1. In world space.</param> /// <param name="point2">Point on body2. In world space.</param> /// <param name="normal">The normal pointing to body2.</param> /// <param name="penetration">The estimated penetration depth.</param> public Contact AddContact(JVector point1, JVector point2, JVector normal, float penetration, ContactSettings contactSettings) { JVector relPos1; JVector.Subtract(ref point1, ref body1.position, out relPos1); int index; lock (contactList) { if (this.contactList.Count == 4) { index = SortCachedPoints(ref relPos1, penetration); ReplaceContact(ref point1, ref point2, ref normal, penetration, index, contactSettings); return(null); } index = GetCacheEntry(ref relPos1, contactSettings.breakThreshold); if (index >= 0) { ReplaceContact(ref point1, ref point2, ref normal, penetration, index, contactSettings); return(null); } else { Contact contact = Contact.Pool.GetNew(); contact.Initialize(body1, body2, ref point1, ref point2, ref normal, penetration, true, contactSettings); contactList.Add(contact); return(contact); } } }
private void ReplaceContact(ref JVector point1, ref JVector point2, ref JVector n, float p, int index, ContactSettings contactSettings) { Contact contact = contactList[index]; Debug.Assert(body1 == contact.body1, "Body1 and Body2 not consistent."); contact.Initialize(body1, body2, ref point1, ref point2, ref n, p, false, contactSettings); }
/// <summary> /// Initializes a contact. /// </summary> /// <param name="body1">The first body.</param> /// <param name="body2">The second body.</param> /// <param name="point1">The collision point in worldspace</param> /// <param name="point2">The collision point in worldspace</param> /// <param name="n">The normal pointing to body2.</param> /// <param name="penetration">The estimated penetration depth.</param> public void Initialize(RigidBody body1, RigidBody body2, ref JVector point1, ref JVector point2, ref JVector n, float penetration, bool newContact, ContactSettings settings) { this.body1 = body1; this.body2 = body2; this.normal = n; normal.Normalize(); this.p1 = point1; this.p2 = point2; this.newContact = newContact; JVector.Subtract(ref p1, ref body1.position, out relativePos1); JVector.Subtract(ref p2, ref body2.position, out relativePos2); JVector.Transform(ref relativePos1, ref body1.invOrientation, out realRelPos1); JVector.Transform(ref relativePos2, ref body2.invOrientation, out realRelPos2); this.initialPen = penetration; this.penetration = penetration; body1IsMassPoint = body1.isParticle; body2IsMassPoint = body2.isParticle; // Material Properties if (newContact) { treatBody1AsStatic = body1.isStatic; treatBody2AsStatic = body2.isStatic; accumulatedNormalImpulse = 0.0f; accumulatedTangentImpulse = 0.0f; lostSpeculativeBounce = 0.0f; switch (settings.MaterialCoefficientMixing) { case ContactSettings.MaterialCoefficientMixingType.TakeMaximum: staticFriction = JMath.Max(body1.material.staticFriction, body2.material.staticFriction); dynamicFriction = JMath.Max(body1.material.kineticFriction, body2.material.kineticFriction); restitution = JMath.Max(body1.material.restitution, body2.material.restitution); break; case ContactSettings.MaterialCoefficientMixingType.TakeMinimum: staticFriction = JMath.Min(body1.material.staticFriction, body2.material.staticFriction); dynamicFriction = JMath.Min(body1.material.kineticFriction, body2.material.kineticFriction); restitution = JMath.Min(body1.material.restitution, body2.material.restitution); break; case ContactSettings.MaterialCoefficientMixingType.UseAverage: staticFriction = (body1.material.staticFriction + body2.material.staticFriction) / 2.0f; dynamicFriction = (body1.material.kineticFriction + body2.material.kineticFriction) / 2.0f; restitution = (body1.material.restitution + body2.material.restitution) / 2.0f; break; } } this.settings = settings; }