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 PointMass(Vector_2d loc, float mass = 10, float force = 10) { Pos = loc; OldPos = loc; SetMass(mass); ForceApplied = force; }
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 ConnectionPushClose(World world, int a, int b, float MinAngle, int cona, int conb) : base(world, a, b) { Render = false; Vector_2d Dist = world.PointMassList[PointA].Pos.Sub(world.PointMassList[PointB].Pos); float asqd = world.ConnectionList[cona].UsedDistance * world.ConnectionList[cona].UsedDistance; float bsqd = world.ConnectionList[conb].UsedDistance * world.ConnectionList[conb].UsedDistance; float distance = (float)Math.Sqrt((float)(asqd + bsqd - (2 * world.ConnectionList[cona].UsedDistance * world.ConnectionList[conb].UsedDistance * Math.Cos((float)MinAngle * (3.14 / 180))))); //Cosine rule UsedDistance = distance; }
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 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)); } }
//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))))); }
public Vector_2d get_line_intersection(Vector_2d p0, Vector_2d p1, Vector_2d p2, Vector_2d p3) { Vector_2d s1 = new Vector_2d(); Vector_2d s2 = new Vector_2d(); s1.X = p1.X - p0.X; s1.Y = p1.Y - p0.Y; s2.X = p3.X - p2.X; s2.Y = p3.Y - p2.Y; float s, t; s = (-s1.Y * (p0.X - p2.X) + s1.X * (p0.Y - p2.Y)) / (-s2.X * s1.Y + s1.X * s2.Y); t = (s2.X * (p0.Y - p2.Y) - s2.Y * (p0.X - p2.X)) / (-s2.X * s1.Y + s1.X * s2.Y); if (s > 0 && s < 1 && t > 0 && t < 1) { return(new Vector_2d(p0.X + (t * s1.X), p0.Y + (t * s1.Y))); } return(null); // No collision }
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; } }
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 SpawnPlayer(World world, Vector_2d loc_floor, Player player = null) { //Lergs float LegWidth = 30; int LowerLegA = world.AddPointMass(new PointMass(new Vector_2d(loc_floor.X + LegWidth, loc_floor.Y), 50)); int LowerLegB = world.AddPointMass(new PointMass(new Vector_2d(loc_floor.X - LegWidth, loc_floor.Y), 50)); float LegTaper = 6; float LegHeight = 60; int MidLegA = world.AddPointMass(new PointMass(new Vector_2d(loc_floor.X + LegWidth - LegTaper, loc_floor.Y + (LegHeight / 2)))); int MidLegB = world.AddPointMass(new PointMass(new Vector_2d(loc_floor.X - LegWidth + LegTaper, loc_floor.Y + (LegHeight / 2)))); float HipSize = 15; int HigherLegA = world.AddPointMass(new PointMass(new Vector_2d(loc_floor.X + HipSize, loc_floor.Y + LegHeight))); int HigherLegB = world.AddPointMass(new PointMass(new Vector_2d(loc_floor.X - HipSize, loc_floor.Y + LegHeight))); int LowerBody = world.AddPointMass(new PointMass(new Vector_2d(loc_floor.X, loc_floor.Y + LegHeight + HipSize))); int LowerHip = world.AddPointMass(new PointMass(new Vector_2d(loc_floor.X, loc_floor.Y + LegHeight - HipSize))); int LowerLegACon = world.AddConnection(new ConnectionStaticDistance(world, LowerLegA, MidLegA)); int LowerLegBCon = world.AddConnection(new ConnectionStaticDistance(world, LowerLegB, MidLegB)); int HigherLegACon = world.AddConnection(new ConnectionStaticDistance(world, MidLegA, HigherLegA)); int HigherLegBCon = world.AddConnection(new ConnectionStaticDistance(world, MidLegB, HigherLegB)); int HipConA = world.AddConnection(new ConnectionStaticDistance(world, HigherLegA, LowerBody)); int HipConB = world.AddConnection(new ConnectionStaticDistance(world, HigherLegB, LowerBody)); int HipConC = world.AddConnection(new ConnectionStaticDistance(world, HigherLegA, LowerHip)); int HipConD = world.AddConnection(new ConnectionStaticDistance(world, HigherLegB, LowerHip)); int HipConE = world.AddConnection(new ConnectionStaticDistance(world, HigherLegA, HigherLegB)); int HipConF = world.AddConnection(new ConnectionStaticDistance(world, LowerBody, LowerHip)); //world.AddConnection(new ConnectionPushClose(world, LowerLegA, HigherLegA, 90, LowerLegACon, HigherLegACon)); //world.AddConnection(new ConnectionPushClose(world, LowerLegB, HigherLegB, 90, LowerLegBCon, HigherLegBCon)); //world.AddConnection(new ConnectionPushClose(world, LowerBody, MidLegA, 90, HipConA, HigherLegACon)); //world.AddConnection(new ConnectionPushClose(world, LowerBody, MidLegB, 90, HipConB, HigherLegBCon)); //Body float BodyHeight = 30; int MidBody = world.AddPointMass(new PointMass(new Vector_2d(loc_floor.X, loc_floor.Y + LegHeight + HipSize + (BodyHeight / 2)))); int HigherBody = world.AddPointMass(new PointMass(new Vector_2d(loc_floor.X, loc_floor.Y + LegHeight + HipSize + BodyHeight))); int ShoulderWidth = 15; int ShoulderHeight = 10; int ShoulderA = world.AddPointMass(new PointMass(new Vector_2d(loc_floor.X + ShoulderWidth, loc_floor.Y + LegHeight + HipSize + BodyHeight + ShoulderHeight))); int ShoulderB = world.AddPointMass(new PointMass(new Vector_2d(loc_floor.X - ShoulderWidth, loc_floor.Y + LegHeight + HipSize + BodyHeight + ShoulderHeight))); float NeckHeight = 30; int LowerNeck = world.AddPointMass(new PointMass(new Vector_2d(loc_floor.X, loc_floor.Y + LegHeight + HipSize + BodyHeight + ShoulderHeight + (NeckHeight / 2)))); int HigherNeck = world.AddPointMass(new PointMass(new Vector_2d(loc_floor.X, loc_floor.Y + LegHeight + HipSize + BodyHeight + ShoulderHeight + (NeckHeight)))); int HeadSize = 20; int TopHead = world.AddPointMass(new PointMass(new Vector_2d(loc_floor.X, loc_floor.Y + LegHeight + HipSize + BodyHeight + ShoulderHeight + NeckHeight + HeadSize))); int HeadA = world.AddPointMass(new PointMass(new Vector_2d(loc_floor.X + HeadSize, loc_floor.Y + LegHeight + HipSize + BodyHeight + ShoulderHeight + NeckHeight + (HeadSize / 2)))); int HeadB = world.AddPointMass(new PointMass(new Vector_2d(loc_floor.X - HeadSize, loc_floor.Y + LegHeight + HipSize + BodyHeight + ShoulderHeight + NeckHeight + (HeadSize / 2)))); int ArmTaper = 30; int UpperArmDrop = 10; int LowerArmDrop = 20; int MidArmA = world.AddPointMass(new PointMass(new Vector_2d(loc_floor.X + ShoulderWidth + (ArmTaper / 2), loc_floor.Y + LegHeight + HipSize + BodyHeight + ShoulderHeight - UpperArmDrop))); int MidArmB = world.AddPointMass(new PointMass(new Vector_2d(loc_floor.X - (ShoulderWidth + (ArmTaper / 2)), loc_floor.Y + LegHeight + HipSize + BodyHeight + ShoulderHeight - UpperArmDrop))); int LowerArmA = world.AddPointMass(new PointMass(new Vector_2d(loc_floor.X + ShoulderWidth + ArmTaper, loc_floor.Y + LegHeight + HipSize + BodyHeight + ShoulderHeight - (UpperArmDrop + LowerArmDrop)))); int LowerArmB = world.AddPointMass(new PointMass(new Vector_2d(loc_floor.X - (ShoulderWidth + ArmTaper), loc_floor.Y + LegHeight + HipSize + BodyHeight + ShoulderHeight - (UpperArmDrop + LowerArmDrop)))); int LowerBodyCon = world.AddConnection(new ConnectionStaticDistance(world, LowerBody, MidBody)); int HigherBodyCon = world.AddConnection(new ConnectionStaticDistance(world, MidBody, HigherBody)); int ShoulderConA = world.AddConnection(new ConnectionStaticDistance(world, HigherBody, ShoulderA)); int ShoulderConB = world.AddConnection(new ConnectionStaticDistance(world, HigherBody, ShoulderB)); int ShoulderConC = world.AddConnection(new ConnectionStaticDistance(world, HigherBody, LowerNeck)); int ShoulderConE = world.AddConnection(new ConnectionStaticDistance(world, LowerNeck, ShoulderA)); int ShoulderConF = world.AddConnection(new ConnectionStaticDistance(world, LowerNeck, ShoulderB)); int ShoulderCon = world.AddConnection(new ConnectionStaticDistance(world, ShoulderA, ShoulderB)); int NeckConA = world.AddConnection(new ConnectionStaticDistance(world, LowerNeck, HigherNeck)); int NeckConB = world.AddConnection(new ConnectionStaticDistance(world, HigherNeck, TopHead)); int HeadMidCon = world.AddConnection(new ConnectionStaticDistance(world, HeadA, HeadB)); int HeadUpperSideConA = world.AddConnection(new ConnectionStaticDistance(world, HeadA, TopHead)); int HeadUpperSideConB = world.AddConnection(new ConnectionStaticDistance(world, HeadB, TopHead)); int HeadLowerSideConA = world.AddConnection(new ConnectionStaticDistance(world, HeadA, HigherNeck)); int HeadLowerSideConB = world.AddConnection(new ConnectionStaticDistance(world, HeadB, HigherNeck)); int ArmUpperConA = world.AddConnection(new ConnectionStaticDistance(world, ShoulderA, MidArmA)); int ArmUpperConB = world.AddConnection(new ConnectionStaticDistance(world, ShoulderB, MidArmB)); int ArmLowerConA = world.AddConnection(new ConnectionStaticDistance(world, MidArmA, LowerArmA)); int ArmLowerConB = world.AddConnection(new ConnectionStaticDistance(world, MidArmB, LowerArmB)); if (player != null) { player.JointsId.Add(new int[] { HigherLegA, MidLegA, LowerLegA }); player.JointsId.Add(new int[] { HigherLegB, MidLegB, LowerLegB }); player.JointsId.Add(new int[] { LowerBody, HigherLegA, MidLegA }); player.JointsId.Add(new int[] { LowerBody, HigherLegB, MidLegB }); player.JointsId.Add(new int[] { LowerHip, LowerBody, MidBody }); player.JointsId.Add(new int[] { LowerBody, MidBody, HigherBody }); player.JointsId.Add(new int[] { MidBody, HigherBody, LowerNeck }); player.JointsId.Add(new int[] { HigherBody, LowerNeck, HigherNeck }); player.JointsId.Add(new int[] { LowerNeck, HigherNeck, TopHead }); player.JointsId.Add(new int[] { HigherBody, ShoulderA, MidArmA }); player.JointsId.Add(new int[] { HigherBody, ShoulderB, MidArmB }); player.JointsId.Add(new int[] { ShoulderA, MidArmA, LowerArmA }); player.JointsId.Add(new int[] { ShoulderB, MidArmB, LowerArmB }); player.JointsId.Add(new int[] { MidArmA, LowerArmA, -1 }); world.PointMassList[LowerArmA].State = 5; player.JointsId.Add(new int[] { MidArmB, LowerArmB, -1 }); world.PointMassList[LowerArmB].State = 5; player.PointMasses.Add(LowerLegA); player.PointMasses.Add(LowerLegB); player.PointMasses.Add(HigherLegA); player.PointMasses.Add(HigherLegB); player.PointMasses.Add(LowerBody); player.PointMasses.Add(LowerHip); player.Connctions.Add(LowerLegACon); player.Connctions.Add(LowerLegBCon); player.Connctions.Add(HigherLegACon); player.Connctions.Add(HigherLegBCon); player.Connctions.Add(HipConA); player.Connctions.Add(HipConB); player.Connctions.Add(HipConC); player.Connctions.Add(HipConD); player.Connctions.Add(HipConE); player.Connctions.Add(HipConF); //Body/head player.PointMasses.Add(MidBody); player.PointMasses.Add(HigherBody); player.PointMasses.Add(ShoulderA); player.PointMasses.Add(ShoulderB); player.PointMasses.Add(LowerNeck); player.PointMasses.Add(HigherNeck); player.PointMasses.Add(TopHead); player.PointMasses.Add(HeadA); player.PointMasses.Add(HeadB); //Arms player.PointMasses.Add(MidArmA); player.PointMasses.Add(MidArmB); player.PointMasses.Add(LowerArmA); player.PointMasses.Add(LowerArmB); player.Connctions.Add(LowerBodyCon); player.Connctions.Add(HigherBodyCon); player.Connctions.Add(ShoulderConA); player.Connctions.Add(ShoulderConB); player.Connctions.Add(ShoulderConC); player.Connctions.Add(ShoulderConE); player.Connctions.Add(ShoulderConF); player.Connctions.Add(ShoulderCon); player.Connctions.Add(NeckConA); player.Connctions.Add(NeckConB); player.Connctions.Add(HeadMidCon); player.Connctions.Add(HeadUpperSideConA); player.Connctions.Add(HeadUpperSideConB); player.Connctions.Add(HeadLowerSideConA); player.Connctions.Add(HeadLowerSideConB); player.Connctions.Add(ArmUpperConA); player.Connctions.Add(ArmUpperConB); player.Connctions.Add(ArmLowerConA); player.Connctions.Add(ArmLowerConB); } }
public float Dot(Vector_2d v) { return((X * v.X) + (Y * v.Y)); }
public Vector_2d Div(Vector_2d v) { return(new Vector_2d(X / v.X, Y / v.Y)); }
public Vector_2d Mult(Vector_2d v) { return(new Vector_2d(X * v.X, Y * v.Y)); }
public Vector_2d Sub(Vector_2d v) { return(new Vector_2d(X - v.X, Y - v.Y)); }
public Vector_2d Add(Vector_2d v) { return(new Vector_2d(X + v.X, Y + v.Y)); }
public void Accerate(Vector_2d vec, World world) { Acceleration = Acceleration.Add(vec); }
public void Spawn(World world, Vector_2d pos) { new SpawnPlayer(world, pos, this); }