/// <summary> /// Consolidates the given contact with its surrounding contacts. /// </summary> /// <param name="contacts"></param> /// <param name="start"></param> /// <returns></returns> private void ConsolidateContacts(List <ContactDescriptor> contacts, ContactDescriptor start) { contacts.Remove(start); List <ContactDescriptor> removedContacts = new List <ContactDescriptor>(); BulletSharp.Math.Vector3 lastPoint = start.Position; int startIndex = contactPoints.IndexOf(contacts); int lowerBound = startIndex; for (; lowerBound >= 0; lowerBound--) { List <ContactDescriptor> lc = GetValidContacts(lowerBound, lastPoint, ref start); int numContacts = lc.Count; if (numContacts == 0) { if (lowerBound == startIndex) { continue; } break; } lastPoint = BulletSharp.Math.Vector3.Zero; foreach (ContactDescriptor c in lc) { lastPoint += c.Position; removedContacts.Add(c); } lastPoint /= numContacts; } for (int i = startIndex; i >= lowerBound; i--) { List <ContactDescriptor> currentContacts = contactPoints[i]; if (currentContacts == null) { continue; } currentContacts.RemoveAll((x) => removedContacts.Contains(x)); } contacts.Add(start); }
/// <summary> /// Finds any robot collisions and adds them to the list of contact points. /// </summary> /// <param name="pm"></param> public void OnVisitPersistentManifold(PersistentManifold pm) { if (!mainState.Tracking) { return; } if (framesPassed == -1) { framesPassed = physicsWorld.frameCount - lastFrameCount; for (int i = 0; i < framesPassed; i++) { ContactPoints.Add(new List <ContactDescriptor>()); } } BRigidBody obA = pm.Body0.UserObject as BRigidBody; BRigidBody obB = pm.Body1.UserObject as BRigidBody; BRigidBody robotBody = obA != null && obA.gameObject.name.StartsWith("node") ? obA : obB != null && obB.gameObject.name.StartsWith("node") ? obB : null; if (robotBody == null) { return; } if (pm.NumContacts < 1) { return; } int numContacts = pm.NumContacts; for (int i = 0; i < Math.Min(framesPassed, numContacts); i++) { ManifoldPoint mp = pm.GetContactPoint(i); ContactDescriptor cd = new ContactDescriptor { AppliedImpulse = mp.AppliedImpulse, Position = (mp.PositionWorldOnA + mp.PositionWorldOnB) * 0.5f, RobotBody = robotBody }; if (ContactPoints[i] != null) { ContactPoints[i].Add(cd); } } }
/// <summary> /// Generates contact information from the given byte buffer. /// </summary> /// <param name="buffer"></param> /// <returns></returns> private static List <List <KeyValuePair <ContactDescriptor, int> > > ReadContactDescriptors(byte[] buffer) { IFormatter formatter = new BinaryFormatter(); List <List <KeyValuePair <ContactDescriptor, int> > > contacts = new List <List <KeyValuePair <ContactDescriptor, int> > >(); BinaryReader reader = new BinaryReader(new MemoryStream(buffer)); int numContactLists = reader.ReadInt32(); for (int i = 0; i < numContactLists; i++) { int contactCount = reader.ReadInt32(); if (contactCount > 0) { List <KeyValuePair <ContactDescriptor, int> > currentContacts = new List <KeyValuePair <ContactDescriptor, int> >(); for (int j = 0; j < contactCount; j++) { ContactDescriptor contact = new ContactDescriptor { AppliedImpulse = reader.ReadSingle(), Position = (BulletSharp.Math.Vector3)formatter.Deserialize(reader.BaseStream) }; currentContacts.Add(new KeyValuePair <ContactDescriptor, int>(contact, reader.ReadInt32())); } contacts.Add(currentContacts); } else { contacts.Add(null); } } return(contacts); }
/// <summary> /// Finds all nearby contacts within the consolidation epsilon. /// </summary> /// <param name="index"></param> /// <param name="point"></param> /// <param name="contact"></param> /// <returns></returns> private List <ContactDescriptor> GetValidContacts(int index, BulletSharp.Math.Vector3 point, ref ContactDescriptor contact) { List <ContactDescriptor> currentContacts = contactPoints[index]; List <ContactDescriptor> validContacts = new List <ContactDescriptor>(); if (currentContacts == null) { return(validContacts); } foreach (ContactDescriptor c in currentContacts) { if (c.RobotBody != contact.RobotBody || c.Equals(contact) || (c.Position - point).Length > ConsolidationEpsilon) { continue; } contact.AppliedImpulse += c.AppliedImpulse; validContacts.Add(c); } return(validContacts); }
/// <summary> /// Renders the GUI. /// </summary> public override void OnGUI() { Rect controlRect = new Rect(ControlButtonMargin, Screen.height - (SliderBottomMargin + SliderThickness + SliderThickness / 2), ButtonSize, ButtonSize); if (GUI.Button(controlRect, string.Empty, rewindStyle)) { if (rewindTime == Tracker.Lifetime) { rewindTime = 0f; } playbackMode = PlaybackMode.Rewind; } controlRect.x += ButtonSize + ControlButtonMargin; if (GUI.Button(controlRect, string.Empty, stopStyle)) { playbackMode = PlaybackMode.Paused; } controlRect.x += ButtonSize + ControlButtonMargin; if (GUI.Button(controlRect, string.Empty, playStyle)) { if (rewindTime == 0f) { rewindTime = Tracker.Lifetime; } playbackMode = PlaybackMode.Play; } controlRect.x = Screen.width - SliderRightMargin + EditButtonMargin; if (GUI.Button(controlRect, string.Empty, collisionStyle)) { editMode = editMode == EditMode.Threshold ? EditMode.None : EditMode.Threshold; } Rect collisionSliderRect = new Rect(controlRect.x + (ButtonSize - SliderThickness) / 2, controlRect.y - CollisionSliderMargin - CollisionSliderHeight, SliderThickness, CollisionSliderHeight); if (editMode == EditMode.Threshold) { contactThreshold = GUI.VerticalSlider(collisionSliderRect, contactThreshold, Mathf.Sqrt(CollisionSliderTopValue), 0f, windowStyle, thumbStyle); Rect collisionLabelRect = new Rect(controlRect.x + controlRect.width, controlRect.y - controlRect.height - CollisionSliderMargin, Screen.width - (controlRect.x + controlRect.width), controlRect.height); GUI.Label(collisionLabelRect, "Impulse Threshold:\n" + AdjustedContactThreshold.ToString("F2") + " Newtons", windowStyle); } controlRect.x += ButtonSize + EditButtonMargin; bool consolidatePressed = editMode == EditMode.Consolidate ? GUI.Button(controlRect, consolidateStyle.hover.background, GUIStyle.none) : GUI.Button(controlRect, string.Empty, consolidateStyle); if (consolidatePressed) { editMode = editMode == EditMode.Consolidate ? EditMode.None : EditMode.Consolidate; } if (editMode == EditMode.Consolidate) { GUI.Label(new Rect(Screen.width - SliderLeftMargin, controlRect.y - InfoBoxHeight - CollisionSliderMargin, Screen.width - SliderLeftMargin, InfoBoxHeight), "Select a collision to consolidate.", windowStyle); } Rect sliderRect = new Rect(SliderLeftMargin, Screen.height - (SliderBottomMargin + SliderThickness), Screen.width - (SliderRightMargin + SliderLeftMargin), SliderThickness); rewindTime = GUI.HorizontalSlider(sliderRect, rewindTime, Tracker.Lifetime, 0.0f, windowStyle, thumbStyle); Rect guiRect = new Rect(0, Screen.height - (SliderBottomMargin + SliderThickness + KeyframeHeight), Screen.width, SliderBottomMargin + SliderThickness + KeyframeHeight); if (!active && (guiRect.Contains(Event.current.mousePosition) || (editMode == EditMode.Threshold && collisionSliderRect.Contains(Event.current.mousePosition))) && Input.GetMouseButton(0)) { DynamicCamera.MovingEnabled = false; active = true; playbackMode = PlaybackMode.Paused; } else if (active && !Input.GetMouseButton(0)) { DynamicCamera.MovingEnabled = true; active = false; } bool circleHovered = false; int i = Tracker.Length - 1; foreach (List <ContactDescriptor> l in contactPoints) { if (l != null) { ContactDescriptor lastContact = default(ContactDescriptor); for (int j = 0; j < l.Count; j++) { ContactDescriptor currentContact = l[j]; if (currentContact.AppliedImpulse >= AdjustedContactThreshold) { float keyframeTime = Tracker.Lifetime - ((float)i / (Tracker.Length - 1)) * Tracker.Lifetime; if (!lastContact.Equals(currentContact)) { lastContact = currentContact; float pixelsPerValue = (sliderRect.width - KeyframeWidth - ThumbWidth / 2) / Tracker.Lifetime; Rect keyframeRect = new Rect( (Tracker.Lifetime - keyframeTime) * pixelsPerValue + sliderRect.x + (ThumbWidth - KeyframeWidth) / 2, sliderRect.y - KeyframeHeight, KeyframeWidth, sliderRect.height); if (GUI.Button(keyframeRect, keyframeTexture, GUIStyle.none)) { rewindTime = keyframeTime; playbackMode = PlaybackMode.Paused; } } Vector3 collisionPoint = camera != null?camera.WorldToScreenPoint(currentContact.Position.ToUnity()) : Vector3.zero; if (collisionPoint.z > 0.0f) { Rect circleRect = new Rect(collisionPoint.x - CircleRadius, Screen.height - (collisionPoint.y + CircleRadius), CircleRadius * 2, CircleRadius * 2); bool shouldActivate = false; if (circleRect.Contains(Event.current.mousePosition) && !circleHovered) { GUI.color = Color.white; SelectedBody = currentContact.RobotBody; shouldActivate = true; } else { GUI.color = new Color(1f, 1f, 1f, Math.Max((CircleRenderDistance - Math.Abs((Tracker.Length - 1 - i) - RewindFrame)) / CircleRenderDistance, 0.1f)); } if (GUI.Button(circleRect, circleTexture, GUIStyle.none) && Event.current.button == 0 && shouldActivate) { if (editMode == EditMode.Consolidate) { ConsolidateContacts(l, currentContact); editMode = EditMode.None; break; } else { rewindTime = keyframeTime; playbackMode = PlaybackMode.Paused; } } if (circleRect.Contains(Event.current.mousePosition) && shouldActivate) { GUI.Label(new Rect(Event.current.mousePosition.x, Event.current.mousePosition.y - InfoBoxHeight, InfoBoxWidth, InfoBoxHeight), "Impulse: " + currentContact.AppliedImpulse.ToString("F2") + " N", windowStyle); } GUI.color = Color.white; circleHovered = circleHovered || shouldActivate; } } } } i--; } if (!circleHovered) { SelectedBody = null; } }