public override void ResolveConstraint(World world)
{
Vector_2d Dist = world.PointMassList[PointA].Pos.Sub(world.PointMassList[PointB].Pos);
float Distance = (float)Math.Sqrt(Dist.Dot(Dist));
float Difference = (UsedDistance - Distance) / Distance;
if (Math.Abs(Difference) > DistanceHeld)//Give before taking damadge
{
Damadge -= 5.0F * Math.Abs(Difference) * world.DeltaTime * world.DeltaConstraint;
Damadge = Math.Max(0, Damadge);//For colour
}
else
{
Damadge = Math.Min(Damadge + (1.0F * world.DeltaTime * world.DeltaConstraint), 100);
}
const float Give = 0;
if (Math.Abs(Distance - UsedDistance) > Give)//Give before moving
{
Vector_2d translate = new Vector_2d((float)(Dist.X * Difference), (float)(Dist.Y * Difference));
translate = translate.Mult(world.DeltaConstraint);
translate = translate.Mult(0.5F);
float scalarP1 = (world.PointMassList[PointA].InverseMass / (world.PointMassList[PointA].InverseMass + world.PointMassList[PointB].InverseMass)) * Stiffness;
float scalarP2 = Stiffness - scalarP1;
//world.PointMassList[PointA].Accerate(translate.Mult(scalarP1),world);
//world.PointMassList[PointB].Accerate(translate.Mult(scalarP2).Inverted(), world);
world.PointMassList[PointA].Pos = world.PointMassList[PointA].Pos.Add(translate.Mult(scalarP1));
world.PointMassList[PointB].Pos = world.PointMassList[PointB].Pos.Sub(translate.Mult(scalarP2));
}
}
public void VelocityColl(PointMass A, Connection B)
{
Vector_2d pos = get_line_intersection(A.Pos, A.OldPos, PointMassList[B.PointA].Pos, PointMassList[B.PointB].Pos);
if (pos != null)
{
float COR = 0.98F;//Coefficent of restetution
Vector_2d ConB = PointMassList[B.PointA].Pos.Sub(PointMassList[B.PointB].Pos);
Vector_2d ConBPerpNormal = ConB.Perpendicular().Div((float)Math.Sqrt(ConB.Dot(ConB)));
Vector_2d Velocity = A.OldPos.Sub(A.Pos);
Vector_2d newVelocity = PointMassList[B.PointA].OldPos.Sub(PointMassList[B.PointA].Pos);
newVelocity = newVelocity.Add(PointMassList[B.PointB].OldPos.Sub(PointMassList[B.PointB].Pos));
//newVelocity = newVelocity.Sub(Velocity);
float Distribution = (float)Math.Sqrt(pos.Sub(PointMassList[B.PointA].Pos).Dot(pos.Sub(PointMassList[B.PointA].Pos))) / B.UsedDistance;
A.Pos = A.OldPos.Add(newVelocity.Mult(COR));
PointMassList[B.PointA].Pos = PointMassList[B.PointA].OldPos.Add(Velocity.Mult(1 - Distribution));
PointMassList[B.PointB].Pos = PointMassList[B.PointB].OldPos.Add(Velocity.Mult(Distribution));
B.Damadge -= (A.Mass * A.DamadgeMulti);
if (A.State == 6 && !A.Grabbed)
{
int conA = Game.World.AddConnection(new ConnectionStaticDistance(Game.World, A.Id, B.PointA));
int conB = Game.World.AddConnection(new ConnectionStaticDistance(Game.World, A.Id, B.PointB));
Game.World.ConnectionList[conA].Render = false;
Game.World.ConnectionList[conB].Render = false;
PointMassList[A.Id].Grabbed = true;
if (A.Player != -1)
{
PlayerList[A.Player].JointActuators.Add(new int[] { conA, A.Id });
PlayerList[A.Player].JointActuators.Add(new int[] { conB, A.Id });
}
}
}
}
public void Coll(PointMass a, PointMass b)
{
Vector_2d diff = a.Pos.Sub(b.Pos);
float InputForce = 1.0F / (float)Math.Sqrt(diff.Dot(diff));
diff.Div(InputForce);
diff.Mult(0.5F);
a.Accerate(diff, this);
b.Accerate(diff.Inverted(), this);
}
public void Intergrate(World world, float friction = 0)
{
friction /= InverseMass;
Vector_2d newOld = Pos;
Vector_2d Friction = new Vector_2d(2 - friction, 2 - friction); //Normal air friction
Vector_2d FrictionOld = new Vector_2d(1 - friction, 1 - friction); //Normal air friction
if (OnGround)
{
//Friction.X = 1 + friction;
//FrictionOld.X = friction;
}
Pos = Pos.Mult(Friction).Sub(OldPos.Mult(FrictionOld));
Pos = Pos.Add(Acceleration.Mult(world.DeltaTime * world.DeltaTime));
OldPos = newOld;
Acceleration = new Vector_2d(0, 0);
}
public override void ResolveConstraint(World world)
{
Vector_2d Dist = world.PointMassList[PointA].Pos.Sub(world.PointMassList[PointB].Pos);
float DistDot = Dist.Dot(Dist);
float Distance = (float)Math.Sqrt(DistDot);
float Difference = (UsedDistance - Distance) / Distance;
if (Difference > 0)
{
Vector_2d translate = new Vector_2d((float)(Dist.X * Difference), (float)(Dist.Y * Difference));
//translate = translate.Mult(Force);
translate = translate.Mult(0.5F).Mult(world.DeltaConstraint);//.Mult(world.DeltaTime);
float scalarP1 = (world.PointMassList[PointA].InverseMass / (world.PointMassList[PointA].InverseMass + world.PointMassList[PointB].InverseMass)) * Stiffness;
float scalarP2 = Stiffness - scalarP1;
world.PointMassList[PointA].Pos = world.PointMassList[PointA].Pos.Add(translate.Mult(scalarP1));
world.PointMassList[PointB].Pos = world.PointMassList[PointB].Pos.Sub(translate.Mult(scalarP2));
}
}
public void CheckBounds(World world)
{
bool Affect = false;
Vector_2d Displace = new Vector_2d();
if (Pos.Y < world.BufferSize)
{
Affect = true;
Displace.Y = (world.BufferSize - Pos.Y);
}
if (Math.Abs(Pos.Y - world.BufferSize) < 5 || Pos.Y < world.BufferSize)
{
OnGround = true;
}
else
{
OnGround = false;
}
if (Pos.Y > world.Size.Y - world.BufferSize)
{
Affect = true;
Displace.Y = ((world.Size.Y - world.BufferSize) - Pos.Y);
}
if (Pos.X < world.BufferSize)
{
Affect = true;
Displace.X = (world.BufferSize - Pos.X);
}
if (Pos.X > world.Size.X - world.BufferSize)
{
Affect = true;
Displace.X = ((world.Size.X - world.BufferSize) - Pos.X);
}
if (Affect)
{
Vector_2d newOld = Pos.Add(Displace.Mult(2F));
Pos = OldPos;
OldPos = newOld;
}
}