/// <summary>
/// Draw a single RoundLine. Usually you want to draw a list of RoundLines
/// at a time instead for better performance.
/// </summary>
public void Draw(RoundLine roundLine, float lineRadius, Color lineColor /*, Matrix viewProjMatrix*/,
float time, string techniqueName)
{
device.VertexDeclaration = vdecl;
device.Vertices[0].SetSource(vb, 0, bytesPerVertex);
device.Indices = ib;
viewProjMatrixParameter.SetValue(mProjection);
timeParameter.SetValue(time);
lineColorParameter.SetValue(lineColor.ToVector4());
lineRadiusParameter.SetValue(lineRadius);
blurThresholdParameter.SetValue(BlurThreshold);
int iData = 0;
translationData[iData++] = roundLine.P0.X;
translationData[iData++] = roundLine.P0.Y;
translationData[iData++] = roundLine.Rho;
translationData[iData++] = roundLine.Theta;
instanceDataParameter.SetValue(translationData);
if (techniqueName == null)
{
effect.CurrentTechnique = effect.Techniques[0];
}
else
{
effect.CurrentTechnique = effect.Techniques[techniqueName];
}
effect.Begin();
EffectPass pass = effect.CurrentTechnique.Passes[0];
pass.Begin();
int numInstancesThisDraw = 1;
device.DrawIndexedPrimitives(PrimitiveType.TriangleList, 0, 0, numVertices, 0, numPrimitivesPerInstance * numInstancesThisDraw);
NumLinesDrawn += numInstancesThisDraw;
pass.End();
effect.End();
}
/// <summary>
/// Given a bunch of lines (lineList) and a disc that wants to move from currentPos
/// to proposedPos, handle intersections and wall sliding and set finalPos to the
/// position that the disc should move to.
/// </summary>
public void CollideAndSlide(List <RoundLine> lineList, Vector2 currentPos, Vector2 proposedPos, float lineRadius, float discRadius, out Vector2 finalPos)
{
Vector2 oldPos = currentPos;
Vector2 oldTarget = proposedPos;
bool pastFirstSlide = false;
// Keep looping until there's no further desire to move somewhere else.
while (Vector2.DistanceSquared(oldPos, oldTarget) > 0.001f * 0.001f)
{
// oldPos should be safe (no intersection with anything in lineList)
Debug.Assert(MinDistanceSquaredDeviation(lineList, oldPos, lineRadius, discRadius) >= 0);
// Find minimum "t" at which we collide with a line
float minT = 1.0f; // Parametric "t" of the closest collision
RoundLine minTLine = null; // The line which causes closest collision
foreach (RoundLine line in lineList)
{
float tMinThisLine;
float minDist = lineRadius + discRadius;
float minDist2 = minDist * minDist;
float curDist2 = line.DistanceSquaredPointToVirtualLine(oldPos);
Debug.Assert(curDist2 - minDist2 >= 0);
line.FindFirstIntersection(oldPos, oldTarget, minDist, out tMinThisLine);
if (tMinThisLine >= 0 && tMinThisLine <= 1)
{
// We can move tMinThisLine toward the line, before intersecting it -- but
// we might intersect other lines first, so keep looking for
// smaller tMinThisLine values on other lines
// But first, refine tMinThisLine (if needed) until it satisfies the distance test
Vector2 newPos = new Vector2(MathHelper.Lerp(oldPos.X, oldTarget.X, tMinThisLine),
MathHelper.Lerp(oldPos.Y, oldTarget.Y, tMinThisLine));
if (line.DistanceSquaredPointToVirtualLine(newPos) - minDist2 < 0)
{
float ta = 0;
float tb = tMinThisLine;
for (int iterRefine = 0; iterRefine < 10; iterRefine++)
{
float tc = (ta + tb) / 2;
Vector2 newPosC = new Vector2(MathHelper.Lerp(oldPos.X, oldTarget.X, tc),
MathHelper.Lerp(oldPos.Y, oldTarget.Y, tc));
float newDistC = line.DistanceSquaredPointToVirtualLine(newPosC);
if (newDistC - minDist2 < 0)
{
tb = tc;
}
else
{
ta = tc;
}
}
tMinThisLine = ta;
}
// Remember this "t" and the line that caused it, if it's the closest so far
if (tMinThisLine < minT)
{
minT = tMinThisLine;
minTLine = line;
}
}
else
{
// This line has no issue with the disc moving to oldTarget...or does it?
// Due to floating point variances, we have to double-check and pick a new "t"
// if oldTarget is actually too close to this line
float newDist = line.DistanceSquaredPointToVirtualLine(oldTarget);
if (newDist - minDist2 < 0)
{
// Find a "t" that is as large as possible while avoiding collision
float ta = 0;
float tb = 1;
for (int i = 0; i < 10; i++)
{
float tc = (ta + tb) / 2;
Vector2 ptC = new Vector2(MathHelper.Lerp(oldPos.X, oldTarget.X, tc),
MathHelper.Lerp(oldPos.Y, oldTarget.Y, tc));
float distC = line.DistanceSquaredPointToVirtualLine(ptC);
if (distC - minDist2 < 0)
{
tb = tc;
}
else
{
ta = tc;
}
}
if (ta < minT)
{
minT = ta;
minTLine = line;
}
}
}
}
// At this point, we've looped through all lines and found the minimum "t" value and its line
if (minTLine == null)
{
// No intersections were found, so move straight to the target
Debug.Assert(MinDistanceSquaredDeviation(lineList, oldTarget, lineRadius, discRadius) >= 0);
oldPos = oldTarget; // no further motion required
}
else
{
// Collide and slide against minTLine
Vector2 newPos = new Vector2(MathHelper.Lerp(oldPos.X, oldTarget.X, minT),
MathHelper.Lerp(oldPos.Y, oldTarget.Y, minT));
Vector2 newTarget;
float minDist2 = (lineRadius + discRadius) * (lineRadius + discRadius);
// Refine minT / newPos til it passes the distance test
float minDistDeviation = MinDistanceSquaredDeviation(lineList, newPos, lineRadius, discRadius);
if (minDistDeviation < 0)
{
float ta = 0;
float tb = minT;
for (int i = 0; i < 10; i++)
{
float tc = (ta + tb) / 2;
Vector2 ptC = new Vector2(MathHelper.Lerp(oldPos.X, oldTarget.X, tc),
MathHelper.Lerp(oldPos.Y, oldTarget.Y, tc));
if (MinDistanceSquaredDeviation(lineList, ptC, lineRadius, discRadius) < 0)
{
tb = tc;
}
else
{
ta = tc;
}
}
minT = ta;
newPos = new Vector2(MathHelper.Lerp(oldPos.X, oldTarget.X, minT),
MathHelper.Lerp(oldPos.Y, oldTarget.Y, minT));
}
Debug.Assert(MinDistanceSquaredDeviation(lineList, newPos, lineRadius, discRadius) >= 0);
// This is a bit of a hack to avoid "jiggling" when the disc is pressed
// against two walls that are at an obtuse angle. Perhaps the real fix
// is to project the new slide vector against the original motion vector?
// In practice, we only ever need to slide once -- this fixes the issue.
bool doSlide = true;
if (pastFirstSlide)
{
newTarget = newPos;
doSlide = false;
}
else
{
pastFirstSlide = true;
}
if (doSlide)
{
Vector2 closestP;
float d2 = minTLine.DistanceSquaredPointToVirtualLine(newPos, out closestP);
RoundLine connectionLine = new RoundLine(newPos, closestP);
Vector2 lineNormal = (newPos - closestP);
lineNormal.Normalize();
// create a normal to the above line
// (which would thus be a tangent to minTLine)
float theta = connectionLine.Theta;
theta += MathHelper.PiOver2;
Vector2 newPoint = new Vector2(newPos.X + (float)Math.Cos(theta), newPos.Y + (float)Math.Sin(theta));
// Project the post-intersection line onto the above line, to provide "wall sliding" effect
// v1 dot v2 = |v2| * (projection of v1 onto v2), and |v2| is 1
Vector2 v1 = oldTarget - newPos;
Vector2 v2 = newPoint - newPos;
float dotprod = Vector2.Dot(v1, v2);
newTarget = newPos + dotprod * v2;
// newTarget should not be too close to minTLine
float newTargetDist = minTLine.DistanceSquaredPointToVirtualLine(newTarget);
if (newTargetDist - minDist2 < 0)
{
float shiftAmtA = 0; // not enough
float shiftAmtB = -(newTargetDist - minDist2) + 0.0001f; // too much
for (int i = 0; i < 10; i++)
{
float shiftAmtC = (shiftAmtA + shiftAmtB) / 2.0f;
Vector2 newTargetTest = newTarget + (shiftAmtC * lineNormal);
float newTargetTestDist = minTLine.DistanceSquaredPointToVirtualLine(newTargetTest);
if (newTargetTestDist - minDist2 >= 0)
{
shiftAmtB = shiftAmtC;
}
else
{
shiftAmtA = shiftAmtC;
}
}
newTarget += shiftAmtB * lineNormal;
}
}
else
{
newTarget = newPos; // No slide
}
// Get ready to loop around and see if we can move from newPos to newTarget
// without colliding with anything
oldPos = newPos;
oldTarget = newTarget;
Debug.Assert(minTLine.DistanceSquaredPointToVirtualLine(newPos) - minDist2 >= 0);
}
}
// oldTarget == oldPos (or is very close), so no further moving/sliding is needed.
finalPos = oldPos;
Debug.Assert(MinDistanceSquaredDeviation(lineList, finalPos, lineRadius, discRadius) >= 0);
}
