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))))); }