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 });
}
}
}
}
//precalculated
public ConnectionStaticDistance(World world, int a, int b)
: base(world, a, b)
{
Vector_2d Dist = world.PointMassList[PointA].Pos.Sub(world.PointMassList[PointB].Pos);
//Resting distances
UsedDistance = (float)Math.Sqrt((double)Dist.Dot(Dist));
//Stiffness = 1;
}
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 override void ResolveConstraint(World world)
{
Vector_2d Avec = world.PointMassList[PointMid].Pos.Sub(world.PointMassList[PointA].Pos).Normal().Perpendicular();
//Whats it with?
if (Direction)
{
Avec.Invert();
}
Vector_2d Bvec = world.PointMassList[PointB].Pos.Sub(world.PointMassList[PointMid].Pos).Normal();
float Dot = Avec.Dot(Bvec);
Dot = (float)Math.Sqrt(Math.Abs(Dot)) * (Dot / Math.Abs(Dot));
float Normal = (Dot / (float)(Math.Sqrt(Avec.Dot(Avec))));
const float Give = 0;//1.0F/10.0F;
if (Math.Abs(1.0F - Normal) > Give)
{
if (Normal < 0)
{
//Extend
UsedDistance = LargestDistance;
}
if (Normal > 0)
{
//Contract
UsedDistance = SmallestDistance;
}
if (Normal == 0)
{
world.PointMassList[PointA].Pos.X += 1;
world.PointMassList[PointB].Pos.X -= 1;
}
base.ResolveConstraint(world);
}
}
public ConnectionPushClose(World world, int a, int b, float MinDistanceSet = -1)
: base(world, a, b)
{
Render = false;
Vector_2d Dist = world.PointMassList[PointA].Pos.Sub(world.PointMassList[PointB].Pos);
//Minamum distance to apply
if (MinDistanceSet == -1)
{
UsedDistance = (float)Math.Sqrt((double)Dist.Dot(Dist));
}
else
{
UsedDistance = MinDistanceSet;
}
}
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));
}
}
//precalculated
public ConnectionRotateToDirection(World world, int a, int mid, int b, bool direction /* 0 = 90, 1 = 270*/)
: base(world, a, b)
{
Direction = direction;
PointMid = mid;
Vector_2d Dist = world.PointMassList[PointA].Pos.Sub(world.PointMassList[PointB].Pos);
UsedDistance = (float)Math.Sqrt((double)Dist.Dot(Dist));
LargestDistance = 1;//To solve bugs
foreach (int k in Game.World.PointMassList[PointMid].Connected)
{
if (k != -1)
{
if (Game.World.ConnectionList[k].PointA == a || Game.World.ConnectionList[k].PointB == a)
{
LargestDistance += Game.World.ConnectionList[k].UsedDistance;
}
if (Game.World.ConnectionList[k].PointA == b || Game.World.ConnectionList[k].PointB == b)
{
LargestDistance += Game.World.ConnectionList[k].UsedDistance;
}
}
}
float adis = 0;
float bdis = 0;
foreach (int k in Game.World.PointMassList[PointMid].Connected)
{
if (k != -1)
{
if (Game.World.ConnectionList[k].PointA == a || Game.World.ConnectionList[k].PointB == a)
{
adis = Game.World.ConnectionList[k].UsedDistance;
}
if (Game.World.ConnectionList[k].PointA == b || Game.World.ConnectionList[k].PointB == b)
{
bdis = Game.World.ConnectionList[k].UsedDistance;
}
}
}
SmallestDistance = (float)Math.Sqrt((float)((adis * adis) + (bdis * bdis) - (2 * adis * bdis * Math.Cos((float)Game.World.PointMassList[PointMid].JointLimit * (3.14 / 180)))));
}