public PartSolver1(World world, PartBase[] parts)
{
_world = world;
_parts = parts;
_hasMoved = new bool[parts.Length]; // defaults to false
_hulls = parts.Select(o => o.CreateCollisionHull(world)).ToArray();
_initialPositions = parts.Select(o => Tuple.Create(o.Position, o.Orientation)).ToArray();
}
public PartSeparator_Part(PartBase part)
{
_part = part;
_isLooseProps = false;
_convexPoints = null;
_size = new Vector3D(0, 0, 0);
_mass = 0;
}
protected PartSeparator_Part()
{
_isLooseProps = false;
_part = null;
_convexPoints = null;
_size = new Vector3D(0, 0, 0);
_mass = 0;
}
private static void DoStepSprtMove(PartBase[] parts, SortedList<int, List<Tuple<Vector3D?, Quaternion?>>> moves)
{
foreach (int partIndex in moves.Keys)
{
foreach (var move in moves[partIndex])
{
if (move.Item1 != null)
{
parts[partIndex].Position += move.Item1.Value;
}
if (move.Item2 != null)
{
parts[partIndex].Orientation = parts[partIndex].Orientation.RotateBy(move.Item2.Value);
}
}
}
}
private void DrawParts(PartBase[] parts, List<Visual3D> visuals, Vector3D offset)
{
Model3DGroup models = new Model3DGroup();
foreach (PartBase part in parts)
{
models.Children.Add(part.Model);
}
ModelVisual3D model = new ModelVisual3D();
model.Content = models;
model.Transform = new TranslateTransform3D(offset);
visuals.Add(model);
_viewport.Children.Add(model);
}
private Tuple<Visual3D, TriangleVisual[]> BreakUpPartVisuals(PartBase[] parts)
{
List<Tuple<MeshGeometry3D, Transform3D>> meshes = new List<Tuple<MeshGeometry3D, Transform3D>>();
// Grab all the meshes out of the parts (doesn't matter which mesh came from which part)
foreach (PartBase part in parts)
{
Transform3DGroup transform = new Transform3DGroup();
transform.Children.Add(new RotateTransform3D(new QuaternionRotation3D(part.Orientation)));
transform.Children.Add(new TranslateTransform3D(part.Position.ToVector()));
foreach (MeshGeometry3D mesh in GetMeshesFromModel(part.Model))
{
meshes.Add(Tuple.Create(mesh, (Transform3D)transform));
}
}
// Call the overload
return BreakUpMeshes(meshes);
}
private void StartScene(PartBase[] parts, double offset)
{
ClearScene();
_parts = parts;
_debugOffset = offset;
_tickCntr = 0;
lblNumTicks.Text = _tickCntr.ToString();
// Draw their initial positions off to the side
DrawParts(parts, _initialVisuals, new Vector3D(0, _debugOffset, 0));
// Create the solver
if (radSolver1.IsChecked.Value)
{
PartSolver1 solver1 = new PartSolver1(_world, parts);
solver1.MovePerStepPercent = .5d;
_solver = solver1;
}
else if (radSolver2.IsChecked.Value)
{
PartSolver2 solver2 = new PartSolver2(_world, parts, false);
solver2.MovePerStepPercent = 1d; //.75d;
solver2.DoRotations = true;
_solver = solver2;
}
else
{
throw new ApplicationException("Unknown solver type");
}
}
private void BuildStandalonePart(PartBase part)
{
EnsureWorldStarted();
ClearCurrent();
// WPF
ModelVisual3D model = new ModelVisual3D();
model.Content = part.Model;
_viewport.Children.Add(model);
_currentVisuals.Add(model);
// Physics
CollisionHull hull = part.CreateCollisionHull(_world);
Body body = new Body(hull, Matrix3D.Identity, part.TotalMass, new Visual3D[] { model });
body.MaterialGroupID = _material_Bot;
body.LinearDamping = .01f;
body.AngularDamping = new Vector3D(.01f, .01f, .01f);
_currentBody = body;
}
private void btnIntersectingCubes3_Click(object sender, RoutedEventArgs e)
{
try
{
EnsureWorldStarted();
ClearCurrent();
//PartDNA dnaGrav = new PartDNA() { PartType = SensorGravity.PARTTYPE, Position = new Point3D(-1.01, 0, 0), Orientation = Quaternion.Identity, Scale = new Vector3D(10, 10, 10) };
//PartDNA dnaSpin = new PartDNA() { PartType = SensorSpin.PARTTYPE, Position = new Point3D(1.01, 0, 0), Orientation = new Quaternion(new Vector3D(0, 0, 1), 30), Scale = new Vector3D(10, 10, 10) };
ShipPartDNA dnaGrav = new ShipPartDNA() { PartType = SensorGravity.PARTTYPE, Position = new Point3D(-1.01, 0, 0), Orientation = new Quaternion(new Vector3D(0, 0, -1), 15), Scale = new Vector3D(10, 10, 10) };
ShipPartDNA dnaSpin = new ShipPartDNA() { PartType = SensorSpin.PARTTYPE, Position = new Point3D(1.01, 0, 0), Orientation = new Quaternion(new Vector3D(0, 0, 1), 45), Scale = new Vector3D(10, 10, 10) };
SensorGravity grav = new SensorGravity(_editorOptions, _itemOptions, dnaGrav, null, null);
SensorSpin spin = new SensorSpin(_editorOptions, _itemOptions, dnaSpin, null);
PartBase[] parts = new PartBase[] { grav, spin };
// Draw before
DrawParts(parts, new Vector3D(-5, 0, 0));
// Separate them
CollisionHull.IntersectionPoint[] intersections;
EnsurePartsNotIntersecting.Separate(out intersections, parts, _world, .01d);
DrawLines(intersections.Select(o => Tuple.Create(o.ContactPoint, o.Normal.ToUnit() * o.PenetrationDistance)).ToArray(), new Vector3D(-5, -5, 0), Colors.Red);
// Draw after
DrawParts(parts, new Vector3D(5, 0, 0));
}
catch (Exception ex)
{
MessageBox.Show(ex.ToString(), this.Title, MessageBoxButton.OK, MessageBoxImage.Error);
}
}
private void btnIntersectingCubes_Click(object sender, RoutedEventArgs e)
{
try
{
EnsureWorldStarted();
ClearCurrent();
ShipPartDNA dnaGrav = new ShipPartDNA() { PartType = SensorGravity.PARTTYPE, Position = new Point3D(-.5, 0, 0), Orientation = Quaternion.Identity, Scale = new Vector3D(10, 10, 10) };
ShipPartDNA dnaSpin = new ShipPartDNA() { PartType = SensorSpin.PARTTYPE, Position = new Point3D(.5, 0, 0), Orientation = Quaternion.Identity, Scale = new Vector3D(10, 10, 10) };
//PartDNA dnaGrav = new PartDNA() { PartType = SensorGravity.PARTTYPE, Position = new Point3D(-1, 0, 0), Orientation = Quaternion.Identity, Scale = new Vector3D(10, 10, 10) };
//PartDNA dnaSpin = new PartDNA() { PartType = SensorSpin.PARTTYPE, Position = new Point3D(1, 0, 0), Orientation = Quaternion.Identity, Scale = new Vector3D(10, 10, 10) };
SensorGravity grav = new SensorGravity(_editorOptions, _itemOptions, dnaGrav, null, null);
SensorSpin spin = new SensorSpin(_editorOptions, _itemOptions, dnaSpin, null);
PartBase[] parts = new PartBase[] { grav, spin };
// Draw before
DrawParts(parts, new Vector3D(-5, 0, 0));
// Separate them
CollisionHull.IntersectionPoint[] intersections;
EnsurePartsNotIntersecting.Separate(out intersections, parts, _world, .01d);
DrawLines(intersections.Select(o => Tuple.Create(o.ContactPoint, o.Normal.ToUnit() * o.PenetrationDistance)).ToArray(), new Vector3D(-5, -5, 0), Colors.Red);
// Draw after
DrawParts(parts, new Vector3D(5, 0, 0));
#region Test
//CollisionHull hull1a, hull2a;
//Transform3DGroup dummy2;
//Quaternion dummy3;
//DiffuseMaterial dummy4;
//double size = 10d * .2d;
//Visual3D model = GetWPFModel(out hull1a, out dummy2, out dummy3, out dummy4, CollisionShapeType.Box, Colors.Tan, Colors.Tan, 1d, new Vector3D(size, size, size), new Point3D(-.6, 5, 0), _defaultDirectionFacing, true); // this doesn't use the offset in the collision hull
//_currentVisuals.Add(model);
//_viewport.Children.Add(model);
//model = GetWPFModel(out hull2a, out dummy2, out dummy3, out dummy4, CollisionShapeType.Box, Colors.Chartreuse, Colors.Chartreuse, 1d, new Vector3D(size, size, size), new Point3D(.6, 5, 0), _defaultDirectionFacing, true);
//_currentVisuals.Add(model);
//_viewport.Children.Add(model);
//TranslateTransform3D offset1 = new TranslateTransform3D(-.6, 0, 0);
//TranslateTransform3D offset2 = new TranslateTransform3D(.6, 0, 0);
//CollisionHull.IntersectionPoint[] points1a = hull1a.GetIntersectingPoints_HullToHull(100, hull2a, 0, offset1, offset2);
//CollisionHull.IntersectionPoint[] points2a = hull2a.GetIntersectingPoints_HullToHull(100, hull1a, 0, offset2, offset1);
//CollisionHull hull1b = CollisionHull.CreateBox(_world, 0, new Vector3D(size, size, size), offset1.Value);
//CollisionHull hull2b = CollisionHull.CreateBox(_world, 0, new Vector3D(size, size, size), offset2.Value);
//CollisionHull.IntersectionPoint[] points1b = hull1b.GetIntersectingPoints_HullToHull(100, hull2b, 0, null, null);
//CollisionHull.IntersectionPoint[] points2b = hull2b.GetIntersectingPoints_HullToHull(100, hull1b, 0, null, null);
#endregion
}
catch (Exception ex)
{
MessageBox.Show(ex.ToString(), this.Title, MessageBoxButton.OK, MessageBoxImage.Error);
}
}
//Attempt2: Include rotation
private static void DoStep2(PartBase[] parts, CollisionHull[] hulls, Tuple<int, int, CollisionHull.IntersectionPoint[]>[] intersections, List<int> changedParts, SortedList<int, double> sizes)
{
SortedList<int, List<Tuple<Vector3D?, Quaternion?>>> moves = new SortedList<int, List<Tuple<Vector3D?, Quaternion?>>>();
// Shoot through all the part pairs
foreach (var intersection in intersections)
{
double mass1 = parts[intersection.Item1].TotalMass;
double mass2 = parts[intersection.Item2].TotalMass;
double totalMass = mass1 + mass2;
double sumPenetration = intersection.Item3.Sum(o => o.PenetrationDistance);
double avgPenetration = sumPenetration / Convert.ToDouble(intersection.Item3.Length);
// Shoot through the intersecting points between these two parts
foreach (var intersectPoint in intersection.Item3)
{
// The sum of scaledDistance needs to add up to avgPenetration
double percentDistance = intersectPoint.PenetrationDistance / sumPenetration;
double scaledDistance = avgPenetration * percentDistance;
double distance1 = scaledDistance * ((totalMass - mass1) / totalMass);
double distance2 = scaledDistance * ((totalMass - mass2) / totalMass);
// Normal is pointing away from the first and toward the second
Vector3D normalUnit = intersectPoint.Normal.ToUnit();
Vector3D translation, torque;
Math3D.SplitForceIntoTranslationAndTorque(out translation, out torque, intersectPoint.ContactPoint - parts[intersection.Item1].Position, normalUnit * (-1d * distance1));
DoStepSprtAddForce(moves, intersection.Item1, translation, DoStep2SprtRotate(torque, sizes[intersection.Item1]));
Math3D.SplitForceIntoTranslationAndTorque(out translation, out torque, intersectPoint.ContactPoint - parts[intersection.Item2].Position, normalUnit * distance2);
DoStepSprtAddForce(moves, intersection.Item2, translation, DoStep2SprtRotate(torque, sizes[intersection.Item2]));
}
}
// Apply the movements
DoStepSprtMove(parts, moves);
// Remember which parts were modified (the list will be deduped later)
changedParts.AddRange(moves.Keys);
}
/// <summary>
/// This will move/rotate parts until they aren't intersecting
/// </summary>
/// <remarks>
/// To save some processing, the final collision hulls are returned (so the caller doesn't have to recreate them)
/// </remarks>
public static CollisionHull[] Separate(out CollisionHull.IntersectionPoint[] allIntersections, PartBase[] parts, World world, double ignoreDepthPercent)
{
// Get collision hulls for all the parts
CollisionHull[] retVal = parts.Select(o => o.CreateCollisionHull(world)).ToArray();
List<CollisionHull.IntersectionPoint> all = new List<CollisionHull.IntersectionPoint>();
List<int> changedParts = new List<int>();
while (true)
{
// See which parts are colliding
SortedList<int, double> sizes;
var intersections = GetIntersections(out sizes, retVal, ignoreDepthPercent);
if (intersections.Length == 0)
{
break;
}
all.AddRange(intersections.SelectMany(o => o.Item3));
DoStep1(parts, retVal, intersections, changedParts);
//DoStep2(parts, retVal, intersections, changedParts, sizes); //Attempt2 is flawed. Newton is returning collision normals that don't seem to be right. They are perpendicular to part1's face, but don't seem to make sense with part2
//Maybe attempt3 should do some kind of jostling. Do translations in the direction that attempt1 is using, but choose a couple random rotations to see if the parts can be separated without going the full translation distance (seems very inneficient though)
//TODO: Remove this when the intersections start taking in offsets
break;
}
// Recreate the final hulls for parts that changed
foreach (int index in changedParts.Distinct())
{
//TODO: Attempt a dispose on the old hull (newton reuses and shares hulls when the values are the same, so it's dangerous to dispose hulls, but not disposing will permanently consume memory)
//retVal[index].Dispose();
retVal[index] = parts[index].CreateCollisionHull(world);
}
// Exit Function
allIntersections = all.ToArray();
return retVal;
}
