public Vector MirrorPos(Vector beginPoint, Vector endPoint)
{
Vector line = endPoint - beginPoint;
Vector e = line.Norm();
Matrix projector = Vector.Diadic(e, e);
Matrix mirror = 2 * projector - Matrix.Identity(2);
return((mirror * this) - (mirror - Matrix.Identity(2)) * beginPoint);
}
public Vector MirrorVel(Vector toMirror)
{
Vector line = toMirror;
Vector e = line.Norm();
Matrix projector = Vector.Diadic(e, e);
Matrix mirror = 2 * projector - Matrix.Identity(2);
return(mirror * this);
}
internal void CheckBodies()
{
Vector surfaceMiddle = (BeginPoint + EndPoint) / 2;
Vector surfaceVect = EndPoint - BeginPoint;
foreach (Body body in bodies)
{
Vector allFroce = new Vector(0, 0);
foreach (Vector force in body.forces)
{
allFroce += force;
}
Vector velNextIter = body.Vel + allFroce / body.Mass * PhysicsTable.dt;
Vector posNextIter = body.Pos + velNextIter * PhysicsTable.dt;
Vector posTransf = body.Pos - surfaceMiddle;
Vector posTransfNextIter = posNextIter - surfaceMiddle;
Vector a = 1.0 / 2 * surfaceVect - posTransf;
Vector b = -1.0 / 2 * surfaceVect - posTransf;
Vector c = posTransfNextIter - posTransf;
if (false && body.Pos.OnLine(BeginPoint, EndPoint))
{
Vector normVec = surfaceVect / surfaceVect.Length;
Matrix projector = Matrix.Identity(2) - Vector.Diadic(normVec, normVec);
Vector surfaceForce = projector * allFroce;
//if (Math.Sign((EndPoint - BeginPoint).X) == Math.Sign(surfaceForce.X))
//{
body.forces.Add(-surfaceForce);
body.Vel = new Vector(body.Vel.X, 0);
//}
}
else
//if (body.Pos.NextToLine(BeginPoint, EndPoint))
if (Math.Sign(Vector.VectorMultAbs(c, a)) != Math.Sign(Vector.VectorMultAbs(c, b)) && Math.Sign(Vector.VectorMultAbs(surfaceVect, posTransf)) != Math.Sign(Vector.VectorMultAbs(surfaceVect, posTransfNextIter)))
{
body.Vel = velNextIter.MirrorVel(BeginPoint, EndPoint);
stillColliding[Counter] = true;
}
}
}