public BasicBody(IDynamicBody dynamics, IElectroMag eMProps, BasicMaterial material, IEdgeIntersector collisionShape, IVolume shape, IOverlapable boundVolume)
{
Dynamics = dynamics;
EMProps = eMProps;
Material = material;
CollisionShape = collisionShape;
Shape = shape;
BoundVolume = boundVolume;
Dynamics.FrameFinished += (sender, e) => FrameFinished?.Invoke(sender, e);
}
public BasicBody(IDynamicBody dynamics, IElectroMag eMProps, BasicMaterial material, IEdgeIntersector collisionShape, IVolume shape, IOverlapable boundVolume)
{
Dynamics = dynamics;
EMProps = eMProps;
Material = material;
CollisionShape = collisionShape;
Shape = shape;
BoundVolume = boundVolume;
Dynamics.FrameFinished += (sender, e) => FrameFinished?.Invoke(sender, e);
}
protected override IEnumerable <Intersection> EnactStrategyInternal(IEdgeIntersector firstShape, IEdgeIntersector secondShape, Transformation firstToSecond)
{
var sphere = firstShape as SphereIntersectorVolume ?? secondShape as SphereIntersectorVolume;
var triangles = firstShape as CompositeIntersector ?? secondShape as CompositeIntersector;
if (sphere == null || triangles == null)
{
throw new ArgumentException("Invalid shape types for this strategy");
}
Vector3 spherePos = sphere == firstShape ? firstToSecond.TransformA.Pos : firstToSecond.TransformB.Pos;
Vector3 trianglesPos = triangles == firstShape ? firstToSecond.TransformA.Pos : firstToSecond.TransformB.Pos;
foreach (TriangleIntersector tri in triangles.Primitives)
{
Vector3 trianglePos = trianglesPos + (1.0 / 3.0) * (tri.A + tri.B + tri.C);
Vector3 sphereToTriangle = trianglePos - spherePos;
if (sphereToTriangle * tri.N > 0)
{
continue; // triangle is facing away from sphere -> no contact
}
// find d in the plane equation (http://mathworld.wolfram.com/Plane.html)
double d = -trianglePos * tri.N;
// http://mathworld.wolfram.com/Point-PlaneDistance.html
double dist = spherePos * tri.N + d;
if (dist > sphere.Radius)
{
continue;
}
Vector3 intersectionPt = spherePos + dist * -tri.N;
// check if itnersectionPt is b/w the 3 vertices
bool b1 = Sign(intersectionPt, spherePos, trianglesPos + tri.A, trianglesPos + tri.B) < 0.0;
bool b2 = Sign(intersectionPt, spherePos, trianglesPos + tri.B, trianglesPos + tri.C) < 0.0;
bool b3 = Sign(intersectionPt, spherePos, trianglesPos + tri.C, trianglesPos + tri.A) < 0.0;
if (b1 != b2)
{
continue;
}
if (b2 != b3)
{
continue;
}
yield return(new Intersection(intersectionPt, triangles == firstShape ? tri.N : -tri.N));
}
}
protected override IEnumerable <Intersection> EnactStrategyInternal(IEdgeIntersector firstShape, IEdgeIntersector secondShape, Transformation firstToSecond)
{
var sphereOne = (SphereIntersectorVolume)firstShape;
var sphereTwo = (SphereIntersectorVolume)secondShape;
var firstPos = firstToSecond.TransformA.Pos;
var secondPos = firstToSecond.TransformB.Pos;
Vector3 firstPosToSecond = secondPos - firstPos;
double totalRadius = sphereOne.Radius + sphereTwo.Radius;
if (firstPosToSecond.Magnitude > totalRadius)
{
yield break;
}
yield return(new Intersection(firstPos + firstPosToSecond * (sphereOne.Radius / totalRadius), firstPosToSecond.UnitDirection));
}
// all of the contacts where second's edges intersect first's body
// handles all cordinate transformation from each body to the world
private IEnumerable <Contact> ContactsOnFirst(IBody first, IBody second)
{
IEdgeIntersector firstShape = first.CollisionShape;
IEdgeIntersector secondShape = second.CollisionShape;
Transform firstTransform = first.Dynamics.Transform;
Transform secondTransform = second.Dynamics.Transform;
var secondToFirst = new Transformation(secondTransform, firstTransform);
if (IntersectStrats.HasStrategy(firstShape, secondShape))
{
return(IntersectStrats.EnactStrategy(firstShape, secondShape, secondToFirst.Reverse()).Select(i => new Contact(i, first, second)));
}
// general case
return
// everything is converted to the firsts local transform coords
(secondShape.Edges.SelectMany(secondEdge => firstShape.FindIntersections(secondToFirst.TransformEdge(secondEdge))
.Select(i => new Contact(firstTransform.ToWorldSpace(i), first, second)))); // then to world coords
}
public BasicBody WithCollisionShape(IEdgeIntersector newCollisionShape) => new BasicBody(Dynamics, EMProps, Material, newCollisionShape, Shape, BoundVolume);
public BasicBody WithCollisionShape(IEdgeIntersector newCollisionShape) => new BasicBody(Dynamics, EMProps, Material, newCollisionShape, Shape, BoundVolume);
