public Intersection(int index1, int index2, double avgSize1, double avgSize2, CollisionHull.IntersectionPoint[] intersections)
{
this.Index1 = index1;
this.Index2 = index2;
this.AvgSize1 = avgSize1;
this.AvgSize2 = avgSize2;
this.Intersections = intersections;
}
//NOTE: Taking in token was added so that if a body needs to change size, a new one can be built using
//the same token. In general, always pass null to guarantee unique tokens for each body
public Body(CollisionHull hull, Matrix3D offsetMatrix, double mass, Visual3D[] visuals = null, long?token = null)
{
_world = hull.World;
_worldHandle = _world.Handle;
this.Visuals = visuals;
//NOTE: The collision hull can take a null offset matrix, but the body can't
_handle = Newton.NewtonCreateBody(_worldHandle, hull.Handle, new NewtonMatrix(offsetMatrix).Matrix);
IntPtr hullHandlePost = Newton.NewtonBodyGetCollision(_handle);
if (hullHandlePost != hull.Handle)
{
// For some reason, this is coming back with a new handle for compound collision hulls. So store this second one as well
ObjectStorage.Instance.AddCollisionHull(hullHandlePost, hull.Clone(hullHandlePost));
// Testing shows the old one to still be out there. I should probably clone the hull with this new handle using a special overload of
// the constructor. But as long as the user doesn't manipulate hulls after they are bound to a body, everything should be fine
//int test = Newton.NewtonCollisionIsTriggerVolume(hull.Handle);
}
// Listen to the move event (the invoke method is a newton callback that converts the newton stuff into c# friendly stuff)
_newtonBodyMoved = new Newton.NewtonSetTransform(InvokeBodyMoved);
Newton.NewtonBodySetTransformCallback(_handle, _newtonBodyMoved);
this.OffsetMatrix = offsetMatrix; // calling this explicitly, because it call OnBodyMoved (making sure the visual is synced)
// Store the default mass matrix
this.Mass = mass; // letting the property set build the mass matrix
if (token == null)
{
this.Token = TokenGenerator.NextToken();
}
else
{
this.Token = token.Value;
}
ObjectStorage.Instance.AddBody(_handle, this);
_world.BodyCreated(this);
}
public static CollisionHull CreateCompoundCollision(WorldBase world, int shapeID, CollisionHull[] hulls)
{
IntPtr[] handles = new IntPtr[hulls.Length];
for (int cntr = 0; cntr < hulls.Length; cntr++)
{
handles[cntr] = hulls[cntr].Handle;
}
IntPtr handle = Newton.NewtonCreateCompoundCollision(world.Handle, hulls.Length, handles, shapeID);
return new CollisionHull(handle, world, null, CollisionShapeType.Compound);
}
/// <summary>
/// This finds intersections between all the hulls
/// </summary>
private static Intersection[] GetIntersections(PartSeparator_Part[] parts, CollisionHull[] hulls, bool[] hasMoved, World world)
{
List<Intersection> retVal = new List<Intersection>();
// Compare each hull to the others
for (int outer = 0; outer < hulls.Length - 1; outer++)
{
double? sizeOuter = null;
for (int inner = outer + 1; inner < hulls.Length; inner++)
{
// Rebuild hulls if nessessary
if (hasMoved[outer])
{
hulls[outer].Dispose();
hulls[outer] = parts[outer].CreateCollisionHull(world);
hasMoved[outer] = false;
}
if (hasMoved[inner])
{
hulls[inner].Dispose();
hulls[inner] = parts[inner].CreateCollisionHull(world);
hasMoved[inner] = false;
}
// Get intersecting points
CollisionHull.IntersectionPoint[] points = hulls[outer].GetIntersectingPoints_HullToHull(100, hulls[inner], 0);
if (points != null && points.Length > 0)
{
sizeOuter = sizeOuter ?? (parts[outer].Size.X + parts[outer].Size.X + parts[outer].Size.X) / 3d;
double sizeInner = (parts[inner].Size.X + parts[inner].Size.X + parts[inner].Size.X) / 3d;
double sumSize = sizeOuter.Value + sizeInner;
double minSize = sumSize * IGNOREDEPTHPERCENT;
// Filter out the shallow penetrations
//TODO: May need to add the lost distance to the remaining intersections
points = points.Where(o => o.PenetrationDistance > minSize).ToArray();
if (points != null && points.Length > 0)
{
retVal.Add(new Intersection(outer, inner, sizeOuter.Value, sizeInner, points));
}
}
}
}
// Exit Function
return retVal.ToArray();
}
public bool GetNearestPoint_HullToHull(out Point3D?contactPointThis, out Point3D?contactPointOther, out Vector3D?normal, CollisionHull otherHull, int threadIndex, Transform3D thisTransform, Transform3D otherTransform)
{
float[] finalOffsetThis = thisTransform == null ? new NewtonMatrix(Matrix3D.Identity).Matrix : new NewtonMatrix(thisTransform.Value).Matrix; // not including the member's offset, because newton is already accounting for it.
float[] finalOffsetOther = thisTransform == null ? new NewtonMatrix(Matrix3D.Identity).Matrix : new NewtonMatrix(otherTransform.Value).Matrix;
NewtonVector3 contactANewt = new NewtonVector3();
NewtonVector3 contactBNewt = new NewtonVector3();
NewtonVector3 normalNewt = new NewtonVector3();
int retVal = Newton.NewtonCollisionClosestPoint(_world.Handle, _handle, finalOffsetThis, otherHull.Handle, finalOffsetOther, contactANewt.Vector, contactBNewt.Vector, normalNewt.Vector, threadIndex);
// Exit Function
if (retVal == 1)
{
contactPointThis = contactANewt.ToPointWPF();
contactPointOther = contactBNewt.ToPointWPF();
normal = normalNewt.ToVectorWPF();
return(true);
}
else
{
contactPointThis = null;
contactPointOther = null;
normal = null;
return(false);
}
}
public CollisionHullDestroyedArgs(WorldBase world, CollisionHull collisionHull)
{
this.World = world;
this.CollisionHull = collisionHull;
}
/// <summary>
/// This returns the intersection points and time of impact between two hulls within a window of time
/// NOTE: The hulls need to start out not touching, and collide within the timestep passed in
/// </summary>
/// <param name="maxReturnCount">The maximum number of points to return</param>
/// <param name="timestep">Maximum time interval consided for the collision calculation</param>
public IntersectionPoint[] GetIntersectingPoints_HullToHull(int maxReturnCount, double timestep, Vector3D velocity, Vector3D angularVelocity, CollisionHull otherHull, Vector3D otherVelocity, Vector3D otherAngularVelocity, int threadIndex)
{
return GetIntersectingPoints_HullToHull(maxReturnCount, timestep, velocity, angularVelocity, otherHull, otherVelocity, otherAngularVelocity, threadIndex, null, null);
}
/// <summary>
/// This returns intersection points between the two hulls (won't return anything if the hulls aren't touching)
/// NOTE: This overload is a snapshot in time, the other overload does collision calculations within a window of time
/// </summary>
/// <param name="maxReturnCount">The maximum number of points to return</param>
public IntersectionPoint[] GetIntersectingPoints_HullToHull(int maxReturnCount, CollisionHull otherHull, int threadIndex)
{
return GetIntersectingPoints_HullToHull(maxReturnCount, otherHull, threadIndex, null, null);
}
/// <summary>
/// This finds intersections between all the hulls
/// </summary>
private static Tuple<int, int, CollisionHull.IntersectionPoint[]>[] GetIntersections(out SortedList<int, double> sizes, CollisionHull[] hulls, double ignoreDepthPercent)
{
List<Tuple<int, int, CollisionHull.IntersectionPoint[]>> retVal = new List<Tuple<int, int, CollisionHull.IntersectionPoint[]>>();
sizes = new SortedList<int, double>();
// Compare each hull to the others
for (int outer = 0; outer < hulls.Length - 1; outer++)
{
for (int inner = outer + 1; inner < hulls.Length; inner++)
{
//TODO: Use the overload that takes offsets
CollisionHull.IntersectionPoint[] points = hulls[outer].GetIntersectingPoints_HullToHull(100, hulls[inner], 0);
//CollisionHull.IntersectionPoint[] points = hulls[inner].GetIntersectingPoints_HullToHull(100, hulls[outer], 0); // the normals seem to be the same when colliding from the other direction
if (points != null && points.Length > 0)
{
double sumSize = GetIntersectionsSprtSize(sizes, hulls, outer) + GetIntersectionsSprtSize(sizes, hulls, inner);
double minSize = sumSize * ignoreDepthPercent;
// Filter out the shallow penetrations
//TODO: May need to add the lost distance to the remaining intersections
points = points.Where(o => o.PenetrationDistance > minSize).ToArray();
if (points != null && points.Length > 0)
{
retVal.Add(Tuple.Create(outer, inner, points));
}
}
}
}
// Exit Function
return retVal.ToArray();
}
/// <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;
}
public IntersectionPoint[] GetIntersectingPoints_HullToHull(int maxReturnCount, double timestep, Vector3D velocity, Vector3D angularVelocity, CollisionHull otherHull, Vector3D otherVelocity, Vector3D otherAngularVelocity, int threadIndex, Transform3D thisTransform, Transform3D otherTransform)
{
float[] finalOffsetThis = thisTransform == null ? new NewtonMatrix(Matrix3D.Identity).Matrix : new NewtonMatrix(thisTransform.Value).Matrix; // not including the member's offset, because newton is already accounting for it.
float[] finalOffsetOther = thisTransform == null ? new NewtonMatrix(Matrix3D.Identity).Matrix : new NewtonMatrix(otherTransform.Value).Matrix;
// Prep some arrays to hold the return values
float[] contacts = new float[3 * maxReturnCount];
float[] normals = new float[3 * maxReturnCount];
float[] penetrationDistances = new float[maxReturnCount];
float[] timesOfImpact = new float[maxReturnCount];
// Call Newton
int pointCount = Newton.NewtonCollisionCollideContinue(_world.Handle, maxReturnCount, Convert.ToSingle(timestep),
_handle, finalOffsetThis, new NewtonVector3(velocity).Vector, new NewtonVector3(angularVelocity).Vector,
otherHull.Handle, finalOffsetOther, new NewtonVector3(otherVelocity).Vector, new NewtonVector3(otherAngularVelocity).Vector,
timesOfImpact, contacts, normals, penetrationDistances,
threadIndex);
// Convert to c#
List <IntersectionPoint> retVal = new List <IntersectionPoint>();
for (int cntr = 0; cntr < pointCount; cntr++)
{
int offset = cntr * 3;
IntersectionPoint point;
point.ContactPoint = new NewtonVector3(contacts[offset], contacts[offset + 1], contacts[offset + 2]).ToPointWPF();
point.Normal = new NewtonVector3(normals[offset], normals[offset + 1], normals[offset + 2]).ToVectorWPF();
point.PenetrationDistance = penetrationDistances[cntr];
point.TimeOfImpact = timesOfImpact[cntr];
retVal.Add(point);
}
// Exit Function
return(retVal.ToArray());
}
/// <summary>
/// This returns the intersection points and time of impact between two hulls within a window of time
/// NOTE: The hulls need to start out not touching, and collide within the timestep passed in
/// </summary>
/// <param name="maxReturnCount">The maximum number of points to return</param>
/// <param name="timestep">Maximum time interval consided for the collision calculation</param>
public IntersectionPoint[] GetIntersectingPoints_HullToHull(int maxReturnCount, double timestep, Vector3D velocity, Vector3D angularVelocity, CollisionHull otherHull, Vector3D otherVelocity, Vector3D otherAngularVelocity, int threadIndex)
{
return(GetIntersectingPoints_HullToHull(maxReturnCount, timestep, velocity, angularVelocity, otherHull, otherVelocity, otherAngularVelocity, threadIndex, null, null));
}
public IntersectionPoint[] GetIntersectingPoints_HullToHull(int maxReturnCount, CollisionHull otherHull, int threadIndex, Transform3D thisTransform, Transform3D otherTransform)
{
float[] finalOffsetThis = thisTransform == null ? new NewtonMatrix(Matrix3D.Identity).Matrix : new NewtonMatrix(thisTransform.Value).Matrix; // not including the member's offset, because newton is already accounting for it.
float[] finalOffsetOther = otherTransform == null ? new NewtonMatrix(Matrix3D.Identity).Matrix : new NewtonMatrix(otherTransform.Value).Matrix;
if (_handle == otherHull._handle && thisTransform == null && otherTransform == null)
{
// Newton throws an exception if trying to collide the same hull
return(new IntersectionPoint[0]);
}
// Prep some arrays to hold the return values
float[] contacts = new float[3 * maxReturnCount];
float[] normals = new float[3 * maxReturnCount];
float[] penetrationDistances = new float[maxReturnCount];
// Call Newton
int pointCount = Newton.NewtonCollisionCollide(_world.Handle, maxReturnCount, _handle, finalOffsetThis, otherHull.Handle, finalOffsetOther, contacts, normals, penetrationDistances, threadIndex);
// Convert to c#
List <IntersectionPoint> retVal = new List <IntersectionPoint>();
for (int cntr = 0; cntr < pointCount; cntr++)
{
int offset = cntr * 3;
IntersectionPoint point;
point.ContactPoint = new NewtonVector3(contacts[offset], contacts[offset + 1], contacts[offset + 2]).ToPointWPF();
point.Normal = new NewtonVector3(normals[offset], normals[offset + 1], normals[offset + 2]).ToVectorWPF();
point.PenetrationDistance = penetrationDistances[cntr];
point.TimeOfImpact = 0; // time of impact has no meaning for this method (I'm just reusing the same structure between two methods)
retVal.Add(point);
}
// Exit Function
return(retVal.ToArray());
}
/// <summary>
/// This returns intersection points between the two hulls (won't return anything if the hulls aren't touching)
/// NOTE: This overload is a snapshot in time, the other overload does collision calculations within a window of time
/// </summary>
/// <param name="maxReturnCount">The maximum number of points to return</param>
public IntersectionPoint[] GetIntersectingPoints_HullToHull(int maxReturnCount, CollisionHull otherHull, int threadIndex)
{
return(GetIntersectingPoints_HullToHull(maxReturnCount, otherHull, threadIndex, null, null));
}
/// <summary>
/// Returns the closest points along the surface of the two hulls (null if the hulls intersect each other)
/// </summary>
public bool GetNearestPoint_HullToHull(out Point3D? contactPointThis, out Point3D? contactPointOther, out Vector3D? normal, CollisionHull otherHull, int threadIndex)
{
return GetNearestPoint_HullToHull(out contactPointThis, out contactPointOther, out normal, otherHull, threadIndex, null, null);
}
//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);
}
public bool GetNearestPoint_HullToHull(out Point3D? contactPointThis, out Point3D? contactPointOther, out Vector3D? normal, CollisionHull otherHull, int threadIndex, Transform3D thisTransform, Transform3D otherTransform)
{
float[] finalOffsetThis = thisTransform == null ? new NewtonMatrix(Matrix3D.Identity).Matrix : new NewtonMatrix(thisTransform.Value).Matrix; // not including the member's offset, because newton is already accounting for it.
float[] finalOffsetOther = thisTransform == null ? new NewtonMatrix(Matrix3D.Identity).Matrix : new NewtonMatrix(otherTransform.Value).Matrix;
NewtonVector3 contactANewt = new NewtonVector3();
NewtonVector3 contactBNewt = new NewtonVector3();
NewtonVector3 normalNewt = new NewtonVector3();
int retVal = Newton.NewtonCollisionClosestPoint(_world.Handle, _handle, finalOffsetThis, otherHull.Handle, finalOffsetOther, contactANewt.Vector, contactBNewt.Vector, normalNewt.Vector, threadIndex);
// Exit Function
if (retVal == 1)
{
contactPointThis = contactANewt.ToPointWPF();
contactPointOther = contactBNewt.ToPointWPF();
normal = normalNewt.ToVectorWPF();
return true;
}
else
{
contactPointThis = null;
contactPointOther = null;
normal = null;
return false;
}
}
private ModelVisual3D GetWPFModel(out CollisionHull hull, out Transform3DGroup transform, out Quaternion rotation, out DiffuseMaterial bodyMaterial, CollisionShapeType shape, Color color, Color reflectionColor, double reflectionIntensity, Vector3D size, Point3D position, DoubleVector directionFacing, bool createHull)
{
// Material
MaterialGroup materials = new MaterialGroup();
bodyMaterial = new DiffuseMaterial(new SolidColorBrush(color));
materials.Children.Add(bodyMaterial);
materials.Children.Add(new SpecularMaterial(new SolidColorBrush(reflectionColor), reflectionIntensity));
// Geometry Model
GeometryModel3D geometry = new GeometryModel3D();
geometry.Material = materials;
geometry.BackMaterial = materials;
hull = null;
switch (shape)
{
case CollisionShapeType.Box:
Vector3D halfSize = size / 2d;
geometry.Geometry = UtilityWPF.GetCube_IndependentFaces(new Point3D(-halfSize.X, -halfSize.Y, -halfSize.Z), new Point3D(halfSize.X, halfSize.Y, halfSize.Z));
if (createHull)
{
hull = CollisionHull.CreateBox(_world, 0, size, null);
}
break;
case CollisionShapeType.Sphere:
geometry.Geometry = UtilityWPF.GetSphere_LatLon(5, size.X, size.Y, size.Z);
if (createHull)
{
hull = CollisionHull.CreateSphere(_world, 0, size, null);
}
break;
case CollisionShapeType.Cylinder:
geometry.Geometry = UtilityWPF.GetCylinder_AlongX(20, size.X, size.Y);
if (createHull)
{
hull = CollisionHull.CreateCylinder(_world, 0, size.X, size.Y, null);
}
break;
case CollisionShapeType.Cone:
geometry.Geometry = UtilityWPF.GetCone_AlongX(20, size.X, size.Y);
if (createHull)
{
hull = CollisionHull.CreateCone(_world, 0, size.X, size.Y, null);
}
break;
default:
throw new ApplicationException("Unexpected CollisionShapeType: " + shape.ToString());
}
// Transform
transform = new Transform3DGroup(); // rotate needs to be added before translate
//rotation = _defaultDirectionFacing.GetAngleAroundAxis(directionFacing); // can't use double vector, it over rotates (not anymore, but this is still isn't rotating correctly)
rotation = Math3D.GetRotation(_defaultDirectionFacing.Standard, directionFacing.Standard);
transform.Children.Add(new RotateTransform3D(new QuaternionRotation3D(rotation)));
transform.Children.Add(new TranslateTransform3D(position.ToVector()));
// Model Visual
ModelVisual3D retVal = new ModelVisual3D();
retVal.Content = geometry;
retVal.Transform = transform;
// Exit Function
return retVal;
}
public IntersectionPoint[] GetIntersectingPoints_HullToHull(int maxReturnCount, CollisionHull otherHull, int threadIndex, Transform3D thisTransform, Transform3D otherTransform)
{
float[] finalOffsetThis = thisTransform == null ? new NewtonMatrix(Matrix3D.Identity).Matrix : new NewtonMatrix(thisTransform.Value).Matrix; // not including the member's offset, because newton is already accounting for it.
float[] finalOffsetOther = otherTransform == null ? new NewtonMatrix(Matrix3D.Identity).Matrix : new NewtonMatrix(otherTransform.Value).Matrix;
if (_handle == otherHull._handle && thisTransform == null && otherTransform == null)
{
// Newton throws an exception if trying to collide the same hull
return new IntersectionPoint[0];
}
// Prep some arrays to hold the return values
float[] contacts = new float[3 * maxReturnCount];
float[] normals = new float[3 * maxReturnCount];
float[] penetrationDistances = new float[maxReturnCount];
// Call Newton
int pointCount = Newton.NewtonCollisionCollide(_world.Handle, maxReturnCount, _handle, finalOffsetThis, otherHull.Handle, finalOffsetOther, contacts, normals, penetrationDistances, threadIndex);
// Convert to c#
List<IntersectionPoint> retVal = new List<IntersectionPoint>();
for (int cntr = 0; cntr < pointCount; cntr++)
{
int offset = cntr * 3;
IntersectionPoint point;
point.ContactPoint = new NewtonVector3(contacts[offset], contacts[offset + 1], contacts[offset + 2]).ToPointWPF();
point.Normal = new NewtonVector3(normals[offset], normals[offset + 1], normals[offset + 2]).ToVectorWPF();
point.PenetrationDistance = penetrationDistances[cntr];
point.TimeOfImpact = 0; // time of impact has no meaning for this method (I'm just reusing the same structure between two methods)
retVal.Add(point);
}
// Exit Function
return retVal.ToArray();
}
private static double GetIntersectionsSprtSize(SortedList<int, double> sizes, CollisionHull[] hulls, int index)
{
if (sizes.ContainsKey(index))
{
return sizes[index];
}
//NOTE: The returned AABB will be bigger than the actual object
Point3D min, max;
hulls[index].CalculateAproximateAABB(out min, out max);
// Just get the average size of this box
double size = ((max.X - min.X) + (max.Y - min.Y) + (max.Z - min.Z)) / 3d;
sizes.Add(index, size);
return size;
}
public IntersectionPoint[] GetIntersectingPoints_HullToHull(int maxReturnCount, double timestep, Vector3D velocity, Vector3D angularVelocity, CollisionHull otherHull, Vector3D otherVelocity, Vector3D otherAngularVelocity, int threadIndex, Transform3D thisTransform, Transform3D otherTransform)
{
float[] finalOffsetThis = thisTransform == null ? new NewtonMatrix(Matrix3D.Identity).Matrix : new NewtonMatrix(thisTransform.Value).Matrix; // not including the member's offset, because newton is already accounting for it.
float[] finalOffsetOther = thisTransform == null ? new NewtonMatrix(Matrix3D.Identity).Matrix : new NewtonMatrix(otherTransform.Value).Matrix;
// Prep some arrays to hold the return values
float[] contacts = new float[3 * maxReturnCount];
float[] normals = new float[3 * maxReturnCount];
float[] penetrationDistances = new float[maxReturnCount];
float[] timesOfImpact = new float[maxReturnCount];
// Call Newton
int pointCount = Newton.NewtonCollisionCollideContinue(_world.Handle, maxReturnCount, Convert.ToSingle(timestep),
_handle, finalOffsetThis, new NewtonVector3(velocity).Vector, new NewtonVector3(angularVelocity).Vector,
otherHull.Handle, finalOffsetOther, new NewtonVector3(otherVelocity).Vector, new NewtonVector3(otherAngularVelocity).Vector,
timesOfImpact, contacts, normals, penetrationDistances,
threadIndex);
// Convert to c#
List<IntersectionPoint> retVal = new List<IntersectionPoint>();
for (int cntr = 0; cntr < pointCount; cntr++)
{
int offset = cntr * 3;
IntersectionPoint point;
point.ContactPoint = new NewtonVector3(contacts[offset], contacts[offset + 1], contacts[offset + 2]).ToPointWPF();
point.Normal = new NewtonVector3(normals[offset], normals[offset + 1], normals[offset + 2]).ToVectorWPF();
point.PenetrationDistance = penetrationDistances[cntr];
point.TimeOfImpact = timesOfImpact[cntr];
retVal.Add(point);
}
// Exit Function
return retVal.ToArray();
}
/// <summary>
/// This returns collision hulls that are offset where the parts are (only recreates hulls for parts that have moved)
/// </summary>
public CollisionHull[] GetCollisionHulls()
{
CollisionHull[] retVal = new CollisionHull[_parts.Length];
for (int cntr = 0; cntr < _parts.Length; cntr++)
{
if (_hasMoved[cntr])
{
retVal[cntr].Dispose();
retVal[cntr] = _parts[cntr].CreateCollisionHull(_world);
_hasMoved[cntr] = false;
}
else
{
retVal[cntr] = _hulls[cntr];
}
}
return retVal;
}
private void btnBodyPropellerPlates_Click(object sender, RoutedEventArgs e)
{
try
{
RemoveCurrentBody();
_world.Pause();
// Material
MaterialGroup materials = new MaterialGroup();
materials.Children.Add(new DiffuseMaterial(new SolidColorBrush(_colors.HullFace)));
materials.Children.Add(_colors.HullFaceSpecular);
#region Templates
// These positions are transformed for each blade
Point3D[] bladePositions = new Point3D[2];
bladePositions[0] = new Point3D(-1d, -.25d, -.05d);
bladePositions[1] = new Point3D(1d, .25d, .05d);
// This tranform is throw away. It's just used to transform points
Transform3DGroup transform = new Transform3DGroup();
transform.Children.Add(new RotateTransform3D(new AxisAngleRotation3D(new Vector3D(1, 0, 0), 90d + 15d))); // rotar tilt (the flow defaults to -x, so face them into the wind)
transform.Children.Add(new TranslateTransform3D(new Vector3D(1d + .5d, 0, 0))); // pull away from the center a bit
transform.Children.Add(new RotateTransform3D(new AxisAngleRotation3D(new Vector3D(0, 0, 1), 90))); // I don't want it along X, I want it along Y
#endregion
// Blades
MeshGeometry3D[] meshBlades = new MeshGeometry3D[StaticRandom.Next(1, 8)];
CollisionHull[] collisionBlades = new CollisionHull[meshBlades.Length];
Transform3D[] transformBlades = new Transform3D[meshBlades.Length];
GeometryModel3D[] geometryBlades = new GeometryModel3D[meshBlades.Length];
ModelVisual3D[] modelBlades = new ModelVisual3D[meshBlades.Length];
for (int cntr = 0; cntr < meshBlades.Length; cntr++)
{
meshBlades[cntr] = UtilityWPF.GetCube_IndependentFaces(bladePositions[0], bladePositions[1]);
collisionBlades[cntr] = CollisionHull.CreateBox(_world, 0, bladePositions[1] - bladePositions[0], transform.Value);
transformBlades[cntr] = transform.Clone();
geometryBlades[cntr] = new GeometryModel3D();
geometryBlades[cntr].Material = materials;
geometryBlades[cntr].BackMaterial = materials;
geometryBlades[cntr].Geometry = meshBlades[cntr];
geometryBlades[cntr].Transform = transformBlades[cntr];
modelBlades[cntr] = new ModelVisual3D();
modelBlades[cntr].Content = geometryBlades[cntr];
modelBlades[cntr].Transform = _modelOrientationTrackball.Transform;
// Prep for the next blade
transform.Children.Add(new RotateTransform3D(new AxisAngleRotation3D(new Vector3D(1, 0, 0), 360d / meshBlades.Length)));
}
// Body
CollisionHull collisionHull = CollisionHull.CreateCompoundCollision(_world, 0, collisionBlades);
_body = new Body(collisionHull, _modelOrientationTrackball.Transform.Value, 1, null);
collisionHull.Dispose();
foreach (CollisionHull hull in collisionBlades)
{
hull.Dispose();
}
_fluidHull = new FluidHull();
_fluidHull.Triangles = FluidHull.FluidTriangle.GetTrianglesFromMesh(meshBlades, transformBlades);
_fluidHull.IsClosedConvexInUniformField = false;
_fluidHull.Transform = _modelOrientationTrackball.Transform;
_fluidHull.Field = _field;
_fluidHull.Body = _body;
// Wireframe
ScreenSpaceLines3D modelWireframe = GetModelWireframe(_fluidHull);
modelWireframe.Transform = _modelOrientationTrackball.Transform;
_viewport.Children.Add(modelWireframe);
// Rope
Point3D bodyAttachPoint = _modelOrientationTrackball.Transform.Transform(new Point3D(.25, 0, 0));
Point3D anchorPoint = _modelOrientationTrackball.Transform.Transform(new Point3D(3, 0, 0));
AddRope(bodyAttachPoint, anchorPoint, Math1D.DegreesToRadians(1d));
// Add to the viewport
foreach (ModelVisual3D model in modelBlades)
{
_viewport.Children.Add(model);
}
// Finish setting up the body
Visual3D[] visuals = new Visual3D[meshBlades.Length + 1];
for (int cntr = 0; cntr < meshBlades.Length; cntr++)
{
visuals[cntr] = modelBlades[cntr];
}
visuals[visuals.Length - 1] = modelWireframe;
_body.Visuals = visuals;
_body.ApplyForceAndTorque += new EventHandler<BodyApplyForceAndTorqueArgs>(Body_ApplyForceAndTorque);
_body.BodyMoved += new EventHandler(Body_BodyMoved);
_world.UnPause();
}
catch (Exception ex)
{
MessageBox.Show(ex.ToString(), this.Title, MessageBoxButton.OK, MessageBoxImage.Error);
}
}
/// <summary>
/// Returns the closest points along the surface of the two hulls (null if the hulls intersect each other)
/// </summary>
public bool GetNearestPoint_HullToHull(out Point3D?contactPointThis, out Point3D?contactPointOther, out Vector3D?normal, CollisionHull otherHull, int threadIndex)
{
return(GetNearestPoint_HullToHull(out contactPointThis, out contactPointOther, out normal, otherHull, threadIndex, null, null));
}
