internal void ApplyRope()
{
beginPoint = attachedBody.Pos;
if (attachedBody2 != null)
{
endPoint = attachedBody2.Pos;
}
Length = (beginPoint - endPoint).Length;
if (Length >= Length0)
{
if (attachedBody2 != null)
{
}
else
{
Vector allFroce = new Vector(0, 0);
foreach (Vector force in attachedBody.forces)
{
allFroce += force;
}
Vector velNextIter = attachedBody.Vel + allFroce / attachedBody.Mass * PhysicsTable.dt;
Vector toMirror = new Vector((beginPoint - endPoint).Angle + Math.PI / 2, 2, true);
attachedBody.Vel = velNextIter.MirrorVel(toMirror);
}
}
//if (attachedBody2 != null) attachedBody2.forces.Add(-K * (Length0 - Length) * (beginPoint - endPoint).Norm());
}
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;
}
}
}