// Calculate the closest points on two shapes given the closest edge on their minkowski difference to (0, 0)
public static ClosestPoints ClosestPointsNew(MinkowskiPoint v0, MinkowskiPoint v1)
{
// Find the closest p(t) on the minkowski difference to (0, 0)
float t = cpCollision.ClosestT(v0.ab, v1.ab);
cpVect p = cpCollision.LerpT(v0.ab, v1.ab, t);
// Interpolate the original support points using the same 't' value as above.
// This gives you the closest surface points in absolute coordinates. NEAT!
cpVect pa = cpCollision.LerpT(v0.a, v1.a, t);
cpVect pb = cpCollision.LerpT(v0.b, v1.b, t);
ulong id = ((v0.id & 0xFFFF) << 16 | (v1.id & 0xFFFF));
// First try calculating the MSA from the minkowski difference edge.
// This gives us a nice, accurate MSA when the surfaces are close together.
cpVect delta = cpVect.cpvsub(v1.ab, v0.ab);
cpVect n = cpVect.cpvnormalize(cpVect.cpvrperp(delta));
float d = cpVect.cpvdot(n, p);
if (d <= 0.0f || (-1.0f < t && t < 1.0f))
{
// If the shapes are overlapping, or we have a regular vertex/edge collision, we are done.
ClosestPoints points = new ClosestPoints(pa, pb, n, d, id);
return(points);
}
else
{
// Vertex/vertex collisions need special treatment since the MSA won't be shared with an axis of the minkowski difference.
float d2 = cpVect.cpvlength(p);
cpVect n2 = cpVect.cpvmult(p, 1.0f / (d2 + float.MinValue));
ClosestPoints points = new ClosestPoints(pa, pb, n2, d2, (ulong)id);
return(points);
}
}
public void Distance(StartEndPoint points, out float distance, out ClosestPoints order)
{
float[] distances = new float[4];
distances[0] = StartPoint.Distance(points.StartPoint);
distances[1] = StartPoint.Distance(points.EndPoint);
distances[2] = EndPoint.Distance(points.StartPoint);
distances[3] = EndPoint.Distance(points.EndPoint);
float minDist = distances.Min();
if (distances[0] == minDist)
{
order = ClosestPoints.StartStart;
}
else if (distances[1] == minDist)
{
order = ClosestPoints.StartEnd;
}
else if (distances[2] == minDist)
{
order = ClosestPoints.EndStart;
}
else
{
order = ClosestPoints.EndEnd;
}
distance = minDist;
}
public void Distance(StartEndPoint points, out float distance, out ClosestPoints order)
{
float[] distances = new float[4];
distances[0] = StartPoint.Distance(points.StartPoint);
distances[1] = StartPoint.Distance(points.EndPoint);
distances[2] = EndPoint.Distance(points.StartPoint);
distances[3] = EndPoint.Distance(points.EndPoint);
float minDist = distances.Min();
if (distances[0] == minDist)
{
order = ClosestPoints.StartStart;
}
else if (distances[1] == minDist)
{
order = ClosestPoints.StartEnd;
}
else if (distances[2] == minDist)
{
order = ClosestPoints.EndStart;
}
else
{
order = ClosestPoints.EndEnd;
}
distance = minDist;
}
private void HandlePointSelection(MouseButtonEventArgs e)
{
const float PointRadius = 3;
Vector pos = (Vector)e.GetPosition(MainImageContainer);
IEnumerable <(IDrawingObject, DrawingPoint)> ClosestPoints = DrawingObjects.Select(obj => (obj, obj.GetClosestPoint(pos)));
if (ClosestPoints.Any())
{
(IDrawingObject, DrawingPoint)ClosestPoint = ClosestPoints.Aggregate((min, x) => min.Item2.dist(pos) < x.Item2.dist(pos) ? min : x);
//Ctrl
if (Keyboard.IsKeyDown(Key.LeftCtrl))
{
//Clicked point
if (ClosestPoint.Item2.dist(pos) <= MaximumSelectDistance)
{
//Clicked once
if (e.ClickCount == 1)
{
//Point already selected
if (selectedPoints.ContainsKey(ClosestPoint.Item2))
{
//Deselect point
selectedPoints.Remove(ClosestPoint.Item2);
}
//Point not selected
else
{
//Select point
selectedPoints.Add(ClosestPoint.Item2, new FilledCircle(ClosestPoint.Item2, PointRadius, System.Drawing.Color.Blue));
}
}
//Clicked twice
else
{
//Select shape
foreach (DrawingPoint p in ClosestPoint.Item1.GetTranslationPoints())
{
if (!selectedPoints.ContainsKey(p))
{
selectedPoints.Add(p, new FilledCircle(p, PointRadius, System.Drawing.Color.Blue));
}
}
}
}
}
//no Ctrl
else
{
//Clicked point
if (ClosestPoint.Item2.dist(pos) <= MaximumSelectDistance)
{
//Clicked once
if (e.ClickCount == 1)
{
//Point not selected
if (!selectedPoints.ContainsKey(ClosestPoint.Item2))
{
//Select only this point
selectedPoints.Clear();
selectedPoints.Add(ClosestPoint.Item2, new FilledCircle(ClosestPoint.Item2, PointRadius, System.Drawing.Color.Blue));
}
}
//Clicked twice
else
{
//Select only this shape
selectedPoints.Clear();
foreach (DrawingPoint p in ClosestPoint.Item1.GetTranslationPoints())
{
if (!selectedPoints.ContainsKey(p))
{
selectedPoints.Add(p, new FilledCircle(p, PointRadius, System.Drawing.Color.Blue));
}
}
}
}
}
}
PreviousMousePosition = (Vector)e.GetPosition(MainImageContainer);
currentState = selectedPoints.Count > 0 ? UIState.MovingExistingPoints : UIState.Nothing;
}
// Calculate the closest points on two shapes given the closest edge on their minkowski difference to (0, 0)
public static ClosestPoints ClosestPointsNew(MinkowskiPoint v0, MinkowskiPoint v1)
{
// Find the closest p(t) on the minkowski difference to (0, 0)
float t = cpCollision.ClosestT(v0.ab, v1.ab);
cpVect p = cpCollision.LerpT(v0.ab, v1.ab, t);
// Interpolate the original support points using the same 't' value as above.
// This gives you the closest surface points in absolute coordinates. NEAT!
cpVect pa = cpCollision.LerpT(v0.a, v1.a, t);
cpVect pb = cpCollision.LerpT(v0.b, v1.b, t);
ulong id = ((v0.id & 0xFFFF) << 16 | (v1.id & 0xFFFF));
// First try calculating the MSA from the minkowski difference edge.
// This gives us a nice, accurate MSA when the surfaces are close together.
cpVect delta = cpVect.cpvsub(v1.ab, v0.ab);
cpVect n = cpVect.cpvnormalize(cpVect.cpvrperp(delta));
float d = cpVect.cpvdot(n, p);
if (d <= 0.0f || (-1.0f < t && t < 1.0f))
{
// If the shapes are overlapping, or we have a regular vertex/edge collision, we are done.
ClosestPoints points = new ClosestPoints(pa, pb, n, d, id);
return points;
}
else
{
// Vertex/vertex collisions need special treatment since the MSA won't be shared with an axis of the minkowski difference.
float d2 = cpVect.cpvlength(p);
cpVect n2 = cpVect.cpvmult(p, 1.0f / (d2 + float.MinValue));
ClosestPoints points = new ClosestPoints(pa, pb, n2, d2, (ulong)id);
return points;
}
}
//MARK: Contact Clipping
public static void ContactPoints(Edge e1, Edge e2, ClosestPoints points, ref cpCollisionInfo info)
{
float mindist = e1.r + e2.r;
if (points.d <= mindist)
{
info.n = new cpVect(points.n);
cpVect n = new cpVect(points.n);
// Distances along the axis parallel to n
float d_e1_a = cpVect.cpvcross(e1.a.p, n);
float d_e1_b = cpVect.cpvcross(e1.b.p, n);
float d_e2_a = cpVect.cpvcross(e2.a.p, n);
float d_e2_b = cpVect.cpvcross(e2.b.p, n);
float e1_denom = 1.0f / (d_e1_b - d_e1_a);
float e2_denom = 1.0f / (d_e2_b - d_e2_a);
// Project the endpoints of the two edges onto the opposing edge, clamping them as necessary.
// Compare the projected points to the collision normal to see if the shapes overlap there.
{
cpVect p1 = cpVect.cpvadd(cpVect.cpvmult(n, e1.r), cpVect.cpvlerp(e1.a.p, e1.b.p, cp.cpfclamp01((d_e2_b - d_e1_a) * e1_denom)));
cpVect p2 = cpVect.cpvadd(cpVect.cpvmult(n, -e2.r), cpVect.cpvlerp(e2.a.p, e2.b.p, cp.cpfclamp01((d_e1_a - d_e2_a) * e2_denom)));
float dist = cpVect.cpvdot(cpVect.cpvsub(p2, p1), n);
if (dist <= 0.0f)
{
ulong hash_1a2b = cp.CP_HASH_PAIR(e1.a.hash, e2.b.hash);
InfoPushContact(ref info, p1, p2, hash_1a2b);
}
}
{
cpVect p1 = cpVect.cpvadd(cpVect.cpvmult(n, e1.r), cpVect.cpvlerp(e1.a.p, e1.b.p, cp.cpfclamp01((d_e2_a - d_e1_a) * e1_denom)));
cpVect p2 = cpVect.cpvadd(cpVect.cpvmult(n, -e2.r), cpVect.cpvlerp(e2.a.p, e2.b.p, cp.cpfclamp01((d_e1_b - d_e2_a) * e2_denom)));
float dist = cpVect.cpvdot(cpVect.cpvsub(p2, p1), n);
if (dist <= 0.0f)
{
ulong hash_1b2a = cp.CP_HASH_PAIR(e1.b.hash, e2.a.hash);
InfoPushContact(ref info, p1, p2, hash_1b2a);
}
}
}
}
