/// <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;
}
}
